franka_hw.h

Go to the documentation of this file.

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#pragma once

#include <array>
#include <atomic>
#include <exception>
#include <functional>
#include <list>
#include <string>

#include <franka/control_types.h>
#include <franka/duration.h>
#include <franka/rate_limiting.h>
#include <franka/robot.h>
#include <franka/robot_state.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <ros/time.h>
#include <urdf/model.h>

#include <franka_hw/control_mode.h>
#include <franka_hw/franka_cartesian_command_interface.h>
#include <franka_hw/franka_model_interface.h>
#include <franka_hw/franka_state_interface.h>
#include <franka_hw/model_base.h>
#include <franka_hw/resource_helpers.h>

namespace franka_hw {

class FrankaHW : public hardware_interface::RobotHW {
 public:
  FrankaHW();

  virtual ~FrankaHW() override = default;

  virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override;

  virtual bool initParameters(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh);

  virtual void initROSInterfaces(ros::NodeHandle& robot_hw_nh);

  virtual void setupParameterCallbacks(ros::NodeHandle& robot_hw_nh);

  virtual void connect();

  virtual bool disconnect();

  virtual bool connected();

  virtual void control(
      const std::function<bool(const ros::Time&, const ros::Duration&)>& ros_callback);

  virtual void update(const franka::RobotState& robot_state);

  virtual bool controllerActive() const noexcept;

  virtual bool checkForConflict(
      const std::list<hardware_interface::ControllerInfo>& info) const override;

  virtual void doSwitch(
      const std::list<hardware_interface::ControllerInfo>& /*start_list*/,
      const std::list<hardware_interface::ControllerInfo>& /*stop_list*/) override;

  virtual bool prepareSwitch(
      const std::list<hardware_interface::ControllerInfo>& start_list,
      const std::list<hardware_interface::ControllerInfo>& stop_list) override;

  virtual std::array<double, 7> getJointPositionCommand() const noexcept;

  virtual std::array<double, 7> getJointVelocityCommand() const noexcept;

  virtual std::array<double, 7> getJointEffortCommand() const noexcept;

  virtual void enforceLimits(const ros::Duration& period);

  virtual void reset();

  virtual void checkJointLimits();

  virtual void read(const ros::Time& time, const ros::Duration& period) override;

  virtual void write(const ros::Time& time, const ros::Duration& period) override;

  virtual franka::Robot& robot() const;

  virtual std::mutex& robotMutex();

  static bool commandHasNaN(const franka::Torques& command);

  static bool commandHasNaN(const franka::JointPositions& command);

  static bool commandHasNaN(const franka::JointVelocities& command);

  static bool commandHasNaN(const franka::CartesianPose& command);

  static bool commandHasNaN(const franka::CartesianVelocities& command);

  static std::vector<double> getCollisionThresholds(const std::string& name,
                                                    const ros::NodeHandle& robot_hw_nh,
                                                    const std::vector<double>& defaults);

 protected:
  template <size_t size>
  static bool arrayHasNaN(const std::array<double, size>& array) {
    return std::any_of(array.begin(), array.end(), [](const double& e) { return std::isnan(e); });
  }

  using Callback = std::function<bool(const franka::RobotState&, franka::Duration)>;

  template <typename T>
  T controlCallback(const T& command,
                    Callback ros_callback,
                    const franka::RobotState& robot_state,
                    franka::Duration time_step) {
    robot_state_libfranka_ = robot_state;
    ros::Time now = ros::Time(0);
    read(now, ros::Duration(time_step.toSec()));

    if (!controller_active_ || (ros_callback && !ros_callback(robot_state, time_step))) {
      return franka::MotionFinished(command);
    }

    write(now, ros::Duration(time_step.toSec()));
    if (commandHasNaN(command)) {
      std::string error_message = "FrankaHW::controlCallback: Got NaN command!";
      ROS_FATAL("%s", error_message.c_str());
      throw std::invalid_argument(error_message);
    }

    return command;
  }

  template <typename T>
  void setupLimitInterface(joint_limits_interface::JointLimitsInterface<T>& limit_interface,
                           hardware_interface::JointCommandInterface& command_interface) {
    joint_limits_interface::SoftJointLimits soft_limits;
    joint_limits_interface::JointLimits joint_limits;
    for (size_t i = 0; i < joint_names_.size(); i++) {
      const std::string& joint_name = joint_names_[i];
      auto urdf_joint = urdf_model_.getJoint(joint_name);
      if (!urdf_joint || !urdf_joint->safety || !urdf_joint->limits) {
        ROS_WARN(
            "FrankaHW: Joint %s has incomplete limits and safety specs. Skipping it in the joint "
            "limit interface!",
            joint_name.c_str());
        continue;
      }
      if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) {
        if (joint_limits_interface::getJointLimits(urdf_joint, joint_limits)) {
          joint_limits.max_acceleration = franka::kMaxJointAcceleration[i];
          joint_limits.has_acceleration_limits = true;
          joint_limits.max_jerk = franka::kMaxJointJerk[i];
          joint_limits.has_jerk_limits = true;
          T limit_handle(command_interface.getHandle(joint_name), joint_limits, soft_limits);
          limit_interface.registerHandle(limit_handle);
        } else {
          ROS_ERROR(
              "FrankaHW: Could not parse joint limit for joint: %s for joint limit interfaces",
              joint_name.c_str());
        }
      } else {
        ROS_ERROR(
            "FrankaHW: Could not parse soft joint limit for joint %s for joint limit interfaces",
            joint_name.c_str());
      }
    }
  }

