franka_hw_sim.h

Go to the documentation of this file.

#pragma once

#include <actionlib/server/simple_action_server.h>
#include <franka/robot_state.h>
#include <franka_gazebo/controller_verifier.h>
#include <franka_gazebo/joint.h>
#include <franka_gazebo/statemachine.h>
#include <franka_hw/franka_model_interface.h>
#include <franka_hw/franka_state_interface.h>
#include <franka_hw/model_base.h>
#include <franka_msgs/ErrorRecoveryAction.h>
#include <gazebo_ros_control/robot_hw_sim.h>
#include <hardware_interface/internal/hardware_resource_manager.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <urdf/model.h>
#include <boost/optional.hpp>
#include <boost_sml/sml.hpp>
#include <cmath>
#include <gazebo/common/common.hh>
#include <gazebo/physics/physics.hh>
#include <map>
#include <memory>
#include <mutex>

namespace franka_gazebo {

const double kDefaultTauExtLowpassFilter = 1.0;  // no filtering per default of tau_ext_hat_filtered

class FrankaHWSim : public gazebo_ros_control::RobotHWSim {
 public:
  FrankaHWSim();

  bool initSim(const std::string& robot_namespace,
               ros::NodeHandle model_nh,
               gazebo::physics::ModelPtr parent,
               const urdf::Model* const urdf,
               std::vector<transmission_interface::TransmissionInfo> transmissions) override;

  void readSim(ros::Time time, ros::Duration period) override;

  void writeSim(ros::Time time, ros::Duration period) override;

  void eStopActive(const bool active) override;

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

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

 private:
  bool robot_initialized_;

  std::unique_ptr<ControllerVerifier> verifier_;

  std::array<double, 3> gravity_earth_;

  std::string arm_id_;
  gazebo::physics::ModelPtr robot_;
  std::map<std::string, std::shared_ptr<franka_gazebo::Joint>> joints_;

  hardware_interface::JointStateInterface jsi_;
  hardware_interface::EffortJointInterface eji_;
  hardware_interface::PositionJointInterface pji_;
  hardware_interface::VelocityJointInterface vji_;
  franka_hw::FrankaStateInterface fsi_;
  franka_hw::FrankaModelInterface fmi_;

  boost::sml::sm<franka_gazebo::StateMachine, boost::sml::thread_safe<std::mutex>> sm_;
  franka::RobotState robot_state_;
  std::unique_ptr<franka_hw::ModelBase> model_;

  double tau_ext_lowpass_filter_;

  ros::Publisher robot_initialized_pub_;
  ros::ServiceServer service_set_ee_;
  ros::ServiceServer service_set_k_;
  ros::ServiceServer service_set_load_;
  ros::ServiceServer service_collision_behavior_;
  ros::ServiceServer service_user_stop_;
  ros::ServiceClient service_controller_list_;
  ros::ServiceClient service_controller_switch_;
  std::unique_ptr<actionlib::SimpleActionServer<franka_msgs::ErrorRecoveryAction>> action_recovery_;

  std::vector<double> lower_force_thresholds_nominal_;
  std::vector<double> upper_force_thresholds_nominal_;

  void initJointStateHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
  void initEffortCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
  void initPositionCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
  void initVelocityCommandHandle(const std::shared_ptr<franka_gazebo::Joint>& joint);
  void initFrankaStateHandle(const std::string& robot,
                             const urdf::Model& urdf,
                             const transmission_interface::TransmissionInfo& transmission);
  void initFrankaModelHandle(const std::string& robot,
                             const urdf::Model& urdf,
                             const transmission_interface::TransmissionInfo& transmission,
                             double singularity_threshold);
  void initServices(ros::NodeHandle& nh);

  void restartControllers();

  void updateRobotState(ros::Time time);
  void updateRobotStateDynamics();

  bool readParameters(const ros::NodeHandle& nh, const urdf::Model& urdf);

  void guessEndEffector(const ros::NodeHandle& nh, const urdf::Model& urdf);

  static double positionControl(Joint& joint, double setpoint, const ros::Duration& period);
  static double velocityControl(Joint& joint, double setpoint, const ros::Duration& period);

  template <int N>
  std::array<double, N> readArray(std::string param, std::string name = "") {
    std::array<double, N> x;

    std::istringstream iss(param);
    std::vector<std::string> values{std::istream_iterator<std::string>{iss},
                                    std::istream_iterator<std::string>{}};
    if (values.size() != N) {
      throw std::invalid_argument("Expected parameter '" + name + "' to have exactely " +
                                  std::to_string(N) + " numbers separated by spaces, but found " +
                                  std::to_string(values.size()));
    }
    std::transform(values.begin(), values.end(), x.begin(),
                   [](std::string v) -> double { return std::stod(v); });
    return x;
  }

  static Eigen::Matrix3d skewMatrix(const Eigen::Vector3d& vec) {
    Eigen::Matrix3d vec_hat;
    // clang-format off
    vec_hat <<
              0, -vec(2),  vec(1),
         vec(2),      0, -vec(0),
        -vec(1),  vec(0),       0;
    // clang-format on
    return vec_hat;
  }

  static Eigen::Matrix3d shiftInertiaTensor(Eigen::Matrix3d I, double m, Eigen::Vector3d p) {
    Eigen::Matrix3d P = skewMatrix(p);
    Eigen::Matrix3d Ip = I + m * P.transpose() * P;
    return Ip;
  }

  void forControlledJoint(
      const std::list<hardware_interface::ControllerInfo>& controllers,
      const std::function<void(franka_gazebo::Joint& joint, const ControlMethod&)>& f);
};

}  // namespace franka_gazebo