diff --git a/robotiq_description/config/robotiq_controllers.yaml b/robotiq_description/config/robotiq_controllers.yaml index 8d0b84d..588ec21 100644 --- a/robotiq_description/config/robotiq_controllers.yaml +++ b/robotiq_description/config/robotiq_controllers.yaml @@ -12,6 +12,8 @@ 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 index 5c3a214..35dfde2 100644 --- a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -149,8 +149,8 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa double gripper_position_command_; std::atomic write_command_; - std::atomic write_force_multiplier_; - std::atomic write_speed_multiplier_; + std::atomic write_force_; + std::atomic write_speed_; std::atomic gripper_current_state_; double reactivate_gripper_cmd_; diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index 67b1094..75f5f36 100644 --- 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 }; @@ -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());