From 095e19b93e78ec00fd75a32a42b1d21d28194422 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Fri, 30 Oct 2026 21:13:02 +0000 Subject: [PATCH 1/4] `PreciseFlexArm`: picks and places write a Cartesian station `StationType` empties a station set by `locAngles`, so every pick and place targeted 0, 0, 0. The station is now written with `locXyz` and `locConfig`. A rail arm is refused. Co-Authored-By: Claude Opus 5.5 --- .../precise_flex/driver/features/arm.py | 30 ++++++++---- .../brooks/precise_flex/tests/wire_tests.py | 46 +++++++++++++++++-- 2 files changed, 62 insertions(+), 14 deletions(-) diff --git a/pylabrobot/brooks/precise_flex/driver/features/arm.py b/pylabrobot/brooks/precise_flex/driver/features/arm.py index 11519aea351..44e55d676e8 100644 --- a/pylabrobot/brooks/precise_flex/driver/features/arm.py +++ b/pylabrobot/brooks/precise_flex/driver/features/arm.py @@ -404,17 +404,27 @@ async def _unchecked_fw_move_one_axis(self, axis: PreciseFlexAxis, position: flo """ await self._driver.send_command(f"MoveOneAxis {int(axis)} {position} {self.profile_index}") - async def _unchecked_fw_set_joint_angles( - self, - station_index: int, - joint_position: JointState, + async def _unchecked_fw_set_station_cartesian( + self, station_index: int, joint_position: JointState ) -> None: - """Set joint angles for a station, handling rail configuration.""" - angles_str = " ".join(str(joint_position[axis]) for axis in self._WIRE_ORDER) + """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: - # Here the rail's value goes first. - angles_str = f"{joint_position[PreciseFlexAxis.RAIL]} {angles_str}" - await self._driver.send_command(f"locAngles {station_index} {angles_str}") + 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 = (pose.rotation.z + 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, @@ -2040,7 +2050,7 @@ async def _guarded_plate_j( """ 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() diff --git a/pylabrobot/brooks/precise_flex/tests/wire_tests.py b/pylabrobot/brooks/precise_flex/tests/wire_tests.py index 3b54fa486ad..a16c6ef4e3f 100644 --- a/pylabrobot/brooks/precise_flex/tests/wire_tests.py +++ b/pylabrobot/brooks/precise_flex/tests/wire_tests.py @@ -1161,7 +1161,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", @@ -1177,7 +1178,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", @@ -1194,7 +1196,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", @@ -1210,7 +1213,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", @@ -1717,6 +1721,40 @@ 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, []) + + class TestClosingTheGripperSensesForce(unittest.IsolatedAsyncioTestCase): """Only the jaws' own moves close them, limiting the force; anything else closing them is asked.""" From 615f47c89d701b684ff806e5ab0725c278e38e42 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Sun, 1 Nov 2026 18:11:09 +0000 Subject: [PATCH 2/4] PreciseFlex wire tests: one line per table row where it fits Co-Authored-By: Claude Opus 5.5 --- .../brooks/precise_flex/tests/wire_tests.py | 804 +++--------------- 1 file changed, 106 insertions(+), 698 deletions(-) diff --git a/pylabrobot/brooks/precise_flex/tests/wire_tests.py b/pylabrobot/brooks/precise_flex/tests/wire_tests.py index a16c6ef4e3f..372d7ba169d 100644 --- a/pylabrobot/brooks/precise_flex/tests/wire_tests.py +++ b/pylabrobot/brooks/precise_flex/tests/wire_tests.py @@ -195,56 +195,12 @@ def _rail(arm: PreciseFlexDriver) -> PreciseFlexRail: # (name, call, commands sent, exception raised or None), each on an arm already set up. _CASES: List[_Case] = [ - ( - "stop", - lambda arm: arm.stop(), - [ - "attach 0", - "hp 0", - " exit", - ], - None, - ), - ( - "request_mode", - lambda arm: arm.request_mode(), - [ - "mode", - ], - None, - ), - ( - "set_response_mode_pc", - lambda arm: arm.set_response_mode("pc"), - [ - "mode 0", - ], - None, - ), - ( - "set_response_mode_verbose", - lambda arm: arm.set_response_mode("verbose"), - [ - "mode 1", - ], - None, - ), - ( - "request_system_state", - lambda arm: arm.request_system_state(), - [ - "sysState", - ], - None, - ), - ( - "power_on_robot", - lambda arm: arm.power_on_robot(), - [ - "hp 1 20", - ], - None, - ), + ("stop", lambda arm: arm.stop(), ["attach 0", "hp 0", " exit"], None), + ("request_mode", lambda arm: arm.request_mode(), ["mode"], None), + ("set_response_mode_pc", lambda arm: arm.set_response_mode("pc"), ["mode 0"], None), + ("set_response_mode_verbose", lambda arm: arm.set_response_mode("verbose"), ["mode 1"], None), + ("request_system_state", lambda arm: arm.request_system_state(), ["sysState"], None), + ("power_on_robot", lambda arm: arm.power_on_robot(), ["hp 1 20"], None), ( "recover_from_fault", lambda arm: arm.recover_from_fault(), @@ -262,691 +218,252 @@ def _rail(arm: PreciseFlexDriver) -> PreciseFlexRail: ], None, ), - ( - "power_off_robot", - lambda arm: arm.power_off_robot(), - [ - "hp 0", - ], - None, - ), - ( - "set_power_on", - lambda arm: arm.set_power(True, 20), - [ - "hp 1 20", - ], - None, - ), - ( - "set_power_off", - lambda arm: arm.set_power(False), - [ - "hp 0", - ], - None, - ), - ( - "request_power_state", - lambda arm: arm.request_power_state(), - [ - "hp", - ], - None, - ), - ( - "attach", - lambda arm: arm.attach(1), - [ - "attach 1", - ], - None, - ), - ( - "attach_query", - lambda arm: arm.attach(), - [ - "attach", - ], - None, - ), - ( - "detach", - lambda arm: arm.detach(), - [ - "attach 0", - ], - None, - ), + ("power_off_robot", lambda arm: arm.power_off_robot(), ["hp 0"], None), + ("set_power_on", lambda arm: arm.set_power(True, 20), ["hp 1 20"], None), + ("set_power_off", lambda arm: arm.set_power(False), ["hp 0"], None), + ("request_power_state", lambda arm: arm.request_power_state(), ["hp"], None), + ("attach", lambda arm: arm.attach(1), ["attach 1"], None), + ("attach_query", lambda arm: arm.attach(), ["attach"], None), + ("detach", lambda arm: arm.detach(), ["attach 0"], None), ( "home", lambda arm: arm.home(), - [ - "state", - "state", - "wherej", - "home", - "state", - "state", - "wherej", - ], - None, - ), - ( - "home_all", - lambda arm: arm.home_all(), - [ - "homeAll", - ], - None, - ), - ( - "request_state", - lambda arm: arm.arm.request_state(), - [ - "state", - ], - None, - ), - ( - "request_parameter", - lambda arm: arm.request_parameter(2700), - [ - "pd 2700", - ], + ["state", "state", "wherej", "home", "state", "state", "wherej"], None, ), + ("home_all", lambda arm: arm.home_all(), ["homeAll"], None), + ("request_state", lambda arm: arm.arm.request_state(), ["state"], None), + ("request_parameter", lambda arm: arm.request_parameter(2700), ["pd 2700"], None), ( "request_parameter_indexed", lambda arm: arm.request_parameter(16050, 1, 2, 3), - [ - "pd 16050 1 2 3", - ], - None, - ), - ( - "set_parameter", - lambda arm: arm.set_parameter(2704, 140), - [ - "pc 2704 140", - ], + ["pd 16050 1 2 3"], None, ), + ("set_parameter", lambda arm: arm.set_parameter(2704, 140), ["pc 2704 140"], None), ( "set_parameter_indexed", lambda arm: arm.set_parameter(16050, 5, 1, 2, 3), - [ - "pc 16050 1 2 3 5", - ], + ["pc 16050 1 2 3 5"], None, ), ( "set_axis_parameter", lambda arm: arm.set_axis_parameter(2700, PreciseFlexAxis.SHOULDER, 300), - [ - "pc 2700 1 0 2 300", - ], - None, - ), - ( - "nop", - lambda arm: arm.nop(), - [ - "nop", - ], - None, - ), - ( - "request_signal", - lambda arm: arm.request_signal(10001), - [ - "sig 10001", - ], - None, - ), - ( - "set_signal", - lambda arm: arm.set_signal(10001, 1), - [ - "sig 10001 1", - ], - None, - ), - ( - "request_monitor_speed", - lambda arm: arm.arm.request_monitor_speed(), - [ - "mspeed", - ], - None, - ), - ( - "set_monitor_speed", - lambda arm: arm.arm.set_monitor_speed(50), - [ - "mspeed 50", - ], - None, - ), - ( - "request_payload", - lambda arm: arm.arm.request_payload(), - [ - "payload", - ], - None, - ), - ( - "set_payload", - lambda arm: arm.arm.set_payload(25), - [ - "payload 25", - ], - None, - ), - ( - "request_profile_speed", - lambda arm: arm.arm.request_profile_speed(1), - [ - "Speed 1", - ], - None, - ), - ( - "set_profile_speed", - lambda arm: arm.arm.set_profile_speed(1, 40), - [ - "Speed 1 40", - ], - None, - ), - ( - "request_profile_speed2", - lambda arm: arm.arm.request_profile_speed2(1), - [ - "Speed2 1", - ], - None, - ), - ( - "set_profile_speed2", - lambda arm: arm.arm.set_profile_speed2(1, 30), - [ - "Speed2 1 30", - ], + ["pc 2700 1 0 2 300"], None, ), + ("nop", lambda arm: arm.nop(), ["nop"], None), + ("request_signal", lambda arm: arm.request_signal(10001), ["sig 10001"], None), + ("set_signal", lambda arm: arm.set_signal(10001, 1), ["sig 10001 1"], None), + ("request_monitor_speed", lambda arm: arm.arm.request_monitor_speed(), ["mspeed"], None), + ("set_monitor_speed", lambda arm: arm.arm.set_monitor_speed(50), ["mspeed 50"], None), + ("request_payload", lambda arm: arm.arm.request_payload(), ["payload"], None), + ("set_payload", lambda arm: arm.arm.set_payload(25), ["payload 25"], None), + ("request_profile_speed", lambda arm: arm.arm.request_profile_speed(1), ["Speed 1"], None), + ("set_profile_speed", lambda arm: arm.arm.set_profile_speed(1, 40), ["Speed 1 40"], None), + ("request_profile_speed2", lambda arm: arm.arm.request_profile_speed2(1), ["Speed2 1"], None), + ("set_profile_speed2", lambda arm: arm.arm.set_profile_speed2(1, 30), ["Speed2 1 30"], None), ( "request_profile_acceleration", lambda arm: arm.arm.request_profile_acceleration(1), - [ - "Accel 1", - ], + ["Accel 1"], None, ), ( "set_profile_acceleration", lambda arm: arm.arm.set_profile_acceleration(1, 60), - [ - "Accel 1 60", - ], + ["Accel 1 60"], None, ), ( "request_profile_acceleration_ramp", lambda arm: arm.arm.request_profile_acceleration_ramp(1), - [ - "AccRamp 1", - ], + ["AccRamp 1"], None, ), ( "set_profile_acceleration_ramp", lambda arm: arm.arm.set_profile_acceleration_ramp(1, 0.2), - [ - "AccRamp 1 0.2", - ], + ["AccRamp 1 0.2"], None, ), ( "request_profile_deceleration", lambda arm: arm.arm.request_profile_deceleration(1), - [ - "Decel 1", - ], + ["Decel 1"], None, ), ( "set_profile_deceleration", lambda arm: arm.arm.set_profile_deceleration(1, 70), - [ - "Decel 1 70", - ], + ["Decel 1 70"], None, ), ( "request_profile_deceleration_ramp", lambda arm: arm.arm.request_profile_deceleration_ramp(1), - [ - "DecRamp 1", - ], + ["DecRamp 1"], None, ), ( "set_profile_deceleration_ramp", lambda arm: arm.arm.set_profile_deceleration_ramp(1, 0.3), - [ - "DecRamp 1 0.3", - ], + ["DecRamp 1 0.3"], None, ), ( "request_profile_in_range", lambda arm: arm.arm.request_profile_in_range(1), - [ - "InRange 1", - ], - None, - ), - ( - "set_profile_in_range", - lambda arm: arm.arm.set_profile_in_range(1, 10), - [ - "InRange 1 10", - ], + ["InRange 1"], None, ), + ("set_profile_in_range", lambda arm: arm.arm.set_profile_in_range(1, 10), ["InRange 1 10"], None), ( "request_profile_straight", lambda arm: arm.arm.request_profile_straight(1), - [ - "Straight 1", - ], + ["Straight 1"], None, ), ( "set_profile_straight_on", lambda arm: arm.arm.set_profile_straight(1, True), - [ - "Straight 1 1", - ], + ["Straight 1 1"], None, ), ( "set_profile_straight_off", lambda arm: arm.arm.set_profile_straight(1, False), - [ - "Straight 1 0", - ], + ["Straight 1 0"], None, ), ( "request_motion_profile_values", lambda arm: arm.arm.request_motion_profile_values(1), - [ - "Profile 1", - ], + ["Profile 1"], None, ), ( "set_motion_profile_values", lambda arm: arm.arm.set_motion_profile_values(1, 40, 30, 60, 70, 0.2, 0.3, 10, True), - [ - "Profile 1 40 30 60 70 0.2 0.3 10 -1", - ], - None, - ), - ( - "release_brake", - lambda arm: arm.arm.release_brake(2), - [ - "releaseBrake 2", - ], - None, - ), - ( - "set_brake", - lambda arm: arm.arm.reengage_brake(2), - [ - "setBrake 2", - ], - None, - ), - ( - "zero_torque_on", - lambda arm: arm.arm.start_zero_torque(3), - [ - "zeroTorque 1 3", - ], - None, - ), - ( - "zero_torque_off", - lambda arm: arm.arm.stop_zero_torque(), - [ - "zeroTorque 0", - ], + ["Profile 1 40 30 60 70 0.2 0.3 10 -1"], None, ), + ("release_brake", lambda arm: arm.arm.release_brake(2), ["releaseBrake 2"], None), + ("set_brake", lambda arm: arm.arm.reengage_brake(2), ["setBrake 2"], None), + ("zero_torque_on", lambda arm: arm.arm.start_zero_torque(3), ["zeroTorque 1 3"], None), + ("zero_torque_off", lambda arm: arm.arm.stop_zero_torque(), ["zeroTorque 0"], None), ( "start_freedrive_mode_all", lambda arm: arm.arm.start_freedrive_mode(), - [ - "freemode 1", - "freemode 2", - "freemode 3", - "freemode 4", - ], + ["freemode 1", "freemode 2", "freemode 3", "freemode 4"], None, ), ( "start_freedrive_mode_axes", lambda arm: arm.arm.start_freedrive_mode([1, 2]), - [ - "freemode 1", - "freemode 2", - ], - None, - ), - ( - "stop_freedrive_mode", - lambda arm: arm.arm.stop_freedrive_mode(), - [ - "freemode -1", - ], - None, - ), - ( - "halt", - lambda arm: arm.arm.halt(), - [ - "halt", - ], - None, - ), - ( - "change_config", - lambda arm: arm.arm.change_elbow_orientation(1), - [ - "ChangeConfig 1", - ], + ["freemode 1", "freemode 2"], None, ), + ("stop_freedrive_mode", lambda arm: arm.arm.stop_freedrive_mode(), ["freemode -1"], None), + ("halt", lambda arm: arm.arm.halt(), ["halt"], None), + ("change_config", lambda arm: arm.arm.change_elbow_orientation(1), ["ChangeConfig 1"], None), ( "change_config2", lambda arm: arm.arm.change_elbow_orientation_by_algorithm(1), - [ - "ChangeConfig2 1", - ], - None, - ), - ( - "request_manufacturer", - lambda arm: arm.request_manufacturer(), - [ - "pd 100", - ], - None, - ), - ( - "request_controller_model", - lambda arm: arm.request_controller_model(), - [ - "pd 101", - ], - None, - ), - ( - "request_hardware_version", - lambda arm: arm.request_hardware_version(), - [ - "pd 102", - ], - None, - ), - ( - "request_gpl_version", - lambda arm: arm.request_gpl_version(), - [ - "pd 103", - ], - None, - ), - ( - "request_controller_serial", - lambda arm: arm.request_controller_serial(), - [ - "pd 110", - ], - None, - ), - ( - "request_robot_name", - lambda arm: arm.request_robot_name(), - [ - "pd 2002", - ], - None, - ), - ( - "request_robot_type", - lambda arm: arm.request_robot_type(), - [ - "pd 116", - ], - None, - ), - ( - "request_axis_count", - lambda arm: arm.request_axis_count(), - [ - "pd 2000", - ], - None, - ), - ( - "request_extra_axis_count", - lambda arm: arm.request_extra_axis_count(), - [ - "pd 2004", - ], - None, - ), - ( - "request_axis_mask", - lambda arm: arm.request_axis_mask(), - [ - "pd 2003", - ], - None, - ), - ( - "request_version", - lambda arm: arm.request_version(), - [ - "version", - ], + ["ChangeConfig2 1"], None, ), + ("request_manufacturer", lambda arm: arm.request_manufacturer(), ["pd 100"], None), + ("request_controller_model", lambda arm: arm.request_controller_model(), ["pd 101"], None), + ("request_hardware_version", lambda arm: arm.request_hardware_version(), ["pd 102"], None), + ("request_gpl_version", lambda arm: arm.request_gpl_version(), ["pd 103"], None), + ("request_controller_serial", lambda arm: arm.request_controller_serial(), ["pd 110"], None), + ("request_robot_name", lambda arm: arm.request_robot_name(), ["pd 2002"], None), + ("request_robot_type", lambda arm: arm.request_robot_type(), ["pd 116"], None), + ("request_axis_count", lambda arm: arm.request_axis_count(), ["pd 2000"], None), + ("request_extra_axis_count", lambda arm: arm.request_extra_axis_count(), ["pd 2004"], None), + ("request_axis_mask", lambda arm: arm.request_axis_mask(), ["pd 2003"], None), + ("request_version", lambda arm: arm.request_version(), ["version"], None), ( "request_joint_limits_soft", lambda arm: arm.arm.request_joint_limits(), - [ - "pd 16078", - "pd 16077", - ], + ["pd 16078", "pd 16077"], None, ), ( "request_joint_limits_hard", lambda arm: arm.arm.request_joint_limits(hard=True), - [ - "pd 16076", - "pd 16075", - ], - None, - ), - ( - "request_reference_speed", - lambda arm: arm.arm.request_reference_speed(), - [ - "pd 2700", - ], + ["pd 16076", "pd 16075"], None, ), + ("request_reference_speed", lambda arm: arm.arm.request_reference_speed(), ["pd 2700"], None), ( "request_reference_acceleration", lambda arm: arm.arm.request_reference_acceleration(), - [ - "pd 2702", - ], - None, - ), - ( - "request_link_lengths", - lambda arm: arm.arm.request_link_lengths(), - [ - "pd 16050", - ], - None, - ), - ( - "request_tool_length", - lambda arm: arm.arm.request_tool_length(), - [ - "pd 16051", - ], + ["pd 2702"], None, ), + ("request_link_lengths", lambda arm: arm.arm.request_link_lengths(), ["pd 16050"], None), + ("request_tool_length", lambda arm: arm.arm.request_tool_length(), ["pd 16051"], None), ( "request_kinematic_parameters", lambda arm: arm.arm.request_kinematic_parameters(), - [ - "pd 16050", - "pd 16051", - ], + ["pd 16050", "pd 16051"], None, ), ( "request_reference_cartesian_speed", lambda arm: arm.arm.request_reference_cartesian_speed(), - [ - "pd 2701", - ], + ["pd 2701"], None, ), ( "request_reference_cartesian_acceleration", lambda arm: arm.arm.request_reference_cartesian_acceleration(), - [ - "pd 2703", - ], - None, - ), - ( - "request_max_speed_percent", - lambda arm: arm.arm.request_max_speed_percent(), - [ - "pd 2704", - ], + ["pd 2703"], None, ), + ("request_max_speed_percent", lambda arm: arm.arm.request_max_speed_percent(), ["pd 2704"], None), ( "request_max_acceleration_percent", lambda arm: arm.arm.request_max_acceleration_percent(), - [ - "pd 2705", - ], + ["pd 2705"], None, ), ( "request_max_deceleration_percent", lambda arm: arm.arm.request_max_deceleration_percent(), - [ - "pd 2706", - ], - None, - ), - ( - "request_base", - lambda arm: arm.arm.request_base(), - [ - "base", - ], - None, - ), - ( - "set_base", - lambda arm: arm.arm.set_base(1.0, 2.0, 3.0, 4.0), - [ - "base 1.0 2.0 3.0 4.0", - ], + ["pd 2706"], None, ), + ("request_base", lambda arm: arm.arm.request_base(), ["base"], None), + ("set_base", lambda arm: arm.arm.set_base(1.0, 2.0, 3.0, 4.0), ["base 1.0 2.0 3.0 4.0"], None), ( "request_tool_transformation_values", lambda arm: arm.arm.request_tool_transformation_values(), - [ - "tool", - ], - None, - ), - ( - "reset", - lambda arm: arm.reset(1), - [ - "reset 1", - ], - None, - ), - ( - "request_selected_robot", - lambda arm: arm.request_selected_robot(), - [ - "selectRobot", - ], - None, - ), - ( - "select_robot", - lambda arm: arm.select_robot(1), - [ - "selectRobot 1", - ], + ["tool"], None, ), + ("reset", lambda arm: arm.reset(1), ["reset 1"], None), + ("request_selected_robot", lambda arm: arm.request_selected_robot(), ["selectRobot"], None), + ("select_robot", lambda arm: arm.select_robot(1), ["selectRobot 1"], None), ( "recover_axes_within_limits", lambda arm: arm.arm.recover_axes_within_limits(), - [ - "state", - "state", - "wherej", - ], + ["state", "state", "wherej"], None, ), ( "request_joint_state", lambda arm: arm.arm.request_joint_state(), - [ - "state", - "state", - "wherej", - ], + ["state", "state", "wherej"], None, ), ( "move_to_joint_position", lambda arm: arm.arm.move_to_joint_state(_J), - [ - "state", - "state", - "wherej", - "moveJ 1 200.0 10.0 170.0 20.0 100.0", - "state", - "state", - "wherej", - ], + ["state", "state", "wherej", "moveJ 1 200.0 10.0 170.0 20.0 100.0", "state", "state", "wherej"], None, ), ( @@ -967,11 +484,7 @@ def _rail(arm: PreciseFlexDriver) -> PreciseFlexRail: ( "request_gripper_pose", lambda arm: arm.arm.request_gripper_pose(), - [ - "state", - "state", - "wherej", - ], + ["state", "state", "wherej"], None, ), ( @@ -1037,122 +550,47 @@ def _rail(arm: PreciseFlexDriver) -> PreciseFlexRail: ], None, ), - ( - "dest_c", - lambda arm: arm.arm._unchecked_fw_request_cartesian_destination(), - [ - "destC", - ], - None, - ), - ( - "dest_j", - lambda arm: arm.arm.request_destination_joint_state(), - [ - "destJ", - ], - None, - ), - ( - "here_j", - lambda arm: arm.arm.set_station_to_current_joint_state(2), - [ - "hereJ 2", - ], - None, - ), + ("dest_c", lambda arm: arm.arm._unchecked_fw_request_cartesian_destination(), ["destC"], None), + ("dest_j", lambda arm: arm.arm.request_destination_joint_state(), ["destJ"], None), + ("here_j", lambda arm: arm.arm.set_station_to_current_joint_state(2), ["hereJ 2"], None), ( "here_c", lambda arm: arm.arm._unchecked_fw_set_station_to_current_cartesian_location(2), - [ - "hereC 2", - ], + ["hereC 2"], None, ), ( "move_gripper_open", lambda arm: arm.gripper.move_to_jaw_position(110.0), - [ - "state", - "state", - "wherej", - "ReleasePlate 130.0 50.0", - "state", - "state", - "wherej", - ], + ["state", "state", "wherej", "ReleasePlate 130.0 50.0", "state", "state", "wherej"], None, ), ( "move_gripper_close", lambda arm: arm.gripper.move_to_jaw_position(50.0), - [ - "state", - "state", - "wherej", - "GraspPlate 70.0 50.0 10.0", - "state", - "state", - "wherej", - ], + ["state", "state", "wherej", "GraspPlate 70.0 50.0 10.0", "state", "state", "wherej"], None, ), ( "move_to_jaw_position_firmware_units", lambda arm: arm.gripper.move_to_jaw_position_firmware_units(120.0), - [ - "state", - "state", - "wherej", - "ReleasePlate 120.0 50.0", - "state", - "state", - "wherej", - ], + ["state", "state", "wherej", "ReleasePlate 120.0 50.0", "state", "state", "wherej"], None, ), ( "move_gripper_joint_position_force", lambda arm: arm.gripper.move_to_jaw_position_firmware_units(70.0, force_sensing=True), - [ - "state", - "state", - "wherej", - "GraspPlate 70.0 50.0 10.0", - "state", - "state", - "wherej", - ], + ["state", "state", "wherej", "GraspPlate 70.0 50.0 10.0", "state", "state", "wherej"], None, ), ( "move_gripper_position", lambda arm: arm.gripper.move_to_jaw_position(50.0, force_sensing=False), - [ - "state", - "state", - "wherej", - "MoveOneAxis 5 70.0 1", - "state", - "state", - "wherej", - ], - None, - ), - ( - "sense_fully_closed", - lambda arm: arm.gripper.sense_fully_closed(), - [ - "IsFullyClosed", - ], + ["state", "state", "wherej", "MoveOneAxis 5 70.0 1", "state", "state", "wherej"], None, ), - ( - "sense_each_fully_closed", - lambda arm: arm.gripper.sense_each_fully_closed(), - [], - ValueError, - ), + ("sense_fully_closed", lambda arm: arm.gripper.sense_fully_closed(), ["IsFullyClosed"], None), + ("sense_each_fully_closed", lambda arm: arm.gripper.sense_each_fully_closed(), [], ValueError), ( "pick_up_at_joint_position", lambda arm: arm.arm.pick_up_at_joint_state(_J, resource_width=85.0), @@ -1237,14 +675,7 @@ def _rail(arm: PreciseFlexDriver) -> PreciseFlexRail: ], None, ), - ( - "send_command", - lambda arm: arm.send_command("pd 2000"), - [ - "pd 2000", - ], - None, - ), + ("send_command", lambda arm: arm.send_command("pd 2000"), ["pd 2000"], None), ( "move_to_location_rail_position_without_rail", lambda arm: arm.arm.move_to_location(_LOC, direction=0.0, rail_position=300.0), @@ -1273,12 +704,7 @@ def _rail(arm: PreciseFlexDriver) -> PreciseFlexRail: ( "rail_move_rail", lambda arm: _rail(arm).move_rail(250.0), - [ - "state", - "state", - "Rail 1 250.0", - "MoveRail 1 1", - ], + ["state", "state", "Rail 1 250.0", "MoveRail 1 1"], None, ), ( @@ -1298,37 +724,19 @@ def _rail(arm: PreciseFlexDriver) -> PreciseFlexRail: ( "rail_move_to_location", lambda arm: arm.arm.move_to_location(_LOC, direction=0.0, rail_position=300.0), - [ - "state", - "state", - "Rail 1 300.0", - "MoveRail 1 1", - "state", - "state", - "wherej", - ], + ["state", "state", "Rail 1 300.0", "MoveRail 1 1", "state", "state", "wherej"], ValueError, ), ( "rail_start_freedrive_mode", lambda arm: arm.arm.start_freedrive_mode(), - [ - "freemode 1", - "freemode 2", - "freemode 3", - "freemode 4", - "freemode 6", - ], + ["freemode 1", "freemode 2", "freemode 3", "freemode 4", "freemode 6"], None, ), ( "rail_request_joint_state", lambda arm: arm.arm.request_joint_state(), - [ - "state", - "state", - "wherej", - ], + ["state", "state", "wherej"], None, ), ( From 6126a88375a03b8f975e71359f6d7e8b44bb363a Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Mon, 2 Nov 2026 17:24:51 +0000 Subject: [PATCH 3/4] PreciseFlexArm: keep _unchecked_fw_set_joint_angles It wraps the controller's locAngles, a real command, even with picks and places now writing a Cartesian station. A wire test pins what it sends, with and without a rail. Co-Authored-By: Claude Opus 5.5 --- .../brooks/precise_flex/driver/features/arm.py | 12 ++++++++++++ pylabrobot/brooks/precise_flex/tests/wire_tests.py | 11 +++++++++++ 2 files changed, 23 insertions(+) diff --git a/pylabrobot/brooks/precise_flex/driver/features/arm.py b/pylabrobot/brooks/precise_flex/driver/features/arm.py index ac34387dd42..7944af25662 100644 --- a/pylabrobot/brooks/precise_flex/driver/features/arm.py +++ b/pylabrobot/brooks/precise_flex/driver/features/arm.py @@ -407,6 +407,18 @@ async def _unchecked_fw_move_one_axis(self, axis: PreciseFlexAxis, position: flo """ await self._driver.send_command(f"MoveOneAxis {int(axis)} {position} {self.profile_index}") + async def _unchecked_fw_set_joint_angles( + self, + station_index: int, + joint_position: JointState, + ) -> None: + """Set joint angles for a station, handling rail configuration.""" + angles_str = " ".join(str(joint_position[axis]) for axis in self._WIRE_ORDER) + if self._driver._has_rail: + # Here the rail's value goes first. + 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: diff --git a/pylabrobot/brooks/precise_flex/tests/wire_tests.py b/pylabrobot/brooks/precise_flex/tests/wire_tests.py index 4ec0150fe61..4da7222f374 100644 --- a/pylabrobot/brooks/precise_flex/tests/wire_tests.py +++ b/pylabrobot/brooks/precise_flex/tests/wire_tests.py @@ -1190,6 +1190,17 @@ async def test_a_rail_arm_is_refused_before_the_station_is_written(self): 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.""" From 2ddc73b2e985fcea4a9b62705d46d6a1ba18cba8 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Wed, 11 Nov 2026 18:42:34 +0000 Subject: [PATCH 4/4] `_guarded_plate_j`: check the wrist where the controller takes it The controller reaches a Cartesian station with the wrist at the heading wrapped to +-180, less the shoulder and the elbow. A station taught a full turn from that made `pickplate` turn the wrist 360 degrees on the way down, past a column check run on the taught wrist. The heading is rounded before it is wrapped, so the check and the station sent cannot differ at exactly +-180. Co-Authored-By: Claude Opus 5.5 --- .../precise_flex/driver/features/arm.py | 17 +++++++++++++--- .../brooks/precise_flex/tests/device_tests.py | 20 +++++++++++++++++++ 2 files changed, 34 insertions(+), 3 deletions(-) diff --git a/pylabrobot/brooks/precise_flex/driver/features/arm.py b/pylabrobot/brooks/precise_flex/driver/features/arm.py index 7944af25662..3f720651522 100644 --- a/pylabrobot/brooks/precise_flex/driver/features/arm.py +++ b/pylabrobot/brooks/precise_flex/driver/features/arm.py @@ -433,7 +433,7 @@ async def _unchecked_fw_set_station_cartesian( 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 = (pose.rotation.z + 180) % 360 - 180 + 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" @@ -2051,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. @@ -2062,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_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 "")