From 8298bce2209b0aaa2994323c57759fab7b7c3da6 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Fri, 26 Jan 2024 12:16:22 -0700 Subject: [PATCH 1/5] add velocity interface Signed-off-by: Paul Gesel --- .../robotiq_driver/hardware_interface.hpp | 4 ++++ robotiq_driver/src/hardware_interface.cpp | 19 +++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index 3a80499..bdbcd24 100644 --- a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -149,11 +149,15 @@ 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 gripper_current_state_; double reactivate_gripper_cmd_; std::atomic reactivate_gripper_async_cmd_; double reactivate_gripper_response_; + double gripper_force_multiplier_; + double gripper_speed_multiplier_; std::atomic> reactivate_gripper_async_response_; }; diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index 2a293cb..d6811c6 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -187,6 +187,19 @@ std::vector RobotiqGripperHardwareInterfac 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("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; + + 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; + command_interfaces.emplace_back( hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_)); command_interfaces.emplace_back(hardware_interface::CommandInterface( @@ -279,6 +292,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)); + 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)); return hardware_interface::return_type::OK; } @@ -301,6 +318,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_); // Read the state of the gripper. gripper_current_state_.store(this->driver_->get_gripper_position()); From 5b1adfd6048c56a5437b653b8087f71c95779d8a Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 30 Jan 2024 08:09:03 -0700 Subject: [PATCH 2/5] 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()); From cea992fa430b4ce50c3dabb42ec63f70ed2d76a8 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 30 Jan 2024 15:14:46 -0700 Subject: [PATCH 3/5] fix units for commands Signed-off-by: Paul Gesel --- robotiq_description/config/robotiq_controllers.yaml | 2 ++ .../include/robotiq_driver/hardware_interface.hpp | 4 ++-- robotiq_driver/src/hardware_interface.cpp | 13 ++++++++----- 3 files changed, 12 insertions(+), 7 deletions(-) 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()); From 0f14c5b5c872636e5d36ac9bd8de3f2a55c5bf7b Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Fri, 2 Feb 2024 11:38:36 -0700 Subject: [PATCH 4/5] update config Signed-off-by: Paul Gesel --- robotiq_description/config/robotiq_controllers.yaml | 2 +- robotiq_driver/src/hardware_interface.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/robotiq_description/config/robotiq_controllers.yaml b/robotiq_description/config/robotiq_controllers.yaml index 588ec21..b2677dc 100644 --- a/robotiq_description/config/robotiq_controllers.yaml +++ b/robotiq_description/config/robotiq_controllers.yaml @@ -4,7 +4,7 @@ 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 diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index 75f5f36..9ed1a45 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -44,8 +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 double kGripperMaxSpeed = 0.150; // mm/s +constexpr double kGripperMaxforce = 235; // N constexpr uint8_t kGripperRange = kGripperMaxPos - kGripperMinPos; constexpr auto kGripperCommsLoopPeriod = std::chrono::milliseconds{ 10 }; @@ -294,9 +294,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_ = kGripperMaxSpeed*std::clamp(fabs(gripper_speed_)/kGripperMaxSpeed, 0.0, 1.0); + 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); + 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; From 70167a24cdbbda8419369ab5ea02088395cd9e64 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 14 Feb 2024 16:41:11 -0700 Subject: [PATCH 5/5] update interface names Signed-off-by: Paul Gesel --- robotiq_driver/src/hardware_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index 9ed1a45..07a3af0 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -191,13 +191,13 @@ std::vector RobotiqGripperHardwareInterfac info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_)); command_interfaces.emplace_back( - hardware_interface::CommandInterface(info_.joints[0].name, "gripper_speed", &gripper_speed_)); + 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, "gripper_effort", &gripper_force_)); + 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;