diff --git a/pylabrobot/brooks/precise_flex/arm_backend.py b/pylabrobot/brooks/precise_flex/arm_backend.py index 0ea4078f2d1..895a2b7f1d5 100644 --- a/pylabrobot/brooks/precise_flex/arm_backend.py +++ b/pylabrobot/brooks/precise_flex/arm_backend.py @@ -21,7 +21,7 @@ import logging import warnings from dataclasses import dataclass -from typing import ClassVar, Dict, List, Literal, Optional +from typing import ClassVar, Dict, List, Literal, Optional, Sequence from pylabrobot.capabilities.arms.backend import ( CanFreedrive, @@ -512,6 +512,51 @@ async def _cart_to_joints(self, cart: PreciseFlexCartesianPose) -> JointPose: joints[Axis.RAIL] = cart.rail_position return joints + # [int] + def _plan_cartesian_pose_route( + self, + poses: Sequence[PreciseFlexCartesianPose], + planned_joints: JointPose, + planned_pose: PreciseFlexCartesianPose, + ) -> List[JointPose]: + """Plan a Cartesian pose route into joint targets from an initial state snapshot. + + Unlike :meth:`_cart_to_joints`, this helper does not query the controller for every + waypoint. Omitted pose fields inherit from the *planned* previous pose so IK branch + selection remains continuous across the route. + """ + planned_joints = dict(planned_joints) + targets: List[JointPose] = [] + for pose in poses: + cart = dataclasses.replace( + pose, + orientation=planned_pose.orientation if pose.orientation is None else pose.orientation, + wrist=planned_pose.wrist if pose.wrist is None else pose.wrist, + # PF400 IK expects a shoulder/reference rail position even on rail-less arms. + # Mirror _cart_to_joints(): omitted pose fields inherit from the previous pose. + rail_position=planned_pose.rail_position + if pose.rail_position is None + else pose.rail_position, + ) + ik_joints = _snap_to_current( + kinematics.ik(cart, p=self._kinematics_params), + planned_joints, + cart.wrist, + ) + target = dict(planned_joints) + target[Axis.BASE] = ik_joints[1] + target[Axis.SHOULDER] = ik_joints[2] + target[Axis.ELBOW] = ik_joints[3] + target[Axis.WRIST] = ik_joints[4] + if self._has_rail and cart.rail_position is not None: + target[Axis.RAIL] = cart.rail_position + + self._assert_within_soft_limits(planned_joints, target) + targets.append(target) + planned_joints = target + planned_pose = cart + return targets + # -- speed & motion profiles -------------------------------------------------------------- # [cmd] @@ -1811,6 +1856,22 @@ class MoveToLocationParams(BackendParams): wrist: Optional[Wrist] = None rail_position: Optional[float] = None + # [cmd] + @dataclass + class MoveThroughCartesianPosesParams(BackendParams): + """PreciseFlex arm parameters for blended Cartesian pose routes. + + Args: + speed_pct: Movement speed override as a percentage (0-100). If None, uses the + current speed setting. + blend: When True, temporarily set the active motion profile's ``InRange`` value to + ``-1`` so the controller may blend through intermediate waypoints instead of stopping + at each one. The original profile is restored after the final waypoint is reached. + """ + + speed_pct: Optional[float] = None + blend: bool = True + async def move_to_location( self, location: Coordinate, @@ -1839,6 +1900,66 @@ async def move_to_location( joints = await self._cart_to_joints(coords) await self._move_j(profile_index=self.profile_index, joint_coords=joints) + # [cmd] + async def move_through_cartesian_poses( + self, + poses: Sequence[PreciseFlexCartesianPose], + backend_params: Optional[BackendParams] = None, + ) -> None: + """Move through a sequence of Cartesian poses using one planned IK route. + + The standard Cartesian move path snapshots the current state for each waypoint, + which waits for end-of-motion between moves. For taught air-transit routes, this + method snapshots state once, plans each subsequent IK target from the previous + planned target, queues the joint moves, and waits only after the final waypoint. + + This is a PreciseFlex-specific primitive: intermediate waypoints may be blended + by the controller and should not be used for operations that require an exact + stop, gripper action, or physical contact at every pose. + """ + if not isinstance(backend_params, self.MoveThroughCartesianPosesParams): + backend_params = PreciseFlexArmBackend.MoveThroughCartesianPosesParams() + if not poses: + return + if backend_params.speed_pct is not None: + await self._set_speed(backend_params.speed_pct) + + planned_joints, planned_pose = await self._request_state() + targets = self._plan_cartesian_pose_route(poses, planned_joints, planned_pose) + + profile_index = self.profile_index + original_profile = None + should_restore_profile = False + if backend_params.blend: + original_profile = await self.request_motion_profile_values(profile_index) + should_restore_profile = original_profile[7] != -1 + if should_restore_profile: + await self.set_motion_profile_values( + original_profile[0], + original_profile[1], + original_profile[2], + original_profile[3], + original_profile[4], + original_profile[5], + original_profile[6], + -1, + original_profile[8], + ) + + moves_sent = 0 + waited_for_eom = False + try: + for target in targets: + await self._move_j(profile_index=profile_index, joint_coords=target) + moves_sent += 1 + await self.driver._wait_for_eom() + waited_for_eom = True + finally: + if should_restore_profile and original_profile is not None: + if moves_sent > 0 and not waited_for_eom: + await self.driver._wait_for_eom() + await self.set_motion_profile_values(*original_profile) + # [cmd] async def dest_c(self, arg1: int = 0) -> tuple[float, float, float, float, float, float, int]: """Get the destination or current Cartesian location of the robot. diff --git a/pylabrobot/brooks/precise_flex/tests/arm_backend_tests.py b/pylabrobot/brooks/precise_flex/tests/arm_backend_tests.py index 90589a4a034..a5155dbfe1b 100644 --- a/pylabrobot/brooks/precise_flex/tests/arm_backend_tests.py +++ b/pylabrobot/brooks/precise_flex/tests/arm_backend_tests.py @@ -1,8 +1,9 @@ import unittest from typing import Tuple -from unittest.mock import AsyncMock, MagicMock +from unittest.mock import AsyncMock, MagicMock, patch -from pylabrobot.brooks.precise_flex import Axis, PreciseFlexArmBackend +from pylabrobot.brooks.precise_flex import Axis, PreciseFlexArmBackend, PreciseFlexCartesianPose +from pylabrobot.resources import Coordinate, Rotation def _make_backend( @@ -210,3 +211,142 @@ async def test_park_without_position_falls_back_to_movetosafe(self): await self.backend.park() self.driver.send_command.assert_awaited_once_with("movetosafe") self.assertEqual(self._movej_cmds(), []) + + +class TestPreciseFlexSmoothCartesianRoute(unittest.IsolatedAsyncioTestCase): + def setUp(self): + self.backend, self.driver = _make_backend() + self.driver._wait_for_eom = AsyncMock() + self.current_joints = { + Axis.BASE: 100.0, + Axis.SHOULDER: 0.0, + Axis.ELBOW: 180.0, + Axis.WRIST: 180.0, + Axis.GRIPPER: 70.0, + } + self.current_pose = PreciseFlexCartesianPose( + location=Coordinate(10.0, 20.0, 100.0), + rotation=Rotation(x=-180.0, y=90.0, z=0.0), + rail_position=123.0, + orientation="right", + wrist="ccw", + ) + self.backend._request_state = AsyncMock(return_value=(self.current_joints, self.current_pose)) + + def _stub_profile_transport(self, profile: str = "1 50 0 100 100 0 0 25 0") -> None: + async def respond(command: str) -> str: + if command == "Profile 1": + return profile + return "" + + self.driver.send_command = AsyncMock(side_effect=respond) + + def _movej_cmds(self) -> list[str]: + return [ + c.args[0] for c in self.driver.send_command.call_args_list if c.args[0].startswith("moveJ") + ] + + def _profile_cmds(self) -> list[str]: + return [ + c.args[0] + for c in self.driver.send_command.call_args_list + if c.args[0].startswith("Profile") + ] + + async def test_move_through_cartesian_poses_plans_from_one_state_snapshot(self): + """A smooth route snapshots state once, fills omitted pose fields from the planned pose, + queues all joint moves, then waits once at the end.""" + self._stub_profile_transport() + poses = [ + PreciseFlexCartesianPose( + location=Coordinate(200.0, 20.0, 110.0), + rotation=Rotation(x=-180.0, y=90.0, z=10.0), + ), + PreciseFlexCartesianPose( + location=Coordinate(210.0, 20.0, 120.0), + rotation=Rotation(x=-180.0, y=90.0, z=20.0), + ), + ] + + with patch( + "pylabrobot.brooks.precise_flex.arm_backend.kinematics.ik", + side_effect=[ + {1: 110.0, 2: 10.0, 3: 20.0, 4: 30.0, 6: 123.0}, + {1: 120.0, 2: 11.0, 3: 21.0, 4: 31.0, 6: 123.0}, + ], + ) as ik: + await self.backend.move_through_cartesian_poses(poses) + + self.backend._request_state.assert_awaited_once() + self.driver._wait_for_eom.assert_awaited_once() + self.assertEqual( + self._movej_cmds(), + [ + "moveJ 1 110.0 10.0 20.0 30.0 70.0", + "moveJ 1 120.0 11.0 21.0 31.0 70.0", + ], + ) + planned_pose_args = [call.args[0] for call in ik.call_args_list] + self.assertEqual([pose.orientation for pose in planned_pose_args], ["right", "right"]) + self.assertEqual([pose.wrist for pose in planned_pose_args], ["ccw", "ccw"]) + # Rail-less PF400 still needs the shoulder/reference rail position for IK. + self.assertEqual([pose.rail_position for pose in planned_pose_args], [123.0, 123.0]) + + async def test_move_through_cartesian_poses_temporarily_enables_blending(self): + self._stub_profile_transport() + pose = PreciseFlexCartesianPose( + location=Coordinate(200.0, 20.0, 110.0), + rotation=Rotation(x=-180.0, y=90.0, z=10.0), + ) + + with patch( + "pylabrobot.brooks.precise_flex.arm_backend.kinematics.ik", + return_value={1: 110.0, 2: 10.0, 3: 20.0, 4: 30.0, 6: 123.0}, + ): + await self.backend.move_through_cartesian_poses([pose]) + + self.assertEqual( + self._profile_cmds(), + [ + "Profile 1", + "Profile 1 50.0 0.0 100.0 100.0 0.0 0.0 -1 0", + "Profile 1 50.0 0.0 100.0 100.0 0.0 0.0 25.0 0", + ], + ) + + async def test_move_through_cartesian_poses_can_skip_profile_blending(self): + pose = PreciseFlexCartesianPose( + location=Coordinate(200.0, 20.0, 110.0), + rotation=Rotation(x=-180.0, y=90.0, z=10.0), + ) + + with patch( + "pylabrobot.brooks.precise_flex.arm_backend.kinematics.ik", + return_value={1: 110.0, 2: 10.0, 3: 20.0, 4: 30.0, 6: 123.0}, + ): + await self.backend.move_through_cartesian_poses( + [pose], + backend_params=PreciseFlexArmBackend.MoveThroughCartesianPosesParams(blend=False), + ) + + self.assertEqual(self._profile_cmds(), []) + self.assertEqual(self._movej_cmds(), ["moveJ 1 110.0 10.0 20.0 30.0 70.0"]) + self.driver._wait_for_eom.assert_awaited_once() + + async def test_move_through_cartesian_poses_blocks_before_motion_on_limit_failure(self): + pose = PreciseFlexCartesianPose( + location=Coordinate(200.0, 20.0, 110.0), + rotation=Rotation(x=-180.0, y=90.0, z=10.0), + ) + self.backend._assert_within_soft_limits = MagicMock(side_effect=ValueError("bad target")) + + with patch( + "pylabrobot.brooks.precise_flex.arm_backend.kinematics.ik", + return_value={1: 110.0, 2: 10.0, 3: 20.0, 4: 30.0, 6: 123.0}, + ): + with self.assertRaisesRegex(ValueError, "bad target"): + await self.backend.move_through_cartesian_poses([pose]) + + self.assertEqual(self._movej_cmds(), []) + self.assertEqual(self._profile_cmds(), []) + self.driver._wait_for_eom.assert_not_awaited() diff --git a/todo-documentation-backendparameters.md b/todo-documentation-backendparameters.md index d9a2d9b3de7..0bc5785b155 100644 --- a/todo-documentation-backendparameters.md +++ b/todo-documentation-backendparameters.md @@ -62,6 +62,7 @@ classes received new or expanded docstrings in this pass: - `PreciseFlexArmBackend.DropParams` -- new docstring - `PreciseFlexArmBackend.MoveToJointPositionParams` -- new docstring - `PreciseFlexArmBackend.MoveToLocationParams` -- new docstring +- `PreciseFlexArmBackend.MoveThroughCartesianPosesParams` -- new docstring ### Already documented (no changes needed) - `MultidropCombiPeristalticDispensingBackend.DispenseParams`