fix units for commands

Signed-off-by: Paul Gesel <[email protected]>

robotiq_driver/include/robotiq_driver/hardware_interface.hpp

@@ -149,8 +149,8 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa
   double gripper_position_command_;
 
   std::atomic<uint8_t> write_command_;
-  std::atomic<uint8_t> write_force_multiplier_;
-  std::atomic<uint8_t> write_speed_multiplier_;
+  std::atomic<uint8_t> write_force_;
+  std::atomic<uint8_t> write_speed_;
   std::atomic<uint8_t> gripper_current_state_;
 
   double reactivate_gripper_cmd_;

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 };
@@ -292,9 +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));
-  // TODO these values do not have the correct units
-  write_speed_multiplier_.store(uint8_t(gripper_speed_ * 0xFF));
-  write_force_multiplier_.store(uint8_t(gripper_force_ * 0xFF));
+  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;
 }
@@ -317,8 +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_multiplier_.load());
-      this->driver_->set_force(write_force_multiplier_.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());