From 11e1d25a95fc98d57f6488d30bba3e0f9abadb82 Mon Sep 17 00:00:00 2001 From: miike Date: Wed, 2 Sep 2026 20:18:31 -0400 Subject: [PATCH 1/6] PreciseFlex: split setup() into connect(), initialize() and disconnect() setup() did four things behind one name: open the socket, agree the response mode, raise high power and attach, then home. A caller that wants to read a position, or to reconnect after a controller restart, had no way to ask for part of that. connect() opens the link and sets the response mode. initialize() raises power, attaches, leaves freedrive and reads the controller's configuration. disconnect() detaches, drops power and closes the link. Neither connect nor initialize moves the arm; home() is still the only verb that sweeps it, and setup() still calls all of them in order, so existing callers are unaffected. Configuration discovery moves into _discover_configuration() and stays best-effort. Because it can fail, has_configuration says whether the arm actually read its own limits, which a caller that would rather adapt than be raised at can now check. --- .../brooks/precise_flex/precise_flex.py | 51 +++++-- .../precise_flex/tests/precise_flex_tests.py | 137 ++++++++++++++++++ 2 files changed, 177 insertions(+), 11 deletions(-) diff --git a/pylabrobot/brooks/precise_flex/precise_flex.py b/pylabrobot/brooks/precise_flex/precise_flex.py index 63da4884488..01e7c579e13 100644 --- a/pylabrobot/brooks/precise_flex/precise_flex.py +++ b/pylabrobot/brooks/precise_flex/precise_flex.py @@ -277,25 +277,39 @@ def _parse_reply_ensure_successful(self, reply: bytes) -> str: }, ) async def setup(self, skip_home: bool = False): - """Initialize the PreciseFlex driver. - - Opens the socket connection, sets response mode to PC, powers on the - robot, attaches it, and (optionally) homes it. + """Bring the arm fully up: link, control, and (unless skipped) home. Args: skip_home: If True, skip the homing step during setup. """ - await self.io.setup() - await self.set_response_mode("pc") - await self.power_on_robot() - await self.attach(1) + await self.connect() + await self.initialize() if not skip_home: await self.home() + await self._handle_out_of_range_axes() + + async def connect(self) -> None: + """Open the link and agree the response protocol. Powers nothing, moves nothing.""" + await self.io.setup() + await self.set_response_mode("pc") logger.debug("[PreciseFlex %s] connected: port=%s", self.io._host, self.io._port) + async def initialize(self) -> None: + """Raise high power, take control, and adopt the controller's own configuration. + + Moves nothing. Homing is ``home()``, deliberately separate: it sweeps the arm + through its whole envelope, which is not something to do just to bring it up. + """ + await self.power_on_robot() + await self.attach(1) await self.stop_freedrive_mode() - # Resolve the device configuration once and adopt it as the source of truth; - # without it the class defaults stay in place. + await self._discover_configuration() + + async def _discover_configuration(self) -> None: + """Adopt what the controller reports, so the class defaults are not used blind. + + The link lengths land here, so skipping this leaves IK solving for the wrong arm. + """ try: self._configuration = await self._request_configuration() except Exception as exc: # discovery is best-effort @@ -310,7 +324,6 @@ async def setup(self, skip_home: bool = False): self.parking_position = self.PARKING_POSITION_RIGHT self._log_configuration_summary(self._configuration) self._assess_configuration(self._configuration) - await self._handle_out_of_range_axes() @evented_operation( "precise_flex.stop", @@ -318,6 +331,13 @@ async def setup(self, skip_home: bool = False): ) async def stop(self): """Stop the PreciseFlex driver.""" + await self.disconnect() + + async def disconnect(self) -> None: + """Hand the arm back and close the link. Moves nothing. + + Drops high power as well as releasing the link, because ``initialize`` raised it. + """ await self.detach() await self.power_off_robot() await self._exit() @@ -1621,6 +1641,15 @@ def configuration(self) -> "PreciseFlexConfiguration": raise RuntimeError("Configuration is not available until setup() has run.") return self._configuration + @property + def has_configuration(self) -> bool: + """Whether the controller's configuration was actually read. + + Discovery is best-effort, so an arm can finish setup and still not know its own + limits. A caller that would rather adapt than be raised at asks this first. + """ + return self._configuration is not None + async def _request_configuration(self) -> "PreciseFlexConfiguration": """Read the controller's identity, axes, limits, kinematics, and envelope. diff --git a/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py b/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py index 907aa07357f..c30c4f0af16 100644 --- a/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py +++ b/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py @@ -521,3 +521,140 @@ async def test_move_to_location_is_also_guarded(self): await self.arm.move_to_location(Coordinate(400.0, 0.0, 200.0), 0.0) self.assertIn(Axis.SHOULDER, ctx.exception.axes) self.assertEqual(self._cmds("moveJ"), []) + + +def _make_linked_arm() -> PreciseFlex: + """An arm whose socket is stubbed too, for asserting on the bring-up sequence.""" + arm = _make_arm() + arm.io = MagicMock() + arm.io.setup = AsyncMock() + arm.io.stop = AsyncMock() + arm.io.write = AsyncMock() + arm.io._host = "localhost" + arm.io._port = 10100 + return arm + + +class TestPreciseFlexLifecycle(unittest.IsolatedAsyncioTestCase): + """Opening the link, taking control, and homing are three separate verbs. + + A caller that only wants to read a position can connect and initialize without + the arm ever moving; only ``home`` sweeps it. + """ + + def setUp(self): + self.arm = _make_linked_arm() + + def _sent(self) -> list[str]: + return [c.args[0] for c in mocked(self.arm.send_command).call_args_list] + + def _assert_moved_nothing(self): + for command in self._sent(): + verb = command.split()[0].lower() + self.assertNotIn( + verb, + ("home", "homeall", "movej", "movec", "moveoneaxis", "gripper"), + f"bring-up must not move the arm, but it sent {command!r}", + ) + + async def test_connect_opens_the_link_and_agrees_the_protocol(self): + await self.arm.connect() + mocked(self.arm.io.setup).assert_awaited_once() + self.assertEqual(self._sent(), ["mode 0"]) + + async def test_connect_does_not_raise_power(self): + await self.arm.connect() + self.assertNotIn("hp 1", self._sent()) + self._assert_moved_nothing() + + async def test_an_arm_whose_discovery_failed_says_it_has_no_configuration(self): + """Discovery is best-effort, so bring-up succeeding is not proof the arm knows its + own limits, and a caller above has no other way to tell the two apart.""" + self.arm._request_configuration = AsyncMock(side_effect=RuntimeError("no controller")) + + await self.arm.initialize() + + self.assertFalse(self.arm.has_configuration) + with self.assertRaises(RuntimeError): + self.arm.configuration + + async def test_initialize_takes_control_without_moving(self): + self.arm._request_configuration = AsyncMock(side_effect=RuntimeError("no controller")) + await self.arm.initialize() + sent = self._sent() + self.assertIn("attach 1", sent) + self.assertIn("freemode -1", sent) + self.assertTrue(any(c.startswith("hp 1") for c in sent), sent) + self._assert_moved_nothing() + + async def test_initialize_adopts_what_the_controller_reports(self): + # The link lengths ride on this: without it the arm solves IK for a different machine. + discovered = MagicMock() + discovered.soft_limits = { + Axis.SHOULDER: (-93.0, 93.0), + Axis.ELBOW: (12.0, 348.0), + Axis.WRIST: (-960.0, 960.0), + } + self.arm._request_configuration = AsyncMock(return_value=discovered) + self.arm._adopt_configuration = MagicMock() + self.arm._log_configuration_summary = MagicMock() + self.arm._assess_configuration = MagicMock() + + await self.arm.initialize() + + self.arm._adopt_configuration.assert_called_once_with(discovered) + self.assertTrue(self.arm.has_configuration) + + async def test_initialize_falls_back_to_defaults_when_discovery_fails(self): + self.arm._request_configuration = AsyncMock(side_effect=RuntimeError("no controller")) + self.arm._adopt_configuration = MagicMock() + + await self.arm.initialize() + + self.arm._adopt_configuration.assert_not_called() + + async def test_disconnect_hands_the_arm_back_and_closes_the_link(self): + await self.arm.disconnect() + sent = self._sent() + self.assertIn("attach 0", sent) + self.assertIn("hp 0", sent) + mocked(self.arm.io.write).assert_awaited_once_with(b"exit\n") + mocked(self.arm.io.stop).assert_awaited_once() + + async def test_setup_connects_then_initializes_then_homes_in_that_order(self): + calls: list[str] = [] + self.arm.connect = AsyncMock(side_effect=lambda: calls.append("connect")) + self.arm.initialize = AsyncMock(side_effect=lambda: calls.append("initialize")) + self.arm.home = AsyncMock(side_effect=lambda: calls.append("home")) + self.arm._handle_out_of_range_axes = AsyncMock() + + await self.arm.setup() + + self.assertEqual(calls, ["connect", "initialize", "home"]) + + async def test_setup_skip_home_brings_the_arm_up_without_sweeping_it(self): + self.arm.connect = AsyncMock() + self.arm.initialize = AsyncMock() + self.arm.home = AsyncMock() + self.arm._handle_out_of_range_axes = AsyncMock() + + await self.arm.setup(skip_home=True) + + mocked(self.arm.home).assert_not_awaited() + + async def test_setup_still_checks_soft_limits_when_discovery_fails(self): + # Discovery is best-effort, but losing it must not silently skip the + # out-of-range recovery that makes an unusable arm usable again. + self.arm.connect = AsyncMock() + self.arm.home = AsyncMock() + self.arm._request_configuration = AsyncMock(side_effect=RuntimeError("no controller")) + self.arm._handle_out_of_range_axes = AsyncMock() + + await self.arm.setup() + + mocked(self.arm._handle_out_of_range_axes).assert_awaited_once() + + async def test_stop_is_disconnect(self): + self.arm.disconnect = AsyncMock() + await self.arm.stop() + mocked(self.arm.disconnect).assert_awaited_once() From 04a0daff6c72c51051bde518555a98e8bbe80512 Mon Sep 17 00:00:00 2001 From: miike Date: Wed, 2 Sep 2026 20:19:46 -0400 Subject: [PATCH 2/6] PreciseFlex: wait for the arm to stop before commanding the gripper or the rail moveJ returns as soon as the controller accepts it, not when the arm arrives, and the connection stays free during travel so the next command is sent immediately. Joint and Cartesian moves already wait behind _wait_for_eom via _guarded_move_j, but move_gripper, move_gripper_joint_position and move_rail did not, so each could act while the arm was still moving. A grip issued after an approach closes the jaws on the way down onto the plate. All three now wait first. _wait_for_eom's docstring says it is the barrier every motion-starting command crosses, and which commands reach it directly rather than through the guarded move. --- .../brooks/precise_flex/precise_flex.py | 9 ++++ .../precise_flex/tests/precise_flex_tests.py | 49 +++++++++++++++++-- 2 files changed, 53 insertions(+), 5 deletions(-) diff --git a/pylabrobot/brooks/precise_flex/precise_flex.py b/pylabrobot/brooks/precise_flex/precise_flex.py index 01e7c579e13..570ed4a67f5 100644 --- a/pylabrobot/brooks/precise_flex/precise_flex.py +++ b/pylabrobot/brooks/precise_flex/precise_flex.py @@ -539,6 +539,12 @@ async def _wait_for_eom( samples, so a user interrupt can stop the move mid-flight via ``halt`` and other controller commands (status, vision, barcode) can run during motion. + That free connection is also the hazard, so this is the barrier every command that *starts* + motion crosses first: the gripper and the rail call it directly, joint and Cartesian moves reach + it through ``request_joint_position`` inside ``_guarded_move_j``. ``moveJ`` returns as soon as + the controller accepts it, so without the barrier the next command lands mid-travel - a grip + issued after the approach closes the jaws while the arm is still descending onto the plate. + Raises: TimeoutError: if the arm never settles within ``timeout`` seconds. OperationInterrupted: on a user interrupt (the arm is halted and the connection kept). @@ -2394,6 +2400,7 @@ async def move_gripper( f"axis range [{self._gripper_soft_min}, {self._gripper_soft_max}] - check " f"closed_gripper_position (currently {self.closed_gripper_position})." ) + await self._wait_for_eom() if force_sensing: await self._set_grip_close_pos(units) await self.send_command("gripper 2") @@ -2419,6 +2426,7 @@ async def move_gripper_joint_position( This is the counterpart to :meth:`move_gripper` for integrations with taught joint-space routes. The caller owns the joint calibration. """ + await self._wait_for_eom() if force_sensing: await self._set_grip_close_pos(position) await self.send_command("gripper 2") @@ -2467,6 +2475,7 @@ async def move_rail(self, rail_position: float) -> None: """ if not self._has_rail: raise RuntimeError("This arm does not have a rail.") + await self._wait_for_eom() await self._set_rail_position(self._rail_position_index, rail_position) await self._move_rail(station_id=self._rail_position_index) diff --git a/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py b/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py index c30c4f0af16..79b5ea48ad7 100644 --- a/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py +++ b/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py @@ -22,14 +22,24 @@ def mocked(method: object) -> AsyncMock: def _make_arm(closed_gripper_position: float = 500.0) -> PreciseFlex: - """An arm whose transport is stubbed out, so tests assert on the commands it would send.""" + """An arm whose transport is stubbed out, so tests assert on the commands it would send. + + ``wherej`` answers with a steady pose, which is what the settle poll every motion + command waits behind reads. + """ arm = PreciseFlex( host="localhost", gripper_length=162.0, gripper_z_offset=0.0, closed_gripper_position=closed_gripper_position, ) - arm.send_command = AsyncMock(return_value="") # type: ignore[method-assign] + + async def reply(command: str) -> str: + if command == "wherej": + return "40.48 84.76 229.84 -312.57 503.0" + return "" + + arm.send_command = AsyncMock(side_effect=reply) # type: ignore[method-assign] return arm @@ -44,12 +54,12 @@ def _sent_commands(self) -> list[str]: async def test_move_gripper_force_sensing_false_opens_with_position(self): # 80 mm ⇒ 500 + (80 - 60) = 520 firmware units. await self.arm.move_gripper(width=80.0, force_sensing=False) - self.assertEqual(self._sent_commands(), ["GripOpenPos 520.0", "gripper 1"]) + self.assertEqual(self._sent_commands()[-2:], ["GripOpenPos 520.0", "gripper 1"]) async def test_move_gripper_force_sensing_true_closes_with_position(self): # 60 mm (the closed reference) ⇒ exactly closed_gripper_position. await self.arm.move_gripper(width=60.0, force_sensing=True) - self.assertEqual(self._sent_commands(), ["GripClosePos 500.0", "gripper 2"]) + self.assertEqual(self._sent_commands()[-2:], ["GripClosePos 500.0", "gripper 2"]) async def test_move_gripper_position_command_precedes_move(self): await self.arm.move_gripper(width=120.0, force_sensing=False) @@ -79,7 +89,7 @@ async def test_closed_gripper_position_shifts_units(self): await arm.move_gripper(width=80.0, force_sensing=False) commands = [c.args[0] for c in mocked(arm.send_command).call_args_list] # 80 mm ⇒ 1000 + (80 - 60) = 1020 units. - self.assertEqual(commands, ["GripOpenPos 1020.0", "gripper 1"]) + self.assertEqual(commands[-2:], ["GripOpenPos 1020.0", "gripper 1"]) def test_mm_to_firmware_units_helper(self): # Direct check of the linear mapping. @@ -138,6 +148,8 @@ async def test_gripper_event_and_nested_firmware_commands_inherit_resource_conte self.assertEqual( [event.data["command"] for event in firmware_events], [ + "wherej", # the settle poll the jaws wait behind + "wherej", "GripOpenPos 520.0", "gripper 1", ], @@ -658,3 +670,30 @@ async def test_stop_is_disconnect(self): self.arm.disconnect = AsyncMock() await self.arm.stop() mocked(self.arm.disconnect).assert_awaited_once() + + +class TestMotionSettlesBeforeTheNextCommand(unittest.IsolatedAsyncioTestCase): + """``moveJ`` returns when the controller accepts it, not when the arm arrives. + + Without a settle poll in front of them, the gripper and the rail act while the arm is + still travelling: a grip issued after an approach closes the jaws on the way down. + """ + + def setUp(self): + self.arm = _make_arm() + + def _sent(self) -> list[str]: + return [c.args[0] for c in mocked(self.arm.send_command).call_args_list] + + async def test_the_jaws_wait_for_the_arm_to_stop(self): + await self.arm.move_gripper(width=80.0, force_sensing=True) + self.assertEqual(self._sent()[0], "wherej") + + async def test_a_joint_space_grip_waits_too(self): + await self.arm.move_gripper_joint_position(510.0, force_sensing=False) + self.assertEqual(self._sent()[0], "wherej") + + async def test_the_rail_waits_too(self): + self.arm._has_rail = True + await self.arm.move_rail(120.0) + self.assertEqual(self._sent()[0], "wherej") From 77008064053af9e38e6a8fbf93fec35079cb2d86 Mon Sep 17 00:00:00 2001 From: miike Date: Wed, 2 Sep 2026 20:20:44 -0400 Subject: [PATCH 3/6] PreciseFlex: keep gripper targets inside the axis, and anchor widths on the calibration pair Two faults on the same axis. Commanding the gripper past the end of its axis leaves a standing position error that the controller reports as an overheating motor (-3104), after which it refuses every move, gripper or not, until the arm is homed. It happened twice in one bench session: opening to the advertised maximum of 134.0 landed the axis at 134.062, because the servo overshoots. move_gripper and move_gripper_joint_position now hold every target half a unit inside the discovered soft limits via _within_gripper_limits, and gripper_joint_range exposes those limits so a caller above the driver can see the ceiling it must not compute past. Separately, setup() copied the discovered gripper-axis limits straight into min_gripper_width and max_gripper_width, which are millimetres. That put axis units in a millimetre field and silently changed what every width meant after discovery. Both ends now convert through the construction-time calibration pair, which _anchor_width_mm pins, so a width commands the same jaw travel before and after discovery. The range check gains an epsilon, since an advertised end converts back a few ulps outside the limit it came from. gripper_width_range on the configuration is typed and says it returns controller units, not millimetres. --- pylabrobot/brooks/precise_flex/config.py | 7 +- .../brooks/precise_flex/precise_flex.py | 101 ++++++++++++---- .../precise_flex/tests/precise_flex_tests.py | 110 ++++++++++++++++++ 3 files changed, 195 insertions(+), 23 deletions(-) diff --git a/pylabrobot/brooks/precise_flex/config.py b/pylabrobot/brooks/precise_flex/config.py index 70caecd7a0e..48607d9d7a7 100644 --- a/pylabrobot/brooks/precise_flex/config.py +++ b/pylabrobot/brooks/precise_flex/config.py @@ -81,7 +81,12 @@ class PreciseFlexConfiguration: reach_class: Literal["standard", "extended", "unknown"] = "extended" @property - def gripper_width_range(self) -> tuple: + def gripper_width_range(self) -> tuple[float, float]: + """Travel limits of the gripper axis, in the controller's own units. + + Not millimetres: a jaw width reaches these through + ``PreciseFlex.closed_gripper_position``, so callers must convert. + """ return self.soft_limits[Axis.GRIPPER] @property diff --git a/pylabrobot/brooks/precise_flex/precise_flex.py b/pylabrobot/brooks/precise_flex/precise_flex.py index 570ed4a67f5..4fb7c70bb37 100644 --- a/pylabrobot/brooks/precise_flex/precise_flex.py +++ b/pylabrobot/brooks/precise_flex/precise_flex.py @@ -29,6 +29,11 @@ logger = logging.getLogger(__name__) +# Float dust allowed when a converted jaw width is compared with the axis limit it +# was derived from. +_GRIPPER_UNIT_EPS = 1e-6 +_GRIPPER_LIMIT_HEADROOM = 0.5 + # InRange sentinel that lets the controller blend through waypoints instead of stopping at each one. BLEND_IN_RANGE = -1 @@ -188,9 +193,11 @@ def __init__( Every recovery is logged. closed_gripper_position: firmware-unit value (passed to ``GripClosePos`` / ``GripOpenPos``) at which the jaws are at :attr:`min_gripper_width`. - Depends on the mounted gripper. The conversion mm → firmware units is - linear with slope 1: ``units = closed_gripper_position + (width_mm - - min_gripper_width)``. + Depends on the mounted gripper. That pairing is the anchor for the + mm → firmware unit conversion, which is linear with slope 1, and it is + frozen at construction: setup discovers the axis limits and rewrites + :attr:`min_gripper_width`, so reading it later would move what a width + means. parking_position: initial value for the public, runtime-settable ``parking_position`` that ``park()`` moves to. Leave None (the default) and setup fills the generic default RIGHT pose (planar fold, Z column at 3/4 of the discovered travel); reassign it any time to park @@ -207,6 +214,9 @@ def __init__( self._has_rail = has_rail self._is_dual_gripper = is_dual_gripper self.closed_gripper_position = closed_gripper_position + # closed_gripper_position was calibrated against whatever min_gripper_width read + # when it was measured, so that width is the anchor and discovery must not move it. + self._anchor_width_mm = self.min_gripper_width self._kinematics_params = kinematics.PF400Params( gripper_length=gripper_length, gripper_z_offset=gripper_z_offset ) @@ -1385,17 +1395,17 @@ async def _set_grasp_data( raise ValueError(f"finger_speed_pct must be between 0 and 100, got {finger_speed_pct}") await self.send_command(f"GraspData {plate_width} {finger_speed_pct} {grasp_force}") - async def _set_grip_detail(self): + async def _set_grip_detail(self) -> None: """Configure a default vertical station type for pick/place operations.""" await self.send_command(f"StationType {self.location_index} 1 0 100 0 10") def _mm_to_firmware_units(self, width_mm: float) -> float: """Convert a jaw width (mm) to the firmware's native position unit. - Anchored at :attr:`closed_gripper_position`, which is the firmware value - when the jaws are at :attr:`min_gripper_width`. Slope is 1 (1 mm = 1 unit). + Anchored on the construction-time calibration pair, so a given width commands the + same jaw travel whether or not setup has discovered the axis limits. Slope is 1. """ - return self.closed_gripper_position + (width_mm - self.min_gripper_width) + return self.closed_gripper_position + (width_mm - self._anchor_width_mm) # -- rail primitives ---------------------------------------------------------------------- @@ -1764,7 +1774,10 @@ def _adopt_configuration(self, config: "PreciseFlexConfiguration") -> None: """ gmin, gmax = config.gripper_width_range self._gripper_soft_min, self._gripper_soft_max = gmin, gmax - self.min_gripper_width, self.max_gripper_width = gmin, gmax + # The limits are gripper-axis units. Both ends convert through the anchor: copying + # them in would put units in a millimetre field and change what a width means. + self.min_gripper_width = self._anchor_width_mm + (gmin - self.closed_gripper_position) + self.max_gripper_width = self._anchor_width_mm + (gmax - self.closed_gripper_position) self._kinematics_params = config.kinematics self._has_rail = config.has_rail self._is_dual_gripper = config.is_dual_gripper @@ -2351,8 +2364,8 @@ async def here_c(self, location_index: int) -> None: # -- gripper ------------------------------------------------------------------------------ - # Physical jaw range for the PF400 servoed gripper. Overridden at setup from the - # gripper-axis soft limits (DataIDs 16078/16077, Axis.GRIPPER) when discoverable. + # Physical jaw range for the PF400 servoed gripper. The minimum doubles as the + # calibration anchor for closed_gripper_position; setup converts both ends off it. min_gripper_width: float = 60.0 max_gripper_width: float = 145.0 # Gripper-axis soft limits (GripOpenPos/GripClosePos units), read at setup; None until then. @@ -2375,8 +2388,11 @@ async def move_gripper( """Move the PreciseFlex gripper jaws. ``force_sensing=False`` drives to the open position (``gripper 1``); - ``force_sensing=True`` drives to the close position with force feedback - (``gripper 2``), which may stop short of ``width`` on contact. + ``force_sensing=True`` drives to the close position (``gripper 2``). Both are + position moves: ``gripper 2`` limits the force it applies getting there, but it + does not stop the jaws where they meet something. Command the width a held + object wants, not a tighter one. Commanding past a held object leaves a standing + position error the controller reports as an overheating motor (-3104). Not interruptible: the ``gripper`` firmware command blocks the controller's command interpreter until the jaws finish (hardware-verified, like ``waitForEom``), so a user interrupt cannot halt it @@ -2390,16 +2406,20 @@ async def move_gripper( force_sensing, ) units = self._mm_to_firmware_units(width) - if ( - self._gripper_soft_min is not None - and self._gripper_soft_max is not None - and not (self._gripper_soft_min <= units <= self._gripper_soft_max) - ): - raise ValueError( - f"gripper width {width} mm maps to firmware units {units:.1f}, outside the gripper " - f"axis range [{self._gripper_soft_min}, {self._gripper_soft_max}] - check " - f"closed_gripper_position (currently {self.closed_gripper_position})." - ) + if self._gripper_soft_min is not None and self._gripper_soft_max is not None: + # An advertised end converts back through a subtract and an add, so it can land a + # few ulps outside the limit it was derived from. That is dust, not out of range. + if not ( + self._gripper_soft_min - _GRIPPER_UNIT_EPS + <= units + <= self._gripper_soft_max + _GRIPPER_UNIT_EPS + ): + raise ValueError( + f"gripper width {width} mm maps to firmware units {units:.1f}, outside the gripper " + f"axis range [{self._gripper_soft_min}, {self._gripper_soft_max}] - check " + f"closed_gripper_position (currently {self.closed_gripper_position})." + ) + units = self._within_gripper_limits(units) await self._wait_for_eom() if force_sensing: await self._set_grip_close_pos(units) @@ -2425,7 +2445,15 @@ async def move_gripper_joint_position( This is the counterpart to :meth:`move_gripper` for integrations with taught joint-space routes. The caller owns the joint calibration. + + A target outside the axis is held to the nearest end rather than refused, + which is the opposite of :meth:`move_gripper`. A width in mm that lands + out of range means ``closed_gripper_position`` is wrong, which is worth + raising on; a taught joint position that does is a route reaching a little + past the stop, and holding it there is what the route wanted. Either way + the arm never receives a target past the end, which is what strands it. """ + position = self._within_gripper_limits(position) await self._wait_for_eom() if force_sensing: await self._set_grip_close_pos(position) @@ -2434,6 +2462,35 @@ async def move_gripper_joint_position( await self._set_grip_open_pos(position) await self.send_command("gripper 1") + def _within_gripper_limits(self, units: float) -> float: + """A gripper target held a little inside the axis' soft limits. + + Both ends are adopted together off one discovered tuple, so the arm has either + read its limits or read neither, and before setup there is nothing to hold to. + """ + if self._gripper_soft_min is None or self._gripper_soft_max is None: + return units + low = self._gripper_soft_min + _GRIPPER_LIMIT_HEADROOM + high = self._gripper_soft_max - _GRIPPER_LIMIT_HEADROOM + held = min(max(units, low), high) + if held != units: + logger.warning( + "[PreciseFlex %s] gripper target %s held to %s, inside [%s, %s]", + self.io._host, + units, + held, + self._gripper_soft_min, + self._gripper_soft_max, + ) + return held + + @property + def gripper_joint_range(self) -> tuple[Optional[float], Optional[float]]: + """The gripper axis' soft limits in controller units, None at either end the arm + has not read. A jaw width in mm reaches these through ``closed_gripper_position``. + """ + return self._gripper_soft_min, self._gripper_soft_max + async def is_gripper_closed(self) -> bool: """(Single Gripper Only) Tests if the gripper is fully closed by checking the end-of-travel sensor. diff --git a/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py b/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py index 79b5ea48ad7..b1ea7323fce 100644 --- a/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py +++ b/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py @@ -2,6 +2,7 @@ from typing import cast from unittest.mock import AsyncMock, MagicMock, patch +from pylabrobot.brooks.precise_flex.precise_flex import _GRIPPER_LIMIT_HEADROOM from pylabrobot.brooks.precise_flex import ( Axis, OutOfRangeOfMotionError, @@ -697,3 +698,112 @@ async def test_the_rail_waits_too(self): self.arm._has_rail = True await self.arm.move_rail(120.0) self.assertEqual(self._sent()[0], "wherej") + + +class TestGripperTargetsStayInsideTheirLimits(unittest.IsolatedAsyncioTestCase): + """Commanding past the end of the gripper axis strands the arm: the controller then + refuses every move, gripper or not, until it is homed. No caller reaches the end.""" + + def setUp(self): + self.arm = _make_arm(closed_gripper_position=500.0) + self.arm._gripper_soft_min, self.arm._gripper_soft_max = 490.0, 560.0 + + def _sent_commands(self) -> list[str]: + return [c.args[0] for c in mocked(self.arm.send_command).call_args_list] + + async def test_the_jaws_never_open_past_the_axis_ceiling(self): + """Held short of the stop: the servo overshoots, and a target at the end lands + outside it, after which the controller refuses every move until the arm homes.""" + await self.arm.move_gripper_joint_position(999.0, force_sensing=False) + ceiling = 560.0 - _GRIPPER_LIMIT_HEADROOM + self.assertEqual(self._sent_commands()[-2:], [f"GripOpenPos {ceiling}", "gripper 1"]) + + async def test_the_jaws_never_close_below_the_axis_floor(self): + await self.arm.move_gripper_joint_position(0.0, force_sensing=True) + floor = 490.0 + _GRIPPER_LIMIT_HEADROOM + self.assertEqual(self._sent_commands()[-2:], [f"GripClosePos {floor}", "gripper 2"]) + + async def test_a_target_already_inside_the_limits_is_commanded_as_asked(self): + await self.arm.move_gripper_joint_position(520.0, force_sensing=True) + self.assertEqual(self._sent_commands()[-2:], ["GripClosePos 520.0", "gripper 2"]) + + def test_the_axis_limits_are_readable_without_reaching_into_the_arm(self): + """A composition above the driver has to be able to see the ceiling it must not + compute a target past, and it cannot read private state to find it.""" + self.assertEqual(self.arm.gripper_joint_range, (490.0, 560.0)) + + def test_an_arm_that_has_read_no_limits_reports_neither_end(self): + self.assertEqual(_make_arm().gripper_joint_range, (None, None)) + + async def test_an_arm_that_has_read_no_limits_commands_what_it_was_asked_for(self): + """Before discovery there is no ceiling to hold a target under, and inventing one + would refuse widths the fitted gripper reaches.""" + arm = _make_arm(closed_gripper_position=500.0) + + await arm.move_gripper_joint_position(999.0, force_sensing=False) + + sent = [c.args[0] for c in mocked(arm.send_command).call_args_list] + self.assertEqual(sent[-2:], ["GripOpenPos 999.0", "gripper 1"]) + + +class TestGripperWidthsAdoptedFromSoftLimits(unittest.IsolatedAsyncioTestCase): + """A width has to mean the same jaw travel before and after the axis limits are read. + + Bench numbers: the gripper axis reports [69.0, 134.0] and the fitted gripper closes at + 75.5. The two differ, which is the case that used to strand the top of the range and + shift every width underneath it. + """ + + AXIS_MIN, AXIS_MAX = 69.0, 134.0 + CLOSED_AT = 75.5 + + def setUp(self): + self.arm = _make_arm(closed_gripper_position=self.CLOSED_AT) + + def _discover(self, arm=None, limits=None): + discovered = MagicMock() + discovered.gripper_width_range = limits or (self.AXIS_MIN, self.AXIS_MAX) + discovered.has_rail = False + discovered.is_dual_gripper = False + (arm or self.arm)._adopt_configuration(discovered) + + def _sent(self, arm=None) -> list[str]: + return [c.args[0] for c in mocked((arm or self.arm).send_command).call_args_list] + + async def test_a_width_commands_the_same_travel_before_and_after_discovery(self): + await self.arm.move_gripper(80.0, force_sensing=False) + before = self._sent() + self._discover() + await self.arm.move_gripper(80.0, force_sensing=False) + after = self._sent()[len(before) :] + self.assertEqual( + [c for c in before if c.startswith("GripOpenPos")], + [c for c in after if c.startswith("GripOpenPos")], + "discovery moved what 80 mm means", + ) + + async def test_opening_to_the_advertised_max_stops_short_of_the_axis_ceiling(self): + """The servo overshoots, so a target at the stop lands past it and the controller + then refuses every move until the arm is homed.""" + self._discover() + await self.arm.move_gripper(self.arm.max_gripper_width, force_sensing=False) + self.assertIn(f"GripOpenPos {self.AXIS_MAX - _GRIPPER_LIMIT_HEADROOM}", self._sent()) + + async def test_closing_to_the_advertised_min_stops_above_the_axis_floor(self): + self._discover() + await self.arm.move_gripper(self.arm.min_gripper_width, force_sensing=True) + self.assertIn(f"GripClosePos {self.AXIS_MIN + _GRIPPER_LIMIT_HEADROOM}", self._sent()) + + async def test_an_advertised_end_survives_its_own_float_round_trip(self): + # Limits that are not halves: the reconverted ceiling lands a few ulps out, and the + # guard has to read that as float dust rather than as out of range. + arm = _make_arm(closed_gripper_position=75.53) + self._discover(arm=arm, limits=(70.3, 134.097)) + await arm.move_gripper(arm.max_gripper_width, force_sensing=False) + self.assertIn(f"GripOpenPos {134.097 - _GRIPPER_LIMIT_HEADROOM}", self._sent(arm)) + + def test_the_soft_limits_stay_in_firmware_units(self): + self._discover() + self.assertEqual( + (self.arm._gripper_soft_min, self.arm._gripper_soft_max), (self.AXIS_MIN, self.AXIS_MAX) + ) From cb39d098af54aeacf9db0f491b253bfa640b7973 Mon Sep 17 00:00:00 2001 From: miike Date: Wed, 2 Sep 2026 20:21:38 -0400 Subject: [PATCH 4/6] PreciseFlex: add move_one_axis, move_one_axis_relative and move_to_safe Moving a single axis meant building a whole joint pose and hoping the other axes carried their live values. move_one_axis takes an axis and an absolute target; move_one_axis_relative takes a signed offset. Both go through the same guarded move as any other commanded motion, so a blocked axis still recovers and retries. The relative version offsets from the pose read inside the guarded move rather than from a reading taken beforehand, so a retry after recovery offsets from where the axis actually ended up. move_to_safe runs the controller's own movetosafe retraction. It was buried inside park()'s fallback branch and unreachable on its own. It is a route the controller plans itself, so the docstring says what cannot be checked from here: the pose is not readable and not validated against the soft limits. _set_grasp_data becomes set_grasp_data. A caller that composes its own pick has to be able to set the grip before it, and there is nothing private about writing GraspData. --- .../brooks/precise_flex/precise_flex.py | 80 ++++++++++++++++--- .../precise_flex/tests/precise_flex_tests.py | 79 +++++++++++++++++- 2 files changed, 146 insertions(+), 13 deletions(-) diff --git a/pylabrobot/brooks/precise_flex/precise_flex.py b/pylabrobot/brooks/precise_flex/precise_flex.py index 4fb7c70bb37..d4200d61497 100644 --- a/pylabrobot/brooks/precise_flex/precise_flex.py +++ b/pylabrobot/brooks/precise_flex/precise_flex.py @@ -778,7 +778,7 @@ async def _move_j(self, profile_index: int, joint_coords: JointPose) -> None: ) await self.send_command(f"moveJ {profile_index} {angles_str}") - async def _move_one_axis(self, axis: Axis, position: float) -> None: + async def _recover_axis(self, axis: Axis, position: float) -> None: """Move a single axis to an absolute position (firmware ``MoveOneAxis``). Used for recovery: the controller blocks a normal move while an axis is out of @@ -1374,12 +1374,15 @@ async def _request_grasp_data(self) -> tuple[float, float, float]: raise PreciseFlexError(-1, "Unexpected response format from GraspData command.") return (float(parts[0]), float(parts[1]), float(parts[2])) - async def _set_grasp_data( + async def set_grasp_data( self, plate_width: float, finger_speed_pct: float, grasp_force: float ) -> None: - """Set the data to be used for the next force-controlled PickPlate command grip operation. + """Set the data the next force-controlled PickPlate grip will use. - This data remains in effect until the next GraspData command or the system is restarted. + Stateful, and not sticky: ``pick_up_at_location`` and ``pick_up_at_station`` + write it themselves on every call, so data set here is lost if one of those + runs before the pick that wanted it. Otherwise it stands until the next + GraspData command or a controller restart. Args: plate_width: The plate width in mm. @@ -1915,7 +1918,7 @@ async def recover_axes_within_limits( hi, target, ) - await self._move_one_axis(axis, target) + await self._recover_axis(axis, target) await self._wait_for_eom() recovered[axis] = target finally: @@ -2037,7 +2040,7 @@ async def _guarded_move_j(self, build_target: Callable[[JointPose], JointPose]) When an axis is out of range the controller blocks the move (-1012). With ``recover_out_of_range`` set, this drives the offending axes back into range once (``recover_axes_within_limits``) and - retries; otherwise the ``OutOfRangeOfMotionError`` propagates. Recovery uses ``_move_one_axis``, a + retries; otherwise the ``OutOfRangeOfMotionError`` propagates. Recovery uses ``_recover_axis``, a different primitive, so it cannot recurse here. """ @@ -2110,6 +2113,48 @@ async def move_to_joint_position( await self._set_speed(speed_pct) await self._guarded_move_j(lambda current: {**current, **position}) + async def move_one_axis( + self, + axis: Axis, + position: float, + speed_pct: Optional[float] = None, + ) -> None: + """Move one axis to an absolute position, leaving every other axis where it is. + + Guarded like any other commanded move, unlike ``_recover_axis``, which skips + the guard on purpose so it can free an axis the controller has already blocked. + + Args: + axis: The axis to move. + position: Absolute target for that axis. + speed_pct: Movement speed override as a percentage (0-100). If None, uses the + current speed setting. + """ + await self.move_to_joint_position({axis: position}, speed_pct=speed_pct) + + async def move_one_axis_relative( + self, + axis: Axis, + distance: float, + speed_pct: Optional[float] = None, + ) -> None: + """Shift one axis by ``distance`` from where it is now, leaving the others alone. + + The offset is applied to the pose read inside the guarded move rather than to a + position read beforehand, so it cannot act on a stale reading. If the first + attempt is blocked and recovery shifts the axis, the retry offsets from the + recovered position, which is what a relative move should mean. + + Args: + axis: The axis to move. + distance: Signed offset to apply to that axis, in the axis's own units. + speed_pct: Movement speed override as a percentage (0-100). If None, uses the + current speed setting. + """ + if speed_pct is not None: + await self._set_speed(speed_pct) + await self._guarded_move_j(lambda current: {**current, axis: current[axis] + distance}) + async def request_gripper_pose(self) -> PreciseFlexCartesianPose: """Get the current pose using our kinematics model (no firmware `wherec`).""" _, pose = await self._request_state() @@ -2569,7 +2614,7 @@ async def pick_up_at_joint_position( position, resource_width, ) - await self._set_grasp_data( + await self.set_grasp_data( plate_width=resource_width, finger_speed_pct=finger_speed_pct, grasp_force=grasp_force, @@ -2664,7 +2709,7 @@ async def pick_up_at_location( orientation=orientation, wrist=wrist, ) - await self._set_grasp_data( + await self.set_grasp_data( plate_width=resource_width, finger_speed_pct=finger_speed_pct, grasp_force=grasp_force, @@ -2739,7 +2784,7 @@ async def _pick_plate_j(self, joint_position: JointPose): if ret_code == "0": raise PreciseFlexError(-1, "the force-controlled gripper detected no plate present.") - async def _place_plate_j(self, joint_position: JointPose): + async def _place_plate_j(self, joint_position: JointPose) -> None: """Place a plate at the specified position using joint coordinates.""" await self._set_joint_angles(self.location_index, joint_position) await self._set_grip_detail() @@ -2748,12 +2793,12 @@ async def _place_plate_j(self, joint_position: JointPose): f"placeplate {self.location_index} {horizontal_compliance_int} {self.horizontal_compliance_torque}" ) - async def _pick_plate_c(self, cartesian_position: PreciseFlexCartesianPose): + async def _pick_plate_c(self, cartesian_position: PreciseFlexCartesianPose) -> None: """Pick a plate at a Cartesian position via IK + joint-space pickplate.""" joints = await self._cart_to_joints(cartesian_position) await self._pick_plate_j(joints) - async def _place_plate_c(self, cartesian_position: PreciseFlexCartesianPose): + async def _place_plate_c(self, cartesian_position: PreciseFlexCartesianPose) -> None: """Place a plate at a Cartesian position via IK + joint-space placeplate.""" joints = await self._cart_to_joints(cartesian_position) await self._place_plate_j(joints) @@ -2791,7 +2836,18 @@ async def park(self) -> None: position=self._parking_pose_with_default_z(self.parking_position) ) else: - await self.send_command("movetosafe") + await self.move_to_safe() + + async def move_to_safe(self) -> None: + """Run the controller's own retraction to its taught safe position. + + This is the firmware ``movetosafe``: a sequence of safe moves the controller plans itself, not a + single joint target. The pose and the route live in the controller, so neither can be read back + or checked against the soft limits from here. ``park()`` is the counterpart this driver can + reason about. No collision checks against 3rd-party obstacles. + """ + await self._wait_for_eom() + await self.send_command("movetosafe") def _validate_parking_position(self, position: JointPose) -> None: """Reject anything that is not a JointPose of in-range axes (limits checked once known).""" diff --git a/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py b/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py index b1ea7323fce..68ca8733d40 100644 --- a/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py +++ b/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py @@ -295,7 +295,7 @@ async def test_park_respects_an_explicit_base(self): async def test_park_without_position_falls_back_to_movetosafe(self): """While parking_position is unset (no configuration), park() uses the firmware movetosafe.""" await self.arm.park() - mocked(self.arm.send_command).assert_awaited_once_with("movetosafe") + mocked(self.arm.send_command).assert_awaited_with("movetosafe") self.assertEqual(self._movej_cmds(), []) @@ -699,6 +699,10 @@ async def test_the_rail_waits_too(self): await self.arm.move_rail(120.0) self.assertEqual(self._sent()[0], "wherej") + async def test_the_controller_s_safe_retraction_waits_too(self): + await self.arm.move_to_safe() + self.assertEqual(self._sent()[0], "wherej") + class TestGripperTargetsStayInsideTheirLimits(unittest.IsolatedAsyncioTestCase): """Commanding past the end of the gripper axis strands the arm: the controller then @@ -807,3 +811,76 @@ def test_the_soft_limits_stay_in_firmware_units(self): self.assertEqual( (self.arm._gripper_soft_min, self.arm._gripper_soft_max), (self.AXIS_MIN, self.AXIS_MAX) ) + + +class TestPreciseFlexSingleAxisMoves(unittest.IsolatedAsyncioTestCase): + """One axis moves and the rest hold their live values. + + Both verbs run through the guarded joint path, so a target outside the soft + limits is refused here rather than by the controller. + """ + + def setUp(self): + self.arm = _make_arm() + self.arm._wait_for_eom = AsyncMock() # type: ignore[method-assign] + self.arm._configuration = MagicMock( + z_range=(0.0, 400.0), + soft_limits={ + Axis.BASE: (0.0, 400.0), + Axis.SHOULDER: (-93.0, 93.0), + Axis.ELBOW: (12.0, 348.0), + Axis.WRIST: (-960.0, 960.0), + }, + ) + # wherej, no rail: base shoulder elbow wrist gripper + self.arm.send_command = AsyncMock(return_value="50 10 200 90 0") # type: ignore[method-assign] + + def _movej_cmds(self) -> list[str]: + return [ + c.args[0] + for c in mocked(self.arm.send_command).call_args_list + if c.args[0].startswith("moveJ") + ] + + async def test_move_one_axis_moves_only_that_axis(self): + await self.arm.move_one_axis(Axis.SHOULDER, 42.0) + # shoulder becomes 42; base/elbow/wrist/gripper carry from the live pose. + self.assertEqual(self._movej_cmds(), ["moveJ 1 50.0 42.0 200.0 90.0 0.0"]) + + async def test_move_one_axis_relative_offsets_from_the_live_position(self): + await self.arm.move_one_axis_relative(Axis.ELBOW, -20.0) + # elbow 200 - 20 = 180; everything else carries from the live pose. + self.assertEqual(self._movej_cmds(), ["moveJ 1 50.0 10.0 180.0 90.0 0.0"]) + + async def test_move_one_axis_uses_the_guarded_path_not_the_recovery_primitive(self): + # MoveOneAxis is the unguarded recovery primitive; a normal move must not use it. + await self.arm.move_one_axis(Axis.SHOULDER, 42.0) + sent = [c.args[0] for c in mocked(self.arm.send_command).call_args_list] + self.assertFalse([c for c in sent if c.startswith("MoveOneAxis")], sent) + + async def test_move_one_axis_refuses_a_target_outside_the_soft_limits(self): + with self.assertRaises(ValueError): + await self.arm.move_one_axis(Axis.SHOULDER, 200.0) + self.assertEqual(self._movej_cmds(), []) + + async def test_move_one_axis_relative_refuses_an_offset_that_leaves_the_limits(self): + with self.assertRaises(ValueError): + await self.arm.move_one_axis_relative(Axis.SHOULDER, 500.0) + self.assertEqual(self._movej_cmds(), []) + + +class TestPreciseFlexMoveToSafe(unittest.IsolatedAsyncioTestCase): + """The controller's own safe retraction, reachable without going through park().""" + + def setUp(self): + self.arm = _make_arm() + + async def test_move_to_safe_hands_the_route_to_the_controller(self): + await self.arm.move_to_safe() + mocked(self.arm.send_command).assert_awaited_with("movetosafe") + + async def test_move_to_safe_commands_no_joint_target(self): + # The controller plans the route, so the driver must not send joints of its own. + await self.arm.move_to_safe() + sent = [c.args[0] for c in mocked(self.arm.send_command).call_args_list] + self.assertFalse([c for c in sent if c.startswith(("moveJ", "moveC"))], sent) From 716a96c79c97740eebee628013130d179cdbe3d0 Mon Sep 17 00:00:00 2001 From: miike Date: Wed, 2 Sep 2026 20:22:42 -0400 Subject: [PATCH 5/6] PreciseFlex: add at_speed, so one move can run slowly without leaving the arm slow move_to_location's speed_pct sticks: it writes the profile speed and never puts it back, so a slow approach left the arm slow for everything after it. There was no way to run a single move at its own speed. at_speed() is that primitive. It reads the current speed, sets the new one, and restores in a finally, so a fault mid-move cannot leave the arm slow. If the restore itself fails it logs what the arm is still set to before raising, because at that point nothing else will reset it. Passing None does nothing at all, so a caller can hand a speed straight through without branching. A caller scopes whatever it likes inside it, including the rail traverse, which matters on a place: the arm is carrying the plate down the rail, and that is the leg the caller asked to go slowly. move_to_location's docstring now says its speed sticks and points at at_speed for the scoped case. --- .../brooks/precise_flex/precise_flex.py | 58 ++++++++++-- .../precise_flex/tests/precise_flex_tests.py | 91 +++++++++++++++++++ 2 files changed, 140 insertions(+), 9 deletions(-) diff --git a/pylabrobot/brooks/precise_flex/precise_flex.py b/pylabrobot/brooks/precise_flex/precise_flex.py index d4200d61497..c1f97f53ff8 100644 --- a/pylabrobot/brooks/precise_flex/precise_flex.py +++ b/pylabrobot/brooks/precise_flex/precise_flex.py @@ -5,7 +5,18 @@ import logging import time import warnings -from typing import Callable, ClassVar, Dict, List, Literal, NamedTuple, Optional, Sequence +from contextlib import asynccontextmanager +from typing import ( + AsyncIterator, + Callable, + ClassVar, + Dict, + List, + Literal, + NamedTuple, + Optional, + Sequence, +) from pylabrobot.brooks.precise_flex import kinematics from pylabrobot.brooks.precise_flex.config import Axis, PreciseFlexConfiguration @@ -1211,6 +1222,34 @@ async def _request_speed(self) -> float: """Get the current speed percentage of the arm's movement.""" return await self.request_profile_speed(self.profile_index) + @asynccontextmanager + async def at_speed(self, speed_pct: Optional[float]) -> AsyncIterator[None]: + """Run a move at its own speed, then put the profile speed back. + + The restore belongs here rather than with the caller: a fault between the move + and the restore would otherwise leave the arm slow for everything after it. + """ + if speed_pct is None: + yield + return + prior = await self._request_speed() + await self._set_speed(speed_pct) + try: + yield + finally: + try: + await self._set_speed(prior) + except Exception: + # Raising here replaces whatever the move was already failing with, so name the + # state plainly: the arm is still at the move's speed and nothing else will reset it. + logger.error( + "[PreciseFlex %s] could not restore profile speed to %s; the arm is still at %s", + self.io._host, + prior, + speed_pct, + ) + raise + # -- brakes, torque & freedrive ----------------------------------------------------------- async def release_brake(self, axis: int) -> None: @@ -2107,7 +2146,8 @@ async def move_to_joint_position( Args: position: Target joint pose. Omitted axes keep their live values. speed_pct: Movement speed override as a percentage (0-100). If None, uses the current - speed setting. + speed setting. This STICKS: it is not restored afterwards. Wrap a call in + ``at_speed`` instead to scope a speed to one move. """ if speed_pct is not None: await self._set_speed(speed_pct) @@ -2697,9 +2737,7 @@ async def pick_up_at_location( direction, resource_width, ) - if rail_position is not None: - await self.move_rail(rail_position) - elif self._has_rail: + if rail_position is None and self._has_rail: raise ValueError( "rail_position must be specified for pick_up_at_location when using a rail-equipped arm." ) @@ -2709,6 +2747,8 @@ async def pick_up_at_location( orientation=orientation, wrist=wrist, ) + if rail_position is not None: + await self.move_rail(rail_position) await self.set_grasp_data( plate_width=resource_width, finger_speed_pct=finger_speed_pct, @@ -2759,9 +2799,7 @@ async def drop_at_location( direction, resource_width, ) - if rail_position is not None: - await self.move_rail(rail_position) - elif self._has_rail: + if rail_position is None and self._has_rail: raise ValueError( "rail_position must be specified for drop_at_location when using a rail-equipped arm." ) @@ -2771,9 +2809,11 @@ async def drop_at_location( orientation=orientation, wrist=wrist, ) + if rail_position is not None: + await self.move_rail(rail_position) await self._place_plate_c(cartesian_position=coords) - async def _pick_plate_j(self, joint_position: JointPose): + async def _pick_plate_j(self, joint_position: JointPose) -> None: """Pick a plate from the specified position using joint coordinates.""" await self._set_joint_angles(self.location_index, joint_position) await self._set_grip_detail() diff --git a/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py b/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py index 68ca8733d40..d7938b7154d 100644 --- a/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py +++ b/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py @@ -8,6 +8,7 @@ OutOfRangeOfMotionError, PreciseFlex, PreciseFlexCartesianPose, + PreciseFlexError, ) from pylabrobot.events import EventBus, PLREvent, event_context, use_event_bus from pylabrobot.resources import Coordinate, Rotation @@ -884,3 +885,93 @@ async def test_move_to_safe_commands_no_joint_target(self): await self.arm.move_to_safe() sent = [c.args[0] for c in mocked(self.arm.send_command).call_args_list] self.assertFalse([c for c in sent if c.startswith(("moveJ", "moveC"))], sent) + + +class TestAMoveCanRunAtItsOwnSpeed(unittest.IsolatedAsyncioTestCase): + """A slow move must not leave the arm slow for everything that follows. + + ``at_speed`` is the primitive; a caller scopes whatever it likes inside it. + """ + + LOCATION = Coordinate(329.9, 80.29, 40.48) + + def setUp(self): + self.arm = _make_arm() + for name in ("_pick_plate_c", "_place_plate_c", "set_grasp_data"): + patcher = patch.object(self.arm, name, AsyncMock()) + patcher.start() + self.addCleanup(patcher.stop) + + self.speeds: list[float] = [] + + async def record_set(pct: float) -> None: + self.speeds.append(pct) + + for name, mock in ( + ("_set_speed", AsyncMock(side_effect=record_set)), + ("_request_speed", AsyncMock(return_value=100.0)), + ): + patcher = patch.object(self.arm, name, mock) + patcher.start() + self.addCleanup(patcher.stop) + + async def test_a_move_outside_any_scope_leaves_the_profile_untouched(self): + await self.arm.pick_up_at_location(self.LOCATION, direction=2.03, resource_width=80.0) + self.assertEqual(self.speeds, [], "a move with no speed of its own must not write one") + + async def test_no_speed_asked_for_writes_nothing_and_restores_nothing(self): + async with self.arm.at_speed(None): + await self.arm.pick_up_at_location(self.LOCATION, direction=2.03, resource_width=80.0) + self.assertEqual(self.speeds, []) + + async def test_a_pick_at_its_own_speed_puts_the_prior_speed_back(self): + async with self.arm.at_speed(20.0): + await self.arm.pick_up_at_location(self.LOCATION, direction=2.03, resource_width=80.0) + self.assertEqual(self.speeds, [20.0, 100.0]) + + async def test_a_place_at_its_own_speed_puts_the_prior_speed_back(self): + async with self.arm.at_speed(15.0): + await self.arm.drop_at_location(self.LOCATION, direction=2.03, resource_width=80.0) + self.assertEqual(self.speeds, [15.0, 100.0]) + + async def test_the_rail_traverse_runs_at_the_move_s_speed_too(self): + # On a place the arm is carrying the plate down the rail, which is the whole reason a + # move asks to go slowly. The scope has to cover the traverse, not just the place. + self.arm._has_rail = True + order: list[str] = [] + mocked(self.arm._set_speed).side_effect = lambda pct: order.append(f"speed={pct}") + + with patch.object( + self.arm, "move_rail", AsyncMock(side_effect=lambda mm: order.append("rail")) + ): + async with self.arm.at_speed(15.0): + await self.arm.drop_at_location( + self.LOCATION, direction=2.03, resource_width=80.0, rail_position=120.0 + ) + + self.assertEqual(order, ["speed=15.0", "rail", "speed=100.0"]) + + async def test_a_speed_the_arm_will_not_accept_is_refused_before_it_moves(self): + # Rejecting it after the traverse leaves the arm somewhere it was not asked to be. + self.arm._has_rail = True + mocked(self.arm._set_speed).side_effect = ValueError("speed_pct must be 0-100") + + with patch.object(self.arm, "move_rail", AsyncMock()) as rail: + with self.assertRaises(ValueError): + async with self.arm.at_speed(400.0): + await self.arm.drop_at_location( + self.LOCATION, direction=2.03, resource_width=80.0, rail_position=120.0 + ) + + rail.assert_not_called() + + async def test_a_fault_mid_move_still_puts_the_prior_speed_back(self): + mocked(self.arm._pick_plate_c).side_effect = PreciseFlexError(0, "no plate present") + + with self.assertRaises(PreciseFlexError): + async with self.arm.at_speed(20.0): + await self.arm.pick_up_at_location( + self.LOCATION, direction=2.03, resource_width=80.0 + ) + + self.assertEqual(self.speeds, [20.0, 100.0]) From 9f799100b581f0f264c5e79f6cb1c937339ca34f Mon Sep 17 00:00:00 2001 From: miike Date: Fri, 4 Sep 2026 09:28:54 -0400 Subject: [PATCH 6/6] PreciseFlex: end bring-up on a failed configuration read, and refuse a gripper move with no limits Discovery was best-effort: a failed read logged a warning and initialize() carried on. The link lengths come from that read, so an arm that finished bring-up without one solves IK for a different machine, and the gripper has no soft limits to hold a target inside. Both are now fatal to initialize(), which is what a caller can act on. has_configuration goes with it; there is nothing left to ask once bring-up either succeeded or raised. A gripper move with no limits used to pass the target through untouched, so the one case where the driver knows least about the arm was the one case it checked nothing. Commanding past the end of the axis is what leaves a standing position error the controller reports as an overheating motor, after which it refuses every move until the arm is homed. move_gripper and move_gripper_joint_position now refuse instead, naming the missing bring-up rather than the missing limits. The window is still reachable after connect() and before initialize(), which is why the check is at the movers and not only at discovery. The gripper_width_range docstring claimed the value was not in millimetres. Whether the gripper axis is millimetre-scaled is not established anywhere in this driver, so the claim comes out rather than being restated. Co-Authored-By: Claude Opus 5 --- pylabrobot/brooks/precise_flex/config.py | 5 - .../brooks/precise_flex/precise_flex.py | 74 ++++----- .../precise_flex/tests/precise_flex_tests.py | 155 +++++++++++------- 3 files changed, 132 insertions(+), 102 deletions(-) diff --git a/pylabrobot/brooks/precise_flex/config.py b/pylabrobot/brooks/precise_flex/config.py index 48607d9d7a7..babde0b6b6b 100644 --- a/pylabrobot/brooks/precise_flex/config.py +++ b/pylabrobot/brooks/precise_flex/config.py @@ -82,11 +82,6 @@ class PreciseFlexConfiguration: @property def gripper_width_range(self) -> tuple[float, float]: - """Travel limits of the gripper axis, in the controller's own units. - - Not millimetres: a jaw width reaches these through - ``PreciseFlex.closed_gripper_position``, so callers must convert. - """ return self.soft_limits[Axis.GRIPPER] @property diff --git a/pylabrobot/brooks/precise_flex/precise_flex.py b/pylabrobot/brooks/precise_flex/precise_flex.py index c1f97f53ff8..30339e43d96 100644 --- a/pylabrobot/brooks/precise_flex/precise_flex.py +++ b/pylabrobot/brooks/precise_flex/precise_flex.py @@ -329,17 +329,11 @@ async def initialize(self) -> None: async def _discover_configuration(self) -> None: """Adopt what the controller reports, so the class defaults are not used blind. - The link lengths land here, so skipping this leaves IK solving for the wrong arm. + A failed read ends ``initialize()``. The link lengths land here, so carrying on + without them leaves IK solving for the wrong arm and the gripper with no limits + to hold a target inside. """ - try: - self._configuration = await self._request_configuration() - except Exception as exc: # discovery is best-effort - logger.warning( - "[PreciseFlex %s] could not read configuration, using defaults: %s", - self.io._host, - exc, - ) - return + self._configuration = await self._request_configuration() self._adopt_configuration(self._configuration) if self.parking_position is None: self.parking_position = self.PARKING_POSITION_RIGHT @@ -1699,15 +1693,6 @@ def configuration(self) -> "PreciseFlexConfiguration": raise RuntimeError("Configuration is not available until setup() has run.") return self._configuration - @property - def has_configuration(self) -> bool: - """Whether the controller's configuration was actually read. - - Discovery is best-effort, so an arm can finish setup and still not know its own - limits. A caller that would rather adapt than be raised at asks this first. - """ - return self._configuration is not None - async def _request_configuration(self) -> "PreciseFlexConfiguration": """Read the controller's identity, axes, limits, kinematics, and envelope. @@ -2491,20 +2476,16 @@ async def move_gripper( force_sensing, ) units = self._mm_to_firmware_units(width) - if self._gripper_soft_min is not None and self._gripper_soft_max is not None: - # An advertised end converts back through a subtract and an add, so it can land a - # few ulps outside the limit it was derived from. That is dust, not out of range. - if not ( - self._gripper_soft_min - _GRIPPER_UNIT_EPS - <= units - <= self._gripper_soft_max + _GRIPPER_UNIT_EPS - ): - raise ValueError( - f"gripper width {width} mm maps to firmware units {units:.1f}, outside the gripper " - f"axis range [{self._gripper_soft_min}, {self._gripper_soft_max}] - check " - f"closed_gripper_position (currently {self.closed_gripper_position})." - ) - units = self._within_gripper_limits(units) + soft_min, soft_max = self._gripper_limits() + # An advertised end converts back through a subtract and an add, so it can land a + # few ulps outside the limit it was derived from. That is dust, not out of range. + if not (soft_min - _GRIPPER_UNIT_EPS <= units <= soft_max + _GRIPPER_UNIT_EPS): + raise ValueError( + f"gripper width {width} mm maps to firmware units {units:.1f}, outside the gripper " + f"axis range [{soft_min}, {soft_max}] - check closed_gripper_position " + f"(currently {self.closed_gripper_position})." + ) + units = self._within_gripper_limits(units) await self._wait_for_eom() if force_sensing: await self._set_grip_close_pos(units) @@ -2547,16 +2528,25 @@ async def move_gripper_joint_position( await self._set_grip_open_pos(position) await self.send_command("gripper 1") - def _within_gripper_limits(self, units: float) -> float: - """A gripper target held a little inside the axis' soft limits. + def _gripper_limits(self) -> tuple[float, float]: + """The gripper axis' soft limits, refusing if the arm has not read them. - Both ends are adopted together off one discovered tuple, so the arm has either - read its limits or read neither, and before setup there is nothing to hold to. + Both ends are adopted together off one discovered tuple, so the arm has read both + or neither. A target sent past the end is what strands the axis, so a caller that + has not brought the arm up is told rather than obeyed. """ if self._gripper_soft_min is None or self._gripper_soft_max is None: - return units - low = self._gripper_soft_min + _GRIPPER_LIMIT_HEADROOM - high = self._gripper_soft_max - _GRIPPER_LIMIT_HEADROOM + raise RuntimeError( + "the gripper axis' limits have not been read, so a target cannot be held " + "inside them. Run initialize() before moving the gripper." + ) + return self._gripper_soft_min, self._gripper_soft_max + + def _within_gripper_limits(self, units: float) -> float: + """A gripper target held a little inside the axis' soft limits.""" + soft_min, soft_max = self._gripper_limits() + low = soft_min + _GRIPPER_LIMIT_HEADROOM + high = soft_max - _GRIPPER_LIMIT_HEADROOM held = min(max(units, low), high) if held != units: logger.warning( @@ -2564,8 +2554,8 @@ def _within_gripper_limits(self, units: float) -> float: self.io._host, units, held, - self._gripper_soft_min, - self._gripper_soft_max, + soft_min, + soft_max, ) return held diff --git a/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py b/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py index d7938b7154d..a1e1ef7f568 100644 --- a/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py +++ b/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py @@ -2,7 +2,6 @@ from typing import cast from unittest.mock import AsyncMock, MagicMock, patch -from pylabrobot.brooks.precise_flex.precise_flex import _GRIPPER_LIMIT_HEADROOM from pylabrobot.brooks.precise_flex import ( Axis, OutOfRangeOfMotionError, @@ -10,6 +9,7 @@ PreciseFlexCartesianPose, PreciseFlexError, ) +from pylabrobot.brooks.precise_flex.precise_flex import _GRIPPER_LIMIT_HEADROOM from pylabrobot.events import EventBus, PLREvent, event_context, use_event_bus from pylabrobot.resources import Coordinate, Rotation @@ -45,10 +45,35 @@ async def reply(command: str) -> str: return arm +def _discovers(arm: PreciseFlex, limits: tuple = (490.0, 560.0)) -> MagicMock: + """Let bring-up read a configuration, without asserting on what the summary logs.""" + discovered = MagicMock() + discovered.gripper_width_range = limits + discovered.has_rail = False + discovered.is_dual_gripper = False + # Wide enough that the default parking pose validates against them. + discovered.soft_limits = {axis: (-1000.0, 1000.0) for axis in Axis} + arm._request_configuration = AsyncMock(return_value=discovered) # type: ignore[method-assign] + arm._log_configuration_summary = MagicMock() # type: ignore[method-assign] + arm._assess_configuration = MagicMock() # type: ignore[method-assign] + return discovered + + +def _with_gripper_limits(arm: PreciseFlex, limits: tuple = ()) -> PreciseFlex: + """An arm that has read its gripper axis limits, which a gripper move now requires. + + The default span is wide enough that a test asserting on the commanded units is not + also asserting on the headroom held back at each end. + """ + lo, hi = limits or (arm.closed_gripper_position - 100.0, arm.closed_gripper_position + 200.0) + arm._gripper_soft_min, arm._gripper_soft_max = lo, hi + return arm + + class TestPreciseFlex400Gripper(unittest.IsolatedAsyncioTestCase): def setUp(self): # closed_gripper_position=500 ⇒ min_gripper_width(60mm) maps to 500 units. - self.arm = _make_arm(closed_gripper_position=500.0) + self.arm = _with_gripper_limits(_make_arm(closed_gripper_position=500.0)) def _sent_commands(self) -> list[str]: return [c.args[0] for c in mocked(self.arm.send_command).call_args_list] @@ -87,7 +112,7 @@ async def test_min_max_gripper_width_advertised(self): async def test_closed_gripper_position_shifts_units(self): # Different anchor ⇒ same width yields a different firmware-unit target. - arm = _make_arm(closed_gripper_position=1000.0) + arm = _with_gripper_limits(_make_arm(closed_gripper_position=1000.0)) await arm.move_gripper(width=80.0, force_sensing=False) commands = [c.args[0] for c in mocked(arm.send_command).call_args_list] # 80 mm ⇒ 1000 + (80 - 60) = 1020 units. @@ -102,7 +127,7 @@ def test_mm_to_firmware_units_helper(self): class TestPreciseFlexEvents(unittest.IsolatedAsyncioTestCase): async def test_gripper_event_uses_default_length_unit_field(self): - arm = _make_arm() + arm = _with_gripper_limits(_make_arm()) events: list[PLREvent] = [] event_bus = EventBus() event_bus.subscribe(events.append) @@ -122,6 +147,7 @@ async def test_gripper_event_and_nested_firmware_commands_inherit_resource_conte ) arm.io.write = AsyncMock() # type: ignore[method-assign] arm.io.readline = AsyncMock(return_value=b"0\n") # type: ignore[method-assign] + _with_gripper_limits(arm) events: list[PLREvent] = [] event_bus = EventBus() event_bus.subscribe(events.append) @@ -581,19 +607,21 @@ async def test_connect_does_not_raise_power(self): self.assertNotIn("hp 1", self._sent()) self._assert_moved_nothing() - async def test_an_arm_whose_discovery_failed_says_it_has_no_configuration(self): - """Discovery is best-effort, so bring-up succeeding is not proof the arm knows its - own limits, and a caller above has no other way to tell the two apart.""" - self.arm._request_configuration = AsyncMock(side_effect=RuntimeError("no controller")) + async def test_bring_up_fails_when_the_arm_cannot_read_its_configuration(self): + """The link lengths ride on that read, so an arm that finished bring-up without it + would solve IK for a different machine and hold gripper targets against nothing.""" + self.arm._request_configuration = AsyncMock( # type: ignore[method-assign] + side_effect=RuntimeError("no controller") + ) - await self.arm.initialize() + with self.assertRaisesRegex(RuntimeError, "no controller"): + await self.arm.initialize() - self.assertFalse(self.arm.has_configuration) with self.assertRaises(RuntimeError): self.arm.configuration async def test_initialize_takes_control_without_moving(self): - self.arm._request_configuration = AsyncMock(side_effect=RuntimeError("no controller")) + _discovers(self.arm) await self.arm.initialize() sent = self._sent() self.assertIn("attach 1", sent) @@ -609,23 +637,26 @@ async def test_initialize_adopts_what_the_controller_reports(self): Axis.ELBOW: (12.0, 348.0), Axis.WRIST: (-960.0, 960.0), } - self.arm._request_configuration = AsyncMock(return_value=discovered) - self.arm._adopt_configuration = MagicMock() - self.arm._log_configuration_summary = MagicMock() - self.arm._assess_configuration = MagicMock() + self.arm._request_configuration = AsyncMock(return_value=discovered) # type: ignore[method-assign] + self.arm._adopt_configuration = MagicMock() # type: ignore[method-assign] + self.arm._log_configuration_summary = MagicMock() # type: ignore[method-assign] + self.arm._assess_configuration = MagicMock() # type: ignore[method-assign] await self.arm.initialize() - self.arm._adopt_configuration.assert_called_once_with(discovered) - self.assertTrue(self.arm.has_configuration) + mocked(self.arm._adopt_configuration).assert_called_once_with(discovered) + self.assertIs(self.arm.configuration, discovered) - async def test_initialize_falls_back_to_defaults_when_discovery_fails(self): - self.arm._request_configuration = AsyncMock(side_effect=RuntimeError("no controller")) - self.arm._adopt_configuration = MagicMock() + async def test_a_failed_read_is_not_adopted_as_a_configuration(self): + self.arm._request_configuration = AsyncMock( # type: ignore[method-assign] + side_effect=RuntimeError("no controller") + ) + self.arm._adopt_configuration = MagicMock() # type: ignore[method-assign] - await self.arm.initialize() + with self.assertRaises(RuntimeError): + await self.arm.initialize() - self.arm._adopt_configuration.assert_not_called() + mocked(self.arm._adopt_configuration).assert_not_called() async def test_disconnect_hands_the_arm_back_and_closes_the_link(self): await self.arm.disconnect() @@ -637,39 +668,55 @@ async def test_disconnect_hands_the_arm_back_and_closes_the_link(self): async def test_setup_connects_then_initializes_then_homes_in_that_order(self): calls: list[str] = [] - self.arm.connect = AsyncMock(side_effect=lambda: calls.append("connect")) - self.arm.initialize = AsyncMock(side_effect=lambda: calls.append("initialize")) - self.arm.home = AsyncMock(side_effect=lambda: calls.append("home")) - self.arm._handle_out_of_range_axes = AsyncMock() + self.arm.connect = AsyncMock(side_effect=lambda: calls.append("connect")) # type: ignore[method-assign] + self.arm.initialize = AsyncMock(side_effect=lambda: calls.append("initialize")) # type: ignore[method-assign] + self.arm.home = AsyncMock(side_effect=lambda: calls.append("home")) # type: ignore[method-assign] + self.arm._handle_out_of_range_axes = AsyncMock() # type: ignore[method-assign] await self.arm.setup() self.assertEqual(calls, ["connect", "initialize", "home"]) async def test_setup_skip_home_brings_the_arm_up_without_sweeping_it(self): - self.arm.connect = AsyncMock() - self.arm.initialize = AsyncMock() - self.arm.home = AsyncMock() - self.arm._handle_out_of_range_axes = AsyncMock() + self.arm.connect = AsyncMock() # type: ignore[method-assign] + self.arm.initialize = AsyncMock() # type: ignore[method-assign] + self.arm.home = AsyncMock() # type: ignore[method-assign] + self.arm._handle_out_of_range_axes = AsyncMock() # type: ignore[method-assign] await self.arm.setup(skip_home=True) mocked(self.arm.home).assert_not_awaited() - async def test_setup_still_checks_soft_limits_when_discovery_fails(self): - # Discovery is best-effort, but losing it must not silently skip the - # out-of-range recovery that makes an unusable arm usable again. - self.arm.connect = AsyncMock() - self.arm.home = AsyncMock() - self.arm._request_configuration = AsyncMock(side_effect=RuntimeError("no controller")) - self.arm._handle_out_of_range_axes = AsyncMock() + async def test_setup_stops_when_the_arm_cannot_read_its_configuration(self): + # Bring-up that cannot read the arm must not report success: the caller would get + # an arm solving IK for a different machine. + self.arm.connect = AsyncMock() # type: ignore[method-assign] + self.arm.home = AsyncMock() # type: ignore[method-assign] + self.arm._request_configuration = AsyncMock( # type: ignore[method-assign] + side_effect=RuntimeError("no controller") + ) + self.arm._handle_out_of_range_axes = AsyncMock() # type: ignore[method-assign] + + with self.assertRaisesRegex(RuntimeError, "no controller"): + await self.arm.setup() + + mocked(self.arm.home).assert_not_awaited() + mocked(self.arm._handle_out_of_range_axes).assert_not_awaited() + + async def test_setup_checks_soft_limits_once_the_arm_is_up(self): + # The out-of-range recovery is what makes an arm parked outside its limits usable + # again, so bring-up has to reach it. + self.arm.connect = AsyncMock() # type: ignore[method-assign] + self.arm.home = AsyncMock() # type: ignore[method-assign] + _discovers(self.arm) + self.arm._handle_out_of_range_axes = AsyncMock() # type: ignore[method-assign] await self.arm.setup() mocked(self.arm._handle_out_of_range_axes).assert_awaited_once() async def test_stop_is_disconnect(self): - self.arm.disconnect = AsyncMock() + self.arm.disconnect = AsyncMock() # type: ignore[method-assign] await self.arm.stop() mocked(self.arm.disconnect).assert_awaited_once() @@ -682,7 +729,7 @@ class TestMotionSettlesBeforeTheNextCommand(unittest.IsolatedAsyncioTestCase): """ def setUp(self): - self.arm = _make_arm() + self.arm = _with_gripper_limits(_make_arm()) def _sent(self) -> list[str]: return [c.args[0] for c in mocked(self.arm.send_command).call_args_list] @@ -740,15 +787,18 @@ def test_the_axis_limits_are_readable_without_reaching_into_the_arm(self): def test_an_arm_that_has_read_no_limits_reports_neither_end(self): self.assertEqual(_make_arm().gripper_joint_range, (None, None)) - async def test_an_arm_that_has_read_no_limits_commands_what_it_was_asked_for(self): - """Before discovery there is no ceiling to hold a target under, and inventing one - would refuse widths the fitted gripper reaches.""" + async def test_an_arm_that_has_read_no_limits_refuses_to_move_the_jaws(self): + """With no ceiling to hold a target under there is nothing to make it safe, and + a target past the end is what strands the axis. The caller is told, not obeyed.""" arm = _make_arm(closed_gripper_position=500.0) - await arm.move_gripper_joint_position(999.0, force_sensing=False) + with self.assertRaisesRegex(RuntimeError, "initialize"): + await arm.move_gripper_joint_position(999.0, force_sensing=False) + with self.assertRaisesRegex(RuntimeError, "initialize"): + await arm.move_gripper(80.0, force_sensing=False) sent = [c.args[0] for c in mocked(arm.send_command).call_args_list] - self.assertEqual(sent[-2:], ["GripOpenPos 999.0", "gripper 1"]) + self.assertEqual([c for c in sent if c.startswith("Grip") or c.startswith("gripper")], []) class TestGripperWidthsAdoptedFromSoftLimits(unittest.IsolatedAsyncioTestCase): @@ -775,16 +825,13 @@ def _discover(self, arm=None, limits=None): def _sent(self, arm=None) -> list[str]: return [c.args[0] for c in mocked((arm or self.arm).send_command).call_args_list] - async def test_a_width_commands_the_same_travel_before_and_after_discovery(self): - await self.arm.move_gripper(80.0, force_sensing=False) - before = self._sent() + async def test_a_width_means_the_same_travel_before_and_after_discovery(self): + """The jaws cannot be moved before discovery, so this reads the mapping directly: + adopting the axis limits must not shift the anchor a width is measured from.""" + before = self.arm._mm_to_firmware_units(80.0) self._discover() - await self.arm.move_gripper(80.0, force_sensing=False) - after = self._sent()[len(before) :] self.assertEqual( - [c for c in before if c.startswith("GripOpenPos")], - [c for c in after if c.startswith("GripOpenPos")], - "discovery moved what 80 mm means", + before, self.arm._mm_to_firmware_units(80.0), "discovery moved what 80 mm means" ) async def test_opening_to_the_advertised_max_stops_short_of_the_axis_ceiling(self): @@ -970,8 +1017,6 @@ async def test_a_fault_mid_move_still_puts_the_prior_speed_back(self): with self.assertRaises(PreciseFlexError): async with self.arm.at_speed(20.0): - await self.arm.pick_up_at_location( - self.LOCATION, direction=2.03, resource_width=80.0 - ) + await self.arm.pick_up_at_location(self.LOCATION, direction=2.03, resource_width=80.0) self.assertEqual(self.speeds, [20.0, 100.0])