fix(driver): estimate joint velocity so stall detection works - #92
Draft
jlongvalRobotiq wants to merge 1 commit into
Draft
jlongvalRobotiq wants to merge 1 commit into
jlongvalRobotiq wants to merge 1 commit into
Conversation
… 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 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01W8KnAMvo3aZyPWCQ886WBT
This was referenced Sep 25, 2026
Collaborator
|
The stall detection this fixed is now decided from the gripper's object detection (#100, #101), so the driver's velocity estimate has no consumer on the real gripper. I recommend closing this PR: #29 tracks the remaining |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Fixes #29.
Where this comes from
The gripper MCP (
mcp/, onfeat/mcp) is going through an acceptance matrix before its merge tomain, with the expected result for each case written down before the run. Pathway B runs it on a real 2F-85 on the bench, and on the first move it hit #29: every move came backstalled, not at the goal, and at a position partway along. A close over nothing was reported as "stopped on an object" at 77.9 mm, while the fingers carried on to 1.1 mm a moment later. The bug is in the driver, so the fix goes againstmain, not into the MCP stack.Why
The driver's velocity state interface was always 0.0.
parallel_gripper_action_controllerdeclares a stall when velocity stays below 0.001 rad/s forstall_timeout(0.05 s), so the stall check fired 50 ms into every goal and returned the result while the fingers were still moving. Anything reading the result, not only the MCP, got a wrong answer.read()now feeds each position sample, with the controller manager's timestamp, into the SDK'sVelocityEstimator(robotiq/grippers#42). The estimator is a first-order low-pass filter over the change in position, and it never divides by the sample interval, so a short interval cannot blow it up. With a 100 ms time constant, one spurious position count on a 2F-85 (0.0035 rad) shows up as 0.035 rad/s and falls back under the stall threshold within about 0.4 s of the fingers stopping. Any commanded speed stays far above the threshold.on_activateresets the estimator, so the calibration sweep and the settled starting position don't count as motion.stall_timeoutgoes from 0.05 s to 1.0 s, in both controller configs. The controller starts that clock when it accepts the goal, but the first position count from the gripper arrives 50 to 200 ms later (Modbus round trip plus motor start). At 0.05 s, about one close in ten still returned early. At 0.3 s, the slow final approach still tripped it once in twenty moves: near the end, position counts come more than 0.4 s apart and the estimate decays under the threshold in between. With 1.0 s, a real stop is reported about 1.4 s after the fingers stop (the estimate's decay plus the timeout).What
hardware_interface.hpp/.cpp: aRobotiq::VelocityEstimatormember with a 100 ms time constant, fed inread(), reset inon_activate.robotiq_controllers.yamlandrobotiq_controllers.humble.yaml:stall_timeout: 1.0.extern/grippers: moved to the head of feat: VelocityEstimator, a filtered rate from successive positions grippers#42 (7f5a6a0), which sits on the commit already pinned.Before merge
Draft until robotiq/grippers#42 merges. After it does,
extern/grippersgets re-pinned to that merge commit, not the PR head.Verified
Bench, real 2F-85 with TSF fingers, the MCP train on top: open and close report
reachedwith the fingers at their stops, a close on a block reportsstopped_on_objectat the block's width, and the 20-cycle open/close run (40 moves) had no wrong result.