Skip to content

Commit 2ac4bd7

Browse files
miikeemiikee
committed
Merge branch 'feature/scara-backend-fixes' into dev-precise-flex-pf400
2 parents 4175d76 + a332ab5 commit 2ac4bd7

File tree

3 files changed

+105
-57
lines changed

3 files changed

+105
-57
lines changed

‎pylabrobot/arms/precise_flex/pf_3400.py‎

Lines changed: 40 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -12,36 +12,52 @@ def __init__(self, host: str, port: int = 10100, timeout=20, has_rail: bool = Fa
1212
self._has_rail = has_rail
1313

1414
def convert_to_joint_space(self, position: List[float]) -> PreciseFlexJointCoords:
15-
"""Convert a list of joint angles to a PreciseFlexJointCoords object.
15+
"""Convert parsed joint values to PreciseFlexJointCoords.
1616
1717
Args:
18-
position: List of 6 joint angles in order: [rail, base, shoulder, elbow, wrist, gripper]
19-
This matches the output format of the wherej command.
18+
position: List of 6 floats from _parse_angles_response() (always padded to 6).
19+
For has_rail=True: [rail, base, shoulder, elbow, wrist, gripper]
20+
For has_rail=False: [base, shoulder, elbow, wrist, gripper, 0.0(padding)]
2021
2122
Returns:
22-
PreciseFlexJointCoords with all joint values mapped correctly.
23+
PreciseFlexJointCoords with joint values mapped based on robot configuration.
2324
"""
24-
if len(position) != 6:
25-
raise ValueError("Position must be a list of 6 joint angles.")
26-
return PreciseFlexJointCoords(
27-
rail=position[0],
28-
base=position[1],
29-
shoulder=position[2],
30-
elbow=position[3],
31-
wrist=position[4],
32-
gripper=position[5],
33-
)
25+
if len(position) < 5:
26+
raise ValueError("Position must have at least 5 joint angles.")
3427

35-
# def convert_to_joints_array(self, position: PreciseFlexJointCoords) -> List[float]:
36-
# """Convert a JointSpace object to a list of joint angles."""
37-
# return [
38-
# 0,
39-
# position.base,
40-
# position.shoulder,
41-
# position.elbow,
42-
# position.wrist,
43-
# position.gripper,
44-
# ] # PF400 has 5 joints, last is fixed
28+
if self._has_rail:
29+
if len(position) < 6:
30+
raise ValueError("Position must have 6 joint angles for robot with rail.")
31+
return PreciseFlexJointCoords(
32+
rail=position[0],
33+
base=position[1],
34+
shoulder=position[2],
35+
elbow=position[3],
36+
wrist=position[4],
37+
gripper=position[5],
38+
)
39+
else:
40+
# No rail: positions 0-4 are [base, shoulder, elbow, wrist, gripper]
41+
# position[5] is 0.0 padding from _parse_angles_response() - ignore it
42+
return PreciseFlexJointCoords(
43+
rail=0.0,
44+
base=position[0],
45+
shoulder=position[1],
46+
elbow=position[2],
47+
wrist=position[3],
48+
gripper=position[4],
49+
)
50+
51+
def convert_to_joints_array(self, position: PreciseFlexJointCoords) -> List[float]:
52+
"""Convert a JointSpace object to a list of joint angles."""
53+
return [
54+
position.rail,
55+
position.base,
56+
position.shoulder,
57+
position.elbow,
58+
position.wrist,
59+
position.gripper,
60+
]
4561

4662
async def move_j(self, profile_index: int, joint_coords: PreciseFlexJointCoords) -> None:
4763
"""Move the robot using joint coordinates, handling rail configuration."""

‎pylabrobot/arms/precise_flex/pf_400.py‎

Lines changed: 40 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -12,36 +12,52 @@ def __init__(self, host: str, port: int = 10100, timeout=20, has_rail: bool = Fa
1212
self._has_rail = has_rail
1313

1414
def convert_to_joint_space(self, position: List[float]) -> PreciseFlexJointCoords:
15-
"""Convert a list of joint angles to a PreciseFlexJointCoords object.
15+
"""Convert parsed joint values to PreciseFlexJointCoords.
1616
1717
Args:
18-
position: List of 6 joint angles in order: [rail, base, shoulder, elbow, wrist, gripper]
19-
This matches the output format of the wherej command.
18+
position: List of 6 floats from _parse_angles_response() (always padded to 6).
19+
For has_rail=True: [rail, base, shoulder, elbow, wrist, gripper]
20+
For has_rail=False: [base, shoulder, elbow, wrist, gripper, 0.0(padding)]
2021
2122
Returns:
22-
PreciseFlexJointCoords with all joint values mapped correctly.
23+
PreciseFlexJointCoords with joint values mapped based on robot configuration.
2324
"""
24-
if len(position) != 6:
25-
raise ValueError("Position must be a list of 6 joint angles.")
26-
return PreciseFlexJointCoords(
27-
rail=position[0],
28-
base=position[1],
29-
shoulder=position[2],
30-
elbow=position[3],
31-
wrist=position[4],
32-
gripper=position[5],
33-
)
25+
if len(position) < 5:
26+
raise ValueError("Position must have at least 5 joint angles.")
3427

35-
# def convert_to_joints_array(self, position: PreciseFlexJointCoords) -> List[float]:
36-
# """Convert a PreciseFlexJointCoords object to a list of joint angles."""
37-
# return [
38-
# 0,
39-
# position.base,
40-
# position.shoulder,
41-
# position.elbow,
42-
# position.wrist,
43-
# 0,
44-
# ] # PF400 has 4 joints, last two are fixed
28+
if self._has_rail:
29+
if len(position) < 6:
30+
raise ValueError("Position must have 6 joint angles for robot with rail.")
31+
return PreciseFlexJointCoords(
32+
rail=position[0],
33+
base=position[1],
34+
shoulder=position[2],
35+
elbow=position[3],
36+
wrist=position[4],
37+
gripper=position[5],
38+
)
39+
else:
40+
# No rail: positions 0-4 are [base, shoulder, elbow, wrist, gripper]
41+
# position[5] is 0.0 padding from _parse_angles_response() - ignore it
42+
return PreciseFlexJointCoords(
43+
rail=0.0,
44+
base=position[0],
45+
shoulder=position[1],
46+
elbow=position[2],
47+
wrist=position[3],
48+
gripper=position[4],
49+
)
50+
51+
def convert_to_joints_array(self, position: PreciseFlexJointCoords) -> List[float]:
52+
"""Convert a PreciseFlexJointCoords object to a list of joint angles."""
53+
return [
54+
position.rail,
55+
position.base,
56+
position.shoulder,
57+
position.elbow,
58+
position.wrist,
59+
position.gripper,
60+
]
4561

4662
async def move_j(self, profile_index: int, joint_coords: PreciseFlexJointCoords) -> None:
4763
"""Move the robot using joint coordinates, handling rail configuration."""

‎pylabrobot/arms/precise_flex/precise_flex_backend.py‎

Lines changed: 25 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,7 @@ def __init__(self, host: str, port: int = 10100, timeout=20) -> None:
4949
@abstractmethod
5050
def convert_to_joint_space(self, position: List[float]) -> PreciseFlexJointCoords:
5151
"""Convert a tuple of joint angles to a PreciseFlexJointCoords object."""
52+
...
5253

5354
@abstractmethod
5455
def convert_to_joints_array(self, position: PreciseFlexJointCoords) -> List[float]:
@@ -90,7 +91,7 @@ async def setup(self):
9091
await self.io.setup()
9192
await self.set_mode("pc")
9293
await self.power_on_robot()
93-
await self.attach()
94+
await self.attach(1)
9495

9596
async def stop(self):
9697
"""Stop the PreciseFlex backend."""
@@ -140,16 +141,16 @@ async def move_to_safe(self) -> None:
140141

141142
def _convert_orientation_int_to_enum(self, orientation_int: int) -> Optional[ElbowOrientation]:
142143
if orientation_int == 1:
143-
return ElbowOrientation.LEFT
144-
if orientation_int == 2:
145144
return ElbowOrientation.RIGHT
145+
if orientation_int == 2:
146+
return ElbowOrientation.LEFT
146147
return None
147148

148149
def _convert_orientation_enum_to_int(self, orientation: Optional[ElbowOrientation]) -> int:
149150
if orientation == ElbowOrientation.LEFT:
150-
return 1
151-
if orientation == ElbowOrientation.RIGHT:
152151
return 2
152+
if orientation == ElbowOrientation.RIGHT:
153+
return 1
153154
return 0
154155

155156
async def home_all(self) -> None:
@@ -313,10 +314,22 @@ async def place_plate(
313314
await self._place_plate_c(cartesian_position=position, access=access)
314315

315316
async def move_to(self, position: Union[PreciseFlexCartesianCoords, List[float]]):
316-
"""Move the arm to a specified position in 3D space."""
317+
"""Move the arm to a specified position in 3D space.
318+
319+
Args:
320+
position: Either CartesianCoords or a 6-element list [rail, base, shoulder, elbow, wrist, gripper]
321+
"""
317322
if isinstance(position, list):
318-
joint_coords = self.convert_to_joint_space(position)
319-
print(joint_coords)
323+
if len(position) < 6:
324+
raise ValueError("Joint list must have 6 elements: [rail, base, shoulder, elbow, wrist, gripper]")
325+
joint_coords = PreciseFlexJointCoords(
326+
rail=position[0],
327+
base=position[1],
328+
shoulder=position[2],
329+
elbow=position[3],
330+
wrist=position[4],
331+
gripper=position[5],
332+
)
320333
await self.move_j(profile_index=self.profile_index, joint_coords=joint_coords)
321334
elif isinstance(position, PreciseFlexCartesianCoords):
322335
await self.move_c(profile_index=self.profile_index, cartesian_coords=position)
@@ -449,6 +462,7 @@ async def _pick_plate_c(
449462
await self.set_location_xyz(self.location_index, cartesian_position)
450463
await self._set_grip_detail(access)
451464
orientation_int = self._convert_orientation_enum_to_int(cartesian_position.orientation)
465+
orientation_int |= 0x1000 # GPL_Single: restrict wrist to ±180°
452466
await self.set_location_config(self.location_index, orientation_int)
453467
await self.pick_plate_from_stored_position(
454468
self.location_index, self.horizontal_compliance, self.horizontal_compliance_torque
@@ -463,6 +477,7 @@ async def _place_plate_c(
463477
await self.set_location_xyz(self.location_index, cartesian_position)
464478
await self._set_grip_detail(access)
465479
orientation_int = self._convert_orientation_enum_to_int(cartesian_position.orientation)
480+
orientation_int |= 0x1000 # GPL_Single: restrict wrist to ±180°
466481
await self.set_location_config(self.location_index, orientation_int)
467482
await self.place_plate_to_stored_position(
468483
self.location_index, self.horizontal_compliance, self.horizontal_compliance_torque
@@ -972,7 +987,7 @@ async def set_location_xyz(
972987
f"{cartesian_position.location.z} "
973988
f"{cartesian_position.rotation.yaw} "
974989
f"{cartesian_position.rotation.pitch} "
975-
f"{cartesian_position.rotation.y}"
990+
f"{cartesian_position.rotation.roll}"
976991
)
977992

978993
async def get_location_z_clearance(self, location_index: int) -> tuple[int, float, bool]:
@@ -1574,6 +1589,7 @@ async def move_c(
15741589

15751590
if cartesian_coords.orientation is not None:
15761591
config_int = self._convert_orientation_enum_to_int(cartesian_coords.orientation)
1592+
config_int |= 0x1000 # GPL_Single: restrict wrist to ±180°
15771593
cmd += f"{config_int}"
15781594

15791595
await self.send_command(cmd)

0 commit comments

Comments
 (0)