Add force and velocity hardware interfaces

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:

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_;
 };
 

robotiq_driver/src/hardware_interface.cpp

@@ -44,6 +44,8 @@
 
 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 };
@@ -60,15 +62,15 @@
   communication_thread_is_running_.store(false);
   if (communication_thread_.joinable())
   {
     communication_thread_.join();
   }
 }
 
 // This constructor is use for testing only.
 RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface(std::unique_ptr<DriverFactory> driver_factory)
   : driver_factory_{ std::move(driver_factory) }
 {
 }
 
 hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info)
 {
@@ -139,14 +141,14 @@
 }
 
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state)
 {
   RCLCPP_DEBUG(kLogger, "on_configure");
   try
   {
     if (hardware_interface::SystemInterface::on_configure(previous_state) != CallbackReturn::SUCCESS)
     {
       return CallbackReturn::ERROR;
     }
 
     // Open the serial port and handshake.
@@ -154,15 +156,15 @@
     if (!connected)
     {
       RCLCPP_ERROR(kLogger, "Cannot connect to the Robotiq gripper");
       return CallbackReturn::ERROR;
     }
   }
   catch (const std::exception& e)
   {
     RCLCPP_ERROR(kLogger, "Cannot configure the Robotiq gripper: %s", e.what());
     return CallbackReturn::ERROR;
   }
   return CallbackReturn::SUCCESS;
 }
 
 std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface::export_state_interfaces()
@@ -187,6 +189,19 @@
 
   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(
@@ -196,7 +211,7 @@
 }
 
 hardware_interface::CallbackReturn
 RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
 {
   RCLCPP_DEBUG(kLogger, "on_activate");
 
@@ -214,8 +229,8 @@
     driver_->deactivate();
     driver_->activate();
 
     communication_thread_is_running_.store(true);
     communication_thread_ = std::thread([this] { this->background_task(); });
   }
   catch (const std::exception& e)
   {
@@ -228,15 +243,15 @@
 }
 
 hardware_interface::CallbackReturn
 RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
 {
   RCLCPP_DEBUG(kLogger, "on_deactivate");
 
   communication_thread_is_running_.store(false);
   communication_thread_.join();
   if (communication_thread_.joinable())
   {
     communication_thread_.join();
   }
 
   try
@@ -246,16 +261,16 @@
   catch (const std::exception& e)
   {
     RCLCPP_ERROR(kLogger, "Failed to deactivate the Robotiq gripper: %s", e.what());
     return CallbackReturn::ERROR;
   }
   RCLCPP_INFO(kLogger, "Robotiq Gripper successfully deactivated!");
   return CallbackReturn::SUCCESS;
 }
 
 hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclcpp::Time& /*time*/,
                                                                       const rclcpp::Duration& /*period*/)
 {
   gripper_position_ = gripper_closed_pos_ * (gripper_current_state_.load() - kGripperMinPos) / kGripperRange;
 
   if (!std::isnan(reactivate_gripper_cmd_))
   {
@@ -273,17 +288,21 @@
   return hardware_interface::return_type::OK;
 }
 
 hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rclcpp::Time& /*time*/,
                                                                        const rclcpp::Duration& /*period*/)
 {
   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;
 }
 
 void RobotiqGripperHardwareInterface::background_task()
 {
   while (communication_thread_is_running_.load())
   {
@@ -295,12 +314,14 @@
       {
         this->driver_->deactivate();
         this->driver_->activate();
         reactivate_gripper_async_cmd_.store(false);
         reactivate_gripper_async_response_.store(true);
       }
 
       // 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());
@@ -308,11 +329,11 @@
     catch (std::exception& e)
     {
       RCLCPP_ERROR(kLogger, "Error: %s", e.what());
     }
 
     std::this_thread::sleep_for(kGripperCommsLoopPeriod);
   }
 }
 
 }  // namespace robotiq_driver