diff --git a/pylabrobot/brooks/precise_flex/config.py b/pylabrobot/brooks/precise_flex/config.py index 70caecd7a0e..babde0b6b6b 100644 --- a/pylabrobot/brooks/precise_flex/config.py +++ b/pylabrobot/brooks/precise_flex/config.py @@ -81,7 +81,7 @@ class PreciseFlexConfiguration: reach_class: Literal["standard", "extended", "unknown"] = "extended" @property - def gripper_width_range(self) -> tuple: + def gripper_width_range(self) -> tuple[float, float]: 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 63da4884488..30339e43d96 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 @@ -29,6 +40,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 +204,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 +225,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 ) @@ -277,40 +298,47 @@ 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. - 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 + await self._discover_configuration() + + async def _discover_configuration(self) -> None: + """Adopt what the controller reports, so the class defaults are not used blind. + + 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. + """ + self._configuration = await self._request_configuration() self._adopt_configuration(self._configuration) if self.parking_position is None: 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 +346,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() @@ -519,6 +554,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). @@ -742,7 +783,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 @@ -1175,6 +1216,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: @@ -1338,12 +1407,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. @@ -1359,17 +1431,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 ---------------------------------------------------------------------- @@ -1729,7 +1801,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 @@ -1867,7 +1942,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: @@ -1989,7 +2064,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. """ @@ -2056,12 +2131,55 @@ 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) 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() @@ -2316,8 +2434,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. @@ -2340,8 +2458,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 @@ -2355,16 +2476,17 @@ 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) - ): + 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 [{self._gripper_soft_min}, {self._gripper_soft_max}] - check " - f"closed_gripper_position (currently {self.closed_gripper_position})." + 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) await self.send_command("gripper 2") @@ -2389,7 +2511,16 @@ 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) await self.send_command("gripper 2") @@ -2397,6 +2528,44 @@ async def move_gripper_joint_position( await self._set_grip_open_pos(position) await self.send_command("gripper 1") + 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 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: + 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( + "[PreciseFlex %s] gripper target %s held to %s, inside [%s, %s]", + self.io._host, + units, + held, + soft_min, + 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. @@ -2438,6 +2607,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) @@ -2474,7 +2644,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, @@ -2557,9 +2727,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." ) @@ -2569,7 +2737,9 @@ async def pick_up_at_location( orientation=orientation, wrist=wrist, ) - await self._set_grasp_data( + 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, grasp_force=grasp_force, @@ -2619,9 +2789,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." ) @@ -2631,9 +2799,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() @@ -2644,7 +2814,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() @@ -2653,12 +2823,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) @@ -2696,7 +2866,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 907aa07357f..a1e1ef7f568 100644 --- a/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py +++ b/pylabrobot/brooks/precise_flex/tests/precise_flex_tests.py @@ -7,7 +7,9 @@ OutOfRangeOfMotionError, PreciseFlex, 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 @@ -22,21 +24,56 @@ 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 + + +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] @@ -44,12 +81,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) @@ -75,11 +112,11 @@ 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. - 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. @@ -90,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) @@ -110,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) @@ -138,6 +176,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", ], @@ -282,7 +322,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(), []) @@ -521,3 +561,462 @@ 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_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") + ) + + with self.assertRaisesRegex(RuntimeError, "no controller"): + await self.arm.initialize() + + with self.assertRaises(RuntimeError): + self.arm.configuration + + async def test_initialize_takes_control_without_moving(self): + _discovers(self.arm) + 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) # 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() + + mocked(self.arm._adopt_configuration).assert_called_once_with(discovered) + self.assertIs(self.arm.configuration, discovered) + + 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] + + with self.assertRaises(RuntimeError): + await self.arm.initialize() + + 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() + 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")) # 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() # 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_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() # type: ignore[method-assign] + 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 = _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] + + 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") + + 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 + 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_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) + + 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([c for c in sent if c.startswith("Grip") or c.startswith("gripper")], []) + + +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_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() + self.assertEqual( + 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): + """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) + ) + + +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) + + +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])