From 5b1adfd6048c56a5437b653b8087f71c95779d8a Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 30 Jan 2024 08:09:03 -0700 Subject: [PATCH] export new interfaces for force and velocity Signed-off-by: Paul Gesel --- .../robotiq_driver/hardware_interface.hpp | 4 +-- robotiq_driver/src/hardware_interface.cpp | 25 +++++++++---------- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index bdbcd24..5c3a214 100644 --- a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -156,8 +156,8 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa double reactivate_gripper_cmd_; std::atomic reactivate_gripper_async_cmd_; double reactivate_gripper_response_; - double gripper_force_multiplier_; - double gripper_speed_multiplier_; + double gripper_force_; + double gripper_speed_; std::atomic> reactivate_gripper_async_response_; }; diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index d6811c6..67b1094 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -189,16 +189,16 @@ std::vector RobotiqGripperHardwareInterfac info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_)); command_interfaces.emplace_back( - hardware_interface::CommandInterface("multipliers", "gripper_speed_multiplier", &gripper_speed_multiplier_)); - gripper_speed_multiplier_ = info_.hardware_parameters.count("gripper_speed_multiplier") ? - info_.hardware_parameters.count("gripper_speed_multiplier") : - 1.0; + hardware_interface::CommandInterface(info_.joints[0].name, "gripper_speed", &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("multipliers", "gripper_force_multiplier", &gripper_force_multiplier_)); - gripper_force_multiplier_ = info_.hardware_parameters.count("gripper_force_multiplier") ? - info_.hardware_parameters.count("gripper_force_multiplier") : - 1.0; + hardware_interface::CommandInterface(info_.joints[0].name, "gripper_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_)); @@ -292,10 +292,9 @@ 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_multiplier_ = std::clamp(gripper_speed_multiplier_, 0.0, 1.0); - write_speed_multiplier_.store(uint8_t(gripper_speed_multiplier_ * 0xFF)); - gripper_force_multiplier_ = std::clamp(gripper_force_multiplier_, 0.0, 1.0); - write_force_multiplier_.store(uint8_t(gripper_force_multiplier_ * 0xFF)); + // 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)); return hardware_interface::return_type::OK; } @@ -319,7 +318,7 @@ 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_); + this->driver_->set_force(write_force_multiplier_.load()); // Read the state of the gripper. gripper_current_state_.store(this->driver_->get_gripper_position());