Expose STARBackend.request_iswap_relative_wrist_orientation() (#667)

Author: BioCam

Untitled.ipynb

@@ -0,0 +1,110 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": 2,
+   "id": "6174d785-e532-4df1-ba61-235f0fe9f000",
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "SCARA Arm Position -> X: 134.69, Y: 163.40, Z: 35.00 | Joint Angles: [30°, 45°, -20°], Z Offset: -15\n"
+     ]
+    }
+   ],
+   "source": [
+    "import math\n",
+    "\n",
+    "class SCARARobotArm:\n",
+    "    def __init__(self, link_lengths, base_height=0.0):\n",
+    "        \"\"\"\n",
+    "        Initializes the SCARA robot arm with a dynamic number of revolute joints.\n",
+    "\n",
+    "        :param link_lengths: List of link lengths [L1, L2, ...]\n",
+    "        :param base_height: Base height for the prismatic joint (Z-axis)\n",
+    "        \"\"\"\n",
+    "        self.link_lengths = link_lengths\n",
+    "        self.n_joints = len(link_lengths)\n",
+    "        self.joint_angles = [0.0] * self.n_joints  # In degrees\n",
+    "        self.base_height = base_height\n",
+    "        self.z_offset = 0.0  # Vertical prismatic joint (Z-axis)\n",
+    "\n",
+    "    def set_joint_angles(self, angles):\n",
+    "        \"\"\"\n",
+    "        Sets the revolute joint angles (in degrees).\n",
+    "\n",
+    "        :param angles: List of joint angles, same length as link_lengths\n",
+    "        \"\"\"\n",
+    "        if len(angles) != self.n_joints:\n",
+    "            raise ValueError(\"Number of angles must match number of links.\")\n",
+    "        self.joint_angles = angles\n",
+    "\n",
+    "    def set_z_offset(self, z):\n",
+    "        \"\"\"Set vertical offset for the prismatic joint (Z-axis).\"\"\"\n",
+    "        self.z_offset = z\n",
+    "\n",
+    "    def forward_kinematics(self):\n",
+    "        \"\"\"\n",
+    "        Calculates the end-effector position (x, y, z) based on current joint angles.\n",
+    "        Returns:\n",
+    "            (x, y, z): End-effector Cartesian coordinates\n",
+    "        \"\"\"\n",
+    "        x, y = 0.0, 0.0\n",
+    "        angle_sum = 0.0\n",
+    "\n",
+    "        for i in range(self.n_joints):\n",
+    "            angle_sum += math.radians(self.joint_angles[i])\n",
+    "            x += self.link_lengths[i] * math.cos(angle_sum)\n",
+    "            y += self.link_lengths[i] * math.sin(angle_sum)\n",
+    "\n",
+    "        z = self.base_height + self.z_offset\n",
+    "        return (x, y, z)\n",
+    "\n",
+    "    def __str__(self):\n",
+    "        x, y, z = self.forward_kinematics()\n",
+    "        angles_str = \", \".join([f\"{a}°\" for a in self.joint_angles])\n",
+    "        return (f\"SCARA Arm Position -> X: {x:.2f}, Y: {y:.2f}, Z: {z:.2f} | \"\n",
+    "                f\"Joint Angles: [{angles_str}], Z Offset: {self.z_offset}\")\n",
+    "\n",
+    "# Example usage:\n",
+    "if __name__ == \"__main__\":\n",
+    "    # Create a 3-joint SCARA-like robot (not common, but for flexibility/testing)\n",
+    "    arm = SCARARobotArm(link_lengths=[100, 75, 50], base_height=50)\n",
+    "    arm.set_joint_angles([30, 45, -20])\n",
+    "    arm.set_z_offset(-15)\n",
+    "    print(arm)\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "259ff2bb-7891-4f33-af24-6ec565cd0cc7",
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3 (ipykernel)",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.12.11"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}

pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py

