From ae82db64acbf85c01e271bafe664448041772df2 Mon Sep 17 00:00:00 2001 From: Jordan Longval Date: Tue, 22 Sep 2026 15:34:12 -0400 Subject: [PATCH] fix(driver): estimate joint velocity from position so stall detection means something The velocity state interface was a constant 0.0, so parallel_gripper_action_controller's stall detector (velocity below 0.001 rad/s for 50 ms) fired 50 ms into every goal and returned the result while the fingers were still travelling (ros#29). Every consumer reading the result saw stalled=true, reached_goal=false and a mid-stroke position; the MCP acceptance run on a real 2F-85 reported "stopped on an object" on a close over nothing, at 77.9 mm, with the fingers reaching 1.1 mm a moment later. read() now feeds each position sample and the controller manager's time into the SDK's VelocityEstimator (robotiq/grippers#42, a first-order low-pass over the position difference that never divides by the sample interval). The time constant is 100 ms: one spurious gPO count on a 2F-85 (0.0035 rad) reads as 0.035 rad/s and decays under the stall threshold within about 0.4 s of the fingers stopping, while any commanded speed stays far above it. on_activate resets the estimator, so the calibration sweep and the settled seed do not read as travel. stall_timeout goes from 0.05 s to 1.0 s in both controller configs. The controller starts that clock when it accepts the goal, and the gripper's first position count arrives 50 to 200 ms later (Modbus round trip, motor start), so at 0.05 s one close in about ten still returned early, on the bench at 80.5 mm with the fingers on their way to 26 mm. At 0.3 s the slow final approach still tripped it once in twenty moves (declared stopped at 82.4 mm, at 85.0 mm a second later): near the end the position counts come more than 0.4 s apart and the estimate decays under the threshold in between. A stop is now reported about 1.4 s after the fingers stop: the estimate's decay plus the timeout. extern/grippers moves to the head of robotiq/grippers#42, which sits on the commit already pinned; to be re-pinned once that PR merges. Co-Authored-By: Claude Fable 5.1 Claude-Session: https://claude.ai/code/session_01W8KnAMvo3aZyPWCQ886WBT --- extern/grippers | 2 +- .../config/robotiq_controllers.humble.yaml | 2 +- .../robotiq_description/config/robotiq_controllers.yaml | 2 +- .../include/robotiq_driver/hardware_interface.hpp | 5 +++++ grippers/robotiq_driver/src/hardware_interface.cpp | 7 +++---- 5 files changed, 11 insertions(+), 7 deletions(-) diff --git a/extern/grippers b/extern/grippers index 2cbd42e..7f5a6a0 160000 --- a/extern/grippers +++ b/extern/grippers @@ -1 +1 @@ -Subproject commit 2cbd42e6edc7573c2adc5effd7eef4b3c8328485 +Subproject commit 7f5a6a09a5476692225d87ce22dba0f3c3a83740 diff --git a/grippers/robotiq_description/config/robotiq_controllers.humble.yaml b/grippers/robotiq_description/config/robotiq_controllers.humble.yaml index 9fc6404..b8a1750 100644 --- a/grippers/robotiq_description/config/robotiq_controllers.humble.yaml +++ b/grippers/robotiq_description/config/robotiq_controllers.humble.yaml @@ -35,7 +35,7 @@ robotiq_gripper_controller: max_effort: 50.0 # TODO tune allow_stalling: true - stall_timeout: 0.05 + stall_timeout: 1.0 goal_tolerance: 0.02 robotiq_activation_controller: diff --git a/grippers/robotiq_description/config/robotiq_controllers.yaml b/grippers/robotiq_description/config/robotiq_controllers.yaml index 806e6ed..a831c19 100644 --- a/grippers/robotiq_description/config/robotiq_controllers.yaml +++ b/grippers/robotiq_description/config/robotiq_controllers.yaml @@ -36,7 +36,7 @@ robotiq_gripper_controller: # Optional (recommended) state_interfaces: ["position", "velocity"] allow_stalling: true - stall_timeout: 0.05 + stall_timeout: 1.0 goal_tolerance: 0.02 robotiq_activation_controller: diff --git a/grippers/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/grippers/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index b9c0602..32ac133 100644 --- a/grippers/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/grippers/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -37,6 +37,7 @@ #pragma once +#include #include #include #include @@ -50,6 +51,7 @@ #include #include +#include #include #include @@ -62,6 +64,8 @@ namespace robotiq_driver { inline constexpr const char* kObjectStatusInterface = "object_status"; + +inline constexpr std::chrono::milliseconds kVelocityTimeConstant{100}; inline constexpr const char* kMotorCurrentInterface = "motor_current"; inline constexpr const char* kGripperFaultInterface = "gripper_fault"; inline constexpr const char* kGripperFaultSeverityInterface = "gripper_fault_severity"; @@ -188,6 +192,7 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa double gripper_position_ = 0.0; double gripper_velocity_ = 0.0; + Robotiq::VelocityEstimator velocity_estimator_{kVelocityTimeConstant}; // gCU in amperes, and gOBJ verbatim. Doubles to match the other // interfaces: Jazzy can carry a uint8_t, but only through the diff --git a/grippers/robotiq_driver/src/hardware_interface.cpp b/grippers/robotiq_driver/src/hardware_interface.cpp index fef270a..1ddacf1 100644 --- a/grippers/robotiq_driver/src/hardware_interface.cpp +++ b/grippers/robotiq_driver/src/hardware_interface.cpp @@ -392,6 +392,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( // Seed both sides from that settled reading so the first exported state, and // any hold target derived from it, describe where the fingers actually are. gripper_position_ = jointPositionFromRegister(status.position, parameters_.closed_position); + velocity_estimator_.reset(); gripper_velocity_ = 0.0; gripper_motor_current_ = motorCurrentFromRegister(status.current); gripper_object_status_ = static_cast(status.gripperStatus.objectDetection()); @@ -437,7 +438,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivat return CallbackReturn::SUCCESS; } -hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclcpp::Time& /*time*/, +hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclcpp::Time& time, const rclcpp::Duration& /*period*/) { if(!gripper_) @@ -447,9 +448,7 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclc const Robotiq::GripperStatus status = gripper_->getStatus(); gripper_position_ = jointPositionFromRegister(status.position, parameters_.closed_position); - // The status block carries no velocity — the gripper reports position and - // motor current only. - gripper_velocity_ = 0.0; + gripper_velocity_ = velocity_estimator_.update(gripper_position_, std::chrono::nanoseconds{time.nanoseconds()}); gripper_motor_current_ = motorCurrentFromRegister(status.current); gripper_object_status_ = static_cast(status.gripperStatus.objectDetection()); gripper_fault_ = {gripperFaultFromRegister(status.faultStatus),