fix(mcp): take the achieved position from the controller's result - #91
jlongvalRobotiq wants to merge 1 commit into
Conversation
…d its verdict parallel_gripper_action_controller fills `result.state.position` but never `result.state.name`, so the lookup by joint name always missed and the achieved position came from the MCP's own latest `joint_states` sample instead. At 500 Hz that subscription runs behind the controller: on the bench (real 2F-85, driver with a working velocity estimate) one open in five reported 82.4 mm and "stopped on an object" while the controller had succeeded on 83.9 mm, the next Modbus reading, and the fingers rested at 85.0 mm a second later. A result that names no joint and carries exactly one position is that joint's. classify also trusts `reached_goal` before comparing positions: the controller's goal tolerance (0.02 rad, about 2.1 mm) is wider than the datasheet's closed tolerance (1.5 mm), so a move the controller finished could still be called an object by the MCP. The position comparison remains the verdict when the controller did not reach, which is where the stall flag used to lie (ros#29). Found on row 4 of the acceptance matrix, pathway B (bench), 20 open/close cycles with nothing between the fingers: 5 wrong results before, 0 after. Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01W8KnAMvo3aZyPWCQ886WBT
1fa86a5 to
6289e37
Compare
ebarnett3
left a comment
ebarnett3
left a comment
There was a problem hiding this comment.
Approve with one doc fix before merge. Both code changes are correct and match the upstream controller: parallel_gripper_action_controller_impl.hpp (jazzy) does state.position.resize(1) and writes state.position[0] on reach and on stall, and never touches state.name. A canceled goal gets an empty Result(), so the fallback still runs there and the new rule does not read a stale position. The reached_goal short-circuit is sound: the controller only sets it inside goal_tolerance, and both allow_stalling configs leave reached_goal=false on a stall, so the position comparison still decides the object case. Tests cover both new branches.
Findings and nits are inline.
Shelved (pre-existing, not for this PR)
mcp/gripper_mcp/ros_backend.py:326-336— theGripperCommandbranches in_goal/_result_positionare Humble-only (Humble keeps PickNik's action) and carry noHumble EOL:tag.mcp/gripper_mcp/ros_backend.py:208—read_statefalls back to0.0rad (fully open) when the joint is missing from a freshjoint_states; a raise would be safer than a silent "open".
| return "refused" | ||
| if motion.timed_out: | ||
| return "incomplete" | ||
| if motion.reached_goal: |
There was a problem hiding this comment.
1. Low — service.py module docstring now contradicts classify (service.py:15-21, outside the diff)
mcp/gripper_mcp/service.py:15-21. The paragraph still says the verdict is read "from where the fingers ended up against where they were sent" and that the controller's flags do "not steer the outcome". After this PR reached_goal steers first. Rewrite the paragraph to the new rule, e.g.: "The controller's reached_goal is the verdict when it is set: its goal tolerance is wider than the datasheet's closed tolerance, so the position comparison would otherwise call a finished move an object. When it is not set, the verdict is read from where the fingers ended up against where they were sent, never from the stalled flag (#29: on the real driver that flag trips on every goal), which is passed through to the caller as-is." Keep the gOBJ stopgap sentence. Since #92 fixes the "no velocity is ever computed" cause, prefer wording that does not assert it as current fact.
| if not names and len(state.position) == 1: | ||
| return state.position[0] |
There was a problem hiding this comment.
2. Low — the unnamed-position rule is the action result's contract, but lives in the joint_states lookup
mcp/gripper_mcp/ros_messages.py:108-109. position_of is also called on joint_states samples (ros_backend.py:208, :283). A JointState on that topic always names its joints, so the branch is dead there and reads as a generic heuristic. Nothing says why it exists. Either add a one-line why (# parallel_gripper_action_controller fills result.state.position but never result.state.name) or split it out so the rule sits on the result path only:
def result_position_of(state, joint: str, fallback: float) -> float:
if not state.name:
return state.position[0] if len(state.position) == 1 else fallback
return position_of(state, joint, fallback)and call it from _result_position (ros_backend.py:334); move the two new tests with it.
| return "incomplete" | ||
| if motion.reached_goal: | ||
| return "reached" | ||
| if stopped_on_something(commanded_mm, achieved_mm, stroke): |
There was a problem hiding this comment.
3. Low — the object-detection floor is now the controller's goal_tolerance, not closed_tolerance_mm
mcp/gripper_mcp/service.py:222 with grippers/robotiq_description/config/robotiq_controllers*.yaml (goal_tolerance: 0.02 rad ≈ 2.1 mm). A stop within 2.1 mm of the target is reached; closed_tolerance_mm (1.5 mm) only matters when the controller did not reach. Not a regression (a 1 mm stop was already reached), and the PR body says so, but the datasheet yaml now documents a tolerance that is not the effective one. A sentence in the docstring or the datasheet comment is enough for this PR.
Don't align the two numbers: the position comparison itself is on its way out. Once the controller takes stall and reached from the gripper's object status (#96, follow-up to #92), reached_goal means "at requested position" and stalled means "object detected" straight from the firmware, and classify should trust both flags and drop closed_tolerance_mm and the #29 rule. That is the consumer-side half of that issue, not a change here.
| "the controller reached within its own tolerance, wider than ours", | ||
| ], | ||
| ) | ||
| def test_classify_decides_from_position_not_the_stall_flag( |
There was a problem hiding this comment.
Nit (test_service.py:242): mcp/tests/test_service.py:242 — test_classify_decides_from_position_not_the_stall_flag now holds a row decided by reached_goal, not position. Rename to test_classify_trusts_reached_goal_then_position or move the new row to its own test.
| (motion(reached_goal=False, stalled=True), 85.0, 49.0, "stopped_on_object"), | ||
| (motion(reached_goal=True), 30.0, 30.0, "reached"), | ||
| (motion(reached_goal=False, stalled=True), 30.0, 30.0, "reached"), | ||
| (motion(reached_goal=True), 85.0, 83.1, "reached"), |
There was a problem hiding this comment.
Nit: mcp/tests/test_service.py:226 — the new row's id, "the controller reached within its own tolerance, wider than ours", is the clearest statement of the rule in the PR; the docstring fix in finding 1 could reuse it.
2 participants
Where this comes from
Before merging
feat/mcpintomain, we ran a full QA pass of the gripper MCP server by hand, on a real 2F-85 on the bench, with Claude Code as the client. #93 lists every test. The runs on fake hardware found the three bugs fixed in #84, #85 and #86, now merged intofeat/mcp. This PR fixes the first problem found on the real gripper.The test that found it: 20 open/close cycles with nothing between the fingers. Expected: every result
reached. Observed: about one open in five came backstopped_on_objectat 82.4 mm, while the controller had reported success at 83.9 mm and the fingers were resting at 85.0 mm a second later.Why
The MCP took the achieved position from the controller's result, and fell back to its own latest
joint_statessample when the result named no joint.parallel_gripper_action_controllerfillsresult.state.positionbut neverresult.state.name, so the lookup always missed and the fallback always ran. At 500 Hz the Python subscription runs behind the controller, so the fallback read a position from partway through the move, and a move that had finished was called an object.Once the position was right, a second disagreement showed up. The controller's goal tolerance is 0.02 rad, about 2.1 mm, wider than the datasheet's 1.5 mm closed tolerance. So the controller could report
reached_goaland the MCP could still say "stopped on an object" on the same move. When the controller reached its goal, that verdict wins. When it didn't, the position comparison decides, which is the case where the stall flag used to be wrong (#29).What
ros_messages.py:position_oftakes the single position of a result that names no joint as that joint's.service.py:classifyreturnsreachedwhen the controller reportsreached_goal, before it compares positions.reachedeven when the position is outside the MCP's tolerance.Verified
uv run pytest: 198 passed.stopped_on_objectat 26.6 mm, held).