Add FK-based STARBackend.iswap_request_pose() + iswap_request_joint_state()

[BioCam]

Adds a forward-kinematics-based iSWAP grip-center accessor as an alternative to the legacy request_iswap_position() (C0 QG), which is empirically broken in 9 of 13 tested machine states. The new path queries each joint encoder directly and computes the grip center from per-machine EEPROM calibration — no firmware-state dependency.

Three new entry points on STARBackend:

class STARBackend:
    class iSWAPAxis(IntEnum):   # X, Y, Z, ROTATION, WRIST, GRIPPER + predicates

    async def iswap_request_joint_state(self) -> Dict[int, float]:
        """Snapshot of current joint positions keyed by iSWAPAxis."""

    async def iswap_request_pose(self) -> CartesianCoords:
        """Grip-center position + gripper yaw, computed via FK."""

The legacy request_iswap_position() is untouched — no deprecation, no behavior change. Callers can migrate at their own pace.

Why

request_iswap_position() sends C0 QG, which on our test STAR (iSWAP-equipped) returns the rotation drive position rather than the grip center in most states. Confirmed on hardware:

Scenario	OLD request_iswap_position	NEW iswap_request_pose.location	Delta
Parked (W=91°, T=−135°)	(745, 627.4, 272.5) ← rotation drive	(886, 493, 272.5), yaw=−88.65°	195 mm
W=+45°, T=0° (off-stop)	(745, 300, 272.5) ← rotation drive	(980, 203, 272.5), yaw=+0.01°	255 mm
W=−45°, T=0° (off-stop)	(745, 300, 272.5) ← rotation drive	(648, 65, 272.5), yaw=−90.0°	255 mm
W=FRONT, T=STRAIGHT	(745, 24, 272.5)	(745, 24.5, 272.5)	0.5 mm
W=LEFT, T=STRAIGHT	(469, 300, 272.5)	(469.5, 300, 272.5)	0.5 mm
W=FRONT, T=LEFT	(883, 162, 272.5)	(882.7, 164.5, 272.5)	2.5 mm
5 post-park-state scenarios (after park, after FY, 3 explicit retries)	(745, 627.4, 272.5) every call	(886, 493, 272.5) every call	195 mm × 5

Failure modes: C0 QG returns sub-mm-correct values only when both W and T joints are at named EEPROM stops and the kinematic configuration is one of a few specific arrangements. Off-stop W angles, parked states, and certain W+T combinations all degrade silently to "return rotation drive position." Worse, the failures are deterministic (repeatable across 5 consecutive calls in Phase 8) — so any "is the value stable?" check passes while the value itself is 195+ mm wrong.

The "0.5 mm" residual in the agreement scenarios is also informative: even when C0 QG works, it's using nominal Hamilton defaults (L=138, ±45°) rather than the EEPROM-stored per-machine values (L1=137.8, L2=137.7, STRAIGHT=−45.01°, LEFT=+45.94° on this machine). The FK is per-machine calibrated; C0 QG is not.

How

iswap_request_pose is two operations:

1. iswap_request_joint_state() composes the six existing per-axis read methods (iswap_rotation_drive_request_x/y/z/angle, iswap_wrist_drive_request_angle, iswap_gripper_request_width) into a Dict[iSWAPAxis, float].

2. _iswap_fk(joints, l1, l2, wrist_straight) is a static, pure function that runs a 2-link SCARA forward kinematics:

   α₁ = W − 90°             (link 1 deck angle; CCW from +x)
   α₂ = α₁ + (T − T_STRAIGHT_eeprom)
   grip_x = base_x + L₁·cos α₁ + L₂·cos α₂
   grip_y = base_y + L₁·sin α₁ + L₂·sin α₂
   grip_z = base_z − 13 mm (finger-plane offset)
   yaw    = α₂                (link 2 deck direction)
   

   Sign convention follows right-hand rule about +z (CCW positive looking down) — same phrasing PF400 and KX2 modules use on v1b1.

L₁, L₂, and the wrist STRAIGHT calibration are read from EEPROM once during setup() and cached on the backend; subsequent iswap_request_pose calls don't re-query EEPROM.

API surface added

Symbol	Kind
STARBackend.iSWAPAxis(IntEnum)	Nested enum: X, Y, Z, ROTATION, WRIST, GRIPPER + is_in_kinematic_chain and is_revolute properties
STARBackend._iswap_fk(...)	Static method, pure FK; testable without firmware
STARBackend.iswap_request_joint_state()	Async; returns Dict[int, float]
STARBackend.iswap_request_pose()	Async; returns pylabrobot.arms.standard.CartesianCoords
STARBackend._iswap_link_1_length, _iswap_link_2_length	Private; cached at setup

CartesianCoords already exists on main (pylabrobot/arms/standard.py). Setup-time caching is added inside the existing set_up_iswap() block.

Testing

Unit tests (STAR_tests.py::TestiSWAP*, 7 tests):

• TestiSWAPForwardKinematics (4): hand-computed expected poses vs _iswap_fk output for FRONT, LEFT, FRONT+RIGHT, REVERSE configurations.
• TestiSWAPAxisPredicates (1): the kinematic-chain partition (X/Y/Z/ROTATION/WRIST in, GRIPPER out).
• TestiSWAPRequestJointState (1): mocked per-axis methods → verified dict shape.
• TestiSWAPRequestPose (1): full mocked E2E path joints → FK → pose, including EEPROM-STRAIGHT drift propagation.

