Expose STARBackend.iswap_rotation_drive_request_position()#1014
Merged
BioCam merged 9 commits intoPyLabRobot:mainfrom Apr 27, 2026
Merged
Conversation
Contributor
There was a problem hiding this comment.
Pull request overview
This PR extends the Hamilton STAR backend’s iSWAP querying capabilities by exposing a deck-coordinate position query for the iSWAP rotation drive (joint 1) and making rotation/wrist orientation inference more tolerant to per-machine calibration drift.
Changes:
- Cache and use an EEPROM-derived X-offset (
kg) to compute the iSWAP rotation drive’s deck X coordinate. - Add
iswap_rotation_drive_request_position()to return the rotation drive position in deck coordinates. - Replace fixed increment windows for rotation/wrist orientation detection with nearest-neighbour classification + tolerance and improved error messages.
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
Comment on lines
+10039
to
+10086
| async def _iswap_rotation_drive_request_x_offset(self) -> float: | ||
| """Read the X-offset i.e. X-axis center <-> iSWAP rotation drive, in mm. | ||
|
|
||
| Stored in the master EEPROM as parameter `kg`. | ||
| Default: 34.0 mm, but typically tuned per machine during service calibration. | ||
| Required for deriving the iSWAP rotation drive's deck X coordinate from | ||
| the X-arm carriage center. | ||
| Cached on the backend as `_iswap_rotation_drive_x_offset_mm` during setup. | ||
| """ | ||
| if not self.extended_conf.left_x_drive.iswap_installed: | ||
| raise RuntimeError("iSWAP is not installed") | ||
| resp = await self.send_command(module="C0", command="RA", ra="kg", fmt="kg###") | ||
| return cast(int, resp["kg"]) / 10.0 | ||
|
|
||
| # 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_position(self) -> Coordinate: | ||
| """Position of the iSWAP rotation drive (joint 1) in deck coordinates, mm. | ||
|
|
||
| Composition: | ||
| x = request_left_x_arm_position() - _iswap_rotation_drive_x_offset_mm | ||
| y = iswap_rotation_drive_request_y() | ||
| z = request_iswap_z_position() + iswap_rotation_drive_z_offset_above_finger_mm | ||
|
|
||
| The Z offset (13 mm) is the structural drop from the rotation drive | ||
| plane to the gripper finger plane. R0 RZ is Hamilton-calibrated to | ||
| the finger plane, so we add 13 mm to recover the rotation drive's | ||
| true Z. | ||
| """ | ||
|
|
||
| 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: | ||
| raise ValueError("Call setup() first") | ||
|
|
||
| x_arm_center = await self.request_left_x_arm_position() | ||
| rotation_drive_y = await self.iswap_rotation_drive_request_y() | ||
| finger_plane_z = (await self.request_iswap_position()).z | ||
|
|
||
| return Coordinate( | ||
| x=x_arm_center - self._iswap_rotation_drive_x_offset_mm, | ||
| y=rotation_drive_y, | ||
| z=finger_plane_z + self.iswap_rotation_drive_z_offset_above_finger_mm, | ||
| ) |
There was a problem hiding this comment.
New iSWAP position/orientation logic is introduced here (X-offset EEPROM read + deck-coordinate composition, and nearest-neighbour orientation classification with tolerances), but there are no unit tests covering these behaviors. Since this repo already has Hamilton STAR backend tests, please add tests that mock the relevant firmware queries and assert the computed Coordinate and orientation classification (including boundary/tolerance cases).
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Another one for the "Tame the iSWAP" (Part 3)