diff --git a/pylabrobot/arms/__init__.py b/pylabrobot/arms/__init__.py new file mode 100644 index 00000000000..9d2ef1bda47 --- /dev/null +++ b/pylabrobot/arms/__init__.py @@ -0,0 +1,5 @@ +from .arm import * +from .orientable_arm import * +from .joint_arm import * +from .backend import * +from .standard import * diff --git a/pylabrobot/arms/arm.py b/pylabrobot/arms/arm.py new file mode 100644 index 00000000000..cccee6ad676 --- /dev/null +++ b/pylabrobot/arms/arm.py @@ -0,0 +1,426 @@ +from dataclasses import dataclass +from typing import List, Literal, Optional, Tuple, Union + +import logging + +from pylabrobot.arms.backend import _BaseArmBackend, GripperArmBackend +from pylabrobot.arms.standard import GripDirection +from pylabrobot.device import Device +from pylabrobot.resources import ( + Coordinate, + Lid, + Plate, + PlateAdapter, + Resource, + ResourceHolder, + ResourceStack, + Trash, +) +from pylabrobot.resources.rotation import Rotation +from pylabrobot.legacy.tilting.tilter import Tilter +from pylabrobot.serializer import SerializableMixin + + +logger = logging.getLogger(__name__) + +GripOrientation = Union[GripDirection, float] + + +@dataclass +class _PickedUpState: + resource: Resource + offset: Coordinate + pickup_distance_from_top: float + resource_width: float + rotation: Rotation = Rotation() + + +class _BaseArm(Device): + """Base class for all arm types. Not instantiated directly.""" + + def __init__(self, backend, reference_resource: Resource): + super().__init__(backend=backend) + self.backend: _BaseArmBackend = backend + self._reference_resource = reference_resource + self._picked_up: Optional[_PickedUpState] = None + self._holding_resource_width: Optional[float] = None + + async def setup(self, **backend_kwargs): + await super().setup(**backend_kwargs) + self._picked_up = None + self._holding_resource_width = None + + async def stop(self): + await super().stop() + self._picked_up = None + self._holding_resource_width = None + + def _state_updated(self): + pass + + @property + def holding(self) -> bool: + return self._holding_resource_width is not None + + def get_picked_up_resource(self) -> Optional[Resource]: + if self._picked_up is not None: + return self._picked_up.resource + return None + + # -- gripper / motion control ----------------------------------------------- + + async def open_gripper( + self, gripper_width: float, backend_params: Optional[SerializableMixin] = None + ) -> None: + return await self.backend.open_gripper( + gripper_width=gripper_width, backend_params=backend_params + ) + + async def close_gripper( + self, gripper_width: float, backend_params: Optional[SerializableMixin] = None + ) -> None: + return await self.backend.close_gripper( + gripper_width=gripper_width, backend_params=backend_params + ) + + async def is_gripper_closed(self, backend_params: Optional[SerializableMixin] = None) -> bool: + return await self.backend.is_gripper_closed(backend_params=backend_params) + + async def halt(self, backend_params: Optional[SerializableMixin] = None) -> None: + """Stop any ongoing movement of the arm.""" + return await self.backend.halt(backend_params=backend_params) + + async def park(self, backend_params: Optional[SerializableMixin] = None) -> None: + """Park the arm to its default position.""" + return await self.backend.park(backend_params=backend_params) + + # -- holding state ----------------------------------------------------------- + + def _begin_holding(self, resource_width: float): + if self.holding: + name = self._picked_up.resource.name if self._picked_up else "" + raise RuntimeError(f"Already holding{' ' + name if name else ''}") + self._holding_resource_width = resource_width + + def _end_holding(self): + self._picked_up = None + self._holding_resource_width = None + + # -- coordinate computation ------------------------------------------------- + + def _pickup_location( + self, + resource: Resource, + offset: Coordinate, + pickup_distance_from_top: float, + ) -> Coordinate: + assert self._reference_resource is not None + center = resource.center().rotated(resource.get_absolute_rotation()) + if resource.is_in_subtree_of(self._reference_resource): + loc = resource.get_location_wrt(self._reference_resource, "l", "f", "t") + center + offset + else: + loc = center + offset + return Coordinate(loc.x, loc.y, loc.z - pickup_distance_from_top) + + def _destination_location( + self, + resource: Resource, + destination: Union[ResourceStack, ResourceHolder, Resource, Coordinate], + resource_rotation_wrt_destination_wrt_local: float, + ) -> Coordinate: + assert self._reference_resource is not None + if isinstance(destination, ResourceStack): + assert destination.direction == "z" + return destination.get_location_wrt( + self._reference_resource + ) + destination.get_new_child_location( + resource.rotated(z=resource_rotation_wrt_destination_wrt_local) + ).rotated(destination.get_absolute_rotation()) + elif isinstance(destination, Coordinate): + return destination + elif isinstance(destination, ResourceHolder): + if destination.resource is not None and destination.resource is not resource: + raise RuntimeError("Destination already has a plate") + child_wrt_parent = destination.get_default_child_location( + resource.rotated(z=resource_rotation_wrt_destination_wrt_local) + ).rotated(destination.get_absolute_rotation()) + return destination.get_location_wrt(self._reference_resource) + child_wrt_parent + elif isinstance(destination, PlateAdapter): + if not isinstance(resource, Plate): + raise ValueError("Only plates can be moved to a PlateAdapter") + adjusted_plate_anchor = destination.compute_plate_location( + resource.rotated(z=resource_rotation_wrt_destination_wrt_local) + ).rotated(destination.get_absolute_rotation()) + return destination.get_location_wrt(self._reference_resource) + adjusted_plate_anchor + elif isinstance(destination, Plate) and isinstance(resource, Lid): + plate_location = destination.get_location_wrt(self._reference_resource) + child_wrt_parent = destination.get_lid_location( + resource.rotated(z=resource_rotation_wrt_destination_wrt_local) + ).rotated(destination.get_absolute_rotation()) + return plate_location + child_wrt_parent + else: + return destination.get_location_wrt(self._reference_resource) + + def _compute_end_effector_location( + self, + resource: Resource, + to_location: Coordinate, + offset: Coordinate, + pickup_distance_from_top: float, + rotation_applied_by_move: float, + ) -> Coordinate: + center = resource.center().rotated( + Rotation(z=resource.get_absolute_rotation().z + rotation_applied_by_move) + ) + loc = to_location + center + offset + return Coordinate( + loc.x, + loc.y, + loc.z + resource.get_absolute_size_z() - pickup_distance_from_top, + ) + + def _move_location( + self, + resource: Resource, + to: Coordinate, + offset: Coordinate, + pickup_distance_from_top: float, + ) -> Coordinate: + return to + resource.get_anchor("c", "c", "t") - Coordinate(z=pickup_distance_from_top) + offset + + def _resolve_pickup_distance( + self, resource: Resource, pickup_distance_from_top: Optional[float] + ) -> float: + if pickup_distance_from_top is not None: + return pickup_distance_from_top + if resource.preferred_pickup_location is not None: + logger.debug( + "Using preferred pickup location for resource %s as pickup_distance_from_top was " + "not specified.", + resource.name, + ) + return resource.get_size_z() - resource.preferred_pickup_location.z + logger.debug( + "No preferred pickup location for resource %s. Using default pickup distance of 5mm.", + resource.name, + ) + return 5.0 + + def _assign_after_drop( + self, + resource: Resource, + destination: Union[ResourceStack, ResourceHolder, Resource, Coordinate], + ) -> None: + assert self._reference_resource is not None + resource.unassign() + if isinstance(destination, Coordinate): + destination -= self._reference_resource.location + self._reference_resource.assign_child_resource(resource, location=destination) + elif isinstance(destination, ResourceHolder): + destination.assign_child_resource(resource) + elif isinstance(destination, ResourceStack): + if destination.direction != "z": + raise ValueError("Only ResourceStacks with direction 'z' are currently supported") + destination.assign_child_resource(resource) + elif isinstance(destination, Tilter): + destination.assign_child_resource(resource, location=destination.child_location) + elif isinstance(destination, PlateAdapter): + if not isinstance(resource, Plate): + raise ValueError("Only plates can be moved to a PlateAdapter") + destination.assign_child_resource( + resource, location=destination.compute_plate_location(resource) + ) + elif isinstance(destination, Plate) and isinstance(resource, Lid): + destination.assign_child_resource(resource) + elif isinstance(destination, Trash): + pass + else: + destination.assign_child_resource( + resource, location=destination.get_location_wrt(self._reference_resource) + ) + + def _compute_drop( + self, + resource: Resource, + destination: Union[ResourceStack, ResourceHolder, Resource, Coordinate], + offset: Coordinate, + pickup_distance_from_top: float, + rotation_applied_by_move: float = 0, + ) -> Tuple[Coordinate, float]: + resource_absolute_rotation_after_move = ( + resource.get_absolute_rotation().z + rotation_applied_by_move + ) + dest_rotation = ( + destination.get_absolute_rotation().z if not isinstance(destination, Coordinate) else 0 + ) + resource_rotation_wrt_destination = resource_absolute_rotation_after_move - dest_rotation + resource_rotation_wrt_destination_wrt_local = ( + resource_rotation_wrt_destination - resource.rotation.z + ) + + if isinstance(destination, ResourceStack): + if resource_rotation_wrt_destination % 180 != 0: + raise ValueError( + "Resource rotation wrt ResourceStack must be a multiple of 180 degrees, " + f"got {resource_rotation_wrt_destination} degrees" + ) + + to_location = self._destination_location( + resource, destination, resource_rotation_wrt_destination_wrt_local + ) + location = self._compute_end_effector_location( + resource, to_location, offset, pickup_distance_from_top, rotation_applied_by_move + ) + return location, resource_rotation_wrt_destination + + def _prepare_pickup( + self, + resource: Resource, + offset: Coordinate, + pickup_distance_from_top: Optional[float], + ) -> Tuple[Coordinate, float]: + pickup_distance_from_top = self._resolve_pickup_distance(resource, pickup_distance_from_top) + assert resource.get_absolute_rotation().x == 0 and resource.get_absolute_rotation().y == 0 + assert resource.get_absolute_rotation().z % 90 == 0 + location = self._pickup_location(resource, offset, pickup_distance_from_top) + return location, pickup_distance_from_top + + def _prepare_drop( + self, + destination: Union[ResourceStack, ResourceHolder, Resource, Coordinate], + ) -> Resource: + if self._picked_up is None: + raise RuntimeError("No resource picked up") + if isinstance(destination, Resource): + destination.check_can_drop_resource_here(self._picked_up.resource) + return self._picked_up.resource + + def _finalize_drop( + self, + resource: Resource, + destination: Union[ResourceStack, ResourceHolder, Resource, Coordinate], + resource_rotation_wrt_destination: float, + ) -> None: + self._end_holding() + self._state_updated() + resource.rotate(z=resource_rotation_wrt_destination - resource.rotation.z) + self._assign_after_drop(resource, destination) + + +class Arm(_BaseArm): + """A simple robotic arm without rotation capability. E.g. Hamilton core grippers.""" + + def __init__( + self, + backend: GripperArmBackend, + reference_resource: Resource, + grip_axis: Literal["x", "y"] = "x", + ): + super().__init__(backend=backend, reference_resource=reference_resource) + self.backend: GripperArmBackend = backend + self._grip_axis = grip_axis + + def _resource_width(self, resource: Resource) -> float: + if self._grip_axis == "y": + return resource.get_absolute_size_y() + return resource.get_absolute_size_x() + + async def pick_up_at_location( + self, + location: Coordinate, + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ): + self._begin_holding(resource_width) + await self.backend.pick_up_at_location( + location=location, resource_width=resource_width, backend_params=backend_params + ) + + async def pick_up_resource( + self, + resource: Resource, + offset: Coordinate = Coordinate.zero(), + pickup_distance_from_top: Optional[float] = None, + backend_params: Optional[SerializableMixin] = None, + ): + location, pickup_distance_from_top = self._prepare_pickup( + resource, offset, pickup_distance_from_top + ) + resource_width = self._resource_width(resource) + await self.pick_up_at_location(location, resource_width, backend_params) + self._picked_up = _PickedUpState( + resource=resource, + offset=offset, + pickup_distance_from_top=pickup_distance_from_top, + resource_width=resource_width, + ) + self._state_updated() + + async def drop_at_location( + self, + location: Coordinate, + backend_params: Optional[SerializableMixin] = None, + ): + if not self.holding: + raise RuntimeError("Not holding anything") + await self.backend.drop_at_location( + location=location, resource_width=self._holding_resource_width, backend_params=backend_params + ) + self._end_holding() + + async def drop_resource( + self, + destination: Union[ResourceStack, ResourceHolder, Resource, Coordinate], + offset: Coordinate = Coordinate.zero(), + backend_params: Optional[SerializableMixin] = None, + ): + resource = self._prepare_drop(destination) + location, rotation = self._compute_drop( + resource=resource, + destination=destination, + offset=offset, + pickup_distance_from_top=self._picked_up.pickup_distance_from_top, + ) + await self.drop_at_location(location, backend_params) + self._finalize_drop(resource, destination, rotation) + + async def move_to_location( + self, location: Coordinate, backend_params: Optional[SerializableMixin] = None + ): + await self.backend.move_to_location(location=location, backend_params=backend_params) + + async def move_picked_up_resource( + self, + to: Coordinate, + offset: Coordinate = Coordinate.zero(), + backend_params: Optional[SerializableMixin] = None, + ): + if self._picked_up is None: + raise RuntimeError("No resource picked up") + location = self._move_location( + self._picked_up.resource, to, offset, self._picked_up.pickup_distance_from_top + ) + await self.backend.move_to_location(location=location, backend_params=backend_params) + + async def move_resource( + self, + resource: Resource, + to: Union[ResourceStack, ResourceHolder, Resource, Coordinate], + intermediate_locations: Optional[List[Coordinate]] = None, + pickup_offset: Coordinate = Coordinate.zero(), + destination_offset: Coordinate = Coordinate.zero(), + pickup_distance_from_top: float = 0, + pickup_backend_params: Optional[SerializableMixin] = None, + drop_backend_params: Optional[SerializableMixin] = None, + ): + await self.pick_up_resource( + resource=resource, + offset=pickup_offset, + pickup_distance_from_top=pickup_distance_from_top, + backend_params=pickup_backend_params, + ) + for loc in intermediate_locations or []: + await self.move_picked_up_resource(to=loc) + await self.drop_resource( + destination=to, offset=destination_offset, backend_params=drop_backend_params + ) diff --git a/pylabrobot/arms/arm_tests.py b/pylabrobot/arms/arm_tests.py new file mode 100644 index 00000000000..fb2bc4d0cb7 --- /dev/null +++ b/pylabrobot/arms/arm_tests.py @@ -0,0 +1,252 @@ +import unittest +from unittest.mock import AsyncMock, MagicMock + +from pylabrobot.arms.arm import Arm +from pylabrobot.arms.orientable_arm import OrientableArm +from pylabrobot.arms.joint_arm import JointArm +from pylabrobot.arms.backend import ( + GripperArmBackend, + OrientableGripperArmBackend, + JointGripperArmBackend, +) +from pylabrobot.arms.standard import GripDirection +from pylabrobot.resources import Coordinate, Resource, ResourceHolder + + +def _assert_location(test, call, x, y, z, places=1): + """Assert the location kwarg of a mock call matches expected coordinates.""" + loc = call.kwargs["location"] + test.assertAlmostEqual(loc.x, x, places=places) + test.assertAlmostEqual(loc.y, y, places=places) + test.assertAlmostEqual(loc.z, z, places=places) + + +def _make_deck_with_sites(): + """Create a fictional deck with two sites and a plate. + + Deck: 1000x1000x0 at origin. + Site A at (100, 100, 50), site B at (100, 300, 50). + Plate: 120x80x10 assigned to site A. + """ + deck = Resource("deck", size_x=1000, size_y=1000, size_z=0) + + site_a = ResourceHolder("site_a", size_x=130, size_y=90, size_z=0) + deck.assign_child_resource(site_a, location=Coordinate(100, 100, 50)) + + site_b = ResourceHolder("site_b", size_x=130, size_y=90, size_z=0) + deck.assign_child_resource(site_b, location=Coordinate(100, 300, 50)) + + plate = Resource("plate", size_x=120, size_y=80, size_z=10) + site_a.assign_child_resource(plate, location=Coordinate(5, 5, 0)) + + return deck, site_a, site_b, plate + + +class TestArm(unittest.IsolatedAsyncioTestCase): + """Test Arm (ArmBackend, no rotation). E.g. Hamilton core grippers.""" + + async def asyncSetUp(self): + self.mock_backend = MagicMock(spec=GripperArmBackend) + for method_name in [ + "pick_up_at_location", + "drop_at_location", + "move_to_location", + "open_gripper", + "close_gripper", + "is_gripper_closed", + "halt", + "park", + ]: + setattr(self.mock_backend, method_name, AsyncMock()) + + self.deck, self.site_a, self.site_b, self.plate = _make_deck_with_sites() + self.arm = Arm(backend=self.mock_backend, reference_resource=self.deck) + + async def test_pick_up_resource(self): + # plate center: site_a(100,100,50) + child_loc(5,5,0) + center(60,40,10) = (165, 145, 60) + # pickup_distance_from_top=2 → z = 60 - 2 = 58 + await self.arm.pick_up_resource(self.plate, pickup_distance_from_top=2) + call = self.mock_backend.pick_up_at_location.call_args + _assert_location(self, call, 165, 145, 58) + # default grip_axis="x" → resource_width is X size = 120 + self.assertAlmostEqual(call.kwargs["resource_width"], 120) + + async def test_drop_resource(self): + await self.arm.pick_up_resource(self.plate, pickup_distance_from_top=2) + await self.arm.drop_resource(self.site_b) + call = self.mock_backend.drop_at_location.call_args + # site_b(100,300,50) + default_child_loc(0,0,0) + center(60,40,10) = (160, 340, 60) + # pickup_distance_from_top=2 → z = 60 - 2 = 58 + _assert_location(self, call, 160, 340, 58) + self.assertEqual(self.plate.parent.name, "site_b") + + async def test_pick_up_at_location(self): + location = Coordinate(x=100, y=200, z=300) + await self.arm.pick_up_at_location(location, resource_width=80.0) + self.mock_backend.pick_up_at_location.assert_called_once_with( + location=location, resource_width=80.0, backend_params=None + ) + + async def test_drop_at_location(self): + location = Coordinate(x=100, y=200, z=300) + await self.arm.pick_up_at_location(location, resource_width=80.0) + await self.arm.drop_at_location(location) + self.mock_backend.drop_at_location.assert_called_once_with( + location=location, resource_width=80.0, backend_params=None + ) + + async def test_move_to_location(self): + location = Coordinate(x=100, y=200, z=300) + await self.arm.move_to_location(location) + self.mock_backend.move_to_location.assert_called_once_with( + location=location, backend_params=None + ) + + async def test_open_gripper(self): + await self.arm.open_gripper(gripper_width=50.0) + self.mock_backend.open_gripper.assert_called_once_with(gripper_width=50.0, backend_params=None) + + async def test_halt(self): + await self.arm.halt() + self.mock_backend.halt.assert_called_once() + + async def test_park(self): + await self.arm.park() + self.mock_backend.park.assert_called_once() + + async def test_grip_axis_y(self): + """With grip_axis='y', resource_width should be the Y size.""" + arm_y = Arm(backend=self.mock_backend, reference_resource=self.deck, grip_axis="y") + await arm_y.pick_up_resource(self.plate, pickup_distance_from_top=2) + call = self.mock_backend.pick_up_at_location.call_args + # plate size_y=80 + self.assertAlmostEqual(call.kwargs["resource_width"], 80) + + +class TestOrientableArm(unittest.IsolatedAsyncioTestCase): + """Test OrientableArm coordinate computation with fictional resources.""" + + async def asyncSetUp(self): + self.mock_backend = MagicMock(spec=OrientableGripperArmBackend) + for method_name in [ + "pick_up_at_location", + "drop_at_location", + "move_to_location", + ]: + setattr(self.mock_backend, method_name, AsyncMock()) + + self.deck, self.site_a, self.site_b, self.plate = _make_deck_with_sites() + self.arm = OrientableArm(backend=self.mock_backend, reference_resource=self.deck) + + async def test_pick_up_front(self): + await self.arm.pick_up_resource( + self.plate, pickup_distance_from_top=2, direction=GripDirection.FRONT + ) + call = self.mock_backend.pick_up_at_location.call_args + _assert_location(self, call, 165, 145, 58) + self.assertAlmostEqual(call.kwargs["direction"], 0.0) + # FRONT → X width = 120 + self.assertAlmostEqual(call.kwargs["resource_width"], 120) + + async def test_pick_up_right(self): + await self.arm.pick_up_resource( + self.plate, pickup_distance_from_top=2, direction=GripDirection.RIGHT + ) + call = self.mock_backend.pick_up_at_location.call_args + self.assertAlmostEqual(call.kwargs["direction"], 90.0) + # RIGHT → Y width = 80 + self.assertAlmostEqual(call.kwargs["resource_width"], 80) + + async def test_drop_at_location(self): + location = Coordinate(x=100, y=200, z=300) + await self.arm.pick_up_at_location(location, resource_width=80.0, direction=0.0) + await self.arm.drop_at_location(location, direction=180.0) + self.mock_backend.drop_at_location.assert_called_once_with( + location=location, direction=180.0, resource_width=80.0, backend_params=None + ) + + async def test_move_to_location(self): + location = Coordinate(x=100, y=200, z=300) + await self.arm.move_to_location(location, direction=90.0) + self.mock_backend.move_to_location.assert_called_once_with( + location=location, direction=90.0, backend_params=None + ) + + async def test_move_plate(self): + """Pick from site_a, drop at site_b.""" + await self.arm.pick_up_resource( + self.plate, pickup_distance_from_top=2, direction=GripDirection.FRONT + ) + await self.arm.drop_resource(self.site_b, direction=GripDirection.FRONT) + drop_call = self.mock_backend.drop_at_location.call_args + _assert_location(self, drop_call, 160, 340, 58) + self.assertEqual(self.plate.parent.name, "site_b") + + +class TestJointArm(unittest.IsolatedAsyncioTestCase): + async def asyncSetUp(self): + self.mock_backend = MagicMock(spec=JointGripperArmBackend) + for method_name in [ + "get_joint_position", + "get_cartesian_position", + "open_gripper", + "close_gripper", + "is_gripper_closed", + "halt", + "park", + "pick_up_at_location", + "drop_at_location", + "move_to_location", + ]: + setattr(self.mock_backend, method_name, AsyncMock()) + self.reference_resource = Resource("deck", size_x=1000, size_y=1000, size_z=0) + self.arm = JointArm(backend=self.mock_backend, reference_resource=self.reference_resource) + + async def test_get_joint_position(self): + await self.arm.get_joint_position() + self.mock_backend.get_joint_position.assert_called_once() + + async def test_get_cartesian_position(self): + await self.arm.get_cartesian_position() + self.mock_backend.get_cartesian_position.assert_called_once() + + async def test_open_gripper(self): + await self.arm.open_gripper(gripper_width=50.0) + self.mock_backend.open_gripper.assert_called_once_with(gripper_width=50.0, backend_params=None) + + async def test_close_gripper(self): + await self.arm.close_gripper(gripper_width=50.0) + self.mock_backend.close_gripper.assert_called_once_with(gripper_width=50.0, backend_params=None) + + async def test_is_gripper_closed(self): + await self.arm.is_gripper_closed() + self.mock_backend.is_gripper_closed.assert_called_once() + + async def test_halt(self): + await self.arm.halt() + self.mock_backend.halt.assert_called_once() + + async def test_park(self): + await self.arm.park() + self.mock_backend.park.assert_called_once() + + async def test_pick_up_at_location(self): + location = Coordinate(x=100, y=200, z=300) + await self.arm.pick_up_at_location(location, resource_width=10.0, direction=90.0) + self.mock_backend.pick_up_at_location.assert_called_once_with( + location=location, + direction=90.0, + resource_width=10.0, + backend_params=None, + ) + + async def test_drop_at_location(self): + location = Coordinate(x=100, y=200, z=300) + await self.arm.pick_up_at_location(location, resource_width=10.0, direction=0.0) + await self.arm.drop_at_location(location, direction=180.0) + self.mock_backend.drop_at_location.assert_called_once_with( + location=location, + direction=180.0, + resource_width=10.0, + backend_params=None, + ) diff --git a/pylabrobot/arms/backend.py b/pylabrobot/arms/backend.py new file mode 100644 index 00000000000..f74f04f1821 --- /dev/null +++ b/pylabrobot/arms/backend.py @@ -0,0 +1,210 @@ +from abc import ABCMeta, abstractmethod +from typing import Dict, List, Optional + +from pylabrobot.arms.standard import ArmPosition +from pylabrobot.device import DeviceBackend +from pylabrobot.resources import Coordinate +from pylabrobot.resources.rotation import Rotation +from pylabrobot.serializer import SerializableMixin + + +# ArmBackend: +# - pick_up_at_location +# - drop_at_location +# - move_to_location +# - get_cartesian_position +# - is_holding_resource + +# CanGrip +# - open_gripper +# - close_gripper +# - is_gripper_closed + +# CanSuction +# - start_suction +# - stop_suction + +# CanFreedrive +# - start_freedrive_mode +# - stop_freedrive_mode + +# Joints +# - pick_up_at_joint_position +# - drop_at_joint_position +# - get_joint_position + + +class CanFreedrive(metaclass=ABCMeta): + """Mixin for arms that support freedrive (manual guidance) mode.""" + + @abstractmethod + async def start_freedrive_mode( + self, free_axes: List[int], backend_params: Optional[SerializableMixin] = None + ) -> None: + """Enter freedrive mode, allowing manual movement of the specified joints. + + Args: + free_axes: List of joint indices to free. Use [0] for all axes. + """ + + @abstractmethod + async def stop_freedrive_mode(self, backend_params: Optional[SerializableMixin] = None) -> None: + """Exit freedrive mode.""" + + +class _BaseArmBackend(DeviceBackend, metaclass=ABCMeta): + @abstractmethod + async def halt(self, backend_params: Optional[SerializableMixin] = None) -> None: + """Stop any ongoing movement of the arm.""" + + @abstractmethod + async def park(self, backend_params: Optional[SerializableMixin] = None) -> None: + """Park the arm to its default position.""" + + +class GripperArmBackend(_BaseArmBackend, metaclass=ABCMeta): + """Backend for a simple arm (no rotation capability). E.g. Hamilton core grippers.""" + + @abstractmethod + async def open_gripper( + self, gripper_width: float, backend_params: Optional[SerializableMixin] = None + ) -> None: + """Open the arm's gripper.""" + + @abstractmethod + async def close_gripper( + self, gripper_width: float, backend_params: Optional[SerializableMixin] = None + ) -> None: + """Close the arm's gripper.""" + + @abstractmethod + async def is_gripper_closed(self, backend_params: Optional[SerializableMixin] = None) -> bool: + """Check if the gripper is currently closed.""" + + @abstractmethod + async def pick_up_at_location( + self, + location: Coordinate, + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Pick up at the specified location.""" + + @abstractmethod + async def drop_at_location( + self, + location: Coordinate, + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Drop at the specified location.""" + + @abstractmethod + async def move_to_location( + self, location: Coordinate, backend_params: Optional[SerializableMixin] = None + ) -> None: + """Move the held object to the specified location.""" + + +class OrientableGripperArmBackend(_BaseArmBackend, metaclass=ABCMeta): + """Backend for an arm with rotation capability. E.g. Hamilton iSwap.""" + + @abstractmethod + async def pick_up_at_location( + self, + location: Coordinate, + direction: float, + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Pick up at the specified location with rotation.""" + + @abstractmethod + async def drop_at_location( + self, + location: Coordinate, + direction: float, + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Drop at the specified location with rotation.""" + + @abstractmethod + async def move_to_location( + self, + location: Coordinate, + direction: Rotation, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Move the held object to the specified location with rotation.""" + + +class JointGripperArmBackend(OrientableGripperArmBackend, metaclass=ABCMeta): + """Backend for a joint-space arm with rotation capability. E.g. PreciseFlex, KX2.""" + + @abstractmethod + async def pick_up_at_joint_position( + self, + position: Dict[int, float], + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Pick up at the specified joint position.""" + + @abstractmethod + async def drop_at_joint_position( + self, + position: Dict[int, float], + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Drop at the specified joint position.""" + + @abstractmethod + async def move_to_joint_position( + self, position: Dict[int, float], backend_params: Optional[SerializableMixin] = None + ) -> None: + """Move the arm to the specified joint position.""" + + @abstractmethod + async def get_joint_position( + self, backend_params: Optional[SerializableMixin] = None + ) -> Dict[int, float]: + """Get the current position of the arm in joint space.""" + + @abstractmethod + async def get_cartesian_position( + self, backend_params: Optional[SerializableMixin] = None + ) -> ArmPosition: + """Get the current position of the arm in Cartesian space.""" + + +class ArticulatedGripperArmBackend(_BaseArmBackend, metaclass=ABCMeta): + @abstractmethod + async def pick_up_at_location( + self, + location: Coordinate, + rotation: Rotation, + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Pick up at the specified location with rotation.""" + + @abstractmethod + async def drop_at_location( + self, + location: Coordinate, + rotation: Rotation, + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Drop at the specified location with rotation.""" + + @abstractmethod + async def move_to_location( + self, + location: Coordinate, + rotation: Rotation, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Move the held object to the specified location with rotation.""" diff --git a/pylabrobot/arms/joint_arm.py b/pylabrobot/arms/joint_arm.py new file mode 100644 index 00000000000..bd0b4d828cd --- /dev/null +++ b/pylabrobot/arms/joint_arm.py @@ -0,0 +1,50 @@ +from typing import Dict, Optional + +from pylabrobot.arms.backend import JointGripperArmBackend +from pylabrobot.arms.orientable_arm import OrientableArm +from pylabrobot.arms.standard import ArmPosition +from pylabrobot.resources import Resource +from pylabrobot.serializer import SerializableMixin + + +class JointArm(OrientableArm): + """An arm with joint-space control and rotation capability. E.g. PreciseFlex, KX2.""" + + def __init__(self, backend: JointGripperArmBackend, reference_resource: Resource): + super().__init__(backend=backend, reference_resource=reference_resource) + self.backend: JointGripperArmBackend = backend # type: ignore[assignment] + + async def pick_up_at_joint_position( + self, + position: Dict[int, float], + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + await self.backend.pick_up_at_joint_position( + position=position, resource_width=resource_width, backend_params=backend_params + ) + + async def drop_at_joint_position( + self, + position: Dict[int, float], + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + await self.backend.drop_at_joint_position( + position=position, resource_width=resource_width, backend_params=backend_params + ) + + async def move_to_joint_position( + self, position: Dict[int, float], backend_params: Optional[SerializableMixin] = None + ) -> None: + await self.backend.move_to_joint_position(position=position, backend_params=backend_params) + + async def get_joint_position( + self, backend_params: Optional[SerializableMixin] = None + ) -> Dict[int, float]: + return await self.backend.get_joint_position(backend_params=backend_params) + + async def get_cartesian_position( + self, backend_params: Optional[SerializableMixin] = None + ) -> ArmPosition: + return await self.backend.get_cartesian_position(backend_params=backend_params) diff --git a/pylabrobot/arms/orientable_arm.py b/pylabrobot/arms/orientable_arm.py new file mode 100644 index 00000000000..f69267f3843 --- /dev/null +++ b/pylabrobot/arms/orientable_arm.py @@ -0,0 +1,145 @@ +from typing import Optional, Union + +from pylabrobot.arms.arm import _BaseArm, _PickedUpState, GripOrientation +from pylabrobot.arms.backend import OrientableGripperArmBackend +from pylabrobot.arms.standard import GripDirection +from pylabrobot.resources import Coordinate, Resource, ResourceHolder, ResourceStack +from pylabrobot.resources.rotation import Rotation +from pylabrobot.serializer import SerializableMixin + + +_GRIP_DIRECTION_TO_DEGREES = { + GripDirection.FRONT: 0.0, + GripDirection.RIGHT: 90.0, + GripDirection.BACK: 180.0, + GripDirection.LEFT: 270.0, +} + + +def _resolve_direction(direction: GripOrientation) -> float: + if isinstance(direction, GripDirection): + return _GRIP_DIRECTION_TO_DEGREES[direction] + return direction + + +class OrientableArm(_BaseArm): + """An arm with rotation capability. E.g. Hamilton iSWAP.""" + + def __init__(self, backend: OrientableGripperArmBackend, reference_resource: Resource): + super().__init__(backend=backend, reference_resource=reference_resource) + self.backend: OrientableGripperArmBackend = backend # type: ignore # Union, any OrientableArmBackend + + @staticmethod + def _resource_width_for_direction(resource: Resource, direction: float) -> float: + # TODO: resource rotation is not taken into account here. + if direction % 180 == 0: + return resource.get_absolute_size_x() + else: + return resource.get_absolute_size_y() + + async def pick_up_at_location( + self, + location: Coordinate, + resource_width: float, + direction: GripOrientation = 0.0, + backend_params: Optional[SerializableMixin] = None, + ): + dir_degrees = _resolve_direction(direction) + self._begin_holding(resource_width) + await self.backend.pick_up_at_location( + location=location, + direction=dir_degrees, + resource_width=resource_width, + backend_params=backend_params, + ) + + async def pick_up_resource( + self, + resource: Resource, + offset: Coordinate = Coordinate.zero(), + pickup_distance_from_top: Optional[float] = None, + direction: GripOrientation = GripDirection.FRONT, + backend_params: Optional[SerializableMixin] = None, + ): + location, pickup_distance_from_top = self._prepare_pickup( + resource, offset, pickup_distance_from_top + ) + dir_degrees = _resolve_direction(direction) + resource_width = self._resource_width_for_direction(resource, dir_degrees) + # if gripper: + await self.pick_up_at_location(location, resource_width, dir_degrees, backend_params) + # if suction: + # TODO: + self._picked_up = _PickedUpState( + resource=resource, + offset=offset, + pickup_distance_from_top=pickup_distance_from_top, + resource_width=resource_width, + rotation=Rotation(z=dir_degrees), + ) + self._state_updated() + + async def drop_at_location( + self, + location: Coordinate, + direction: GripOrientation, + backend_params: Optional[SerializableMixin] = None, + ): + if not self.holding: + raise RuntimeError("Not holding anything") + await self.backend.drop_at_location( + location=location, + direction=_resolve_direction(direction), + resource_width=self._holding_resource_width, + backend_params=backend_params, + ) + self._end_holding() + + async def drop_resource( + self, + destination: Union[ResourceStack, ResourceHolder, Resource, Coordinate], + offset: Coordinate = Coordinate.zero(), + direction: GripOrientation = GripDirection.FRONT, + backend_params: Optional[SerializableMixin] = None, + ): + resource = self._prepare_drop(destination) + drop_dir = _resolve_direction(direction) + rotation_applied_by_move = (drop_dir - self._picked_up.rotation.z) % 360 + location, rotation = self._compute_drop( + resource=resource, + destination=destination, + offset=offset, + pickup_distance_from_top=self._picked_up.pickup_distance_from_top, + rotation_applied_by_move=rotation_applied_by_move, + ) + await self.drop_at_location(location, drop_dir, backend_params) + self._finalize_drop(resource, destination, rotation) + + async def move_to_location( + self, + location: Coordinate, + direction: GripOrientation = 0.0, + backend_params: Optional[SerializableMixin] = None, + ): + await self.backend.move_to_location( + location=location, + direction=_resolve_direction(direction), + backend_params=backend_params, + ) + + async def move_picked_up_resource( + self, + to: Coordinate, + direction: GripOrientation, + offset: Coordinate = Coordinate.zero(), + backend_params: Optional[SerializableMixin] = None, + ): + if self._picked_up is None: + raise RuntimeError("No resource picked up") + dir_degrees = _resolve_direction(direction) + location = self._move_location( + self._picked_up.resource, to, offset, self._picked_up.pickup_distance_from_top + ) + await self.backend.move_to_location( + location=location, direction=dir_degrees, backend_params=backend_params + ) diff --git a/pylabrobot/arms/standard.py b/pylabrobot/arms/standard.py new file mode 100644 index 00000000000..641db3ce7e2 --- /dev/null +++ b/pylabrobot/arms/standard.py @@ -0,0 +1,41 @@ +from dataclasses import dataclass +import enum + +from pylabrobot.resources import Coordinate, Rotation + + +@dataclass +class ArmPosition: + """Position of the arm in Cartesian space. Subclass for robot-specific fields.""" + + location: Coordinate + rotation: Rotation + + +class GripDirection(enum.Enum): + FRONT = enum.auto() + BACK = enum.auto() + LEFT = enum.auto() + RIGHT = enum.auto() + + +@dataclass(frozen=True) +class ResourcePickup: + location: Coordinate # center of end effector when gripping the resource + rotation: Rotation # rotation of end effector when gripping the resource + resource_width: float + + +@dataclass(frozen=True) +class ResourceMove: + """Moving a resource that was already picked up.""" + + location: Coordinate # center of end effector when moving the resource + rotation: Rotation # rotation of end effector when moving the resource + + +@dataclass(frozen=True) +class ResourceDrop: + location: Coordinate # center of end effector when dropping the resource + rotation: Rotation # rotation of end effector when dropping the resource + resource_width: float diff --git a/pylabrobot/brooks/__init__.py b/pylabrobot/brooks/__init__.py new file mode 100644 index 00000000000..437e839b1e9 --- /dev/null +++ b/pylabrobot/brooks/__init__.py @@ -0,0 +1 @@ +from .precise_flex import PreciseFlexBackend, PreciseFlex400Backend, PreciseFlex3400Backend diff --git a/pylabrobot/brooks/pf400_test.ipynb b/pylabrobot/brooks/pf400_test.ipynb new file mode 100644 index 00000000000..8095c4f27b8 --- /dev/null +++ b/pylabrobot/brooks/pf400_test.ipynb @@ -0,0 +1,383 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# PreciseFlex 400 — Teaching & Freedrive Notebook\n", + "\n", + "Uses the `JointArm` frontend with a `PreciseFlex400Backend`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import json\n", + "from pathlib import Path\n", + "\n", + "from pylabrobot.arms.joint_arm import JointArm\n", + "from pylabrobot.brooks.precise_flex import (\n", + " PreciseFlex400Backend,\n", + " PreciseFlexBackend,\n", + " ElbowOrientation,\n", + " PFAxis,\n", + ")\n", + "from pylabrobot.resources import Coordinate, Resource, Rotation\n", + "\n", + "POSITIONS_FILE = Path(\"positions.json\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "def save_position(name: str, pos):\n", + " \"\"\"Save a named position to disk.\"\"\"\n", + " positions = json.loads(POSITIONS_FILE.read_text()) if POSITIONS_FILE.exists() else {}\n", + " positions[name] = {\n", + " \"x\": pos.location.x, \"y\": pos.location.y, \"z\": pos.location.z,\n", + " \"roll\": pos.rotation.x, \"pitch\": pos.rotation.y, \"yaw\": pos.rotation.z,\n", + " \"orientation\": pos.orientation.value if pos.orientation else None,\n", + " }\n", + " POSITIONS_FILE.write_text(json.dumps(positions, indent=2))\n", + " print(f\"Saved '{name}'\")\n", + "\n", + "def load_position(name: str):\n", + " \"\"\"Load a named position from disk.\"\"\"\n", + " positions = json.loads(POSITIONS_FILE.read_text())\n", + " p = positions[name]\n", + " return (\n", + " Coordinate(p[\"x\"], p[\"y\"], p[\"z\"]),\n", + " Rotation(p[\"roll\"], p[\"pitch\"], p[\"yaw\"]),\n", + " ElbowOrientation(p[\"orientation\"]) if p[\"orientation\"] else None,\n", + " )\n", + "\n", + "def list_positions():\n", + " \"\"\"List all saved positions.\"\"\"\n", + " if not POSITIONS_FILE.exists():\n", + " print(\"No saved positions.\")\n", + " return\n", + " positions = json.loads(POSITIONS_FILE.read_text())\n", + " for name, p in positions.items():\n", + " print(f\" {name}: x={p['x']:.1f} y={p['y']:.1f} z={p['z']:.1f} \"\n", + " f\"yaw={p['yaw']:.1f} {p['orientation'] or ''}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Connect" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "backend = PreciseFlex400Backend(host=\"192.168.0.1\")\n", + "reference = Resource(\"workcell\", size_x=2000, size_y=2000, size_z=0)\n", + "arm = JointArm(backend=backend, reference_resource=reference)\n", + "\n", + "await arm.setup()\n", + "print(f\"Version: {await backend.get_version()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Read current position" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "cart = await arm.get_cartesian_position()\n", + "print(f\"Cartesian: x={cart.location.x:.1f}, y={cart.location.y:.1f}, z={cart.location.z:.1f}\")\n", + "print(f\"Rotation: yaw={cart.rotation.z:.1f}, pitch={cart.rotation.y:.1f}, roll={cart.rotation.x:.1f}\")\n", + "print(f\"Elbow: {cart.orientation}\")\n", + "print()\n", + "joints = await arm.get_joint_position()\n", + "print(f\"Joints: {joints}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Freedrive mode\n", + "\n", + "Enter freedrive to manually position the arm. Use `[0]` to free all axes." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "await backend.start_freedrive_mode([0])\n", + "print(\"Freedrive ON -- move the arm manually\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Read position while in freedrive\n", + "cart = await arm.get_cartesian_position()\n", + "print(f\"x={cart.location.x:.1f}, y={cart.location.y:.1f}, z={cart.location.z:.1f}, \"\n", + " f\"yaw={cart.rotation.z:.1f}, orientation={cart.orientation}\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "await backend.stop_freedrive_mode()\n", + "print(\"Freedrive OFF\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Teach & save positions\n", + "\n", + "Use freedrive to move the arm, then save the position to disk." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Save current position with a name\n", + "pos = await arm.get_cartesian_position()\n", + "save_position(\"home\", pos)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "# Save another position\n", + "pos = await arm.get_cartesian_position()\n", + "save_position(\"plate_pickup\", pos)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "list_positions()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Replay saved positions" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "loc, rot, orientation = load_position(\"home\")\n", + "await arm.move_to_location(\n", + " location=loc,\n", + " direction=rot,\n", + " backend_params=PreciseFlexBackend.MoveToLocationParams(orientation=orientation, speed=30),\n", + ")\n", + "print(\"Moved to 'home'\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "loc, rot, orientation = load_position(\"plate_pickup\")\n", + "await arm.move_to_location(\n", + " location=loc,\n", + " direction=rot,\n", + " backend_params=PreciseFlexBackend.MoveToLocationParams(orientation=orientation, speed=30),\n", + ")\n", + "print(\"Moved to 'plate_pickup'\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Move with speed control" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "await arm.move_to_location(\n", + " location=Coordinate(300, 0, 200),\n", + " direction=Rotation(0, 0, 0),\n", + " backend_params=PreciseFlexBackend.MoveToLocationParams(speed=20),\n", + ")\n", + "print(await arm.get_cartesian_position())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Lefty / Righty\n", + "\n", + "Pass `ElbowOrientation` through `backend_params`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "await arm.move_to_location(\n", + " location=Coordinate(300, 0, 200),\n", + " direction=Rotation(0, 0, 0),\n", + " backend_params=PreciseFlexBackend.MoveToLocationParams(\n", + " orientation=ElbowOrientation.RIGHT, speed=30,\n", + " ),\n", + ")\n", + "print(\"Righty:\", await arm.get_cartesian_position())" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "await arm.move_to_location(\n", + " location=Coordinate(300, 0, 200),\n", + " direction=Rotation(0, 0, 0),\n", + " backend_params=PreciseFlexBackend.MoveToLocationParams(\n", + " orientation=ElbowOrientation.LEFT, speed=30,\n", + " ),\n", + ")\n", + "print(\"Lefty:\", await arm.get_cartesian_position())" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "await backend.change_config()\n", + "print(\"Flipped:\", await arm.get_cartesian_position())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Move joints" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "await arm.move_to_joint_position(\n", + " {PFAxis.BASE: 45.0},\n", + " backend_params=PreciseFlexBackend.MoveToJointPositionParams(speed=30),\n", + ")\n", + "print(await arm.get_joint_position())" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Gripper" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "await arm.open_gripper(gripper_width=80.0)\n", + "print(f\"Gripper closed: {await arm.is_gripper_closed()}\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "await arm.close_gripper(gripper_width=10.0)\n", + "print(f\"Gripper closed: {await arm.is_gripper_closed()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Disconnect" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "await backend.move_to_safe()\n", + "await arm.stop()\n", + "print(\"Done\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "name": "python", + "version": "3.9.0" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/pylabrobot/brooks/precise_flex.py b/pylabrobot/brooks/precise_flex.py new file mode 100644 index 00000000000..3fab3eac824 --- /dev/null +++ b/pylabrobot/brooks/precise_flex.py @@ -0,0 +1,1746 @@ +import asyncio +import warnings +from abc import ABC +from dataclasses import dataclass +from enum import Enum, IntEnum +from typing import Dict, List, Literal, Optional, Union + +from pylabrobot.arms.backend import CanFreedrive, JointGripperArmBackend +from pylabrobot.io.socket import Socket +from pylabrobot.resources import Coordinate, Rotation +from pylabrobot.serializer import SerializableMixin + + +# --------------------------------------------------------------------------- +# Supporting types +# --------------------------------------------------------------------------- + + +class ElbowOrientation(Enum): + RIGHT = "right" + LEFT = "left" + + +class PFAxis(IntEnum): + BASE = 1 + SHOULDER = 2 + ELBOW = 3 + WRIST = 4 + GRIPPER = 5 + RAIL = 6 + + +@dataclass +class CartesianCoords: + location: Coordinate + rotation: Rotation + + +@dataclass +class PreciseFlexCartesianCoords(CartesianCoords): + orientation: Optional[ElbowOrientation] = None + + +@dataclass +class VerticalAccess: + """Access location from above (most common pattern for stacks and tube racks). + + Args: + approach_height_mm: Height above the target position to move to before descending to grip + clearance_mm: Vertical distance to retract after gripping before lateral movement + gripper_offset_mm: Additional vertical offset added when holding a plate + """ + + approach_height_mm: float = 100 + clearance_mm: float = 100 + gripper_offset_mm: float = 10 + + +@dataclass +class HorizontalAccess: + """Access location from the side (for hotel-style plate carriers). + + Args: + approach_distance_mm: Horizontal distance in front of the target to stop before moving in + clearance_mm: Horizontal distance to retract after gripping before lifting + lift_height_mm: Vertical distance to lift the plate after horizontal retract + gripper_offset_mm: Additional vertical offset added when holding a plate + """ + + approach_distance_mm: float = 50 + clearance_mm: float = 50 + lift_height_mm: float = 100 + gripper_offset_mm: float = 10 + + +AccessPattern = Union[VerticalAccess, HorizontalAccess] + + +ERROR_CODES: Dict[int, Dict[str, str]] = { + 0: {"text": "Success", "description": "Command completed successfully."}, + 1: { + "text": "Unrecognized command", + "description": "An unrecognized command was entered on the command line.", + }, + 2: {"text": "Missing or extra parameter(s)", "description": "Wrong number of arguments."}, + 3: { + "text": "Invalid parameter value", + "description": "A value is out of range or its type is invalid.", + }, + 5: {"text": "Need to home", "description": "Robot requires homing before this operation."}, + 6: {"text": "Aborted by user", "description": "An operation was stopped by a user command."}, + 7: { + "text": "Robot not attached", + "description": "Must attach robot before performing this command.", + }, + 8: { + "text": "High power not enabled", + "description": "Motor power must be enabled before this command.", + }, + 9: { + "text": "Robot in motion", + "description": "Cannot perform this command while robot is moving.", + }, + 10: {"text": "Location not set", "description": "The specified location has not been set."}, + 11: { + "text": "Following error", + "description": "Robot motion caused a following error (motor stall or collision).", + }, + 12: {"text": "Timeout waiting for EOM", "description": "Timed out waiting for end of motion."}, + 13: { + "text": "Robot already attached", + "description": "Robot is already attached by another thread.", + }, + 14: { + "text": "Out of range", + "description": "Target position is outside the robot's reachable workspace.", + }, + 15: {"text": "Profile error", "description": "Invalid profile index or profile parameter."}, + 16: {"text": "Station not found", "description": "The specified station does not exist."}, + 17: {"text": "Pallet error", "description": "Invalid pallet configuration or index."}, + 18: { + "text": "Singularity", + "description": "Requested position would cause a kinematic singularity.", + }, + 19: {"text": "Limit exceeded", "description": "A joint limit would be exceeded for this move."}, + 20: {"text": "Communication error", "description": "Internal communication failure."}, +} + + +# --------------------------------------------------------------------------- +# Exceptions +# --------------------------------------------------------------------------- + + +class PreciseFlexError(Exception): + def __init__(self, replycode: int, message: str): + self.replycode = replycode + self.message = message + if replycode in ERROR_CODES: + text = ERROR_CODES[replycode]["text"] + description = ERROR_CODES[replycode]["description"] + super().__init__(f"PreciseFlexError {replycode}: {text}. {description} - {message}") + else: + super().__init__(f"PreciseFlexError {replycode}: {message}") + + +# --------------------------------------------------------------------------- +# Backend +# --------------------------------------------------------------------------- + + +class PreciseFlexBackend(JointGripperArmBackend, CanFreedrive, ABC): + """Backend for the PreciseFlex robotic arm. + + Default to using Cartesian coordinates; some methods in Brook's TCS + don't work with Joint coordinates. + + Documentation and error codes available at + https://www2.brooksautomation.com/#Root/Welcome.htm + """ + + def __init__( + self, + host: str, + port: int = 10100, + is_dual_gripper: bool = False, + has_rail: bool = False, + timeout: int = 20, + ) -> None: + super().__init__() + self.io = Socket(human_readable_device_name="Precise Flex Arm", host=host, port=port) + self.profile_index: int = 1 + self.location_index: int = 1 + self.horizontal_compliance: bool = False + self.horizontal_compliance_torque: int = 0 + self.timeout = timeout + self._has_rail = has_rail + self._is_dual_gripper = is_dual_gripper + if is_dual_gripper: + warnings.warn( + "Dual gripper support is experimental and may not work as expected.", UserWarning + ) + + # -- coordinate conversion helpers ----------------------------------------- + + def _convert_to_cartesian_space( + self, position: tuple[float, float, float, float, float, float, Optional[ElbowOrientation]] + ) -> PreciseFlexCartesianCoords: + """Convert a tuple of cartesian coordinates to a CartesianCoords object.""" + if len(position) != 7: + raise ValueError( + "Position must be a tuple of 7 values (x, y, z, yaw, pitch, roll, orientation)." + ) + orientation = ElbowOrientation(position[6]) + return PreciseFlexCartesianCoords( + location=Coordinate(position[0], position[1], position[2]), + rotation=Rotation(position[5], position[4], position[3]), + orientation=orientation, + ) + + def _convert_to_cartesian_array( + self, position: PreciseFlexCartesianCoords + ) -> tuple[float, float, float, float, float, float, int]: + """Convert a CartesianCoords object to a list of cartesian coordinates.""" + orientation_int = self._convert_orientation_enum_to_int(position.orientation) + return ( + position.location.x, + position.location.y, + position.location.z, + position.rotation.yaw, + position.rotation.pitch, + position.rotation.roll, + orientation_int, + ) + + def _convert_orientation_int_to_enum(self, orientation_int: int) -> Optional[ElbowOrientation]: + if orientation_int == 1: + return ElbowOrientation.RIGHT + if orientation_int == 2: + return ElbowOrientation.LEFT + return None + + def _convert_orientation_enum_to_int(self, orientation: Optional[ElbowOrientation]) -> int: + if orientation == ElbowOrientation.LEFT: + return 2 + if orientation == ElbowOrientation.RIGHT: + return 1 + return 0 + + # -- lifecycle ------------------------------------------------------------- + + async def setup(self, skip_home: bool = False): + """Initialize the PreciseFlex backend.""" + await self.io.setup() + await self.set_response_mode("pc") + await self.power_on_robot() + await self.attach(1) + if not skip_home: + await self.home() + + async def stop(self): + """Stop the PreciseFlex backend.""" + await self.detach() + await self.power_off_robot() + await self.exit() + await self.io.stop() + + # -- high-level motion API ------------------------------------------------- + + async def _set_speed(self, speed_percent: float): + """Set the speed percentage of the arm's movement (0-100).""" + await self.set_profile_speed(self.profile_index, speed_percent) + + async def _get_speed(self) -> float: + """Get the current speed percentage of the arm's movement.""" + return await self.get_profile_speed(self.profile_index) + + async def open_gripper( + self, gripper_width: float, backend_params: Optional[SerializableMixin] = None + ): + """Open the gripper to the specified width.""" + await self._set_grip_open_pos(gripper_width) + await self.send_command("gripper 1") + + async def close_gripper( + self, gripper_width: float, backend_params: Optional[SerializableMixin] = None + ): + """Close the gripper to the specified width.""" + await self._set_grip_close_pos(gripper_width) + await self.send_command("gripper 2") + + async def halt(self, backend_params: Optional[SerializableMixin] = None): + """Stops the current robot immediately but leaves power on.""" + await self.send_command("halt") + + async def home(self) -> None: + """Home the robot associated with this thread. + + Note: + Requires power to be enabled. + Requires robot to be attached. + Waits until the homing is complete. + """ + await self.send_command("home") + + async def move_to_safe(self) -> None: + """Moves the robot to Safe Position. + + Does not include checks for collision with 3rd party obstacles inside the work volume of the robot. + """ + await self.send_command("movetosafe") + + async def home_all(self) -> None: + """Home all robots. + + Note: + Requires power to be enabled. + Requires that robots not be attached. + """ + await self.send_command("homeAll") + + async def attach(self, attach_state: Optional[int] = None) -> int: + """Attach or release the robot, or get attachment state. + + Args: + attach_state: If omitted, returns the attachment state. 0 = release the robot; 1 = attach the robot. + + Returns: + If attach_state is omitted, returns 0 if robot is not attached, -1 if attached. Otherwise returns 0 on success. + + Note: + The robot must be attached to allow motion commands. + """ + if attach_state is None: + response = await self.send_command("attach") + return int(response) + await self.send_command(f"attach {attach_state}") + return 0 + + async def detach(self): + """Detach the robot.""" + await self.attach(0) + + async def power_on_robot(self): + """Power on the robot.""" + await self.set_power(True, self.timeout) + + async def power_off_robot(self): + """Power off the robot.""" + await self.set_power(False) + + async def approach( + self, + position: Union[PreciseFlexCartesianCoords, Dict[int, float]], + access: Optional[AccessPattern] = None, + ): + """Move the arm to an approach position (offset from target). + + Args: + position: Target position (CartesianCoords or Dict[int, float]) + access: Access pattern defining how to approach the target. Defaults to VerticalAccess() if not specified. + + Example: + # Simple vertical approach (default) + await backend.approach(position) + + # Horizontal hotel-style approach + await backend.approach( + position, + HorizontalAccess( + approach_distance_mm=50, + clearance_mm=50, + lift_height_mm=100 + ) + ) + """ + if access is None: + access = VerticalAccess() + if isinstance(position, dict): + await self._approach_j(position, access) + elif isinstance(position, PreciseFlexCartesianCoords): + await self._approach_c(position, access) + else: + raise TypeError("Position must be of type Dict[int, float] or CartesianCoords.") + + async def park(self, backend_params: Optional[SerializableMixin] = None) -> None: + """Park the arm to its default safe position.""" + await self.move_to_safe() + + # -- JointArmBackend interface (joint-space) -------------------------------- + + @dataclass + class PickUpParams(SerializableMixin): + access: Optional[AccessPattern] = None + finger_speed_percent: float = 50.0 + grasp_force: float = 10.0 + orientation: Optional[ElbowOrientation] = None + + async def pick_up_at_joint_position( + self, + position: Dict[int, float], + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Pick up at the specified joint position.""" + if not isinstance(backend_params, self.PickUpParams): + backend_params = PreciseFlexBackend.PickUpParams() + access = backend_params.access or VerticalAccess() + await self._set_grasp_data( + plate_width=resource_width, + finger_speed_percent=backend_params.finger_speed_percent, + grasp_force=backend_params.grasp_force, + ) + await self._pick_plate_j(position, access) + + @dataclass + class DropParams(SerializableMixin): + access: Optional[AccessPattern] = None + orientation: Optional[ElbowOrientation] = None + + async def drop_at_joint_position( + self, + position: Dict[int, float], + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Drop at the specified joint position.""" + if not isinstance(backend_params, self.DropParams): + backend_params = PreciseFlexBackend.DropParams() + access = backend_params.access or VerticalAccess() + await self._place_plate_j(position, access) + + @dataclass + class MoveToJointPositionParams(SerializableMixin): + speed: Optional[float] = None + + async def move_to_joint_position( + self, + position: Dict[int, float], + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Move the arm to the specified joint position.""" + if not isinstance(backend_params, self.MoveToJointPositionParams): + backend_params = PreciseFlexBackend.MoveToJointPositionParams() + if backend_params.speed is not None: + await self._set_speed(backend_params.speed) + current = await self.get_joint_position() + joint_coords = {**current, **position} + await self._move_j(profile_index=self.profile_index, joint_coords=joint_coords) + + async def get_joint_position( + self, backend_params: Optional[SerializableMixin] = None + ) -> Dict[int, float]: + """Get the current joint position of the arm.""" + await self._wait_for_eom() + num_tries = 2 + for _ in range(num_tries): + data = await self.send_command("wherej") + parts = data.split() + if len(parts) > 0: + break + else: + raise PreciseFlexError(-1, "Unexpected response format from wherej command.") + return self._parse_angles_response(parts) + + async def get_cartesian_position( + self, backend_params: Optional[SerializableMixin] = None + ) -> PreciseFlexCartesianCoords: + """Get the current position of the arm in Cartesian space.""" + await self._wait_for_eom() + num_tries = 2 + for _ in range(num_tries): + data = await self.send_command("wherec") + parts = data.split() + if len(parts) == 7: + break + else: + raise PreciseFlexError(-1, "Unexpected response format from wherec command.") + x, y, z, yaw, pitch, roll = self._parse_xyz_response(parts[0:6]) + config = int(parts[6]) + enum_thing = self._convert_orientation_int_to_enum(config) + return self._convert_to_cartesian_space(position=(x, y, z, yaw, pitch, roll, enum_thing)) + + # -- OrientableArmBackend interface (Cartesian) ----------------------------- + + async def pick_up_at_location( + self, + location: Coordinate, + direction: float, + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Pick up at the specified Cartesian location.""" + if not isinstance(backend_params, self.PickUpParams): + backend_params = PreciseFlexBackend.PickUpParams() + access = backend_params.access or VerticalAccess() + coords = PreciseFlexCartesianCoords( + location=location, rotation=Rotation(z=direction), orientation=backend_params.orientation + ) + await self._set_grasp_data( + plate_width=resource_width, + finger_speed_percent=backend_params.finger_speed_percent, + grasp_force=backend_params.grasp_force, + ) + await self._pick_plate_c(cartesian_position=coords, access=access) + + async def drop_at_location( + self, + location: Coordinate, + direction: float, + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Drop at the specified Cartesian location.""" + if not isinstance(backend_params, self.DropParams): + backend_params = PreciseFlexBackend.DropParams() + access = backend_params.access or VerticalAccess() + coords = PreciseFlexCartesianCoords( + location=location, rotation=Rotation(z=direction), orientation=backend_params.orientation + ) + await self._place_plate_c(cartesian_position=coords, access=access) + + @dataclass + class MoveToLocationParams(SerializableMixin): + speed: Optional[float] = None + orientation: Optional[ElbowOrientation] = None + + async def move_to_location( + self, + location: Coordinate, + direction: Rotation, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Move the arm to the specified Cartesian location.""" + if not isinstance(backend_params, self.MoveToLocationParams): + backend_params = PreciseFlexBackend.MoveToLocationParams() + if backend_params.speed is not None: + await self._set_speed(backend_params.speed) + coords = PreciseFlexCartesianCoords( + location=location, rotation=direction, orientation=backend_params.orientation + ) + await self._move_c(profile_index=self.profile_index, cartesian_coords=coords) + + async def is_gripper_closed(self, backend_params: Optional[SerializableMixin] = None) -> bool: + """(Single Gripper Only) Tests if the gripper is fully closed by checking the end-of-travel sensor. + + Returns: + For standard gripper: True if the gripper is within 2mm of fully closed, otherwise False. + """ + if self._is_dual_gripper: + raise ValueError("IsGripperClosed command is only valid for single gripper robots.") + response = await self.send_command("IsFullyClosed") + return int(response) == -1 + + async def are_grippers_closed(self) -> tuple[bool, bool]: + """(Dual Gripper Only) Tests if each gripper is fully closed by checking the end-of-travel sensors.""" + if not self._is_dual_gripper: + raise ValueError("AreGrippersClosed command is only valid for dual gripper robots.") + response = await self.send_command("IsFullyClosed") + ret_int = int(response) + gripper_1_closed = (ret_int & 1) != 0 + gripper_2_closed = (ret_int & 2) != 0 + return (gripper_1_closed, gripper_2_closed) + + async def start_freedrive_mode(self, free_axes: List[int], backend_params=None) -> None: + """Enter freedrive mode, allowing manual movement of the specified joints. + + The robot must be attached to enter free mode. + + Args: + free_axes: List of joint indices to free. Use [0] for all axes. + """ + for axis in free_axes: + await self.send_command(f"freemode {axis}") + + async def stop_freedrive_mode(self, backend_params=None) -> None: + """Exit freedrive mode for all axes.""" + await self.send_command("freemode -1") + + # -- communication --------------------------------------------------------- + + async def send_command(self, command: str) -> str: + await self.io.write(command.encode("utf-8") + b"\n") + reply = await self.io.readline() + return self._parse_reply_ensure_successful(reply) + + def _parse_reply_ensure_successful(self, reply: bytes) -> str: + """Parse reply from Precise Flex. + + Expected format: b'replycode data message\r\n' + - replycode is an integer at the beginning + - data is rest of the line (excluding CRLF) + """ + text = reply.decode().strip() + if not text: + raise PreciseFlexError(-1, "Empty reply from device.") + parts = text.split(" ", 1) + if len(parts) == 1: + replycode = int(parts[0]) + data = "" + else: + replycode, data = int(parts[0]), parts[1] + if replycode != 0: + raise PreciseFlexError(replycode, data) + return data + + # -- internal pick/place helpers ------------------------------------------- + + async def _approach_j(self, joint_position: Dict[int, float], access: AccessPattern): + """Move the arm to a position above the specified coordinates. + + The approach behavior depends on the access pattern: + - VerticalAccess: Approaches from above using approach_height_mm + - HorizontalAccess: Approaches from the side using approach_distance_mm + """ + await self._set_joint_angles(self.location_index, joint_position) + await self._set_grip_detail(access) + await self._move_to_stored_location_appro(self.location_index, self.profile_index) + + async def _pick_plate_j(self, joint_position: Dict[int, float], access: AccessPattern): + """Pick a plate from the specified position using joint coordinates.""" + await self._set_joint_angles(self.location_index, joint_position) + await self._set_grip_detail(access) + horizontal_compliance_int = 1 if self.horizontal_compliance else 0 + ret_code = await self.send_command( + f"pickplate {self.location_index} {horizontal_compliance_int} {self.horizontal_compliance_torque}" + ) + if ret_code == "0": + raise PreciseFlexError(-1, "the force-controlled gripper detected no plate present.") + + async def _place_plate_j(self, joint_position: Dict[int, float], access: AccessPattern): + """Place a plate at the specified position using joint coordinates.""" + await self._set_joint_angles(self.location_index, joint_position) + await self._set_grip_detail(access) + horizontal_compliance_int = 1 if self.horizontal_compliance else 0 + await self.send_command( + f"placeplate {self.location_index} {horizontal_compliance_int} {self.horizontal_compliance_torque}" + ) + + async def _approach_c( + self, + cartesian_position: PreciseFlexCartesianCoords, + access: AccessPattern, + ): + """Move the arm to a position above the specified coordinates. + + The approach behavior depends on the access pattern: + - VerticalAccess: Approaches from above using approach_height_mm + - HorizontalAccess: Approaches from the side using approach_distance_mm + """ + await self._set_location_xyz(self.location_index, cartesian_position) + await self._set_grip_detail(access) + orientation_int = self._convert_orientation_enum_to_int(cartesian_position.orientation) + await self._set_location_config(self.location_index, orientation_int) + await self._move_to_stored_location_appro(self.location_index, self.profile_index) + + async def _pick_plate_c( + self, + cartesian_position: PreciseFlexCartesianCoords, + access: AccessPattern, + ): + """Pick a plate from the specified position using Cartesian coordinates.""" + await self._set_location_xyz(self.location_index, cartesian_position) + await self._set_grip_detail(access) + orientation_int = self._convert_orientation_enum_to_int(cartesian_position.orientation) + orientation_int |= 0x1000 # GPL_Single: restrict wrist to ±180° + await self._set_location_config(self.location_index, orientation_int) + horizontal_compliance_int = 1 if self.horizontal_compliance else 0 + ret_code = await self.send_command( + f"pickplate {self.location_index} {horizontal_compliance_int} {self.horizontal_compliance_torque}" + ) + if ret_code == "0": + raise PreciseFlexError(-1, "the force-controlled gripper detected no plate present.") + + async def _place_plate_c( + self, + cartesian_position: PreciseFlexCartesianCoords, + access: AccessPattern, + ): + """Place a plate at the specified position using Cartesian coordinates.""" + await self._set_location_xyz(self.location_index, cartesian_position) + await self._set_grip_detail(access) + orientation_int = self._convert_orientation_enum_to_int(cartesian_position.orientation) + orientation_int |= 0x1000 # GPL_Single: restrict wrist to ±180° + await self._set_location_config(self.location_index, orientation_int) + horizontal_compliance_int = 1 if self.horizontal_compliance else 0 + await self.send_command( + f"placeplate {self.location_index} {horizontal_compliance_int} {self.horizontal_compliance_torque}" + ) + + async def _set_grip_detail(self, access: AccessPattern): + """Configure station type for pick/place operations based on access pattern. + + Calls TCS set_station_type command to configure how the robot interprets + clearance values and performs approach/retract motions. + + Args: + access: Access pattern (VerticalAccess or HorizontalAccess) defining how to approach and retract from the location. + """ + if isinstance(access, VerticalAccess): + await self.send_command( + f"StationType {self.location_index} 1 0 {access.clearance_mm} 0 {access.gripper_offset_mm}" + ) + elif isinstance(access, HorizontalAccess): + await self.send_command( + f"StationType {self.location_index} 0 0 {access.clearance_mm} {access.lift_height_mm} {access.gripper_offset_mm}" + ) + else: + raise TypeError("Access pattern must be VerticalAccess or HorizontalAccess.") + + # -- GENERAL COMMANDS ------------------------------------------------------ + + async def get_base(self) -> tuple[float, float, float, float]: + """Get the robot base offset. + + Returns: + A tuple containing (x_offset, y_offset, z_offset, z_rotation) + """ + data = await self.send_command("base") + parts = data.split() + if len(parts) != 4: + raise PreciseFlexError(-1, "Unexpected response format from base command.") + return (float(parts[0]), float(parts[1]), float(parts[2]), float(parts[3])) + + async def set_base( + self, x_offset: float, y_offset: float, z_offset: float, z_rotation: float + ) -> None: + """Set the robot base offset. + + Args: + x_offset: Base X offset + y_offset: Base Y offset + z_offset: Base Z offset + z_rotation: Base Z rotation + + Note: + The robot must be attached to set the base. + Setting the base pauses any robot motion in progress. + """ + await self.send_command(f"base {x_offset} {y_offset} {z_offset} {z_rotation}") + + async def exit(self) -> None: + """Close the communications link immediately. + + Note: + Does not affect any robots that may be active. + """ + await self.io.write(b"exit\n") + + async def get_power_state(self) -> int: + """Get the current robot power state. + + Returns: + Current power state (0 = disabled, 1 = enabled) + """ + response = await self.send_command("hp") + return int(response) + + async def set_power(self, enable: bool, timeout: int = 0) -> None: + """Enable or disable robot high power. + + Args: + enable: True to enable power, False to disable + timeout: Wait timeout for power to come on. + 0 or omitted = do not wait for power to come on + > 0 = wait this many seconds for power to come on + -1 = wait indefinitely for power to come on + + Raises: + PreciseFlexError: If power does not come on within the specified timeout. + """ + power_state = 1 if enable else 0 + if timeout == 0: + await self.send_command(f"hp {power_state}") + else: + await self.send_command(f"hp {power_state} {timeout}") + + ResponseMode = Literal["pc", "verbose"] + + async def get_mode(self) -> ResponseMode: + """Get the current response mode. + + Returns: + Current mode (0 = PC mode, 1 = verbose mode) + """ + response = await self.send_command("mode") + mapping: Dict[int, PreciseFlexBackend.ResponseMode] = {0: "pc", 1: "verbose"} + return mapping[int(response)] + + async def set_response_mode(self, mode: ResponseMode) -> None: + """Set the response mode. + + Args: + mode: Response mode to set. + 0 = Select PC mode + 1 = Select verbose mode + + Note: + When using serial communications, the mode change does not take effect + until one additional command has been processed. + """ + if mode not in ["pc", "verbose"]: + raise ValueError("Mode must be 'pc' or 'verbose'") + mapping = {"pc": 0, "verbose": 1} + await self.send_command(f"mode {mapping[mode]}") + + async def get_monitor_speed(self) -> int: + """Get the global system (monitor) speed. + + Returns: + Current monitor speed as a percentage (1-100) + """ + response = await self.send_command("mspeed") + return int(response) + + async def set_monitor_speed(self, speed_percent: int) -> None: + """Set the global system (monitor) speed. + + Args: + speed_percent: Speed percentage between 1 and 100, where 100 means full speed. + + Raises: + ValueError: If speed_percent is not between 1 and 100. + """ + if not (1 <= speed_percent <= 100): + raise ValueError("Speed percent must be between 1 and 100") + await self.send_command(f"mspeed {speed_percent}") + + async def nop(self) -> None: + """No operation command. + + Does nothing except return the standard reply. Can be used to see if the link + is active or to check for exceptions. + """ + await self.send_command("nop") + + async def get_payload(self) -> int: + """Get the payload percent value for the current robot. + + Returns: + Current payload as a percentage of maximum (0-100) + """ + response = await self.send_command("payload") + return int(response) + + async def set_payload(self, payload_percent: int) -> None: + """Set the payload percent of maximum for the currently selected or attached robot. + + Args: + payload_percent: Payload percentage from 0 to 100 indicating the percent of the maximum payload the robot is carrying. + + Raises: + ValueError: If payload_percent is not between 0 and 100. + + Note: + If the robot is moving, waits for the robot to stop before setting a value. + """ + if not (0 <= payload_percent <= 100): + raise ValueError("Payload percent must be between 0 and 100") + await self.send_command(f"payload {payload_percent}") + + async def set_parameter( + self, + data_id: int, + value, + unit_number: Optional[int] = None, + sub_unit: Optional[int] = None, + array_index: Optional[int] = None, + ) -> None: + """Change a value in the controller's parameter database. + + Args: + data_id: DataID of parameter. + value: New parameter value. If string, will be quoted automatically. + unit_number: Unit number, usually the robot number (1 - N_ROB). + sub_unit: Sub-unit, usually 0. + array_index: Array index. + + Note: + Updated values are not saved in flash unless a save-to-flash operation + is performed (see DataID 901). + """ + if unit_number is not None and sub_unit is not None and array_index is not None: + if isinstance(value, str): + await self.send_command(f'pc {data_id} {unit_number} {sub_unit} {array_index} "{value}"') + else: + await self.send_command(f"pc {data_id} {unit_number} {sub_unit} {array_index} {value}") + else: + if isinstance(value, str): + await self.send_command(f'pc {data_id} "{value}"') + else: + await self.send_command(f"pc {data_id} {value}") + + async def get_parameter( + self, + data_id: int, + unit_number: Optional[int] = None, + sub_unit: Optional[int] = None, + array_index: Optional[int] = None, + ) -> str: + """Get the value of a numeric parameter database item. + + Args: + data_id: DataID of parameter. + unit_number: Unit number, usually the robot number (1-NROB). + sub_unit: Sub-unit, usually 0. + array_index: Array index. + + Returns: + str: The numeric value of the specified database parameter. + """ + if unit_number is not None: + if sub_unit is not None: + if array_index is not None: + response = await self.send_command(f"pd {data_id} {unit_number} {sub_unit} {array_index}") + else: + response = await self.send_command(f"pd {data_id} {unit_number} {sub_unit}") + else: + response = await self.send_command(f"pd {data_id} {unit_number}") + else: + response = await self.send_command(f"pd {data_id}") + return response + + async def reset(self, robot_number: int) -> None: + """Reset the threads associated with the specified robot. + + Stops and restarts the threads for the specified robot. Any TCP/IP connections + made by these threads are broken. This command can only be sent to the status thread. + + Args: + robot_number: The number of the robot thread to reset, from 1 to N_ROB. Must not be zero. + + Raises: + ValueError: If robot_number is zero or negative. + """ + if robot_number <= 0: + raise ValueError("Robot number must be greater than zero") + await self.send_command(f"reset {robot_number}") + + async def get_selected_robot(self) -> int: + """Get the number of the currently selected robot. + + Returns: + The number of the currently selected robot. + """ + response = await self.send_command("selectRobot") + return int(response) + + async def select_robot(self, robot_number: int) -> None: + """Change the robot associated with this communications link. + + Does not affect the operation or attachment state of the robot. The status thread + may select any robot or 0. Except for the status thread, a robot may only be + selected by one thread at a time. + + Args: + robot_number: The new robot to be connected to this thread (1 to N_ROB) or 0 for none. + """ + await self.send_command(f"selectRobot {robot_number}") + + async def get_signal(self, signal_number: int) -> int: + """Get the value of the specified digital input or output signal. + + Args: + signal_number: The number of the digital signal to get. + + Returns: + The current signal value. + """ + response = await self.send_command(f"sig {signal_number}") + sig_id, sig_val = response.split() + return int(sig_val) + + async def set_signal(self, signal_number: int, value: int) -> None: + """Set the specified digital input or output signal. + + Args: + signal_number: The number of the digital signal to set. + value: The signal value to set. 0 = off, non-zero = on. + """ + await self.send_command(f"sig {signal_number} {value}") + + async def get_system_state(self) -> int: + """Get the global system state code. + + Returns: + The global system state code. Please see documentation for DataID 234. + """ + response = await self.send_command("sysState") + return int(response) + + async def get_tool_transformation_values(self) -> tuple[float, float, float, float, float, float]: + """Get the current tool transformation values. + + Returns: + A tuple containing (X, Y, Z, yaw, pitch, roll) for the tool transformation. + """ + data = await self.send_command("tool") + if data.startswith("tool: "): + data = data[6:] + parts = data.split() + if len(parts) != 6: + raise PreciseFlexError(-1, "Unexpected response format from tool command.") + x, y, z, yaw, pitch, roll = self._parse_xyz_response(parts) + return (x, y, z, yaw, pitch, roll) + + async def set_tool_transformation_values( + self, x: float, y: float, z: float, yaw: float, pitch: float, roll: float + ) -> None: + """Set the robot tool transformation. + + The robot must be attached to set the tool. Setting the tool pauses any robot motion in progress. + + Args: + x: Tool X coordinate. + y: Tool Y coordinate. + z: Tool Z coordinate. + yaw: Tool yaw rotation. + pitch: Tool pitch rotation. + roll: Tool roll rotation. + """ + await self.send_command(f"tool {x} {y} {z} {yaw} {pitch} {roll}") + + async def get_version(self) -> str: + """Get the current version of TCS and any installed plug-ins. + + Returns: + str: The current version information. + """ + return await self.send_command("version") + + # -- LOCATION COMMANDS ----------------------------------------------------- + + async def _set_joint_angles( + self, + location_index: int, + joint_position: Dict[int, float], + ) -> None: + """Set joint angles for stored location, handling rail configuration.""" + if self._has_rail: + await self.send_command( + f"locAngles {location_index} " + f"{joint_position[PFAxis.RAIL]} " + f"{joint_position[PFAxis.BASE]} " + f"{joint_position[PFAxis.SHOULDER]} " + f"{joint_position[PFAxis.ELBOW]} " + f"{joint_position[PFAxis.WRIST]} " + f"{joint_position[PFAxis.GRIPPER]}" + ) + else: + await self.send_command( + f"locAngles {location_index} " + f"{joint_position[PFAxis.BASE]} " + f"{joint_position[PFAxis.SHOULDER]} " + f"{joint_position[PFAxis.ELBOW]} " + f"{joint_position[PFAxis.WRIST]} " + f"{joint_position[PFAxis.GRIPPER]}" + ) + + async def _set_location_xyz( + self, + location_index: int, + cartesian_position: PreciseFlexCartesianCoords, + ) -> None: + """Set the Cartesian position values for the specified station index. + + Args: + location_index: The station index, from 1 to N_LOC. + cartesian_position: The Cartesian position to set. + """ + await self.send_command( + f"locXyz {location_index} " + f"{cartesian_position.location.x} " + f"{cartesian_position.location.y} " + f"{cartesian_position.location.z} " + f"{cartesian_position.rotation.yaw} " + f"{cartesian_position.rotation.pitch} " + f"{cartesian_position.rotation.roll}" + ) + + async def _set_location_config(self, location_index: int, config_value: int) -> None: + """Set the Config property for the specified location. + + Args: + location_index: The station index, from 1 to N_LOC. + config_value: The new Config property value as a bit mask where: + - 0 = None (no configuration specified) + - 0x01 = GPL_Righty (right shouldered configuration) + - 0x02 = GPL_Lefty (left shouldered configuration) + - 0x04 = GPL_Above (elbow above the wrist) + - 0x08 = GPL_Below (elbow below the wrist) + - 0x10 = GPL_Flip (wrist pitched up) + - 0x20 = GPL_NoFlip (wrist pitched down) + - 0x1000 = GPL_Single (restrict wrist axis to +/- 180 degrees) + Values can be combined using bitwise OR. + + Raises: + ValueError: If config_value contains invalid bits or conflicting configurations. + """ + GPL_RIGHTY = 0x01 + GPL_LEFTY = 0x02 + GPL_ABOVE = 0x04 + GPL_BELOW = 0x08 + GPL_FLIP = 0x10 + GPL_NOFLIP = 0x20 + GPL_SINGLE = 0x1000 + ALL_VALID_BITS = ( + GPL_RIGHTY | GPL_LEFTY | GPL_ABOVE | GPL_BELOW | GPL_FLIP | GPL_NOFLIP | GPL_SINGLE + ) + if config_value & ~ALL_VALID_BITS: + raise ValueError(f"Invalid config bits specified: 0x{config_value:X}") + if (config_value & GPL_RIGHTY) and (config_value & GPL_LEFTY): + raise ValueError("Cannot specify both GPL_Righty and GPL_Lefty") + if (config_value & GPL_ABOVE) and (config_value & GPL_BELOW): + raise ValueError("Cannot specify both GPL_Above and GPL_Below") + if (config_value & GPL_FLIP) and (config_value & GPL_NOFLIP): + raise ValueError("Cannot specify both GPL_Flip and GPL_NoFlip") + await self.send_command(f"locConfig {location_index} {config_value}") + + async def dest_c(self, arg1: int = 0) -> tuple[float, float, float, float, float, float, int]: + """Get the destination or current Cartesian location of the robot. + + Args: + arg1: Selects return value. Defaults to 0. + 0 = Return current Cartesian location if robot is not moving + 1 = Return target Cartesian location of the previous or current move + + Returns: + A tuple containing (X, Y, Z, yaw, pitch, roll, config) + If arg1 = 1 or robot is moving, returns the target location. + If arg1 = 0 and robot is not moving, returns the current location. + """ + if arg1 == 0: + data = await self.send_command("destC") + else: + data = await self.send_command(f"destC {arg1}") + parts = data.split() + if len(parts) != 7: + raise PreciseFlexError(-1, "Unexpected response format from destC command.") + x, y, z, yaw, pitch, roll = self._parse_xyz_response(parts[:6]) + config = int(parts[6]) + return (x, y, z, yaw, pitch, roll, config) + + async def dest_j(self, arg1: int = 0) -> Dict[int, float]: + """Get the destination or current joint location of the robot. + + Args: + arg1: Selects return value. Defaults to 0. + 0 = Return current joint location if robot is not moving + 1 = Return target joint location of the previous or current move + + Returns: + A dict mapping PFAxis to float values. + If arg1 = 1 or robot is moving, returns the target joint positions. + If arg1 = 0 and robot is not moving, returns the current joint positions. + """ + if arg1 == 0: + data = await self.send_command("destJ") + else: + data = await self.send_command(f"destJ {arg1}") + parts = data.split() + if not parts: + raise PreciseFlexError(-1, "Unexpected response format from destJ command.") + return self._parse_angles_response(parts) + + async def here_j(self, location_index: int) -> None: + """Record the current position of the selected robot into the specified Location as angles. + + The Location is automatically set to type "angles". + + Args: + location_index: The station index, from 1 to N_LOC. + """ + await self.send_command(f"hereJ {location_index}") + + async def here_c(self, location_index: int) -> None: + """Record the current position of the selected robot into the specified Location as Cartesian. + + The Location object is automatically set to type "Cartesian". + Can be used to change the pallet origin (index 1,1,1) value. + + Args: + location_index: The station index, from 1 to N_LOC. + """ + await self.send_command(f"hereC {location_index}") + + # -- PROFILE COMMANDS ------------------------------------------------------ + + async def get_profile_speed(self, profile_index: int) -> float: + """Get the speed property of the specified profile. + + Args: + profile_index: The profile index to query. + + Returns: + float: The current speed as a percentage. 100 = full speed. + """ + response = await self.send_command(f"Speed {profile_index}") + profile, speed = response.split() + return float(speed) + + async def set_profile_speed(self, profile_index: int, speed_percent: float) -> None: + """Set the speed property of the specified profile. + + Args: + profile_index: The profile index to modify. + speed_percent: The new speed as a percentage. 100 = full speed. + Values > 100 may be accepted depending on system configuration. + """ + await self.send_command(f"Speed {profile_index} {speed_percent}") + + async def get_profile_speed2(self, profile_index: int) -> float: + """Get the speed2 property of the specified profile. + + Args: + profile_index: The profile index to query. + + Returns: + float: The current speed2 as a percentage. Used for Cartesian moves. + """ + response = await self.send_command(f"Speed2 {profile_index}") + profile, speed2 = response.split() + return float(speed2) + + async def set_profile_speed2(self, profile_index: int, speed2_percent: float) -> None: + """Set the speed2 property of the specified profile. + + Args: + profile_index: The profile index to modify. + speed2_percent: The new speed2 as a percentage. 100 = full speed. + Used for Cartesian moves. Normally set to 0. + """ + await self.send_command(f"Speed2 {profile_index} {speed2_percent}") + + async def get_profile_accel(self, profile_index: int) -> float: + """Get the acceleration property of the specified profile. + + Args: + profile_index: The profile index to query. + + Returns: + float: The current acceleration as a percentage. 100 = maximum acceleration. + """ + response = await self.send_command(f"Accel {profile_index}") + profile, accel = response.split() + return float(accel) + + async def set_profile_accel(self, profile_index: int, accel_percent: float) -> None: + """Set the acceleration property of the specified profile. + + Args: + profile_index: The profile index to modify. + accel_percent: The new acceleration as a percentage. 100 = maximum acceleration. + Maximum value depends on system configuration. + """ + await self.send_command(f"Accel {profile_index} {accel_percent}") + + async def get_profile_accel_ramp(self, profile_index: int) -> float: + """Get the acceleration ramp property of the specified profile. + + Args: + profile_index: The profile index to query. + + Returns: + float: The current acceleration ramp time in seconds. + """ + response = await self.send_command(f"AccRamp {profile_index}") + profile, accel_ramp = response.split() + return float(accel_ramp) + + async def set_profile_accel_ramp(self, profile_index: int, accel_ramp_seconds: float) -> None: + """Set the acceleration ramp property of the specified profile. + + Args: + profile_index: The profile index to modify. + accel_ramp_seconds: The new acceleration ramp time in seconds. + """ + await self.send_command(f"AccRamp {profile_index} {accel_ramp_seconds}") + + async def get_profile_decel(self, profile_index: int) -> float: + """Get the deceleration property of the specified profile. + + Args: + profile_index: The profile index to query. + + Returns: + float: The current deceleration as a percentage. 100 = maximum deceleration. + """ + response = await self.send_command(f"Decel {profile_index}") + profile, decel = response.split() + return float(decel) + + async def set_profile_decel(self, profile_index: int, decel_percent: float) -> None: + """Set the deceleration property of the specified profile. + + Args: + profile_index: The profile index to modify. + decel_percent: The new deceleration as a percentage. 100 = maximum deceleration. + Maximum value depends on system configuration. + """ + await self.send_command(f"Decel {profile_index} {decel_percent}") + + async def get_profile_decel_ramp(self, profile_index: int) -> float: + """Get the deceleration ramp property of the specified profile. + + Args: + profile_index: The profile index to query. + + Returns: + float: The current deceleration ramp time in seconds. + """ + response = await self.send_command(f"DecRamp {profile_index}") + profile, decel_ramp = response.split() + return float(decel_ramp) + + async def set_profile_decel_ramp(self, profile_index: int, decel_ramp_seconds: float) -> None: + """Set the deceleration ramp property of the specified profile. + + Args: + profile_index: The profile index to modify. + decel_ramp_seconds: The new deceleration ramp time in seconds. + """ + await self.send_command(f"DecRamp {profile_index} {decel_ramp_seconds}") + + async def get_profile_in_range(self, profile_index: int) -> float: + """Get the InRange property of the specified profile. + + Args: + profile_index: The profile index to query. + + Returns: + float: The current InRange value (-1 to 100). + -1 = do not stop at end of motion if blending is possible + 0 = always stop but do not check end point error + > 0 = wait until close to end point (larger numbers mean less position error allowed) + """ + response = await self.send_command(f"InRange {profile_index}") + profile, in_range = response.split() + return float(in_range) + + async def set_profile_in_range(self, profile_index: int, in_range_value: float) -> None: + """Set the InRange property of the specified profile. + + Args: + profile_index: The profile index to modify. + in_range_value: The new InRange value from -1 to 100. + -1 = do not stop at end of motion if blending is possible + 0 = always stop but do not check end point error + > 0 = wait until close to end point (larger numbers mean less position error allowed) + + Raises: + ValueError: If in_range_value is not between -1 and 100. + """ + if not (-1 <= in_range_value <= 100): + raise ValueError("InRange value must be between -1 and 100") + await self.send_command(f"InRange {profile_index} {in_range_value}") + + async def get_profile_straight(self, profile_index: int) -> bool: + """Get the Straight property of the specified profile. + + Args: + profile_index: The profile index to query. + + Returns: + The current Straight property value. + True = follow a straight-line path + False = follow a joint-based path (coordinated axes movement) + """ + response = await self.send_command(f"Straight {profile_index}") + profile, straight = response.split() + return straight == "True" + + async def set_profile_straight(self, profile_index: int, straight_mode: bool) -> None: + """Set the Straight property of the specified profile. + + Args: + profile_index: The profile index to modify. + straight_mode: The path type to use. + True = follow a straight-line path + False = follow a joint-based path (robot axes move in coordinated manner) + + Raises: + ValueError: If straight_mode is not True or False. + """ + straight_int = 1 if straight_mode else 0 + await self.send_command(f"Straight {profile_index} {straight_int}") + + async def set_motion_profile_values( + self, + profile: int, + speed: float, + speed2: float, + acceleration: float, + deceleration: float, + acceleration_ramp: float, + deceleration_ramp: float, + in_range: float, + straight: bool, + ): + """ + Set motion profile values for the specified profile index on the PreciseFlex robot. + + Args: + profile: Profile index to set values for. + speed: Percentage of maximum speed. 100 = full speed. Values >100 may be accepted depending on system config. + speed2: Secondary speed setting, typically for Cartesian moves. Normally 0. Interpreted as a percentage. + acceleration: Percentage of maximum acceleration. 100 = full accel. + deceleration: Percentage of maximum deceleration. 100 = full decel. + acceleration_ramp: Acceleration ramp time in seconds. + deceleration_ramp: Deceleration ramp time in seconds. + in_range: InRange value, from -1 to 100. -1 = allow blending, 0 = stop without checking, >0 = enforce position accuracy. + straight: If True, follow a straight-line path (-1). If False, follow a joint-based path (0). + """ + if not (0 <= speed): + raise ValueError("Speed must be > 0 (percent).") + if not (0 <= speed2): + raise ValueError("Speed2 must be > 0 (percent).") + if not (0 <= acceleration <= 100): + raise ValueError("Acceleration must be between 0 and 100 (percent).") + if not (0 <= deceleration <= 100): + raise ValueError("Deceleration must be between 0 and 100 (percent).") + if acceleration_ramp < 0: + raise ValueError("Acceleration ramp must be >= 0 (seconds).") + if deceleration_ramp < 0: + raise ValueError("Deceleration ramp must be >= 0 (seconds).") + if not (-1 <= in_range <= 100): + raise ValueError("InRange must be between -1 and 100.") + straight_int = -1 if straight else 0 + await self.send_command( + f"Profile {profile} {speed} {speed2} {acceleration} {deceleration} " + f"{acceleration_ramp} {deceleration_ramp} {in_range} {straight_int}" + ) + + async def get_motion_profile_values( + self, profile: int + ) -> tuple[int, float, float, float, float, float, float, float, bool]: + """ + Get the current motion profile values for the specified profile index on the PreciseFlex robot. + + Args: + profile: Profile index to get values for. + + Returns: + A tuple containing (profile, speed, speed2, acceleration, deceleration, acceleration_ramp, deceleration_ramp, in_range, straight) + - profile: Profile index + - speed: Percentage of maximum speed + - speed2: Secondary speed setting + - acceleration: Percentage of maximum acceleration + - deceleration: Percentage of maximum deceleration + - acceleration_ramp: Acceleration ramp time in seconds + - deceleration_ramp: Deceleration ramp time in seconds + - in_range: InRange value (-1 to 100) + - straight: True if straight-line path, False if joint-based path + """ + data = await self.send_command(f"Profile {profile}") + parts = data.split(" ") + if len(parts) != 9: + raise PreciseFlexError(-1, "Unexpected response format from device.") + return ( + int(parts[0]), + float(parts[1]), + float(parts[2]), + float(parts[3]), + float(parts[4]), + float(parts[5]), + float(parts[6]), + float(parts[7]), + int(parts[8]) != 0, + ) + + # -- MOTION COMMANDS ------------------------------------------------------- + + async def _move_to_stored_location(self, location_index: int, profile_index: int) -> None: + """Move to the location specified by the station index using the specified profile. + + Args: + location_index: The index of the location to which the robot moves. + profile_index: The profile index for this move. + + Note: + Requires that the robot be attached. + """ + await self.send_command(f"move {location_index} {profile_index}") + + async def _move_to_stored_location_appro(self, location_index: int, profile_index: int) -> None: + """Approach the location specified by the station index using the specified profile. + + This is similar to `_move_to_stored_location` except that the Z clearance value is included. + + Args: + location_index: The index of the location to which the robot moves. + profile_index: The profile index for this move. + + Note: + Requires that the robot be attached. + """ + await self.send_command(f"moveAppro {location_index} {profile_index}") + + async def _move_c( + self, + profile_index: int, + cartesian_coords: PreciseFlexCartesianCoords, + ) -> None: + """Move the robot to the Cartesian location specified by the arguments. + + Args: + profile_index: The profile index to use for this motion. + cartesian_coords: The Cartesian coordinates to which the robot should move. + + Note: + Requires that the robot be attached. + """ + cmd = ( + f"moveC {profile_index} " + f"{cartesian_coords.location.x} " + f"{cartesian_coords.location.y} " + f"{cartesian_coords.location.z} " + f"{cartesian_coords.rotation.yaw} " + f"{cartesian_coords.rotation.pitch} " + f"{cartesian_coords.rotation.roll} " + ) + if cartesian_coords.orientation is not None: + config_int = self._convert_orientation_enum_to_int(cartesian_coords.orientation) + config_int |= 0x1000 + cmd += f"{config_int}" + await self.send_command(cmd) + + async def _move_j(self, profile_index: int, joint_coords: Dict[int, float]) -> None: + """Move the robot using joint coordinates, handling rail configuration.""" + if self._has_rail: + angles_str = ( + f"{joint_coords[PFAxis.BASE]} " + f"{joint_coords[PFAxis.SHOULDER]} " + f"{joint_coords[PFAxis.ELBOW]} " + f"{joint_coords[PFAxis.WRIST]} " + f"{joint_coords[PFAxis.GRIPPER]} " + f"{joint_coords[PFAxis.RAIL]} " + ) + else: + angles_str = ( + f"{joint_coords[PFAxis.BASE]} " + f"{joint_coords[PFAxis.SHOULDER]} " + f"{joint_coords[PFAxis.ELBOW]} " + f"{joint_coords[PFAxis.WRIST]} " + f"{joint_coords[PFAxis.GRIPPER]}" + ) + await self.send_command(f"moveJ {profile_index} {angles_str}") + + async def release_brake(self, axis: int) -> None: + """Release the axis brake. + + Overrides the normal operation of the brake. It is important that the brake not be set + while a motion is being performed. This feature is used to lock an axis to prevent + motion or jitter. + + Args: + axis: The number of the axis whose brake should be released. + """ + await self.send_command(f"releaseBrake {axis}") + + async def set_brake(self, axis: int) -> None: + """Set the axis brake. + + Overrides the normal operation of the brake. It is important not to set a brake on an + axis that is moving as it may damage the brake or damage the motor. + + Args: + axis: The number of the axis whose brake should be set. + """ + await self.send_command(f"setBrake {axis}") + + async def state(self) -> str: + """Return state of motion. + + This value indicates the state of the currently executing or last completed robot motion. + For additional information, please see 'Robot.TrajState' in the GPL reference manual. + + Returns: + str: The current motion state. + """ + return await self.send_command("state") + + async def _wait_for_eom(self) -> None: + """Wait for the robot to reach the end of the current motion. + + Waits for the robot to reach the end of the current motion or until it is stopped by + some other means. Does not reply until the robot has stopped. + """ + await self.send_command("waitForEom") + await asyncio.sleep(0.2) + + async def zero_torque(self, enable: bool, axis_mask: int = 1) -> None: + """Sets or clears zero torque mode for the selected robot. + + Individual axes may be placed into zero torque mode while the remaining axes are servoing. + + Args: + enable: If True, enable torque mode for axes specified by axis_mask. If False, disable torque mode for the entire robot. + axis_mask: The bit mask specifying the axes to be placed in torque mode when enable is True. The mask is computed by OR'ing the axis bits: 1 = axis 1, 2 = axis 2, 4 = axis 3, 8 = axis 4, etc. Ignored when enable is False. + """ + if enable: + assert axis_mask > 0, "axis_mask must be greater than 0" + await self.send_command(f"zeroTorque 1 {axis_mask}") + else: + await self.send_command("zeroTorque 0") + + # -- PAROBOT COMMANDS ------------------------------------------------------ + + async def change_config(self, grip_mode: int = 0) -> None: + """Change Robot configuration from Righty to Lefty or vice versa using customizable locations. + + Uses customizable locations to avoid hitting robot during change. + Does not include checks for collision inside work volume of the robot. + Can be customized by user for their work cell configuration. + + Args: + grip_mode: Gripper control mode. + 0 = do not change gripper (default) + 1 = open gripper + 2 = close gripper + """ + await self.send_command(f"ChangeConfig {grip_mode}") + + async def change_config2(self, grip_mode: int = 0) -> None: + """Change Robot configuration from Righty to Lefty or vice versa using algorithm. + + Uses an algorithm to avoid hitting robot during change. + Does not include checks for collision inside work volume of the robot. + Can be customized by user for their work cell configuration. + + Args: + grip_mode: Gripper control mode. + 0 = do not change gripper (default) + 1 = open gripper + 2 = close gripper + """ + await self.send_command(f"ChangeConfig2 {grip_mode}") + + async def _get_grasp_data(self) -> tuple[float, float, float]: + """Get the data to be used for the next force-controlled PickPlate command grip operation. + + Returns: + A tuple containing (plate_width_mm, finger_speed_percent, grasp_force) + """ + data = await self.send_command("GraspData") + parts = data.split() + if len(parts) != 3: + raise PreciseFlexError(-1, "Unexpected response format from GraspData command.") + return (float(parts[0]), float(parts[1]), float(parts[2])) + + async def _set_grasp_data( + self, plate_width: float, finger_speed_percent: float, grasp_force: float + ) -> None: + """Set the data to be used for the next force-controlled PickPlate command grip operation. + + This data remains in effect until the next GraspData command or the system is restarted. + + Args: + plate_width: The plate width in mm. + finger_speed_percent: The finger speed during grasp where 100 means 100%. + grasp_force: The gripper squeezing force, in Newtons. + A positive value indicates the fingers must close to grasp. + A negative value indicates the fingers must open to grasp. + """ + await self.send_command(f"GraspData {plate_width} {finger_speed_percent} {grasp_force}") + + async def _get_grip_close_pos(self) -> float: + """Get the gripper close position for the servoed gripper. + + Returns: + float: The current gripper close position. + """ + data = await self.send_command("GripClosePos") + return float(data) + + async def _set_grip_close_pos(self, close_position: float) -> None: + """Set the gripper close position for the servoed gripper. + + The close position may be changed by a force-controlled grip operation. + + Args: + close_position: The new gripper close position. + """ + await self.send_command(f"GripClosePos {close_position}") + + async def _get_grip_open_pos(self) -> float: + """Get the gripper open position for the servoed gripper. + + Returns: + float: The current gripper open position. + """ + data = await self.send_command("GripOpenPos") + return float(data) + + async def _set_grip_open_pos(self, open_position: float) -> None: + """Set the gripper open position for the servoed gripper. + + Args: + open_position: The new gripper open position. + """ + await self.send_command(f"GripOpenPos {open_position}") + + # -- parsing helpers ------------------------------------------------------- + + def _parse_xyz_response( + self, parts: List[str] + ) -> tuple[float, float, float, float, float, float]: + if len(parts) != 6: + raise PreciseFlexError(-1, "Unexpected response format for Cartesian coordinates.") + return ( + float(parts[0]), + float(parts[1]), + float(parts[2]), + float(parts[3]), + float(parts[4]), + float(parts[5]), + ) + + def _parse_angles_response(self, parts: List[str]) -> Dict[int, float]: + """Parse angle values from a response string. + + For self._has_rail=True: wire order is [base, shoulder, elbow, wrist, gripper, rail] + For self._has_rail=False: wire order is [base, shoulder, elbow, wrist, gripper] + """ + if len(parts) < 3: + raise PreciseFlexError(-1, "Unexpected response format for angles.") + if self._has_rail: + return { + PFAxis.RAIL: float(parts[5]) if len(parts) > 5 else 0.0, + PFAxis.BASE: float(parts[0]), + PFAxis.SHOULDER: float(parts[1]), + PFAxis.ELBOW: float(parts[2]), + PFAxis.WRIST: float(parts[3]) if len(parts) > 3 else 0.0, + PFAxis.GRIPPER: float(parts[4]) if len(parts) > 4 else 0.0, + } + return { + PFAxis.RAIL: 0.0, + PFAxis.BASE: float(parts[0]), + PFAxis.SHOULDER: float(parts[1]), + PFAxis.ELBOW: float(parts[2]) if len(parts) > 2 else 0.0, + PFAxis.WRIST: float(parts[3]) if len(parts) > 3 else 0.0, + PFAxis.GRIPPER: float(parts[4]) if len(parts) > 4 else 0.0, + } + + +# --------------------------------------------------------------------------- +# Concrete model backends +# --------------------------------------------------------------------------- + + +class PreciseFlex400Backend(PreciseFlexBackend): + """Backend for the PreciseFlex 400 robotic arm.""" + + def __init__( + self, host: str, port: int = 10100, has_rail: bool = False, timeout: int = 20 + ) -> None: + super().__init__(host=host, port=port, has_rail=has_rail, timeout=timeout) + + +class PreciseFlex3400Backend(PreciseFlexBackend): + """Backend for the PreciseFlex 3400 robotic arm.""" + + def __init__( + self, host: str, port: int = 10100, has_rail: bool = False, timeout: int = 20 + ) -> None: + super().__init__(host=host, port=port, has_rail=has_rail, timeout=timeout) diff --git a/pylabrobot/hamilton/liquid_handlers/star/core.py b/pylabrobot/hamilton/liquid_handlers/star/core.py new file mode 100644 index 00000000000..b5edd0ffd68 --- /dev/null +++ b/pylabrobot/hamilton/liquid_handlers/star/core.py @@ -0,0 +1,199 @@ +from dataclasses import dataclass +from typing import Optional + +from pylabrobot.arms.backend import GripperArmBackend +from pylabrobot.legacy.liquid_handling.backends.hamilton.base import HamiltonLiquidHandler +from pylabrobot.resources import Coordinate +from pylabrobot.serializer import SerializableMixin + + +class CoreGripper(GripperArmBackend): + """Backend for Hamilton CoRe gripper tools. + + The CoRe gripper uses two pipetting channels to grip plates along the Y axis. + Tool management (pick up / return) is handled by the STAR backend. + """ + + def __init__(self, interface: HamiltonLiquidHandler): + self.interface = interface + + # -- lifecycle -------------------------------------------------------------- + + async def setup(self) -> None: + pass + + async def stop(self) -> None: + pass + + # -- ArmBackend interface --------------------------------------------------- + + @dataclass + class PickUpParams(SerializableMixin): + grip_strength: int = 15 + y_gripping_speed: float = 5.0 + z_speed: float = 50.0 + minimum_traverse_height: float = 280.0 + z_position_at_end: float = 280.0 + + async def pick_up_at_location( + self, + location: Coordinate, + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Pick up a plate at the specified location. + + Args: + location: Plate center position [mm]. + resource_width: Plate width in Y direction [mm]. + backend_params: CoreGripper.PickUpParams for firmware-specific settings. + """ + if not isinstance(backend_params, CoreGripper.PickUpParams): + backend_params = CoreGripper.PickUpParams() + + open_gripper_position = resource_width + 3.0 + plate_width = resource_width - 3.0 + + if not 0 <= abs(location.x) <= 3000.0: + raise ValueError("x_position must be between -3000.0 and 3000.0") + if not 0 <= abs(location.y) <= 650.0: + raise ValueError("y_position must be between -650.0 and 650.0") + if not 0 <= abs(location.z) <= 360.0: + raise ValueError("z_position must be between -360.0 and 360.0") + if not 0 <= backend_params.grip_strength <= 99: + raise ValueError("grip_strength must be between 0 and 99") + if not 0 <= backend_params.minimum_traverse_height <= 360.0: + raise ValueError("minimum_traverse_height must be between 0 and 360.0") + if not 0 <= backend_params.z_position_at_end <= 360.0: + raise ValueError("z_position_at_end must be between 0 and 360.0") + + await self.interface.send_command( + module="C0", + command="ZP", + xs=f"{abs(round(location.x * 10)):05}", + xd=int(location.x < 0), + yj=f"{abs(round(location.y * 10)):04}", + yv=f"{round(backend_params.y_gripping_speed * 10):04}", + zj=f"{abs(round(location.z * 10)):04}", + zy=f"{round(backend_params.z_speed * 10):04}", + yo=f"{round(open_gripper_position * 10):04}", + yg=f"{round(plate_width * 10):04}", + yw=f"{backend_params.grip_strength:02}", + th=f"{round(backend_params.minimum_traverse_height * 10):04}", + te=f"{round(backend_params.z_position_at_end * 10):04}", + ) + + @dataclass + class DropParams(SerializableMixin): + z_press_on_distance: float = 0.0 + z_speed: float = 50.0 + minimum_traverse_height: float = 280.0 + z_position_at_end: float = 280.0 + + async def drop_at_location( + self, + location: Coordinate, + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Drop a plate at the specified location. + + Args: + location: Plate center position [mm]. + resource_width: Plate width [mm]. Used to compute open gripper position. + backend_params: CoreGripper.DropParams for firmware-specific settings. + """ + if not isinstance(backend_params, CoreGripper.DropParams): + backend_params = CoreGripper.DropParams() + + open_gripper_position = resource_width + 3.0 + + if not 0 <= abs(location.x) <= 3000.0: + raise ValueError("x_position must be between -3000.0 and 3000.0") + if not 0 <= abs(location.y) <= 650.0: + raise ValueError("y_position must be between -650.0 and 650.0") + if not 0 <= abs(location.z) <= 360.0: + raise ValueError("z_position must be between -360.0 and 360.0") + if not 0 <= backend_params.minimum_traverse_height <= 360.0: + raise ValueError("minimum_traverse_height must be between 0 and 360.0") + if not 0 <= backend_params.z_position_at_end <= 360.0: + raise ValueError("z_position_at_end must be between 0 and 360.0") + + await self.interface.send_command( + module="C0", + command="ZR", + xs=f"{abs(round(location.x * 10)):05}", + xd=int(location.x < 0), + yj=f"{abs(round(location.y * 10)):04}", + zj=f"{abs(round(location.z * 10)):04}", + zi=f"{round(backend_params.z_press_on_distance * 10):03}", + zy=f"{round(backend_params.z_speed * 10):04}", + yo=f"{round(open_gripper_position * 10):04}", + th=f"{round(backend_params.minimum_traverse_height * 10):04}", + te=f"{round(backend_params.z_position_at_end * 10):04}", + ) + + @dataclass + class MoveToLocationParams(SerializableMixin): + acceleration_index: int = 4 + z_speed: float = 50.0 + minimum_traverse_height: float = 280.0 + + async def move_to_location( + self, + location: Coordinate, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Move a held plate to a new position without releasing it. + + Args: + location: Target plate center position [mm]. + backend_params: CoreGripper.MoveToLocationParams for firmware-specific settings. + """ + if not isinstance(backend_params, CoreGripper.MoveToLocationParams): + backend_params = CoreGripper.MoveToLocationParams() + + if not 0 <= abs(location.x) <= 3000.0: + raise ValueError("x_position must be between -3000.0 and 3000.0") + if not 0 <= abs(location.y) <= 650.0: + raise ValueError("y_position must be between -650.0 and 650.0") + if not 0 <= abs(location.z) <= 360.0: + raise ValueError("z_position must be between -360.0 and 360.0") + if not 0 <= backend_params.minimum_traverse_height <= 360.0: + raise ValueError("minimum_traverse_height must be between 0 and 360.0") + + await self.interface.send_command( + module="C0", + command="ZM", + xs=f"{abs(round(location.x * 10)):05}", + xd=int(location.x < 0), + xg=backend_params.acceleration_index, + yj=f"{abs(round(location.y * 10)):04}", + zj=f"{abs(round(location.z * 10)):04}", + zy=f"{round(backend_params.z_speed * 10):04}", + th=f"{round(backend_params.minimum_traverse_height * 10):04}", + ) + + async def open_gripper( + self, gripper_width: float, backend_params: Optional[SerializableMixin] = None + ) -> None: + """Open the CoRe gripper.""" + await self.interface.send_command(module="C0", command="ZO") + + async def close_gripper( + self, gripper_width: float, backend_params: Optional[SerializableMixin] = None + ) -> None: + raise NotImplementedError( + "CoreGripper does not support close_gripper directly. Use pick_up_at_location instead." + ) + + async def is_gripper_closed(self, backend_params: Optional[SerializableMixin] = None) -> bool: + raise NotImplementedError() + + async def halt(self, backend_params: Optional[SerializableMixin] = None) -> None: + raise NotImplementedError() + + async def park(self, backend_params: Optional[SerializableMixin] = None) -> None: + raise NotImplementedError( + "CoreGripper does not support park. Tool management is handled by the STAR backend." + ) diff --git a/pylabrobot/hamilton/liquid_handlers/star/core_test.ipynb b/pylabrobot/hamilton/liquid_handlers/star/core_test.ipynb new file mode 100644 index 00000000000..b15f936d5cc --- /dev/null +++ b/pylabrobot/hamilton/liquid_handlers/star/core_test.ipynb @@ -0,0 +1,259 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# CoreGripper + Arm: Simple Resource Move Test\n", + "\n", + "Tests the `CoreGripper` backend through `Arm` with real PLR resources and the real STAR firmware interface.\n", + "\n", + "Tool management (pick up / return gripper tools) is still handled by the STAR backend." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "%load_ext autoreload\n", + "%autoreload 2" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "from pylabrobot.arms.arm import Arm\n", + "from pylabrobot.hamilton.liquid_handlers.star.core import CoreGripper\n", + "from pylabrobot.legacy.liquid_handling.backends.hamilton.STAR_backend import STARBackend\n", + "from pylabrobot.resources.hamilton.hamilton_decks import STARDeck\n", + "from pylabrobot.resources.hamilton.plate_carriers import PLT_CAR_L5AC_A00\n", + "from pylabrobot.resources.corning.plates import Cor_96_wellplate_360ul_Fb" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Set up deck with carrier and plate" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Plate 'my_plate' is in: carrier-1\n", + "Plate absolute location: Coordinate(396.500, 167.500, 183.120)\n", + "Destination site: carrier-2\n", + "Destination location: Coordinate(396.500, 263.500, 186.150)\n" + ] + } + ], + "source": [ + "deck = STARDeck(core_grippers=\"1000uL-at-waste\")\n", + "\n", + "carrier = PLT_CAR_L5AC_A00(\"carrier\")\n", + "deck.assign_child_resource(carrier, rails=14)\n", + "\n", + "plate = Cor_96_wellplate_360ul_Fb(\"my_plate\")\n", + "carrier[1].assign_child_resource(plate)\n", + "\n", + "print(f\"Plate '{plate.name}' is in: {plate.parent.name}\")\n", + "print(f\"Plate absolute location: {plate.get_absolute_location()}\")\n", + "print(f\"Destination site: {carrier[2].name}\")\n", + "print(f\"Destination location: {carrier[2].get_absolute_location()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Create CoreGripper backend with real STAR interface" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "2026-03-21 15:31:31,715 - pylabrobot.io.usb - INFO - Finding USB device...\n", + "2026-03-21 15:31:31,720 - pylabrobot.io.usb - INFO - Found USB device.\n", + "2026-03-21 15:31:31,720 - pylabrobot.io.usb - INFO - Found endpoints. \n", + "Write:\n", + " ENDPOINT 0x2: Bulk OUT ===============================\n", + " bLength : 0x7 (7 bytes)\n", + " bDescriptorType : 0x5 Endpoint\n", + " bEndpointAddress : 0x2 OUT\n", + " bmAttributes : 0x2 Bulk\n", + " wMaxPacketSize : 0x40 (64 bytes)\n", + " bInterval : 0x0 \n", + "Read:\n", + " ENDPOINT 0x81: Bulk IN ===============================\n", + " bLength : 0x7 (7 bytes)\n", + " bDescriptorType : 0x5 Endpoint\n", + " bEndpointAddress : 0x81 IN\n", + " bmAttributes : 0x2 Bulk\n", + " wMaxPacketSize : 0x40 (64 bytes)\n", + " bInterval : 0x0\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Arm ready\n" + ] + } + ], + "source": [ + "star_backend = STARBackend()\n", + "star_backend.set_deck(deck)\n", + "await star_backend.setup()\n", + "\n", + "core_backend = CoreGripper(interface=star_backend)\n", + "\n", + "arm = Arm(backend=core_backend, reference_resource=deck, grip_axis=\"y\")\n", + "print(\"Arm ready\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Pick up core gripper tools\n", + "\n", + "Tool management is handled by the legacy STAR backend for now." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Core gripper tools picked up, core_parked: False\n" + ] + } + ], + "source": [ + "await star_backend.pick_up_core_gripper_tools(front_channel=7)\n", + "print(f\"Core gripper tools picked up, core_parked: {star_backend.core_parked}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Pick up plate from carrier[1]" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [], + "source": [ + "arm._end_holding()" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Picked up 'my_plate'\n" + ] + } + ], + "source": [ + "await arm.pick_up_resource(plate)\n", + "print(f\"Picked up '{plate.name}'\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Drop plate at carrier[2]" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Plate 'my_plate' is now in: carrier-0\n", + "Plate absolute location: Coordinate(396.500, 071.500, 183.120)\n" + ] + } + ], + "source": [ + "await arm.drop_resource(carrier[0])\n", + "print(f\"Plate '{plate.name}' is now in: {plate.parent.name}\")\n", + "print(f\"Plate absolute location: {plate.get_absolute_location()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Return core gripper tools" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "await star_backend.return_core_gripper_tools()\n", + "print(f\"Core gripper tools returned, core_parked: {star_backend.core_parked}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "env", + "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.10.15" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/pylabrobot/hamilton/liquid_handlers/star/core_tests.py b/pylabrobot/hamilton/liquid_handlers/star/core_tests.py new file mode 100644 index 00000000000..7ae9b818a0a --- /dev/null +++ b/pylabrobot/hamilton/liquid_handlers/star/core_tests.py @@ -0,0 +1,166 @@ +import unittest +from unittest.mock import AsyncMock, MagicMock + +from pylabrobot.hamilton.liquid_handlers.star.core import CoreGripper +from pylabrobot.resources import Coordinate + + +class TestCoreGripperCommands(unittest.IsolatedAsyncioTestCase): + """Test that CoreGripper methods produce the exact same firmware commands as the legacy + STARBackend equivalents.""" + + async def asyncSetUp(self): + self.mock_interface = MagicMock() + self.mock_interface.send_command = AsyncMock() + self.core = CoreGripper(interface=self.mock_interface) + + async def test_pick_up_at_location(self): + """ZP with default params, plate width 86mm at (347.9, 114.2, 187.4).""" + await self.core.pick_up_at_location( + location=Coordinate(347.9, 114.2, 187.4), + resource_width=86.0, + ) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="ZP", + xs="03479", + xd=0, + yj="1142", + yv="0050", + zj="1874", + zy="0500", + yo="0890", + yg="0830", + yw="15", + th="2800", + te="2800", + ) + + async def test_pick_up_at_location_custom_params(self): + """ZP with custom grip strength and speeds.""" + await self.core.pick_up_at_location( + location=Coordinate(500.0, 200.0, 150.0), + resource_width=127.76, + backend_params=CoreGripper.PickUpParams( + grip_strength=20, + y_gripping_speed=10.0, + z_speed=80.0, + minimum_traverse_height=300.0, + z_position_at_end=290.0, + ), + ) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="ZP", + xs="05000", + xd=0, + yj="2000", + yv="0100", + zj="1500", + zy="0800", + yo="1308", + yg="1248", + yw="20", + th="3000", + te="2900", + ) + + async def test_drop_at_location(self): + """ZR with default params, plate width 86mm at (347.9, 306.2, 187.4).""" + await self.core.drop_at_location( + location=Coordinate(347.9, 306.2, 187.4), + resource_width=86.0, + ) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="ZR", + xs="03479", + xd=0, + yj="3062", + zj="1874", + zi="000", + zy="0500", + yo="0890", + th="2800", + te="2800", + ) + + async def test_drop_at_location_custom_params(self): + """ZR with custom press distance.""" + await self.core.drop_at_location( + location=Coordinate(500.0, 200.0, 150.0), + resource_width=86.0, + backend_params=CoreGripper.DropParams( + z_press_on_distance=5.0, + z_speed=30.0, + minimum_traverse_height=300.0, + z_position_at_end=290.0, + ), + ) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="ZR", + xs="05000", + xd=0, + yj="2000", + zj="1500", + zi="050", + zy="0300", + yo="0890", + th="3000", + te="2900", + ) + + async def test_move_to_location(self): + """ZM with default params at (500.0, 200.0, 150.0).""" + await self.core.move_to_location( + location=Coordinate(500.0, 200.0, 150.0), + ) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="ZM", + xs="05000", + xd=0, + xg=4, + yj="2000", + zj="1500", + zy="0500", + th="2800", + ) + + async def test_move_to_location_custom_params(self): + """ZM with custom acceleration and speed.""" + await self.core.move_to_location( + location=Coordinate(800.0, 300.0, 200.0), + backend_params=CoreGripper.MoveToLocationParams( + acceleration_index=2, + z_speed=30.0, + minimum_traverse_height=350.0, + ), + ) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="ZM", + xs="08000", + xd=0, + xg=2, + yj="3000", + zj="2000", + zy="0300", + th="3500", + ) + + async def test_open_gripper(self): + """ZO command.""" + await self.core.open_gripper(gripper_width=0) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="ZO", + ) diff --git a/pylabrobot/hamilton/liquid_handlers/star/iswap.py b/pylabrobot/hamilton/liquid_handlers/star/iswap.py new file mode 100644 index 00000000000..c63307dde4d --- /dev/null +++ b/pylabrobot/hamilton/liquid_handlers/star/iswap.py @@ -0,0 +1,338 @@ +from dataclasses import dataclass +from typing import Optional, cast + +from pylabrobot.arms.backend import OrientableGripperArmBackend +from pylabrobot.legacy.liquid_handling.backends.hamilton.base import HamiltonLiquidHandler +from pylabrobot.resources import Coordinate +from pylabrobot.serializer import SerializableMixin + + +def _direction_degrees_to_grip_direction(degrees: float) -> int: + """Convert rotation angle in degrees to firmware grip_direction (1-4). + + Firmware: 1 = negative Y (front), 2 = positive X (right), + 3 = positive Y (back), 4 = negative X (left). + """ + normalized = round(degrees) % 360 + mapping = {0: 1, 90: 2, 180: 3, 270: 4} + if normalized not in mapping: + raise ValueError(f"grip direction must be a multiple of 90 degrees, got {degrees}") + return mapping[normalized] + + +class iSWAP(OrientableGripperArmBackend): + def __init__(self, interface: HamiltonLiquidHandler): + self.interface = interface + self._version: Optional[str] = None + self._parked: Optional[bool] = None + + @property + def version(self) -> str: + """Firmware version string. Available after setup.""" + if self._version is None: + raise RuntimeError("iSWAP version not loaded. Call setup() first.") + return self._version + + @property + def parked(self) -> bool: + return self._parked is True + + async def setup(self) -> None: + self._version = await self._request_version() + + async def _request_version(self) -> str: + """Request the iSWAP firmware version from the device.""" + return cast(str, (await self.interface.send_command("R0", "RF", fmt="rf" + "&" * 15))["rf"]) + + @dataclass + class ParkParams(SerializableMixin): + minimum_traverse_height: float = 284.0 + + async def park(self, backend_params: Optional[SerializableMixin] = None) -> None: + """Park the iSWAP. + + Args: + backend_params: iSWAP.ParkParams with minimum_traverse_height. + """ + if not isinstance(backend_params, iSWAP.ParkParams): + backend_params = iSWAP.ParkParams() + + if not 0 <= backend_params.minimum_traverse_height <= 360.0: + raise ValueError("minimum_traverse_height must be between 0 and 360.0") + + await self.interface.send_command( + module="C0", + command="PG", + th=round(backend_params.minimum_traverse_height * 10), + ) + self._parked = True + + async def open_gripper( + self, gripper_width: float, backend_params: Optional[SerializableMixin] = None + ) -> None: + """Open the iSWAP gripper. + + Args: + gripper_width: Open position [mm]. + backend_params: Unused, reserved for future use. + """ + if not 0 <= gripper_width <= 999.9: + raise ValueError("gripper_width must be between 0 and 999.9") + + await self.interface.send_command( + module="C0", command="GF", go=f"{round(gripper_width * 10):04}" + ) + + @dataclass + class CloseGripperParams(SerializableMixin): + grip_strength: int = 5 + plate_width_tolerance: float = 0 + + async def close_gripper( + self, gripper_width: float, backend_params: Optional[SerializableMixin] = None + ) -> None: + """Close the iSWAP gripper. + + Args: + gripper_width: Plate width [mm]. + backend_params: iSWAP.CloseGripperParams with grip_strength and plate_width_tolerance. + """ + if not isinstance(backend_params, iSWAP.CloseGripperParams): + backend_params = iSWAP.CloseGripperParams() + + if not 0 <= backend_params.grip_strength <= 9: + raise ValueError("grip_strength must be between 0 and 9") + if not 0 <= gripper_width <= 999.9: + raise ValueError("gripper_width must be between 0 and 999.9") + if not 0 <= backend_params.plate_width_tolerance <= 9.9: + raise ValueError("plate_width_tolerance must be between 0 and 9.9") + + await self.interface.send_command( + module="C0", + command="GC", + gw=backend_params.grip_strength, + gb=f"{round(gripper_width * 10):04}", + gt=f"{round(backend_params.plate_width_tolerance * 10):02}", + ) + + @dataclass + class PickUpParams(SerializableMixin): + minimum_traverse_height: float = 280.0 + z_position_at_end: float = 280.0 + grip_strength: int = 4 + plate_width_tolerance: float = 2.0 + collision_control_level: int = 0 + acceleration_index_high_acc: int = 4 + acceleration_index_low_acc: int = 1 + fold_up_at_end: bool = False + + async def pick_up_at_location( + self, + location: Coordinate, + direction: float, + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Pick up a plate at the specified location. + + Args: + location: Plate center position [mm]. + direction: Grip direction in degrees (0=front, 90=right, 180=back, 270=left). + resource_width: Plate width [mm]. + backend_params: iSWAP.PickUpParams for firmware-specific settings. + """ + if not isinstance(backend_params, iSWAP.PickUpParams): + backend_params = iSWAP.PickUpParams() + + open_gripper_position = resource_width + 3.0 + plate_width_for_firmware = round(resource_width * 10) - 33 + + if not 0 <= abs(location.x) <= 3000.0: + raise ValueError("x_position must be between -3000.0 and 3000.0") + if not 0 <= abs(location.y) <= 650.0: + raise ValueError("y_position must be between -650.0 and 650.0") + if not 0 <= abs(location.z) <= 360.0: + raise ValueError("z_position must be between -360.0 and 360.0") + if not 0 <= backend_params.minimum_traverse_height <= 360.0: + raise ValueError("minimum_traverse_height must be between 0 and 360.0") + if not 0 <= backend_params.z_position_at_end <= 360.0: + raise ValueError("z_position_at_end must be between 0 and 360.0") + if not 1 <= backend_params.grip_strength <= 9: + raise ValueError("grip_strength must be between 1 and 9") + if not 0 <= open_gripper_position <= 999.9: + raise ValueError("open_gripper_position must be between 0 and 999.9") + if not 0 <= resource_width <= 999.9: + raise ValueError("resource_width must be between 0 and 999.9") + if not 0 <= backend_params.plate_width_tolerance <= 9.9: + raise ValueError("plate_width_tolerance must be between 0 and 9.9") + if not 0 <= backend_params.collision_control_level <= 1: + raise ValueError("collision_control_level must be 0 or 1") + if not 0 <= backend_params.acceleration_index_high_acc <= 4: + raise ValueError("acceleration_index_high_acc must be between 0 and 4") + if not 0 <= backend_params.acceleration_index_low_acc <= 4: + raise ValueError("acceleration_index_low_acc must be between 0 and 4") + + grip_dir = _direction_degrees_to_grip_direction(direction) + + await self.interface.send_command( + module="C0", + command="PP", + xs=f"{abs(round(location.x * 10)):05}", + xd=int(location.x < 0), + yj=f"{abs(round(location.y * 10)):04}", + yd=int(location.y < 0), + zj=f"{abs(round(location.z * 10)):04}", + zd=int(location.z < 0), + gr=grip_dir, + th=f"{round(backend_params.minimum_traverse_height * 10):04}", + te=f"{round(backend_params.z_position_at_end * 10):04}", + gw=backend_params.grip_strength, + go=f"{round(open_gripper_position * 10):04}", + gb=f"{plate_width_for_firmware:04}", + gt=f"{round(backend_params.plate_width_tolerance * 10):02}", + ga=backend_params.collision_control_level, + gc=backend_params.fold_up_at_end, + ) + self._parked = False + + @dataclass + class DropParams(SerializableMixin): + minimum_traverse_height: float = 280.0 + z_position_at_end: float = 280.0 + collision_control_level: int = 0 + acceleration_index_high_acc: int = 4 + acceleration_index_low_acc: int = 1 + fold_up_at_end: bool = False + + async def drop_at_location( + self, + location: Coordinate, + direction: float, + resource_width: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Drop a plate at the specified location. + + Args: + location: Plate center position [mm]. + direction: Grip direction in degrees (0=front, 90=right, 180=back, 270=left). + resource_width: Plate width [mm]. Used to compute open gripper position. + backend_params: iSWAP.DropParams for firmware-specific settings. + """ + if not isinstance(backend_params, iSWAP.DropParams): + backend_params = iSWAP.DropParams() + + open_gripper_position = resource_width + 3.0 + + if not 0 <= abs(location.x) <= 3000.0: + raise ValueError("x_position must be between -3000.0 and 3000.0") + if not 0 <= abs(location.y) <= 650.0: + raise ValueError("y_position must be between -650.0 and 650.0") + if not 0 <= abs(location.z) <= 360.0: + raise ValueError("z_position must be between -360.0 and 360.0") + if not 0 <= backend_params.minimum_traverse_height <= 360.0: + raise ValueError("minimum_traverse_height must be between 0 and 360.0") + if not 0 <= backend_params.z_position_at_end <= 360.0: + raise ValueError("z_position_at_end must be between 0 and 360.0") + if not 0 <= open_gripper_position <= 999.9: + raise ValueError("open_gripper_position must be between 0 and 999.9") + if not 0 <= backend_params.collision_control_level <= 1: + raise ValueError("collision_control_level must be 0 or 1") + if not 0 <= backend_params.acceleration_index_high_acc <= 4: + raise ValueError("acceleration_index_high_acc must be between 0 and 4") + if not 0 <= backend_params.acceleration_index_low_acc <= 4: + raise ValueError("acceleration_index_low_acc must be between 0 and 4") + + grip_dir = _direction_degrees_to_grip_direction(direction) + + await self.interface.send_command( + module="C0", + command="PR", + xs=f"{abs(round(location.x * 10)):05}", + xd=int(location.x < 0), + yj=f"{abs(round(location.y * 10)):04}", + yd=int(location.y < 0), + zj=f"{abs(round(location.z * 10)):04}", + zd=int(location.z < 0), + th=f"{round(backend_params.minimum_traverse_height * 10):04}", + te=f"{round(backend_params.z_position_at_end * 10):04}", + gr=grip_dir, + go=f"{round(open_gripper_position * 10):04}", + ga=backend_params.collision_control_level, + gc=backend_params.fold_up_at_end, + ) + self._parked = False + + async def is_gripper_closed(self) -> bool: + """Check if the iSWAP is holding a plate. + + Returns: + True if holding a plate, False otherwise. + """ + resp = await self.interface.send_command(module="C0", command="QP", fmt="ph#") + return resp is not None and resp["ph"] == 1 + + async def stop(self) -> None: + raise NotImplementedError() + + @dataclass + class MoveToLocationParams(SerializableMixin): + minimum_traverse_height: float = 360.0 + collision_control_level: int = 1 + acceleration_index_high_acc: int = 4 + acceleration_index_low_acc: int = 1 + + async def move_to_location( + self, + location: Coordinate, + direction: float, + backend_params: Optional[SerializableMixin] = None, + ) -> None: + """Move a held plate to a new position without releasing it. + + Args: + location: Target plate center position [mm]. + direction: Grip direction in degrees (0=front, 90=right, 180=back, 270=left). + backend_params: iSWAP.MoveToLocationParams for firmware-specific settings. + """ + if not isinstance(backend_params, iSWAP.MoveToLocationParams): + backend_params = iSWAP.MoveToLocationParams() + + if not 0 <= abs(location.x) <= 3000.0: + raise ValueError("x_position must be between -3000.0 and 3000.0") + if not 0 <= abs(location.y) <= 650.0: + raise ValueError("y_position must be between -650.0 and 650.0") + if not 0 <= abs(location.z) <= 360.0: + raise ValueError("z_position must be between -360.0 and 360.0") + if not 0 <= backend_params.minimum_traverse_height <= 360.0: + raise ValueError("minimum_traverse_height must be between 0 and 360.0") + if not 0 <= backend_params.collision_control_level <= 1: + raise ValueError("collision_control_level must be 0 or 1") + if not 0 <= backend_params.acceleration_index_high_acc <= 4: + raise ValueError("acceleration_index_high_acc must be between 0 and 4") + if not 0 <= backend_params.acceleration_index_low_acc <= 4: + raise ValueError("acceleration_index_low_acc must be between 0 and 4") + + grip_dir = _direction_degrees_to_grip_direction(direction) + + await self.interface.send_command( + module="C0", + command="PM", + xs=f"{abs(round(location.x * 10)):05}", + xd=int(location.x < 0), + yj=f"{abs(round(location.y * 10)):04}", + yd=int(location.y < 0), + zj=f"{abs(round(location.z * 10)):04}", + zd=int(location.z < 0), + gr=grip_dir, + th=f"{round(backend_params.minimum_traverse_height * 10):04}", + ga=backend_params.collision_control_level, + xe=f"{backend_params.acceleration_index_high_acc} {backend_params.acceleration_index_low_acc}", + ) + self._parked = False + + async def halt(self) -> None: + raise NotImplementedError() + + async def move_to_safe(self) -> None: + raise NotImplementedError() diff --git a/pylabrobot/hamilton/liquid_handlers/star/iswap_test.ipynb b/pylabrobot/hamilton/liquid_handlers/star/iswap_test.ipynb new file mode 100644 index 00000000000..85da446dec6 --- /dev/null +++ b/pylabrobot/hamilton/liquid_handlers/star/iswap_test.ipynb @@ -0,0 +1,281 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# iSWAP + OrientableArm: Simple Resource Move Test\n", + "\n", + "Tests the `iSWAP` backend through `OrientableArm` with real PLR resources and the real STAR firmware interface." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "%load_ext autoreload\n", + "%autoreload 2" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": "from pylabrobot.arms.orientable_arm import OrientableArm\nfrom pylabrobot.arms.standard import GripDirection\nfrom pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAP\nfrom pylabrobot.legacy.liquid_handling.backends.hamilton.STAR_backend import STARBackend\nfrom pylabrobot.resources.hamilton.hamilton_decks import STARLetDeck\nfrom pylabrobot.resources.hamilton.plate_carriers import PLT_CAR_L5AC_A00\nfrom pylabrobot.resources.corning.plates import Cor_96_wellplate_360ul_Fb" + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Set up deck with carrier and plate" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Plate 'my_plate' is in: carrier-1\n", + "Plate absolute location: Coordinate(396.500, 167.500, 183.120)\n", + "Destination site: carrier-2\n", + "Destination location: Coordinate(396.500, 263.500, 186.150)\n" + ] + } + ], + "source": [ + "deck = STARLetDeck()\n", + "\n", + "carrier = PLT_CAR_L5AC_A00(\"carrier\")\n", + "deck.assign_child_resource(carrier, rails=14)\n", + "\n", + "plate = Cor_96_wellplate_360ul_Fb(\"my_plate\")\n", + "carrier[1].assign_child_resource(plate)\n", + "\n", + "print(f\"Plate '{plate.name}' is in: {plate.parent.name}\")\n", + "print(f\"Plate absolute location: {plate.get_absolute_location()}\")\n", + "print(f\"Destination site: {carrier[2].name}\")\n", + "print(f\"Destination location: {carrier[2].get_absolute_location()}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Create iSWAP backend with real STAR interface" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "2026-03-20 22:03:16,770 - pylabrobot.io.usb - INFO - Finding USB device...\n", + "2026-03-20 22:03:16,811 - pylabrobot.io.usb - INFO - Found USB device.\n", + "2026-03-20 22:03:16,817 - pylabrobot.io.usb - INFO - Found endpoints. \n", + "Write:\n", + " ENDPOINT 0x2: Bulk OUT ===============================\n", + " bLength : 0x7 (7 bytes)\n", + " bDescriptorType : 0x5 Endpoint\n", + " bEndpointAddress : 0x2 OUT\n", + " bmAttributes : 0x2 Bulk\n", + " wMaxPacketSize : 0x40 (64 bytes)\n", + " bInterval : 0x0 \n", + "Read:\n", + " ENDPOINT 0x81: Bulk IN ===============================\n", + " bLength : 0x7 (7 bytes)\n", + " bDescriptorType : 0x5 Endpoint\n", + " bEndpointAddress : 0x81 IN\n", + " bmAttributes : 0x2 Bulk\n", + " wMaxPacketSize : 0x40 (64 bytes)\n", + " bInterval : 0x0\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "cmd C0RMid0001\n", + "cmd C0QMid0002\n", + "cmd C0QWid0003\n", + "cmd C0ZAid0004\n", + "cmd C0EVid0005\n", + "cmd C0RTid0006\n", + "cmd I0QWid0007\n", + "cmd P1VYid0008\n", + "cmd P2VYid0009\n", + "cmd P3VYid0010\n", + "cmd P4VYid0011\n", + "cmd P5VYid0012\n", + "cmd P6VYid0013\n", + "cmd P7VYid0014\n", + "cmd P8VYid0015\n", + "cmd P9VYid0016\n", + "cmd PAVYid0017\n", + "cmd PBVYid0018\n", + "cmd PCVYid0019\n", + "cmd C0IVid0020\n", + "cmd R0QWid0021\n", + "cmd I0XPid0022xp54\n", + "cmd C0PGid0023th2800\n", + "cmd H0QWid0024\n", + "cmd H0RFid0025\n", + "cmd H0QUid0026\n", + "cmd H0QGid0027\n", + "OrientableArm ready, iSWAP parked: False\n" + ] + } + ], + "source": [ + "star_backend = STARBackend()\n", + "star_backend.set_deck(deck)\n", + "await star_backend.setup()\n", + "\n", + "iswap_backend = iSWAP(interface=star_backend)\n", + "\n", + "arm = OrientableArm(backend=iswap_backend, reference_resource=deck)\n", + "print(f\"OrientableArm ready, iSWAP parked: {iswap_backend.parked}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Pick up plate from carrier[0]" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "cmd C0PPid0030xs04604xd0yj2102yd0zj1923zd0gr1th2800te2800gw4go1308gb1245gt20ga0gc0\n", + "Picked up 'my_plate'\n" + ] + } + ], + "source": [ + "await arm.pick_up_resource(\n", + " plate,\n", + " direction=GripDirection.FRONT,\n", + " backend_params=iSWAP.PickUpParams(\n", + " minimum_traverse_height=280.0,\n", + " z_position_at_end=280.0,\n", + " grip_strength=4,\n", + " plate_width_tolerance=2.0,\n", + " collision_control_level=0,\n", + " fold_up_at_end=False,\n", + " ),\n", + ")\n", + "print(f\"Picked up '{plate.name}'\")" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "cmd C0PGid0029th2840\n" + ] + } + ], + "source": [ + "await arm.park()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Drop plate at carrier[2]" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "cmd C0PRid0031xs04604xd0yj3062yd0zj1923zd0th2800te2800gr1go1308ga0gc0\n", + "Plate 'my_plate' is now in: carrier-2\n", + "Plate absolute location: Coordinate(396.500, 263.500, 183.120)\n" + ] + } + ], + "source": [ + "await arm.drop_resource(carrier[2], direction=GripDirection.FRONT)\n", + "print(f\"Plate '{plate.name}' is now in: {plate.parent.name}\")\n", + "print(f\"Plate absolute location: {plate.get_absolute_location()}\")" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "metadata": {}, + "outputs": [], + "source": [ + "await arm.pick_up_resource( plate, direction=GripDirection.LEFT,)\n", + "await arm.drop_resource(carrier[2], direction=GripDirection.RIGHT)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Park and check state" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "await arm.park()\n", + "print(f\"iSWAP parked: {iswap_backend.parked}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "env", + "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.10.15" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} \ No newline at end of file diff --git a/pylabrobot/hamilton/liquid_handlers/star/iswap_tests.py b/pylabrobot/hamilton/liquid_handlers/star/iswap_tests.py new file mode 100644 index 00000000000..8dd85f91f76 --- /dev/null +++ b/pylabrobot/hamilton/liquid_handlers/star/iswap_tests.py @@ -0,0 +1,194 @@ +import unittest +from unittest.mock import AsyncMock, MagicMock + +from pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAP +from pylabrobot.resources import Coordinate + + +class TestiSWAPCommands(unittest.IsolatedAsyncioTestCase): + """Test that iSWAP methods produce the exact same firmware commands as the legacy + STARBackend equivalents.""" + + async def asyncSetUp(self): + self.mock_interface = MagicMock() + self.mock_interface.send_command = AsyncMock() + self.iswap = iSWAP(interface=self.mock_interface) + + async def test_pick_up_at_location(self): + """C0PPid0001xs03479xd0yj1142yd0zj1874zd0gr1th2800te2800gw4go1308gb1245gt20ga0gc0""" + await self.iswap.pick_up_at_location( + location=Coordinate(347.9, 114.2, 187.4), + direction=0.0, + resource_width=127.76, + ) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="PP", + xs="03479", + xd=0, + yj="1142", + yd=0, + zj="1874", + zd=0, + gr=1, + th="2800", + te="2800", + gw=4, + go="1308", + gb="1245", + gt="20", + ga=0, + gc=False, + ) + + async def test_pick_up_grip_direction_left(self): + """C0PPid0003xs10427xd0yj3286yd0zj2063zd0gr4th2800te2800gw4go1308gb1245gt20ga0gc0""" + await self.iswap.pick_up_at_location( + location=Coordinate(1042.7, 328.6, 206.3), + direction=270.0, + resource_width=127.76, + ) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="PP", + xs="10427", + xd=0, + yj="3286", + yd=0, + zj="2063", + zd=0, + gr=4, + th="2800", + te="2800", + gw=4, + go="1308", + gb="1245", + gt="20", + ga=0, + gc=False, + ) + + async def test_drop_at_location(self): + """C0PRid0002xs03479xd0yj3062yd0zj1874zd0th2800te2800gr1go1308ga0gc0""" + await self.iswap.drop_at_location( + location=Coordinate(347.9, 306.2, 187.4), + direction=0.0, + resource_width=127.76, + ) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="PR", + xs="03479", + xd=0, + yj="3062", + yd=0, + zj="1874", + zd=0, + th="2800", + te="2800", + gr=1, + go="1308", + ga=0, + gc=False, + ) + + async def test_drop_grip_direction_left(self): + """C0PRid0002xs10427xd0yj3286yd0zj2063zd0th2800te2800gr4go1308ga0gc0""" + await self.iswap.drop_at_location( + location=Coordinate(1042.7, 328.6, 206.3), + direction=270.0, + resource_width=127.76, + ) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="PR", + xs="10427", + xd=0, + yj="3286", + yd=0, + zj="2063", + zd=0, + th="2800", + te="2800", + gr=4, + go="1308", + ga=0, + gc=False, + ) + + async def test_park(self): + await self.iswap.park() + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="PG", + th=2840, + ) + self.assertTrue(self.iswap.parked) + + async def test_park_custom_height(self): + await self.iswap.park(backend_params=iSWAP.ParkParams(minimum_traverse_height=200.0)) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="PG", + th=2000, + ) + + async def test_open_gripper(self): + await self.iswap.open_gripper(gripper_width=130.8) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="GF", + go="1308", + ) + + async def test_close_gripper(self): + await self.iswap.close_gripper( + gripper_width=86.0, + backend_params=iSWAP.CloseGripperParams(grip_strength=5, plate_width_tolerance=0), + ) + + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="GC", + gw=5, + gb="0860", + gt="00", + ) + + async def test_is_gripper_closed(self): + self.mock_interface.send_command.return_value = {"ph": 1} + result = await self.iswap.is_gripper_closed() + self.assertTrue(result) + self.mock_interface.send_command.assert_called_once_with( + module="C0", + command="QP", + fmt="ph#", + ) + + async def test_is_gripper_open(self): + self.mock_interface.send_command.return_value = {"ph": 0} + result = await self.iswap.is_gripper_closed() + self.assertFalse(result) + + async def test_parked_state_after_pick(self): + await self.iswap.pick_up_at_location( + location=Coordinate(100, 100, 100), + direction=0.0, + resource_width=80.0, + ) + self.assertFalse(self.iswap.parked) + + async def test_parked_state_after_drop(self): + await self.iswap.drop_at_location( + location=Coordinate(100, 100, 100), + direction=0.0, + resource_width=80.0, + ) + self.assertFalse(self.iswap.parked) diff --git a/pylabrobot/legacy/arms/precise_flex/pf_3400.py b/pylabrobot/legacy/arms/precise_flex/pf_3400.py index 5b35474576b..cd4ef885bda 100644 --- a/pylabrobot/legacy/arms/precise_flex/pf_3400.py +++ b/pylabrobot/legacy/arms/precise_flex/pf_3400.py @@ -1,8 +1,12 @@ +"""Legacy. Use pylabrobot.brooks.PreciseFlex3400Backend instead.""" + +from pylabrobot.brooks.precise_flex import PreciseFlex3400Backend as _NewBackend from pylabrobot.legacy.arms.precise_flex.precise_flex_backend import PreciseFlexBackend class PreciseFlex3400Backend(PreciseFlexBackend): - """Backend for the PreciseFlex 3400 robotic arm.""" + """Legacy. Use pylabrobot.brooks.PreciseFlex3400Backend instead.""" def __init__(self, host: str, port: int = 10100, has_rail: bool = False, timeout=20) -> None: super().__init__(host=host, port=port, has_rail=has_rail, timeout=timeout) + self._new = _NewBackend(host=host, port=port, has_rail=has_rail, timeout=timeout) diff --git a/pylabrobot/legacy/arms/precise_flex/pf_400.py b/pylabrobot/legacy/arms/precise_flex/pf_400.py index b0a5ec0d837..1a7735585c4 100644 --- a/pylabrobot/legacy/arms/precise_flex/pf_400.py +++ b/pylabrobot/legacy/arms/precise_flex/pf_400.py @@ -1,8 +1,12 @@ +"""Legacy. Use pylabrobot.brooks.PreciseFlex400Backend instead.""" + +from pylabrobot.brooks.precise_flex import PreciseFlex400Backend as _NewBackend from pylabrobot.legacy.arms.precise_flex.precise_flex_backend import PreciseFlexBackend class PreciseFlex400Backend(PreciseFlexBackend): - """Backend for the PreciseFlex 400 robotic arm.""" + """Legacy. Use pylabrobot.brooks.PreciseFlex400Backend instead.""" def __init__(self, host: str, port: int = 10100, has_rail: bool = False, timeout=20) -> None: super().__init__(host=host, port=port, has_rail=has_rail, timeout=timeout) + self._new = _NewBackend(host=host, port=port, has_rail=has_rail, timeout=timeout) diff --git a/pylabrobot/legacy/arms/precise_flex/precise_flex_backend.py b/pylabrobot/legacy/arms/precise_flex/precise_flex_backend.py index afba04dfe69..36c1ce64b5c 100644 --- a/pylabrobot/legacy/arms/precise_flex/precise_flex_backend.py +++ b/pylabrobot/legacy/arms/precise_flex/precise_flex_backend.py @@ -1,8 +1,10 @@ -import asyncio +"""Legacy. Use pylabrobot.brooks instead.""" + import warnings from abc import ABC -from typing import Dict, List, Literal, Optional, Union +from typing import Dict, List, Optional, Union +from pylabrobot.brooks import precise_flex as _new_module from pylabrobot.io.socket import Socket from pylabrobot.legacy.arms.backend import ( AccessPattern, @@ -11,31 +13,64 @@ VerticalAccess, ) from pylabrobot.legacy.arms.precise_flex.coords import ElbowOrientation, PreciseFlexCartesianCoords -from pylabrobot.legacy.arms.precise_flex.error_codes import ERROR_CODES -from pylabrobot.legacy.arms.precise_flex.joints import PFAxis from pylabrobot.resources import Coordinate, Rotation -class PreciseFlexError(Exception): - def __init__(self, replycode: int, message: str): - self.replycode = replycode - self.message = message +PreciseFlexError = _new_module.PreciseFlexError - # Map error codes to text and descriptions - error_info = ERROR_CODES - if replycode in error_info: - text = error_info[replycode]["text"] - description = error_info[replycode]["description"] - super().__init__(f"PreciseFlexError {replycode}: {text}. {description} - {message}") - else: - super().__init__(f"PreciseFlexError {replycode}: {message}") + +def _to_new_coords( + position: Union[PreciseFlexCartesianCoords, Dict[int, float]], +) -> Union[_new_module.PreciseFlexCartesianCoords, Dict[int, float]]: + """Convert legacy CartesianCoords to new module's CartesianCoords.""" + if isinstance(position, PreciseFlexCartesianCoords): + orientation = None + if position.orientation is not None: + orientation = _new_module.ElbowOrientation(position.orientation.value) + return _new_module.PreciseFlexCartesianCoords( + location=position.location, + rotation=position.rotation, + orientation=orientation, + ) + return position -class PreciseFlexBackend(SCARABackend, ABC): - """Backend for the PreciseFlex robotic arm - Default to using Cartesian coordinates, some methods in Brook's TCS don't work with Joint coordinates. +def _to_new_access(access: Optional[AccessPattern]) -> Optional[_new_module.AccessPattern]: + """Convert legacy AccessPattern to new module's AccessPattern.""" + if access is None: + return None + if isinstance(access, VerticalAccess): + return _new_module.VerticalAccess( + approach_height_mm=access.approach_height_mm, + clearance_mm=access.clearance_mm, + gripper_offset_mm=access.gripper_offset_mm, + ) + if isinstance(access, HorizontalAccess): + return _new_module.HorizontalAccess( + approach_distance_mm=access.approach_distance_mm, + clearance_mm=access.clearance_mm, + lift_height_mm=access.lift_height_mm, + gripper_offset_mm=access.gripper_offset_mm, + ) + return None + + +def _from_new_coords( + position: _new_module.PreciseFlexCartesianCoords, +) -> PreciseFlexCartesianCoords: + """Convert new module's CartesianCoords to legacy CartesianCoords.""" + orientation = None + if position.orientation is not None: + orientation = ElbowOrientation(position.orientation.value) + return PreciseFlexCartesianCoords( + location=position.location, + rotation=position.rotation, + orientation=orientation, + ) - Documentation and error codes available at https://www2.brooksautomation.com/#Root/Welcome.htm - """ + +class PreciseFlexBackend(SCARABackend, ABC): + """Legacy. Use pylabrobot.brooks.PreciseFlexBackend instead.""" def __init__( self, @@ -46,6 +81,8 @@ def __init__( timeout=20, ) -> None: super().__init__() + self._new: _new_module.PreciseFlexBackend # set by subclasses + # Keep these for any legacy code that accesses them directly self.io = Socket(human_readable_device_name="Precise Flex Arm", host=host, port=port) self.profile_index: int = 1 self.location_index: int = 1 @@ -62,7 +99,6 @@ def __init__( def _convert_to_cartesian_space( self, position: tuple[float, float, float, float, float, float, Optional[ElbowOrientation]] ) -> PreciseFlexCartesianCoords: - """Convert a tuple of cartesian coordinates to a CartesianCoords object.""" if len(position) != 7: raise ValueError( "Position must be a tuple of 7 values (x, y, z, yaw, pitch, roll, orientation)." @@ -77,7 +113,6 @@ def _convert_to_cartesian_space( def _convert_to_cartesian_array( self, position: PreciseFlexCartesianCoords ) -> tuple[float, float, float, float, float, float, int]: - """Convert a CartesianCoords object to a list of cartesian coordinates.""" orientation_int = self._convert_orientation_enum_to_int(position.orientation) arr = ( position.location.x, @@ -91,59 +126,31 @@ def _convert_to_cartesian_array( return arr async def setup(self, skip_home: bool = False): - """Initialize the PreciseFlex backend.""" - await self.io.setup() - await self.set_response_mode("pc") - await self.power_on_robot() - await self.attach(1) - if not skip_home: - await self.home() + await self._new.setup(skip_home=skip_home) async def stop(self): - """Stop the PreciseFlex backend.""" - await self.detach() - await self.power_off_robot() - await self.exit() - await self.io.stop() + await self._new.stop() async def set_speed(self, speed_percent: float): - """Set the speed percentage of the arm's movement (0-100).""" - await self.set_profile_speed(self.profile_index, speed_percent) + await self._new._set_speed(speed_percent) async def get_speed(self) -> float: - """Get the current speed percentage of the arm's movement.""" - return await self.get_profile_speed(self.profile_index) + return await self._new._get_speed() async def open_gripper(self, gripper_width: float): - """Open the gripper to the specified width. If no width is specified, opens to the default open position.""" - await self._set_grip_open_pos(gripper_width) - await self.send_command("gripper 1") + await self._new.open_gripper(gripper_width) async def close_gripper(self, gripper_width: float): - """Close the gripper to the specified width. If no width is specified, closes to the default close position.""" - await self._set_grip_close_pos(gripper_width) - await self.send_command("gripper 2") + await self._new.close_gripper(gripper_width) async def halt(self): - """Stops the current robot immediately but leaves power on.""" - await self.send_command("halt") + await self._new.halt() async def home(self) -> None: - """Home the robot associated with this thread. - - Note: - Requires power to be enabled. - Requires robot to be attached. - Waits until the homing is complete. - """ - await self.send_command("home") + await self._new.home() async def move_to_safe(self) -> None: - """Moves the robot to Safe Position. - - Does not include checks for collision with 3rd party obstacles inside the work volume of the robot. - """ - await self.send_command("movetosafe") + await self._new.move_to_safe() def _convert_orientation_int_to_enum(self, orientation_int: int) -> Optional[ElbowOrientation]: if orientation_int == 1: @@ -160,78 +167,26 @@ def _convert_orientation_enum_to_int(self, orientation: Optional[ElbowOrientatio return 0 async def home_all(self) -> None: - """Home all robots. - - Note: - Requires power to be enabled. - Requires that robots not be attached. - """ - await self.send_command("homeAll") + await self._new.home_all() async def attach(self, attach_state: Optional[int] = None) -> int: - """Attach or release the robot, or get attachment state. - - Args: - attach_state: If omitted, returns the attachment state. 0 = release the robot; 1 = attach the robot. - - Returns: - If attach_state is omitted, returns 0 if robot is not attached, -1 if attached. Otherwise returns 0 on success. - - Note: - The robot must be attached to allow motion commands. - """ - if attach_state is None: - response = await self.send_command("attach") - return int(response) - await self.send_command(f"attach {attach_state}") - return 0 + return await self._new.attach(attach_state) async def detach(self): - """Detach the robot.""" - await self.attach(0) + await self._new.detach() async def power_on_robot(self): - """Power on the robot.""" - await self.set_power(True, self.timeout) + await self._new.power_on_robot() async def power_off_robot(self): - """Power off the robot.""" - await self.set_power(False) + await self._new.power_off_robot() async def approach( self, position: Union[PreciseFlexCartesianCoords, Dict[int, float]], access: Optional[AccessPattern] = None, ): - """Move the arm to an approach position (offset from target). - - Args: - position: Target position (CartesianCoords or Dict[int, float]) - access: Access pattern defining how to approach the target. Defaults to VerticalAccess() if not specified. - - Example: - # Simple vertical approach (default) - await backend.approach(position) - - # Horizontal hotel-style approach - await backend.approach( - position, - HorizontalAccess( - approach_distance_mm=50, - clearance_mm=50, - lift_height_mm=100 - ) - ) - """ - if access is None: - access = VerticalAccess() - - if isinstance(position, dict): - await self._approach_j(position, access) - elif isinstance(position, PreciseFlexCartesianCoords): - await self._approach_c(position, access) - else: - raise TypeError("Position must be of type Dict[int, float] or CartesianCoords.") + await self._new.approach(_to_new_coords(position), _to_new_access(access)) async def pick_up_resource( self, @@ -241,1493 +196,341 @@ async def pick_up_resource( finger_speed_percent: float = 50.0, grasp_force: float = 10.0, ): - """Pick a plate from the specified position. - - Args: - position: Target position for pickup (CartesianCoords only, joint coords not supported) - plate_width: Gripper width in millimeters used when gripping the plate. - access: How to access the location (VerticalAccess or HorizontalAccess). Defaults to VerticalAccess() if not specified. - finger_speed_percent: Speed percentage for the gripper fingers (1-100) - grasp_force: Grasp force in Newtons - - Raises: - ValueError: If position is not CartesianCoords - - Example: - # Simple vertical pick (default) - await backend.pick_plate(position) - - # Vertical pick with custom clearance - await backend.pick_plate(position, VerticalAccess(clearance_mm=150)) - - # Horizontal hotel-style pick - await backend.pick_plate( - position, - HorizontalAccess( - approach_distance_mm=50, - clearance_mm=50, - lift_height_mm=100 - ) - ) - """ - if access is None: - access = VerticalAccess() - - await self.set_grasp_data( - plate_width=plate_width, + converted = _to_new_coords(position) + params = _new_module.PreciseFlexBackend.PickUpParams( + access=_to_new_access(access), finger_speed_percent=finger_speed_percent, grasp_force=grasp_force, ) - - if isinstance(position, PreciseFlexCartesianCoords): - await self._pick_plate_c(cartesian_position=position, access=access) - elif isinstance(position, dict): - await self._pick_plate_j(position, access) + if isinstance(converted, dict): + await self._new.pick_up_at_joint_position(converted, plate_width, backend_params=params) else: - raise TypeError("Position must be of type Dict[int, float] or CartesianCoords.") + await self._new.pick_up_at_location( + converted.location, converted.rotation.z, plate_width, backend_params=params + ) async def drop_resource( self, position: Union[PreciseFlexCartesianCoords, Dict[int, float]], access: Optional[AccessPattern] = None, ): - """Place a plate at the specified position. - - Args: - position: Target position for placement (CartesianCoords only, joint coords not supported) - access: How to access the location (VerticalAccess or HorizontalAccess). Defaults to VerticalAccess() if not specified. - - Raises: - ValueError: If position is not CartesianCoords - - Example: - # Simple vertical place (default) - await backend.place_plate(position) - - # Vertical place with custom clearance - await backend.place_plate(position, VerticalAccess(clearance_mm=150)) - - # Horizontal hotel-style place - await backend.place_plate( - position, - HorizontalAccess( - approach_distance_mm=50, - clearance_mm=50, - lift_height_mm=100 - ) + converted = _to_new_coords(position) + params = _new_module.PreciseFlexBackend.DropParams(access=_to_new_access(access)) + if isinstance(converted, dict): + await self._new.drop_at_joint_position(converted, resource_width=0, backend_params=params) + else: + await self._new.drop_at_location( + converted.location, converted.rotation.z, resource_width=0, backend_params=params ) - """ - if access is None: - access = VerticalAccess() - - if not isinstance(position, PreciseFlexCartesianCoords): - raise TypeError("place_plate only supports CartesianCoords for PreciseFlex.") - await self._place_plate_c(cartesian_position=position, access=access) async def move_to(self, position: Union[PreciseFlexCartesianCoords, Dict[int, float]]): - """Move the arm to a specified position in 3D space. - - Args: - position: Either CartesianCoords or a dict mapping PFAxis to float values. - When using a dict, any unspecified axes will be filled in from the current position. - """ - print(position, isinstance(position, dict)) - if isinstance(position, dict): - current = await self.get_joint_position() - joint_coords = {**current, **position} - await self.move_j(profile_index=self.profile_index, joint_coords=joint_coords) - elif isinstance(position, PreciseFlexCartesianCoords): - await self.move_c(profile_index=self.profile_index, cartesian_coords=position) + converted = _to_new_coords(position) + if isinstance(converted, dict): + await self._new.move_to_joint_position(converted) else: - raise TypeError("Position must be of type Dict[int, float] or CartesianCoords.") + await self._new.move_to_location(converted.location, converted.rotation) async def get_joint_position(self) -> Dict[int, float]: - """Get the current joint position of the arm.""" - - await self.wait_for_eom() - - num_tries = 2 - for _ in range(num_tries): - data = await self.send_command("wherej") - parts = data.split() - if len(parts) > 0: - break - else: - raise PreciseFlexError(-1, "Unexpected response format from wherej command.") - - return self._parse_angles_response(parts) + return await self._new.get_joint_position() async def get_cartesian_position(self) -> PreciseFlexCartesianCoords: - """Get the current position of the arm in 3D space.""" - - await self.wait_for_eom() - - num_tries = 2 - for _ in range(num_tries): - data = await self.send_command("wherec") - parts = data.split() - if len(parts) == 7: - break - else: - raise PreciseFlexError(-1, "Unexpected response format from wherec command.") - - x, y, z, yaw, pitch, roll = self._parse_xyz_response(parts[0:6]) - config = int(parts[6]) - - # return (x, y, z, yaw, pitch, roll, config) - enum_thing = self._convert_orientation_int_to_enum(config) - - return self._convert_to_cartesian_space(position=(x, y, z, yaw, pitch, roll, enum_thing)) + result = await self._new.get_cartesian_position() + return _from_new_coords(result) async def send_command(self, command: str) -> str: - await self.io.write(command.encode("utf-8") + b"\n") - reply = await self.io.readline() - - return self._parse_reply_ensure_successful(reply) + return await self._new.send_command(command) def _parse_reply_ensure_successful(self, reply: bytes) -> str: - """Parse reply from Precise Flex. - - Expected format: b'replycode data message\r\n' - - replycode is an integer at the beginning - - data is rest of the line (excluding CRLF) - """ - text = reply.decode().strip() # removes \r\n - if not text: - raise PreciseFlexError(-1, "Empty reply from device.") - - parts = text.split(" ", 1) - if len(parts) == 1: - replycode = int(parts[0]) - data = "" - else: - replycode, data = int(parts[0]), parts[1] - - if replycode != 0: - # if error is reported, the data part generally contains the error message - raise PreciseFlexError(replycode, data) - - return data - - async def _approach_j(self, joint_position: Dict[int, float], access: AccessPattern): - """Move the arm to a position above the specified coordinates. - - The approach behavior depends on the access pattern: - - VerticalAccess: Approaches from above using approach_height_mm - - HorizontalAccess: Approaches from the side using approach_distance_mm - """ - await self.set_joint_angles(self.location_index, joint_position) - await self._set_grip_detail(access) - await self.move_to_stored_location_appro(self.location_index, self.profile_index) - - async def _pick_plate_j(self, joint_position: Dict[int, float], access: AccessPattern): - """Pick a plate from the specified position using joint coordinates.""" - await self.set_joint_angles(self.location_index, joint_position) - await self._set_grip_detail(access) - await self.pick_plate_from_stored_position( - self.location_index, self.horizontal_compliance, self.horizontal_compliance_torque - ) + return self._new._parse_reply_ensure_successful(reply) - async def _place_plate_j(self, joint_position: Dict[int, float], access: AccessPattern): - """Place a plate at the specified position using joint coordinates.""" - await self.set_joint_angles(self.location_index, joint_position) - await self._set_grip_detail(access) - await self.place_plate_to_stored_position( - self.location_index, self.horizontal_compliance, self.horizontal_compliance_torque - ) - - async def _approach_c( - self, - cartesian_position: PreciseFlexCartesianCoords, - access: AccessPattern, - ): - """Move the arm to a position above the specified coordinates. - - The approach behavior depends on the access pattern: - - VerticalAccess: Approaches from above using approach_height_mm - - HorizontalAccess: Approaches from the side using approach_distance_mm - """ - await self.set_location_xyz(self.location_index, cartesian_position) - await self._set_grip_detail(access) - orientation_int = self._convert_orientation_enum_to_int(cartesian_position.orientation) - await self.set_location_config(self.location_index, orientation_int) - await self.move_to_stored_location_appro(self.location_index, self.profile_index) - - async def _pick_plate_c( - self, - cartesian_position: PreciseFlexCartesianCoords, - access: AccessPattern, - ): - """Pick a plate from the specified position using Cartesian coordinates.""" - await self.set_location_xyz(self.location_index, cartesian_position) - await self._set_grip_detail(access) - orientation_int = self._convert_orientation_enum_to_int(cartesian_position.orientation) - orientation_int |= 0x1000 # GPL_Single: restrict wrist to ±180° - await self.set_location_config(self.location_index, orientation_int) - await self.pick_plate_from_stored_position( - self.location_index, self.horizontal_compliance, self.horizontal_compliance_torque - ) - - async def _place_plate_c( - self, - cartesian_position: PreciseFlexCartesianCoords, - access: AccessPattern, - ): - """Place a plate at the specified position using Cartesian coordinates.""" - await self.set_location_xyz(self.location_index, cartesian_position) - await self._set_grip_detail(access) - orientation_int = self._convert_orientation_enum_to_int(cartesian_position.orientation) - orientation_int |= 0x1000 # GPL_Single: restrict wrist to ±180° - await self.set_location_config(self.location_index, orientation_int) - await self.place_plate_to_stored_position( - self.location_index, self.horizontal_compliance, self.horizontal_compliance_torque - ) - - async def _set_grip_detail(self, access: AccessPattern): - """Configure station type for pick/place operations based on access pattern. - - Calls TCS set_station_type command to configure how the robot interprets - clearance values and performs approach/retract motions. - - Args: - access: Access pattern (VerticalAccess or HorizontalAccess) defining how to approach and retract from the location. - """ - if isinstance(access, VerticalAccess): - # Vertical access: access_type=1, z_clearance is vertical distance - await self.set_station_type( - station_id=self.location_index, - access_type=1, - location_type=0, - z_clearance=access.clearance_mm, - z_above=0, - z_grasp_offset=access.gripper_offset_mm, - ) - elif isinstance(access, HorizontalAccess): - # Horizontal access: access_type=0, z_clearance is horizontal distance - await self.set_station_type( - station_id=self.location_index, - access_type=0, - location_type=0, - z_clearance=access.clearance_mm, - z_above=access.lift_height_mm, - z_grasp_offset=access.gripper_offset_mm, - ) - else: - raise TypeError("Access pattern must be VerticalAccess or HorizontalAccess.") - - # region GENERAL COMMANDS - - async def get_base(self) -> tuple[float, float, float, float]: - """Get the robot base offset. + async def is_gripper_closed(self) -> bool: + return await self._new.is_gripper_closed() - Returns: - A tuple containing (x_offset, y_offset, z_offset, z_rotation) - """ - data = await self.send_command("base") - parts = data.split() - if len(parts) != 4: - raise PreciseFlexError(-1, "Unexpected response format from base command.") + async def are_grippers_closed(self) -> tuple[bool, bool]: + return await self._new.are_grippers_closed() - x_offset = float(parts[0]) - y_offset = float(parts[1]) - z_offset = float(parts[2]) - z_rotation = float(parts[3]) + async def freedrive_mode(self, free_axes: List[int]) -> None: + await self._new.start_freedrive_mode(free_axes) - return (x_offset, y_offset, z_offset, z_rotation) + async def end_freedrive_mode(self) -> None: + await self._new.stop_freedrive_mode() async def set_base( self, x_offset: float, y_offset: float, z_offset: float, z_rotation: float ) -> None: - """Set the robot base offset. + await self._new.set_base(x_offset, y_offset, z_offset, z_rotation) - Args: - x_offset: Base X offset - y_offset: Base Y offset - z_offset: Base Z offset - z_rotation: Base Z rotation - - Note: - The robot must be attached to set the base. - Setting the base pauses any robot motion in progress. - """ - await self.send_command(f"base {x_offset} {y_offset} {z_offset} {z_rotation}") + async def get_base(self) -> tuple[float, float, float, float]: + return await self._new.get_base() async def exit(self) -> None: - """Close the communications link immediately. - - Note: - Does not affect any robots that may be active. - """ - await self.io.write(b"exit\n") + await self._new.exit() async def get_power_state(self) -> int: - """Get the current robot power state. - - Returns: - Current power state (0 = disabled, 1 = enabled) - """ - response = await self.send_command("hp") - return int(response) + return await self._new.get_power_state() async def set_power(self, enable: bool, timeout: int = 0) -> None: - """Enable or disable robot high power. - - Args: - enable: True to enable power, False to disable - timeout: Wait timeout for power to come on. - 0 or omitted = do not wait for power to come on - > 0 = wait this many seconds for power to come on - -1 = wait indefinitely for power to come on - - Raises: - PreciseFlexError: If power does not come on within the specified timeout. - """ - power_state = 1 if enable else 0 - if timeout == 0: - await self.send_command(f"hp {power_state}") - else: - await self.send_command(f"hp {power_state} {timeout}") - - ResponseMode = Literal["pc", "verbose"] - - async def get_mode(self) -> ResponseMode: - """Get the current response mode. - - Returns: - Current mode (0 = PC mode, 1 = verbose mode) - """ - response = await self.send_command("mode") - mapping: Dict[int, PreciseFlexBackend.ResponseMode] = { - 0: "pc", - 1: "verbose", - } - return mapping[int(response)] - - async def set_response_mode(self, mode: ResponseMode) -> None: - """Set the response mode. - - Args: - mode: Response mode to set. - 0 = Select PC mode - 1 = Select verbose mode - - Note: - When using serial communications, the mode change does not take effect - until one additional command has been processed. - """ - if mode not in ["pc", "verbose"]: - raise ValueError("Mode must be 'pc' or 'verbose'") - mapping = { - "pc": 0, - "verbose": 1, - } - await self.send_command(f"mode {mapping[mode]}") - - async def get_monitor_speed(self) -> int: - """Get the global system (monitor) speed. + await self._new.set_power(enable, timeout) - Returns: - Current monitor speed as a percentage (1-100) - """ - response = await self.send_command("mspeed") - return int(response) + async def get_mode(self): + return await self._new.get_mode() - async def set_monitor_speed(self, speed_percent: int) -> None: - """Set the global system (monitor) speed. + async def set_response_mode(self, mode) -> None: + await self._new.set_response_mode(mode) - Args: - speed_percent: Speed percentage between 1 and 100, where 100 means full speed. + async def get_monitor_speed(self) -> int: + return await self._new.get_monitor_speed() - Raises: - ValueError: If speed_percent is not between 1 and 100. - """ - if not (1 <= speed_percent <= 100): - raise ValueError("Speed percent must be between 1 and 100") - await self.send_command(f"mspeed {speed_percent}") + async def set_monitor_speed(self, speed_percent: int) -> None: + await self._new.set_monitor_speed(speed_percent) async def nop(self) -> None: - """No operation command. - - Does nothing except return the standard reply. Can be used to see if the link - is active or to check for exceptions. - """ - await self.send_command("nop") + await self._new.nop() async def get_payload(self) -> int: - """Get the payload percent value for the current robot. - - Returns: - Current payload as a percentage of maximum (0-100) - """ - response = await self.send_command("payload") - return int(response) + return await self._new.get_payload() async def set_payload(self, payload_percent: int) -> None: - """Set the payload percent of maximum for the currently selected or attached robot. - - Args: - payload_percent: Payload percentage from 0 to 100 indicating the percent of the maximum payload the robot is carrying. - - Raises: - ValueError: If payload_percent is not between 0 and 100. + await self._new.set_payload(payload_percent) - Note: - If the robot is moving, waits for the robot to stop before setting a value. - """ - if not (0 <= payload_percent <= 100): - raise ValueError("Payload percent must be between 0 and 100") - await self.send_command(f"payload {payload_percent}") + async def set_parameter(self, data_id, value, unit_number=None, sub_unit=None, array_index=None): + await self._new.set_parameter(data_id, value, unit_number, sub_unit, array_index) - async def set_parameter( - self, - data_id: int, - value, - unit_number: Optional[int] = None, - sub_unit: Optional[int] = None, - array_index: Optional[int] = None, - ) -> None: - """Change a value in the controller's parameter database. - - Args: - data_id: DataID of parameter. - value: New parameter value. If string, will be quoted automatically. - unit_number: Unit number, usually the robot number (1 - N_ROB). - sub_unit: Sub-unit, usually 0. - array_index: Array index. - - Note: - Updated values are not saved in flash unless a save-to-flash operation - is performed (see DataID 901). - """ - if unit_number is not None and sub_unit is not None and array_index is not None: - # 5 argument format - if isinstance(value, str): - await self.send_command(f'pc {data_id} {unit_number} {sub_unit} {array_index} "{value}"') - else: - await self.send_command(f"pc {data_id} {unit_number} {sub_unit} {array_index} {value}") - else: - # 2 argument format - if isinstance(value, str): - await self.send_command(f'pc {data_id} "{value}"') - else: - await self.send_command(f"pc {data_id} {value}") - - async def get_parameter( - self, - data_id: int, - unit_number: Optional[int] = None, - sub_unit: Optional[int] = None, - array_index: Optional[int] = None, - ) -> str: - """Get the value of a numeric parameter database item. - - Args: - data_id: DataID of parameter. - unit_number: Unit number, usually the robot number (1-NROB). - sub_unit: Sub-unit, usually 0. - array_index: Array index. - - Returns: - str: The numeric value of the specified database parameter. - """ - if unit_number is not None: - if sub_unit is not None: - if array_index is not None: - response = await self.send_command(f"pd {data_id} {unit_number} {sub_unit} {array_index}") - else: - response = await self.send_command(f"pd {data_id} {unit_number} {sub_unit}") - else: - response = await self.send_command(f"pd {data_id} {unit_number}") - else: - response = await self.send_command(f"pd {data_id}") - return response + async def get_parameter(self, data_id, unit_number=None, sub_unit=None, array_index=None): + return await self._new.get_parameter(data_id, unit_number, sub_unit, array_index) async def reset(self, robot_number: int) -> None: - """Reset the threads associated with the specified robot. - - Stops and restarts the threads for the specified robot. Any TCP/IP connections - made by these threads are broken. This command can only be sent to the status thread. - - Args: - robot_number: The number of the robot thread to reset, from 1 to N_ROB. Must not be zero. - - Raises: - ValueError: If robot_number is zero or negative. - """ - if robot_number <= 0: - raise ValueError("Robot number must be greater than zero") - await self.send_command(f"reset {robot_number}") + await self._new.reset(robot_number) async def get_selected_robot(self) -> int: - """Get the number of the currently selected robot. - - Returns: - The number of the currently selected robot. - """ - response = await self.send_command("selectRobot") - return int(response) + return await self._new.get_selected_robot() async def select_robot(self, robot_number: int) -> None: - """Change the robot associated with this communications link. - - Does not affect the operation or attachment state of the robot. The status thread - may select any robot or 0. Except for the status thread, a robot may only be - selected by one thread at a time. - - Args: - robot_number: The new robot to be connected to this thread (1 to N_ROB) or 0 for none. - """ - await self.send_command(f"selectRobot {robot_number}") + await self._new.select_robot(robot_number) async def get_signal(self, signal_number: int) -> int: - """Get the value of the specified digital input or output signal. - - Args: - signal_number: The number of the digital signal to get. - - Returns: - The current signal value. - """ - response = await self.send_command(f"sig {signal_number}") - sig_id, sig_val = response.split() - return int(sig_val) + return await self._new.get_signal(signal_number) async def set_signal(self, signal_number: int, value: int) -> None: - """Set the specified digital input or output signal. - - Args: - signal_number: The number of the digital signal to set. - value: The signal value to set. 0 = off, non-zero = on. - """ - await self.send_command(f"sig {signal_number} {value}") + await self._new.set_signal(signal_number, value) async def get_system_state(self) -> int: - """Get the global system state code. + return await self._new.get_system_state() - Returns: - The global system state code. Please see documentation for DataID 234. - """ - response = await self.send_command("sysState") - return int(response) - - async def get_tool_transformation_values(self) -> tuple[float, float, float, float, float, float]: - """Get the current tool transformation values. - - Returns: - A tuple containing (X, Y, Z, yaw, pitch, roll) for the tool transformation. - """ - data = await self.send_command("tool") - # Remove "tool:" prefix if present - if data.startswith("tool: "): - data = data[6:] + async def get_tool_transformation_values(self): + return await self._new.get_tool_transformation_values() - parts = data.split() - if len(parts) != 6: - raise PreciseFlexError(-1, "Unexpected response format from tool command.") - - x, y, z, yaw, pitch, roll = self._parse_xyz_response(parts) - - return (x, y, z, yaw, pitch, roll) - - async def set_tool_transformation_values( - self, x: float, y: float, z: float, yaw: float, pitch: float, roll: float - ) -> None: - """Set the robot tool transformation. - - The robot must be attached to set the tool. Setting the tool pauses any robot motion in progress. - - Args: - x: Tool X coordinate. - y: Tool Y coordinate. - z: Tool Z coordinate. - yaw: Tool yaw rotation. - pitch: Tool pitch rotation. - roll: Tool roll rotation. - """ - await self.send_command(f"tool {x} {y} {z} {yaw} {pitch} {roll}") + async def set_tool_transformation_values(self, x, y, z, yaw, pitch, roll): + await self._new.set_tool_transformation_values(x, y, z, yaw, pitch, roll) async def get_version(self) -> str: - """Get the current version of TCS and any installed plug-ins. - - Returns: - str: The current version information. - """ - return await self.send_command("version") - - # region LOCATION COMMANDS - - async def get_location_angles(self, location_index: int) -> tuple[int, int, Dict[int, float]]: - """Get the angle values for the specified station index. - - Args: - location_index: The station index, from 1 to N_LOC. + return await self._new.get_version() - Returns: - A tuple containing (type_code, station_index, angles_dict) - - Raises: - PreciseFlexError: If attempting to get angles from a Cartesian location. - """ + async def get_location_angles(self, location_index): data = await self.send_command(f"locAngles {location_index}") parts = data.split(" ") - type_code = int(parts[0]) if type_code != 1: - raise PreciseFlexError(-1, "Location is not of angles type.") - + raise _new_module.PreciseFlexError(-1, "Location is not of angles type.") station_index = int(parts[1]) angles = self._parse_angles_response(parts[2:]) - return (type_code, station_index, angles) - async def set_joint_angles( - self, - location_index: int, - joint_position: Dict[int, float], - ) -> None: - """Set joint angles for stored location, handling rail configuration.""" - if self._has_rail: - await self.send_command( - f"locAngles {location_index} " - f"{joint_position[PFAxis.RAIL]} " - f"{joint_position[PFAxis.BASE]} " - f"{joint_position[PFAxis.SHOULDER]} " - f"{joint_position[PFAxis.ELBOW]} " - f"{joint_position[PFAxis.WRIST]} " - f"{joint_position[PFAxis.GRIPPER]}" - ) - else: - await self.send_command( - f"locAngles {location_index} " - f"{joint_position[PFAxis.BASE]} " - f"{joint_position[PFAxis.SHOULDER]} " - f"{joint_position[PFAxis.ELBOW]} " - f"{joint_position[PFAxis.WRIST]} " - f"{joint_position[PFAxis.GRIPPER]}" - ) - - async def get_location_xyz( - self, location_index: int - ) -> tuple[int, int, float, float, float, float, float, float]: - """Get the Cartesian position values for the specified station index. + async def set_joint_angles(self, location_index, joint_position): + await self._new._set_joint_angles(location_index, joint_position) - Args: - location_index: The station index, from 1 to N_LOC. - - Returns: - A tuple containing (type_code, station_index, X, Y, Z, yaw, pitch, roll) - - Raises: - PreciseFlexError: If attempting to get Cartesian position from an angles type location. - """ + async def get_location_xyz(self, location_index): data = await self.send_command(f"locXyz {location_index}") parts = data.split(" ") - type_code = int(parts[0]) if type_code != 0: - raise PreciseFlexError(-1, "Location is not of Cartesian type.") - + raise _new_module.PreciseFlexError(-1, "Location is not of Cartesian type.") if len(parts) != 8: - raise PreciseFlexError(-1, "Unexpected response format from locXyz command.") - + raise _new_module.PreciseFlexError(-1, "Unexpected response format from locXyz command.") station_index = int(parts[1]) x, y, z, yaw, pitch, roll = self._parse_xyz_response(parts[2:8]) - return (type_code, station_index, x, y, z, yaw, pitch, roll) - async def set_location_xyz( - self, - location_index: int, - cartesian_position: PreciseFlexCartesianCoords, - ) -> None: - """Set the Cartesian position values for the specified station index. - - Args: - location_index: The station index, from 1 to N_LOC. - cartesian_position: The Cartesian position to set. - """ - await self.send_command( - f"locXyz {location_index} " - f"{cartesian_position.location.x} " - f"{cartesian_position.location.y} " - f"{cartesian_position.location.z} " - f"{cartesian_position.rotation.yaw} " - f"{cartesian_position.rotation.pitch} " - f"{cartesian_position.rotation.roll}" - ) + async def set_location_xyz(self, location_index, cartesian_position): + await self._new._set_location_xyz(location_index, _to_new_coords(cartesian_position)) - async def get_location_z_clearance(self, location_index: int) -> tuple[int, float, bool]: - """Get the ZClearance and ZWorld properties for the specified location. - - Args: - location_index: The station index, from 1 to N_LOC. - - Returns: - A tuple containing (station_index, z_clearance, z_world) - """ + async def get_location_z_clearance(self, location_index): data = await self.send_command(f"locZClearance {location_index}") parts = data.split(" ") - if len(parts) != 3: - raise PreciseFlexError(-1, "Unexpected response format from locZClearance command.") - + raise _new_module.PreciseFlexError( + -1, "Unexpected response format from locZClearance command." + ) station_index = int(parts[0]) z_clearance = float(parts[1]) - z_world = True if float(parts[2]) != 0 else False - + z_world = float(parts[2]) != 0 return (station_index, z_clearance, z_world) - async def set_location_z_clearance( - self, location_index: int, z_clearance: float, z_world: Optional[bool] = None - ) -> None: - """Set the ZClearance and ZWorld properties for the specified location. - - Args: - location_index: The station index, from 1 to N_LOC. - z_clearance: The new ZClearance property value. - z_world (float, optional): The new ZWorld property value. If omitted, only ZClearance is set. - """ + async def set_location_z_clearance(self, location_index, z_clearance, z_world=None): if z_world is None: await self.send_command(f"locZClearance {location_index} {z_clearance}") else: z_world_int = 1 if z_world else 0 await self.send_command(f"locZClearance {location_index} {z_clearance} {z_world_int}") - async def get_location_config(self, location_index: int) -> tuple[int, int]: - """Get the Config property for the specified location. - - Args: - location_index: The station index, from 1 to N_LOC. - - Returns: - A tuple containing (station_index, config_value) - config_value is a bit mask where: - - 0 = None (no configuration specified) - - 0x01 = GPL_Righty (right shouldered configuration) - - 0x02 = GPL_Lefty (left shouldered configuration) - - 0x04 = GPL_Above (elbow above the wrist) - - 0x08 = GPL_Below (elbow below the wrist) - - 0x10 = GPL_Flip (wrist pitched up) - - 0x20 = GPL_NoFlip (wrist pitched down) - - 0x1000 = GPL_Single (restrict wrist axis to +/- 180 degrees) - Values can be combined using bitwise OR. - """ + async def get_location_config(self, location_index): data = await self.send_command(f"locConfig {location_index}") parts = data.split(" ") - if len(parts) != 2: - raise PreciseFlexError(-1, "Unexpected response format from locConfig command.") + raise _new_module.PreciseFlexError(-1, "Unexpected response format from locConfig command.") + return (int(parts[0]), int(parts[1])) - station_index = int(parts[0]) - config_value = int(parts[1]) - - return (station_index, config_value) - - async def set_location_config(self, location_index: int, config_value: int) -> None: - """Set the Config property for the specified location. - - Args: - location_index: The station index, from 1 to N_LOC. - config_value: The new Config property value as a bit mask where: - - 0 = None (no configuration specified) - - 0x01 = GPL_Righty (right shouldered configuration) - - 0x02 = GPL_Lefty (left shouldered configuration) - - 0x04 = GPL_Above (elbow above the wrist) - - 0x08 = GPL_Below (elbow below the wrist) - - 0x10 = GPL_Flip (wrist pitched up) - - 0x20 = GPL_NoFlip (wrist pitched down) - - 0x1000 = GPL_Single (restrict wrist axis to +/- 180 degrees) - Values can be combined using bitwise OR. - - Raises: - ValueError: If config_value contains invalid bits or conflicting configurations. - """ - # Define valid bit masks - GPL_RIGHTY = 0x01 - GPL_LEFTY = 0x02 - GPL_ABOVE = 0x04 - GPL_BELOW = 0x08 - GPL_FLIP = 0x10 - GPL_NOFLIP = 0x20 - GPL_SINGLE = 0x1000 - - # All valid bits - ALL_VALID_BITS = ( - GPL_RIGHTY | GPL_LEFTY | GPL_ABOVE | GPL_BELOW | GPL_FLIP | GPL_NOFLIP | GPL_SINGLE - ) + async def set_location_config(self, location_index, config_value): + await self._new._set_location_config(location_index, config_value) - # Check for invalid bits - if config_value & ~ALL_VALID_BITS: - raise ValueError(f"Invalid config bits specified: 0x{config_value:X}") + async def dest_c(self, arg1=0): + return await self._new.dest_c(arg1) - # Check for conflicting configurations - if (config_value & GPL_RIGHTY) and (config_value & GPL_LEFTY): - raise ValueError("Cannot specify both GPL_Righty and GPL_Lefty") + async def dest_j(self, arg1=0): + return await self._new.dest_j(arg1) - if (config_value & GPL_ABOVE) and (config_value & GPL_BELOW): - raise ValueError("Cannot specify both GPL_Above and GPL_Below") + async def here_j(self, location_index): + await self._new.here_j(location_index) - if (config_value & GPL_FLIP) and (config_value & GPL_NOFLIP): - raise ValueError("Cannot specify both GPL_Flip and GPL_NoFlip") + async def here_c(self, location_index): + await self._new.here_c(location_index) - await self.send_command(f"locConfig {location_index} {config_value}") + async def get_profile_speed(self, profile_index): + return await self._new.get_profile_speed(profile_index) - async def dest_c(self, arg1: int = 0) -> tuple[float, float, float, float, float, float, int]: - """Get the destination or current Cartesian location of the robot. + async def set_profile_speed(self, profile_index, speed_percent): + await self._new.set_profile_speed(profile_index, speed_percent) - Args: - arg1: Selects return value. Defaults to 0. - 0 = Return current Cartesian location if robot is not moving - 1 = Return target Cartesian location of the previous or current move + async def get_profile_speed2(self, profile_index): + return await self._new.get_profile_speed2(profile_index) - Returns: - A tuple containing (X, Y, Z, yaw, pitch, roll, config) - If arg1 = 1 or robot is moving, returns the target location. - If arg1 = 0 and robot is not moving, returns the current location. - """ - if arg1 == 0: - data = await self.send_command("destC") - else: - data = await self.send_command(f"destC {arg1}") + async def set_profile_speed2(self, profile_index, speed2_percent): + await self._new.set_profile_speed2(profile_index, speed2_percent) - parts = data.split() - if len(parts) != 7: - raise PreciseFlexError(-1, "Unexpected response format from destC command.") + async def get_profile_accel(self, profile_index): + return await self._new.get_profile_accel(profile_index) - x, y, z, yaw, pitch, roll = self._parse_xyz_response(parts[:6]) - config = int(parts[6]) + async def set_profile_accel(self, profile_index, accel_percent): + await self._new.set_profile_accel(profile_index, accel_percent) - return (x, y, z, yaw, pitch, roll, config) + async def get_profile_accel_ramp(self, profile_index): + return await self._new.get_profile_accel_ramp(profile_index) - async def dest_j(self, arg1: int = 0) -> Dict[int, float]: - """Get the destination or current joint location of the robot. + async def set_profile_accel_ramp(self, profile_index, accel_ramp_seconds): + await self._new.set_profile_accel_ramp(profile_index, accel_ramp_seconds) - Args: - arg1: Selects return value. Defaults to 0. - 0 = Return current joint location if robot is not moving - 1 = Return target joint location of the previous or current move + async def get_profile_decel(self, profile_index): + return await self._new.get_profile_decel(profile_index) - Returns: - A dict mapping PFAxis to float values. - If arg1 = 1 or robot is moving, returns the target joint positions. - If arg1 = 0 and robot is not moving, returns the current joint positions. - """ - if arg1 == 0: - data = await self.send_command("destJ") - else: - data = await self.send_command(f"destJ {arg1}") + async def set_profile_decel(self, profile_index, decel_percent): + await self._new.set_profile_decel(profile_index, decel_percent) - parts = data.split() - if not parts: - raise PreciseFlexError(-1, "Unexpected response format from destJ command.") + async def get_profile_decel_ramp(self, profile_index): + return await self._new.get_profile_decel_ramp(profile_index) - return self._parse_angles_response(parts) + async def set_profile_decel_ramp(self, profile_index, decel_ramp_seconds): + await self._new.set_profile_decel_ramp(profile_index, decel_ramp_seconds) - async def here_j(self, location_index: int) -> None: - """Record the current position of the selected robot into the specified Location as angles. - - The Location is automatically set to type "angles". + async def get_profile_in_range(self, profile_index): + return await self._new.get_profile_in_range(profile_index) - Args: - location_index: The station index, from 1 to N_LOC. - """ - await self.send_command(f"hereJ {location_index}") + async def set_profile_in_range(self, profile_index, in_range_value): + await self._new.set_profile_in_range(profile_index, in_range_value) - async def here_c(self, location_index: int) -> None: - """Record the current position of the selected robot into the specified Location as Cartesian. - - The Location object is automatically set to type "Cartesian". - Can be used to change the pallet origin (index 1,1,1) value. - - Args: - location_index: The station index, from 1 to N_LOC. - """ - await self.send_command(f"hereC {location_index}") + async def get_profile_straight(self, profile_index): + return await self._new.get_profile_straight(profile_index) - # region PROFILE COMMANDS - - async def get_profile_speed(self, profile_index: int) -> float: - """Get the speed property of the specified profile. - - Args: - profile_index: The profile index to query. - - Returns: - float: The current speed as a percentage. 100 = full speed. - """ - response = await self.send_command(f"Speed {profile_index}") - profile, speed = response.split() - return float(speed) - - async def set_profile_speed(self, profile_index: int, speed_percent: float) -> None: - """Set the speed property of the specified profile. - - Args: - profile_index: The profile index to modify. - speed_percent: The new speed as a percentage. 100 = full speed. - Values > 100 may be accepted depending on system configuration. - """ - await self.send_command(f"Speed {profile_index} {speed_percent}") - - async def get_profile_speed2(self, profile_index: int) -> float: - """Get the speed2 property of the specified profile. - - Args: - profile_index: The profile index to query. - - Returns: - float: The current speed2 as a percentage. Used for Cartesian moves. - """ - response = await self.send_command(f"Speed2 {profile_index}") - profile, speed2 = response.split() - return float(speed2) - - async def set_profile_speed2(self, profile_index: int, speed2_percent: float) -> None: - """Set the speed2 property of the specified profile. - - Args: - profile_index: The profile index to modify. - speed2_percent: The new speed2 as a percentage. 100 = full speed. - Used for Cartesian moves. Normally set to 0. - """ - await self.send_command(f"Speed2 {profile_index} {speed2_percent}") - - async def get_profile_accel(self, profile_index: int) -> float: - """Get the acceleration property of the specified profile. - - Args: - profile_index: The profile index to query. - - Returns: - float: The current acceleration as a percentage. 100 = maximum acceleration. - """ - response = await self.send_command(f"Accel {profile_index}") - profile, accel = response.split() - return float(accel) - - async def set_profile_accel(self, profile_index: int, accel_percent: float) -> None: - """Set the acceleration property of the specified profile. - - Args: - profile_index: The profile index to modify. - accel_percent: The new acceleration as a percentage. 100 = maximum acceleration. - Maximum value depends on system configuration. - """ - await self.send_command(f"Accel {profile_index} {accel_percent}") - - async def get_profile_accel_ramp(self, profile_index: int) -> float: - """Get the acceleration ramp property of the specified profile. - - Args: - profile_index: The profile index to query. - - Returns: - float: The current acceleration ramp time in seconds. - """ - response = await self.send_command(f"AccRamp {profile_index}") - profile, accel_ramp = response.split() - return float(accel_ramp) - - async def set_profile_accel_ramp(self, profile_index: int, accel_ramp_seconds: float) -> None: - """Set the acceleration ramp property of the specified profile. - - Args: - profile_index: The profile index to modify. - accel_ramp_seconds: The new acceleration ramp time in seconds. - """ - await self.send_command(f"AccRamp {profile_index} {accel_ramp_seconds}") - - async def get_profile_decel(self, profile_index: int) -> float: - """Get the deceleration property of the specified profile. - - Args: - profile_index: The profile index to query. - - Returns: - float: The current deceleration as a percentage. 100 = maximum deceleration. - """ - response = await self.send_command(f"Decel {profile_index}") - profile, decel = response.split() - return float(decel) - - async def set_profile_decel(self, profile_index: int, decel_percent: float) -> None: - """Set the deceleration property of the specified profile. - - Args: - profile_index: The profile index to modify. - decel_percent: The new deceleration as a percentage. 100 = maximum deceleration. - Maximum value depends on system configuration. - """ - await self.send_command(f"Decel {profile_index} {decel_percent}") - - async def get_profile_decel_ramp(self, profile_index: int) -> float: - """Get the deceleration ramp property of the specified profile. - - Args: - profile_index: The profile index to query. - - Returns: - float: The current deceleration ramp time in seconds. - """ - response = await self.send_command(f"DecRamp {profile_index}") - profile, decel_ramp = response.split() - return float(decel_ramp) - - async def set_profile_decel_ramp(self, profile_index: int, decel_ramp_seconds: float) -> None: - """Set the deceleration ramp property of the specified profile. - - Args: - profile_index: The profile index to modify. - decel_ramp_seconds: The new deceleration ramp time in seconds. - """ - await self.send_command(f"DecRamp {profile_index} {decel_ramp_seconds}") - - async def get_profile_in_range(self, profile_index: int) -> float: - """Get the InRange property of the specified profile. - - Args: - profile_index: The profile index to query. - - Returns: - float: The current InRange value (-1 to 100). - -1 = do not stop at end of motion if blending is possible - 0 = always stop but do not check end point error - > 0 = wait until close to end point (larger numbers mean less position error allowed) - """ - response = await self.send_command(f"InRange {profile_index}") - profile, in_range = response.split() - return float(in_range) - - async def set_profile_in_range(self, profile_index: int, in_range_value: float) -> None: - """Set the InRange property of the specified profile. - - Args: - profile_index: The profile index to modify. - in_range_value: The new InRange value from -1 to 100. - -1 = do not stop at end of motion if blending is possible - 0 = always stop but do not check end point error - > 0 = wait until close to end point (larger numbers mean less position error allowed) - - Raises: - ValueError: If in_range_value is not between -1 and 100. - """ - if not (-1 <= in_range_value <= 100): - raise ValueError("InRange value must be between -1 and 100") - await self.send_command(f"InRange {profile_index} {in_range_value}") - - async def get_profile_straight(self, profile_index: int) -> bool: - """Get the Straight property of the specified profile. - - Args: - profile_index: The profile index to query. - - Returns: - The current Straight property value. - True = follow a straight-line path - False = follow a joint-based path (coordinated axes movement) - """ - response = await self.send_command(f"Straight {profile_index}") - profile, straight = response.split() - - return straight == "True" - - async def set_profile_straight(self, profile_index: int, straight_mode: bool) -> None: - """Set the Straight property of the specified profile. - - Args: - profile_index: The profile index to modify. - straight_mode: The path type to use. - True = follow a straight-line path - False = follow a joint-based path (robot axes move in coordinated manner) - - Raises: - ValueError: If straight_mode is not True or False. - """ - straight_int = 1 if straight_mode else 0 - await self.send_command(f"Straight {profile_index} {straight_int}") + async def set_profile_straight(self, profile_index, straight_mode): + await self._new.set_profile_straight(profile_index, straight_mode) async def set_motion_profile_values( self, - profile: int, - speed: float, - speed2: float, - acceleration: float, - deceleration: float, - acceleration_ramp: float, - deceleration_ramp: float, - in_range: float, - straight: bool, + profile, + speed, + speed2, + acceleration, + deceleration, + acceleration_ramp, + deceleration_ramp, + in_range, + straight, ): - """ - Set motion profile values for the specified profile index on the PreciseFlex robot. - - Args: - profile: Profile index to set values for. - speed: Percentage of maximum speed. 100 = full speed. Values >100 may be accepted depending on system config. - speed2: Secondary speed setting, typically for Cartesian moves. Normally 0. Interpreted as a percentage. - acceleration: Percentage of maximum acceleration. 100 = full accel. - deceleration: Percentage of maximum deceleration. 100 = full decel. - acceleration_ramp: Acceleration ramp time in seconds. - deceleration_ramp: Deceleration ramp time in seconds. - in_range: InRange value, from -1 to 100. -1 = allow blending, 0 = stop without checking, >0 = enforce position accuracy. - straight: If True, follow a straight-line path (-1). If False, follow a joint-based path (0). - """ - if not (0 <= speed): - raise ValueError("Speed must be > 0 (percent).") - - if not (0 <= speed2): - raise ValueError("Speed2 must be > 0 (percent).") - - if not (0 <= acceleration <= 100): - raise ValueError("Acceleration must be between 0 and 100 (percent).") - - if not (0 <= deceleration <= 100): - raise ValueError("Deceleration must be between 0 and 100 (percent).") - - if acceleration_ramp < 0: - raise ValueError("Acceleration ramp must be >= 0 (seconds).") - - if deceleration_ramp < 0: - raise ValueError("Deceleration ramp must be >= 0 (seconds).") - - if not (-1 <= in_range <= 100): - raise ValueError("InRange must be between -1 and 100.") - - straight_int = -1 if straight else 0 - await self.send_command( - f"Profile {profile} {speed} {speed2} {acceleration} {deceleration} {acceleration_ramp} {deceleration_ramp} {in_range} {straight_int}" - ) - - async def get_motion_profile_values( - self, profile: int - ) -> tuple[int, float, float, float, float, float, float, float, bool]: - """ - Get the current motion profile values for the specified profile index on the PreciseFlex robot. - - Args: - profile: Profile index to get values for. - - Returns: - A tuple containing (profile, speed, speed2, acceleration, deceleration, acceleration_ramp, deceleration_ramp, in_range, straight) - - profile: Profile index - - speed: Percentage of maximum speed - - speed2: Secondary speed setting - - acceleration: Percentage of maximum acceleration - - deceleration: Percentage of maximum deceleration - - acceleration_ramp: Acceleration ramp time in seconds - - deceleration_ramp: Deceleration ramp time in seconds - - in_range: InRange value (-1 to 100) - - straight: True if straight-line path, False if joint-based path - """ - data = await self.send_command(f"Profile {profile}") - parts = data.split(" ") - if len(parts) != 9: - raise PreciseFlexError(-1, "Unexpected response format from device.") - - return ( - int(parts[0]), - float(parts[1]), - float(parts[2]), - float(parts[3]), - float(parts[4]), - float(parts[5]), - float(parts[6]), - float(parts[7]), - False if int(parts[8]) == 0 else True, + await self._new.set_motion_profile_values( + profile, + speed, + speed2, + acceleration, + deceleration, + acceleration_ramp, + deceleration_ramp, + in_range, + straight, ) - # region MOTION COMMANDS - async def move_to_stored_location(self, location_index: int, profile_index: int) -> None: - """Move to the location specified by the station index using the specified profile. - - Args: - location_index: The index of the location to which the robot moves. - profile_index: The profile index for this move. - - Note: - Requires that the robot be attached. - """ - await self.send_command(f"move {location_index} {profile_index}") + async def get_motion_profile_values(self, profile): + return await self._new.get_motion_profile_values(profile) - async def move_to_stored_location_appro(self, location_index: int, profile_index: int) -> None: - """Approach the location specified by the station index using the specified profile. + async def move_to_stored_location(self, location_index, profile_index): + await self._new._move_to_stored_location(location_index, profile_index) - This is similar to `move_to_stored_location` except that the Z clearance value is included. + async def move_to_stored_location_appro(self, location_index, profile_index): + await self._new._move_to_stored_location_appro(location_index, profile_index) - Args: - location_index: The index of the location to which the robot moves. - profile_index: The profile index for this move. - - Note: - Requires that the robot be attached. - """ - await self.send_command(f"moveAppro {location_index} {profile_index}") - - async def move_extra_axis( - self, axis1_position: float, axis2_position: Optional[float] = None - ) -> None: - """Post a move for one or two extra axes during the next Cartesian motion. - - Does not cause the robot to move at this time. Only some kinematic modules support extra axes. - - Args: - axis1_position: The destination position for the 1st extra axis. - axis2_position (float, optional): The destination position for the 2nd extra axis, if any. - - Note: - Requires that the robot be attached. - """ + async def move_extra_axis(self, axis1_position, axis2_position=None): if axis2_position is None: await self.send_command(f"moveExtraAxis {axis1_position}") else: await self.send_command(f"moveExtraAxis {axis1_position} {axis2_position}") - async def move_one_axis( - self, axis_number: int, destination_position: float, profile_index: int - ) -> None: - """Move a single axis to the specified position using the specified profile. - - Args: - axis_number: The number of the axis to move. - destination_position: The destination position for this axis. - profile_index: The index of the profile to use during this motion. - - Note: - Requires that the robot be attached. - """ + async def move_one_axis(self, axis_number, destination_position, profile_index): await self.send_command(f"moveOneAxis {axis_number} {destination_position} {profile_index}") - async def move_c( - self, - profile_index: int, - cartesian_coords: PreciseFlexCartesianCoords, - ) -> None: - """Move the robot to the Cartesian location specified by the arguments. - - Args: - profile_index: The profile index to use for this motion. - cartesian_coords: The Cartesian coordinates to which the robot should move. - - Note: - Requires that the robot be attached. - """ - - cmd = ( - f"moveC {profile_index} " - f"{cartesian_coords.location.x} " - f"{cartesian_coords.location.y} " - f"{cartesian_coords.location.z} " - f"{cartesian_coords.rotation.yaw} " - f"{cartesian_coords.rotation.pitch} " - f"{cartesian_coords.rotation.roll} " - ) - - if cartesian_coords.orientation is not None: - config_int = self._convert_orientation_enum_to_int(cartesian_coords.orientation) - config_int |= 0x1000 # GPL_Single: restrict wrist to ±180° - cmd += f"{config_int}" - - await self.send_command(cmd) - - async def move_j(self, profile_index: int, joint_coords: Dict[int, float]) -> None: - """Move the robot using joint coordinates, handling rail configuration.""" - if self._has_rail: - angles_str = ( - f"{joint_coords[PFAxis.BASE]} " - f"{joint_coords[PFAxis.SHOULDER]} " - f"{joint_coords[PFAxis.ELBOW]} " - f"{joint_coords[PFAxis.WRIST]} " - f"{joint_coords[PFAxis.GRIPPER]} " - f"{joint_coords[PFAxis.RAIL]} " - ) - else: - angles_str = ( - f"{joint_coords[PFAxis.BASE]} " - f"{joint_coords[PFAxis.SHOULDER]} " - f"{joint_coords[PFAxis.ELBOW]} " - f"{joint_coords[PFAxis.WRIST]} " - f"{joint_coords[PFAxis.GRIPPER]}" - ) - await self.send_command(f"moveJ {profile_index} {angles_str}") - - async def release_brake(self, axis: int) -> None: - """Release the axis brake. - - Overrides the normal operation of the brake. It is important that the brake not be set - while a motion is being performed. This feature is used to lock an axis to prevent - motion or jitter. - - Args: - axis: The number of the axis whose brake should be released. - """ - await self.send_command(f"releaseBrake {axis}") - - async def set_brake(self, axis: int) -> None: - """Set the axis brake. - - Overrides the normal operation of the brake. It is important not to set a brake on an - axis that is moving as it may damage the brake or damage the motor. - - Args: - axis: The number of the axis whose brake should be set. - """ - await self.send_command(f"setBrake {axis}") - - async def state(self) -> str: - """Return state of motion. - - This value indicates the state of the currently executing or last completed robot motion. - For additional information, please see 'Robot.TrajState' in the GPL reference manual. - - Returns: - str: The current motion state. - """ - return await self.send_command("state") + async def move_c(self, profile_index, cartesian_coords): + await self._new._move_c(profile_index, _to_new_coords(cartesian_coords)) - async def wait_for_eom(self) -> None: - """Wait for the robot to reach the end of the current motion. + async def move_j(self, profile_index, joint_coords): + await self._new._move_j(profile_index, joint_coords) - Waits for the robot to reach the end of the current motion or until it is stopped by - some other means. Does not reply until the robot has stopped. - """ - await self.send_command("waitForEom") - await asyncio.sleep(0.2) # Small delay to ensure command is fully processed + async def release_brake(self, axis): + await self._new.release_brake(axis) - async def zero_torque(self, enable: bool, axis_mask: int = 1) -> None: - """Sets or clears zero torque mode for the selected robot. + async def set_brake(self, axis): + await self._new.set_brake(axis) - Individual axes may be placed into zero torque mode while the remaining axes are servoing. + async def state(self): + return await self._new.state() - Args: - enable: If True, enable torque mode for axes specified by axis_mask. If False, disable torque mode for the entire robot. - axis_mask: The bit mask specifying the axes to be placed in torque mode when enable is True. The mask is computed by OR'ing the axis bits: 1 = axis 1, 2 = axis 2, 4 = axis 3, 8 = axis 4, etc. Ignored when enable is False. - """ + async def wait_for_eom(self): + await self._new._wait_for_eom() - if enable: - assert axis_mask > 0, "axis_mask must be greater than 0" - await self.send_command(f"zeroTorque 1 {axis_mask}") - else: - await self.send_command("zeroTorque 0") - - # region PAROBOT COMMANDS - - async def change_config(self, grip_mode: int = 0) -> None: - """Change Robot configuration from Righty to Lefty or vice versa using customizable locations. - - Uses customizable locations to avoid hitting robot during change. - Does not include checks for collision inside work volume of the robot. - Can be customized by user for their work cell configuration. - - Args: - grip_mode: Gripper control mode. - 0 = do not change gripper (default) - 1 = open gripper - 2 = close gripper - """ - await self.send_command(f"ChangeConfig {grip_mode}") - - async def change_config2(self, grip_mode: int = 0) -> None: - """Change Robot configuration from Righty to Lefty or vice versa using algorithm. - - Uses an algorithm to avoid hitting robot during change. - Does not include checks for collision inside work volume of the robot. - Can be customized by user for their work cell configuration. - - Args: - grip_mode: Gripper control mode. - 0 = do not change gripper (default) - 1 = open gripper - 2 = close gripper - """ - await self.send_command(f"ChangeConfig2 {grip_mode}") - - async def get_grasp_data(self) -> tuple[float, float, float]: - """Get the data to be used for the next force-controlled PickPlate command grip operation. - - Returns: - A tuple containing (plate_width_mm, finger_speed_percent, grasp_force) - """ - data = await self.send_command("GraspData") - parts = data.split() + async def zero_torque(self, enable, axis_mask=1): + await self._new.zero_torque(enable, axis_mask) - if len(parts) != 3: - raise PreciseFlexError(-1, "Unexpected response format from GraspData command.") + async def change_config(self, grip_mode=0): + await self._new.change_config(grip_mode) - plate_width = float(parts[0]) - finger_speed = float(parts[1]) - grasp_force = float(parts[2]) + async def change_config2(self, grip_mode=0): + await self._new.change_config2(grip_mode) - return (plate_width, finger_speed, grasp_force) + async def get_grasp_data(self): + return await self._new._get_grasp_data() - async def set_grasp_data( - self, plate_width: float, finger_speed_percent: float, grasp_force: float - ) -> None: - """Set the data to be used for the next force-controlled PickPlate command grip operation. - - This data remains in effect until the next GraspData command or the system is restarted. - - Args: - plate_width_mm: The plate width in mm. - finger_speed_percent: The finger speed during grasp where 100 means 100%. - grasp_force: The gripper squeezing force, in Newtons. - A positive value indicates the fingers must close to grasp. - A negative value indicates the fingers must open to grasp. - """ - await self.send_command(f"GraspData {plate_width} {finger_speed_percent} {grasp_force}") - - async def _get_grip_close_pos(self) -> float: - """Get the gripper close position for the servoed gripper. - - Returns: - float: The current gripper close position. - """ - data = await self.send_command("GripClosePos") - return float(data) + async def set_grasp_data(self, plate_width, finger_speed_percent, grasp_force): + await self._new._set_grasp_data(plate_width, finger_speed_percent, grasp_force) - async def _set_grip_close_pos(self, close_position: float) -> None: - """Set the gripper close position for the servoed gripper. + async def _get_grip_close_pos(self): + return await self._new._get_grip_close_pos() - The close position may be changed by a force-controlled grip operation. + async def _set_grip_close_pos(self, close_position): + await self._new._set_grip_close_pos(close_position) - Args: - close_position: The new gripper close position. - """ - await self.send_command(f"GripClosePos {close_position}") + async def _get_grip_open_pos(self): + return await self._new._get_grip_open_pos() - async def _get_grip_open_pos(self) -> float: - """Get the gripper open position for the servoed gripper. + async def _set_grip_open_pos(self, open_position): + await self._new._set_grip_open_pos(open_position) - Returns: - float: The current gripper open position. - """ - data = await self.send_command("GripOpenPos") - return float(data) - - async def _set_grip_open_pos(self, open_position: float) -> None: - """Set the gripper open position for the servoed gripper. - - Args: - open_position: The new gripper open position. - """ - await self.send_command(f"GripOpenPos {open_position}") - - async def move_rail( - self, station_id: Optional[int] = None, mode: int = 0, rail_destination: Optional[float] = None - ) -> None: - """Moves the optional linear rail. - - The rail may be moved immediately or simultaneously with the next pick or place motion. - The location may be associated with the station or specified explicitly. - - Args: - station_id: The destination station ID. Only used if rail_destination is omitted. - mode: Mode of operation. - 0 or omitted = cancel any pending MoveRail - 1 = Move rail immediately - 2 = Move rail during next pick or place - rail_destination (float, optional): If specified, use this value as the rail destination - rather than the station location. - """ + async def move_rail(self, station_id=None, mode=0, rail_destination=None): if rail_destination is not None: await self.send_command(f"MoveRail {station_id or ''} {mode} {rail_destination}") elif station_id is not None: @@ -1735,102 +538,39 @@ async def move_rail( else: await self.send_command(f"MoveRail {mode}") - async def get_pallet_index(self, station_id: int) -> tuple[int, int, int, int]: - """Get the current pallet index values for the specified station. - - Args: - station_id: Station ID, from 1 to N_LOC. - - Returns: - A tuple containing (station_id, pallet_index_x, pallet_index_y, pallet_index_z) - """ + async def get_pallet_index(self, station_id): data = await self.send_command(f"PalletIndex {station_id}") parts = data.split() - if len(parts) != 4: - raise PreciseFlexError(-1, "Unexpected response format from PalletIndex command.") - - station_id = int(parts[0]) - pallet_index_x = int(parts[1]) - pallet_index_y = int(parts[2]) - pallet_index_z = int(parts[3]) - - return (station_id, pallet_index_x, pallet_index_y, pallet_index_z) + raise _new_module.PreciseFlexError(-1, "Unexpected response format from PalletIndex command.") + return (int(parts[0]), int(parts[1]), int(parts[2]), int(parts[3])) async def set_pallet_index( - self, station_id: int, pallet_index_x: int = 0, pallet_index_y: int = 0, pallet_index_z: int = 0 - ) -> None: - """Set the pallet index value from 1 to n of the station used by subsequent pick or place. - - If an index argument is 0 or omitted, the corresponding index is not changed. - Negative values generate an error. - - Args: - station_id: Station ID, from 1 to N_LOC. - pallet_index_x: Pallet index X. If 0 or omitted, X index is not changed. - pallet_index_y: Pallet index Y. If 0 or omitted, Y index is not changed. - pallet_index_z: Pallet index Z. If 0 or omitted, Z index is not changed. - - Raises: - ValueError: If any index value is negative. - """ - if pallet_index_x < 0: - raise ValueError("Pallet index X cannot be negative") - if pallet_index_y < 0: - raise ValueError("Pallet index Y cannot be negative") - if pallet_index_z < 0: - raise ValueError("Pallet index Z cannot be negative") - + self, station_id, pallet_index_x=0, pallet_index_y=0, pallet_index_z=0 + ): await self.send_command( f"PalletIndex {station_id} {pallet_index_x} {pallet_index_y} {pallet_index_z}" ) - async def get_pallet_origin( - self, station_id: int - ) -> tuple[int, float, float, float, float, float, float, int]: - """Get the current pallet origin data for the specified station. - - Args: - station_id: Station ID, from 1 to N_LOC. - - Returns: - A tuple containing (station_id, x, y, z, yaw, pitch, roll, config) - """ + async def get_pallet_origin(self, station_id): data = await self.send_command(f"PalletOrigin {station_id}") parts = data.split() - if len(parts) != 8: - raise PreciseFlexError(-1, "Unexpected response format from PalletOrigin command.") - - station_id = int(parts[0]) - x = float(parts[1]) - y = float(parts[2]) - z = float(parts[3]) - yaw = float(parts[4]) - pitch = float(parts[5]) - roll = float(parts[6]) - config = int(parts[7]) - - return (station_id, x, y, z, yaw, pitch, roll, config) - - async def set_pallet_origin( - self, - station_id: int, - cartesian_coords: PreciseFlexCartesianCoords, - ) -> None: - """Define the origin of a pallet reference frame. - - Specifies the world location and orientation of the (1,1,1) pallet position. - Must be followed by a PalletX command. - - The orientation and configuration specified here determines the world orientation - of the robot during all pick or place operations using this pallet. - - Args: - station_id: Station ID, from 1 to N_LOC. - cartesian_coords: The Cartesian coordinates defining the pallet origin. - """ + raise _new_module.PreciseFlexError( + -1, "Unexpected response format from PalletOrigin command." + ) + return ( + int(parts[0]), + float(parts[1]), + float(parts[2]), + float(parts[3]), + float(parts[4]), + float(parts[5]), + float(parts[6]), + int(parts[7]), + ) + async def set_pallet_origin(self, station_id, cartesian_coords): cmd = ( f"PalletOrigin {station_id} " f"{cartesian_coords.location.x} " @@ -1840,162 +580,50 @@ async def set_pallet_origin( f"{cartesian_coords.rotation.pitch} " f"{cartesian_coords.rotation.roll} " ) - if cartesian_coords.orientation is not None: config_int = self._convert_orientation_enum_to_int(cartesian_coords.orientation) cmd += f"{config_int}" - await self.send_command(cmd) - async def get_pallet_x(self, station_id: int) -> tuple[int, int, float, float, float]: - """Get the current pallet X data for the specified station. - - Args: - station_id: Station ID, from 1 to N_LOC. - - Returns: - A tuple containing (station_id, x_position_count, world_x, world_y, world_z) - """ + async def get_pallet_x(self, station_id): data = await self.send_command(f"PalletX {station_id}") parts = data.split() - if len(parts) != 5: - raise PreciseFlexError(-1, "Unexpected response format from PalletX command.") + raise _new_module.PreciseFlexError(-1, "Unexpected response format from PalletX command.") + return (int(parts[0]), int(parts[1]), float(parts[2]), float(parts[3]), float(parts[4])) - station_id = int(parts[0]) - x_position_count = int(parts[1]) - world_x = float(parts[2]) - world_y = float(parts[3]) - world_z = float(parts[4]) - - return (station_id, x_position_count, world_x, world_y, world_z) - - async def set_pallet_x( - self, station_id: int, x_position_count: int, world_x: float, world_y: float, world_z: float - ) -> None: - """Define the last point on the pallet X axis. - - Specifies the world location of the (n,1,1) pallet position, where n is the x_position_count value. - Must follow a PalletOrigin command. - - Args: - station_id: Station ID, from 1 to N_LOC. - x_position_count: X position count. - world_x: World location X coordinate. - world_y: World location Y coordinate. - world_z: World location Z coordinate. - """ + async def set_pallet_x(self, station_id, x_position_count, world_x, world_y, world_z): await self.send_command( f"PalletX {station_id} {x_position_count} {world_x} {world_y} {world_z}" ) - async def get_pallet_y(self, station_id: int) -> tuple[int, int, float, float, float]: - """Get the current pallet Y data for the specified station. - - Args: - station_id: Station ID, from 1 to N_LOC. - - Returns: - A tuple containing (station_id, y_position_count, world_x, world_y, world_z) - """ + async def get_pallet_y(self, station_id): data = await self.send_command(f"PalletY {station_id}") parts = data.split() - if len(parts) != 5: - raise PreciseFlexError(-1, "Unexpected response format from PalletY command.") - - station_id = int(parts[0]) - y_position_count = int(parts[1]) - world_x = float(parts[2]) - world_y = float(parts[3]) - world_z = float(parts[4]) - - return (station_id, y_position_count, world_x, world_y, world_z) + raise _new_module.PreciseFlexError(-1, "Unexpected response format from PalletY command.") + return (int(parts[0]), int(parts[1]), float(parts[2]), float(parts[3]), float(parts[4])) - async def set_pallet_y( - self, station_id: int, y_position_count: int, world_x: float, world_y: float, world_z: float - ) -> None: - """Define the last point on the pallet Y axis. - - Specifies the world location of the (1,n,1) pallet position, where n is the y_position_count value. - If this command is executed, a 2 or 3-dimensional pallet is assumed. - Must follow a PalletX command. - - Args: - station_id: Station ID, from 1 to N_LOC. - y_position_count: Y position count. - world_x: World location X coordinate. - world_y: World location Y coordinate. - world_z: World location Z coordinate. - """ + async def set_pallet_y(self, station_id, y_position_count, world_x, world_y, world_z): await self.send_command( f"PalletY {station_id} {y_position_count} {world_x} {world_y} {world_z}" ) - async def get_pallet_z(self, station_id: int) -> tuple[int, int, float, float, float]: - """Get the current pallet Z data for the specified station. - - Args: - station_id: Station ID, from 1 to N_LOC. - - Returns: - A tuple containing (station_id, z_position_count, world_x, world_y, world_z) - """ + async def get_pallet_z(self, station_id): data = await self.send_command(f"PalletZ {station_id}") parts = data.split() - if len(parts) != 5: - raise PreciseFlexError(-1, "Unexpected response format from PalletZ command.") - - station_id = int(parts[0]) - z_position_count = int(parts[1]) - world_x = float(parts[2]) - world_y = float(parts[3]) - world_z = float(parts[4]) + raise _new_module.PreciseFlexError(-1, "Unexpected response format from PalletZ command.") + return (int(parts[0]), int(parts[1]), float(parts[2]), float(parts[3]), float(parts[4])) - return (station_id, z_position_count, world_x, world_y, world_z) - - async def set_pallet_z( - self, station_id: int, z_position_count: int, world_x: float, world_y: float, world_z: float - ) -> None: - """Define the last point on the pallet Z axis. - - Specifies the world location of the (1,1,n) pallet position, where n is the z_position_count value. - If this command is executed, a 3-dimensional pallet is assumed. - Must follow a PalletX and PalletY command. - - Args: - station_id: Station ID, from 1 to N_LOC. - z_position_count: Z position count. - world_x: World location X coordinate. - world_y: World location Y coordinate. - world_z: World location Z coordinate. - """ + async def set_pallet_z(self, station_id, z_position_count, world_x, world_y, world_z): await self.send_command( f"PalletZ {station_id} {z_position_count} {world_x} {world_y} {world_z}" ) async def pick_plate_station( - self, - station_id: int, - horizontal_compliance: bool = False, - horizontal_compliance_torque: int = 0, - ) -> bool: - """Moves to a predefined position or pallet location and picks up plate. - - If the arm must change configuration, it automatically goes through the Park position. - At the conclusion of this routine, the arm is left gripping the plate and stopped at the nest approach position. - Use Teach function to teach station pick point. - - Args: - station_id: Station ID, from 1 to Max. - horizontal_compliance: If True, enable horizontal compliance while closing the gripper to allow centering around the plate. - horizontal_compliance_torque: The % of the original horizontal holding torque to be retained during compliance. If omitted, 0 is used. - - Returns: - bool: True if the plate was successfully grasped or force control was not used. - False if the force-controlled gripper detected no plate present. - """ + self, station_id, horizontal_compliance=False, horizontal_compliance_torque=0 + ): horizontal_compliance_int = 1 if horizontal_compliance else 0 ret_code = await self.send_command( f"PickPlate {station_id} {horizontal_compliance_int} {horizontal_compliance_torque}" @@ -2003,359 +631,92 @@ async def pick_plate_station( return ret_code != "0" async def place_plate_station( - self, - station_id: int, - horizontal_compliance: bool = False, - horizontal_compliance_torque: int = 0, - ) -> None: - """Moves to a predefined position or pallet location and places a plate. - - If the arm must change configuration, it automatically goes through the Park position. - At the conclusion of this routine, the arm is left gripping the plate and stopped at the nest approach position. - Use Teach function to teach station place point. - - Args: - station_id: Station ID, from 1 to Max. - horizontal_compliance: If True, enable horizontal compliance during the move to place the plate, to allow centering in the fixture. - horizontal_compliance_torque: The % of the original horizontal holding torque to be retained during compliance. If omitted, 0 is used. - """ + self, station_id, horizontal_compliance=False, horizontal_compliance_torque=0 + ): horizontal_compliance_int = 1 if horizontal_compliance else 0 await self.send_command( f"PlacePlate {station_id} {horizontal_compliance_int} {horizontal_compliance_torque}" ) - async def get_rail_position(self, station_id: int) -> float: - """Get the position of the optional rail axis that is associated with a station. - - Args: - station_id: Station ID, from 1 to Max. - - Returns: - float: The current rail position for the specified station. - """ + async def get_rail_position(self, station_id): data = await self.send_command(f"Rail {station_id}") return float(data) - async def set_rail_position(self, station_id: int, rail_position: float) -> None: - """Set the position of the optional rail axis that is associated with a station. - - The station rail data is loaded and saved by the LoadFile and StoreFile commands. - - Args: - station_id: Station ID, from 1 to Max. - rail_position: The new rail position. - """ + async def set_rail_position(self, station_id, rail_position): await self.send_command(f"Rail {station_id} {rail_position}") - async def teach_plate_station(self, station_id: int, z_clearance: float = 50.0) -> None: - """Sets the plate location to the current robot position and configuration. - - The location is saved as Cartesian coordinates. Z clearance must be high enough to withdraw the gripper. - If this station is a pallet, the pallet indices must be set to 1, 1, 1. The pallet frame is not changed, - only the location relative to the pallet. - - Args: - station_id: Station ID, from 1 to Max. - z_clearance: The Z Clearance value. If omitted, a value of 50 is used. If specified and non-zero, this value is used. - """ + async def teach_plate_station(self, station_id, z_clearance=50.0): await self.send_command(f"TeachPlate {station_id} {z_clearance}") - async def get_station_type(self, station_id: int) -> tuple[int, int, int, float, float, float]: - """Get the station configuration for the specified station ID. - - Args: - station_id: Station ID, from 1 to Max. - - Returns: - A tuple containing (station_id, access_type, location_type, z_clearance, z_above, z_grasp_offset) - - station_id: The station ID - - access_type: 0 = horizontal, 1 = vertical - - location_type: 0 = normal single, 1 = pallet (1D, 2D, 3D) - - z_clearance: ZClearance value in mm - - z_above: ZAbove value in mm - - z_grasp_offset: ZGrasp offset - """ + async def get_station_type(self, station_id): data = await self.send_command(f"StationType {station_id}") parts = data.split() - if len(parts) != 6: - raise PreciseFlexError(-1, "Unexpected response format from StationType command.") - - station_id = int(parts[0]) - access_type = int(parts[1]) - location_type = int(parts[2]) - z_clearance = float(parts[3]) - z_above = float(parts[4]) - z_grasp_offset = float(parts[5]) - - return (station_id, access_type, location_type, z_clearance, z_above, z_grasp_offset) + raise _new_module.PreciseFlexError(-1, "Unexpected response format from StationType command.") + return ( + int(parts[0]), + int(parts[1]), + int(parts[2]), + float(parts[3]), + float(parts[4]), + float(parts[5]), + ) async def set_station_type( - self, - station_id: int, - access_type: int, - location_type: int, - z_clearance: float, - z_above: float, - z_grasp_offset: float, - ) -> None: - """Set the station configuration for the specified station ID. - - Args: - station_id: Station ID, from 1 to Max. - access_type: The station access type. - 0 = horizontal (for "hotel" carriers accessed by horizontal move) - 1 = vertical (for stacks or tube racks accessed with vertical motion) - location_type: The location type. - 0 = normal single location - 1 = pallet (1D, 2D, or 3D regular arrays requiring column, row, and layer index) - z_clearance: ZClearance value in mm. The horizontal or vertical distance - from the final location used when approaching or departing from a station. - z_above: ZAbove value in mm. The vertical offset used with horizontal - access when approaching or departing from the location. - z_grasp_offset: ZGrasp offset. Added to ZClearance when an object is - being held to compensate for the part in the gripper. - - Raises: - ValueError: If access_type or location_type are not valid values. - """ - if access_type not in [0, 1]: - raise ValueError("Access type must be 0 (horizontal) or 1 (vertical)") - - if location_type not in [0, 1]: - raise ValueError("Location type must be 0 (normal single) or 1 (pallet)") - + self, station_id, access_type, location_type, z_clearance, z_above, z_grasp_offset + ): await self.send_command( f"StationType {station_id} {access_type} {location_type} {z_clearance} {z_above} {z_grasp_offset}" ) - # region SSGRIP COMMANDS - - async def home_all_if_no_plate(self) -> int: - """Tests if the gripper is holding a plate. If not, enable robot power and home all robots. - - Returns: - -1 if no plate detected and the command succeeded, 0 if a plate was detected. - """ + async def home_all_if_no_plate(self): response = await self.send_command("HomeAll_IfNoPlate") return int(response) - async def _grasp_plate( - self, plate_width_mm: float, finger_speed_percent: int, grasp_force: float - ) -> int: - """Grasps a plate with limited force. - - Low level method. Use `pick_plate` instead for typical pick-and-place operations. - - A plate can be grasped by opening or closing the gripper. The actual commanded gripper - width generated by this function is a few mm smaller (or larger) than plate_width_mm - to permit the servos PID loop to generate the gripping force. - - Args: - plate_width_mm: Plate width in mm. Should be accurate to within about 1 mm. - finger_speed_percent: Percent speed to close fingers. 1 to 100. - grasp_force: Maximum gripper squeeze force in Newtons. - A positive value indicates the fingers must close to grasp. - A negative value indicates the fingers must open to grasp. - - Returns: - -1 if the plate has been grasped, 0 if the final gripping force indicates no plate. - - Raises: - ValueError: If finger_speed_percent is not between 1 and 100. - """ - if not (1 <= finger_speed_percent <= 100): - raise ValueError("Finger speed percent must be between 1 and 100") - + async def _grasp_plate(self, plate_width_mm, finger_speed_percent, grasp_force): response = await self.send_command( f"GraspPlate {plate_width_mm} {finger_speed_percent} {grasp_force}" ) return int(response) - async def _release_plate( - self, open_width_mm: float, finger_speed_percent: int, in_range: float = 0.0 - ) -> None: - """Releases the plate after a GraspPlate command. - - Low level method. Use `place_plate` instead for typical pick-and-place operations. - - Opens (or closes) the gripper to the specified width and cancels the force limit - once the plate is released to avoid applying an excessive force to the plate. - - Args: - open_width_mm: Open width in mm. - finger_speed_percent: Percent speed to open fingers. 1 to 100. - in_range: Optional. The standard InRange profile property for the gripper open move. - If omitted, a zero value is assumed. - - Raises: - ValueError: If finger_speed_percent is not between 1 and 100. - """ - if not (1 <= finger_speed_percent <= 100): - raise ValueError("Finger speed percent must be between 1 and 100") - + async def _release_plate(self, open_width_mm, finger_speed_percent, in_range=0.0): await self.send_command(f"ReleasePlate {open_width_mm} {finger_speed_percent} {in_range}") - async def is_gripper_closed(self) -> bool: - """(Single Gripper Only) Tests if the gripper is fully closed by checking the end-of-travel sensor. - - Returns: - For standard gripper: True if the gripper is within 2mm of fully closed, otherwise False. - """ - if self._is_dual_gripper: - raise ValueError("IsGripperClosed command is only valid for single gripper robots.") - response = await self.send_command("IsFullyClosed") - return int(response) == -1 - - async def are_grippers_closed(self) -> tuple[bool, bool]: - """(Dual Gripper Only) Tests if each gripper is fully closed by checking the end-of-travel sensors.""" - if not self._is_dual_gripper: - raise ValueError("AreGrippersClosed command is only valid for dual gripper robots.") - response = await self.send_command("IsFullyClosed") - ret_int = int(response) - gripper_1_closed = (ret_int & 1) != 0 - gripper_2_closed = (ret_int & 2) != 0 - return (gripper_1_closed, gripper_2_closed) - - async def set_active_gripper( - self, gripper_id: int, spin_mode: int = 0, profile_index: Optional[int] = None - ) -> None: - """(Dual Gripper Only) Sets the currently active gripper and modifies the tool reference frame. - - Args: - gripper_id: Gripper ID, either 1 or 2. Determines which gripper is set to active. - spin_mode: Optional spin mode. - 0 or omitted = do not rotate the gripper 180deg immediately. - 1 = Rotate gripper 180deg immediately. - profile_index: Profile Index to use for spin motion. - - Raises: - ValueError: If gripper_id is not 1 or 2, or if spin_mode is not 0 or 1. - """ - if gripper_id not in [1, 2]: - raise ValueError("Gripper ID must be 1 or 2") - - if spin_mode not in [0, 1]: - raise ValueError("Spin mode must be 0 or 1") - + async def set_active_gripper(self, gripper_id, spin_mode=0, profile_index=None): if profile_index is not None: await self.send_command(f"SetActiveGripper {gripper_id} {spin_mode} {profile_index}") else: await self.send_command(f"SetActiveGripper {gripper_id} {spin_mode}") - async def get_active_gripper(self) -> int: - """(Dual Gripper Only) Returns the currently active gripper. - - Returns: - 1 if Gripper A is active, 2 if Gripper B is active. - """ - if not self._is_dual_gripper: - raise ValueError("GetActiveGripper command is only valid for dual gripper robots.") + async def get_active_gripper(self): response = await self.send_command("GetActiveGripper") return int(response) - async def freedrive_mode(self, free_axes: List[int]) -> None: - """Enter freedrive mode, allowing manual movement of the specified joints. - - The robot must be attached to enter free mode. - - Args: - free_axes: List of joint indices to free. Use [0] for all axes. - """ - for axis in free_axes: - await self.send_command(f"freemode {axis}") - - async def end_freedrive_mode(self) -> None: - """Exit freedrive mode for all axes.""" - await self.send_command("freemode -1") - async def pick_plate_from_stored_position( - self, - position_id: int, - horizontal_compliance: bool = False, - horizontal_compliance_torque: int = 0, + self, position_id, horizontal_compliance=False, horizontal_compliance_torque=0 ): - """Pick an item at the specified position ID. - - Args: - position_id: The ID of the position where the plate should be picked. - horizontal_compliance: enable horizontal compliance while closing the gripper to allow centering around the plate. - horizontal_compliance_torque: The % of the original horizontal holding torque to be retained during compliance. If omitted, 0 is used. - """ horizontal_compliance_int = 1 if horizontal_compliance else 0 ret_code = await self.send_command( f"pickplate {position_id} {horizontal_compliance_int} {horizontal_compliance_torque}" ) if ret_code == "0": - raise PreciseFlexError(-1, "the force-controlled gripper detected no plate present.") + raise _new_module.PreciseFlexError( + -1, "the force-controlled gripper detected no plate present." + ) async def place_plate_to_stored_position( - self, - position_id: int, - horizontal_compliance: bool = False, - horizontal_compliance_torque: int = 0, + self, position_id, horizontal_compliance=False, horizontal_compliance_torque=0 ): - """Place an item at the specified position ID. - - Args: - position_id: The ID of the position where the plate should be placed. - horizontal_compliance: enable horizontal compliance during the move to place the plate, to allow centering in the fixture. - horizontal_compliance_torque: The % of the original horizontal holding torque to be retained during compliance. If omitted, 0 is used. - """ horizontal_compliance_int = 1 if horizontal_compliance else 0 await self.send_command( f"placeplate {position_id} {horizontal_compliance_int} {horizontal_compliance_torque}" ) - async def teach_position(self, position_id: int, z_clearance: float = 50.0): - """Sets the plate location to the current robot position and configuration. The location is saved as Cartesian coordinates. - - Args: - position_id: The ID of the position to be taught. - z_clearance: Optional. The Z Clearance value. If omitted, a value of 50 is used. Z clearance must be high enough to withdraw the gripper. - """ + async def teach_position(self, position_id, z_clearance=50.0): await self.send_command(f"teachplate {position_id} {z_clearance}") - def _parse_xyz_response( - self, parts: List[str] - ) -> tuple[float, float, float, float, float, float]: - if len(parts) != 6: - raise PreciseFlexError(-1, "Unexpected response format for Cartesian coordinates.") - - x = float(parts[0]) - y = float(parts[1]) - z = float(parts[2]) - yaw = float(parts[3]) - pitch = float(parts[4]) - roll = float(parts[5]) - - return (x, y, z, yaw, pitch, roll) - - def _parse_angles_response(self, parts: List[str]) -> Dict[int, float]: - """Parse angle values from a response string. - - For self._has_rail=True: wire order is [base, shoulder, elbow, wrist, gripper, rail] - For self._has_rail=False: wire order is [base, shoulder, elbow, wrist, gripper] - """ - - if len(parts) < 3: - raise PreciseFlexError(-1, "Unexpected response format for angles.") - - if self._has_rail: - return { - PFAxis.RAIL: float(parts[5]) if len(parts) > 5 else 0.0, - PFAxis.BASE: float(parts[0]), - PFAxis.SHOULDER: float(parts[1]), - PFAxis.ELBOW: float(parts[2]), - PFAxis.WRIST: float(parts[3]) if len(parts) > 3 else 0.0, - PFAxis.GRIPPER: float(parts[4]) if len(parts) > 4 else 0.0, - } - - return { - PFAxis.RAIL: 0.0, - PFAxis.BASE: float(parts[0]), - PFAxis.SHOULDER: float(parts[1]), - PFAxis.ELBOW: float(parts[2]) if len(parts) > 2 else 0.0, - PFAxis.WRIST: float(parts[3]) if len(parts) > 3 else 0.0, - PFAxis.GRIPPER: float(parts[4]) if len(parts) > 4 else 0.0, - } + def _parse_xyz_response(self, parts): + return self._new._parse_xyz_response(parts) + + def _parse_angles_response(self, parts): + return self._new._parse_angles_response(parts) diff --git a/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py index 3e479f03f9a..f6b318b290e 100644 --- a/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py @@ -1346,6 +1346,12 @@ def __init__( self._iswap_version: Optional[str] = None # loaded lazily + from pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAP + from pylabrobot.hamilton.liquid_handlers.star.core import CoreGripper + + self._new_iswap = iSWAP(interface=self) + self._new_core = CoreGripper(interface=self) + self._default_1d_symbology: Barcode1DSymbology = "Code 128 (Subset B and C)" self._setup_done = False @@ -1482,10 +1488,8 @@ def core_parked(self) -> bool: return self._core_parked is True async def get_iswap_version(self) -> str: - """Lazily load the iSWAP version. Use cached value if available.""" - if self._iswap_version is None: - self._iswap_version = await self.request_iswap_version() - return self._iswap_version + """Legacy. Use self._new_iswap.version instead.""" + return self._new_iswap.version async def request_pip_channel_version(self, channel: int) -> str: return cast( @@ -4054,45 +4058,27 @@ async def core_pick_up_resource( y_gripping_speed: float = 5.0, front_channel: int = 7, ): - """Pick up resource with CoRe gripper tool - Low level component of :meth:`move_resource` - - Args: - resource: Resource to pick up. - offset: Offset from resource position in mm. - pickup_distance_from_top: Distance from top of resource to pick up. - minimum_traverse_height_at_beginning_of_a_command: Minimum traverse height at beginning of a - command [mm] (refers to all channels independent of tip pattern parameter 'tm'). Must be - between 0 and 360. - grip_strength: Grip strength (0 = weak, 99 = strong). Must be between 0 and 99. Default 15. - z_speed: Z speed [mm/s]. Must be between 0.4 and 128.7. Default 50.0. - y_gripping_speed: Y gripping speed [mm/s]. Must be between 0 and 370.0. Default 5.0. - front_channel: Channel 1. Must be between 1 and self._num_channels - 1. Default 7. - """ + """Legacy. Use CoreGripper.pick_up_at_location instead.""" + from pylabrobot.hamilton.liquid_handlers.star.core import CoreGripper - # Get center of source plate. Also gripping height and plate width. center = resource.get_location_wrt(self.deck, x="c", y="c", z="b") + offset grip_height = center.z + resource.get_absolute_size_z() - pickup_distance_from_top - grip_width = resource.get_absolute_size_y() # grip width is y size of resource + grip_width = resource.get_absolute_size_y() if self.core_parked: await self.pick_up_core_gripper_tools(front_channel=front_channel) - await self.core_get_plate( - x_position=round(center.x * 10), - x_direction=0, - y_position=round(center.y * 10), - y_gripping_speed=round(y_gripping_speed * 10), - z_position=round(grip_height * 10), - z_speed=round(z_speed * 10), - open_gripper_position=round(grip_width * 10) + 30, - plate_width=round(grip_width * 10) - 30, - grip_strength=grip_strength, - minimum_traverse_height_at_beginning_of_a_command=round( - (minimum_traverse_height_at_beginning_of_a_command or self._iswap_traversal_height) * 10 - ), - minimum_z_position_at_the_command_end=round( - (minimum_z_position_at_the_command_end or self._iswap_traversal_height) * 10 + await self._new_core.pick_up_at_location( + location=Coordinate(center.x, center.y, grip_height), + resource_width=grip_width, + backend_params=CoreGripper.PickUpParams( + grip_strength=grip_strength, + y_gripping_speed=y_gripping_speed, + z_speed=z_speed, + minimum_traverse_height=( + minimum_traverse_height_at_beginning_of_a_command or self._iswap_traversal_height + ), + z_position_at_end=(minimum_z_position_at_the_command_end or self._iswap_traversal_height), ), ) @@ -4103,29 +4089,17 @@ async def core_move_picked_up_resource( acceleration_index: int = 4, z_speed: float = 50.0, ): - """After a resource is picked up, move it to a new location but don't release it yet. - Low level component of :meth:`move_resource` - - Args: - location: Location to move to. - minimum_traverse_height_at_beginning_of_a_command: Minimum traverse height at beginning of a - command [0.1mm] (refers to all channels independent of tip pattern parameter 'tm'). Must be - between 0 and 3600. Default 3600. - acceleration_index: Acceleration index (0 = 0.1 mm/s2, 1 = 0.2 mm/s2, 2 = 0.5 mm/s2, - 3 = 1.0 mm/s2, 4 = 2.0 mm/s2, 5 = 5.0 mm/s2, 6 = 10.0 mm/s2, 7 = 20.0 mm/s2). Must be - between 0 and 7. Default 4. - z_speed: Z speed [0.1mm/s]. Must be between 3 and 1600. Default 500. - """ - - await self.core_move_plate_to_position( - x_position=round(center.x * 10), - x_direction=0, - x_acceleration_index=acceleration_index, - y_position=round(center.y * 10), - z_position=round(center.z * 10), - z_speed=round(z_speed * 10), - minimum_traverse_height_at_beginning_of_a_command=round( - (minimum_traverse_height_at_beginning_of_a_command or self._iswap_traversal_height) * 10 + """Legacy. Use CoreGripper.move_to_location instead.""" + from pylabrobot.hamilton.liquid_handlers.star.core import CoreGripper + + await self._new_core.move_to_location( + location=center, + backend_params=CoreGripper.MoveToLocationParams( + acceleration_index=acceleration_index, + z_speed=z_speed, + minimum_traverse_height=( + minimum_traverse_height_at_beginning_of_a_command or self._iswap_traversal_height + ), ), ) @@ -4138,42 +4112,26 @@ async def core_release_picked_up_resource( z_position_at_the_command_end: Optional[float] = None, return_tool: bool = True, ): - """Place resource with CoRe gripper tool - Low level component of :meth:`move_resource` - - Args: - resource: Location to place. - pickup_distance_from_top: Distance from top of resource to place. - offset: Offset from resource position in mm. - minimum_traverse_height_at_beginning_of_a_command: Minimum traverse height at beginning of a - command [mm] (refers to all channels independent of tip pattern parameter 'tm'). Must be - between 0 and 360.0. - z_position_at_the_command_end: Minimum z-Position at end of a command [mm] (refers to all - channels independent of tip pattern parameter 'tm'). Must be between 0 and 360.0 - return_tool: Return tool to wasteblock mount after placing. Default True. - """ + """Legacy. Use CoreGripper.drop_at_location instead.""" + from pylabrobot.hamilton.liquid_handlers.star.core import CoreGripper - # Get center of destination location. Also gripping height and plate width. grip_height = location.z + resource.get_absolute_size_z() - pickup_distance_from_top grip_width = resource.get_absolute_size_y() - await self.core_put_plate( - x_position=round(location.x * 10), - x_direction=0, - y_position=round(location.y * 10), - z_position=round(grip_height * 10), - z_press_on_distance=0, - z_speed=500, - open_gripper_position=round(grip_width * 10) + 30, - minimum_traverse_height_at_beginning_of_a_command=round( - (minimum_traverse_height_at_beginning_of_a_command or self._iswap_traversal_height) * 10 - ), - z_position_at_the_command_end=round( - (z_position_at_the_command_end or self._iswap_traversal_height) * 10 + await self._new_core.drop_at_location( + location=Coordinate(location.x, location.y, grip_height), + resource_width=grip_width, + backend_params=CoreGripper.DropParams( + minimum_traverse_height=( + minimum_traverse_height_at_beginning_of_a_command or self._iswap_traversal_height + ), + z_position_at_end=(z_position_at_the_command_end or self._iswap_traversal_height), ), - return_tool=return_tool, ) + if return_tool: + await self.return_core_gripper_tools() + async def pick_up_resource( self, pickup: ResourcePickup, @@ -6290,13 +6248,12 @@ async def pick_up_core_gripper_tools( front_offset: Optional[Coordinate] = None, back_offset: Optional[Coordinate] = None, ): - """Get CoRe gripper tool from wasteblock mount.""" + """Legacy. Tool management will be migrated in a future commit.""" if not 0 < front_channel < self.num_channels: raise ValueError(f"front_channel must be between 1 and {self.num_channels - 1} (inclusive)") back_channel = front_channel - 1 - # Only enforce x equality if both offsets are explicitly provided. if front_offset is not None and back_offset is not None and front_offset.x != back_offset.x: raise ValueError("front_offset.x and back_offset.x must be the same") @@ -6311,25 +6268,24 @@ async def pick_up_core_gripper_tools( if front_offset is not None and back_offset is not None and front_offset.z != back_offset.z: raise ValueError("front_offset.z and back_offset.z must be the same") z_offset = 0 if front_offset is None else front_offset.z + begin_z_coord = round(235.0 + self.core_adjustment.z + z_offset) end_z_coord = round(225.0 + self.core_adjustment.z + z_offset) - - command_output = await self.send_command( + await self.send_command( module="C0", command="ZT", xs=f"{round(xs * 10):05}", xd="0", ya=f"{round(back_channel_y_center * 10):04}", yb=f"{round(front_channel_y_center * 10):04}", - pa=f"{back_channel + 1:02}", # star is 1-indexed - pb=f"{front_channel + 1:02}", # star is 1-indexed + pa=f"{back_channel + 1:02}", + pb=f"{front_channel + 1:02}", tp=f"{round(begin_z_coord * 10):04}", tz=f"{round(end_z_coord * 10):04}", th=round(self._iswap_traversal_height * 10), tt="14", ) self._core_parked = False - return command_output async def put_core(self): warnings.warn("Deprecated. Use return_core_gripper_tools instead.", DeprecationWarning) @@ -6341,9 +6297,8 @@ async def return_core_gripper_tools( front_offset: Optional[Coordinate] = None, back_offset: Optional[Coordinate] = None, ): - """Put CoRe gripper tool at wasteblock mount.""" + """Legacy. Tool management will be migrated in a future commit.""" - # Only enforce x equality if both offsets are explicitly provided. if front_offset is not None and back_offset is not None and back_offset.x != front_offset.x: raise ValueError("back_offset.x and front_offset.x must be the same") @@ -6361,7 +6316,7 @@ async def return_core_gripper_tools( begin_z_coord = round(215.0 + self.core_adjustment.z + z_offset) end_z_coord = round(205.0 + self.core_adjustment.z + z_offset) - command_output = await self.send_command( + await self.send_command( module="C0", command="ZS", xs=f"{round(xs * 10):05}", @@ -6374,11 +6329,10 @@ async def return_core_gripper_tools( te=round(self._iswap_traversal_height * 10), ) self._core_parked = True - return command_output async def core_open_gripper(self): - """Open CoRe gripper tool.""" - return await self.send_command(module="C0", command="ZO") + """Legacy. Use CoreGripper.open_gripper instead.""" + await self._new_core.open_gripper(gripper_width=0) @need_iswap_parked async def core_get_plate( @@ -6395,7 +6349,7 @@ async def core_get_plate( minimum_traverse_height_at_beginning_of_a_command: int = 2750, minimum_z_position_at_the_command_end: int = 2750, ): - """Get plate with CoRe gripper tool from wasteblock mount.""" + """Legacy. Use CoreGripper.pick_up_at_location instead.""" assert 0 <= x_position <= 30000, "x_position must be between 0 and 30000" assert 0 <= x_direction <= 1, "x_direction must be between 0 and 1" @@ -6445,7 +6399,7 @@ async def core_put_plate( z_position_at_the_command_end: int = 2750, return_tool: bool = True, ): - """Put plate with CoRe gripper tool and return to wasteblock mount.""" + """Legacy. Use CoreGripper.drop_at_location instead.""" assert 0 <= x_position <= 30000, "x_position must be between 0 and 30000" assert 0 <= x_direction <= 1, "x_direction must be between 0 and 1" @@ -6491,7 +6445,7 @@ async def core_move_plate_to_position( z_speed: int = 500, minimum_traverse_height_at_beginning_of_a_command: int = 3600, ): - """Move a plate with CoRe gripper tool.""" + """Legacy. Use CoreGripper.move_to_location instead.""" command_output = await self.send_command( module="C0", @@ -9454,20 +9408,10 @@ async def open_not_initialized_gripper(self): return await self.send_command(module="C0", command="GI") async def iswap_open_gripper(self, open_position: Optional[float] = None): - """Open gripper - - Args: - open_position: Open position [mm] (0.1 mm = 16 increments) The gripper moves to pos + 20. - Must be between 0 and 9999. Default 1320 for iSWAP 4.0 (landscape). Default to - 910 for iSWAP 3 (portrait). - """ - + """Legacy. Use self._new_iswap.open_gripper instead.""" if open_position is None: - open_position = 91.0 if (await self.get_iswap_version()).startswith("3") else 132.0 - - assert 0 <= open_position <= 999.9, "open_position must be between 0 and 999.9" - - return await self.send_command(module="C0", command="GF", go=f"{round(open_position * 10):04}") + open_position = 91.0 if self._new_iswap.version.startswith("3") else 132.0 + return await self._new_iswap.open_gripper(gripper_width=open_position) async def iswap_close_gripper( self, @@ -9475,26 +9419,15 @@ async def iswap_close_gripper( plate_width: float = 0, plate_width_tolerance: float = 0, ): - """Close gripper - - The gripper should be at the position plate_width+plate_width_tolerance+2.0mm before sending this command. - - Args: - grip_strength: Grip strength. 0 = low . 9 = high. Default 5. - plate_width: Plate width [mm] (gb should be > min. Pos. + stop ramp + gt -> gb > 760 + 5 + g ) - plate_width_tolerance: Plate width tolerance [mm]. Must be between 0 and 9.9. Default 2.0. - """ - - assert 0 <= grip_strength <= 9, "grip_strength must be between 0 and 9" - assert 0 <= plate_width <= 999.9, "plate_width must be between 0 and 999.9" - assert 0 <= plate_width_tolerance <= 9.9, "plate_width_tolerance must be between 0 and 9.9" - - return await self.send_command( - module="C0", - command="GC", - gw=grip_strength, - gb=f"{round(plate_width * 10):04}", - gt=f"{round(plate_width_tolerance * 10):02}", + """Legacy. Use self._new_iswap.close_gripper instead.""" + from pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAP + + return await self._new_iswap.close_gripper( + gripper_width=plate_width, + backend_params=iSWAP.CloseGripperParams( + grip_strength=grip_strength, + plate_width_tolerance=plate_width_tolerance, + ), ) # -------------- 3.17.2 Stack handling commands CP -------------- @@ -9546,82 +9479,35 @@ async def iswap_get_plate( acceleration_index_low_acc: int = 1, iswap_fold_up_sequence_at_the_end_of_process: bool = False, ): - """Get plate using iswap. - - Args: - x_position: Plate center in X direction [0.1mm]. Must be between 0 and 30000. Default 0. - x_direction: X-direction. 0 = positive 1 = negative. Must be between 0 and 1. Default 0. - y_position: Plate center in Y direction [0.1mm]. Must be between 0 and 6500. Default 0. - y_direction: Y-direction. 0 = positive 1 = negative. Must be between 0 and 1. Default 0. - z_position: Plate gripping height in Z direction. Must be between 0 and 3600. Default 0. - z_direction: Z-direction. 0 = positive 1 = negative. Must be between 0 and 1. Default 0. - grip_direction: Grip direction. 1 = negative Y, 2 = positive X, 3 = positive Y, - 4 =negative X. Must be between 1 and 4. Default 1. - minimum_traverse_height_at_beginning_of_a_command: Minimum traverse height at beginning of - a command 0.1mm]. Must be between 0 and 3600. Default 3600. - z_position_at_the_command_end: Z-Position at the command end [0.1mm]. Must be between 0 - and 3600. Default 3600. - grip_strength: Grip strength 0 = low .. 9 = high. Must be between 1 and 9. Default 5. - open_gripper_position: Open gripper position [0.1mm]. Must be between 0 and 9999. - Default 860. - plate_width: plate width [0.1mm]. Must be between 0 and 9999. Default 860. - plate_width_tolerance: plate width tolerance [0.1mm]. Must be between 0 and 99. Default 860. - collision_control_level: collision control level 1 = high 0 = low. Must be between 0 and 1. - Default 1. - acceleration_index_high_acc: acceleration index high acc. Must be between 0 and 4. Default 4. - acceleration_index_low_acc: acceleration index high acc. Must be between 0 and 4. Default 1. - iswap_fold_up_sequence_at_the_end_of_process: fold up sequence at the end of process. Default False. - """ - - assert 0 <= x_position <= 30000, "x_position must be between 0 and 30000" - assert 0 <= x_direction <= 1, "x_direction must be between 0 and 1" - assert 0 <= y_position <= 6500, "y_position must be between 0 and 6500" - assert 0 <= y_direction <= 1, "y_direction must be between 0 and 1" - assert 0 <= z_position <= 3600, "z_position must be between 0 and 3600" - assert 0 <= z_direction <= 1, "z_direction must be between 0 and 1" - assert 1 <= grip_direction <= 4, "grip_direction must be between 1 and 4" - assert 0 <= minimum_traverse_height_at_beginning_of_a_command <= 3600, ( - "minimum_traverse_height_at_beginning_of_a_command must be between 0 and 3600" - ) - assert 0 <= z_position_at_the_command_end <= 3600, ( - "z_position_at_the_command_end must be between 0 and 3600" - ) - assert 1 <= grip_strength <= 9, "grip_strength must be between 1 and 9" - assert 0 <= open_gripper_position <= 9999, "open_gripper_position must be between 0 and 9999" - assert 0 <= plate_width <= 9999, "plate_width must be between 0 and 9999" - assert 0 <= plate_width_tolerance <= 99, "plate_width_tolerance must be between 0 and 99" - assert 0 <= collision_control_level <= 1, "collision_control_level must be between 0 and 1" - assert 0 <= acceleration_index_high_acc <= 4, ( - "acceleration_index_high_acc must be between 0 and 4" - ) - assert 0 <= acceleration_index_low_acc <= 4, ( - "acceleration_index_low_acc must be between 0 and 4" - ) - - command_output = await self.send_command( - module="C0", - command="PP", - xs=f"{x_position:05}", - xd=x_direction, - yj=f"{y_position:04}", - yd=y_direction, - zj=f"{z_position:04}", - zd=z_direction, - gr=grip_direction, - th=f"{minimum_traverse_height_at_beginning_of_a_command:04}", - te=f"{z_position_at_the_command_end:04}", - gw=grip_strength, - go=f"{open_gripper_position:04}", - gb=f"{plate_width:04}", - gt=f"{plate_width_tolerance:02}", - ga=collision_control_level, - # xe=f"{acceleration_index_high_acc} {acceleration_index_low_acc}", - gc=iswap_fold_up_sequence_at_the_end_of_process, + """Legacy. Use self._new_iswap.pick_up_at_location instead.""" + from pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAP + + # Convert 0.1mm increments + direction flags to signed mm + x_mm = x_position / 10.0 * (-1 if x_direction else 1) + y_mm = y_position / 10.0 * (-1 if y_direction else 1) + z_mm = z_position / 10.0 * (-1 if z_direction else 1) + # Convert grip_direction 1-4 to degrees: 1→0, 2→90, 3→180, 4→270 + direction_deg = (grip_direction - 1) * 90.0 + # plate_width arrives pre-adjusted (-33 in 0.1mm units) from legacy callers. + # The new iSWAP backend applies that adjustment internally, so we reverse it here. + raw_plate_width_mm = (plate_width + 33) / 10.0 + result = await self._new_iswap.pick_up_at_location( + location=Coordinate(x_mm, y_mm, z_mm), + direction=direction_deg, + resource_width=raw_plate_width_mm, + backend_params=iSWAP.PickUpParams( + minimum_traverse_height=minimum_traverse_height_at_beginning_of_a_command / 10.0, + z_position_at_end=z_position_at_the_command_end / 10.0, + grip_strength=grip_strength, + plate_width_tolerance=plate_width_tolerance / 10.0, + collision_control_level=collision_control_level, + acceleration_index_high_acc=acceleration_index_high_acc, + acceleration_index_low_acc=acceleration_index_low_acc, + fold_up_at_end=iswap_fold_up_sequence_at_the_end_of_process, + ), ) - - # Once the command has completed successfully, set _iswap_parked to false self._iswap_parked = False - return command_output + return result async def iswap_put_plate( self, @@ -9640,75 +9526,31 @@ async def iswap_put_plate( acceleration_index_low_acc: int = 1, iswap_fold_up_sequence_at_the_end_of_process: bool = False, ): - """put plate - - Args: - x_position: Plate center in X direction [0.1mm]. Must be between 0 and 30000. Default 0. - x_direction: X-direction. 0 = positive 1 = negative. Must be between 0 and 1. Default 0. - y_position: Plate center in Y direction [0.1mm]. Must be between 0 and 6500. Default 0. - y_direction: Y-direction. 0 = positive 1 = negative. Must be between 0 and 1. Default 0. - z_position: Plate gripping height in Z direction. Must be between 0 and 3600. Default 0. - z_direction: Z-direction. 0 = positive 1 = negative. Must be between 0 and 1. Default 0. - grip_direction: Grip direction. 1 = negative Y, 2 = positive X, 3 = positive Y, 4 = negative - X. Must be between 1 and 4. Default 1. - minimum_traverse_height_at_beginning_of_a_command: Minimum traverse height at beginning of a - command 0.1mm]. Must be between 0 and 3600. Default 3600. - z_position_at_the_command_end: Z-Position at the command end [0.1mm]. Must be between 0 and - 3600. Default 3600. - open_gripper_position: Open gripper position [0.1mm]. Must be between 0 and 9999. Default - 860. - collision_control_level: collision control level 1 = high 0 = low. Must be between 0 and 1. - Default 1. - acceleration_index_high_acc: acceleration index high acc. Must be between 0 and 4. - Default 4. - acceleration_index_low_acc: acceleration index high acc. Must be between 0 and 4. - Default 1. - iswap_fold_up_sequence_at_the_end_of_process: fold up sequence at the end of process. Default False. - """ - - assert 0 <= x_position <= 30000, "x_position must be between 0 and 30000" - assert 0 <= x_direction <= 1, "x_direction must be between 0 and 1" - assert 0 <= y_position <= 6500, "y_position must be between 0 and 6500" - assert 0 <= y_direction <= 1, "y_direction must be between 0 and 1" - assert 0 <= z_position <= 3600, "z_position must be between 0 and 3600" - assert 0 <= z_direction <= 1, "z_direction must be between 0 and 1" - assert 1 <= grip_direction <= 4, "grip_direction must be between 1 and 4" - assert 0 <= minimum_traverse_height_at_beginning_of_a_command <= 3600, ( - "minimum_traverse_height_at_beginning_of_a_command must be between 0 and 3600" - ) - assert 0 <= z_position_at_the_command_end <= 3600, ( - "z_position_at_the_command_end must be between 0 and 3600" - ) - assert 0 <= open_gripper_position <= 9999, "open_gripper_position must be between 0 and 9999" - assert 0 <= collision_control_level <= 1, "collision_control_level must be between 0 and 1" - assert 0 <= acceleration_index_high_acc <= 4, ( - "acceleration_index_high_acc must be between 0 and 4" - ) - assert 0 <= acceleration_index_low_acc <= 4, ( - "acceleration_index_low_acc must be between 0 and 4" - ) - - command_output = await self.send_command( - module="C0", - command="PR", - xs=f"{x_position:05}", - xd=x_direction, - yj=f"{y_position:04}", - yd=y_direction, - zj=f"{z_position:04}", - zd=z_direction, - th=f"{minimum_traverse_height_at_beginning_of_a_command:04}", - te=f"{z_position_at_the_command_end:04}", - gr=grip_direction, - go=f"{open_gripper_position:04}", - ga=collision_control_level, - # xe=f"{acceleration_index_high_acc} {acceleration_index_low_acc}" - gc=iswap_fold_up_sequence_at_the_end_of_process, + """Legacy. Use self._new_iswap.drop_at_location instead.""" + from pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAP + + x_mm = x_position / 10.0 * (-1 if x_direction else 1) + y_mm = y_position / 10.0 * (-1 if y_direction else 1) + z_mm = z_position / 10.0 * (-1 if z_direction else 1) + direction_deg = (grip_direction - 1) * 90.0 + # open_gripper_position is now computed as resource_width + 3.0 inside the new backend. + # Legacy callers pass open_gripper_position = (plate_width + 3) * 10, so derive resource_width. + resource_width_mm = open_gripper_position / 10.0 - 3.0 + result = await self._new_iswap.drop_at_location( + location=Coordinate(x_mm, y_mm, z_mm), + direction=direction_deg, + resource_width=resource_width_mm, + backend_params=iSWAP.DropParams( + minimum_traverse_height=minimum_traverse_height_at_beginning_of_a_command / 10.0, + z_position_at_end=z_position_at_the_command_end / 10.0, + collision_control_level=collision_control_level, + acceleration_index_high_acc=acceleration_index_high_acc, + acceleration_index_low_acc=acceleration_index_low_acc, + fold_up_at_end=iswap_fold_up_sequence_at_the_end_of_process, + ), ) - - # Once the command has completed successfully, set _iswap_parked to false self._iswap_parked = False - return command_output + return result async def request_iswap_rotation_drive_position_increments(self) -> int: """Query the iSWAP rotation drive position (units: increments) from the firmware.""" @@ -10101,14 +9943,8 @@ async def request_iswap_in_parking_position(self): return await self.send_command(module="C0", command="RG", fmt="rg#") async def request_plate_in_iswap(self) -> bool: - """Request plate in iSWAP - - Returns: - True if holding a plate, False otherwise. - """ - - resp = await self.send_command(module="C0", command="QP", fmt="ph#") - return resp is not None and resp["ph"] == 1 + """Legacy. Use self._new_iswap.is_gripper_closed instead.""" + return await self._new_iswap.is_gripper_closed() async def request_iswap_position(self) -> Coordinate: """Request iSWAP position ( grip center ) @@ -10149,7 +9985,7 @@ async def request_iswap_initialization_status(self) -> bool: async def request_iswap_version(self) -> str: """Firmware command for getting iswap version""" - return cast(str, (await self.send_command("R0", "RF", fmt="rf" + "&" * 15))["rf"]) + return self._new_iswap.version # -------------- 3.18 Cover and port control --------------