@@ -6763,6 +6763,90 @@ async def iswap_put_plate(
     self._iswap_parked = False
     return command_output
 
+  async def request_iswap_rotation_drive_position_increments(self) -> int:
+    """Query the iSWAP rotation drive position (units: increments) from the firmware."""
+    response = await self.send_command(module="R0", command="RS", fmt="rs######")
+    return cast(int, response["rs"])
+
+  async def request_iswap_rotation_drive_orientation(self) -> "RotationDriveOrientation":
+    """
+    Request the iSWAP rotation drive orientation.
+    This is the orientation of the iSWAP rotation drive (relative to the machine).
+
+    Uses empirically determined increment values:
+      FRONT: -25 ± 50
+      RIGHT: +29068 ± 50
+      LEFT:  -29116 ± 50
+
+    Returns:
+      RotationDriveOrientation: The interpreted rotation orientation (LEFT, FRONT, RIGHT).
+    """
+    # Map motor increments to rotation orientations (constant lookup table).
+    rotation_orientation_to_motor_increment_dict = {
+      STARBackend.RotationDriveOrientation.FRONT: range(-75, 26),
+      STARBackend.RotationDriveOrientation.RIGHT: range(29018, 29119),
+      STARBackend.RotationDriveOrientation.LEFT: range(-29166, -29065),
+    }
+
+    motor_position_increments = await self.request_iswap_rotation_drive_position_increments()
+
+    for orientation, increment_range in rotation_orientation_to_motor_increment_dict.items():
+      if motor_position_increments in increment_range:
+        return orientation
+
+    raise ValueError(
+      f"Unknown rotation orientation: {motor_position_increments}. "
+      f"Expected one of {list(rotation_orientation_to_motor_increment_dict)}."
+    )
+
+  async def request_iswap_wrist_drive_position_increments(self) -> int:
+    """Query the iSWAP wrist drive position (units: increments) from the firmware."""
+    response = await self.send_command(module="R0", command="RT", fmt="rt######")
+    return cast(int, response["rt"])
+
+  async def request_iswap_wrist_drive_orientation(self) -> "WristDriveOrientation":
+    """
+    Request the iSWAP wrist drive orientation.
+    This is the orientation of the iSWAP wrist drive (always in relation to the
+    iSWAP arm/rotation drive).
+
+    e.g.:
+    1) iSWAP RotationDriveOrientation.FRONT (i.e. pointing to the front of the machine) +
+       iSWAP WristDriveOrientation.STRAIGHT (i.e. wrist is also pointing to the front)
+
+    2) iSWAP RotationDriveOrientation.LEFT (i.e. pointing to the left of the machine) +
+       iSWAP WristDriveOrientation.STRAIGHT (i.e. wrist is also pointing to the left)
+
+    3) iSWAP RotationDriveOrientation.FRONT (i.e. pointing to the front of the machine) +
+       iSWAP WristDriveOrientation.RIGHT (i.e. wrist is pointing to the left !)
+
+    The relative wrist orientation is reported as a motor position increment by the STAR
+    firmware. This value is mapped to a `WristDriveOrientation` enum member.
+
+    Returns:
+      WristDriveOrientation: The interpreted wrist orientation
+      (e.g., RIGHT, STRAIGHT, LEFT, REVERSE).
+    """
+
+    # Map motor increments to wrist orientations (constant lookup table).
+    wrist_orientation_to_motor_increment_dict = {
+      STARBackend.WristDriveOrientation.RIGHT: range(-26_627, -26_527),
+      STARBackend.WristDriveOrientation.STRAIGHT: range(-8_804, -8_704),
+      STARBackend.WristDriveOrientation.LEFT: range(9_051, 9_151),
+      STARBackend.WristDriveOrientation.REVERSE: range(26_802, 26_902),
+    }
+
+    motor_position_increments = await self.request_iswap_wrist_drive_position_increments()
+
+    for orientation, increment_range in wrist_orientation_to_motor_increment_dict.items():
+      if motor_position_increments in increment_range:
+        return orientation
+
+    raise ValueError(
+      f"Unknown wrist orientation: {motor_position_increments}. "
+      f"Expected one of {list(wrist_orientation_to_motor_increment_dict)}."
+    )
+
   async def iswap_rotate(
     self,
     rotation_drive: "RotationDriveOrientation",
@@ -7491,13 +7575,13 @@ async def rotate_iswap_rotation_drive(self, orientation: RotationDriveOrientatio
       wp=orientation.value,
     )
 
-  class WristOrientation(enum.Enum):
+  class WristDriveOrientation(enum.Enum):
     RIGHT = 1
     STRAIGHT = 2
     LEFT = 3
     REVERSE = 4
 
-  async def rotate_iswap_wrist(self, orientation: WristOrientation):
+  async def rotate_iswap_wrist(self, orientation: WristDriveOrientation):
     return await self.send_command(
       module="R0",
       command="TP",