franka_gripper_sim.h

Go to the documentation of this file.

#pragma once

#include <functional>
#include <mutex>

#include <actionlib/server/simple_action_server.h>
#include <control_msgs/GripperCommandAction.h>
#include <control_toolbox/pid.h>
#include <controller_interface/controller.h>
#include <franka_gripper/GraspAction.h>
#include <franka_gripper/GraspEpsilon.h>
#include <franka_gripper/HomingAction.h>
#include <franka_gripper/MoveAction.h>
#include <franka_gripper/StopAction.h>
#include <franka_hw/trigger_rate.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <realtime_tools/realtime_publisher.h>
#include <ros/time.h>
#include <sensor_msgs/JointState.h>

namespace franka_gazebo {

const double kMaxFingerWidth = 0.08;

const double kDefaultMoveWidthTolerance = 0.001;

const double kDefaultGripperActionWidthTolerance = 0.005;

const double kDefaultGripperActionSpeed = 0.1;

const double kGraspRestingThreshold = 0.003;

const int kGraspConsecutiveSamples = 10;

class FrankaGripperSim
    : public controller_interface::Controller<hardware_interface::EffortJointInterface> {
 public:
  enum State {
    IDLE,      
    HOLDING,   
    MOVING,    
    GRASPING,  
  };

  struct Config {
    double width_desired;  
    double speed_desired;  
    double force_desired;  
    franka_gripper::GraspEpsilon tolerance;  
  };

  bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle& nh) override;
  void starting(const ros::Time&) override;
  void update(const ros::Time& now, const ros::Duration& period) override;

 private:
  State state_ = State::IDLE;
  Config config_;

  franka_hw::TriggerRate rate_trigger_{30.0};
  control_toolbox::Pid pid1_;
  control_toolbox::Pid pid2_;
  realtime_tools::RealtimePublisher<sensor_msgs::JointState> pub_;
  hardware_interface::JointHandle finger1_;
  hardware_interface::JointHandle finger2_;

  std::mutex mutex_;

  // Configurable by parameters
  int speed_samples_;
  double speed_threshold_;
  double speed_default_;
  double tolerance_move_;            
  double tolerance_gripper_action_;  

  std::unique_ptr<actionlib::SimpleActionServer<franka_gripper::StopAction>> action_stop_;
  std::unique_ptr<actionlib::SimpleActionServer<franka_gripper::HomingAction>> action_homing_;
  std::unique_ptr<actionlib::SimpleActionServer<franka_gripper::MoveAction>> action_move_;
  std::unique_ptr<actionlib::SimpleActionServer<franka_gripper::GraspAction>> action_grasp_;
  std::unique_ptr<actionlib::SimpleActionServer<control_msgs::GripperCommandAction>> action_gc_;

  void setState(const State&& state);
  void setConfig(const Config&& config);
  void transition(const State&& state, const Config&& config);

  void control(hardware_interface::JointHandle& joint,
               control_toolbox::Pid&,
               double q_d,
               double dq_d,
               double f_d,
               const ros::Duration& period);

  void interrupt(const std::string& message, const State& except);

  void waitUntilStateChange();

  void onStopGoal(const franka_gripper::StopGoalConstPtr& goal);
  void onHomingGoal(const franka_gripper::HomingGoalConstPtr& goal);
  void onMoveGoal(const franka_gripper::MoveGoalConstPtr& goal);
  void onGraspGoal(const franka_gripper::GraspGoalConstPtr& goal);
  void onGripperActionGoal(const control_msgs::GripperCommandGoalConstPtr& goal);

  bool grasp(double width, double speed, double force, const franka_gripper::GraspEpsilon& epsilon);

  bool move(double width, double speed);
};
}  // namespace franka_gazebo