diff --git a/docs/user_guide/01_material-handling/arms/c_scara/precise-flex-pf400/hello-world.ipynb b/docs/user_guide/01_material-handling/arms/c_scara/precise-flex-pf400/hello-world.ipynb index 60297290c6e..faac246e281 100644 --- a/docs/user_guide/01_material-handling/arms/c_scara/precise-flex-pf400/hello-world.ipynb +++ b/docs/user_guide/01_material-handling/arms/c_scara/precise-flex-pf400/hello-world.ipynb @@ -32,7 +32,7 @@ "from pylabrobot.arms.precise_flex.pf_400 import PreciseFlex400Backend\n", "\n", "from pylabrobot.arms.precise_flex.coords import PreciseFlexCartesianCoords\n", - "from pylabrobot.arms.precise_flex.joints import PreciseFlexJointCoords\n", + "from pylabrobot.arms.precise_flex.joints import PFAxis\n", "from pylabrobot.resources import Coordinate, Rotation" ] }, @@ -144,14 +144,14 @@ "metadata": {}, "outputs": [], "source": [ - "location = PreciseFlexJointCoords(\n", - " base=99.981,\n", - " shoulder=-36.206,\n", - " elbow=83.063,\n", - " wrist=-331.7,\n", - " gripper=126.084,\n", - " rail=0.0\n", - ")\n", + "location = {\n", + " PFAxis.BASE: 99.981,\n", + " PFAxis.SHOULDER: -36.206,\n", + " PFAxis.ELBOW: 83.063,\n", + " PFAxis.WRIST: -331.7,\n", + " PFAxis.GRIPPER: 126.084,\n", + " PFAxis.RAIL: 0.0,\n", + "}\n", "await arm.move_to(location)" ] }, diff --git a/pylabrobot/arms/backend.py b/pylabrobot/arms/backend.py index 6be521095ac..ccf8e437a0e 100644 --- a/pylabrobot/arms/backend.py +++ b/pylabrobot/arms/backend.py @@ -1,9 +1,8 @@ from abc import ABCMeta, abstractmethod from dataclasses import dataclass -from typing import Optional, Union +from typing import Dict, Optional, Union from pylabrobot.arms.precise_flex.coords import PreciseFlexCartesianCoords -from pylabrobot.arms.standard import JointCoords from pylabrobot.machines.backend import MachineBackend @@ -78,20 +77,20 @@ async def move_to_safe(self) -> None: @abstractmethod async def approach( self, - position: Union[PreciseFlexCartesianCoords, JointCoords], + position: Union[PreciseFlexCartesianCoords, Dict[int, float]], access: Optional[AccessPattern] = None, ) -> None: """Move the arm to an approach position (offset from target). Args: - position: Target position (CartesianCoords or JointCoords) + position: Target position (CartesianCoords or joint position dict) access: Access pattern defining how to approach the target. Defaults to VerticalAccess() if not specified. """ @abstractmethod async def pick_up_resource( self, - position: Union[PreciseFlexCartesianCoords, JointCoords], + position: Union[PreciseFlexCartesianCoords, Dict[int, float]], plate_width: float, access: Optional[AccessPattern] = None, ) -> None: @@ -105,7 +104,7 @@ async def pick_up_resource( @abstractmethod async def drop_resource( self, - position: Union[PreciseFlexCartesianCoords, JointCoords], + position: Union[PreciseFlexCartesianCoords, Dict[int, float]], access: Optional[AccessPattern] = None, ) -> None: """Place a plate at the specified position. @@ -116,11 +115,11 @@ async def drop_resource( """ @abstractmethod - async def move_to(self, position: Union[PreciseFlexCartesianCoords, JointCoords]) -> None: + async def move_to(self, position: Union[PreciseFlexCartesianCoords, Dict[int, float]]) -> None: """Move the arm to a specified position in 3D space or in joint space.""" @abstractmethod - async def get_joint_position(self) -> JointCoords: + async def get_joint_position(self) -> Dict[int, float]: """Get the current position of the arm in joint space.""" @abstractmethod diff --git a/pylabrobot/arms/precise_flex/joints.py b/pylabrobot/arms/precise_flex/joints.py index d8704e94c7d..6fd40344a7e 100644 --- a/pylabrobot/arms/precise_flex/joints.py +++ b/pylabrobot/arms/precise_flex/joints.py @@ -1,24 +1,10 @@ -from dataclasses import dataclass -from typing import Iterator +from enum import IntEnum -@dataclass -class PreciseFlexJointCoords: - base: float - shoulder: float - elbow: float - wrist: float - gripper: float - rail: float = 0 - - def __iter__(self) -> Iterator[float]: - return iter( - [ - self.rail, - self.base, - self.shoulder, - self.elbow, - self.wrist, - self.gripper, - ] - ) +class PFAxis(IntEnum): + BASE = 1 + SHOULDER = 2 + ELBOW = 3 + WRIST = 4 + GRIPPER = 5 + RAIL = 6 diff --git a/pylabrobot/arms/precise_flex/precise_flex_backend.py b/pylabrobot/arms/precise_flex/precise_flex_backend.py index e80c43c1a68..da0b08da464 100644 --- a/pylabrobot/arms/precise_flex/precise_flex_backend.py +++ b/pylabrobot/arms/precise_flex/precise_flex_backend.py @@ -1,7 +1,7 @@ import asyncio import warnings from abc import ABC -from typing import Dict, Iterable, List, Literal, Optional, Union +from typing import Dict, List, Literal, Optional, Union from pylabrobot.arms.backend import ( AccessPattern, @@ -11,7 +11,7 @@ ) from pylabrobot.arms.precise_flex.coords import ElbowOrientation, PreciseFlexCartesianCoords from pylabrobot.arms.precise_flex.error_codes import ERROR_CODES -from pylabrobot.arms.precise_flex.joints import PreciseFlexJointCoords +from pylabrobot.arms.precise_flex.joints import PFAxis from pylabrobot.io.socket import Socket from pylabrobot.resources import Coordinate, Rotation @@ -59,30 +59,6 @@ def __init__( "Dual gripper support is experimental and may not work as expected.", UserWarning ) - def _convert_to_joint_space(self, position: Iterable[float]) -> PreciseFlexJointCoords: - """Convert joint list to PreciseFlexJointCoords. - - Args: - position: List of 6 floats (always padded to 6). position[0] must be 0.0 if robot has no rail. - """ - - position = list(position) - - if len(position) < 6: - raise ValueError("Position must have 6 joint angles for robot with rail.") - - if not self._has_rail and position[0] != 0.0: - raise ValueError("Position[0] (rail) must be 0.0 for robot without rail.") - - return PreciseFlexJointCoords( - rail=position[0], - base=position[1], - shoulder=position[2], - elbow=position[3], - wrist=position[4], - gripper=position[5], - ) - def _convert_to_cartesian_space( self, position: tuple[float, float, float, float, float, float, Optional[ElbowOrientation]] ) -> PreciseFlexCartesianCoords: @@ -224,13 +200,13 @@ async def power_off_robot(self): async def approach( self, - position: Union[PreciseFlexCartesianCoords, Iterable[float]], + 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 Iterable[float]) + position: Target position (CartesianCoords or Dict[int, float]) access: Access pattern defining how to approach the target. Defaults to VerticalAccess() if not specified. Example: @@ -250,17 +226,16 @@ async def approach( if access is None: access = VerticalAccess() - if isinstance(position, list): - joint_position = self._convert_to_joint_space(position) - await self._approach_j(joint_position, access) + 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 Iterable[float] or CartesianCoords.") + raise TypeError("Position must be of type Dict[int, float] or CartesianCoords.") async def pick_up_resource( self, - position: Union[PreciseFlexCartesianCoords, Iterable[float]], + position: Union[PreciseFlexCartesianCoords, Dict[int, float]], plate_width: float, access: Optional[AccessPattern] = None, finger_speed_percent: float = 50.0, @@ -269,7 +244,7 @@ async def pick_up_resource( """Pick a plate from the specified position. Args: - position: Target position for pickup (CartesianCoords only, PreciseFlexJointCoords not supported) + 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) @@ -306,21 +281,20 @@ async def pick_up_resource( if isinstance(position, PreciseFlexCartesianCoords): await self._pick_plate_c(cartesian_position=position, access=access) - elif isinstance(position, list): - joint_position = self._convert_to_joint_space(position) - await self._pick_plate_j(joint_position, access) + elif isinstance(position, dict): + await self._pick_plate_j(position, access) else: - raise TypeError("Position must be of type Iterable[float] or CartesianCoords.") + raise TypeError("Position must be of type Dict[int, float] or CartesianCoords.") async def drop_resource( self, - position: Union[PreciseFlexCartesianCoords, Iterable[float]], + 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, PreciseFlexJointCoords not supported) + 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: @@ -350,33 +324,25 @@ async def drop_resource( 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, Iterable[float]]): + 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 6-element list [rail, base, shoulder, elbow, wrist, gripper] + 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. """ - if isinstance(position, list): - if len(position) < 6: - raise ValueError( - "Joint list must have 6 elements: [rail, base, shoulder, elbow, wrist, gripper]" - ) - joint_coords = PreciseFlexJointCoords( - rail=position[0], - base=position[1], - shoulder=position[2], - elbow=position[3], - wrist=position[4], - gripper=position[5], - ) + 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) else: - raise TypeError("Position must be of type Iterable[float] or CartesianCoords.") + raise TypeError("Position must be of type Dict[int, float] or CartesianCoords.") - async def get_joint_position(self) -> PreciseFlexJointCoords: - """Get the current position of the arm in 3D space.""" + async def get_joint_position(self) -> Dict[int, float]: + """Get the current joint position of the arm.""" await self.wait_for_eom() @@ -389,8 +355,7 @@ async def get_joint_position(self) -> PreciseFlexJointCoords: else: raise PreciseFlexError(-1, "Unexpected response format from wherej command.") - axes = list(self._parse_angles_response(parts)) - return self._convert_to_joint_space(axes) + return self._parse_angles_response(parts) async def get_cartesian_position(self) -> PreciseFlexCartesianCoords: """Get the current position of the arm in 3D space.""" @@ -444,7 +409,7 @@ def _parse_reply_ensure_successful(self, reply: bytes) -> str: return data - async def _approach_j(self, joint_position: PreciseFlexJointCoords, access: AccessPattern): + 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: @@ -455,7 +420,7 @@ async def _approach_j(self, joint_position: PreciseFlexJointCoords, access: Acce 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: PreciseFlexJointCoords, access: AccessPattern): + 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) @@ -463,7 +428,7 @@ async def _pick_plate_j(self, joint_position: PreciseFlexJointCoords, access: Ac self.location_index, self.horizontal_compliance, self.horizontal_compliance_torque ) - async def _place_plate_j(self, joint_position: PreciseFlexJointCoords, access: AccessPattern): + 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) @@ -891,16 +856,14 @@ async def get_version(self) -> str: # region LOCATION COMMANDS - async def get_location_angles( - self, location_index: int - ) -> tuple[int, int, float, float, float, float, float, float]: + 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. Returns: - A tuple containing (type_code, station_index, angle1, angle2, angle3, angle4, angle5, angle6) + A tuple containing (type_code, station_index, angles_dict) Raises: PreciseFlexError: If attempting to get angles from a Cartesian location. @@ -913,35 +876,34 @@ async def get_location_angles( raise PreciseFlexError(-1, "Location is not of angles type.") station_index = int(parts[1]) - angle1, angle2, angle3, angle4, angle5, angle6 = self._parse_angles_response(parts[2:]) + angles = self._parse_angles_response(parts[2:]) - return (type_code, station_index, angle1, angle2, angle3, angle4, angle5, angle6) + return (type_code, station_index, angles) async def set_joint_angles( self, location_index: int, - joint_position: PreciseFlexJointCoords, + 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.rail} " - f"{joint_position.base} " - f"{joint_position.shoulder} " - f"{joint_position.elbow} " - f"{joint_position.wrist} " - f"{joint_position.gripper}" + 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: - # Exclude rail for robots without rail await self.send_command( f"locAngles {location_index} " - f"{joint_position.base} " - f"{joint_position.shoulder} " - f"{joint_position.elbow} " - f"{joint_position.wrist} " - f"{joint_position.gripper}" + 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( @@ -1137,7 +1099,7 @@ async def dest_c(self, arg1: int = 0) -> tuple[float, float, float, float, float return (x, y, z, yaw, pitch, roll, config) - async def dest_j(self, arg1: int = 0) -> tuple[float, float, float, float, float, float]: + async def dest_j(self, arg1: int = 0) -> Dict[int, float]: """Get the destination or current joint location of the robot. Args: @@ -1146,7 +1108,7 @@ async def dest_j(self, arg1: int = 0) -> tuple[float, float, float, float, float 1 = Return target joint location of the previous or current move Returns: - A list containing [axis1, axis2, ..., axisn] + 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. """ @@ -1159,10 +1121,7 @@ async def dest_j(self, arg1: int = 0) -> tuple[float, float, float, float, float if not parts: raise PreciseFlexError(-1, "Unexpected response format from destJ command.") - # Ensure we have exactly 6 elements, padding with 0.0 if necessary - angle1, angle2, angle3, angle4, angle5, angle6 = self._parse_angles_response(parts) - - return (angle1, angle2, angle3, angle4, angle5, angle6) + 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. @@ -1570,25 +1529,24 @@ async def move_c( await self.send_command(cmd) - async def move_j(self, profile_index: int, joint_coords: PreciseFlexJointCoords) -> None: + 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.base} " - f"{joint_coords.shoulder} " - f"{joint_coords.elbow} " - f"{joint_coords.wrist} " - f"{joint_coords.gripper} " - f"{joint_coords.rail} " + 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: - # Exclude rail for robots without rail angles_str = ( - f"{joint_coords.base} " - f"{joint_coords.shoulder} " - f"{joint_coords.elbow} " - f"{joint_coords.wrist} " - f"{joint_coords.gripper}" + 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}") @@ -2383,32 +2341,31 @@ def _parse_xyz_response( return (x, y, z, yaw, pitch, roll) - def _parse_angles_response( - self, parts: List[str] - ) -> tuple[float, float, float, float, float, float]: - """ - For self._has_rail=True: [rail, base, shoulder, elbow, wrist, gripper] - For self._has_rail=False: [base, shoulder, elbow, wrist, gripper, 0.0(padding)] + 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 ( - float(parts[5]) if len(parts) > 5 else 0.0, - float(parts[0]), - float(parts[1]), - float(parts[2]), - float(parts[3]) if len(parts) > 3 else 0.0, - float(parts[4]) if len(parts) > 4 else 0.0, - ) - - return ( - 0.0, - float(parts[0]), - float(parts[1]), - float(parts[2]) if len(parts) > 2 else 0.0, - float(parts[3]) if len(parts) > 3 else 0.0, - float(parts[4]) if len(parts) > 4 else 0.0, - ) + 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, + } diff --git a/pylabrobot/arms/precise_flex/precise_flex_backend_tests.py b/pylabrobot/arms/precise_flex/precise_flex_backend_tests.py index ce1e1c21a4b..7f73e633f78 100644 --- a/pylabrobot/arms/precise_flex/precise_flex_backend_tests.py +++ b/pylabrobot/arms/precise_flex/precise_flex_backend_tests.py @@ -1,9 +1,10 @@ import unittest +from typing import Dict from unittest.mock import AsyncMock, patch from pylabrobot.arms.backend import HorizontalAccess, VerticalAccess from pylabrobot.arms.precise_flex.coords import ElbowOrientation, PreciseFlexCartesianCoords -from pylabrobot.arms.precise_flex.joints import PreciseFlexJointCoords +from pylabrobot.arms.precise_flex.joints import PFAxis from pylabrobot.arms.precise_flex.precise_flex_backend import PreciseFlexBackend, PreciseFlexError from pylabrobot.io.socket import Socket # Import Socket for mocking from pylabrobot.resources import Coordinate, Rotation @@ -56,30 +57,6 @@ async def test_init(self): self.assertEqual(backend_with_rail.horizontal_compliance_torque, 0) self.assertEqual(backend_with_rail.timeout, 20) - async def test_convert_to_joint_space_no_rail(self): - position = [0.0, 10.0, 20.0, 30.0, 40.0, 50.0] - joint_coords = self.backend._convert_to_joint_space(position) - self.assertEqual(joint_coords.rail, 0.0) - self.assertEqual(joint_coords.base, 10.0) - self.assertEqual(joint_coords.shoulder, 20.0) - self.assertEqual(joint_coords.elbow, 30.0) - self.assertEqual(joint_coords.wrist, 40.0) - self.assertEqual(joint_coords.gripper, 50.0) - - async def test_convert_to_joint_space_no_rail_error(self): - position = [1.0, 10.0, 20.0, 30.0, 40.0, 50.0] - with self.assertRaisesRegex( - ValueError, r"Position\[0\] \(rail\) must be 0.0 for robot without rail." - ): - self.backend._convert_to_joint_space(position) - - async def test_convert_to_joint_space_too_few_elements(self): - position = [0.0, 10.0, 20.0, 30.0, 40.0] - with self.assertRaisesRegex( - ValueError, "Position must have 6 joint angles for robot with rail." - ): - self.backend._convert_to_joint_space(position) - async def test_convert_to_cartesian_space(self): position = (1.0, 2.0, 3.0, 4.0, 5.0, 6.0, ElbowOrientation.RIGHT) cartesian_coords = self.backend._convert_to_cartesian_space(position) @@ -237,7 +214,14 @@ async def test_approach_joint_space(self): b"0 StationType 1 1 0 50.0 0.0 0.0\r\n", # _set_grip_detail b"0 moveAppro 1 1\r\n", # move_to_stored_location_appro ] - position = [0.0, 10.0, 20.0, 30.0, 40.0, 50.0] + position: Dict[int, float] = { + PFAxis.RAIL: 0.0, + PFAxis.BASE: 10.0, + PFAxis.SHOULDER: 20.0, + PFAxis.ELBOW: 30.0, + PFAxis.WRIST: 40.0, + PFAxis.GRIPPER: 50.0, + } await self.backend.approach(position) self.mock_socket_instance.write.assert_any_call(b"locAngles 1 10.0 20.0 30.0 40.0 50.0\n") self.mock_socket_instance.write.assert_any_call(b"StationType 1 1 0 100 0 10\n") @@ -263,7 +247,7 @@ async def test_approach_cartesian_space(self): async def test_approach_invalid_position_type(self): with self.assertRaisesRegex( - TypeError, r"Position must be of type Iterable\[float\] or CartesianCoords." + TypeError, r"Position must be of type Dict\[int, float\] or CartesianCoords." ): await self.backend.approach("invalid") # type: ignore @@ -294,7 +278,7 @@ async def test_pick_plate_invalid_position_type(self): b"0 OK\r\n", # For set_grasp_data ] with self.assertRaisesRegex( - TypeError, r"Position must be of type Iterable\[float\] or CartesianCoords." + TypeError, r"Position must be of type Dict\[int, float\] or CartesianCoords." ): await self.backend.pick_up_resource("invalid", plate_width=1.0) # type: ignore @@ -320,11 +304,21 @@ async def test_place_plate_invalid_position_type(self): with self.assertRaisesRegex( TypeError, "place_plate only supports CartesianCoords for PreciseFlex." ): - await self.backend.drop_resource([1, 2, 3, 4, 5, 6]) + await self.backend.drop_resource("invalid") # type: ignore async def test_move_to_joint_space(self): - self.mock_socket_instance.readline.return_value = b"0 moveJ 1 0.0 10.0 20.0 30.0 40.0 50.0\r\n" - position = [0.0, 10.0, 20.0, 30.0, 40.0, 50.0] + self.mock_socket_instance.readline.side_effect = [ + b"0 OK\r\n", # waitForEom + b"0 0.0 0.0 0.0 0.0 0.0\r\n", # wherej response (current position) + b"0 moveJ 1 10.0 20.0 30.0 40.0 50.0\r\n", # moveJ response + ] + position: Dict[int, float] = { + PFAxis.BASE: 10.0, + PFAxis.SHOULDER: 20.0, + PFAxis.ELBOW: 30.0, + PFAxis.WRIST: 40.0, + PFAxis.GRIPPER: 50.0, + } await self.backend.move_to(position) self.mock_socket_instance.write.assert_called_with(b"moveJ 1 10.0 20.0 30.0 40.0 50.0\n") @@ -340,7 +334,7 @@ async def test_move_to_cartesian_space(self): async def test_move_to_invalid_position_type(self): with self.assertRaisesRegex( - TypeError, "Position must be of type Iterable\\[float\\] or CartesianCoords." + TypeError, r"Position must be of type Dict\[int, float\] or CartesianCoords." ): await self.backend.move_to("invalid") # type: ignore @@ -349,12 +343,12 @@ async def test_get_joint_position(self): b"0 10.0 20.0 30.0 40.0 50.0\r\n" # 5 values for base, shoulder, elbow, wrist, gripper ) joint_coords = await self.backend.get_joint_position() - self.assertEqual(joint_coords.rail, 0.0) - self.assertEqual(joint_coords.base, 10.0) - self.assertEqual(joint_coords.shoulder, 20.0) - self.assertEqual(joint_coords.elbow, 30.0) - self.assertEqual(joint_coords.wrist, 40.0) - self.assertEqual(joint_coords.gripper, 50.0) + self.assertEqual(joint_coords[PFAxis.RAIL], 0.0) + self.assertEqual(joint_coords[PFAxis.BASE], 10.0) + self.assertEqual(joint_coords[PFAxis.SHOULDER], 20.0) + self.assertEqual(joint_coords[PFAxis.ELBOW], 30.0) + self.assertEqual(joint_coords[PFAxis.WRIST], 40.0) + self.assertEqual(joint_coords[PFAxis.GRIPPER], 50.0) self.mock_socket_instance.write.assert_called_with(b"wherej\n") async def test_get_cartesian_position(self): @@ -394,9 +388,14 @@ async def test_approach_j(self): b"0 StationType 1 1 0 50.0 0.0 0.0\r\n", # _set_grip_detail b"0 moveAppro 1 1\r\n", # move_to_stored_location_appro ] - joint_coords = PreciseFlexJointCoords( - rail=0.0, base=10.0, shoulder=20.0, elbow=30.0, wrist=40.0, gripper=50.0 - ) + joint_coords: Dict[int, float] = { + PFAxis.RAIL: 0.0, + PFAxis.BASE: 10.0, + PFAxis.SHOULDER: 20.0, + PFAxis.ELBOW: 30.0, + PFAxis.WRIST: 40.0, + PFAxis.GRIPPER: 50.0, + } await self.backend._approach_j(joint_coords, VerticalAccess()) self.mock_socket_instance.write.assert_any_call(b"locAngles 1 10.0 20.0 30.0 40.0 50.0\n") self.mock_socket_instance.write.assert_any_call(b"StationType 1 1 0 100 0 10\n") @@ -408,9 +407,14 @@ async def test_pick_plate_j(self): b"0 StationType 1 1 0 50.0 0.0 0.0\r\n", # _set_grip_detail b"0 1\r\n", # pick_plate_from_stored_position ] - joint_coords = PreciseFlexJointCoords( - rail=0.0, base=10.0, shoulder=20.0, elbow=30.0, wrist=40.0, gripper=50.0 - ) + joint_coords: Dict[int, float] = { + PFAxis.RAIL: 0.0, + PFAxis.BASE: 10.0, + PFAxis.SHOULDER: 20.0, + PFAxis.ELBOW: 30.0, + PFAxis.WRIST: 40.0, + PFAxis.GRIPPER: 50.0, + } await self.backend._pick_plate_j(joint_coords, VerticalAccess()) self.mock_socket_instance.write.assert_any_call(b"locAngles 1 10.0 20.0 30.0 40.0 50.0\n") self.mock_socket_instance.write.assert_any_call(b"StationType 1 1 0 100 0 10\n") @@ -422,9 +426,14 @@ async def test_place_plate_j(self): b"0 StationType 1 1 0 50.0 0.0 0.0\r\n", # _set_grip_detail b"0 placeplate 1 0 0\r\n", # place_plate_to_stored_position ] - joint_coords = PreciseFlexJointCoords( - rail=0.0, base=10.0, shoulder=20.0, elbow=30.0, wrist=40.0, gripper=50.0 - ) + joint_coords: Dict[int, float] = { + PFAxis.RAIL: 0.0, + PFAxis.BASE: 10.0, + PFAxis.SHOULDER: 20.0, + PFAxis.ELBOW: 30.0, + PFAxis.WRIST: 40.0, + PFAxis.GRIPPER: 50.0, + } await self.backend._place_plate_j(joint_coords, VerticalAccess()) self.mock_socket_instance.write.assert_any_call(b"locAngles 1 10.0 20.0 30.0 40.0 50.0\n") self.mock_socket_instance.write.assert_any_call(b"StationType 1 1 0 100 0 10\n") @@ -715,15 +724,15 @@ async def test_set_tool(self): async def test_get_location_angles(self): self.mock_socket_instance.readline.return_value = b"0 1 1 10.0 20.0 30.0 40.0 50.0 60.0\r\n" # type_code, station_index, 5 values for base, shoulder, elbow, wrist, gripper - type_code, station_index, a1, a2, a3, a4, a5, a6 = await self.backend.get_location_angles(1) + type_code, station_index, angles = await self.backend.get_location_angles(1) self.assertEqual(type_code, 1) self.assertEqual(station_index, 1) - self.assertEqual(a1, 0.0) # rail - self.assertEqual(a2, 10.0) # base - self.assertEqual(a3, 20.0) - self.assertEqual(a4, 30.0) - self.assertEqual(a5, 40.0) - self.assertEqual(a6, 50.0) + self.assertEqual(angles[PFAxis.RAIL], 0.0) + self.assertEqual(angles[PFAxis.BASE], 10.0) + self.assertEqual(angles[PFAxis.SHOULDER], 20.0) + self.assertEqual(angles[PFAxis.ELBOW], 30.0) + self.assertEqual(angles[PFAxis.WRIST], 40.0) + self.assertEqual(angles[PFAxis.GRIPPER], 50.0) self.mock_socket_instance.write.assert_called_with(b"locAngles 1\n") async def test_get_location_angles_invalid_type(self): @@ -733,9 +742,14 @@ async def test_get_location_angles_invalid_type(self): async def test_set_joint_angles_no_rail(self): self.mock_socket_instance.readline.return_value = b"0 locAngles 1 10.0 20.0 30.0 40.0 50.0\r\n" - joint_coords = PreciseFlexJointCoords( - rail=0.0, base=10.0, shoulder=20.0, elbow=30.0, wrist=40.0, gripper=50.0 - ) + joint_coords: Dict[int, float] = { + PFAxis.RAIL: 0.0, + PFAxis.BASE: 10.0, + PFAxis.SHOULDER: 20.0, + PFAxis.ELBOW: 30.0, + PFAxis.WRIST: 40.0, + PFAxis.GRIPPER: 50.0, + } await self.backend.set_joint_angles(1, joint_coords) self.mock_socket_instance.write.assert_called_with(b"locAngles 1 10.0 20.0 30.0 40.0 50.0\n") @@ -745,9 +759,14 @@ async def test_set_joint_angles_with_rail(self): self.mock_socket_instance.readline.return_value = ( b"0 locAngles 1 0.0 10.0 20.0 30.0 40.0 50.0\r\n" ) - joint_coords = PreciseFlexJointCoords( - rail=0.0, base=10.0, shoulder=20.0, elbow=30.0, wrist=40.0, gripper=50.0 - ) + joint_coords: Dict[int, float] = { + PFAxis.RAIL: 0.0, + PFAxis.BASE: 10.0, + PFAxis.SHOULDER: 20.0, + PFAxis.ELBOW: 30.0, + PFAxis.WRIST: 40.0, + PFAxis.GRIPPER: 50.0, + } await backend_with_rail.set_joint_angles(1, joint_coords) self.mock_socket_instance.write.assert_called_with( b"locAngles 1 0.0 10.0 20.0 30.0 40.0 50.0\n" @@ -872,25 +891,25 @@ async def test_dest_j(self): self.mock_socket_instance.readline.return_value = ( b"0 10.0 20.0 30.0 40.0 50.0\r\n" # 5 values for base, shoulder, elbow, wrist, gripper ) - a1, a2, a3, a4, a5, a6 = await self.backend.dest_j() - self.assertEqual(a1, 0.0) # rail - self.assertEqual(a2, 10.0) # base - self.assertEqual(a3, 20.0) - self.assertEqual(a4, 30.0) - self.assertEqual(a5, 40.0) - self.assertEqual(a6, 50.0) + angles = await self.backend.dest_j() + self.assertEqual(angles[PFAxis.RAIL], 0.0) + self.assertEqual(angles[PFAxis.BASE], 10.0) + self.assertEqual(angles[PFAxis.SHOULDER], 20.0) + self.assertEqual(angles[PFAxis.ELBOW], 30.0) + self.assertEqual(angles[PFAxis.WRIST], 40.0) + self.assertEqual(angles[PFAxis.GRIPPER], 50.0) self.mock_socket_instance.write.assert_called_with(b"destJ\n") self.mock_socket_instance.readline.return_value = ( b"0 10.0 20.0 30.0 40.0 50.0\r\n" # 5 values for base, shoulder, elbow, wrist, gripper ) - a1, a2, a3, a4, a5, a6 = await self.backend.dest_j(1) - self.assertEqual(a1, 0.0) # rail - self.assertEqual(a2, 10.0) # base - self.assertEqual(a3, 20.0) - self.assertEqual(a4, 30.0) - self.assertEqual(a5, 40.0) - self.assertEqual(a6, 50.0) + angles = await self.backend.dest_j(1) + self.assertEqual(angles[PFAxis.RAIL], 0.0) + self.assertEqual(angles[PFAxis.BASE], 10.0) + self.assertEqual(angles[PFAxis.SHOULDER], 20.0) + self.assertEqual(angles[PFAxis.ELBOW], 30.0) + self.assertEqual(angles[PFAxis.WRIST], 40.0) + self.assertEqual(angles[PFAxis.GRIPPER], 50.0) self.mock_socket_instance.write.assert_called_with(b"destJ 1\n") async def test_dest_j_invalid_response(self): @@ -1524,13 +1543,13 @@ async def test_parse_xyz_response(self): async def test_parse_angles_response_no_rail(self): parts = ["10.0", "20.0", "30.0", "40.0", "50.0"] - a1, a2, a3, a4, a5, a6 = self.backend._parse_angles_response(parts) - self.assertEqual(a1, 0.0) - self.assertEqual(a2, 10.0) - self.assertEqual(a3, 20.0) - self.assertEqual(a4, 30.0) - self.assertEqual(a5, 40.0) - self.assertEqual(a6, 50.0) + angles = self.backend._parse_angles_response(parts) + self.assertEqual(angles[PFAxis.RAIL], 0.0) + self.assertEqual(angles[PFAxis.BASE], 10.0) + self.assertEqual(angles[PFAxis.SHOULDER], 20.0) + self.assertEqual(angles[PFAxis.ELBOW], 30.0) + self.assertEqual(angles[PFAxis.WRIST], 40.0) + self.assertEqual(angles[PFAxis.GRIPPER], 50.0) with self.assertRaisesRegex(PreciseFlexError, "Unexpected response format for angles."): self.backend._parse_angles_response(["10.0", "20.0"]) @@ -1538,14 +1557,14 @@ async def test_parse_angles_response_no_rail(self): async def test_parse_angles_response_with_rail(self): backend_with_rail = PreciseFlexBackend(has_rail=True, host="localhost", port=10100) backend_with_rail.io = self.mock_socket_instance - parts = ["10.0", "20.0", "30.0", "40.0", "50.0", "0.0"] - a1, a2, a3, a4, a5, a6 = backend_with_rail._parse_angles_response(parts) - self.assertEqual(a1, 0.0) - self.assertEqual(a2, 10.0) - self.assertEqual(a3, 20.0) - self.assertEqual(a4, 30.0) - self.assertEqual(a5, 40.0) - self.assertEqual(a6, 50.0) + parts = ["0.0", "10.0", "20.0", "30.0", "40.0", "50.0"] + angles = backend_with_rail._parse_angles_response(parts) + self.assertEqual(angles[PFAxis.RAIL], 50.0) + self.assertEqual(angles[PFAxis.BASE], 0.0) + self.assertEqual(angles[PFAxis.SHOULDER], 10.0) + self.assertEqual(angles[PFAxis.ELBOW], 20.0) + self.assertEqual(angles[PFAxis.WRIST], 30.0) + self.assertEqual(angles[PFAxis.GRIPPER], 40.0) with self.assertRaisesRegex(PreciseFlexError, "Unexpected response format for angles."): backend_with_rail._parse_angles_response(["0.0", "10.0"]) diff --git a/pylabrobot/arms/scara.py b/pylabrobot/arms/scara.py index 8256e34a75b..8095e751b15 100644 --- a/pylabrobot/arms/scara.py +++ b/pylabrobot/arms/scara.py @@ -1,9 +1,7 @@ -from collections.abc import Iterable -from typing import Optional, Union +from typing import Dict, Optional, Union from pylabrobot.arms.backend import AccessPattern, SCARABackend from pylabrobot.arms.precise_flex.coords import PreciseFlexCartesianCoords -from pylabrobot.arms.standard import JointCoords from pylabrobot.machines.machine import Machine @@ -16,15 +14,13 @@ def __init__(self, backend: SCARABackend): async def move_to( self, - position: Union[PreciseFlexCartesianCoords, Iterable[float]], + position: Union[PreciseFlexCartesianCoords, Dict[int, float]], **backend_kwargs, ) -> None: """Move the arm to a specified position in 3D space or joint space.""" - if isinstance(position, Iterable) and not isinstance(position, list): - position = list(position) return await self.backend.move_to(position, **backend_kwargs) - async def get_joint_position(self, **backend_kwargs) -> JointCoords: + async def get_joint_position(self, **backend_kwargs) -> Dict[int, float]: """Get the current position of the arm in joint space.""" return await self.backend.get_joint_position(**backend_kwargs) @@ -55,23 +51,21 @@ async def move_to_safe(self, **backend_kwargs) -> None: async def approach( self, - position: Union[PreciseFlexCartesianCoords, JointCoords], + position: Union[PreciseFlexCartesianCoords, Dict[int, float]], access: Optional[AccessPattern] = None, **backend_kwargs, ) -> None: """Move the arm to an approach position (offset from target). Args: - position: Target position (CartesianCoords or JointCoords) + position: Target position (CartesianCoords or joint position dict) access: Access pattern defining how to approach the target. Defaults to VerticalAccess() if not specified. """ - if isinstance(position, Iterable) and not isinstance(position, list): - position = list(position) return await self.backend.approach(position, access=access, **backend_kwargs) async def pick_up_resource( self, - position: Union[PreciseFlexCartesianCoords, JointCoords], + position: Union[PreciseFlexCartesianCoords, Dict[int, float]], plate_width: float, access: Optional[AccessPattern] = None, **backend_kwargs, @@ -83,15 +77,13 @@ async def pick_up_resource( access: Access pattern defining how to approach and retract. Defaults to VerticalAccess() if not specified. plate_width: gripper width in millimeters used when gripping the plate. """ - if isinstance(position, Iterable) and not isinstance(position, list): - position = list(position) return await self.backend.pick_up_resource( plate_width=plate_width, position=position, access=access, **backend_kwargs ) async def drop_resource( self, - position: Union[PreciseFlexCartesianCoords, JointCoords], + position: Union[PreciseFlexCartesianCoords, Dict[int, float]], access: Optional[AccessPattern] = None, **backend_kwargs, ) -> None: @@ -101,6 +93,4 @@ async def drop_resource( position: Target position for placement access: Access pattern defining how to approach and retract. Defaults to VerticalAccess() if not specified. """ - if isinstance(position, Iterable) and not isinstance(position, list): - position = list(position) return await self.backend.drop_resource(position, access=access, **backend_kwargs) diff --git a/pylabrobot/arms/standard.py b/pylabrobot/arms/standard.py index a8d702deead..a3fb8c4f3a4 100644 --- a/pylabrobot/arms/standard.py +++ b/pylabrobot/arms/standard.py @@ -1,10 +1,7 @@ from dataclasses import dataclass -from typing import Iterable from pylabrobot.resources import Coordinate, Rotation -JointCoords = Iterable[float] - @dataclass class CartesianCoords: