diff --git a/pylabrobot/brooks/precise_flex/driver/features/arm.py b/pylabrobot/brooks/precise_flex/driver/features/arm.py index aefb0630ab4..3f720651522 100644 --- a/pylabrobot/brooks/precise_flex/driver/features/arm.py +++ b/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", diff --git a/pylabrobot/brooks/precise_flex/tests/device_tests.py b/pylabrobot/brooks/precise_flex/tests/device_tests.py index 024423f3512..63d4b20ccf9 100644 --- a/pylabrobot/brooks/precise_flex/tests/device_tests.py +++ b/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 "") diff --git a/pylabrobot/brooks/precise_flex/tests/wire_tests.py b/pylabrobot/brooks/precise_flex/tests/wire_tests.py index 73a6822e692..541c898adda 100644 --- a/pylabrobot/brooks/precise_flex/tests/wire_tests.py +++ b/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."""