From 48a66324df2ab1c945bab8c07d45ec5fa35e19fd Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Thu, 30 Apr 2026 19:37:19 +0100 Subject: [PATCH 1/5] expose iswap_rotation_drive_move_y --- .../backends/hamilton/STAR_backend.py | 81 +++++++++++++++++++ 1 file changed, 81 insertions(+) diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index 7a2dd59ed88..8731015ce56 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -9775,6 +9775,8 @@ async def iswap_request_link_2_length(self) -> float: iswap_rotation_drive_min_increment = -30032 # ~ -93 deg iswap_rotation_drive_max_increment = 30032 # ~ +93 deg iswap_rotation_drive_deg_per_increment = 0.00309619077 + iswap_rotation_drive_y_speed_increment_range = (50, 8_000) + iswap_rotation_drive_diameter = 30.5 class RotationDriveOrientation(enum.Enum): LEFT = 1 @@ -9822,6 +9824,85 @@ async def iswap_rotation_drive_request_y(self) -> float: iswap_y_pos = resp["ry"][1] # 0 = FW counter, 1 = HW counter return round(STARBackend.y_drive_increment_to_mm(iswap_y_pos), 1) + async def iswap_rotation_drive_move_y( + self, + y: float, + speed: float = 220.0, + acceleration_level: int = 2, + current_protection_limiter: int = 7, + make_space: bool = False, + ): + """Move the iSWAP rotation drive to an absolute Y position (R0 YA). + + Args: + y: Target Y coordinate in mm. + speed: Max velocity in mm/sec. Default 220.0. + acceleration_level: Acceleration index, 1 or 2. Default 2. + current_protection_limiter: Motor current limit, 0-7. Default 7. + make_space: If True, reposition pipetting channels in a single + synchronous JY move when channel 0 is in the way and can be cleared. + If False, raise so the caller decides. + """ + if not self.extended_conf.left_x_drive.iswap_installed: + raise RuntimeError("iSWAP is not installed") + + iswap_radius = STARBackend.iswap_rotation_drive_diameter / 2 + channel_0_radius = self._channels_minimum_y_spacing[0] / 2 + channel_0_y = await self.request_y_pos_channel_n(0) + + compressed_channel_0_y = self.extended_conf.left_arm_min_y_position + sum( + self._channels_minimum_y_spacing[1:] + ) + + max_y = self.extended_conf.pip_maximal_y_position + absolute_min_y = self.extended_conf.left_arm_min_y_position + if not (absolute_min_y <= y <= max_y): + raise ValueError(f"y must be between {absolute_min_y} and {max_y} mm, got {y} mm") + + target_channel_0_y = y - channel_0_radius - iswap_radius + if channel_0_y > target_channel_0_y: + if target_channel_0_y < compressed_channel_0_y: + raise ValueError( + f"y={y} mm is unreachable: would require channel 0 at " + f"{target_channel_0_y} mm, below the compressed floor " + f"{compressed_channel_0_y} mm" + ) + if not make_space: + raise ValueError( + f"y={y} mm requires channel 0 at <= {target_channel_0_y} mm " + f"(currently {channel_0_y} mm); pass make_space=True to " + f"reposition channels" + ) + await self.move_all_channels_in_z_safety() + await self.position_channels_in_y_direction({0: target_channel_0_y}, make_space=True) + + speed_increments = STARBackend.mm_to_y_drive_increment(speed) + speed_min, speed_max = STARBackend.iswap_rotation_drive_y_speed_increment_range + if not (speed_min <= speed_increments <= speed_max): + raise ValueError( + f"speed must be between " + f"{STARBackend.y_drive_increment_to_mm(speed_min)} and " + f"{STARBackend.y_drive_increment_to_mm(speed_max)} mm/sec, " + f"got {speed} mm/sec" + ) + + if not (1 <= acceleration_level <= 2): + raise ValueError(f"acceleration_level must be between 1 and 2, got {acceleration_level}") + + if not (0 <= current_protection_limiter <= 7): + raise ValueError( + f"current_protection_limiter must be between 0 and 7, got {current_protection_limiter}" + ) + + await self.send_command( + module="R0", + command="YA", + ya=f"{round(STARBackend.mm_to_y_drive_increment(y)):05}", + yv=f"{round(speed_increments):04}", + yr=f"{int(acceleration_level)}", + yw=f"{int(current_protection_limiter)}", + ) + # Vertical drop from the iSWAP rotation drive plane to the gripper finger # plane. R0 RZ is calibrated to the finger plane; the rotation drive sits # 13 mm above it. From 4d121881a8f9139724fffa0060a4d92fc087d171 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Thu, 30 Apr 2026 19:47:21 +0100 Subject: [PATCH 2/5] shuffle iswap move y next below move x --- .../backends/hamilton/STAR_backend.py | 118 +++++++++--------- 1 file changed, 59 insertions(+), 59 deletions(-) diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index a59bb89cad4..70928ec4f9b 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -9871,6 +9871,64 @@ async def iswap_rotation_drive_request_y(self) -> float: iswap_y_pos = resp["ry"][1] # 0 = FW counter, 1 = HW counter return round(STARBackend.y_drive_increment_to_mm(iswap_y_pos), 1) + # Vertical drop from the iSWAP rotation drive plane to the gripper finger + # plane. R0 RZ is calibrated to the finger plane; the rotation drive sits + # 13 mm above it. + iswap_rotation_drive_z_offset_above_finger_mm = 13.0 + + async def iswap_rotation_drive_request_z(self) -> float: + """Request iSWAP rotation drive Z position (deck coordinates), in mm. + + Adds the 13 mm structural offset to the gripper finger plane (C0 QG). + """ + if not self.extended_conf.left_x_drive.iswap_installed: + raise RuntimeError("iSWAP is not installed") + finger_plane_z = (await self.request_iswap_position()).z + return finger_plane_z + STARBackend.iswap_rotation_drive_z_offset_above_finger_mm + + async def iswap_rotation_drive_request_position(self) -> Coordinate: + """Position of the iSWAP rotation drive (joint 1) in deck coordinates, mm.""" + return Coordinate( + x=await self.iswap_rotation_drive_request_x(), + y=await self.iswap_rotation_drive_request_y(), + z=await self.iswap_rotation_drive_request_z(), + ) + + async def experimental_iswap_rotation_drive_move_x( + self, + x: float, + acceleration_level: int = 3, + current_protection_limiter: int = 7, + ): + """Move the iSWAP rotation drive to an absolute X position (deck coordinates). + + Thin wrapper around `x_arm_move` that translates rotation-drive X into + X-arm carriage X using the cached `kg` offset. + + Args: + x: Target rotation-drive X coordinate in mm. + acceleration_level: Acceleration index (hardware units), 1-5. Default 3. + current_protection_limiter: Motor current limit (hardware units), 0-7. Default 7. + """ + # TODO: remove "experimental_" prefix once x_arm_move has been optimised + + if not self.extended_conf.left_x_drive.iswap_installed: + raise RuntimeError("iSWAP is not installed") + if self._iswap_rotation_drive_x_offset_mm is None: + self._iswap_rotation_drive_x_offset_mm = await self._iswap_rotation_drive_request_x_offset() + kg = self._iswap_rotation_drive_x_offset_mm + + x_min = 90.0 - kg + x_max = 1350.0 - kg + if not (x_min <= x <= x_max): + raise ValueError(f"x must be between {x_min} and {x_max} mm, is {x}") + + return await self.experimental_x_arm_move( + x=x + kg, + acceleration_level=acceleration_level, + current_protection_limiter=current_protection_limiter, + ) + async def iswap_rotation_drive_move_y( self, y: float, @@ -9879,7 +9937,7 @@ async def iswap_rotation_drive_move_y( current_protection_limiter: int = 7, make_space: bool = False, ): - """Move the iSWAP rotation drive to an absolute Y position (R0 YA). + """Move the iSWAP rotation drive to an absolute Y position. Args: y: Target Y coordinate in mm. @@ -9950,64 +10008,6 @@ async def iswap_rotation_drive_move_y( yw=f"{int(current_protection_limiter)}", ) - # Vertical drop from the iSWAP rotation drive plane to the gripper finger - # plane. R0 RZ is calibrated to the finger plane; the rotation drive sits - # 13 mm above it. - iswap_rotation_drive_z_offset_above_finger_mm = 13.0 - - async def iswap_rotation_drive_request_z(self) -> float: - """Request iSWAP rotation drive Z position (deck coordinates), in mm. - - Adds the 13 mm structural offset to the gripper finger plane (C0 QG). - """ - if not self.extended_conf.left_x_drive.iswap_installed: - raise RuntimeError("iSWAP is not installed") - finger_plane_z = (await self.request_iswap_position()).z - return finger_plane_z + STARBackend.iswap_rotation_drive_z_offset_above_finger_mm - - async def iswap_rotation_drive_request_position(self) -> Coordinate: - """Position of the iSWAP rotation drive (joint 1) in deck coordinates, mm.""" - return Coordinate( - x=await self.iswap_rotation_drive_request_x(), - y=await self.iswap_rotation_drive_request_y(), - z=await self.iswap_rotation_drive_request_z(), - ) - - async def experimental_iswap_rotation_drive_move_x( - self, - x: float, - acceleration_level: int = 3, - current_protection_limiter: int = 7, - ): - """Move the iSWAP rotation drive to an absolute X position (deck coordinates). - - Thin wrapper around `x_arm_move` that translates rotation-drive X into - X-arm carriage X using the cached `kg` offset. - - Args: - x: Target rotation-drive X coordinate in mm. - acceleration_level: Acceleration index (hardware units), 1-5. Default 3. - current_protection_limiter: Motor current limit (hardware units), 0-7. Default 7. - """ - # TODO: remove "experimental_" prefix once x_arm_move has been optimised - - if not self.extended_conf.left_x_drive.iswap_installed: - raise RuntimeError("iSWAP is not installed") - if self._iswap_rotation_drive_x_offset_mm is None: - self._iswap_rotation_drive_x_offset_mm = await self._iswap_rotation_drive_request_x_offset() - kg = self._iswap_rotation_drive_x_offset_mm - - x_min = 90.0 - kg - x_max = 1350.0 - kg - if not (x_min <= x <= x_max): - raise ValueError(f"x must be between {x_min} and {x_max} mm, is {x}") - - return await self.experimental_x_arm_move( - x=x + kg, - acceleration_level=acceleration_level, - current_protection_limiter=current_protection_limiter, - ) - async def iswap_rotation_drive_request_predefined_positions(self) -> Dict[str, int]: """Read the iSWAP rotation drive (W) predefined-position table from EEPROM. From 561bca4206e6a4dfad4cb29df1b9a7d56eb40a60 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 13:02:39 -0700 Subject: [PATCH 3/5] Expose `STARBackend.iswap_rotation_drive_move_z()` Adds the absolute Z-axis sibling to `iswap_rotation_drive_move_x` (#1018) and `iswap_rotation_drive_move_y` (#1020). Wraps `R0 ZA` and accepts the rotation-drive plane Z (matching `iswap_rotation_drive_request_z`); the 13 mm offset to the gripper finger plane is applied internally. `acceleration` is exposed in mm/sec^2. Co-Authored-By: Claude Opus 4.7 (1M context) --- .../backends/hamilton/STAR_backend.py | 82 +++++++++++++++++++ 1 file changed, 82 insertions(+) diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index 70928ec4f9b..9b774aea921 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -9875,6 +9875,14 @@ async def iswap_rotation_drive_request_y(self) -> float: # plane. R0 RZ is calibrated to the finger plane; the rotation drive sits # 13 mm above it. iswap_rotation_drive_z_offset_above_finger_mm = 13.0 + # R0 ZA firmware spec (SFGT.3112): za=-187..+26661 incr (default 24600), + # zv=50..15000 incr/sec (default 11000), zr=5..999 in 1000 incr/sec^2 units + # (default 60), zw=0..7 (default 6). Position is in finger-plane coords; + # `iswap_rotation_drive_move_z` adds the 13 mm offset for the user. + iswap_rotation_drive_z_min_increment = -187 + iswap_rotation_drive_z_max_increment = 26_661 + iswap_rotation_drive_z_speed_increment_range = (50, 15_000) + iswap_rotation_drive_z_acceleration_increment_range = (5, 999) async def iswap_rotation_drive_request_z(self) -> float: """Request iSWAP rotation drive Z position (deck coordinates), in mm. @@ -10008,6 +10016,80 @@ async def iswap_rotation_drive_move_y( yw=f"{int(current_protection_limiter)}", ) + async def iswap_rotation_drive_move_z( + self, + z: float, + speed: float = 118.0, + acceleration: float = 643.66, + current_protection_limiter: int = 6, + ): + """Move the iSWAP rotation drive to an absolute Z position (deck coordinates). + + `z` is the rotation-drive plane Z, matching what + `iswap_rotation_drive_request_z` returns. The 13 mm offset to the gripper + finger plane (where R0 ZA is calibrated) is applied internally. + + Args: + z: Target rotation-drive Z coordinate in mm. + speed: Max velocity in mm/sec. Default 118.0 (firmware default). + acceleration: Acceleration in mm/sec^2. Default 643.66 + (firmware default 60 in 1000 incr/sec^2 units). + current_protection_limiter: Motor current limit, 0-7. Default 6 + (firmware default). + """ + if not self.extended_conf.left_x_drive.iswap_installed: + raise RuntimeError("iSWAP is not installed") + + z_min_incr = STARBackend.iswap_rotation_drive_z_min_increment + z_max_incr = STARBackend.iswap_rotation_drive_z_max_increment + absolute_min_z = ( + STARBackend.z_drive_increment_to_mm(z_min_incr) + + STARBackend.iswap_rotation_drive_z_offset_above_finger_mm + ) + absolute_max_z = ( + STARBackend.z_drive_increment_to_mm(z_max_incr) + + STARBackend.iswap_rotation_drive_z_offset_above_finger_mm + ) + if not (absolute_min_z <= z <= absolute_max_z): + raise ValueError(f"z must be between {absolute_min_z} and {absolute_max_z} mm, got {z} mm") + + finger_plane_z = z - STARBackend.iswap_rotation_drive_z_offset_above_finger_mm + z_increments = STARBackend.mm_to_z_drive_increment(finger_plane_z) + + speed_increments = STARBackend.mm_to_z_drive_increment(speed) + speed_min, speed_max = STARBackend.iswap_rotation_drive_z_speed_increment_range + if not (speed_min <= speed_increments <= speed_max): + raise ValueError( + f"speed must be between " + f"{STARBackend.z_drive_increment_to_mm(speed_min)} and " + f"{STARBackend.z_drive_increment_to_mm(speed_max)} mm/sec, " + f"got {speed} mm/sec" + ) + + acceleration_increments = STARBackend.mm_to_z_drive_increment(acceleration / 1000) + accel_min, accel_max = STARBackend.iswap_rotation_drive_z_acceleration_increment_range + if not (accel_min <= acceleration_increments <= accel_max): + raise ValueError( + f"acceleration must be between " + f"{STARBackend.z_drive_increment_to_mm(accel_min * 1000)} and " + f"{STARBackend.z_drive_increment_to_mm(accel_max * 1000)} mm/sec^2, " + f"got {acceleration} mm/sec^2" + ) + + if not (0 <= current_protection_limiter <= 7): + raise ValueError( + f"current_protection_limiter must be between 0 and 7, got {current_protection_limiter}" + ) + + await self.send_command( + module="R0", + command="ZA", + za=f"{round(z_increments):+06}", + zv=f"{round(speed_increments):05}", + zr=f"{round(acceleration_increments):03}", + zw=f"{int(current_protection_limiter)}", + ) + async def iswap_rotation_drive_request_predefined_positions(self) -> Dict[str, int]: """Read the iSWAP rotation drive (W) predefined-position table from EEPROM. From f6f9bda38adccbec404c895cecae9d3f51cb4354 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Fri, 1 May 2026 13:39:01 +0100 Subject: [PATCH 4/5] clarify `bottom` return (per LFB PLR expectation) PLR users expect left-front-bottom referencing; for channels this changes to center-center in x-y but in z people still expect bottom; since we are referencing the actual rotation drive here and its bottom is 13 mm above the finger_z_plane I have added a short comment to make this clearer --- .../liquid_handling/backends/hamilton/STAR_backend.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index 6f3b1e60833..5d2519756b4 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -9876,10 +9876,9 @@ async def iswap_rotation_drive_request_y(self) -> float: # plane. R0 RZ is calibrated to the finger plane; the rotation drive sits # 13 mm above it. iswap_rotation_drive_z_offset_above_finger_mm = 13.0 - # R0 ZA firmware spec (SFGT.3112): za=-187..+26661 incr (default 24600), - # zv=50..15000 incr/sec (default 11000), zr=5..999 in 1000 incr/sec^2 units - # (default 60), zw=0..7 (default 6). Position is in finger-plane coords; - # `iswap_rotation_drive_move_z` adds the 13 mm offset for the user. + + # Position is in finger-plane coords; `iswap_rotation_drive_move_z` adds + # the 13 mm offset to return the bottom position of the rotation drive. iswap_rotation_drive_z_min_increment = -187 iswap_rotation_drive_z_max_increment = 26_661 iswap_rotation_drive_z_speed_increment_range = (50, 15_000) From a1b6e381c1356a63aeaddf19cc9f4e5309394094 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Fri, 1 May 2026 13:40:26 +0100 Subject: [PATCH 5/5] `make format` --- pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index 5d2519756b4..fca96c26d34 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py @@ -9876,7 +9876,7 @@ async def iswap_rotation_drive_request_y(self) -> float: # plane. R0 RZ is calibrated to the finger plane; the rotation drive sits # 13 mm above it. iswap_rotation_drive_z_offset_above_finger_mm = 13.0 - + # Position is in finger-plane coords; `iswap_rotation_drive_move_z` adds # the 13 mm offset to return the bottom position of the rotation drive. iswap_rotation_drive_z_min_increment = -187