Add smooth PreciseFlex Cartesian route primitive

pylabrobot/brooks/precise_flex/arm_backend.py

@@ -12,16 +12,16 @@
 - L3 - high-level capability API: joint-space and cartesian motion, gripper, rail, pick & place, parking.
 
 Within each group methods run reads (``request_``) then writes (``set_``) then actions, with private
-helpers last. Each method carries a tag for who calls it: ``[PLR]`` implements a capability-ABC method
-that PyLabRobot core drives, ``[cmd]`` is a public PreciseFlex firmware command outside the ABC, and
-``[int]`` is a private helper or primitive.
+helpers last. A method that implements a capability-ABC method PyLabRobot core drives is tagged
+``[PLR]``; the rest are public PreciseFlex firmware commands (no leading underscore) or private
+helpers/primitives (leading underscore).
 """
 
 import dataclasses
 import logging
 import warnings
 from dataclasses import dataclass
-from typing import Callable, ClassVar, Dict, List, Literal, Optional
+from typing import Callable, ClassVar, Dict, List, Literal, NamedTuple, Optional, Sequence
 
 from pylabrobot.capabilities.arms.backend import (
   CanFreedrive,
@@ -48,6 +48,23 @@
 
 logger = logging.getLogger(__name__)
 
+# InRange sentinel that lets the controller blend through waypoints instead of stopping at each one.
+BLEND_IN_RANGE = -1
+
+
+class MotionProfile(NamedTuple):
+  """A controller motion profile, as reported by ``Profile <n>`` (field order matches the wire)."""
+
+  profile: int
+  speed: float
+  speed2: float
+  acceleration: float
+  deceleration: float
+  acceleration_ramp: float
+  deceleration_ramp: float
+  in_range: float  # -1 (BLEND_IN_RANGE) to 100; -1 blends, 0 stops, >0 enforces position accuracy
+  straight: bool  # True = straight-line path, False = joint-based path
+
 
 def _parse_scalar(response: str) -> float:
   """Parse the first numeric field of a DataID reply.
@@ -492,10 +509,9 @@ async def _cart_to_joints(self, cart: PreciseFlexCartesianPose) -> JointPose:
       rail_position=current.rail_position if cart.rail_position is None else cart.rail_position,
     )
     ik_joints = _snap_to_current(kinematics.ik(cart, p=self._kinematics_params), joints, cart.wrist)
-    joints[Axis.BASE] = ik_joints[1]
-    joints[Axis.SHOULDER] = ik_joints[2]
-    joints[Axis.ELBOW] = ik_joints[3]
-    joints[Axis.WRIST] = ik_joints[4]
+    # IK only solves the arm axes; gripper and rail keep their current values.
+    for axis in (Axis.BASE, Axis.SHOULDER, Axis.ELBOW, Axis.WRIST):
+      joints[axis] = ik_joints[axis]
     if cart.rail_position is not None:
       joints[Axis.RAIL] = cart.rail_position
     return joints
@@ -769,32 +785,21 @@ async def set_profile_straight(self, profile_index: int, straight_mode: bool) ->
     straight_int = 1 if straight_mode else 0
     await self.driver.send_command(f"Straight {profile_index} {straight_int}")
 
-  async def request_motion_profile_values(
-    self, profile: int
-  ) -> tuple[int, float, float, float, float, float, float, float, bool]:
+  async def request_motion_profile_values(self, profile: int) -> MotionProfile:
     """
     Get the current motion profile values for the specified profile index on the PreciseFlex robot.
 
     Args:
       profile: Profile index to get values for.
 
     Returns:
-      A tuple containing (profile, speed, speed2, acceleration, deceleration, acceleration_ramp, deceleration_ramp, in_range, straight)
-        - profile: Profile index
-        - speed: Percentage of maximum speed
-        - speed2: Secondary speed setting
-        - acceleration: Percentage of maximum acceleration
-        - deceleration: Percentage of maximum deceleration
-        - acceleration_ramp: Acceleration ramp time in seconds
-        - deceleration_ramp: Deceleration ramp time in seconds
-        - in_range: InRange value (-1 to 100)
-        - straight: True if straight-line path, False if joint-based path
+      A :class:`MotionProfile` with the profile's speed, acceleration, ramps, InRange and path mode.
     """
     data = await self.driver.send_command(f"Profile {profile}")
     parts = data.split(" ")
     if len(parts) != 9:
       raise PreciseFlexError(-1, "Unexpected response format from device.")
-    return (
+    return MotionProfile(
       int(parts[0]),
       float(parts[1]),
       float(parts[2]),
@@ -1800,6 +1805,106 @@ async def move_to_location(
     joints = await self._cart_to_joints(coords)
     await self._guarded_move_j(lambda _current: joints)
 
+  async def _plan_cartesian_pose_route(
+    self, poses: Sequence[PreciseFlexCartesianPose]
+  ) -> List[JointPose]:
+    """Plan a Cartesian pose route into joint targets, snapshotting state once.
+
+    Unlike :meth:`_cart_to_joints`, this does not query the controller for every waypoint: it
+    reads the current state once and resolves each waypoint's IK from the previous waypoint's
+    result. Omitted pose fields inherit from the previous pose so IK branch selection remains
+    continuous across the route.
+    """
+    prev_joints, prev_pose = await self._request_state()
+    targets: List[JointPose] = []
+    for pose in poses:
+      cart = dataclasses.replace(
+        pose,
+        orientation=prev_pose.orientation if pose.orientation is None else pose.orientation,
+        wrist=prev_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=prev_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),
+        prev_joints,
+        cart.wrist,
+      )
+      # IK only solves the arm axes; gripper and rail keep the previous values.
+      target = dict(prev_joints)
+      for axis in (Axis.BASE, Axis.SHOULDER, Axis.ELBOW, Axis.WRIST):
+        target[axis] = ik_joints[axis]
+      if self._has_rail and cart.rail_position is not None:
+        target[Axis.RAIL] = cart.rail_position
+
+      self._assert_within_soft_limits(prev_joints, target)
+      targets.append(target)
+      prev_joints = target
+      prev_pose = cart
+    return targets
+
+  @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_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)
+
+    targets = await self._plan_cartesian_pose_route(poses)
+
+    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.in_range != BLEND_IN_RANGE
+      if should_restore_profile:
+        await self.set_motion_profile_values(*original_profile._replace(in_range=BLEND_IN_RANGE))
+
+    try:
+      for target in targets:
+        await self._move_j(profile_index=profile_index, joint_coords=target)
+    finally:
+      # Let queued motion settle before returning or restoring the profile - restoring InRange
+      # mid-move would change the in-flight blend.
+      try:
+        await self.driver._wait_for_eom()
+      finally:
+        if should_restore_profile and original_profile is not None:
+          await self.set_motion_profile_values(*original_profile)
+
   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.
 

pylabrobot/brooks/precise_flex/tests/arm_backend_tests.py

@@ -6,8 +6,9 @@
   Axis,
   OutOfRangeOfMotionError,
   PreciseFlexArmBackend,
+  PreciseFlexCartesianPose,
 )
-from pylabrobot.resources import Coordinate
+from pylabrobot.resources import Coordinate, Rotation
 
 
 def _make_backend(
@@ -217,6 +218,143 @@ async def test_park_without_position_falls_back_to_movetosafe(self):
     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()
+
+
 _LOGGER = "pylabrobot.brooks.precise_flex.arm_backend"
 
 

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`