diff --git a/Untitled.ipynb b/Untitled.ipynb new file mode 100644 index 00000000000..202c9f4fec9 --- /dev/null +++ b/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 +} diff --git a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py index 7d019f31e7f..3019d8eb5b2 100644 --- a/pylabrobot/liquid_handling/backends/hamilton/STAR_backend.py +++ b/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",