No tautological tests (e.g. "raises when condition X" mirroring if condition X: raise). Every test independently computes its expected output.

Empirical (real STAR, log + notebook attached):

• 13 scenarios on a real iSWAP-equipped STAR (full safety prelude: channels + 96-head to Z safety, iSWAP carriage to (X=745, Y=300)).
• 9 disagreements ranging 2.5–255 mm (8 of the 9 at ~195 mm or worse) where C0 QG returns rotation drive position; 4 agreements with the new FK consistently ~0.5 mm more accurate (the 0.5 mm comes from FK using EEPROM-calibrated L1=137.8/L2=137.7, while C0 QG uses nominal 138 mm).
• Both APIs verified deterministic (0.0 mm deviation across 5 repeated calls).
• See _dev/test_iswap_fk_pose.ipynb and _dev/logs/2026-05-15_15-58-08_iswap_fk_testing.log in the cb-protocol-library sister repo.

Portability to v1b1

Drop-in for v1b1's arm-capability framework:

v1b1 contract	This branch	Port effort
request_gripper_pose() -> CartesianPose (from _BaseArmBackend)	iswap_request_pose() -> CartesianCoords	rename method, rename type (CartesianCoords → CartesianPose)
request_joint_position() -> JointPose=Dict[int,float] (from HasJoints)	iswap_request_joint_state() -> Dict[int, float]	rename method
Per-arm Axis(IntEnum) (cf. KX2 paa/kx2/config.py::Axis)	STARBackend.iSWAPAxis	unnest + rename to Axis in iswap/ subpackage
Per-arm fk(joints, config) -> CartesianPose (cf. PF400 brooks/kinematics.py, KX2 paa/kx2/kinematics.py)	STARBackend._iswap_fk (positional params)	move to iswap/kinematics.py; bundle params into iSWAPConfig
Generic kinematics utilities (PR #880 capabilities/arms/kinematics.py: gripper_speed, sample_gripper_speed_along_trajectory, ...)	not used here; consumes Callable[[Dict[K,float]], Coordinate]	_iswap_fk already matches that signature — drop-in

No FK math change at port time. No test math change. Mostly find-and-replace.

Follow-ups (separate PRs)

1. Consolidate iSWAP setup state into iSWAPInformation dataclass — bundles the 6+ scattered _iswap_* cached attrs (link lengths, predefined increments, X offset, parking Y, fw version) into one dataclass, parallel to the existing Head96Information. ~150–250 LOC; enables STAR_chatterbox.py to populate a single _DEFAULT_ISWAP_INFORMATION instead of 6+ assignments.
2. Chatterbox / simulation support — depends on (1); adds iswap_request_pose() working against the chatterbox.
3. Eventual v1b1 port — mechanical (see table above).

Test plan / checklist

• Math: _iswap_fk against hand-computed expected values at 4 canonical configurations
• Math: SCARA planarity invariant (rotation.x = y = 0)
• Math: per-machine EEPROM STRAIGHT drift propagates correctly
• Wiring: joint state composition + axis enum keys
• Wiring: full E2E pose call against mocked per-axis methods
• Setup: cached state populated when iSWAP installed
• Real STAR: 13 scenarios, agreement table generated
• Real STAR: deterministic across repeated calls (Phase 8)
• make format, make lint, make typecheck, full STAR_tests.py (76/76) all pass

Files

• pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py (+150) — iSWAPAxis enum, cached attrs, setup population, _iswap_fk, two new async methods.
• pylabrobot/liquid_handling/backends/hamilton/STAR_tests.py (+152) — TestiSWAP* (4 classes, 7 tests).

[Copilot]

Pull request overview

Adds a forward-kinematics-based iSWAP pose accessor on STARBackend as a more reliable alternative to the legacy request_iswap_position() (C0 QG) firmware call, which empirically returns rotation-drive position rather than the grip center in most states. The new path queries each joint encoder and computes the grip center via a 2-link SCARA FK using per-machine EEPROM-calibrated link lengths and STRAIGHT angle. The legacy method is left untouched.

Changes:

• Introduce STARBackend.iSWAPAxis IntEnum (X/Y/Z/ROTATION/WRIST/GRIPPER + is_in_kinematic_chain/is_revolute predicates), and cache _iswap_link_1_length/_iswap_link_2_length from EEPROM in set_up_iswap().
• Add three entry points: pure static _iswap_fk(...), async iswap_request_joint_state(), and async iswap_request_pose() returning CartesianCoords.
• Add unit tests (TestiSWAPForwardKinematics, TestiSWAPAxisPredicates, TestiSWAPRequestJointState, TestiSWAPRequestPose) covering canonical FK configurations, axis predicates, joint composition, and the full mocked I/O→FK path.

Reviewed changes

Copilot reviewed 2 out of 2 changed files in this pull request and generated no comments.

File	Description
pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py	Adds nested iSWAPAxis enum, EEPROM link-length caching at setup, static _iswap_fk SCARA forward-kinematics, and async iswap_request_joint_state/iswap_request_pose methods.
pylabrobot/liquid_handling/backends/hamilton/STAR_tests.py	Adds four TestiSWAP* test classes covering FK math at canonical configurations, axis predicates, and mocked end-to-end joint/pose retrieval.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.