PreciseFlexArm: picks and places write a Cartesian station

pylabrobot/brooks/precise_flex/driver/features/arm.py

@@ -419,6 +419,28 @@ async def _unchecked_fw_set_joint_angles(
       angles_str = f"{joint_position[PreciseFlexAxis.RAIL]} {angles_str}"
     await self._driver.send_command(f"locAngles {station_index} {angles_str}")
 
+  async def _unchecked_fw_set_station_cartesian(
+    self, station_index: int, joint_position: JointState
+  ) -> None:
+    """Write a station as the controller's Cartesian location, from the joint state to stand at.
+
+    ``pickplate`` and ``placeplate`` take a Cartesian station, and ``StationType`` empties one set
+    by ``locAngles``. The controller's z is the Z joint.
+
+    Raises:
+      NotImplementedError: On an arm with a rail, whose station has not been run on an arm.
+    """
+    if self._driver._has_rail:
+      raise NotImplementedError("a pick or a place is not supported on a rail-equipped arm yet")
+    pose = kinematics.fk(joint_position, self._driver._kinematics_params)
+    heading = (round(pose.rotation.z, 3) + 180) % 360 - 180
+    await self._driver.send_command(
+      f"locXyz {station_index} {round(pose.location.x, 3)} {round(pose.location.y, 3)} "
+      f"{round(joint_position[PreciseFlexAxis.BASE], 3)} {round(heading, 3)} 90 180"
+    )
+    config = 1 if pose.orientation == "right" else 2
+    await self._driver.send_command(f"locConfig {station_index} {config}")
+
   def _plan_cartesian_pose(
     self,
     pose: PreciseFlexCartesianPose,
@@ -2029,7 +2051,9 @@ async def _guarded_plate_j(
     """Write the station and run the controller's pick or place at it, guarded.
 
     Only the straight joint move to the station is checked against the column and the base plate.
-    The controller's own approach, depart and any detour through its park pose are not seen by it.
+    The wrist is checked where the controller takes it: the heading wrapped to +-180, less the
+    shoulder and the elbow. The controller's own approach, depart and any detour through its park
+    pose are not seen by it.
 
     Args:
       build_station: maps the pose just read to the station's full joint state.
@@ -2040,12 +2064,21 @@ async def _guarded_plate_j(
         base plate.
     """
 
+    def controller_station(current: JointState) -> List[JointState]:
+      station = dict(build_station(current))
+      # The heading as `_unchecked_fw_set_station_cartesian` sends it, so a wrap of +-180 cannot
+      # differ between the check and the wire.
+      pose = kinematics.fk(joints=station, p=self._driver._kinematics_params)
+      heading = round(pose.rotation.z, 3)
+      station[PreciseFlexAxis.WRIST] += (heading + 180) % 360 - 180 - heading
+      return [station]
+
     async def unchecked_fw_move(targets: Sequence[JointState]) -> None:
-      await self._unchecked_fw_set_joint_angles(self.station_index, targets[0])
+      await self._unchecked_fw_set_station_cartesian(self.station_index, targets[0])
       await self._unchecked_fw_set_grip_detail()
       await unchecked_fw_plate()
 
-    await self._guarded_move(lambda current: [build_station(current)], unchecked_fw_move)
+    await self._guarded_move(build_targets=controller_station, unchecked_fw_move=unchecked_fw_move)
 
   @evented_operation(
     "precise_flex.pick_up_at_joint_position",

pylabrobot/brooks/precise_flex/tests/device_tests.py

@@ -587,6 +587,26 @@ async def test_a_pick_or_place_at_the_column_is_refused_before_the_station_is_wr
               )
         self.assertEqual([call.args[0] for call in sent.call_args_list], ["wherej"])
 
+  async def test_a_pick_is_checked_with_the_wrist_turn_the_controller_takes(self):
+    # The first two: stations the arm reached a full wrist turn from the one taught.
+    for taught, reached in (
+      (joints(-81.17, 105.893, -268.962, 78.0, z=152.993), 91.038),
+      (joints(84.162, 236.177, 37.591, 78.0, z=150.0), -322.409),
+      (joints(-30.0, 150.0, 60.0, 78.0, z=150.0), -300.0),  # a heading of exactly 180
+    ):
+      with self.subTest(taught=taught[PreciseFlexAxis.WRIST]):
+        sent = AsyncMock(side_effect=lambda command: self.HOMED if command == "wherej" else "")
+        with patch.object(self.driver, "send_command", sent):
+          with patch.object(self.arm, "_wait_for_eom", AsyncMock()):
+            with patch.object(self.arm, "_check_path_reachable") as checked:
+              await self.arm._guarded_plate_j(
+                build_station=lambda current: {**current, **taught},
+                unchecked_fw_plate=self.arm._unchecked_fw_pick_plate,
+              )
+        self.assertEqual(checked.call_count, 1)
+        checked_wrist = checked.call_args.args[1][PreciseFlexAxis.WRIST]
+        self.assertAlmostEqual(checked_wrist, reached, delta=0.01)
+
   async def test_each_move_of_a_route_of_poses_is_checked_from_where_the_last_ends(self):
     ahead = self.arm._forward_kinematics(joints(0.0, 180.0, -180.0, 70.7)).gripper_pose
     sent = AsyncMock(side_effect=lambda command: self.HOMED if command == "wherej" else "")

pylabrobot/brooks/precise_flex/tests/wire_tests.py

@@ -593,7 +593,8 @@ def _rail(arm: PreciseFlexDriver) -> PreciseFlexRail:
       "state",
       "state",
       "wherej",
-      "locAngles 1 200.0 10.0 170.0 20.0 100.0",
+      "locXyz 1 -143.818 -2.966 200.0 -160.0 90 180",
+      "locConfig 1 1",
       "StationType 1 1 0 100 0 10",
       "pickplate 1 0 0",
       "state",
@@ -609,7 +610,8 @@ def _rail(arm: PreciseFlexDriver) -> PreciseFlexRail:
       "state",
       "state",
       "wherej",
-      "locAngles 1 200.0 10.0 170.0 20.0 100.0",
+      "locXyz 1 -143.818 -2.966 200.0 -160.0 90 180",
+      "locConfig 1 1",
       "StationType 1 1 0 100 0 10",
       "placeplate 1 0 0",
       "state",
@@ -626,7 +628,8 @@ def _rail(arm: PreciseFlexDriver) -> PreciseFlexRail:
       "state",
       "state",
       "wherej",
-      "locAngles 1 201.25 -38.74046014630248 128.25603427878514 -89.51557413248264 70.685",
+      "locXyz 1 400.0 100.0 201.25 0.0 90 180",
+      "locConfig 1 1",
       "StationType 1 1 0 100 0 10",
       "pickplate 1 0 0",
       "state",
@@ -642,7 +645,8 @@ def _rail(arm: PreciseFlexDriver) -> PreciseFlexRail:
       "state",
       "state",
       "wherej",
-      "locAngles 1 201.25 -38.74046014630248 128.25603427878514 -89.51557413248264 70.685",
+      "locXyz 1 400.0 100.0 201.25 0.0 90 180",
+      "locConfig 1 1",
       "StationType 1 1 0 100 0 10",
       "placeplate 1 0 0",
       "state",
@@ -1194,6 +1198,51 @@ async def test_a_target_at_the_axis_end_is_held_inside_it(self):
     self.assertEqual(moves, ["MoveOneAxis 5 133.5 1"] * 2)
 
 
+class TestPickAndPlaceStation(unittest.IsolatedAsyncioTestCase):
+  """`StationType` empties a station set by `locAngles`, so it is written as a Cartesian one."""
+
+  async def test_the_station_is_where_the_controller_says_those_joints_are(self):
+    # Joints as `wherej` read them, with `wherec`'s x, y, z, heading and configuration there.
+    for joints, station, config in (
+      ((16.484, -31.943, 134.929, -105.062), "353.222 115.96 16.484 -2.076", 1),
+      ((10.192, 69.106, 198.097, -267.011), "255.596 -5.976 10.192 0.19", 2),
+    ):
+      with self.subTest(config=config):
+        fake = _FakeController()
+        arm = _make_arm(fake)
+        axes = (
+          PreciseFlexAxis.BASE,
+          PreciseFlexAxis.SHOULDER,
+          PreciseFlexAxis.ELBOW,
+          PreciseFlexAxis.WRIST,
+        )
+        await arm.arm._unchecked_fw_set_station_cartesian(1, dict(zip(axes, joints)))
+        sent_x, sent_y, sent_z, sent_heading = map(float, fake.sent[0].split()[2:6])
+        for sent, recorded in zip(
+          (sent_x, sent_y, sent_z, sent_heading), map(float, station.split())
+        ):
+          self.assertAlmostEqual(sent, recorded, delta=0.02)
+        self.assertEqual(fake.sent[1], f"locConfig 1 {config}")
+
+  async def test_a_rail_arm_is_refused_before_the_station_is_written(self):
+    fake = _FakeController()
+    arm = _make_arm(fake, has_rail=True)
+    with self.assertRaises(NotImplementedError):
+      await arm.arm._unchecked_fw_set_station_cartesian(1, dict(_J))
+    self.assertEqual(fake.sent, [])
+
+  async def test_joint_angles_are_written_with_the_rail_first(self):
+    for has_rail, joints, sent in (
+      (False, _J, "locAngles 1 200.0 10.0 170.0 20.0 100.0"),
+      (True, _JR, "locAngles 1 500.0 200.0 10.0 170.0 20.0 100.0"),
+    ):
+      with self.subTest(has_rail=has_rail):
+        fake = _FakeController()
+        arm = _make_arm(fake, has_rail=has_rail)
+        await arm.arm._unchecked_fw_set_joint_angles(station_index=1, joint_position=dict(joints))
+        self.assertEqual(fake.sent, [sent])
+
+
 class TestClosingTheGripperSensesForce(unittest.IsolatedAsyncioTestCase):
   """Only the jaws' own moves close them, limiting the force; anything else closing them is asked."""