Skip to content

fix(driver): estimate joint velocity so stall detection works - #92

Draft
jlongvalRobotiq wants to merge 1 commit into
mainfrom
fix/ros-29-filtered-velocity
Draft

jlongvalRobotiq wants to merge 1 commit into
mainfrom
fix/ros-29-filtered-velocity

Conversation

@jlongvalRobotiq

Copy link
Copy Markdown
Collaborator

Fixes #29.

Where this comes from

The gripper MCP (mcp/, on feat/mcp) is going through an acceptance matrix before its merge to main, 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 back stalled, 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 against main, not into the MCP stack.

Why

The driver's velocity state interface was always 0.0. parallel_gripper_action_controller declares a stall when velocity stays below 0.001 rad/s for stall_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's VelocityEstimator (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_activate resets the estimator, so the calibration sweep and the settled starting position don't count as motion.

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, 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

Before merge

Draft until robotiq/grippers#42 merges. After it does, extern/grippers gets 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 reached with the fingers at their stops, a close on a block reports stopped_on_object at the block's width, and the 20-cycle open/close run (40 moves) had no wrong result.

… 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
@ebarnett3

Copy link
Copy Markdown
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 joint_states velocity at low priority, the branch stays for when it is picked up, and it would need re-pinning to robotiq/grippers#42's successor then.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

robotiq_driver: velocity state is always 0.0

2 participants