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