  virtual void setupJointStateInterface(franka::RobotState& robot_state);

  template <typename T>
  void setupJointCommandInterface(std::array<double, 7>& command,
                                  franka::RobotState& state,
                                  bool use_q_d,
                                  T& interface) {
    for (size_t i = 0; i < joint_names_.size(); i++) {
      hardware_interface::JointStateHandle state_handle;
      if (use_q_d) {
        state_handle = hardware_interface::JointStateHandle(joint_names_[i], &state.q_d[i],
                                                            &state.dq_d[i], &state.tau_J[i]);
      } else {
        state_handle = hardware_interface::JointStateHandle(joint_names_[i], &state.q[i],
                                                            &state.dq[i], &state.tau_J[i]);
      }
      hardware_interface::JointHandle joint_handle(state_handle, &command[i]);
      interface.registerHandle(joint_handle);
    }
    registerInterface(&interface);
  }

  virtual void setupFrankaStateInterface(franka::RobotState& robot_state);

  virtual void setupFrankaCartesianPoseInterface(franka::CartesianPose& pose_cartesian_command);

  virtual void setupFrankaCartesianVelocityInterface(
      franka::CartesianVelocities& velocity_cartesian_command);

  virtual void setupFrankaModelInterface(franka::RobotState& robot_state);

  virtual bool setRunFunction(const ControlMode& requested_control_mode,
                              bool limit_rate,
                              double cutoff_frequency,
                              franka::ControllerMode internal_controller);
  virtual void initRobot();

  struct CollisionConfig {
    std::array<double, 7> lower_torque_thresholds_acceleration;
    std::array<double, 7> upper_torque_thresholds_acceleration;
    std::array<double, 7> lower_torque_thresholds_nominal;
    std::array<double, 7> upper_torque_thresholds_nominal;
    std::array<double, 6> lower_force_thresholds_acceleration;
    std::array<double, 6> upper_force_thresholds_acceleration;
    std::array<double, 6> lower_force_thresholds_nominal;
    std::array<double, 6> upper_force_thresholds_nominal;
  };

  CollisionConfig collision_config_;
  hardware_interface::JointStateInterface joint_state_interface_{};
  FrankaStateInterface franka_state_interface_{};
  hardware_interface::PositionJointInterface position_joint_interface_{};
  hardware_interface::VelocityJointInterface velocity_joint_interface_{};
  hardware_interface::EffortJointInterface effort_joint_interface_{};
  FrankaPoseCartesianInterface franka_pose_cartesian_interface_{};
  FrankaVelocityCartesianInterface franka_velocity_cartesian_interface_{};
  FrankaModelInterface franka_model_interface_{};
  joint_limits_interface::PositionJointSoftLimitsInterface position_joint_limit_interface_{};
  joint_limits_interface::VelocityJointSoftLimitsInterface velocity_joint_limit_interface_{};
  joint_limits_interface::EffortJointSoftLimitsInterface effort_joint_limit_interface_{};

  std::mutex libfranka_state_mutex_;
  std::mutex ros_state_mutex_;
  franka::RobotState robot_state_libfranka_{};
  franka::RobotState robot_state_ros_{};

  std::mutex libfranka_cmd_mutex_;
  franka::JointPositions position_joint_command_libfranka_;
  franka::JointVelocities velocity_joint_command_libfranka_;
  franka::Torques effort_joint_command_libfranka_;
  franka::CartesianPose pose_cartesian_command_libfranka_;
  franka::CartesianVelocities velocity_cartesian_command_libfranka_;

  std::mutex ros_cmd_mutex_;
  franka::JointPositions position_joint_command_ros_;
  franka::JointVelocities velocity_joint_command_ros_;
  franka::Torques effort_joint_command_ros_;
  franka::CartesianPose pose_cartesian_command_ros_;
  franka::CartesianVelocities velocity_cartesian_command_ros_;

  std::mutex robot_mutex_;
  std::unique_ptr<franka::Robot> robot_;
  std::unique_ptr<franka_hw::ModelBase> model_;

  std::array<std::string, 7> joint_names_;
  std::string arm_id_;
  std::string robot_ip_;
  urdf::Model urdf_model_;
  double joint_limit_warning_threshold_{0.1};
  franka::RealtimeConfig realtime_config_;

  bool initialized_{false};
  std::atomic_bool controller_active_{false};
  ControlMode current_control_mode_ = ControlMode::None;

  std::function<franka::ControllerMode()> get_internal_controller_;
  std::function<bool()> get_limit_rate_;
  std::function<double()> get_cutoff_frequency_;
  std::function<void(franka::Robot&, Callback)> run_function_;
};

}  // namespace franka_hw