Add force and velocity hardware interfaces (#44)

* add velocity interface Signed-off-by: Paul Gesel <[email protected]> * export new interfaces for force and velocity Signed-off-by: Paul Gesel <[email protected]> * fix units for commands Signed-off-by: Paul Gesel <[email protected]> * update config Signed-off-by: Paul Gesel <[email protected]> * update interface names Signed-off-by: Paul Gesel <[email protected]> --------- Signed-off-by: Paul Gesel <[email protected]>
PickNikRobotics · Feb 15, 2024 · d739382 · d739382
1 parent 480fe39
commit d739382
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 1 deletion.
diff --git a/robotiq_description/config/robotiq_controllers.yaml b/robotiq_description/config/robotiq_controllers.yaml
@@ -4,14 +4,16 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
     robotiq_gripper_controller:
-      type: position_controllers/GripperActionController
+      type: antipodal_gripper_action_controller/GripperActionController
     robotiq_activation_controller:
       type: robotiq_controllers/RobotiqActivationController
 
 robotiq_gripper_controller:
   ros__parameters:
     default: true
     joint: robotiq_85_left_knuckle_joint
+    use_effort_interface: true
+    use_speed_interface: true
 
 robotiq_activation_controller:
   ros__parameters:

diff --git a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp
@@ -149,11 +149,15 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa
   double gripper_position_command_;
 
   std::atomic<uint8_t> write_command_;
+  std::atomic<uint8_t> write_force_;
+  std::atomic<uint8_t> write_speed_;
   std::atomic<uint8_t> gripper_current_state_;
 
   double reactivate_gripper_cmd_;
   std::atomic<bool> reactivate_gripper_async_cmd_;
   double reactivate_gripper_response_;
+  double gripper_force_;
+  double gripper_speed_;
   std::atomic<std::optional<bool>> reactivate_gripper_async_response_;
 };
 

diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp
@@ -44,6 +44,8 @@ const auto kLogger = rclcpp::get_logger("RobotiqGripperHardwareInterface");
 
 constexpr uint8_t kGripperMinPos = 3;
 constexpr uint8_t kGripperMaxPos = 230;
+constexpr double kGripperMaxSpeed = 0.150;  // mm/s
+constexpr double kGripperMaxforce = 235;    // N
 constexpr uint8_t kGripperRange = kGripperMaxPos - kGripperMinPos;
 
 constexpr auto kGripperCommsLoopPeriod = std::chrono::milliseconds{ 10 };
@@ -187,6 +189,19 @@ std::vector<hardware_interface::CommandInterface> RobotiqGripperHardwareInterfac
 
   command_interfaces.emplace_back(hardware_interface::CommandInterface(
       info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_));
+
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_velocity", &gripper_speed_));
+  gripper_speed_ = info_.hardware_parameters.count("gripper_speed_multiplier") ?
+                       info_.hardware_parameters.count("gripper_speed_multiplier") :
+                       1.0;
+
+  command_interfaces.emplace_back(
+      hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_effort", &gripper_force_));
+  gripper_force_ = info_.hardware_parameters.count("gripper_force_multiplier") ?
+                       info_.hardware_parameters.count("gripper_force_multiplier") :
+                       1.0;
+
   command_interfaces.emplace_back(
       hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_));
   command_interfaces.emplace_back(hardware_interface::CommandInterface(
@@ -279,6 +294,10 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rcl
   double gripper_pos = (gripper_position_command_ / gripper_closed_pos_) * kGripperRange + kGripperMinPos;
   gripper_pos = std::max(std::min(gripper_pos, 255.0), 0.0);
   write_command_.store(uint8_t(gripper_pos));
+  gripper_speed_ = kGripperMaxSpeed * std::clamp(fabs(gripper_speed_) / kGripperMaxSpeed, 0.0, 1.0);
+  write_speed_.store(uint8_t(gripper_speed_ * 0xFF));
+  gripper_force_ = kGripperMaxforce * std::clamp(fabs(gripper_force_) / kGripperMaxforce, 0.0, 1.0);
+  write_force_.store(uint8_t(gripper_force_ * 0xFF));
 
   return hardware_interface::return_type::OK;
 }
@@ -301,6 +320,8 @@ void RobotiqGripperHardwareInterface::background_task()
 
       // Write the latest command to the gripper.
       this->driver_->set_gripper_position(write_command_.load());
+      this->driver_->set_speed(write_speed_.load());
+      this->driver_->set_force(write_force_.load());
 
       // Read the state of the gripper.
       gripper_current_state_.store(this->driver_->get_gripper_position());