From 6289e37755542764850791234d1fad73888df2e6 Mon Sep 17 00:00:00 2001 From: Jordan Longval Date: Tue, 22 Sep 2026 15:59:57 -0400 Subject: [PATCH] fix(mcp): take the achieved position from the controller's result, and 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 Claude-Session: https://claude.ai/code/session_01W8KnAMvo3aZyPWCQ886WBT --- mcp/gripper_mcp/ros_messages.py | 2 ++ mcp/gripper_mcp/service.py | 2 ++ mcp/tests/test_ros_messages.py | 12 ++++++++++++ mcp/tests/test_service.py | 2 ++ 4 files changed, 18 insertions(+) diff --git a/mcp/gripper_mcp/ros_messages.py b/mcp/gripper_mcp/ros_messages.py index abfd590..74fc543 100644 --- a/mcp/gripper_mcp/ros_messages.py +++ b/mcp/gripper_mcp/ros_messages.py @@ -105,6 +105,8 @@ def advertised_type(names_and_types, action_name: str, known: dict) -> str | Non def position_of(state, joint: str, fallback: float) -> float: names = list(state.name) + if not names and len(state.position) == 1: + return state.position[0] if joint not in names: return fallback return state.position[names.index(joint)] diff --git a/mcp/gripper_mcp/service.py b/mcp/gripper_mcp/service.py index d3b2871..2670377 100644 --- a/mcp/gripper_mcp/service.py +++ b/mcp/gripper_mcp/service.py @@ -219,6 +219,8 @@ def classify( return "refused" if motion.timed_out: return "incomplete" + if motion.reached_goal: + return "reached" if stopped_on_something(commanded_mm, achieved_mm, stroke): return "stopped_on_object" return "reached" diff --git a/mcp/tests/test_ros_messages.py b/mcp/tests/test_ros_messages.py index 8a68517..d0c31ee 100644 --- a/mcp/tests/test_ros_messages.py +++ b/mcp/tests/test_ros_messages.py @@ -116,6 +116,18 @@ def test_a_state_without_the_joint_yields_the_fallback(): assert position_of(state, "knuckle_joint", FALLBACK_RAD) == FALLBACK_RAD +def test_a_result_naming_no_joint_but_carrying_one_position_is_that_joint(): + state = FakeState([], [KNUCKLE_RAD]) + + assert position_of(state, "knuckle_joint", FALLBACK_RAD) == KNUCKLE_RAD + + +def test_a_result_naming_no_joint_with_no_position_yields_the_fallback(): + state = FakeState([], []) + + assert position_of(state, "knuckle_joint", FALLBACK_RAD) == FALLBACK_RAD + + class FakeCancelResponse: def __init__(self, return_code): self.return_code = return_code diff --git a/mcp/tests/test_service.py b/mcp/tests/test_service.py index d930417..2770224 100644 --- a/mcp/tests/test_service.py +++ b/mcp/tests/test_service.py @@ -223,6 +223,7 @@ def test_each_gripper_opens_to_its_own_model_width(): (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"), ], ids=[ "refused", @@ -233,6 +234,7 @@ def test_each_gripper_opens_to_its_own_model_width(): "open blocked part-way", "move landed", "move landed but the driver says stalled (#29)", + "the controller reached within its own tolerance, wider than ours", ], ) def test_classify_decides_from_position_not_the_stall_flag(