Add smooth PreciseFlex Cartesian route primitive

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.

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()

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`