diff --git a/docs/api/pylabrobot.arms.rst b/docs/api/pylabrobot.arms.rst
deleted file mode 100644
index 60a7050203d..00000000000
--- a/docs/api/pylabrobot.arms.rst
+++ /dev/null
@@ -1,42 +0,0 @@
-.. currentmodule:: pylabrobot.arms
-
-pylabrobot.arms package
-=======================
-
-Arm capabilities for picking up, moving, and placing labware.
-
-.. autosummary::
- :toctree: _autosummary
- :nosignatures:
- :recursive:
-
- arm.GripperArm
- orientable_arm.OrientableArm
-
-
-Backends
---------
-
-.. autosummary::
- :toctree: _autosummary
- :nosignatures:
- :recursive:
-
- backend.GripperArmBackend
- backend.OrientableGripperArmBackend
- backend.ArticulatedGripperArmBackend
- backend.CanFreedrive
- backend.HasJoints
- backend.CanGrip
-
-
-Types
------
-
-.. autosummary::
- :toctree: _autosummary
- :nosignatures:
- :recursive:
-
- standard.GripperLocation
- standard.GripDirection
diff --git a/docs/api/pylabrobot.hamilton.rst b/docs/api/pylabrobot.hamilton.rst
index a9ac7e4848e..50d6ae270a1 100644
--- a/docs/api/pylabrobot.hamilton.rst
+++ b/docs/api/pylabrobot.hamilton.rst
@@ -123,7 +123,7 @@ STAR Liquid Handler
.. autoclass:: pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.ParkParams
:members:
-.. autoclass:: pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.CloseGripperParams
+.. autoclass:: pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.GripParams
:members:
.. autoclass:: pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.PickUpParams
diff --git a/docs/api/pylabrobot.paa.rst b/docs/api/pylabrobot.paa.rst
new file mode 100644
index 00000000000..223d88d65dc
--- /dev/null
+++ b/docs/api/pylabrobot.paa.rst
@@ -0,0 +1,32 @@
+.. currentmodule:: pylabrobot.paa
+
+pylabrobot.paa package
+======================
+
+KX2
+---
+
+.. currentmodule:: pylabrobot.paa.kx2
+
+.. autosummary::
+ :toctree: _autosummary
+ :nosignatures:
+ :recursive:
+
+ KX2
+ KX2Driver
+ KX2ArmBackend
+ KX2BarcodeReader
+ KX2BarcodeReaderDriver
+ KX2BarcodeReaderBackend
+ KX2Config
+ Axis
+
+.. autoclass:: pylabrobot.paa.kx2.arm_backend.KX2ArmBackend.CartesianMoveParams
+ :members:
+
+.. autoclass:: pylabrobot.paa.kx2.arm_backend.KX2ArmBackend.JointMoveParams
+ :members:
+
+.. autoclass:: pylabrobot.paa.kx2.arm_backend.KX2ArmBackend.GripParams
+ :members:
diff --git a/docs/api/pylabrobot.rst b/docs/api/pylabrobot.rst
index 53a04635937..fbc21c79768 100644
--- a/docs/api/pylabrobot.rst
+++ b/docs/api/pylabrobot.rst
@@ -43,6 +43,7 @@ Manufacturers
pylabrobot.mettler_toledo
pylabrobot.molecular_devices
pylabrobot.opentrons
+ pylabrobot.paa
pylabrobot.qinstruments
pylabrobot.tecan
pylabrobot.thermo_fisher
diff --git a/docs/resources/index.md b/docs/resources/index.md
index 82f30512712..11fdcfb7a80 100644
--- a/docs/resources/index.md
+++ b/docs/resources/index.md
@@ -55,7 +55,7 @@ PLR's `Resource` subclasses in the inheritance tree are:
| Resource |
| ├── Arm |
- | │ ├── ArticulatedArm |
+ | │ ├── ArticulatedGripperArm |
| │ ├── CartesianArm |
| │ └── SCARA |
diff --git a/docs/user_guide/_getting-started/installation.md b/docs/user_guide/_getting-started/installation.md
index bca89b7d671..6820560d65b 100644
--- a/docs/user_guide/_getting-started/installation.md
+++ b/docs/user_guide/_getting-started/installation.md
@@ -52,6 +52,7 @@ Different machines use different communication modes. Replace `[usb]` with one o
| `cytation-microscopy` | numpy (1.26), opencv-python | Cytation imager |
| `sila` | zeroconf, grpcio | SiLA devices |
| `pico` | opencv-python, numpy, sila | ImageXpress Pico microscope |
+| `canopen` | canopen (pulls python-can) | CANopen devices: e.g. PAA KX2 plate handler. Also requires a vendor CAN driver: PEAK PCAN-Basic on macOS/Windows, the in-tree `peak_pci` module on Linux |
| `dev` | All of the above + testing/linting tools | Development |
Or install all dependencies:
diff --git a/docs/user_guide/brooks/precise_flex/hello-world.ipynb b/docs/user_guide/brooks/precise_flex/hello-world.ipynb
index 236ba7be744..caf1737557c 100644
--- a/docs/user_guide/brooks/precise_flex/hello-world.ipynb
+++ b/docs/user_guide/brooks/precise_flex/hello-world.ipynb
@@ -40,24 +40,11 @@
},
{
"cell_type": "code",
- "execution_count": 3,
+ "execution_count": null,
"id": "klqi0r4257k",
"metadata": {},
- "outputs": [
- {
- "name": "stderr",
- "output_type": "stream",
- "text": [
- "2026-04-20 21:20:11,081 - pylabrobot.brooks.precise_flex - INFO - [PreciseFlex 10.0.0.1] connected: port=10100\n"
- ]
- }
- ],
- "source": [
- "from pylabrobot.brooks import PreciseFlex400\n",
- "\n",
- "pf = PreciseFlex400(host=\"10.0.0.1\", port=10100, has_rail=True)\n",
- "await pf.setup()"
- ]
+ "outputs": [],
+ "source": "from pylabrobot.brooks import PreciseFlex400\n\n# closed_gripper_position is the firmware-unit value at min_gripper_width.\n# Calibrate against your specific gripper before first use.\npf = PreciseFlex400(host=\"10.0.0.1\", closed_gripper_position=500.0, port=10100, has_rail=True)\nawait pf.setup()"
},
{
"cell_type": "markdown",
@@ -74,7 +61,7 @@
"source": [
"## Arm capabilities\n",
"\n",
- "The PreciseFlex exposes an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm` on `pf.arm`. For the full arm API, see [Arms](../../capabilities/arms)."
+ "The PreciseFlex exposes an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm` on `pf.arm`. For the full arm API, see [Arms](../../capabilities/arms)."
]
},
{
@@ -87,34 +74,11 @@
},
{
"cell_type": "code",
- "execution_count": 4,
+ "execution_count": null,
"id": "trbcgxwrh6k",
"metadata": {},
- "outputs": [
- {
- "name": "stderr",
- "output_type": "stream",
- "text": [
- "2026-04-20 21:20:11,097 - pylabrobot.brooks.precise_flex - INFO - [PreciseFlex 10.0.0.1] open_gripper: width_mm=120\n",
- "2026-04-20 21:20:11,233 - pylabrobot.brooks.precise_flex - INFO - [PreciseFlex 10.0.0.1] close_gripper: width_mm=80\n"
- ]
- },
- {
- "data": {
- "text/plain": [
- "False"
- ]
- },
- "execution_count": 4,
- "metadata": {},
- "output_type": "execute_result"
- }
- ],
- "source": [
- "await pf.arm.open_gripper(gripper_width=120)\n",
- "await pf.arm.close_gripper(gripper_width=80)\n",
- "await pf.arm.backend.is_gripper_closed()"
- ]
+ "outputs": [],
+ "source": "await pf.arm.move_gripper(width=120, force_sensing=False)\nawait pf.arm.move_gripper(width=80, force_sensing=True)\nawait pf.arm.backend.is_gripper_closed()"
},
{
"cell_type": "code",
@@ -126,13 +90,13 @@
"name": "stdout",
"output_type": "stream",
"text": [
- "PreciseFlexGripperLocation(location=Coordinate(x=262.0253, y=0.0042, z=1.501), rotation=Rotation(x=-180, y=90, z=0.0010000000000864489), rail=0.015, orientation='left', wrist='ccw')\n",
+ "PreciseFlexCartesianPose(location=Coordinate(x=262.0253, y=0.0042, z=1.501), rotation=Rotation(x=-180, y=90, z=0.0010000000000864489), rail=0.015, orientation='left', wrist='ccw')\n",
"{: 0.015, : 1.501, : 72.977, : 199.323, : -272.299, : 79.8}\n"
]
}
],
"source": [
- "c = await pf.arm.backend.request_gripper_location()\n",
+ "c = await pf.arm.backend.request_gripper_pose()\n",
"j = await pf.arm.backend.request_joint_position()\n",
"print(c)\n",
"print(j)"
@@ -173,7 +137,7 @@
{
"data": {
"text/plain": [
- "PreciseFlexGripperLocation(location=Coordinate(x=262.0253, y=0.0042, z=1.501), rotation=Rotation(x=-180, y=90, z=0.0010000000000864489), rail=0.015, orientation='right', wrist='ccw')"
+ "PreciseFlexCartesianPose(location=Coordinate(x=262.0253, y=0.0042, z=1.501), rotation=Rotation(x=-180, y=90, z=0.0010000000000864489), rail=0.015, orientation='right', wrist='ccw')"
]
},
"execution_count": 7,
@@ -391,4 +355,4 @@
},
"nbformat": 4,
"nbformat_minor": 5
-}
+}
\ No newline at end of file
diff --git a/docs/user_guide/capabilities/arms.md b/docs/user_guide/capabilities/arms.md
index f9a039122ae..0eb57ef1bb5 100644
--- a/docs/user_guide/capabilities/arms.md
+++ b/docs/user_guide/capabilities/arms.md
@@ -1,15 +1,16 @@
# Arms
-Arms are capabilities for picking up, moving, and placing labware (plates, lids, etc.) on the deck. PLR provides two arm types:
+Arms are capabilities for picking up, moving, and placing labware (plates, lids, etc.) on the deck. PLR provides three arm types:
-- {class}`~pylabrobot.capabilities.arms.arm.GripperArm` -- a fixed-axis gripper arm (e.g. Hamilton core grippers). Grips along a single axis.
-- {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm` -- a rotatable gripper arm (e.g. Hamilton iSWAP). Can grip from any direction.
+- {class}`~pylabrobot.capabilities.arms.arm.FixedAxisGripperArm`: a fixed-axis gripper arm (e.g. Hamilton core grippers). Grips along a single deck-fixed axis.
+- {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm`: a rotatable gripper arm (e.g. Hamilton iSWAP). Can grip from any direction.
+- {class}`~pylabrobot.capabilities.arms.articulated_arm.ArticulatedGripperArm`: a fully articulated arm (e.g. UFACTORY xArm 6). Pick/drop with arbitrary 3D rotation.
-Both inherit from `_BaseArm`, which is a {class}`~pylabrobot.capabilities.capability.Capability`.
+All three inherit from {class}`~pylabrobot.capabilities.arms.arm.GripperArm` (the abstract base that owns gripper-width control), which in turn extends `_BaseArm`, a {class}`~pylabrobot.capabilities.capability.Capability`.
## When to use
-Use arms to move plates, lids, and other labware between deck positions -- from a hotel to a reader, from a reader to a shaker, from a shaker to a centrifuge, etc.
+Use arms to move plates, lids, and other labware between deck positions: from a hotel to a reader, from a reader to a shaker, from a shaker to a centrifuge, etc.
## Setup
@@ -21,7 +22,7 @@ from pylabrobot.hamilton.star import STAR
lh = STAR(name="star", ...)
await lh.setup()
-# the arm is at lh.iswap (OrientableArm) or lh.core_gripper (GripperArm)
+# the arm is at lh.iswap (OrientableGripperArm) or lh.core_gripper (FixedAxisGripperArm)
await lh.iswap.move_resource(plate, to=heater_shaker)
```
@@ -50,22 +51,61 @@ await lh.iswap.move_resource(
)
```
-### OrientableArm: grip direction
+### OrientableGripperArm: grip direction
-```python
-from pylabrobot.capabilities.arms.standard import GripDirection
+`direction` is the world yaw of the gripper's front finger, in degrees,
+**CCW about +Z with 0° = +X**. You can pass a float, or one of the
+cardinal-direction strings:
-await lh.iswap.pick_up_resource(plate, direction=GripDirection.LEFT)
-await lh.iswap.drop_resource(reader, direction=GripDirection.FRONT)
+```python
+await lh.iswap.pick_up_resource(plate, direction="left") # = 180°
+await lh.iswap.drop_resource(reader, direction="front") # = 270°
+await lh.iswap.move_to_location(coord, direction=45.0) # 45° CCW from +X
```
+| String | Degrees | World axis (deck frame) |
+|:------------|--------:|:------------------------|
+| `"right"` | `0°` | `+X` |
+| `"back"` | `90°` | `+Y` |
+| `"left"` | `180°` | `-X` |
+| `"front"` | `270°` | `-Y` |
+
## Tips and gotchas
- **Coordinates are in the reference resource's frame** (typically the deck). The arm computes gripper target coordinates from the resource's position, dimensions, and the destination type.
- **`pickup_distance_from_bottom`** controls how far up from the bottom of the resource the gripper grips. If `None`, the resource's `preferred_pickup_location` is used, or a default of 5 mm from the top (`size_z - 5`).
- **Resource tree is updated automatically.** After a successful `drop_resource`, the resource is unassigned from its old parent and assigned to the destination.
-- **`GripOrientation`** is either a {class}`~pylabrobot.capabilities.arms.standard.GripDirection` enum (`FRONT`, `RIGHT`, `BACK`, `LEFT`) or a float in degrees.
-- **`request_gripper_location()`** queries the hardware for the current end effector position. `get_picked_up_resource()` returns the internally tracked state (no hardware call).
+- **`GripperOrientation`** is either a {data}`~pylabrobot.capabilities.arms.standard.GripperDirection` string literal (`"front"`, `"right"`, `"back"`, `"left"`) or a float in degrees, measured CCW about world +Z with 0° = +X.
+- **`request_gripper_pose()`** queries the hardware for the current end effector position. `get_picked_up_resource()` returns the internally tracked state (no hardware call).
+
+## Grippers
+
+Gripper actuation goes through a single fundamental call, `move_gripper(width, force_sensing)`, which has two modes:
+
+- `force_sensing=False`: drive the jaws to `width` mm without force feedback. The jaws reach exactly that width.
+- `force_sensing=True`: close toward `width` mm with force feedback, stopping on contact. The final width may be larger than the target.
+
+Widths are always in mm. Each backend declares its hardware limits via `min_gripper_width` and `max_gripper_width` (either may be `None` if the gripper has no commandable open/close at that end).
+
+`open_gripper()` and `close_gripper()` are convenience wrappers built on top: `open_gripper()` calls `move_gripper(max_gripper_width, force_sensing=False)` and `close_gripper()` calls `move_gripper(min_gripper_width, force_sensing=True)`. They take no width but still forward `backend_params` so backend-specific options (e.g. `iSWAPBackend.GripParams`) work the same as with `move_gripper`. They raise `NotImplementedError` if the corresponding limit is `None`.
+
+## Migration
+
+The arm capability was reshaped in the v1b1 release. The old
+`pylabrobot.arms` shim package has been removed; import from
+`pylabrobot.capabilities.arms.*` instead.
+
+| Old | New |
+|:----|:----|
+| `pylabrobot.arms.GripperArm` (concrete) | {class}`~pylabrobot.capabilities.arms.arm.FixedAxisGripperArm` |
+| `pylabrobot.arms.OrientableArm` | {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm` |
+| `pylabrobot.arms.ArticulatedArm` | {class}`~pylabrobot.capabilities.arms.articulated_arm.ArticulatedGripperArm` |
+| `iSWAPBackend.CloseGripperParams` | {class}`~pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.GripParams` |
+| `open_gripper(width=...)` / `close_gripper(width=...)` | `move_gripper(width=..., force_sensing=...)` |
+
+The name `GripperArm` still exists in `pylabrobot.capabilities.arms.arm` but
+now refers to the abstract base. Code that did `class MyArm(GripperArm)` over
+the old concrete class should subclass `FixedAxisGripperArm` instead.
## Supported hardware
@@ -74,4 +114,4 @@ await lh.iswap.drop_resource(reader, direction=GripDirection.FRONT)
## API reference
-See {class}`~pylabrobot.capabilities.arms.arm.GripperArm`, {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm`, {class}`~pylabrobot.capabilities.arms.backend.GripperArmBackend`, and {class}`~pylabrobot.capabilities.arms.backend.OrientableGripperArmBackend`.
+See {class}`~pylabrobot.capabilities.arms.arm.GripperArm`, {class}`~pylabrobot.capabilities.arms.arm.FixedAxisGripperArm`, {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm`, {class}`~pylabrobot.capabilities.arms.articulated_arm.ArticulatedGripperArm`, {class}`~pylabrobot.capabilities.arms.backend.GripperArmBackend`, and {class}`~pylabrobot.capabilities.arms.backend.OrientableGripperArmBackend`.
diff --git a/docs/user_guide/hamilton/star/core-grippers.ipynb b/docs/user_guide/hamilton/star/core-grippers.ipynb
index ced20d7ae25..e51b851fdc1 100644
--- a/docs/user_guide/hamilton/star/core-grippers.ipynb
+++ b/docs/user_guide/hamilton/star/core-grippers.ipynb
@@ -67,7 +67,7 @@
"source": [
"## Moving resources\n",
"\n",
- "Use {meth}`~pylabrobot.hamilton.liquid_handlers.star.star.STAR.core_grippers` as an async context manager. It picks up the gripper tools when entering and returns them when exiting. The yielded {class}`~pylabrobot.capabilities.arms.arm.GripperArm` has two APIs:\n",
+ "Use {meth}`~pylabrobot.hamilton.liquid_handlers.star.star.STAR.core_grippers` as an async context manager. It picks up the gripper tools when entering and returns them when exiting. The yielded {class}`~pylabrobot.capabilities.arms.arm.FixedAxisGripperArm` has two APIs:\n",
"\n",
"- `move_resource`: a single call that picks up a resource and drops it at a destination.\n",
"- `pick_up_resource` and `drop_resource`: two separate calls for more control over timing."
@@ -157,4 +157,4 @@
},
"nbformat": 4,
"nbformat_minor": 4
-}
+}
\ No newline at end of file
diff --git a/docs/user_guide/hamilton/star/hardware/replacing-iswap.md b/docs/user_guide/hamilton/star/hardware/replacing-iswap.md
index 9d9ae1bc383..94536a15d5e 100644
--- a/docs/user_guide/hamilton/star/hardware/replacing-iswap.md
+++ b/docs/user_guide/hamilton/star/hardware/replacing-iswap.md
@@ -84,11 +84,10 @@ await star.driver.iswap.reengage_break() # firmware command "R0BO"
```python
from pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAPBackend
-from pylabrobot.capabilities.arms.standard import GripDirection
await star.driver.iswap.rotate(
rotation_drive=iSWAPBackend.RotationDriveOrientation.RIGHT,
- grip_direction=GripDirection.BACK,
+ grip_direction="back",
)
```
@@ -97,7 +96,7 @@ await star.driver.iswap.rotate(
```python
await star.driver.iswap.rotate(
rotation_drive=iSWAPBackend.RotationDriveOrientation.LEFT,
- grip_direction=GripDirection.BACK,
+ grip_direction="back",
)
```
diff --git a/docs/user_guide/hamilton/star/iswap.ipynb b/docs/user_guide/hamilton/star/iswap.ipynb
index ed5b6f42eca..95a3bd358f3 100644
--- a/docs/user_guide/hamilton/star/iswap.ipynb
+++ b/docs/user_guide/hamilton/star/iswap.ipynb
@@ -6,7 +6,7 @@
"source": [
"# iSWAP Module\n",
"\n",
- "The iSWAP is a plate transport gripper arm on the Hamilton STAR(let). After {meth}`~pylabrobot.hamilton.liquid_handlers.star.star._HamiltonSTAR.setup`, it is available as `star.iswap` — an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm`."
+ "The iSWAP is a plate transport gripper arm on the Hamilton STAR(let). After {meth}`~pylabrobot.hamilton.liquid_handlers.star.star._HamiltonSTAR.setup`, it is available as `star.iswap` — an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm`."
]
},
{
@@ -160,47 +160,17 @@
},
{
"cell_type": "code",
- "execution_count": 7,
+ "execution_count": null,
"metadata": {},
- "outputs": [
- {
- "name": "stderr",
- "output_type": "stream",
- "text": [
- "2026-04-08 17:52:13,325 - pylabrobot.hamilton.liquid_handlers.star.iswap - INFO - [iSWAP] pick up plate: x=482.9, y=210.2, z=192.3, direction=270 deg, width=85.5 mm\n",
- "2026-04-08 17:52:21,538 - pylabrobot.hamilton.liquid_handlers.star.iswap - INFO - [iSWAP] release plate: x=482.9, y=306.2, z=192.3, direction=270 deg, width=85.5 mm\n"
- ]
- }
- ],
- "source": [
- "from pylabrobot.capabilities.arms.standard import GripDirection\n",
- "\n",
- "await star.iswap.pick_up_resource(plate, direction=GripDirection.LEFT)\n",
- "await star.iswap.drop_resource(plate_carrier[2], direction=GripDirection.LEFT)"
- ]
+ "outputs": [],
+ "source": "await star.iswap.pick_up_resource(plate, direction=\"left\")\nawait star.iswap.drop_resource(plate_carrier[2], direction=\"left\")"
},
{
"cell_type": "code",
- "execution_count": 8,
+ "execution_count": null,
"metadata": {},
- "outputs": [
- {
- "name": "stderr",
- "output_type": "stream",
- "text": [
- "2026-04-08 17:52:27,002 - pylabrobot.hamilton.liquid_handlers.star.iswap - INFO - [iSWAP] pick up plate: x=482.9, y=306.2, z=192.3, direction=90 deg, width=85.5 mm\n",
- "2026-04-08 17:52:33,870 - pylabrobot.hamilton.liquid_handlers.star.iswap - INFO - [iSWAP] release plate: x=482.9, y=210.2, z=192.3, direction=90 deg, width=85.5 mm\n"
- ]
- }
- ],
- "source": [
- "await star.iswap.move_resource(\n",
- " plate, plate_carrier[1],\n",
- " pickup_backend_params=iSWAPBackend.PickUpParams(grip_strength=8),\n",
- " pickup_direction=GripDirection.RIGHT,\n",
- " drop_direction=GripDirection.RIGHT,\n",
- ")"
- ]
+ "outputs": [],
+ "source": "await star.iswap.move_resource(\n plate, plate_carrier[1],\n pickup_backend_params=iSWAPBackend.PickUpParams(grip_strength=8),\n pickup_direction=\"right\",\n drop_direction=\"right\",\n)"
},
{
"cell_type": "markdown",
@@ -282,77 +252,41 @@
"await star.driver.iswap.move_relative_z(step_size=-10)"
]
},
+ {
+ "cell_type": "markdown",
+ "source": "### Gripper control\n\nTwo equivalent ways to actuate the gripper:\n\n```python\n# explicit width — pick when you need a specific target\nawait star.iswap.move_gripper(width=130, force_sensing=False)\nawait star.iswap.move_gripper(width=85, force_sensing=True)\n\n# shortcuts that read max/min from the backend\nawait star.iswap.open_gripper() # == move_gripper(max_gripper_width, force_sensing=False)\nawait star.iswap.close_gripper() # == move_gripper(min_gripper_width, force_sensing=True)\n```\n\nThe subsections below show the explicit-width form, which is what you'll usually want for plate handling. Use the shortcuts when you just need the gripper fully open or fully closed.",
+ "metadata": {}
+ },
{
"cell_type": "markdown",
"metadata": {},
- "source": [
- "### Opening the gripper\n",
- "\n",
- "Open the iSWAP gripper using {meth}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm.open_gripper`. **Warning**: this will release any object that is gripped. Useful for error recovery."
- ]
+ "source": "### Opening the gripper\n\nOpen the iSWAP gripper using {meth}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm.open_gripper`, which drives to the backend's `max_gripper_width`. Useful for error recovery.\n\n```{warning}\nThis will release any object that is gripped.\n```\n\nFor an explicit width, use {meth}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm.move_gripper` with `force_sensing=False`."
},
{
"cell_type": "code",
- "execution_count": 15,
+ "execution_count": null,
"metadata": {},
"outputs": [],
- "source": [
- "await star.iswap.open_gripper(gripper_width=130)"
- ]
+ "source": "await star.iswap.move_gripper(width=130, force_sensing=False)"
},
{
"cell_type": "markdown",
"metadata": {},
- "source": [
- "### Closing the gripper\n",
- "\n",
- "Close the iSWAP gripper. Pass {class}`~pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.CloseGripperParams` via `backend_params` to control grip strength and plate width tolerance."
- ]
+ "source": "### Closing the gripper\n\nClose the iSWAP gripper with force feedback by calling {meth}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm.move_gripper` with `force_sensing=True`. Pass {class}`~pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.GripParams` via `backend_params` to control grip strength and plate width tolerance."
},
{
"cell_type": "code",
- "execution_count": 16,
+ "execution_count": null,
"metadata": {},
- "outputs": [
- {
- "name": "stdout",
- "output_type": "stream",
- "text": [
- "As expected (no plate gripped): {'ISWAP': NoElementError('Plate not found')}\n"
- ]
- }
- ],
- "source": [
- "from pylabrobot.hamilton.liquid_handlers.star.errors import STARFirmwareError\n",
- "\n",
- "try:\n",
- " await star.iswap.close_gripper(gripper_width=120)\n",
- "except STARFirmwareError as e:\n",
- " print(f\"As expected (no plate gripped): {e.errors}\")"
- ]
+ "outputs": [],
+ "source": "from pylabrobot.hamilton.liquid_handlers.star.errors import STARFirmwareError\n\ntry:\n await star.iswap.move_gripper(width=120, force_sensing=True)\nexcept STARFirmwareError as e:\n print(f\"As expected (no plate gripped): {e.errors}\")"
},
{
"cell_type": "code",
- "execution_count": 17,
+ "execution_count": null,
"metadata": {},
- "outputs": [
- {
- "name": "stdout",
- "output_type": "stream",
- "text": [
- "As expected (no plate gripped): {'ISWAP': NoElementError('Plate not found')}\n"
- ]
- }
- ],
- "source": [
- "try:\n",
- " await star.iswap.close_gripper(\n",
- " gripper_width=85,\n",
- " backend_params=iSWAPBackend.CloseGripperParams(grip_strength=8, plate_width_tolerance=1.0),\n",
- " )\n",
- "except STARFirmwareError as e:\n",
- " print(f\"As expected (no plate gripped): {e.errors}\")"
- ]
+ "outputs": [],
+ "source": "try:\n await star.iswap.move_gripper(\n width=85,\n force_sensing=True,\n backend_params=iSWAPBackend.GripParams(grip_strength=8, plate_width_tolerance=1.0),\n )\nexcept STARFirmwareError as e:\n print(f\"As expected (no plate gripped): {e.errors}\")"
},
{
"cell_type": "markdown",
@@ -387,17 +321,10 @@
},
{
"cell_type": "code",
- "execution_count": 19,
+ "execution_count": null,
"metadata": {},
"outputs": [],
- "source": [
- "from pylabrobot.capabilities.arms.standard import GripDirection\n",
- "\n",
- "await star.driver.iswap.rotate(\n",
- " rotation_drive=iSWAPBackend.RotationDriveOrientation.RIGHT,\n",
- " grip_direction=GripDirection.LEFT,\n",
- ")"
- ]
+ "source": "await star.driver.iswap.rotate(\n rotation_drive=iSWAPBackend.RotationDriveOrientation.RIGHT,\n grip_direction=\"left\",\n)"
},
{
"cell_type": "markdown",
@@ -656,4 +583,4 @@
},
"nbformat": 4,
"nbformat_minor": 4
-}
+}
\ No newline at end of file
diff --git a/docs/user_guide/index.md b/docs/user_guide/index.md
index bb273f350cf..0bdbfc7f7cc 100644
--- a/docs/user_guide/index.md
+++ b/docs/user_guide/index.md
@@ -41,6 +41,7 @@ liconic/index
mettler_toledo/index
molecular_devices/index
opentrons/index
+paa/index
qinstruments/index
thermo_fisher/index
ufactory/index
diff --git a/docs/user_guide/paa/index.md b/docs/user_guide/paa/index.md
new file mode 100644
index 00000000000..12881ce2044
--- /dev/null
+++ b/docs/user_guide/paa/index.md
@@ -0,0 +1,9 @@
+# PAA
+
+```{toctree}
+:maxdepth: 1
+
+kx2/hello-world
+kx2/proximity-sensor
+kx2/barcode-reader
+```
diff --git a/docs/user_guide/paa/kx2/barcode-reader.ipynb b/docs/user_guide/paa/kx2/barcode-reader.ipynb
new file mode 100644
index 00000000000..1e070c855f8
--- /dev/null
+++ b/docs/user_guide/paa/kx2/barcode-reader.ipynb
@@ -0,0 +1,175 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "id": "kx2-bcr-intro",
+ "metadata": {},
+ "source": [
+ "# KX2 Barcode Reader\n",
+ "\n",
+ "The KX2's onboard barcode reader is a serial 1D laser scanner wired to the controller PC. It's fully independent of the CAN motor stack, so it works even with the arm e-stopped \u2014 exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`.\n",
+ "\n",
+ "The model that ships with KX2 systems we've inspected is a **Denso MDI-4050** (firmware reports as `BD01J09`). Factory defaults: **9600 8N1**, no flow control. Find your port with `python -m serial.tools.list_ports -v`.\n",
+ "\n",
+ "### macOS: USB-to-serial driver\n",
+ "\n",
+ "The reader cable is typically a Prolific PL2303. macOS bundles a driver for older variants but not the newer GC/HXN chip (USB ID `067b:23a3`). If the device doesn't show up at `/dev/tty.PL2303G-USBtoUART` after plugging it in:\n",
+ "\n",
+ "```bash\n",
+ "brew install --cask prolific-pl2303\n",
+ "open -a PL2303Serial # registers the system extension\n",
+ "```\n",
+ "\n",
+ "Then **System Settings \u2192 Privacy & Security \u2192 Allow** on the \"System software from PL2303Serial was blocked\" prompt, restart if asked, and replug the cable. Verify with `systemextensionsctl list`: `com.prolific.cdc.PLCdcFSDriver` should show `[activated enabled]`.\n",
+ "\n",
+ "### Symbologies\n",
+ "\n",
+ "The MDI-4050 is a 1D laser, so **PDF417 / DataMatrix / QR Code aren't supported** \u2014 you'd need the 2D imager variants (e.g. MDI-4150) for those.\n",
+ "\n",
+ "Typical out-of-the-box 1D enables: UPC-A, UPC-E, EAN-13, EAN-8, Code 39, Tri-Optic, Codabar, Industrial 2/5, Interleaved 2/5, IATA, Code 128, Code 93, GS1 DataBar, GS1 DataBar Limited. Add-on (supplemental) codes, postal codes, MSI/Plessey, Telepen, UK/Plessey, and Code 11 are off by default.\n",
+ "\n",
+ "You can read your unit's full configuration over the wire \u2014 model, firmware, every per-symbology enable flag, prefixes/suffixes, and length limits \u2014 via `dump_config()` (which sends `Z4`):\n",
+ "\n",
+ "```python\n",
+ "text = await bcr.driver.dump_config()\n",
+ "print(text[:200]) # header has MODEL / ROM Ver. / I/F\n",
+ "```\n",
+ "\n",
+ "To **change** which symbologies are enabled, three options in order of practicality:\n",
+ "\n",
+ "1. **Denso's MDI Setup utility** (free Windows download from Denso ADC) \u2014 checkbox UI, talks to the reader over the same serial port, writes to NVRAM.\n",
+ "2. **Configuration barcodes** printed in the MDI-4050 setting manual \u2014 scan a Start \u2192 toggle \u2192 End sequence; no host required.\n",
+ "3. Direct register writes via the ESC protocol \u2014 possible but the per-symbology byte layout is documented only in Denso's protocol manual.\n",
+ "\n",
+ "{class}`~pylabrobot.resources.barcode.Barcode` returned from `scan()` has `symbology=\"ANY 1D\"` by default \u2014 the reader doesn't emit a symbology ID unless prefix/suffix bytes are configured to include one."
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-bcr-setup-header",
+ "metadata": {},
+ "source": [
+ "## Setup\n",
+ "\n",
+ "Pass the serial port path. `setup()` opens the port, does the `Z1` software-version handshake, and puts the reader into single-trigger mode."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-bcr-setup-code",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from pylabrobot.paa.kx2 import KX2BarcodeReader\n",
+ "\n",
+ "bcr = KX2BarcodeReader(port=\"/dev/tty.PL2303G-USBtoUART11240\")\n",
+ "await bcr.setup()\n"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-bcr-scan-md",
+ "metadata": {},
+ "source": [
+ "`scan(read_time=...)` fires the trigger and waits up to `read_time + 1s` for the reader to decode something. Omit `read_time` to use whatever read window is currently programmed on the reader. Returns a {class}`~pylabrobot.resources.barcode.Barcode`."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-bcr-scan-code",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "barcode = await bcr.barcode_scanning.scan(read_time=8)\n",
+ "print(barcode.data)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-bcr-config-md",
+ "metadata": {},
+ "source": [
+ "Change the trigger mode at runtime:"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-bcr-config-code",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "# \"single\" (default), \"multiple\" (two reads per trigger), or \"continuous\".\n",
+ "# await bcr.set_read_mode(\"continuous\")"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-bcr-driver-md",
+ "metadata": {},
+ "source": [
+ "For raw vendor commands or the auto-trigger mode (reader fires on its own), go through `bcr.driver`:"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-bcr-driver-code",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "print(await bcr.driver.get_software_version())\n",
+ "\n",
+ "# Full NVRAM dump \u2014 model, firmware, all symbology enables, prefixes/suffixes, length limits.\n",
+ "config = await bcr.driver.dump_config()\n",
+ "print(config[:200])\n",
+ "\n",
+ "# await bcr.driver.set_auto_trigger(True) # reader scans on its own\n",
+ "# await bcr.driver.set_auto_trigger(False)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-bcr-teardown-header",
+ "metadata": {},
+ "source": [
+ "## Teardown\n",
+ "\n",
+ "`stop()` sends `Y` (trigger off) and `Y2` (reset to a 2s read window), then closes the serial port \u2014 leaving the reader in a known state for the next session."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-bcr-teardown-code",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "await bcr.stop()\n"
+ ]
+ }
+ ],
+ "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": 5
+}
diff --git a/docs/user_guide/paa/kx2/continuous-motion.ipynb b/docs/user_guide/paa/kx2/continuous-motion.ipynb
new file mode 100644
index 00000000000..035b7c6d9df
--- /dev/null
+++ b/docs/user_guide/paa/kx2/continuous-motion.ipynb
@@ -0,0 +1,382 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "id": "kx2-cm-intro",
+ "metadata": {},
+ "source": [
+ "# Continuous motion (PVT)\n",
+ "\n",
+ "The KX2 supports streamed Cartesian trajectories via the Elmo drives' Interpolated Position Mode (IPM). The host samples a target path at the drive's interpolation cadence (8 ms / 125 Hz), pushes (position, velocity) frames into a 16-deep ring buffer ~8 frames ahead of the read pointer, and lets the drive interpolate cubic-Hermite splines between them. The result is smooth, continuous motion — no PPM-style stop-and-restart between segments.\n",
+ "\n",
+ "Three APIs use this transport:\n",
+ "\n",
+ "| API | Input | Use case |\n",
+ "|---|---|---|\n",
+ "| `arm.move_to_location(..., path='linear')` | one target pose | a single straight-line Cartesian move |\n",
+ "| `arm.backend.move_parametric(fn, duration_s)` | a callable `t → pose` | math-defined paths (figure-8, helix, Lissajous) |\n",
+ "| `arm.backend.move_through_waypoints(poses, speed, accel)` | a list of poses | smooth pass-through any sequence of points |\n",
+ "\n",
+ "All three share the same underlying streamer, EMCY handling, and IPM cleanup. See [hello-world](hello-world.ipynb) for the rest of the KX2 API."
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-cm-setup-header",
+ "metadata": {},
+ "source": [
+ "## Setup"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 1,
+ "id": "kx2-cm-setup",
+ "metadata": {
+ "execution": {
+ "iopub.execute_input": "2026-05-17T03:51:14.542746Z",
+ "iopub.status.busy": "2026-05-17T03:51:14.542612Z",
+ "iopub.status.idle": "2026-05-17T03:51:23.563072Z",
+ "shell.execute_reply": "2026-05-17T03:51:23.561338Z"
+ }
+ },
+ "outputs": [
+ {
+ "name": "stderr",
+ "output_type": "stream",
+ "text": [
+ "uptime library not available, timestamps are relative to boot time and not to Epoch UTC\n",
+ "2026-05-16 21:16:47,364 - pylabrobot.paa.kx2.driver - INFO - canopen: connected, nodes=[1, 2, 3, 4, 6]\n"
+ ]
+ }
+ ],
+ "source": [
+ "import math\n",
+ "from pylabrobot.paa.kx2 import KX2, KX2ArmBackend\n",
+ "from pylabrobot.capabilities.arms.standard import CartesianPose\n",
+ "from pylabrobot.resources import Coordinate, Rotation\n",
+ "\n",
+ "kx2 = KX2()\n",
+ "await kx2.setup()"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-cm-linear-header",
+ "metadata": {},
+ "source": [
+ "## 1. Straight-line move\n",
+ "\n",
+ "`path='linear'` on the existing `move_to_location` swaps PPM for the IPM streamer. The gripper tip traces a straight line from current pose to target, sampled at 8 ms intervals along a trapezoidal arc-length profile. `max_gripper_speed` (mm/s) and `max_gripper_acceleration` (mm/s²) are required — the host builds the profile directly from them (no firmware fallback for streamed motion).\n",
+ "\n",
+ "End-of-motion drops a few frames of the trajectory tail (~0.3 mm at 50 mm/s); for tighter terminal-position needs, fall through to the joint-mode `move_to_location` (default `path='joint'`)."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 2,
+ "id": "kx2-cm-linear",
+ "metadata": {
+ "execution": {
+ "iopub.execute_input": "2026-05-17T03:51:23.567365Z",
+ "iopub.status.busy": "2026-05-17T03:51:23.567076Z",
+ "iopub.status.idle": "2026-05-17T03:51:24.343056Z",
+ "shell.execute_reply": "2026-05-17T03:51:24.342528Z"
+ }
+ },
+ "outputs": [],
+ "source": [
+ "start = await kx2.arm.request_gripper_pose()\n",
+ "target = Coordinate(x=start.location.x, y=start.location.y, z=start.location.z + 20)\n",
+ "await kx2.arm.move_to_location(\n",
+ " location=target,\n",
+ " direction=start.rotation.z,\n",
+ " backend_params=KX2ArmBackend.CartesianMoveParams(\n",
+ " max_gripper_speed=50.0, max_gripper_acceleration=200.0, path='linear',\n",
+ " ),\n",
+ ")"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-cm-parametric-header",
+ "metadata": {},
+ "source": [
+ "## 2. Parametric trajectories\n",
+ "\n",
+ "`move_parametric(path_fn, duration_s)` calls `path_fn(t)` at every IPM sample time (`t ∈ [0, duration_s]` seconds) and streams the resulting poses. The function defines *both* shape and pacing — there's no separate speed/accel cap, so the caller is responsible for keeping derivatives within drive limits.\n",
+ "\n",
+ "IK runs on every sample with mandatory joint-limit enforcement; an out-of-range pose anywhere along the trajectory raises `IKError` **before any motion starts**.\n",
+ "\n",
+ "### Figure 8 (lemniscate)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 3,
+ "id": "kx2-cm-fig8",
+ "metadata": {
+ "execution": {
+ "iopub.execute_input": "2026-05-17T03:51:24.344548Z",
+ "iopub.status.busy": "2026-05-17T03:51:24.344461Z",
+ "iopub.status.idle": "2026-05-17T03:51:34.491757Z",
+ "shell.execute_reply": "2026-05-17T03:51:34.491240Z"
+ }
+ },
+ "outputs": [],
+ "source": [
+ "start = await kx2.arm.request_gripper_pose()\n",
+ "Rx, Ry, H, T = 25.0, 12.0, 20.0, 10.0 # 50mm wide, 25mm tall lemniscate, 30mm Z arch, 8s\n",
+ "\n",
+ "def fig8(t):\n",
+ " s = t / T\n",
+ " theta = 2 * math.pi * s\n",
+ " return CartesianPose(\n",
+ " location=Coordinate(\n",
+ " x=start.location.x + Rx * math.sin(theta),\n",
+ " y=start.location.y + Ry * math.sin(2 * theta) / 2,\n",
+ " z=start.location.z + H * math.sin(math.pi * s),\n",
+ " ),\n",
+ " rotation=start.rotation,\n",
+ " )\n",
+ "\n",
+ "await kx2.arm.backend.move_parametric(fig8, duration_s=T)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-cm-helix-header",
+ "metadata": {},
+ "source": [
+ "### Rising helix\n",
+ "\n",
+ "Constant-radius circle in XY, linear ramp in Z. Both endpoints land back at the start XY but the Z is offset."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 4,
+ "id": "kx2-cm-helix",
+ "metadata": {
+ "execution": {
+ "iopub.execute_input": "2026-05-17T03:51:34.493405Z",
+ "iopub.status.busy": "2026-05-17T03:51:34.493307Z",
+ "iopub.status.idle": "2026-05-17T03:51:40.615193Z",
+ "shell.execute_reply": "2026-05-17T03:51:40.614590Z"
+ }
+ },
+ "outputs": [],
+ "source": [
+ "start = await kx2.arm.request_gripper_pose()\n",
+ "R, dZ, turns, T = 30.0, 40.0, 2.0, 6.0\n",
+ "\n",
+ "def helix(t):\n",
+ " s = t / T\n",
+ " theta = 2 * math.pi * turns * s\n",
+ " return CartesianPose(\n",
+ " location=Coordinate(\n",
+ " x=start.location.x + R * math.sin(theta),\n",
+ " y=start.location.y + R * (1 - math.cos(theta)),\n",
+ " z=start.location.z + dZ * s,\n",
+ " ),\n",
+ " rotation=start.rotation,\n",
+ " )\n",
+ "\n",
+ "await kx2.arm.backend.move_parametric(helix, duration_s=T)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-cm-lissa-header",
+ "metadata": {},
+ "source": [
+ "### 3D Lissajous\n",
+ "\n",
+ "Different per-axis frequencies trace intricate closed (or quasi-closed) curves in space. Tune for the shape and for the size; conservative numbers keep elbow/shoulder joint velocity well inside drive limits."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 5,
+ "id": "kx2-cm-lissa",
+ "metadata": {
+ "execution": {
+ "iopub.execute_input": "2026-05-17T03:51:40.616597Z",
+ "iopub.status.busy": "2026-05-17T03:51:40.616509Z",
+ "iopub.status.idle": "2026-05-17T03:51:56.740932Z",
+ "shell.execute_reply": "2026-05-17T03:51:56.740396Z"
+ }
+ },
+ "outputs": [],
+ "source": [
+ "start = await kx2.arm.request_gripper_pose()\n",
+ "Ax, Ay, Az = 12.0, 8.0, 8.0 # small amplitudes keep joint velocity inside drive limits\n",
+ "a, b, c = 2, 1, 1 # frequency ratios\n",
+ "T = 16.0 # seconds — long enough to keep speeds modest\n",
+ "\n",
+ "def lissa(t):\n",
+ " s = t / T\n",
+ " return CartesianPose(\n",
+ " location=Coordinate(\n",
+ " x=start.location.x + Ax * math.sin(2 * math.pi * a * s),\n",
+ " y=start.location.y + Ay * math.sin(2 * math.pi * b * s + math.pi / 4),\n",
+ " z=start.location.z + Az * math.sin(2 * math.pi * c * s),\n",
+ " ),\n",
+ " rotation=start.rotation,\n",
+ " )\n",
+ "\n",
+ "await kx2.arm.backend.move_parametric(lissa, duration_s=T)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-cm-waypoints-header",
+ "metadata": {},
+ "source": [
+ "## 3. Smooth waypoints\n",
+ "\n",
+ "`move_through_waypoints(poses, speed, accel)` fits a centripetal Catmull-Rom spline through the list of poses and reparametrizes it with a trapezoidal arc-length profile capped at `speed` (mm/s) and `accel` (mm/s²). The curve is C¹ continuous — there's no stop at intermediate waypoints.\n",
+ "\n",
+ "Useful when you have a sequence of named locations (sample sites, plate corners, scan path) and want smooth motion through them."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 6,
+ "id": "kx2-cm-waypoints",
+ "metadata": {
+ "execution": {
+ "iopub.execute_input": "2026-05-17T03:51:56.742312Z",
+ "iopub.status.busy": "2026-05-17T03:51:56.742223Z",
+ "iopub.status.idle": "2026-05-17T03:52:01.715157Z",
+ "shell.execute_reply": "2026-05-17T03:52:01.714719Z"
+ }
+ },
+ "outputs": [],
+ "source": [
+ "start = await kx2.arm.request_gripper_pose()\n",
+ "\n",
+ "# Square corners + a Z hop at one corner — gripper rounds each corner smoothly\n",
+ "# rather than stopping at each.\n",
+ "def at(dx=0, dy=0, dz=0):\n",
+ " return CartesianPose(\n",
+ " location=Coordinate(start.location.x + dx, start.location.y + dy, start.location.z + dz),\n",
+ " rotation=start.rotation,\n",
+ " )\n",
+ "\n",
+ "waypoints = [\n",
+ " at(),\n",
+ " at(dx=20),\n",
+ " at(dx=20, dy=20),\n",
+ " at(dx=20, dy=20, dz=15), # hop up\n",
+ " at(dx=20, dy=20), # back down\n",
+ " at(dy=20),\n",
+ " at(),\n",
+ "]\n",
+ "\n",
+ "await kx2.arm.backend.move_through_waypoints(waypoints, speed=25.0, accel=80.0)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-cm-zigzag-header",
+ "metadata": {},
+ "source": [
+ "### Zig-zag scan\n",
+ "\n",
+ "Useful pattern for sweeping over a rectangular region (e.g. raster scanning a plate). The Catmull-Rom spline rounds each corner so the gripper never has to come to a stop."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 7,
+ "id": "kx2-cm-zigzag",
+ "metadata": {
+ "execution": {
+ "iopub.execute_input": "2026-05-17T03:52:01.716905Z",
+ "iopub.status.busy": "2026-05-17T03:52:01.716784Z",
+ "iopub.status.idle": "2026-05-17T03:52:10.558103Z",
+ "shell.execute_reply": "2026-05-17T03:52:10.557606Z"
+ }
+ },
+ "outputs": [],
+ "source": [
+ "start = await kx2.arm.request_gripper_pose()\n",
+ "rows, row_dy, sweep_dx = 4, 10.0, 50.0\n",
+ "\n",
+ "waypoints = []\n",
+ "for r in range(rows):\n",
+ " y = r * row_dy\n",
+ " x_left, x_right = (0.0, sweep_dx) if r % 2 == 0 else (sweep_dx, 0.0)\n",
+ " waypoints.append(CartesianPose(\n",
+ " location=Coordinate(start.location.x + x_left, start.location.y + y, start.location.z),\n",
+ " rotation=start.rotation,\n",
+ " ))\n",
+ " waypoints.append(CartesianPose(\n",
+ " location=Coordinate(start.location.x + x_right, start.location.y + y, start.location.z),\n",
+ " rotation=start.rotation,\n",
+ " ))\n",
+ "\n",
+ "await kx2.arm.backend.move_through_waypoints(waypoints, speed=30.0, accel=100.0)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-cm-safety-header",
+ "metadata": {},
+ "source": [
+ "## Safety notes\n",
+ "\n",
+ "- **Joint limits are enforced by IK**: any pose whose IK solution falls outside the per-axis `[min_travel, max_travel]` raises `IKError` before any frame leaves the host. Catches workspace-violating trajectories at sample time, not at the drive.\n",
+ "- **Drive velocity / acceleration limits are NOT enforced**. `move_parametric` lets the caller define the velocity profile, and `move_through_waypoints` only caps Cartesian speed/accel — neither checks per-axis encoder rates. A swirly trajectory near a kinematic singularity can demand joint speeds way above what the motors can produce; the drives will either lag or fault. Stay inside the workspace and use moderate speed/accel.\n",
+ "- **Cancel/halt during streaming** drops ip-enable and lets the drive consume already-buffered frames before stopping. Coast can run up to ~64 ms past the cancel. For true zero-coast stop, use `await kx2.arm.backend.halt()` (MO=0 on every motion axis)."
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-cm-teardown-header",
+ "metadata": {},
+ "source": [
+ "## Teardown"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 8,
+ "id": "kx2-cm-teardown",
+ "metadata": {
+ "execution": {
+ "iopub.execute_input": "2026-05-17T03:52:10.559447Z",
+ "iopub.status.busy": "2026-05-17T03:52:10.559377Z",
+ "iopub.status.idle": "2026-05-17T03:52:10.562092Z",
+ "shell.execute_reply": "2026-05-17T03:52:10.561785Z"
+ }
+ },
+ "outputs": [],
+ "source": [
+ "await kx2.stop()"
+ ]
+ }
+ ],
+ "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": 5
+}
diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb
new file mode 100644
index 00000000000..e321cd99bfb
--- /dev/null
+++ b/docs/user_guide/paa/kx2/hello-world.ipynb
@@ -0,0 +1,522 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "id": "kx2-intro",
+ "metadata": {},
+ "source": [
+ "# PAA KX2\n",
+ "\n",
+ "The [PAA KX2](https://www.peakanalysisandautomation.com/products/plate-handlers/kx2/) (also sold as KiNEDx KX2) is a 5-axis SCARA plate-handling robot from Peak Analysis and Automation. It supports:\n",
+ "\n",
+ "- [Arms](../../capabilities/arms) (pick/place, Cartesian and joint movement, freedrive teaching)\n",
+ "\n",
+ "The device communicates over CAN bus using the CANopen protocol (CiA-301 + CiA-402 drive profile). A PCAN/SocketCAN-compatible CAN interface is required (e.g. PEAK PCAN-USB).\n",
+ "\n",
+ "| Model | PLR Name | Gripper | Notes |\n",
+ "|---|---|---|---|\n",
+ "| KX2 | `KX2` | servo | 4 motion axes (shoulder, Z, elbow, wrist) + servo gripper |"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-setup-header",
+ "metadata": {},
+ "source": [
+ "## Setup\n",
+ "\n",
+ "`setup()` opens the CAN bus, discovers nodes, reads drive parameters, enables the motion axes, and homes + closes the servo gripper."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 2,
+ "id": "kx2-setup-code",
+ "metadata": {},
+ "outputs": [
+ {
+ "name": "stderr",
+ "output_type": "stream",
+ "text": [
+ "uptime library not available, timestamps are relative to boot time and not to Epoch UTC\n",
+ "2026-05-03 19:56:58,933 - pylabrobot.paa.kx2.driver - INFO - canopen: connected, nodes=[1, 2, 3, 4, 6]\n",
+ "2026-05-03 19:57:01,871 - pylabrobot.paa.kx2.driver - WARNING - node 1: re-enabling (was disabled)\n",
+ "2026-05-03 19:57:02,082 - pylabrobot.paa.kx2.driver - WARNING - motor_enable(state=True) attempt 1/20 failed for node 1 (MO=0); retrying\n",
+ "2026-05-03 19:57:02,704 - pylabrobot.paa.kx2.driver - WARNING - node 3: re-enabling (was disabled)\n",
+ "2026-05-03 19:57:02,835 - pylabrobot.paa.kx2.driver - WARNING - node 4: re-enabling (was disabled)\n"
+ ]
+ }
+ ],
+ "source": [
+ "from pylabrobot.paa.kx2 import KX2\n",
+ "\n",
+ "kx2 = KX2()\n",
+ "await kx2.setup()"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-arm-header",
+ "metadata": {},
+ "source": [
+ "## Arm capabilities\n",
+ "\n",
+ "The KX2 exposes an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm` on `kx2.arm`, backed by {class}`~pylabrobot.paa.kx2.KX2ArmBackend`. Gripper yaw is controlled via a single `direction` float (degrees; 0\u00b0 = front). For the full arm API, see [Arms](../../capabilities/arms).\n",
+ "\n",
+ "The raw driver (`kx2.driver`, a `KX2Driver`) stays available for low-level access \u2014 motor commands, drive diagnostics, binary interpreter, etc."
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-gripper-header",
+ "metadata": {},
+ "source": [
+ "### Gripper control\n",
+ "\n",
+ "The servo gripper is force-aware. `close_gripper` moves to the target width and then verifies the plate is gripped; pass `check_plate_gripped=False` via `GripParams` to skip the verification move. Units are KX2 gripper encoder units (mm-equivalent once calibrated)."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 4,
+ "id": "kx2-gripper-open",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "await kx2.arm.open_gripper(gripper_width=25)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 5,
+ "id": "kx2-gripper-close",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from pylabrobot.paa.kx2 import KX2ArmBackend\n",
+ "\n",
+ "await kx2.arm.close_gripper(\n",
+ " gripper_width=0,\n",
+ " backend_params=KX2ArmBackend.GripParams(check_plate_gripped=False),\n",
+ ")"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 6,
+ "id": "kx2-gripper-status",
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "True"
+ ]
+ },
+ "execution_count": 6,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "source": [
+ "await kx2.arm.is_gripper_closed()"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-gripper-force-md",
+ "metadata": {},
+ "source": [
+ "Cap the gripping force on a per-close basis (as a percentage of the motor's peak current \u2014 10..100, clamped) when closing on fragile labware:"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 7,
+ "id": "kx2-gripper-force",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "await kx2.arm.close_gripper(\n",
+ " gripper_width=0,\n",
+ " backend_params=KX2ArmBackend.GripParams(max_force_percent=30),\n",
+ ")"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-motion-limits-md",
+ "metadata": {},
+ "source": [
+ "### Motion limits\n",
+ "\n",
+ "`motion_limits()` reads the per-axis firmware maxima cached at setup. `JointMoveParams` / `CartesianMoveParams` take `max_gripper_speed` (mm/s) and `max_gripper_acceleration` (mm/s^2), which cap the worst-case Cartesian speed/acceleration at the gripper along the trajectory. `None` (the default) means no Cartesian cap \u2014 joints run at firmware max. Joint speeds get scaled uniformly so the gripper stays under the cap."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 8,
+ "id": "kx2-motion-limits",
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "axis max speed max acceleration\n",
+ "------------- ------------ ----------------\n",
+ "SHOULDER 145.00 deg/s 300.00 deg/s^2 \n",
+ "Z 750.00 mm/s 1000.00 mm/s^2 \n",
+ "ELBOW 80.00 deg/s 180.00 deg/s^2 \n",
+ "WRIST 500.00 deg/s 1000.04 deg/s^2 \n",
+ "SERVO_GRIPPER 47.00 mm/s 200.00 mm/s^2 "
+ ]
+ },
+ "execution_count": 8,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "source": [
+ "kx2.arm.backend.motion_limits()"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-cartesian-header",
+ "metadata": {},
+ "source": [
+ "### Cartesian movement\n",
+ "\n",
+ "Move the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only \u2014 the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity and acceleration."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 9,
+ "id": "11b06c86",
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "CartesianPose(location=Coordinate(x=312.2642, y=564.2383, z=333.7532), rotation=Rotation(x=0, y=0, z=265.67006324798695))"
+ ]
+ },
+ "execution_count": 9,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "source": [
+ "await kx2.arm.request_gripper_pose()"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 15,
+ "id": "kx2-cartesian-code",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from pylabrobot.resources import Coordinate\n",
+ "\n",
+ "await kx2.arm.move_to_location(\n",
+ " location=Coordinate(x=330.124, y=210.552, z=320.0441),\n",
+ " direction=198,\n",
+ " backend_params=KX2ArmBackend.CartesianMoveParams(\n",
+ " max_gripper_speed=200.0, max_gripper_acceleration=1000.0,\n",
+ " ),\n",
+ ")"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "0a41fece",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from pylabrobot.resources import Coordinate\n",
+ "\n",
+ "await kx2.arm.move_to_location(\n",
+ " location=Coordinate(x=130.124, y=210.552, z=420.0441),\n",
+ " direction=100,\n",
+ " backend_params=KX2ArmBackend.CartesianMoveParams(\n",
+ " max_gripper_speed=200.0,\n",
+ " max_gripper_acceleration=1000.0,\n",
+ " path=\"joint\"\n",
+ " ),\n",
+ ")"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-joint-header",
+ "metadata": {},
+ "source": [
+ "### Joint movement\n",
+ "\n",
+ "Move in joint space using the {class}`~pylabrobot.paa.kx2.KX2ArmBackend.Axis` enum. Rotary axes in degrees; Z (and gripper) in mm-equivalent encoder units.\n",
+ "\n",
+ "```{warning}\n",
+ "Moving to arbitrary joint positions can cause the arm to collide with its base, gripper, or nearby equipment. Verify coordinates in a safe workspace first, and start with low velocity.\n",
+ "```"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 1,
+ "id": "kx2-joint-code",
+ "metadata": {},
+ "outputs": [
+ {
+ "ename": "AttributeError",
+ "evalue": "type object 'KX2ArmBackend' has no attribute 'Axis'",
+ "output_type": "error",
+ "traceback": [
+ "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
+ "\u001b[0;31mAttributeError\u001b[0m Traceback (most recent call last)",
+ "Cell \u001b[0;32mIn[1], line 4\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mpylabrobot\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mpaa\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mkx2\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m KX2ArmBackend\n\u001b[1;32m 3\u001b[0m position \u001b[38;5;241m=\u001b[39m {\n\u001b[0;32m----> 4\u001b[0m \u001b[43mKX2ArmBackend\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mAxis\u001b[49m\u001b[38;5;241m.\u001b[39mSHOULDER: \u001b[38;5;241m0.0\u001b[39m,\n\u001b[1;32m 5\u001b[0m KX2ArmBackend\u001b[38;5;241m.\u001b[39mAxis\u001b[38;5;241m.\u001b[39mZ: \u001b[38;5;241m150.0\u001b[39m,\n\u001b[1;32m 6\u001b[0m KX2ArmBackend\u001b[38;5;241m.\u001b[39mAxis\u001b[38;5;241m.\u001b[39mELBOW: \u001b[38;5;241m90.0\u001b[39m,\n\u001b[1;32m 7\u001b[0m KX2ArmBackend\u001b[38;5;241m.\u001b[39mAxis\u001b[38;5;241m.\u001b[39mWRIST: \u001b[38;5;241m0.0\u001b[39m,\n\u001b[1;32m 8\u001b[0m }\n\u001b[1;32m 9\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m kx2\u001b[38;5;241m.\u001b[39marm\u001b[38;5;241m.\u001b[39mbackend\u001b[38;5;241m.\u001b[39mmove_to_joint_position(\n\u001b[1;32m 10\u001b[0m position,\n\u001b[1;32m 11\u001b[0m backend_params\u001b[38;5;241m=\u001b[39mKX2ArmBackend\u001b[38;5;241m.\u001b[39mJointMoveParams(\n\u001b[1;32m 12\u001b[0m max_gripper_speed\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m200.0\u001b[39m, max_gripper_acceleration\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m1000.0\u001b[39m,\n\u001b[1;32m 13\u001b[0m ),\n\u001b[1;32m 14\u001b[0m )\n",
+ "\u001b[0;31mAttributeError\u001b[0m: type object 'KX2ArmBackend' has no attribute 'Axis'"
+ ]
+ }
+ ],
+ "source": [
+ "from pylabrobot.paa.kx2 import KX2ArmBackend\n",
+ "\n",
+ "position = {\n",
+ " KX2ArmBackend.Axis.SHOULDER: 0.0,\n",
+ " KX2ArmBackend.Axis.Z: 150.0,\n",
+ " KX2ArmBackend.Axis.ELBOW: 90.0,\n",
+ " KX2ArmBackend.Axis.WRIST: 0.0,\n",
+ "}\n",
+ "await kx2.arm.backend.move_to_joint_position(\n",
+ " position,\n",
+ " backend_params=KX2ArmBackend.JointMoveParams(\n",
+ " max_gripper_speed=200.0, max_gripper_acceleration=1000.0,\n",
+ " ),\n",
+ ")"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "9a6d0ed1",
+ "metadata": {},
+ "source": [
+ "### Joint-space pick and place\n",
+ "\n",
+ "`pick_up_at_joint_position` / `drop_at_joint_position` do the same thing as their Cartesian counterparts (move, then close/open the gripper), but the target pose is specified in joint space. Useful when you've taught a position via freedrive and want to return to it without going through inverse kinematics."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "0a19ceeb",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "pick_joints = {\n",
+ " KX2ArmBackend.Axis.SHOULDER: 0.0,\n",
+ " KX2ArmBackend.Axis.Z: 150.0,\n",
+ " KX2ArmBackend.Axis.ELBOW: 90.0,\n",
+ " KX2ArmBackend.Axis.WRIST: 0.0,\n",
+ "}\n",
+ "place_joints = {\n",
+ " KX2ArmBackend.Axis.SHOULDER: 30.0,\n",
+ " KX2ArmBackend.Axis.Z: 150.0,\n",
+ " KX2ArmBackend.Axis.ELBOW: 90.0,\n",
+ " KX2ArmBackend.Axis.WRIST: 0.0,\n",
+ "}\n",
+ "\n",
+ "await kx2.arm.backend.pick_up_at_joint_position(pick_joints, resource_width=0)\n",
+ "await kx2.arm.backend.drop_at_joint_position(place_joints, resource_width=30)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-query-header",
+ "metadata": {},
+ "source": [
+ "### Querying position\n",
+ "\n",
+ "Read the current joint angles or Cartesian end-effector pose:"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-query-joints",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "await kx2.arm.backend.request_joint_position()"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-query-cart",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "await kx2.arm.request_gripper_pose()"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-pick-place-header",
+ "metadata": {},
+ "source": [
+ "### Pick and place\n",
+ "\n",
+ "`pick_up_at_location` moves to the target pose and closes the gripper to `resource_width`. `drop_at_location` moves and opens the gripper. Approach and retract motions are the caller's responsibility \u2014 bracket these calls with your own pre- and post-moves."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-pick-place-code",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "pick = Coordinate(x=450.0, y=0.0, z=80.0)\n",
+ "place = Coordinate(x=450.0, y=-200.0, z=80.0)\n",
+ "above = Coordinate(x=0, y=0, z=60.0)\n",
+ "yaw = 0.0\n",
+ "\n",
+ "# pick_up_at_location stores resource_width on the frontend; drop_at_location\n",
+ "# reuses it, so you don't pass it again.\n",
+ "await kx2.arm.move_to_location(pick + above, direction=yaw)\n",
+ "await kx2.arm.pick_up_at_location(location=pick, direction=yaw, resource_width=0)\n",
+ "await kx2.arm.move_to_location(pick + above, direction=yaw)\n",
+ "\n",
+ "await kx2.arm.move_to_location(place + above, direction=yaw)\n",
+ "await kx2.arm.drop_at_location(location=place, direction=yaw)\n",
+ "await kx2.arm.move_to_location(place + above, direction=yaw)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-freedrive-header",
+ "metadata": {},
+ "source": [
+ "### Freedrive (teaching mode)\n",
+ "\n",
+ "Freedrive disables torque on the motion axes so you can push the arm by hand; the servo gripper stays powered. The KX2 frees all motion axes at once \u2014 the `free_axes` argument is accepted for API parity and ignored."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-free-on",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "await kx2.arm.backend.start_freedrive_mode(free_axes=[0])"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-free-read",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "# Manually guide the arm to the desired pose, then read it:\n",
+ "taught = await kx2.arm.request_gripper_pose()\n",
+ "print(taught)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-free-off",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "await kx2.arm.backend.stop_freedrive_mode()"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-misc-header",
+ "metadata": {},
+ "source": [
+ "### Halt and fault diagnostics\n",
+ "\n",
+ "`halt` issues an emergency stop on every motion axis. Driver-level methods give you estop-line state and per-axis fault codes."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-halt",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "await kx2.arm.backend.halt()"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-estop",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "await kx2.driver.get_estop_state()"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-fault",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from pylabrobot.paa.kx2 import KX2ArmBackend\n",
+ "\n",
+ "await kx2.driver.motor_get_fault(KX2ArmBackend.Axis.SHOULDER)"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "kx2-teardown-header",
+ "metadata": {},
+ "source": [
+ "## Teardown"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "kx2-teardown-code",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "await kx2.stop()"
+ ]
+ }
+ ],
+ "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": 5
+}
diff --git a/docs/user_guide/paa/kx2/proximity-sensor.ipynb b/docs/user_guide/paa/kx2/proximity-sensor.ipynb
new file mode 100644
index 00000000000..e0972fdb605
--- /dev/null
+++ b/docs/user_guide/paa/kx2/proximity-sensor.ipynb
@@ -0,0 +1,143 @@
+{
+ "cells": [
+ {
+ "cell_type": "markdown",
+ "id": "proximity-intro",
+ "metadata": {},
+ "source": [
+ "# KX2 Proximity Sensor\n",
+ "\n",
+ "The KX2's onboard IR breakbeam between the gripper fingers, wired \n",
+ "to the Z drive's IO header (pin 4 \u2014 auto-discovered as \n",
+ "`\"ProximitySensor\"`). Binary signal: `True` when the beam is \n",
+ "broken (object present). Useful for confirming an object is in \n",
+ "the gripper before closing, or for finding the Z height of an \n",
+ "object below the gripper.\n"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "proximity-setup-header",
+ "metadata": {},
+ "source": [
+ "## Setup\n"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 1,
+ "id": "proximity-setup-code",
+ "metadata": {},
+ "outputs": [
+ {
+ "name": "stderr",
+ "output_type": "stream",
+ "text": [
+ "uptime library not available, timestamps are relative to boot time and not to Epoch UTC\n",
+ "2026-05-03 19:52:43,005 - pylabrobot.paa.kx2.driver - INFO - canopen: connected, nodes=[1, 2, 3, 4, 6]\n",
+ "2026-05-03 19:52:45,964 - pylabrobot.paa.kx2.driver - WARNING - node 1: re-enabling (was disabled)\n",
+ "2026-05-03 19:52:46,081 - pylabrobot.paa.kx2.driver - WARNING - node 3: re-enabling (was disabled)\n",
+ "2026-05-03 19:52:46,246 - pylabrobot.paa.kx2.driver - WARNING - node 4: re-enabling (was disabled)\n"
+ ]
+ }
+ ],
+ "source": [
+ "from pylabrobot.paa.kx2 import KX2\n",
+ "\n",
+ "kx2 = KX2()\n",
+ "await kx2.setup()\n"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "proximity-read-md",
+ "metadata": {},
+ "source": [
+ "An IR breakbeam between the gripper fingers, wired to the Z drive's IO header. The sensor is binary (no distance, just present/absent) and is exposed via the auto-discovered `\"ProximitySensor\"` digital input on the Z axis (pin 4).\n",
+ "\n",
+ "`read_proximity_sensor()` returns `True` when the beam is interrupted. Useful for confirming an object is in the gripper before closing."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 2,
+ "id": "proximity-read",
+ "metadata": {},
+ "outputs": [
+ {
+ "data": {
+ "text/plain": [
+ "False"
+ ]
+ },
+ "execution_count": 2,
+ "metadata": {},
+ "output_type": "execute_result"
+ }
+ ],
+ "source": [
+ "await kx2.arm.backend.read_proximity_sensor()"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "proximity-search-md",
+ "metadata": {},
+ "source": [
+ "`find_z_with_proximity_sensor(z_start, z_end)` pre-positions Z to `z_start`, then descends toward `z_end`, halting the instant the beam trips. The Elmo drive halts the motor itself on the input edge (sub-ms latency, no software in the loop). Returns the Z where the drive halted; raises `RuntimeError` if the beam never tripped over the full `z_start \u2192 z_end` range.\n",
+ "\n",
+ "Position the arm above an object and open the gripper wider than the object before running.\n"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "proximity-search",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "z_hit = await kx2.arm.backend.find_z_with_proximity_sensor(z_start=400.0, z_end=300.0)\n",
+ "print(f\"object found at Z = {z_hit:.2f}\")\n"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "id": "proximity-teardown-header",
+ "metadata": {},
+ "source": [
+ "## Teardown\n"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "id": "proximity-teardown-code",
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "await kx2.stop()\n"
+ ]
+ }
+ ],
+ "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": 5
+}
diff --git a/docs/user_guide/ufactory/xarm6/hello-world.ipynb b/docs/user_guide/ufactory/xarm6/hello-world.ipynb
index ee8ae92a67c..413f8f2430c 100644
--- a/docs/user_guide/ufactory/xarm6/hello-world.ipynb
+++ b/docs/user_guide/ufactory/xarm6/hello-world.ipynb
@@ -29,7 +29,7 @@
{
"cell_type": "markdown",
"id": "xarm6-arm-header",
- "source": "## Arm capabilities\n\nThe xArm 6 exposes an {class}`~pylabrobot.capabilities.arms.articulated_arm.ArticulatedArm` on `xarm.arm`, backed by {class}`~pylabrobot.ufactory.xarm6.backend.XArm6ArmBackend`. For the full arm API, see [Arms](../../capabilities/arms).",
+ "source": "## Arm capabilities\n\nThe xArm 6 exposes an {class}`~pylabrobot.capabilities.arms.articulated_arm.ArticulatedGripperArm` on `xarm.arm`, backed by {class}`~pylabrobot.ufactory.xarm6.backend.XArm6ArmBackend`. For the full arm API, see [Arms](../../capabilities/arms).",
"metadata": {}
},
{
@@ -41,7 +41,7 @@
{
"cell_type": "code",
"id": "xarm6-gripper-code",
- "source": "await xarm.arm.open_gripper(gripper_width=150)",
+ "source": "await xarm.arm.open_gripper()",
"metadata": {},
"execution_count": null,
"outputs": []
@@ -49,7 +49,7 @@
{
"cell_type": "code",
"id": "a730d6d4",
- "source": "await xarm.arm.close_gripper(gripper_width=71)",
+ "source": "await xarm.arm.close_gripper()",
"metadata": {},
"execution_count": null,
"outputs": []
@@ -107,7 +107,7 @@
{
"cell_type": "code",
"id": "xarm6-query-cart",
- "source": "await xarm.arm.backend.request_gripper_location()",
+ "source": "await xarm.arm.backend.request_gripper_pose()",
"metadata": {},
"execution_count": null,
"outputs": []
@@ -171,7 +171,7 @@
{
"cell_type": "code",
"id": "d3d63cd0",
- "source": "# Manually guide the arm to the desired pose, then read it:\ntaught = await xarm.arm.backend.request_gripper_location()\nprint(taught)",
+ "source": "# Manually guide the arm to the desired pose, then read it:\ntaught = await xarm.arm.backend.request_gripper_pose()\nprint(taught)",
"metadata": {},
"execution_count": null,
"outputs": []
diff --git a/pvt.todo.md b/pvt.todo.md
new file mode 100644
index 00000000000..d4f2f951958
--- /dev/null
+++ b/pvt.todo.md
@@ -0,0 +1,185 @@
+# PVT mode — interpolated motion for KX2
+
+## Why bother
+
+`move_to_location` today is joint-linear: `_cart_to_joints(target)` then a single
+`motors_move_joint`. Cart-space path is curved; |J(q)·q_dot| varies along it.
+Capping peak gripper speed = cap leaves mean ≈ 0.5–0.7·cap (peak/mean ratio of
+the path). Validator confirms peak ≤ cap is hit exactly, but avg/cap stays at
+0.1–0.7 even on long moves. There is no fix inside a single trapezoidal profile.
+
+**Goal**: avg gripper speed ≈ peak ≈ cap during cruise. Two ways:
+1. Cartesian-linear interpolation in `move_to_location` (gripper traces a cart
+ straight line; |J·q_dot| ~ constant = cap).
+2. Joint-linear path but time-varying joint velocity in PVT.
+
+Either way, we need PVT (or PP-on-the-fly) on the drive — single-trapezoid PP
+mode can't vary speed mid-move.
+
+## What we already have
+
+`pylabrobot/paa/kx2/driver.py` is more set up than I expected:
+
+- `pvt_select_mode(enable)` — switches mode object `0x6060` between PP (1) and
+ IP (7), and toggles the interpolation-buffer enable (`0x60C4 sub 6`).
+ Currently called with `False` before every PP move (driver.py:842).
+- `setup()` configures Elmo vendor objects for PVT (driver.py:325–332):
+ - `24768.0 = -1` — interpolation submode (Elmo-specific)
+ - `24772.2 = 16` — buffer size (16 deep)
+ - `24772.5 = 8` — points-per-block? (verify in Elmo docs)
+ - `24770.2 = -3` — time-base exponent (10⁻³ s = ms ticks)
+ - `24669.0 = 1` — enable interpolation
+- RPDO3 is already mapped to `(0x60C1.01 TargetPositionIP, 0x60C1.02 TargetVelocityIP)`
+ with EventDrivenDev transmission — so we can shove a new PVT point with one
+ PDO write per axis.
+- TPDO mapping exists for `PVTHeadPointer (0x2F11)` and `PVTTailPointer (0x2F12)`
+ — buffer occupancy is observable.
+
+So the protocol-level scaffolding is in place. The missing pieces are
+**trajectory generation**, **buffer-fill loop**, and **planner integration**.
+
+## Two implementation flavors
+
+### Flavor A — Cartesian-linear `move_to_location`
+
+Scope: `move_to_location` only. `move_to_joint_position` stays joint-linear
+(joint-linear is the right thing for a joint-space move).
+
+1. Sample the cart line at fine resolution (every ~5 mm or so).
+2. IK each sample, snapping wrist to the previous so we don't flip mid-move.
+3. Compute joint velocities at each sample by finite-difference of consecutive
+ joint positions divided by segment time. Time per segment = segment cart
+ length / cap.
+4. Stream `(joint_pos, joint_vel, dt)` per axis into the PVT FIFO.
+
+avg/cap ≈ 1 modulo accel ramps at the endpoints, which we still pay (start at
+v=0, end at v=0). For long moves this overhead is negligible.
+
+### Flavor B — joint-linear with constant-cart-speed PVT
+
+Scope: cap helper. The path is the same joint-linear path the planner already
+uses; just vary joint velocity over time so |J·q_dot| stays at cap.
+
+1. Sample the joint-linear path at high resolution.
+2. At each sample, compute the joint velocity scale that pins gripper speed at
+ cap: `q_dot(α) = q_dot_unit · cap / |J(q(α)) · q_dot_unit|`, capped by
+ firmware ceiling per axis.
+3. Convert to (joint_pos, joint_vel, dt) tuples (dt = arc-length-in-joint /
+ |q_dot|), stream into PVT.
+
+Easier in some ways (no extra IK calls — the trajectory IS the joint line),
+but the gripper's cart path is still curved, so this only fixes avg vs peak,
+not "the gripper moves in a straight line." Probably less useful for actual
+plate handling than A.
+
+**Recommended default: A.** Plate handling cares about predictable cart
+trajectories. B is theoretically interesting but doesn't change *where* the
+gripper goes, only how fast it goes there.
+
+## Implementation sketch
+
+### `KX2Driver` (driver.py)
+
+New methods:
+
+```python
+async def pvt_push_point(
+ self, node_id: int, position: int, velocity: int, dt_ms: int,
+) -> None: ...
+
+async def pvt_buffer_free(self, node_id: int) -> int: ...
+ # Read head - tail (modulo buffer size) from TPDO cache.
+
+async def pvt_start(self, node_ids: List[int]) -> None: ...
+ # Switch to mode 7, enable IP buffer (0x60C4 sub 6 = 1), set CW = enable.
+
+async def pvt_wait_until_drained(self, node_ids: List[int], timeout: float): ...
+ # Done = head == tail and motion complete.
+
+async def pvt_stop(self, node_ids: List[int]) -> None: ...
+ # Disable buffer, switch back to mode 1 (PP).
+```
+
+Buffer fill: tight async loop that watches `pvt_buffer_free()` per axis, pushes
+points whenever there's headroom, sleeps a few ms otherwise. PVT FIFO is 16
+deep per axis — refill should comfortably keep up at the planner-side rates.
+
+### `KX2ArmBackend` (arm_backend.py)
+
+New planner sibling to `calculate_move_abs_all_axes`:
+
+```python
+async def calculate_move_pvt_cartesian(
+ self,
+ start_pose: KX2GripperLocation,
+ end_pose: KX2GripperLocation,
+ cap: float,
+) -> _PVTPlan: ...
+```
+
+Returns per-axis lists of (pos, vel, t) tuples in encoder/firmware units. The
+existing `_profile`-based planner is untouched.
+
+`move_to_location` decision tree:
+- `backend_params.linear_cart_motion = True` (new flag, default True): use PVT.
+- Else: existing joint-linear PP.
+
+`move_to_joint_position` always uses PP. (Joint moves don't have a "straight
+line" semantic.)
+
+### Tests
+
+- New unit test for trajectory generator: feed a known cart line, check
+ resulting joint waypoints reproject (FK) within tolerance to the line.
+- Integration test on real arm: random A→B Cartesian moves at random caps,
+ validator measures peak via fine-resolution FK sweep over the actual
+ PVT joint trajectory; expect avg/cap ≥ 0.9 on moves >100 mm.
+
+## Gotchas / unknowns
+
+- **Segment time quantum.** Vendor object `24770.2 = -3` sets time exponent to
+ 10⁻³ s, so segment dt is in milliseconds. Verify the resolution and the
+ max representable dt in one PDO before designing the segmentation step.
+- **Velocity unit.** `0x60C1.02 TargetVelocityIP` units may differ from
+ `0x60FF Target Velocity` in PV mode (Elmo sometimes uses
+ position-units-per-IP-period vs counts/sec). Check before scaling.
+- **First/last segment.** PVT cubic-Hermite interp wants v at both ends. First
+ segment must start at v=0 (drive is at rest). Last segment must end at v=0.
+ In between, velocity is whatever the cart-speed-at-this-config calls for.
+- **IK continuity.** Wrist sign and J1/J4 unwrapping must stay consistent
+ across consecutive cart samples. `snap_to_current` handles single-call
+ snapping; for streamed points we'd snap each to the *previous PVT sample*,
+ not to current measured joint pos.
+- **Buffer underrun.** If we push slow and the FIFO drains, the drive faults
+ (Elmo MF for IP underflow). Fill aggressively — point preparation is much
+ faster than CAN throughput so we should be safe, but worth instrumenting.
+- **Buffer overrun.** Drive ignores writes past `head + buffer_size`. Must
+ read-back head/tail before each push.
+- **Mid-move halt.** `halt()` must work in PVT mode too — switch back to PP
+ with current commanded position as the new target, or just rely on CW=quick-
+ stop. Verify on hardware.
+- **Sync across axes.** Each axis has its own PVT FIFO; they advance on the
+ drive's IP-period clock. With per-axis EventDrivenDev RPDOs, alignment
+ between axes is best-effort. May need SynchronousCyclic with SYNC frames if
+ jitter shows up. Prototype event-driven first; only switch to SYNC if cart
+ path drifts from the intended line.
+
+## Out of scope / future
+
+- Blending across multiple `move_to_location` calls (so a pick→approach→drop
+ sequence becomes one continuous PVT stream with no stop between sub-moves).
+ Big UX win for assays but a lot more state to manage.
+- Speed/accel limits beyond the gripper-speed cap (per-axis torque/jerk caps).
+- Online replanning if the path is blocked (collision avoidance) — way out of
+ scope, but PVT does enable it because we control velocity in real time.
+
+## References
+
+- CiA 402 §10.4 (Interpolated Position Mode) — defines `0x60C0`, `0x60C1`,
+ `0x60C4`.
+- Elmo Application Note "Interpolated Position Mode (PVT)" — vendor
+ objects 0x21C0–0x21C7 documenting the buffer protocol and mode submodes.
+- Existing PP planner: `KX2ArmBackend.calculate_move_abs_all_axes`
+ (arm_backend.py:759).
+- Existing PVT scaffolding: `pvt_select_mode` (driver.py:842), setup at
+ driver.py:325–347.
diff --git a/pylabrobot/arms/__init__.py b/pylabrobot/arms/__init__.py
deleted file mode 100644
index 41f1ad785ce..00000000000
--- a/pylabrobot/arms/__init__.py
+++ /dev/null
@@ -1,9 +0,0 @@
-import warnings
-
-warnings.warn(
- "Importing from pylabrobot.arms is deprecated. Use pylabrobot.capabilities.arms instead.",
- DeprecationWarning,
- stacklevel=2,
-)
-
-from pylabrobot.capabilities.arms import * # noqa: F401,F403,E402
diff --git a/pylabrobot/arms/architecture.md b/pylabrobot/arms/architecture.md
deleted file mode 100644
index 99ccf47d5ee..00000000000
--- a/pylabrobot/arms/architecture.md
+++ /dev/null
@@ -1,88 +0,0 @@
-# Arms architecture
-
-## Frontend hierarchy (capabilities)
-
-```
-_BaseArm(Capability)
- │ halt(), park(), get_gripper_location()
- │ resource tracking (pick_up/drop state)
- │
- └── GripperArm
- │ open/close_gripper, is_gripper_closed
- │ pick_up/drop/move at location
- │ pick_up_resource(), drop_resource(), move_resource() (convenience)
- │
- └── OrientableArm
- Arm with rotation. E.g. Hamilton iSWAP, PreciseFlex.
- pick_up/drop/move with direction parameter
-```
-
-Frontend mirrors backend hierarchy exactly.
-Joint-space methods are backend-only (robot-specific), accessed via `arm.backend`.
-
-## Backend hierarchy (capability backends)
-
-```
-_BaseArmBackend(CapabilityBackend)
- │ halt(), park(), get_gripper_location()
- │
- ├── GripperArmBackend
- │ open/close_gripper, is_gripper_closed
- │ pick_up/drop/move at location (no rotation)
- │
- ├── OrientableGripperArmBackend
- │ pick_up/drop/move with direction (float degrees)
- │
- └── ArticulatedGripperArmBackend
- pick_up/drop/move with full Rotation
-```
-
-## Mixins (backend)
-
-- `HasJoints` — joint-space control: pick_up/drop/move at joint position, get_joint_position
-- `CanFreedrive` — freedrive (manual guidance) mode
-
-## Concrete implementations
-
-| Device | Driver | Arm Backend | Frontend |
-|--------|--------|-------------|----------|
-| Hamilton STAR (iSWAP) | STARDriver (shared) | `iSWAP(OrientableGripperArmBackend)` | `OrientableArm` |
-| Hamilton STAR (core) | STARDriver (shared) | `CoreGripper(GripperArmBackend)` | `Arm` |
-| PreciseFlex 400 | `PreciseFlexDriver` | `PreciseFlexArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive)` | `OrientableArm` |
-
-## Usage
-
-Arms are capabilities, not devices. They are owned by a Device:
-
-```python
-class STAR(Device):
- def __init__(self, ...):
- driver = STARDriver(...)
- super().__init__(driver=driver)
- self.iswap = OrientableArm(backend=iSWAP(driver), reference_resource=deck)
- self.core_gripper = GripperArm(backend=CoreGripper(driver), reference_resource=deck)
- self._capabilities = [self.iswap, self.core_gripper]
-```
-
-A standalone arm (like PreciseFlex) is a Device with a single arm capability:
-
-```python
-class PreciseFlex400(Device):
- def __init__(
- self, host, port=10100, has_rail=False, timeout=20, gripper_length=162.0, gripper_z_offset=0.0
- ):
- driver = PreciseFlexDriver(host=host, port=port, timeout=timeout)
- super().__init__(driver=driver)
- backend = PreciseFlexArmBackend(
- driver=driver,
- has_rail=has_rail,
- gripper_length=gripper_length,
- gripper_z_offset=gripper_z_offset,
- )
- self.arm = OrientableArm(backend=backend, reference_resource=self.reference)
- self._capabilities = [self.arm]
-
-# Joint methods accessed via backend (robot-specific):
-await pf.arm.backend.move_to_joint_position({1: 0, 2: 90, 3: 45})
-await pf.arm.backend.start_freedrive_mode(free_axes=[0])
-```
diff --git a/pylabrobot/arms/arm.py b/pylabrobot/arms/arm.py
deleted file mode 100644
index b5b4154b806..00000000000
--- a/pylabrobot/arms/arm.py
+++ /dev/null
@@ -1,9 +0,0 @@
-import warnings
-
-warnings.warn(
- "Importing from pylabrobot.arms.arm is deprecated. Use pylabrobot.capabilities.arms.arm instead.",
- DeprecationWarning,
- stacklevel=2,
-)
-
-from pylabrobot.capabilities.arms.arm import * # noqa: F401,F403,E402
diff --git a/pylabrobot/arms/arm_tests.py b/pylabrobot/arms/arm_tests.py
deleted file mode 100644
index 7860c30f343..00000000000
--- a/pylabrobot/arms/arm_tests.py
+++ /dev/null
@@ -1,181 +0,0 @@
-import unittest
-from unittest.mock import AsyncMock, MagicMock
-
-from pylabrobot.arms.arm import GripperArm
-from pylabrobot.arms.backend import (
- GripperArmBackend,
- OrientableGripperArmBackend,
-)
-from pylabrobot.arms.orientable_arm import OrientableArm
-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 = GripperArm(backend=self.mock_backend, reference_resource=self.deck)
-
- async def test_pick_up_resource(self):
- # plate at site_a(100,100,50) + child_loc(5,5,0), center_xy=(60,40), size_z=10
- # pickup_distance_from_bottom=8 → z = 50 + 8 = 58
- await self.arm.pick_up_resource(self.plate, pickup_distance_from_bottom=8)
- 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_bottom=8)
- 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), size_z=10
- # pickup_distance_from_bottom=8 → z = 50 + 8 = 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 = GripperArm(backend=self.mock_backend, reference_resource=self.deck, grip_axis="y")
- await arm_y.pick_up_resource(self.plate, pickup_distance_from_bottom=8)
- 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_bottom=8, 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_bottom=8, 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_bottom=8, 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")
diff --git a/pylabrobot/arms/articulated_arm.py b/pylabrobot/arms/articulated_arm.py
deleted file mode 100644
index 2336046e98e..00000000000
--- a/pylabrobot/arms/articulated_arm.py
+++ /dev/null
@@ -1,10 +0,0 @@
-import warnings
-
-warnings.warn(
- "Importing from pylabrobot.arms.articulated_arm is deprecated. "
- "Use pylabrobot.capabilities.arms.articulated_arm instead.",
- DeprecationWarning,
- stacklevel=2,
-)
-
-from pylabrobot.capabilities.arms.articulated_arm import * # noqa: F401,F403,E402
diff --git a/pylabrobot/arms/backend.py b/pylabrobot/arms/backend.py
deleted file mode 100644
index 204aca34066..00000000000
--- a/pylabrobot/arms/backend.py
+++ /dev/null
@@ -1,10 +0,0 @@
-import warnings
-
-warnings.warn(
- "Importing from pylabrobot.arms.backend is deprecated. "
- "Use pylabrobot.capabilities.arms.backend instead.",
- DeprecationWarning,
- stacklevel=2,
-)
-
-from pylabrobot.capabilities.arms.backend import * # noqa: F401,F403,E402
diff --git a/pylabrobot/arms/orientable_arm.py b/pylabrobot/arms/orientable_arm.py
deleted file mode 100644
index cc0596a6040..00000000000
--- a/pylabrobot/arms/orientable_arm.py
+++ /dev/null
@@ -1,10 +0,0 @@
-import warnings
-
-warnings.warn(
- "Importing from pylabrobot.arms.orientable_arm is deprecated. "
- "Use pylabrobot.capabilities.arms.orientable_arm instead.",
- DeprecationWarning,
- stacklevel=2,
-)
-
-from pylabrobot.capabilities.arms.orientable_arm import * # noqa: F401,F403,E402
diff --git a/pylabrobot/arms/standard.py b/pylabrobot/arms/standard.py
deleted file mode 100644
index 2f653072394..00000000000
--- a/pylabrobot/arms/standard.py
+++ /dev/null
@@ -1,10 +0,0 @@
-import warnings
-
-warnings.warn(
- "Importing from pylabrobot.arms.standard is deprecated. "
- "Use pylabrobot.capabilities.arms.standard instead.",
- DeprecationWarning,
- stacklevel=2,
-)
-
-from pylabrobot.capabilities.arms.standard import * # noqa: F401,F403,E402
diff --git a/pylabrobot/brooks/pf400_test.ipynb b/pylabrobot/brooks/pf400_test.ipynb
index ae984684660..a0a885748cf 100644
--- a/pylabrobot/brooks/pf400_test.ipynb
+++ b/pylabrobot/brooks/pf400_test.ipynb
@@ -3,32 +3,14 @@
{
"cell_type": "markdown",
"metadata": {},
- "source": [
- "# PreciseFlex 400 — Teaching & Freedrive Notebook\n",
- "\n",
- "Uses the `JointArm` frontend with a `PreciseFlex400Backend`."
- ]
+ "source": "# PreciseFlex 400 — Teaching & Freedrive Notebook\n\nUses the `OrientableGripperArm` 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",
- " ElbowOrientation,\n",
- " PFAxis,\n",
- " PreciseFlex400Backend,\n",
- " PreciseFlexBackend,\n",
- ")\n",
- "from pylabrobot.resources import Coordinate, Resource, Rotation\n",
- "\n",
- "POSITIONS_FILE = Path(\"positions.json\")"
- ]
+ "source": "import json\nfrom pathlib import Path\n\nfrom pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm\nfrom pylabrobot.brooks.precise_flex import (\n ElbowOrientation,\n PFAxis,\n PreciseFlex400Backend,\n PreciseFlexBackend,\n)\nfrom pylabrobot.resources import Coordinate, Resource, Rotation\n\nPOSITIONS_FILE = Path(\"positions.json\")"
},
{
"cell_type": "code",
@@ -88,7 +70,7 @@
"execution_count": null,
"metadata": {},
"outputs": [],
- "source": "backend = PreciseFlex400Backend(host=\"192.168.0.1\")\nreference = Resource(\"workcell\", size_x=2000, size_y=2000, size_z=0)\narm = JointArm(backend=backend, reference_resource=reference)\n\nawait arm.setup()\nprint(f\"Version: {await backend.request_version()}\")"
+ "source": "backend = PreciseFlex400Backend(host=\"192.168.0.1\")\nreference = Resource(\"workcell\", size_x=2000, size_y=2000, size_z=0)\narm = OrientableGripperArm(backend=backend, reference_resource=reference)\n\nawait arm.setup()\nprint(f\"Version: {await backend.request_version()}\")"
},
{
"cell_type": "markdown",
@@ -102,7 +84,7 @@
"execution_count": null,
"metadata": {},
"outputs": [],
- "source": "cart = await arm.request_gripper_location()\nprint(f\"Cartesian: x={cart.location.x:.1f}, y={cart.location.y:.1f}, z={cart.location.z:.1f}\")\nprint(\n f\"Rotation: yaw={cart.rotation.z:.1f}, pitch={cart.rotation.y:.1f}, roll={cart.rotation.x:.1f}\"\n)\nprint(f\"Elbow: {cart.orientation}\")\nprint()\njoints = await arm.request_joint_position()\nprint(f\"Joints: {joints}\")"
+ "source": "cart = await arm.request_gripper_pose()\nprint(f\"Cartesian: x={cart.location.x:.1f}, y={cart.location.y:.1f}, z={cart.location.z:.1f}\")\nprint(\n f\"Rotation: yaw={cart.rotation.z:.1f}, pitch={cart.rotation.y:.1f}, roll={cart.rotation.x:.1f}\"\n)\nprint(f\"Elbow: {cart.orientation}\")\nprint()\njoints = await arm.request_joint_position()\nprint(f\"Joints: {joints}\")"
},
{
"cell_type": "markdown",
@@ -128,7 +110,7 @@
"execution_count": null,
"metadata": {},
"outputs": [],
- "source": "# Read position while in freedrive\ncart = await arm.request_gripper_location()\nprint(\n 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}\"\n)"
+ "source": "# Read position while in freedrive\ncart = await arm.request_gripper_pose()\nprint(\n 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}\"\n)"
},
{
"cell_type": "code",
@@ -154,14 +136,14 @@
"execution_count": null,
"metadata": {},
"outputs": [],
- "source": "# Save current position with a name\npos = await arm.request_gripper_location()\nsave_position(\"home\", pos)"
+ "source": "# Save current position with a name\npos = await arm.request_gripper_pose()\nsave_position(\"home\", pos)"
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
- "source": "# Save another position\npos = await arm.request_gripper_location()\nsave_position(\"plate_pickup\", pos)"
+ "source": "# Save another position\npos = await arm.request_gripper_pose()\nsave_position(\"plate_pickup\", pos)"
},
{
"cell_type": "code",
@@ -221,7 +203,7 @@
"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)\nprint(await arm.request_gripper_location())"
+ "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)\nprint(await arm.request_gripper_pose())"
},
{
"cell_type": "markdown",
@@ -237,21 +219,21 @@
"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,\n speed=30,\n ),\n)\nprint(\"Righty:\", await arm.request_gripper_location())"
+ "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,\n speed=30,\n ),\n)\nprint(\"Righty:\", await arm.request_gripper_pose())"
},
{
"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,\n speed=30,\n ),\n)\nprint(\"Lefty:\", await arm.request_gripper_location())"
+ "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,\n speed=30,\n ),\n)\nprint(\"Lefty:\", await arm.request_gripper_pose())"
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
- "source": "await backend.change_config()\nprint(\"Flipped:\", await arm.request_gripper_location())"
+ "source": "await backend.change_config()\nprint(\"Flipped:\", await arm.request_gripper_pose())"
},
{
"cell_type": "markdown",
@@ -279,20 +261,14 @@
"execution_count": null,
"metadata": {},
"outputs": [],
- "source": [
- "await arm.open_gripper(gripper_width=80.0)\n",
- "print(f\"Gripper closed: {await arm.is_gripper_closed()}\")"
- ]
+ "source": "await arm.move_gripper(width=80.0, force_sensing=False)\nprint(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()}\")"
- ]
+ "source": "await arm.move_gripper(width=10.0, force_sensing=True)\nprint(f\"Gripper closed: {await arm.is_gripper_closed()}\")"
},
{
"cell_type": "markdown",
diff --git a/pylabrobot/brooks/precise_flex.py b/pylabrobot/brooks/precise_flex.py
index 3ee098df369..d36a59ada05 100644
--- a/pylabrobot/brooks/precise_flex.py
+++ b/pylabrobot/brooks/precise_flex.py
@@ -14,7 +14,7 @@
HasJoints,
OrientableGripperArmBackend,
)
-from pylabrobot.capabilities.arms.orientable_arm import OrientableArm
+from pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm
from pylabrobot.capabilities.arms.standard import CartesianPose, JointPose
from pylabrobot.capabilities.capability import BackendParams
from pylabrobot.device import Device, Driver
@@ -335,6 +335,7 @@ def __init__(
driver: PreciseFlexDriver,
gripper_length: float,
gripper_z_offset: float,
+ closed_gripper_position: float,
is_dual_gripper: bool = False,
has_rail: bool = False,
) -> None:
@@ -346,6 +347,11 @@ def __init__(
gripper_z_offset: vertical offset in mm from the wrist plate to the tool tip.
Depends on the mounted gripper; the concrete Device wrapper supplies a
model-appropriate default.
+ closed_gripper_position: firmware-unit value (passed to ``GripClosePos`` /
+ ``GripOpenPos``) at which the jaws are at :attr:`min_gripper_width`.
+ Depends on the mounted gripper. The conversion mm → firmware units is
+ linear with slope 1: ``units = closed_gripper_position + (width_mm -
+ min_gripper_width)``.
"""
super().__init__()
self.driver = driver
@@ -356,6 +362,7 @@ def __init__(
self.horizontal_compliance_torque: int = 0
self._has_rail = has_rail
self._is_dual_gripper = is_dual_gripper
+ self.closed_gripper_position = closed_gripper_position
self._kinematics_params = kinematics.PF400Params(
gripper_length=gripper_length, gripper_z_offset=gripper_z_offset
)
@@ -411,21 +418,43 @@ async def _request_speed(self) -> float:
"""Get the current speed percentage of the arm's movement."""
return await self.request_profile_speed(self.profile_index)
- async def open_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = None
- ):
- """Open the gripper to the specified width."""
- logger.info("[PreciseFlex %s] open_gripper: width_mm=%s", self.driver.io._host, gripper_width)
- await self._set_grip_open_pos(gripper_width)
- await self.driver.send_command("gripper 1")
+ # Physical jaw range for the PF400 servoed gripper.
+ min_gripper_width: float = 60.0
+ max_gripper_width: float = 145.0
+
+ def _mm_to_firmware_units(self, width_mm: float) -> float:
+ """Convert a jaw width (mm) to the firmware's native position unit.
+
+ Anchored at :attr:`closed_gripper_position`, which is the firmware value
+ when the jaws are at :attr:`min_gripper_width`. Slope is 1 (1 mm = 1 unit).
+ """
+ return self.closed_gripper_position + (width_mm - self.min_gripper_width)
- async def close_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = None
+ async def move_gripper(
+ self,
+ width: float,
+ force_sensing: bool = False,
+ backend_params: Optional[BackendParams] = None,
):
- """Close the gripper to the specified width."""
- logger.info("[PreciseFlex %s] close_gripper: width_mm=%s", self.driver.io._host, gripper_width)
- await self._set_grip_close_pos(gripper_width)
- await self.driver.send_command("gripper 2")
+ """Move the PreciseFlex gripper jaws.
+
+ ``force_sensing=False`` drives to the open position (``gripper 1``);
+ ``force_sensing=True`` drives to the close position with force feedback
+ (``gripper 2``), which may stop short of ``width`` on contact.
+ """
+ logger.info(
+ "[PreciseFlex %s] move_gripper: width_mm=%s force_sensing=%s",
+ self.driver.io._host,
+ width,
+ force_sensing,
+ )
+ units = self._mm_to_firmware_units(width)
+ if force_sensing:
+ await self._set_grip_close_pos(units)
+ await self.driver.send_command("gripper 2")
+ else:
+ await self._set_grip_open_pos(units)
+ await self.driver.send_command("gripper 1")
async def halt(self, backend_params: Optional[BackendParams] = None):
"""Stops the current robot immediately but leaves power on."""
@@ -566,7 +595,7 @@ async def request_joint_position(
raise PreciseFlexError(-1, "Unexpected response format from wherej command.")
return self._parse_angles_response(parts)
- async def request_gripper_location(
+ async def request_gripper_pose(
self, backend_params: Optional[BackendParams] = None
) -> PreciseFlexCartesianPose:
"""Get the current pose using our kinematics model (no firmware `wherec`)."""
@@ -1713,6 +1742,7 @@ class PreciseFlex400(Device):
def __init__(
self,
host: str,
+ closed_gripper_position: float,
port: int = 10100,
has_rail: bool = False,
timeout: int = 20,
@@ -1721,6 +1751,9 @@ def __init__(
) -> None:
"""
Args:
+ closed_gripper_position: firmware-unit value at which the jaws are at the
+ backend's :attr:`~PreciseFlexArmBackend.min_gripper_width`. Depends on
+ the specific gripper mounted; calibrate before first use.
gripper_length: wrist-axis → TCP distance in mm. Defaults to 162 mm, which
matches the stock single gripper on the PF400.
gripper_z_offset: vertical offset in mm from the wrist plate to the tool tip.
@@ -1734,9 +1767,10 @@ def __init__(
has_rail=has_rail,
gripper_length=gripper_length,
gripper_z_offset=gripper_z_offset,
+ closed_gripper_position=closed_gripper_position,
)
self.reference = Resource(name="PreciseFlex400", size_x=200, size_y=200, size_z=200)
- self.arm = OrientableArm(backend=backend, reference_resource=self.reference)
+ self.arm = OrientableGripperArm(backend=backend, reference_resource=self.reference)
self._capabilities = [self.arm]
@@ -1748,6 +1782,7 @@ def __init__(
driver: PreciseFlexDriver,
gripper_length: float,
gripper_z_offset: float,
+ closed_gripper_position: float,
has_rail: bool = False,
) -> None:
super().__init__(
@@ -1755,4 +1790,5 @@ def __init__(
has_rail=has_rail,
gripper_length=gripper_length,
gripper_z_offset=gripper_z_offset,
+ closed_gripper_position=closed_gripper_position,
)
diff --git a/pylabrobot/brooks/precise_flex_tests.py b/pylabrobot/brooks/precise_flex_tests.py
new file mode 100644
index 00000000000..73fa9496494
--- /dev/null
+++ b/pylabrobot/brooks/precise_flex_tests.py
@@ -0,0 +1,75 @@
+import unittest
+from typing import Tuple
+from unittest.mock import AsyncMock, MagicMock
+
+from pylabrobot.brooks.precise_flex import PreciseFlex400Backend
+
+
+def _make_backend(
+ closed_gripper_position: float = 500.0,
+) -> Tuple[PreciseFlex400Backend, MagicMock]:
+ driver = MagicMock()
+ driver.send_command = AsyncMock(return_value="")
+ driver.io._host = "localhost"
+ backend = PreciseFlex400Backend(
+ driver=driver,
+ gripper_length=162.0,
+ gripper_z_offset=0.0,
+ closed_gripper_position=closed_gripper_position,
+ )
+ return backend, driver
+
+
+class TestPreciseFlex400Gripper(unittest.IsolatedAsyncioTestCase):
+ def setUp(self):
+ # closed_gripper_position=500 ⇒ min_gripper_width(60mm) maps to 500 units.
+ self.backend, self.driver = _make_backend(closed_gripper_position=500.0)
+
+ def _sent_commands(self) -> list[str]:
+ return [c.args[0] for c in self.driver.send_command.call_args_list]
+
+ async def test_move_gripper_force_sensing_false_opens_with_position(self):
+ # 80 mm ⇒ 500 + (80 - 60) = 520 firmware units.
+ await self.backend.move_gripper(width=80.0, force_sensing=False)
+ self.assertEqual(self._sent_commands(), ["GripOpenPos 520.0", "gripper 1"])
+
+ async def test_move_gripper_force_sensing_true_closes_with_position(self):
+ # 60 mm (the closed reference) ⇒ exactly closed_gripper_position.
+ await self.backend.move_gripper(width=60.0, force_sensing=True)
+ self.assertEqual(self._sent_commands(), ["GripClosePos 500.0", "gripper 2"])
+
+ async def test_move_gripper_position_command_precedes_move(self):
+ await self.backend.move_gripper(width=120.0, force_sensing=False)
+ commands = self._sent_commands()
+ self.assertLess(
+ commands.index("GripOpenPos 560.0"),
+ commands.index("gripper 1"),
+ "Position must be set before the gripper move command fires.",
+ )
+
+ async def test_force_sensing_branches_use_different_firmware_commands(self):
+ await self.backend.move_gripper(width=90.0, force_sensing=False)
+ await self.backend.move_gripper(width=90.0, force_sensing=True)
+ commands = self._sent_commands()
+ self.assertIn("gripper 1", commands)
+ self.assertIn("gripper 2", commands)
+ self.assertIn("GripOpenPos 530.0", commands)
+ self.assertIn("GripClosePos 530.0", commands)
+
+ async def test_min_max_gripper_width_advertised(self):
+ self.assertEqual(self.backend.min_gripper_width, 60.0)
+ self.assertEqual(self.backend.max_gripper_width, 145.0)
+
+ async def test_closed_gripper_position_shifts_units(self):
+ # Different anchor ⇒ same width yields a different firmware-unit target.
+ backend, driver = _make_backend(closed_gripper_position=1000.0)
+ await backend.move_gripper(width=80.0, force_sensing=False)
+ commands = [c.args[0] for c in driver.send_command.call_args_list]
+ # 80 mm ⇒ 1000 + (80 - 60) = 1020 units.
+ self.assertEqual(commands, ["GripOpenPos 1020.0", "gripper 1"])
+
+ def test_mm_to_firmware_units_helper(self):
+ # Direct check of the linear mapping.
+ self.assertEqual(self.backend._mm_to_firmware_units(60.0), 500.0)
+ self.assertEqual(self.backend._mm_to_firmware_units(145.0), 585.0)
+ self.assertEqual(self.backend._mm_to_firmware_units(100.0), 540.0)
diff --git a/pylabrobot/capabilities/arms/architecture.md b/pylabrobot/capabilities/arms/architecture.md
index 99ccf47d5ee..de03f85373b 100644
--- a/pylabrobot/capabilities/arms/architecture.md
+++ b/pylabrobot/capabilities/arms/architecture.md
@@ -1,20 +1,52 @@
# Arms architecture
+## Coordinate convention
+
+Backends share one rotation convention so swapping a Hamilton iSWAP for a
+PreciseFlex (or any future arm) doesn't change what `direction` means at
+the call site.
+
+`direction` (and `CartesianPose.rotation.z`) is the world yaw of the
+gripper's *front finger*, in degrees, measured **CCW about world +Z
+(right-hand rule, looking down)** with **0° = +X**:
+
+| `direction` | World axis | `GripperDirection` (deck frame) |
+|------------:|:----------:|:--------------------------------|
+| `0°` | `+X` | `"right"` |
+| `90°` | `+Y` | `"back"` |
+| `180°` | `-X` | `"left"` |
+| `270°` | `-Y` | `"front"` |
+
+`GripperDirection = Literal["front", "back", "left", "right"]` is a
+string-literal alias for these cardinal degrees, used wherever a deck-
+relative label reads better than a raw number.
+
+The frontends accept `direction: Union[GripperDirection, float]` and
+resolve the label to degrees before handing it to the backend, so
+backend implementations only ever see the float — but every backend
+must interpret that float under the convention above.
+
## Frontend hierarchy (capabilities)
```
_BaseArm(Capability)
- │ halt(), park(), get_gripper_location()
+ │ halt(), park(), request_gripper_pose()
│ resource tracking (pick_up/drop state)
│
- └── GripperArm
- │ open/close_gripper, is_gripper_closed
- │ pick_up/drop/move at location
- │ pick_up_resource(), drop_resource(), move_resource() (convenience)
+ └── GripperArm (abstract base for any arm with a gripper)
+ │ move_gripper, open/close_gripper, is_gripper_closed
+ │
+ ├── FixedAxisGripperArm
+ │ Fixed grip axis (x or y). E.g. Hamilton core grippers.
+ │ pick_up/drop/move at location
+ │
+ ├── OrientableGripperArm
+ │ Arm with rotation. E.g. Hamilton iSWAP, PreciseFlex.
+ │ pick_up/drop/move with direction parameter
│
- └── OrientableArm
- Arm with rotation. E.g. Hamilton iSWAP, PreciseFlex.
- pick_up/drop/move with direction parameter
+ └── ArticulatedGripperArm
+ Arm with full 3D rotation. E.g. UFACTORY xArm 6.
+ pick_up/drop/move with Rotation parameter
```
Frontend mirrors backend hierarchy exactly.
@@ -24,10 +56,10 @@ Joint-space methods are backend-only (robot-specific), accessed via `arm.backend
```
_BaseArmBackend(CapabilityBackend)
- │ halt(), park(), get_gripper_location()
+ │ halt(), park(), request_gripper_pose()
│
├── GripperArmBackend
- │ open/close_gripper, is_gripper_closed
+ │ move_gripper, is_gripper_closed, min/max_gripper_width
│ pick_up/drop/move at location (no rotation)
│
├── OrientableGripperArmBackend
@@ -46,9 +78,9 @@ _BaseArmBackend(CapabilityBackend)
| Device | Driver | Arm Backend | Frontend |
|--------|--------|-------------|----------|
-| Hamilton STAR (iSWAP) | STARDriver (shared) | `iSWAP(OrientableGripperArmBackend)` | `OrientableArm` |
-| Hamilton STAR (core) | STARDriver (shared) | `CoreGripper(GripperArmBackend)` | `Arm` |
-| PreciseFlex 400 | `PreciseFlexDriver` | `PreciseFlexArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive)` | `OrientableArm` |
+| Hamilton STAR (iSWAP) | STARDriver (shared) | `iSWAP(OrientableGripperArmBackend)` | `OrientableGripperArm` |
+| Hamilton STAR (core) | STARDriver (shared) | `CoreGripper(GripperArmBackend)` | `FixedAxisGripperArm` |
+| PreciseFlex 400 | `PreciseFlexDriver` | `PreciseFlexArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive)` | `OrientableGripperArm` |
## Usage
@@ -59,8 +91,8 @@ class STAR(Device):
def __init__(self, ...):
driver = STARDriver(...)
super().__init__(driver=driver)
- self.iswap = OrientableArm(backend=iSWAP(driver), reference_resource=deck)
- self.core_gripper = GripperArm(backend=CoreGripper(driver), reference_resource=deck)
+ self.iswap = OrientableGripperArm(backend=iSWAP(driver), reference_resource=deck)
+ self.core_gripper = FixedAxisGripperArm(backend=CoreGripper(driver), reference_resource=deck)
self._capabilities = [self.iswap, self.core_gripper]
```
@@ -79,7 +111,7 @@ class PreciseFlex400(Device):
gripper_length=gripper_length,
gripper_z_offset=gripper_z_offset,
)
- self.arm = OrientableArm(backend=backend, reference_resource=self.reference)
+ self.arm = OrientableGripperArm(backend=backend, reference_resource=self.reference)
self._capabilities = [self.arm]
# Joint methods accessed via backend (robot-specific):
diff --git a/pylabrobot/capabilities/arms/arm.py b/pylabrobot/capabilities/arms/arm.py
index 07a4d973642..a5f8c903868 100644
--- a/pylabrobot/capabilities/arms/arm.py
+++ b/pylabrobot/capabilities/arms/arm.py
@@ -1,9 +1,10 @@
import logging
+from abc import abstractmethod
from dataclasses import dataclass
-from typing import List, Literal, Optional, Tuple, Union
+from typing import List, Literal, Optional, Tuple, Union, cast
-from pylabrobot.capabilities.arms.backend import GripperArmBackend, _BaseArmBackend
-from pylabrobot.capabilities.arms.standard import CartesianPose, GripDirection
+from pylabrobot.capabilities.arms.backend import CanGrip, GripperArmBackend, _BaseArmBackend
+from pylabrobot.capabilities.arms.standard import CartesianPose, GripperDirection
from pylabrobot.capabilities.capability import BackendParams, Capability
from pylabrobot.legacy.tilting.tilter import Tilter
from pylabrobot.resources import (
@@ -20,7 +21,7 @@
logger = logging.getLogger(__name__)
-GripOrientation = Union[GripDirection, float]
+GripperOrientation = Union[GripperDirection, float]
@dataclass
@@ -72,11 +73,11 @@ async def park(self, backend_params: Optional[BackendParams] = None) -> None:
"""Park the arm to its default position."""
return await self.backend.park(backend_params=backend_params)
- async def request_gripper_location(
+ async def request_gripper_pose(
self, backend_params: Optional[BackendParams] = None
) -> CartesianPose:
"""Get the current location and rotation of the gripper."""
- return await self.backend.request_gripper_location(backend_params=backend_params)
+ return await self.backend.request_gripper_pose(backend_params=backend_params)
# -- holding state -----------------------------------------------------------
@@ -297,7 +298,112 @@ def _finalize_drop(
class GripperArm(_BaseArm):
- """A gripper arm without rotation capability. E.g. Hamilton core grippers."""
+ """Abstract base for arms with a gripper.
+
+ Owns the gripper-width controls (:meth:`move_gripper`, :meth:`open_gripper`,
+ :meth:`close_gripper`, :meth:`is_gripper_closed`) shared by all gripper
+ arms.
+
+ Do not instantiate this class directly. Pick/drop/move signatures differ
+ between fixed-axis, orientable, and articulated arms, so subclass one of:
+
+ - :class:`FixedAxisGripperArm` — grips along a single deck-fixed axis
+ (e.g. Hamilton core grippers).
+ - :class:`OrientableGripperArm` — grip direction is a yaw angle
+ (e.g. Hamilton iSWAP, PreciseFlex).
+ - :class:`ArticulatedGripperArm` — full 3D rotation
+ (e.g. UFACTORY xArm 6).
+ """
+
+ async def move_gripper(
+ self,
+ width: float,
+ force_sensing: bool = False,
+ backend_params: Optional[BackendParams] = None,
+ ) -> None:
+ """Move the gripper jaws.
+
+ Args:
+ width: Target jaw width in mm. Must lie within the backend's advertised
+ ``[min_gripper_width, max_gripper_width]`` range; either bound declared
+ as ``None`` is treated as unbounded on that side.
+ force_sensing: If ``True``, close toward ``width`` with force feedback and
+ stop on contact (final width may be larger than ``width``). If
+ ``False``, drive to exactly ``width`` without feedback.
+ backend_params: Optional backend-specific parameters.
+
+ Raises:
+ ValueError: If ``width`` falls outside the advertised range.
+ """
+ backend = cast(CanGrip, self.backend)
+ min_w = backend.min_gripper_width
+ max_w = backend.max_gripper_width
+ if min_w is not None and width < min_w:
+ raise ValueError(
+ f"width {width} mm is below {type(self.backend).__name__}.min_gripper_width ({min_w} mm)."
+ )
+ if max_w is not None and width > max_w:
+ raise ValueError(
+ f"width {width} mm is above {type(self.backend).__name__}.max_gripper_width ({max_w} mm)."
+ )
+ return await backend.move_gripper(
+ width=width, force_sensing=force_sensing, backend_params=backend_params
+ )
+
+ async def open_gripper(self, backend_params: Optional[BackendParams] = None) -> None:
+ """Drive the gripper to its full open width.
+
+ Calls :meth:`move_gripper` with ``width=max_gripper_width`` and
+ ``force_sensing=False``. Raises :class:`NotImplementedError` if the backend
+ does not declare ``max_gripper_width``.
+ """
+ width = self.backend.max_gripper_width
+ if width is None:
+ raise NotImplementedError(
+ f"{type(self.backend).__name__} does not support open_gripper (max_gripper_width is None)."
+ )
+ return await self.backend.move_gripper(
+ width=width, force_sensing=False, backend_params=backend_params
+ )
+
+ async def close_gripper(self, backend_params: Optional[BackendParams] = None) -> None:
+ """Drive the gripper closed with force feedback.
+
+ Calls :meth:`move_gripper` with ``width=min_gripper_width`` and
+ ``force_sensing=True``. Raises :class:`NotImplementedError` if the backend
+ does not declare ``min_gripper_width``.
+ """
+ width = self.backend.min_gripper_width
+ if width is None:
+ raise NotImplementedError(
+ f"{type(self.backend).__name__} does not support close_gripper (min_gripper_width is None)."
+ )
+ return await self.backend.move_gripper(
+ width=width, force_sensing=True, backend_params=backend_params
+ )
+
+ async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool:
+ """Return whether the gripper is currently in the closed state."""
+ return await self.backend.is_gripper_closed(backend_params=backend_params)
+
+ @abstractmethod
+ async def pick_up_resource(self, resource: Resource, *args, **kwargs) -> None:
+ """Pick up a resource. Concrete subclasses define the full signature."""
+
+ @abstractmethod
+ async def drop_resource(self, destination, *args, **kwargs) -> None:
+ """Drop the held resource at a destination. Subclasses define the signature."""
+
+ @abstractmethod
+ async def move_resource(self, resource: Resource, to, *args, **kwargs) -> None:
+ """Pick up, optionally pass through waypoints, and drop. Subclass-defined signature."""
+
+
+class FixedAxisGripperArm(GripperArm):
+ """A gripper arm without rotation capability. E.g. Hamilton core grippers.
+
+ Grips along a single deck-fixed axis chosen at construction time.
+ """
def __init__(
self,
@@ -309,23 +415,6 @@ def __init__(
self.backend: GripperArmBackend = backend
self._grip_axis = grip_axis
- async def open_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = 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[BackendParams] = 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[BackendParams] = None) -> bool:
- return await self.backend.is_gripper_closed(backend_params=backend_params)
-
def _resource_width(self, resource: Resource) -> float:
if self._grip_axis == "y":
return resource.get_absolute_size_y()
diff --git a/pylabrobot/capabilities/arms/arm_tests.py b/pylabrobot/capabilities/arms/arm_tests.py
index 56c65485982..ccc6b773a07 100644
--- a/pylabrobot/capabilities/arms/arm_tests.py
+++ b/pylabrobot/capabilities/arms/arm_tests.py
@@ -1,13 +1,12 @@
import unittest
from unittest.mock import AsyncMock, MagicMock
-from pylabrobot.capabilities.arms.arm import GripperArm
+from pylabrobot.capabilities.arms.arm import FixedAxisGripperArm, GripperArm
from pylabrobot.capabilities.arms.backend import (
GripperArmBackend,
OrientableGripperArmBackend,
)
-from pylabrobot.capabilities.arms.orientable_arm import OrientableArm
-from pylabrobot.capabilities.arms.standard import GripDirection
+from pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm
from pylabrobot.resources import Coordinate, Resource, ResourceHolder
@@ -49,16 +48,17 @@ async def asyncSetUp(self):
"pick_up_at_location",
"drop_at_location",
"move_to_location",
- "open_gripper",
- "close_gripper",
+ "move_gripper",
"is_gripper_closed",
"halt",
"park",
]:
setattr(self.mock_backend, method_name, AsyncMock())
+ self.mock_backend.min_gripper_width = 50.0
+ self.mock_backend.max_gripper_width = 145.0
self.deck, self.site_a, self.site_b, self.plate = _make_deck_with_sites()
- self.arm = GripperArm(backend=self.mock_backend, reference_resource=self.deck)
+ self.arm = FixedAxisGripperArm(backend=self.mock_backend, reference_resource=self.deck)
async def test_pick_up_resource(self):
# plate at site_a(100,100,50) + child_loc(5,5,0), center_xy=(60,40), size_z=10
@@ -100,9 +100,54 @@ async def test_move_to_location(self):
location=location, backend_params=None
)
+ async def test_move_gripper(self):
+ await self.arm.move_gripper(width=80.0, force_sensing=True)
+ self.mock_backend.move_gripper.assert_called_once_with(
+ width=80.0, force_sensing=True, backend_params=None
+ )
+
+ async def test_move_gripper_below_min_raises(self):
+ with self.assertRaises(ValueError):
+ await self.arm.move_gripper(width=10.0)
+ self.mock_backend.move_gripper.assert_not_called()
+
+ async def test_move_gripper_above_max_raises(self):
+ with self.assertRaises(ValueError):
+ await self.arm.move_gripper(width=200.0)
+ self.mock_backend.move_gripper.assert_not_called()
+
+ async def test_move_gripper_at_min_and_max_allowed(self):
+ await self.arm.move_gripper(width=50.0)
+ await self.arm.move_gripper(width=145.0)
+ self.assertEqual(self.mock_backend.move_gripper.call_count, 2)
+
+ async def test_move_gripper_skips_check_when_bound_is_none(self):
+ self.mock_backend.max_gripper_width = None
+ # No upper bound declared → 999 mm passes through.
+ await self.arm.move_gripper(width=999.0)
+ self.mock_backend.move_gripper.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)
+ await self.arm.open_gripper()
+ self.mock_backend.move_gripper.assert_called_once_with(
+ width=145.0, force_sensing=False, backend_params=None
+ )
+
+ async def test_close_gripper(self):
+ await self.arm.close_gripper()
+ self.mock_backend.move_gripper.assert_called_once_with(
+ width=50.0, force_sensing=True, backend_params=None
+ )
+
+ async def test_open_gripper_unsupported(self):
+ self.mock_backend.max_gripper_width = None
+ with self.assertRaises(NotImplementedError):
+ await self.arm.open_gripper()
+
+ async def test_close_gripper_unsupported(self):
+ self.mock_backend.min_gripper_width = None
+ with self.assertRaises(NotImplementedError):
+ await self.arm.close_gripper()
async def test_halt(self):
await self.arm.halt()
@@ -114,7 +159,9 @@ async def test_park(self):
async def test_grip_axis_y(self):
"""With grip_axis='y', resource_width should be the Y size."""
- arm_y = GripperArm(backend=self.mock_backend, reference_resource=self.deck, grip_axis="y")
+ arm_y = FixedAxisGripperArm(
+ backend=self.mock_backend, reference_resource=self.deck, grip_axis="y"
+ )
await arm_y.pick_up_resource(self.plate, pickup_distance_from_bottom=8)
call = self.mock_backend.pick_up_at_location.call_args
# plate size_y=80
@@ -122,7 +169,7 @@ async def test_grip_axis_y(self):
class TestOrientableArm(unittest.IsolatedAsyncioTestCase):
- """Test OrientableArm coordinate computation with fictional resources."""
+ """Test OrientableGripperArm coordinate computation with fictional resources."""
async def asyncSetUp(self):
self.mock_backend = MagicMock(spec=OrientableGripperArmBackend)
@@ -134,25 +181,23 @@ async def asyncSetUp(self):
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)
+ self.arm = OrientableGripperArm(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_bottom=8, direction=GripDirection.FRONT
- )
+ await self.arm.pick_up_resource(self.plate, pickup_distance_from_bottom=8, direction="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
+ # "front" = -Y in deck frame = 270° under the +X-is-zero convention.
+ self.assertAlmostEqual(call.kwargs["direction"], 270.0)
+ # "front" grips along the X axis → 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_bottom=8, direction=GripDirection.RIGHT
- )
+ await self.arm.pick_up_resource(self.plate, pickup_distance_from_bottom=8, direction="right")
call = self.mock_backend.pick_up_at_location.call_args
- self.assertAlmostEqual(call.kwargs["direction"], 90.0)
- # RIGHT → Y width = 80
+ # "right" = +X = 0° under the +X-is-zero convention.
+ self.assertAlmostEqual(call.kwargs["direction"], 0.0)
+ # "right" grips along the Y axis → Y width = 80
self.assertAlmostEqual(call.kwargs["resource_width"], 80)
async def test_drop_at_location(self):
@@ -172,10 +217,16 @@ async def test_move_to_location(self):
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_bottom=8, direction=GripDirection.FRONT
- )
- await self.arm.drop_resource(self.site_b, direction=GripDirection.FRONT)
+ await self.arm.pick_up_resource(self.plate, pickup_distance_from_bottom=8, direction="front")
+ await self.arm.drop_resource(self.site_b, direction="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 TestGripperArmAbstract(unittest.TestCase):
+ def test_cannot_instantiate_abstract_base(self):
+ with self.assertRaises(TypeError):
+ GripperArm(
+ backend=MagicMock(spec=GripperArmBackend), reference_resource=Resource("x", 1, 1, 1)
+ ) # type: ignore[abstract]
diff --git a/pylabrobot/capabilities/arms/articulated_arm.py b/pylabrobot/capabilities/arms/articulated_arm.py
index 752c31bb8eb..863c9d0091f 100644
--- a/pylabrobot/capabilities/arms/articulated_arm.py
+++ b/pylabrobot/capabilities/arms/articulated_arm.py
@@ -1,32 +1,19 @@
from typing import List, Optional, Union
-from pylabrobot.capabilities.arms.arm import _BaseArm, _PickedUpState
+from pylabrobot.capabilities.arms.arm import GripperArm, _PickedUpState
from pylabrobot.capabilities.arms.backend import ArticulatedGripperArmBackend
from pylabrobot.capabilities.capability import BackendParams
from pylabrobot.resources import Coordinate, Resource, ResourceHolder, ResourceStack
from pylabrobot.resources.rotation import Rotation
-class ArticulatedArm(_BaseArm):
- """An arm with full 3D rotation capability. E.g. a 6-axis robot arm."""
+class ArticulatedGripperArm(GripperArm):
+ """A gripper arm with full 3D rotation capability. E.g. a 6-axis robot arm."""
def __init__(self, backend: ArticulatedGripperArmBackend, reference_resource: Resource):
- super().__init__(backend=backend, reference_resource=reference_resource)
+ super().__init__(backend=backend, reference_resource=reference_resource) # type: ignore[arg-type]
self.backend: ArticulatedGripperArmBackend = backend # type: ignore[assignment]
- async def open_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = None
- ) -> None:
- await self.backend.open_gripper(gripper_width=gripper_width, backend_params=backend_params)
-
- async def close_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = None
- ) -> None:
- await self.backend.close_gripper(gripper_width=gripper_width, backend_params=backend_params)
-
- async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool:
- return await self.backend.is_gripper_closed(backend_params=backend_params)
-
@staticmethod
def _resource_width_for_rotation(resource: Resource, rotation: Rotation) -> float:
if rotation.z % 180 == 0:
diff --git a/pylabrobot/capabilities/arms/backend.py b/pylabrobot/capabilities/arms/backend.py
index a05c7cdfa38..bc64c8e5ed1 100644
--- a/pylabrobot/capabilities/arms/backend.py
+++ b/pylabrobot/capabilities/arms/backend.py
@@ -10,12 +10,12 @@
# - pick_up_at_location
# - drop_at_location
# - move_to_location
-# - request_gripper_location
+# - request_gripper_pose
# - is_holding_resource
# CanGrip
-# - open_gripper
-# - close_gripper
+# - min_gripper_width, max_gripper_width
+# - move_gripper(width, force_sensing)
# - is_gripper_closed
# CanSuction
@@ -37,12 +37,18 @@ class CanFreedrive(metaclass=ABCMeta):
@abstractmethod
async def start_freedrive_mode(
- self, free_axes: List[int], backend_params: Optional[BackendParams] = None
+ self,
+ free_axes: Optional[List[int]] = None,
+ backend_params: Optional[BackendParams] = 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.
+ free_axes: List of joint indices to free. ``None`` or ``[0]`` mean
+ the backend's default ("all freeable axes" — typically all motion
+ axes, excluding load-bearing axes like a gripper that's holding a
+ plate). Backends may reject per-axis selection if they only
+ support all-or-nothing freedrive.
"""
@abstractmethod
@@ -88,19 +94,55 @@ async def request_joint_position(
class CanGrip(metaclass=ABCMeta):
- """Mixin for arms that have a gripper."""
+ """Mixin for arms that have a gripper.
+
+ All widths in this interface are in **millimeters**. Backends that drive
+ their hardware in other units (encoder counts, SDK steps, etc.) must convert
+ from mm internally.
+
+ Gripper actuation has two fundamental modes, exposed through a single
+ :meth:`move_gripper` call:
+ - **Positional** (``force_sensing=False``): move the jaws to ``width`` mm
+ without force feedback. The jaws reach exactly that width.
+ - **Grip** (``force_sensing=True``): close toward ``width`` mm but stop on
+ contact. The final width may be larger than the target.
+
+ Backends must also declare the hardware limits of the gripper via
+ :attr:`min_gripper_width` and :attr:`max_gripper_width`.
+ """
+
+ @property
@abstractmethod
- async def open_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = None
- ) -> None:
- """Open the gripper to the specified width."""
+ def min_gripper_width(self) -> Optional[float]:
+ """Smallest jaw width in mm, or ``None`` if the gripper cannot be commanded
+ closed via :meth:`move_gripper` (e.g. closing happens only as part of
+ pick-up)."""
+ @property
@abstractmethod
- async def close_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = None
+ def max_gripper_width(self) -> Optional[float]:
+ """Largest jaw width in mm, or ``None`` if the gripper cannot be commanded
+ open via :meth:`move_gripper`."""
+
+ @abstractmethod
+ async def move_gripper(
+ self,
+ width: float,
+ force_sensing: bool = False,
+ backend_params: Optional[BackendParams] = None,
) -> None:
- """Close the gripper to the specified width."""
+ """Move the gripper jaws to ``width`` mm.
+
+ Args:
+ width: Target jaw width in mm.
+ force_sensing: If ``True``, close with force feedback and stop on contact.
+ If ``False``, drive the jaws to ``width`` without sensing.
+
+ Backends that do not implement force feedback should either raise
+ :class:`NotImplementedError` when ``force_sensing=True`` is requested or
+ document that the flag has no effect on their hardware.
+ """
@abstractmethod
async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool:
@@ -117,7 +159,7 @@ async def park(self, backend_params: Optional[BackendParams] = None) -> None:
"""Park the arm to its default position."""
@abstractmethod
- async def request_gripper_location(
+ async def request_gripper_pose(
self, backend_params: Optional[BackendParams] = None
) -> CartesianPose:
"""Get the current location and rotation of the gripper."""
diff --git a/pylabrobot/capabilities/arms/kinematics.py b/pylabrobot/capabilities/arms/kinematics.py
new file mode 100644
index 00000000000..3e1963619c7
--- /dev/null
+++ b/pylabrobot/capabilities/arms/kinematics.py
@@ -0,0 +1,160 @@
+"""Generic kinematic helpers shared across arm backends.
+
+Per-arm forward/inverse kinematics live in the arm's own module
+(e.g. ``pylabrobot.paa.kx2.kinematics``); this file is for utilities
+that work over *any* forward-kinematics function regardless of DOF
+count, joint topology, or link layout.
+"""
+
+from math import sqrt
+from typing import Callable, Dict, Hashable, Iterator, Tuple, TypeVar
+
+from pylabrobot.resources import Coordinate
+
+
+K = TypeVar("K", bound=Hashable)
+
+
+def gripper_speed(
+ fk: Callable[[Dict[K, float]], Coordinate],
+ joints: Dict[K, float],
+ joint_velocities: Dict[K, float],
+ eps: float = 1e-6,
+) -> float:
+ """Cartesian gripper speed at this joint snapshot.
+
+ Computes |d/dt fk(joints(t))| via a central finite difference along the
+ joint-velocity direction -- two ``fk`` evaluations regardless of DOF
+ count. Equivalent to forming the Jacobian and computing |J*q_dot|, but
+ avoids materializing J.
+
+ Args:
+ fk: forward kinematics, joints dict -> ``Coordinate``.
+ joints: current joint positions, in whatever units ``fk`` expects.
+ joint_velocities: per-joint rate (same units / second). Missing keys
+ are treated as zero, so callers can pass only the moving joints.
+ eps: finite-difference step in joint units. ``Coordinate`` rounds
+ to 0.1 micron, so ``eps * q_dot`` must shift the coordinate by
+ more than that or the diff collapses to zero -- pass ``eps=1e-3``
+ for typical mm/deg-scale arms. Default ``1e-6`` is fine for
+ non-rounding fixtures (tests).
+
+ Returns:
+ Magnitude of the Cartesian velocity in whatever length units ``fk``
+ returns, per second.
+ """
+ plus = fk({k: q + eps * joint_velocities.get(k, 0.0) for k, q in joints.items()})
+ minus = fk({k: q - eps * joint_velocities.get(k, 0.0) for k, q in joints.items()})
+ inv_2eps = 1.0 / (2.0 * eps)
+ dx = (plus.x - minus.x) * inv_2eps
+ dy = (plus.y - minus.y) * inv_2eps
+ dz = (plus.z - minus.z) * inv_2eps
+ return sqrt(dx * dx + dy * dy + dz * dz)
+
+
+def sample_gripper_speed_along_trajectory(
+ fk: Callable[[Dict[K, float]], Coordinate],
+ joints_start: Dict[K, float],
+ joint_deltas: Dict[K, float],
+ joint_velocities: Dict[K, float],
+ num_samples: int = 20,
+ eps: float = 1e-6,
+) -> Iterator[Tuple[float, float]]:
+ """Yield ``(alpha, gripper_speed)`` along the joint-space path
+ ``joints_start + alpha * joint_deltas`` for ``alpha`` in ``[0, 1]``.
+
+ ``joint_deltas`` is the *direction-aware* per-axis change (signed):
+ for unlimited-travel rotary axes the caller should resolve the
+ shortest-way / always-CW / always-CCW convention before passing in, so
+ this helper walks the same path the arm physically takes (e.g. a
+ ShortestWay move from 350 deg to 10 deg has ``delta = +20``, not
+ ``-340``).
+
+ ``joint_velocities`` is held constant across all samples (representing
+ cruise-phase joint rates); each sample returns the gripper speed at
+ that pose.
+
+ Use ``alpha`` to plot, find the worst-case sample, etc.::
+
+ max_speed = max(s for _, s in sample_gripper_speed_along_trajectory(...))
+
+ Missing keys in either ``joints_start`` or ``joint_deltas`` are
+ treated as zero, so callers can pass only moving joints.
+ """
+ if num_samples < 2:
+ raise ValueError(f"num_samples must be >= 2, got {num_samples}")
+ keys = set(joints_start) | set(joint_deltas)
+ for i in range(num_samples):
+ alpha = i / (num_samples - 1)
+ sample = {
+ k: joints_start.get(k, 0.0) + alpha * joint_deltas.get(k, 0.0) for k in keys
+ }
+ yield alpha, gripper_speed(fk, sample, joint_velocities, eps=eps)
+
+
+def joint_velocities_for_max_gripper_speed(
+ fk: Callable[[Dict[K, float]], Coordinate],
+ joints_start: Dict[K, float],
+ joint_deltas: Dict[K, float],
+ joint_max_velocities: Dict[K, float],
+ max_gripper_speed: float,
+ num_samples: int = 20,
+ eps: float = 1e-6,
+) -> Dict[K, float]:
+ """Signed joint velocities such that worst-case gripper speed along
+ the joint-space path ``joints_start + alpha * joint_deltas`` is at
+ most ``max_gripper_speed`` -- or each axis's firmware ceiling,
+ whichever is tighter.
+
+ Sign of each output velocity is taken from ``joint_deltas`` (axes
+ with zero delta get zero velocity). The caller is responsible for
+ resolving any wrap-around / shortest-way / direction conventions when
+ computing ``joint_deltas`` -- this helper just walks the path it's
+ handed.
+
+ Gripper speed is linear in joint velocities (the Jacobian is), so a
+ single sweep determines the scale factor exactly: if running every
+ axis at ``joint_max_velocities`` produces a worst-case path speed
+ ``M``, the returned velocities are scaled by ``min(1, max_gripper_speed / M)``.
+ No iteration.
+
+ Equally applicable to acceleration -- pass joint max accelerations as
+ ``joint_max_velocities`` and the gripper-accel cap as
+ ``max_gripper_speed``; the math is identical because both reduce to
+ ``|J * rate|`` at each pose.
+
+ Args:
+ fk: forward kinematics, joints dict -> ``Coordinate``.
+ joints_start: starting joint positions.
+ joint_deltas: signed per-axis change along the trajectory. Axis sign
+ determines output velocity direction; zero means "don't move".
+ joint_max_velocities: per-axis firmware ceiling, *unsigned*.
+ max_gripper_speed: Cartesian cap, in whatever length units ``fk``
+ returns, per second.
+ num_samples: poses sampled along the path. 20 is plenty for a SCARA.
+ eps: see :func:`gripper_speed`.
+
+ Returns:
+ Dict of *signed* joint velocities, one per key in
+ ``joint_max_velocities``.
+ """
+ signed_max: Dict[K, float] = {}
+ for k in joint_max_velocities:
+ delta = joint_deltas.get(k, 0.0)
+ if delta > 0:
+ signed_max[k] = joint_max_velocities[k]
+ elif delta < 0:
+ signed_max[k] = -joint_max_velocities[k]
+ else:
+ signed_max[k] = 0.0
+
+ max_v = max(
+ s
+ for _, s in sample_gripper_speed_along_trajectory(
+ fk, joints_start, joint_deltas, signed_max, num_samples=num_samples, eps=eps
+ )
+ )
+ if max_v == 0.0:
+ return signed_max
+ scale = min(1.0, max_gripper_speed / max_v)
+ return {k: scale * v for k, v in signed_max.items()}
diff --git a/pylabrobot/capabilities/arms/kinematics_tests.py b/pylabrobot/capabilities/arms/kinematics_tests.py
new file mode 100644
index 00000000000..2a7ca6ccca0
--- /dev/null
+++ b/pylabrobot/capabilities/arms/kinematics_tests.py
@@ -0,0 +1,237 @@
+import math
+import unittest
+from dataclasses import dataclass
+
+from pylabrobot.capabilities.arms.kinematics import (
+ gripper_speed,
+ joint_velocities_for_max_gripper_speed,
+ sample_gripper_speed_along_trajectory,
+)
+
+
+@dataclass
+class _XYZ:
+ """Non-rounding x/y/z holder for tests; Coordinate rounds to 0.1 micron
+ which makes finite differences degenerate."""
+
+ x: float
+ y: float
+ z: float
+
+
+class GripperSpeedTests(unittest.TestCase):
+ def test_static_pose_zero_speed(self):
+ """All velocities zero -> gripper speed zero, regardless of FK."""
+
+ def fk(j):
+ return _XYZ(x=math.cos(j["a"]), y=math.sin(j["a"]), z=0.0)
+
+ self.assertAlmostEqual(gripper_speed(fk, {"a": 1.5}, {"a": 0.0}), 0.0, places=10)
+
+ def test_pure_translation(self):
+ """fk(q) = (q, 0, 0); dx/dt = q_dot => |v| = |q_dot|."""
+
+ def fk(j):
+ return _XYZ(x=j["q"], y=0.0, z=0.0)
+
+ self.assertAlmostEqual(gripper_speed(fk, {"q": 0.0}, {"q": 7.0}), 7.0, places=6)
+ self.assertAlmostEqual(gripper_speed(fk, {"q": 100.0}, {"q": -3.5}), 3.5, places=6)
+
+ def test_unit_circle_tangential_speed(self):
+ """fk(theta) on the unit circle; |v| = r * |theta_dot|. r=1, theta_dot=2 => |v|=2."""
+
+ def fk(j):
+ return _XYZ(x=math.cos(j["t"]), y=math.sin(j["t"]), z=0.0)
+
+ self.assertAlmostEqual(gripper_speed(fk, {"t": 0.7}, {"t": 2.0}), 2.0, places=6)
+
+ def test_circle_with_radius(self):
+ """fk(theta) on a circle of radius 5; |v| = 5 * |theta_dot|."""
+
+ def fk(j):
+ return _XYZ(x=5.0 * math.cos(j["t"]), y=5.0 * math.sin(j["t"]), z=0.0)
+
+ self.assertAlmostEqual(gripper_speed(fk, {"t": 1.2}, {"t": 3.0}), 15.0, places=5)
+
+ def test_two_dof_quadrature(self):
+ """Two independent linear axes; speeds add in quadrature."""
+
+ def fk(j):
+ return _XYZ(x=j["a"], y=j["b"], z=0.0)
+
+ self.assertAlmostEqual(
+ gripper_speed(fk, {"a": 0, "b": 0}, {"a": 3.0, "b": 4.0}), 5.0, places=6
+ )
+
+ def test_missing_velocity_key_is_zero(self):
+ """Velocity keys can be a subset of joint keys."""
+
+ def fk(j):
+ return _XYZ(x=j["a"] + j["b"], y=0.0, z=0.0)
+
+ self.assertAlmostEqual(gripper_speed(fk, {"a": 0, "b": 0}, {"a": 2.0}), 2.0, places=6)
+
+
+class SampleAlongTrajectoryTests(unittest.TestCase):
+ def test_alpha_endpoints(self):
+ """First sample at alpha=0, last at alpha=1."""
+
+ def fk(j):
+ return _XYZ(x=j["q"], y=0.0, z=0.0)
+
+ samples = list(
+ sample_gripper_speed_along_trajectory(
+ fk, joints_start={"q": 0}, joint_deltas={"q": 10}, joint_velocities={"q": 1.0}, num_samples=5
+ )
+ )
+ self.assertEqual(len(samples), 5)
+ self.assertAlmostEqual(samples[0][0], 0.0)
+ self.assertAlmostEqual(samples[-1][0], 1.0)
+
+ def test_constant_speed_along_linear_fk(self):
+ """For a linear fk, speed is constant along the trajectory."""
+
+ def fk(j):
+ return _XYZ(x=j["q"], y=0.0, z=0.0)
+
+ speeds = [
+ s
+ for _, s in sample_gripper_speed_along_trajectory(
+ fk, {"q": -5}, {"q": 10}, {"q": 2.0}, num_samples=10
+ )
+ ]
+ for s in speeds:
+ self.assertAlmostEqual(s, 2.0, places=6)
+
+ def test_max_speed_at_extension_for_radial_arm(self):
+ """fk(elbow, shoulder) = (r * cos, r * sin) where r = elbow.
+
+ With elbow ramping from 1 to 5 (delta=4) and only shoulder moving
+ (theta_dot=1), tangential speed = r * theta_dot grows monotonically;
+ max at the end."""
+
+ def fk(j):
+ r = j["e"]
+ return _XYZ(x=r * math.cos(j["s"]), y=r * math.sin(j["s"]), z=0.0)
+
+ samples = list(
+ sample_gripper_speed_along_trajectory(
+ fk,
+ joints_start={"e": 1.0, "s": 0.0},
+ joint_deltas={"e": 4.0, "s": 0.0},
+ joint_velocities={"s": 1.0},
+ num_samples=20,
+ )
+ )
+ speeds = [s for _, s in samples]
+ self.assertAlmostEqual(speeds[0], 1.0, places=5)
+ self.assertAlmostEqual(speeds[-1], 5.0, places=5)
+ self.assertEqual(max(speeds), speeds[-1])
+
+ def test_wrap_around_path(self):
+ """Direction-aware deltas: a ShortestWay move from 350 deg to 10 deg
+ walks through 0 (delta=+20), not through 180 (delta=-340). The unit
+ circle FK has constant speed = |theta_dot|, but the path is
+ different -- we sanity-check that the start/end poses are correct."""
+
+ def fk(j):
+ r = math.radians(j["t"])
+ return _XYZ(x=math.cos(r), y=math.sin(r), z=0.0)
+
+ samples = list(
+ sample_gripper_speed_along_trajectory(
+ fk, joints_start={"t": 350.0}, joint_deltas={"t": 20.0},
+ joint_velocities={"t": 1.0}, num_samples=11,
+ )
+ )
+ # alpha=0 -> 350 deg (cos=cos(-10), sin=-sin(10))
+ # alpha=1 -> 370 deg = 10 deg (cos=cos(10), sin=sin(10))
+ # alpha=0.5 -> 360 deg = 0 deg (cos=1, sin=0); proves we walked through 0, not 180.
+ mid_speed = samples[5][1]
+ self.assertAlmostEqual(mid_speed, math.radians(1.0), places=6)
+
+ def test_too_few_samples_raises(self):
+ def fk(j):
+ return _XYZ(x=0.0, y=0.0, z=0.0)
+
+ with self.assertRaises(ValueError):
+ list(sample_gripper_speed_along_trajectory(fk, {}, {}, {}, num_samples=1))
+
+
+class JointVelocitiesForMaxGripperSpeedTests(unittest.TestCase):
+ def test_under_cap_returns_signed_max(self):
+ """Cap not binding -> joints run at signed firmware max."""
+
+ def fk(j):
+ return _XYZ(x=j["q"], y=0.0, z=0.0)
+
+ v = joint_velocities_for_max_gripper_speed(
+ fk, joints_start={"q": 0}, joint_deltas={"q": 10},
+ joint_max_velocities={"q": 2.0}, max_gripper_speed=5.0,
+ )
+ self.assertAlmostEqual(v["q"], 2.0, places=6)
+
+ def test_negative_delta_negates_velocity(self):
+ """Negative delta -> velocity is negative."""
+
+ def fk(j):
+ return _XYZ(x=j["q"], y=0.0, z=0.0)
+
+ v = joint_velocities_for_max_gripper_speed(
+ fk, joints_start={"q": 10}, joint_deltas={"q": -10},
+ joint_max_velocities={"q": 2.0}, max_gripper_speed=5.0,
+ )
+ self.assertAlmostEqual(v["q"], -2.0, places=6)
+
+ def test_zero_delta_zero_velocity(self):
+ """Axes with zero delta get velocity 0, regardless of max."""
+
+ def fk(j):
+ return _XYZ(x=j["a"] + j["b"], y=0.0, z=0.0)
+
+ v = joint_velocities_for_max_gripper_speed(
+ fk,
+ joints_start={"a": 0, "b": 5},
+ joint_deltas={"a": 10, "b": 0},
+ joint_max_velocities={"a": 1.0, "b": 1.0},
+ max_gripper_speed=100.0,
+ )
+ self.assertAlmostEqual(v["a"], 1.0, places=6)
+ self.assertAlmostEqual(v["b"], 0.0, places=6)
+
+ def test_over_cap_scales_linearly(self):
+ """firmware_max=10 gives gripper speed 10; cap=2 -> v=0.2*10=2."""
+
+ def fk(j):
+ return _XYZ(x=j["q"], y=0.0, z=0.0)
+
+ v = joint_velocities_for_max_gripper_speed(
+ fk, joints_start={"q": 0}, joint_deltas={"q": 100},
+ joint_max_velocities={"q": 10.0}, max_gripper_speed=2.0,
+ )
+ self.assertAlmostEqual(v["q"], 2.0, places=6)
+
+ def test_radial_arm_caps_at_full_extension(self):
+ """Radial arm with elbow + shoulder both moving. Verifies (a) the cap
+ is met along the path and (b) every axis is scaled by the same
+ factor (proportions to joint_max preserved)."""
+
+ def fk(j):
+ r = j["e"]
+ return _XYZ(x=r * math.cos(j["s"]), y=r * math.sin(j["s"]), z=0.0)
+
+ start = {"e": 0.0, "s": 0.0}
+ delta = {"e": 5.0, "s": 1.0}
+ joint_max = {"e": 1.0, "s": 2.0}
+ v = joint_velocities_for_max_gripper_speed(
+ fk, start, delta, joint_max, max_gripper_speed=6.0, num_samples=50,
+ )
+ max_speed = max(
+ s for _, s in sample_gripper_speed_along_trajectory(fk, start, delta, v, num_samples=50)
+ )
+ self.assertAlmostEqual(max_speed, 6.0, places=3)
+ self.assertAlmostEqual(v["e"] / joint_max["e"], v["s"] / joint_max["s"], places=6)
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/pylabrobot/capabilities/arms/orientable_arm.py b/pylabrobot/capabilities/arms/orientable_arm.py
index 848364c5fb5..0b0888b8d74 100644
--- a/pylabrobot/capabilities/arms/orientable_arm.py
+++ b/pylabrobot/capabilities/arms/orientable_arm.py
@@ -1,59 +1,63 @@
-from typing import List, Optional, Union
+from typing import Dict, List, Optional, Union
-from pylabrobot.capabilities.arms.arm import GripOrientation, _BaseArm, _PickedUpState
+from pylabrobot.capabilities.arms.arm import GripperArm, GripperOrientation, _PickedUpState
from pylabrobot.capabilities.arms.backend import OrientableGripperArmBackend
-from pylabrobot.capabilities.arms.standard import GripDirection
+from pylabrobot.capabilities.arms.standard import (
+ _GRIPPER_DIRECTION_VALUES,
+ GripperDirection,
+)
from pylabrobot.capabilities.capability import BackendParams
from pylabrobot.resources import Coordinate, Resource, ResourceHolder, ResourceStack
from pylabrobot.resources.rotation import Rotation
-_GRIP_DIRECTION_TO_DEGREES = {
- GripDirection.FRONT: 0.0,
- GripDirection.RIGHT: 90.0,
- GripDirection.BACK: 180.0,
- GripDirection.LEFT: 270.0,
+# Cardinal-direction → degrees under the standard ``rotation.z = 0 → +X``
+# convention (CCW about world +Z, right-hand rule looking down). In the
+# PLR deck frame (+X = right, +Y = back), this maps to: right=+X,
+# back=+Y, left=-X, front=-Y. ``front`` lives at 270° (rather than -90°)
+# so the table reads top-to-bottom in CCW order from +X.
+_GRIPPER_DIRECTION_TO_DEGREES: Dict[GripperDirection, float] = {
+ "right": 0.0,
+ "back": 90.0,
+ "left": 180.0,
+ "front": 270.0,
}
-def _resolve_direction(direction: GripOrientation) -> float:
- if isinstance(direction, GripDirection):
- return _GRIP_DIRECTION_TO_DEGREES[direction]
+def _resolve_direction(direction: GripperOrientation) -> float:
+ if isinstance(direction, str):
+ if direction not in _GRIPPER_DIRECTION_VALUES:
+ raise ValueError(
+ f"direction must be one of {sorted(_GRIPPER_DIRECTION_VALUES)} or a float, "
+ f"got {direction!r}"
+ )
+ return _GRIPPER_DIRECTION_TO_DEGREES[direction]
return direction
-class OrientableArm(_BaseArm):
- """An arm with rotation capability. E.g. Hamilton iSWAP."""
+class OrientableGripperArm(GripperArm):
+ """A gripper arm with rotation capability. E.g. Hamilton iSWAP."""
def __init__(self, backend: OrientableGripperArmBackend, reference_resource: Resource):
- super().__init__(backend=backend, reference_resource=reference_resource)
+ super().__init__(backend=backend, reference_resource=reference_resource) # type: ignore[arg-type]
self.backend: OrientableGripperArmBackend = backend # type: ignore[assignment]
- async def open_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = None
- ) -> None:
- await self.backend.open_gripper(gripper_width=gripper_width, backend_params=backend_params)
-
- async def close_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = None
- ) -> None:
- await self.backend.close_gripper(gripper_width=gripper_width, backend_params=backend_params)
-
- async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool:
- return await self.backend.is_gripper_closed(backend_params=backend_params)
-
@staticmethod
def _resource_width_for_direction(resource: Resource, direction: float) -> float:
+ # Front-finger axis is `direction`; jaws come together perpendicular to
+ # it. At 0°/180° the finger points along ±X (right/left), so the jaws
+ # grip the resource along Y; at 90°/270° the finger points along ±Y
+ # (back/front) so they grip along X.
# 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()
+ else:
+ return resource.get_absolute_size_x()
async def pick_up_at_location(
self,
location: Coordinate,
resource_width: float,
- direction: GripOrientation = 0.0,
+ direction: GripperOrientation = 0.0,
backend_params: Optional[BackendParams] = None,
):
if self.holding:
@@ -73,7 +77,7 @@ async def pick_up_resource(
resource: Resource,
offset: Coordinate = Coordinate.zero(),
pickup_distance_from_bottom: Optional[float] = None,
- direction: GripOrientation = GripDirection.FRONT,
+ direction: GripperOrientation = "front",
backend_params: Optional[BackendParams] = None,
):
location, pickup_distance_from_bottom = self._prepare_pickup(
@@ -81,10 +85,7 @@ async def pick_up_resource(
)
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,
@@ -97,7 +98,7 @@ async def pick_up_resource(
async def drop_at_location(
self,
location: Coordinate,
- direction: GripOrientation,
+ direction: GripperOrientation,
backend_params: Optional[BackendParams] = None,
):
if self._holding_resource_width is None:
@@ -114,7 +115,7 @@ async def drop_resource(
self,
destination: Union[ResourceStack, ResourceHolder, Resource, Coordinate],
offset: Coordinate = Coordinate.zero(),
- direction: GripOrientation = GripDirection.FRONT,
+ direction: GripperOrientation = "front",
backend_params: Optional[BackendParams] = None,
):
resource = self._prepare_drop(destination)
@@ -135,7 +136,7 @@ async def drop_resource(
async def move_to_location(
self,
location: Coordinate,
- direction: GripOrientation = 0.0,
+ direction: GripperOrientation = 0.0,
backend_params: Optional[BackendParams] = None,
):
await self.backend.move_to_location(
@@ -147,7 +148,7 @@ async def move_to_location(
async def move_picked_up_resource(
self,
to: Coordinate,
- direction: GripOrientation,
+ direction: GripperOrientation,
offset: Coordinate = Coordinate.zero(),
backend_params: Optional[BackendParams] = None,
):
@@ -169,8 +170,8 @@ async def move_resource(
pickup_offset: Coordinate = Coordinate.zero(),
destination_offset: Coordinate = Coordinate.zero(),
pickup_distance_from_bottom: Optional[float] = None,
- pickup_direction: GripOrientation = GripDirection.FRONT,
- drop_direction: GripOrientation = GripDirection.FRONT,
+ pickup_direction: GripperOrientation = "front",
+ drop_direction: GripperOrientation = "front",
pickup_backend_params: Optional[BackendParams] = None,
drop_backend_params: Optional[BackendParams] = None,
):
diff --git a/pylabrobot/capabilities/arms/standard.py b/pylabrobot/capabilities/arms/standard.py
index 4b3d983d60a..3adfa40c31e 100644
--- a/pylabrobot/capabilities/arms/standard.py
+++ b/pylabrobot/capabilities/arms/standard.py
@@ -1,6 +1,5 @@
-import enum
from dataclasses import dataclass
-from typing import Dict
+from typing import Dict, Literal, get_args
from pylabrobot.resources import Coordinate, Rotation
@@ -15,11 +14,13 @@ class CartesianPose:
rotation: Rotation
-class GripDirection(enum.Enum):
- FRONT = enum.auto()
- BACK = enum.auto()
- LEFT = enum.auto()
- RIGHT = enum.auto()
+# Cardinal directions in the deck frame. String-literal alias for
+# ``OrientableArm`` ``direction`` arguments. Mapped to degrees by
+# :data:`pylabrobot.capabilities.arms.orientable_arm._GRIPPER_DIRECTION_TO_DEGREES`
+# under the standard ``rotation.z = 0 → +X (CCW about +Z)`` convention:
+# ``"right" = 0°``, ``"back" = 90°``, ``"left" = 180°``, ``"front" = 270°``.
+GripperDirection = Literal["front", "back", "left", "right"]
+_GRIPPER_DIRECTION_VALUES = frozenset(get_args(GripperDirection))
@dataclass(frozen=True)
diff --git a/pylabrobot/hamilton/liquid_handlers/star/core.py b/pylabrobot/hamilton/liquid_handlers/star/core.py
index ab119589664..39d9eba4df6 100644
--- a/pylabrobot/hamilton/liquid_handlers/star/core.py
+++ b/pylabrobot/hamilton/liquid_handlers/star/core.py
@@ -24,8 +24,8 @@ def __init__(self, driver: STARDriver):
# -- lifecycle --------------------------------------------------------------
- async def request_gripper_location(self, backend_params=None) -> CartesianPose:
- raise NotImplementedError("CoreGripper does not support request_gripper_location")
+ async def request_gripper_pose(self, backend_params=None) -> CartesianPose:
+ raise NotImplementedError("CoreGripper does not support request_gripper_pose")
# -- ArmBackend interface ---------------------------------------------------
@@ -211,18 +211,30 @@ async def move_to_location(
th=f"{round(backend_params.minimum_traverse_height * 10):04}",
)
- async def open_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = None
- ) -> None:
- """Open the CoRe gripper."""
- await self.driver.send_command(module="C0", command="ZO")
+ # ``min`` is the fully-closed jaw width. ``max`` is None because the CoRe
+ # gripper has no commandable "open" width: the firmware ZO command opens the
+ # jaws fully and ``width`` is therefore ignored.
+ min_gripper_width: float = 9.0
+ max_gripper_width: Optional[float] = None
- async def close_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = None
+ async def move_gripper(
+ self,
+ width: float,
+ force_sensing: bool = False,
+ backend_params: Optional[BackendParams] = None,
) -> None:
- raise NotImplementedError(
- "CoreGripper does not support close_gripper directly. Use pick_up_at_location instead."
- )
+ """Open the CoRe gripper.
+
+ Sends the firmware ZO command, which opens the jaws fully; ``width`` has
+ no effect. Force-sensing closes happen as part of :meth:`pick_up_at_location`
+ and are not exposed here.
+ """
+ if force_sensing:
+ raise NotImplementedError(
+ "CoreGripper does not support force-sensing closes directly. "
+ "Use pick_up_at_location instead."
+ )
+ await self.driver.send_command(module="C0", command="ZO")
async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool:
raise NotImplementedError("CoreGripper does not support is_gripper_closed")
diff --git a/pylabrobot/hamilton/liquid_handlers/star/iswap.py b/pylabrobot/hamilton/liquid_handlers/star/iswap.py
index 9a9b3193aa6..3a70383a537 100644
--- a/pylabrobot/hamilton/liquid_handlers/star/iswap.py
+++ b/pylabrobot/hamilton/liquid_handlers/star/iswap.py
@@ -7,7 +7,7 @@
from typing import TYPE_CHECKING, Literal, Optional, cast
from pylabrobot.capabilities.arms.backend import OrientableGripperArmBackend
-from pylabrobot.capabilities.arms.standard import CartesianPose, GripDirection
+from pylabrobot.capabilities.arms.standard import CartesianPose, GripperDirection
from pylabrobot.capabilities.capability import BackendParams
from pylabrobot.resources import Coordinate
from pylabrobot.resources.rotation import Rotation
@@ -21,11 +21,11 @@
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).
+ User-facing convention: ``rotation.z = 0 → +X (CCW about +Z)``.
+ Firmware: 1 = -Y (front), 2 = +X (right), 3 = +Y (back), 4 = -X (left).
"""
normalized = round(degrees) % 360
- mapping = {0: 1, 90: 2, 180: 3, 270: 4}
+ mapping = {0: 2, 90: 3, 180: 4, 270: 1}
if normalized not in mapping:
raise ValueError(f"grip direction must be a multiple of 90 degrees, got {degrees}")
return mapping[normalized]
@@ -71,7 +71,7 @@ def use_traversal_height(self, height: float):
finally:
self.traversal_height = original
- async def request_gripper_location(self, backend_params=None) -> CartesianPose:
+ async def request_gripper_pose(self, backend_params=None) -> CartesianPose:
"""Request iSWAP grip center position (C0 QG).
Returns:
@@ -224,17 +224,17 @@ async def rotation_drive_request_y(self) -> float:
async def move_x(self, x: float) -> None:
"""Move iSWAP X to an absolute position [mm]."""
- loc = (await self.request_gripper_location()).location
+ loc = (await self.request_gripper_pose()).location
await self.move_relative_x(step_size=x - loc.x, allow_splitting=True)
async def move_y(self, y: float) -> None:
"""Move iSWAP Y to an absolute position [mm]."""
- loc = (await self.request_gripper_location()).location
+ loc = (await self.request_gripper_pose()).location
await self.move_relative_y(step_size=y - loc.y, allow_splitting=True)
async def move_z(self, z: float) -> None:
"""Move iSWAP Z to an absolute position [mm]."""
- loc = (await self.request_gripper_location()).location
+ loc = (await self.request_gripper_pose()).location
await self.move_relative_z(step_size=z - loc.z, allow_splitting=True)
# -- rotation / wrist drive ------------------------------------------------
@@ -301,7 +301,7 @@ async def request_wrist_drive_orientation(self) -> "iSWAPBackend.WristDriveOrien
async def rotate(
self,
rotation_drive: "iSWAPBackend.RotationDriveOrientation",
- grip_direction: GripDirection,
+ grip_direction: GripperDirection,
gripper_velocity: int = 55_000,
gripper_acceleration: int = 170,
gripper_protection: Literal[0, 1, 2, 3, 4, 5, 6, 7] = 5,
@@ -334,16 +334,16 @@ async def rotate(
else:
raise ValueError(f"Invalid rotation drive orientation: {rotation_drive}")
- if grip_direction.value == GripDirection.FRONT.value:
+ if grip_direction == "front":
position += 1
- elif grip_direction.value == GripDirection.RIGHT.value:
+ elif grip_direction == "right":
position += 2
- elif grip_direction.value == GripDirection.BACK.value:
+ elif grip_direction == "back":
position += 3
- elif grip_direction.value == GripDirection.LEFT.value:
+ elif grip_direction == "left":
position += 4
else:
- raise ValueError("Invalid grip direction")
+ raise ValueError(f"Invalid grip direction: {grip_direction!r}")
await self.driver.send_command(
module="R0",
@@ -588,23 +588,14 @@ async def park(self, backend_params: Optional[BackendParams] = None) -> None:
)
self._parked = True
- async def open_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = 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.driver.send_command(module="C0", command="GF", go=f"{round(gripper_width * 10):04}")
+ # Physical jaw range for the iSWAP gripper. The firmware accepts up to 999.9 mm,
+ # but the mechanism cannot actually reach those extremes.
+ min_gripper_width: float = 50.0
+ max_gripper_width: float = 145.0
@dataclass
- class CloseGripperParams(BackendParams):
- """Parameters for closing the iSWAP gripper.
+ class GripParams(BackendParams):
+ """Parameters for a force-sensing close on the iSWAP gripper.
The gripper should be at position plate_width + plate_width_tolerance + 2.0 mm
before sending this command.
@@ -619,22 +610,37 @@ class CloseGripperParams(BackendParams):
grip_strength: int = 5
plate_width_tolerance: float = 2.0
- async def close_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = None
+ async def move_gripper(
+ self,
+ width: float,
+ force_sensing: bool = False,
+ backend_params: Optional[BackendParams] = None,
) -> None:
- """Close the iSWAP gripper.
+ """Move the iSWAP gripper jaws.
Args:
- gripper_width: Plate width [mm].
- backend_params: iSWAP.CloseGripperParams with grip_strength and plate_width_tolerance.
+ width: Target jaw width [mm]. Must be between 0 and 999.9.
+ force_sensing: If True, close with force feedback (C0 GC) and stop on
+ contact. If False, drive the jaws to ``width`` without sensing (C0 GF).
+ backend_params: iSWAP.GripParams. Only valid when ``force_sensing=True``;
+ passing it with ``force_sensing=False`` raises ``ValueError``.
"""
- if not isinstance(backend_params, iSWAPBackend.CloseGripperParams):
- backend_params = iSWAPBackend.CloseGripperParams()
-
+ if not 0 <= width <= 999.9:
+ raise ValueError("width must be between 0 and 999.9")
+
+ if not force_sensing:
+ if backend_params is not None:
+ raise ValueError(
+ "GripParams is only valid with force_sensing=True. "
+ "Drop backend_params or set force_sensing=True."
+ )
+ await self.driver.send_command(module="C0", command="GF", go=f"{round(width * 10):04}")
+ return
+
+ if not isinstance(backend_params, iSWAPBackend.GripParams):
+ backend_params = iSWAPBackend.GripParams()
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.5 <= backend_params.plate_width_tolerance <= 9.9:
raise ValueError("plate_width_tolerance must be between 0.5 and 9.9")
@@ -642,7 +648,7 @@ async def close_gripper(
module="C0",
command="GC",
gw=backend_params.grip_strength,
- gb=f"{round(gripper_width * 10):04}",
+ gb=f"{round(width * 10):04}",
gt=f"{round(backend_params.plate_width_tolerance * 10):02}",
)
diff --git a/pylabrobot/hamilton/liquid_handlers/star/misc/architecture.md b/pylabrobot/hamilton/liquid_handlers/star/misc/architecture.md
index 375a8cf4405..ee01d1fbd2e 100644
--- a/pylabrobot/hamilton/liquid_handlers/star/misc/architecture.md
+++ b/pylabrobot/hamilton/liquid_handlers/star/misc/architecture.md
@@ -30,7 +30,7 @@ STAR (Device) — only exposes Capabilities
│
pip: PIP ──────────► pip backend (above)
head96: Head96 ────► head96 backend (above)
- iswap: OrientableArm ► iswap backend (above)
+ iswap: OrientableGripperArm ► iswap backend (above)
```
The STAR device only exposes Capabilities (PIP, Head96, iSWAP). Subsystems (autoload, x-arms, cover, wash station) and generic driver methods live on `star._driver`.
@@ -231,7 +231,7 @@ await star.setup()
├─ Wire capability frontends to backends (on STAR device)
│ self.pip = PIP(backend=driver.pip)
│ self.head96 = Head96(backend=driver.head96) # if installed
- │ self.iswap = OrientableArm(backend=driver.iswap) # if installed
+ │ self.iswap = OrientableGripperArm(backend=driver.iswap) # if installed
│
└─ Call _on_setup() for each Capability
```
diff --git a/pylabrobot/hamilton/liquid_handlers/star/misc/core_test.ipynb b/pylabrobot/hamilton/liquid_handlers/star/misc/core_test.ipynb
index 5140927f426..d62ce6b96d9 100644
--- a/pylabrobot/hamilton/liquid_handlers/star/misc/core_test.ipynb
+++ b/pylabrobot/hamilton/liquid_handlers/star/misc/core_test.ipynb
@@ -27,7 +27,7 @@
"metadata": {},
"outputs": [],
"source": [
- "from pylabrobot.arms.arm import Arm\n",
+ "from pylabrobot.capabilities.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.corning.plates import Cor_96_wellplate_360ul_Fb\n",
diff --git a/pylabrobot/hamilton/liquid_handlers/star/misc/demo.ipynb b/pylabrobot/hamilton/liquid_handlers/star/misc/demo.ipynb
index f8a4ee0c43e..58b7b276788 100644
--- a/pylabrobot/hamilton/liquid_handlers/star/misc/demo.ipynb
+++ b/pylabrobot/hamilton/liquid_handlers/star/misc/demo.ipynb
@@ -363,7 +363,7 @@
"text/plain": [
"[,\n",
" ,\n",
- " ]"
+ " ]"
]
},
"execution_count": 24,
@@ -447,7 +447,7 @@
"source": [
"### iSWAP: plate gripper arm\n",
"\n",
- "The iSWAP backend is already implemented. The `OrientableArm` frontend provides `pick_up_resource` / `drop_resource` / `move_resource`."
+ "The iSWAP backend is already implemented. The `OrientableGripperArm` frontend provides `pick_up_resource` / `drop_resource` / `move_resource`."
]
},
{
@@ -484,7 +484,7 @@
"id": "6e2ed94e",
"metadata": {},
"source": [
- "Moving using a function that takes any `OrientableArm`:"
+ "Moving using a function that takes any `OrientableGripperArm`:"
]
},
{
@@ -493,7 +493,7 @@
"id": "d5b972f7",
"metadata": {},
"outputs": [],
- "source": "from pylabrobot.arms.orientable_arm import OrientableArm\n\n\nasync def move_resource(resource, destination, arm: OrientableArm):\n await arm.move_resource(resource, destination, pickup_distance_from_bottom=13.2)\n\n\nawait move_resource(plate2, plt_car2[0], arm=star.iswap)\n# await move_resource(plate2, plt_car2[0], arm=pf400.arm)"
+ "source": "from pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm\n\n\nasync def move_resource(resource, destination, arm: OrientableGripperArm):\n await arm.move_resource(resource, destination, pickup_distance_from_bottom=13.2)\n\n\nawait move_resource(plate2, plt_car2[0], arm=star.iswap)\n# await move_resource(plate2, plt_car2[0], arm=pf400.arm)"
},
{
"cell_type": "markdown",
@@ -515,7 +515,7 @@
"text": [
"C0PGid0015th2800\n",
"C0ZTid0016xs13375xd0ya1250yb1070pa07pb08tp2350tz2250th2800tt14\n",
- "arm type: \n",
+ "arm type: \n",
"C0ZPid0017xs04829xd0yj1142yv0050zj1841zy0500yo0885yg0825yw15th2800te2800\n",
"C0ZRid0018xs04829xd0yj3062zj1841zi000zy0500yo0885th2800te2800\n",
"C0ZSid0019xs13375xd0ya1250yb1070tp2150tz2050th2800te2800\n"
diff --git a/pylabrobot/hamilton/liquid_handlers/star/misc/iswap_test.ipynb b/pylabrobot/hamilton/liquid_handlers/star/misc/iswap_test.ipynb
index c540015a33a..77b082c5787 100644
--- a/pylabrobot/hamilton/liquid_handlers/star/misc/iswap_test.ipynb
+++ b/pylabrobot/hamilton/liquid_handlers/star/misc/iswap_test.ipynb
@@ -4,9 +4,9 @@
"cell_type": "markdown",
"metadata": {},
"source": [
- "# iSWAP + OrientableArm: Simple Resource Move Test\n",
+ "# iSWAP + OrientableGripperArm: Simple Resource Move Test\n",
"\n",
- "Tests the `iSWAP` backend through `OrientableArm` with real PLR resources and the real STAR firmware interface."
+ "Tests the `iSWAP` backend through `OrientableGripperArm` with real PLR resources and the real STAR firmware interface."
]
},
{
@@ -24,15 +24,7 @@
"execution_count": null,
"metadata": {},
"outputs": [],
- "source": [
- "from pylabrobot.arms.orientable_arm import OrientableArm\n",
- "from pylabrobot.arms.standard import GripDirection\n",
- "from pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAP\n",
- "from pylabrobot.legacy.liquid_handling.backends.hamilton.STAR_backend import STARBackend\n",
- "from pylabrobot.resources.corning.plates import Cor_96_wellplate_360ul_Fb\n",
- "from pylabrobot.resources.hamilton.hamilton_decks import STARLetDeck\n",
- "from pylabrobot.resources.hamilton.plate_carriers import PLT_CAR_L5AC_A00"
- ]
+ "source": "from pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm\nfrom pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAP\nfrom pylabrobot.legacy.liquid_handling.backends.hamilton.STAR_backend import STARBackend\nfrom pylabrobot.resources.corning.plates import Cor_96_wellplate_360ul_Fb\nfrom pylabrobot.resources.hamilton.hamilton_decks import STARLetDeck\nfrom pylabrobot.resources.hamilton.plate_carriers import PLT_CAR_L5AC_A00"
},
{
"cell_type": "markdown",
@@ -140,7 +132,7 @@
"cmd H0RFid0025\n",
"cmd H0QUid0026\n",
"cmd H0QGid0027\n",
- "OrientableArm ready, iSWAP parked: False\n"
+ "OrientableGripperArm ready, iSWAP parked: False\n"
]
}
],
@@ -151,8 +143,8 @@
"\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}\")"
+ "arm = OrientableGripperArm(backend=iswap_backend, reference_resource=deck)\n",
+ "print(f\"OrientableGripperArm ready, iSWAP parked: {iswap_backend.parked}\")"
]
},
{
@@ -164,33 +156,10 @@
},
{
"cell_type": "code",
- "execution_count": 10,
+ "execution_count": null,
"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}'\")"
- ]
+ "outputs": [],
+ "source": "await arm.pick_up_resource(\n plate,\n direction=\"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)\nprint(f\"Picked up '{plate.name}'\")"
},
{
"cell_type": "code",
@@ -218,37 +187,17 @@
},
{
"cell_type": "code",
- "execution_count": 11,
+ "execution_count": null,
"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()}\")"
- ]
+ "outputs": [],
+ "source": "await arm.drop_resource(carrier[2], direction=\"front\")\nprint(f\"Plate '{plate.name}' is now in: {plate.parent.name}\")\nprint(f\"Plate absolute location: {plate.get_absolute_location()}\")"
},
{
"cell_type": "code",
- "execution_count": 13,
+ "execution_count": null,
"metadata": {},
"outputs": [],
- "source": [
- "await arm.pick_up_resource(\n",
- " plate,\n",
- " direction=GripDirection.LEFT,\n",
- ")\n",
- "await arm.drop_resource(carrier[2], direction=GripDirection.RIGHT)"
- ]
+ "source": "await arm.pick_up_resource(\n plate,\n direction=\"left\",\n)\nawait arm.drop_resource(carrier[2], direction=\"right\")"
},
{
"cell_type": "markdown",
diff --git a/pylabrobot/hamilton/liquid_handlers/star/star.py b/pylabrobot/hamilton/liquid_handlers/star/star.py
index f7c8de6b92d..ba21452b61f 100644
--- a/pylabrobot/hamilton/liquid_handlers/star/star.py
+++ b/pylabrobot/hamilton/liquid_handlers/star/star.py
@@ -4,8 +4,8 @@
from contextlib import asynccontextmanager
from typing import AsyncIterator, Optional
-from pylabrobot.capabilities.arms.arm import GripperArm
-from pylabrobot.capabilities.arms.orientable_arm import OrientableArm
+from pylabrobot.capabilities.arms.arm import FixedAxisGripperArm
+from pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm
from pylabrobot.capabilities.capability import BackendParams
from pylabrobot.capabilities.liquid_handling.head96 import Head96
from pylabrobot.capabilities.liquid_handling.pip import PIP
@@ -33,7 +33,7 @@ def __init__(self, deck: HamiltonDeck, chatterbox: bool = False):
self.deck = deck
self.pip: PIP # set in setup()
self.head96: Optional[Head96] = None # set in setup() if installed
- self.iswap: Optional[OrientableArm] = None # set in setup() if installed
+ self.iswap: Optional[OrientableGripperArm] = None # set in setup() if installed
async def setup(self, backend_params: Optional[BackendParams] = None):
await self.driver.setup(backend_params=backend_params)
@@ -49,7 +49,7 @@ async def setup(self, backend_params: Optional[BackendParams] = None):
# iSWAP only if installed.
if self.driver.iswap is not None:
- self.iswap = OrientableArm(backend=self.driver.iswap, reference_resource=self.deck)
+ self.iswap = OrientableGripperArm(backend=self.driver.iswap, reference_resource=self.deck)
self._capabilities.append(self.iswap)
# Matches legacy: autoload runs in parallel with arm modules.
@@ -85,7 +85,7 @@ async def core_grippers(
front_offset: Coordinate = Coordinate.zero(),
back_offset: Coordinate = Coordinate.zero(),
traversal_height: float = 280.0,
- ) -> AsyncIterator[GripperArm]:
+ ) -> AsyncIterator[FixedAxisGripperArm]:
"""Context manager that picks up CoRe gripper tools on enter and returns them on exit.
Usage::
@@ -121,7 +121,7 @@ async def core_grippers(
)
backend = CoreGripper(driver=self.driver)
- arm = GripperArm(backend=backend, reference_resource=self.deck, grip_axis="y")
+ arm = FixedAxisGripperArm(backend=backend, reference_resource=self.deck, grip_axis="y")
try:
yield arm
diff --git a/pylabrobot/hamilton/liquid_handlers/star/tests/core_tests.py b/pylabrobot/hamilton/liquid_handlers/star/tests/core_tests.py
index fd386a489a2..7f9a4b4b525 100644
--- a/pylabrobot/hamilton/liquid_handlers/star/tests/core_tests.py
+++ b/pylabrobot/hamilton/liquid_handlers/star/tests/core_tests.py
@@ -156,11 +156,15 @@ async def test_move_to_location_custom_params(self):
th="3500",
)
- async def test_open_gripper(self):
- """ZO command."""
- await self.core.open_gripper(gripper_width=0)
+ async def test_move_gripper_position(self):
+ """ZO command. CoreGripper ignores ``width`` and just opens fully."""
+ await self.core.move_gripper(width=0, force_sensing=False)
self.mock_driver.send_command.assert_called_once_with(
module="C0",
command="ZO",
)
+
+ async def test_move_gripper_force_sensing_not_supported(self):
+ with self.assertRaises(NotImplementedError):
+ await self.core.move_gripper(width=80, force_sensing=True)
diff --git a/pylabrobot/hamilton/liquid_handlers/star/tests/iswap_tests.py b/pylabrobot/hamilton/liquid_handlers/star/tests/iswap_tests.py
index c2d9a173fab..7b7674f15b2 100644
--- a/pylabrobot/hamilton/liquid_handlers/star/tests/iswap_tests.py
+++ b/pylabrobot/hamilton/liquid_handlers/star/tests/iswap_tests.py
@@ -16,9 +16,10 @@ async def asyncSetUp(self):
async def test_pick_up_at_location(self):
"""C0PPid0001xs03479xd0yj1142yd0zj1874zd0gr1th2800te2800gw4go1308gb1245gt20ga0gc0"""
+ # direction=270° = -Y = "front" pickup; firmware gr=1 (front).
await self.iswap.pick_up_at_location(
location=Coordinate(347.9, 114.2, 187.4),
- direction=0.0,
+ direction=270.0,
resource_width=127.76,
)
@@ -44,9 +45,10 @@ async def test_pick_up_at_location(self):
async def test_pick_up_grip_direction_left(self):
"""C0PPid0003xs10427xd0yj3286yd0zj2063zd0gr4th2800te2800gw4go1308gb1245gt20ga0gc0"""
+ # direction=180° = -X = "left" pickup; firmware gr=4 (left).
await self.iswap.pick_up_at_location(
location=Coordinate(1042.7, 328.6, 206.3),
- direction=270.0,
+ direction=180.0,
resource_width=127.76,
)
@@ -72,9 +74,10 @@ async def test_pick_up_grip_direction_left(self):
async def test_drop_at_location(self):
"""C0PRid0002xs03479xd0yj3062yd0zj1874zd0th2800te2800gr1go1308ga0gc0"""
+ # direction=270° = -Y = "front" drop; firmware gr=1 (front).
await self.iswap.drop_at_location(
location=Coordinate(347.9, 306.2, 187.4),
- direction=0.0,
+ direction=270.0,
resource_width=127.76,
)
@@ -97,9 +100,10 @@ async def test_drop_at_location(self):
async def test_drop_grip_direction_left(self):
"""C0PRid0002xs10427xd0yj3286yd0zj2063zd0th2800te2800gr4go1308ga0gc0"""
+ # direction=180° = -X = "left" drop; firmware gr=4 (left).
await self.iswap.drop_at_location(
location=Coordinate(1042.7, 328.6, 206.3),
- direction=270.0,
+ direction=180.0,
resource_width=127.76,
)
@@ -139,8 +143,8 @@ async def test_park_custom_height(self):
th=2000,
)
- async def test_open_gripper(self):
- await self.iswap.open_gripper(gripper_width=130.8)
+ async def test_move_gripper_position(self):
+ await self.iswap.move_gripper(width=130.8, force_sensing=False)
self.mock_driver.send_command.assert_called_once_with(
module="C0",
@@ -148,10 +152,11 @@ async def test_open_gripper(self):
go="1308",
)
- async def test_close_gripper(self):
- await self.iswap.close_gripper(
- gripper_width=86.0,
- backend_params=iSWAPBackend.CloseGripperParams(grip_strength=5, plate_width_tolerance=2.0),
+ async def test_move_gripper_grip(self):
+ await self.iswap.move_gripper(
+ width=86.0,
+ force_sensing=True,
+ backend_params=iSWAPBackend.GripParams(grip_strength=5, plate_width_tolerance=2.0),
)
self.mock_driver.send_command.assert_called_once_with(
@@ -162,6 +167,15 @@ async def test_close_gripper(self):
gt="20",
)
+ async def test_move_gripper_rejects_params_without_force_sensing(self):
+ with self.assertRaises(ValueError):
+ await self.iswap.move_gripper(
+ width=130.0,
+ force_sensing=False,
+ backend_params=iSWAPBackend.GripParams(grip_strength=5),
+ )
+ self.mock_driver.send_command.assert_not_called()
+
async def test_is_gripper_closed(self):
self.mock_driver.send_command.return_value = {"ph": 1}
result = await self.iswap.is_gripper_closed()
diff --git a/pylabrobot/legacy/arms/precise_flex/precise_flex_backend.py b/pylabrobot/legacy/arms/precise_flex/precise_flex_backend.py
index 1f68d47e04a..ef6bfb7513e 100644
--- a/pylabrobot/legacy/arms/precise_flex/precise_flex_backend.py
+++ b/pylabrobot/legacy/arms/precise_flex/precise_flex_backend.py
@@ -261,7 +261,7 @@ async def get_joint_position(self) -> Dict[int, float]:
return await self._new_backend.request_joint_position()
async def get_cartesian_position(self) -> PreciseFlexCartesianCoords:
- result = await self._new_backend.request_gripper_location()
+ result = await self._new_backend.request_gripper_pose()
return _from_new_coords(result)
async def send_command(self, command: str) -> str:
diff --git a/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py
index 6c0be7d8b92..97dfef322aa 100644
--- a/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py
+++ b/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py
@@ -6929,7 +6929,7 @@ async def iswap_open_gripper(self, open_position: Optional[float] = None):
assert 0 <= open_position <= 999.9, "open_position must be between 0 and 999.9"
- return await self._iswap.open_gripper(gripper_width=open_position)
+ return await self._iswap.move_gripper(width=open_position, force_sensing=False)
async def iswap_close_gripper(
self,
@@ -6956,9 +6956,10 @@ async def iswap_close_gripper(
from pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAPBackend
- return await self._iswap.close_gripper(
- gripper_width=plate_width,
- backend_params=iSWAPBackend.CloseGripperParams(
+ return await self._iswap.move_gripper(
+ width=plate_width,
+ force_sensing=True,
+ backend_params=iSWAPBackend.GripParams(
grip_strength=grip_strength,
plate_width_tolerance=plate_width_tolerance,
),
@@ -7390,7 +7391,7 @@ async def request_plate_in_iswap(self) -> bool:
async def request_iswap_position(self) -> Coordinate:
"""Deprecated: use ``star.iswap.get_gripper_location()``."""
- return (await self._iswap.request_gripper_location()).location
+ return (await self._iswap.request_gripper_pose()).location
async def iswap_rotation_drive_request_y(self) -> float:
"""Deprecated: use ``star.iswap.rotation_drive_request_y()``."""
diff --git a/pylabrobot/legacy/liquid_handling/liquid_handler.py b/pylabrobot/legacy/liquid_handling/liquid_handler.py
index 8c8931d9606..581138e70d9 100644
--- a/pylabrobot/legacy/liquid_handling/liquid_handler.py
+++ b/pylabrobot/legacy/liquid_handling/liquid_handler.py
@@ -21,8 +21,8 @@
)
from pylabrobot.capabilities.arms.backend import OrientableGripperArmBackend
-from pylabrobot.capabilities.arms.orientable_arm import OrientableArm
-from pylabrobot.capabilities.arms.standard import GripDirection as _NewGripDirection
+from pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm
+from pylabrobot.capabilities.arms.standard import GripperDirection as _NewGripperDirection
from pylabrobot.capabilities.arms.standard import CartesianPose
from pylabrobot.capabilities.liquid_handling.head96 import Head96
from pylabrobot.capabilities.liquid_handling.head96_backend import (
@@ -305,7 +305,12 @@ async def dispense96(
await self._legacy.dispense96(dispense=legacy_disp, **kw)
-_LEGACY_TO_NEW_GRIP = {d: _NewGripDirection[d.name] for d in GripDirection}
+_LEGACY_TO_NEW_GRIP: Dict[GripDirection, _NewGripperDirection] = {
+ GripDirection.FRONT: "front",
+ GripDirection.BACK: "back",
+ GripDirection.LEFT: "left",
+ GripDirection.RIGHT: "right",
+}
class _ArmAdapter(OrientableGripperArmBackend):
@@ -343,8 +348,8 @@ async def halt(self, backend_params=None):
async def park(self, backend_params=None):
pass
- async def request_gripper_location(self, backend_params=None) -> CartesianPose:
- raise NotImplementedError("request_gripper_location not available via legacy adapter")
+ async def request_gripper_pose(self, backend_params=None) -> CartesianPose:
+ raise NotImplementedError("request_gripper_pose not available via legacy adapter")
async def open_gripper(self, gripper_width, backend_params=None):
pass
@@ -392,7 +397,7 @@ def __init__(
# New capability instances — created during setup()
self._lh_cap: Optional[PIP] = None
self._head96_cap: Optional[Head96] = None
- self._arm_cap: Optional[OrientableArm] = None
+ self._arm_cap: Optional[OrientableGripperArm] = None
# Default offset applied to all 96-head operations. Any offset passed to a 96-head method is
# added to this value.
@@ -446,7 +451,9 @@ async def setup(self, **backend_kwargs):
# Create arm capability with adapter backend
if self.backend.num_arms > 0:
- self._arm_cap = OrientableArm(backend=_ArmAdapter(self.backend), reference_resource=self.deck)
+ self._arm_cap = OrientableGripperArm(
+ backend=_ArmAdapter(self.backend), reference_resource=self.deck
+ )
await self._arm_cap._on_setup()
def serialize_state(self) -> Dict[str, Any]:
diff --git a/pylabrobot/paa/__init__.py b/pylabrobot/paa/__init__.py
new file mode 100644
index 00000000000..e69de29bb2d
diff --git a/pylabrobot/paa/kx2/__init__.py b/pylabrobot/paa/kx2/__init__.py
new file mode 100644
index 00000000000..3ff522e3c49
--- /dev/null
+++ b/pylabrobot/paa/kx2/__init__.py
@@ -0,0 +1,25 @@
+from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend
+from pylabrobot.paa.kx2.barcode_reader import KX2BarcodeReader
+from pylabrobot.paa.kx2.config import (
+ Axis,
+ AxisConfig,
+ GripperParams,
+ GripperFingerSide,
+ KX2Config,
+ ServoGripperConfig,
+)
+from pylabrobot.paa.kx2.kinematics import IKError
+from pylabrobot.paa.kx2.kx2 import KX2
+
+__all__ = [
+ "Axis",
+ "AxisConfig",
+ "GripperParams",
+ "GripperFingerSide",
+ "IKError",
+ "KX2",
+ "KX2ArmBackend",
+ "KX2BarcodeReader",
+ "KX2Config",
+ "ServoGripperConfig",
+]
diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py
new file mode 100644
index 00000000000..5bec4e58b6b
--- /dev/null
+++ b/pylabrobot/paa/kx2/arm_backend.py
@@ -0,0 +1,1719 @@
+import asyncio
+import dataclasses
+import logging
+import time
+import warnings
+from contextlib import asynccontextmanager
+from enum import IntEnum
+from typing import Callable, Dict, List, Literal, Optional, Union
+
+from pylabrobot.capabilities.arms.backend import (
+ CanFreedrive,
+ HasJoints,
+ OrientableGripperArmBackend,
+)
+from pylabrobot.capabilities.arms.standard import CartesianPose
+from pylabrobot.capabilities.capability import BackendParams
+from pylabrobot.paa.kx2 import kinematics
+from pylabrobot.paa.kx2.config import (
+ Axis,
+ AxisConfig,
+ GripperFingerSide,
+ GripperParams,
+ KX2Config,
+ ServoGripperConfig,
+)
+from pylabrobot.paa.kx2.driver import (
+ CanError,
+ EmcyFrame,
+ _ElmoObjectDataType,
+ _InputLogic,
+ JointMoveDirection,
+ KX2Driver,
+ MotorMoveParam,
+ MotorsMovePlan,
+)
+from pylabrobot.resources import Coordinate, Rotation
+
+logger = logging.getLogger(__name__)
+
+
+class HomeStatus(IntEnum):
+ NotHomed = 0
+ Homed = 1
+ InitializedWithoutHoming = 2
+
+
+# Tuple of motion axes — derived from `Axis.is_motion`, kept around because
+# iteration sites (setup, halt, freedrive) want a stable ordering.
+MOTION_AXES = tuple(a for a in Axis if a.is_motion)
+
+
+class KX2ArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive):
+ """Arm-capability backend for the PAA KX2.
+
+ Owns a :class:`KX2Driver` (low-level CAN transport) and implements the
+ capability-based arm interface (``OrientableGripperArmBackend`` +
+ ``HasJoints`` + ``CanFreedrive``) directly on top of the drive primitives.
+
+ This layer owns all robot-specific procedural logic: the axis -> node-ID
+ map, the motion/rail/gripper split for `motor_enable`, homing sequences,
+ estop polling, etc. The driver underneath is a pure CAN transport.
+ """
+
+
+ def __init__(
+ self,
+ driver: KX2Driver,
+ gripper_length: float = 0.0,
+ gripper_z_offset: float = 0.0,
+ gripper_finger_side: GripperFingerSide = "barcode_reader",
+ ) -> None:
+ """
+ Args:
+ driver: low-level KX2 CAN transport.
+ gripper_length: distance from the wrist axis to the gripper's
+ *grip center* (geometric midpoint of the jaws) in mm.
+ Non-negative; the side is encoded in ``gripper_finger_side``,
+ not the sign. The gripper assembly hangs off the wrist axis,
+ so for the same target ``(location, direction)`` IK puts the
+ wrist on opposite sides of the grip center for the two
+ finger-side choices (separated by ``2·gripper_length``).
+ gripper_z_offset: vertical offset from the wrist plate to the
+ grip center in mm. Positive = grip center below the wrist plate.
+ gripper_finger_side: which finger is treated as the gripper's
+ "front". The world yaw reported by
+ :meth:`request_gripper_pose` (and the yaw accepted by
+ :meth:`move_to_location`) points at this finger. Flipping
+ side is a 180° relabel of which finger is "front" — for the
+ same joints the grip center is unchanged and only the
+ reported yaw shifts by 180°.
+ """
+ super().__init__()
+ self.driver = driver
+ # Tooling is user-supplied and known at construction; KX2Config (drive-
+ # read calibration) doesn't exist until setup runs.
+ self._gripper_params = GripperParams(
+ length=float(gripper_length),
+ z_offset=float(gripper_z_offset),
+ finger_side=gripper_finger_side,
+ )
+ self._config: Optional[KX2Config] = None
+ self._freedrive_axes: List[int] = []
+ # Reentrant motion guard. Two callers staging move setpoints
+ # (target/vel/accel SDOs) on the same drives interleave and the drives
+ # execute some random mix. Compound ops (pick_up_at_location,
+ # find_z_with_proximity_sensor, _home_servo_gripper) hold this for their full
+ # duration; the inner motion primitive (motors_move_joint /
+ # motors_move_absolute_execute) re-enters via _motion_owner. halt()
+ # deliberately skips the lock — emergency-stop must interrupt regardless.
+ self._motion_lock = asyncio.Lock()
+ self._motion_owner: Optional[asyncio.Task] = None
+
+ @asynccontextmanager
+ async def _motion_guard(self):
+ current = asyncio.current_task()
+ if self._motion_owner is current:
+ yield
+ return
+ async with self._motion_lock:
+ self._motion_owner = current
+ try:
+ yield
+ finally:
+ self._motion_owner = None
+
+ async def _on_setup(self, backend_params: Optional[BackendParams] = None):
+ # Driver has already brought CAN up (connect + node discovery + PDO
+ # mapping) via Device.setup(). Read per-drive config, then enable motion
+ # axes and the servo gripper.
+ #
+ # If anything below this line raises, tear CAN down so a retry can
+ # re-init. Otherwise the second setup() trips PcanCanInitializationError
+ # because the channel is still half-claimed from the first attempt.
+ try:
+ self._config = await self._read_config()
+ await asyncio.sleep(2) # let drives settle before motor enables
+
+ # Subscribe to drive EMCY frames so faults log immediately and motor
+ # disables are scheduled the moment a fault is reported, rather than
+ # waiting for the next motion call to poll motor_get_fault. Mirrors
+ # KX2RobotControl.cs:15384-15425.
+ self.driver.add_emcy_callback(self._on_emcy)
+
+ # E-stop check: front-load a clear error before motor_enable's retry
+ # loop times out with a cryptic message.
+ if await self.get_estop_state():
+ raise RuntimeError(
+ "KX2 setup failed: E-stop is engaged. Twist the red button to "
+ "release, then call setup() again. (If the button is out, the "
+ "safety-interlock loop or motor-power switch may also be open.)"
+ )
+
+ await self.driver.motors_ensure_enabled([int(a) for a in MOTION_AXES])
+ await self.servo_gripper_initialize()
+ except BaseException:
+ try:
+ await self.driver.stop()
+ except Exception:
+ logger.exception("KX2 setup cleanup: driver.stop() failed; ignoring")
+ raise
+
+ def _on_emcy(
+ self, node_id: int, frame: EmcyFrame, description: str, disable_motors: bool
+ ) -> None:
+ """EMCY callback. Runs on the asyncio loop thread.
+
+ Logs every EMCY at error level. When the drive flagged a fatal fault
+ (``disable_motors=True``), schedules an MO=0 on every motion axis as a
+ coroutine — mirrors the C# motor-disable in KX2RobotControl.cs:15395-15404
+ minus the indicator-light I/O (no PLR analog).
+ """
+ logger.error(
+ "KX2 EMCY axis=%d code=0x%04X elmo=0x%02X: %s",
+ node_id, frame.err_code, frame.elmo_err_code, description,
+ )
+ if not disable_motors:
+ return
+
+ async def _disable_all() -> None:
+ for axis in MOTION_AXES:
+ try:
+ await self.driver.motor_emergency_stop(node_id=axis)
+ except Exception:
+ logger.exception("EMCY auto-disable failed for axis %s", axis.name)
+
+ # _on_emcy is invoked from _dispatch_emcy, which already runs on the
+ # asyncio loop (it's the target of call_soon_threadsafe). Schedule on the
+ # driver's captured loop — get_event_loop() is deprecated and unreliable.
+ self.driver.loop.create_task(_disable_all())
+
+ # -- robot-level homing / estop (moved from driver) ---------------------
+
+ async def get_estop_state(self) -> bool:
+ """Return True if the arm is in estop, False otherwise.
+
+ Reads the shoulder drive's SR (status register) via the binary
+ interpreter. Bits 14/15 encode the stop/safety state.
+ """
+ r = await self.driver.query_int(Axis.SHOULDER, "SR", 1)
+ return (r & 0x4000) == 0 or (r & 0x8000) == 0
+
+ async def gripper_get_homed_status(self) -> HomeStatus:
+ return HomeStatus(await self.driver.query_int(Axis.SERVO_GRIPPER, "UI", 3))
+
+ async def _gripper_set_homed_status(self, status: HomeStatus) -> None:
+ await self.driver.write(Axis.SERVO_GRIPPER, "UI", 3, int(status))
+
+ async def _gripper_reset_encoder_position(self, position: float) -> None:
+ sg = Axis.SERVO_GRIPPER
+ await self.driver.write(sg, "HM", 1, 0)
+ await self.driver.write(sg, "HM", 3, 0)
+ await self.driver.write(sg, "HM", 4, 0)
+ await self.driver.write(sg, "HM", 5, 0)
+ # Old code packed `position` as int32 via `int(round(float(str(position))))`;
+ # preserve that rounding semantic for callers that pass fractional values.
+ await self.driver.write(sg, "HM", 2, int(round(position)))
+ await self.driver.write(sg, "HM", 1, 1)
+
+ async def _gripper_hard_stop_search(
+ self, srch_vel: int, srch_acc: int, max_pe: int, hs_pe: int, timeout: float,
+ ) -> None:
+ sg = Axis.SERVO_GRIPPER
+ await self.driver.write(sg, "ER", 3, max_pe * 10)
+ await self.driver.write(sg, "AC", 0, srch_acc)
+ await self.driver.write(sg, "DC", 0, srch_acc)
+ for i in [3, 4, 5, 2]:
+ await self.driver.write(sg, "HM", i, 0)
+ await self.driver.write(sg, "JV", 0, srch_vel)
+
+ try:
+ params: List[Union[int, float]] = [int(hs_pe), int(timeout * 1000)]
+ last_line = await self.driver.user_program_run(
+ sg, "Home", params, int(timeout), True
+ )
+ if last_line in [1, 2, 3]:
+ raise RuntimeError(f"Homing Script Error {34 + last_line}")
+
+ curr_pos = await self.driver.motor_get_current_position(sg)
+ await self.driver.write(sg, "PA", 0, curr_pos)
+ await self.driver.write(sg, "SP", 0, srch_vel)
+ await self.driver.write(sg, "AC", 0, srch_acc)
+ await self.driver.write(sg, "DC", 0, srch_acc)
+ finally:
+ await asyncio.sleep(0.3)
+ await self.driver.execute(sg, "BG", 0)
+ await asyncio.sleep(0.3)
+ await self.driver.write(sg, "ER", 3, int(max_pe))
+
+ async def _gripper_index_search(
+ self, srch_vel: int, srch_acc: int, positive_direction: bool, timeout: float,
+ ) -> tuple:
+ sg = Axis.SERVO_GRIPPER
+ await self.driver.write(sg, "HM", 1, 0)
+
+ one_revolution = await self.driver.query_int(sg, "CA", 18)
+ if not positive_direction:
+ one_revolution *= -1
+
+ await self.driver.write(sg, "PR", 1, one_revolution)
+ await self.driver.write(sg, "SP", 0, srch_vel)
+ await self.driver.write(sg, "AC", 0, srch_acc)
+ await self.driver.write(sg, "DC", 0, srch_acc)
+
+ await self.driver.write(sg, "HM", 3, 3) # index only
+ await self.driver.write(sg, "HM", 4, 0)
+ await self.driver.write(sg, "HM", 5, 0)
+ await self.driver.write(sg, "HM", 2, 0)
+ await self.driver.write(sg, "HM", 1, 1) # arm
+
+ await self.driver.execute(sg, "BG", 0)
+ await self.driver.wait_for_moves_done([sg], timeout)
+
+ left = await self.driver.query_int(sg, "HM", 1)
+ if left != 0:
+ raise RuntimeError("Homing Failure: Failed to finish index pulse search.")
+
+ captured_position = await self.driver.query_int(sg, "HM", 7)
+ return one_revolution, captured_position
+
+ async def _home_servo_gripper(self, sgc: ServoGripperConfig) -> None:
+ """Hard-stop + index-pulse home for the servo gripper."""
+ sg = Axis.SERVO_GRIPPER
+ timeout = sgc.home_timeout_msec / 1000
+
+ async with self._motion_guard():
+ ca41 = await self.driver.query_int(sg, "CA", 41)
+ if ca41 == 24:
+ raise RuntimeError(
+ f"Servo gripper not ready to home (drive reports CA[41]={ca41}). "
+ "Power-cycle the drive or re-check homing config."
+ )
+
+ try:
+ await self._gripper_hard_stop_search(
+ sgc.home_search_vel, sgc.home_search_accel,
+ sgc.home_default_position_error, sgc.home_hard_stop_position_error,
+ timeout,
+ )
+ except Exception as e:
+ # motor_check_if_move_done already raised with the rich EMCY
+ # description ("Motor Fault: ..."). Don't overwrite it with the
+ # duller MF-bit register read.
+ if str(e).startswith("Motor Fault:"):
+ raise
+ fault = await self.driver.motor_get_fault(sg)
+ if fault is not None:
+ raise RuntimeError(fault) from e
+ raise
+
+ await self.driver.motor_enable(node_id=sg, state=True, use_ds402=False)
+
+ await self._motors_move_absolute_execute_locked(
+ plan=MotorsMovePlan(moves=[MotorMoveParam(
+ node_id=sg, position=sgc.home_hard_stop_offset,
+ velocity=sgc.home_offset_vel, acceleration=sgc.home_offset_accel,
+ relative=False, direction=JointMoveDirection.ShortestWay,
+ )])
+ )
+
+ is_positive = sgc.home_hard_stop_offset > 0
+ await self._gripper_index_search(
+ abs(sgc.home_search_vel), sgc.home_search_accel, is_positive, timeout,
+ )
+
+ await self._motors_move_absolute_execute_locked(
+ plan=MotorsMovePlan(moves=[MotorMoveParam(
+ node_id=sg, position=sgc.home_index_offset,
+ velocity=sgc.home_offset_vel, acceleration=sgc.home_offset_accel,
+ relative=False, direction=JointMoveDirection.ShortestWay,
+ )])
+ )
+ await self._gripper_reset_encoder_position(sgc.home_pos)
+ await self._gripper_set_homed_status(HomeStatus.Homed)
+
+ # -- servo gripper ------------------------------------------------------
+
+ async def servo_gripper_initialize(self):
+ # Don't swallow motor_enable failures here — homing is the next step
+ # and will fault with a confusing "homing failure" error if the motor
+ # never came up. Better to surface the real cause.
+ await self.driver.motor_enable(
+ node_id=Axis.SERVO_GRIPPER, state=True, use_ds402=False
+ )
+ await self.servo_gripper_home()
+ await self.servo_gripper_close()
+
+ async def servo_gripper_home(self) -> None:
+ sgc = self._cfg.servo_gripper
+ if sgc is None:
+ raise RuntimeError("Servo gripper not present")
+ sg = Axis.SERVO_GRIPPER
+ await self.driver.write(sg, "PL", 1, sgc.peak_current)
+ await self.driver.write(sg, "CL", 1, sgc.continuous_current)
+
+ await self._home_servo_gripper(sgc)
+
+ await self._set_servo_gripper_force_limit(100)
+
+ async def _set_servo_gripper_force_limit(self, max_force_percent: int) -> None:
+ """Scale CL (continuous) and PL (peak) current limits to the given
+ percentage of the gripper's full current rating. Clamped to [10, 100].
+
+ The drive enforces an interlock: PL can be lowered to a value below CL
+ only if CL is lowered first, and CL can only be raised when PL is at or
+ above. So: bump PL to full → set CL → set PL to scaled.
+ """
+ sgc = self._cfg.servo_gripper
+ if sgc is None:
+ raise RuntimeError("Servo gripper not present")
+ max_force_percent = max(10, min(max_force_percent, 100))
+
+ cont_current = sgc.continuous_current * max_force_percent / 100.0
+ peak_current = sgc.peak_current * max_force_percent / 100.0
+
+ sg = Axis.SERVO_GRIPPER
+ await self.driver.write(sg, "PL", 1, sgc.peak_current)
+ await self.driver.write(sg, "CL", 1, cont_current)
+ await self.driver.write(sg, "PL", 1, peak_current)
+
+ async def _get_servo_gripper_force_fraction(self) -> float:
+ """Return |IQ| / CL clamped to [0, 1] — the fraction of the configured
+ continuous current the gripper is currently drawing."""
+ sg = Axis.SERVO_GRIPPER
+ cl = await self.driver.query_float(sg, "CL", 1)
+ iq = await self.driver.query_float(sg, "IQ", 0)
+
+ if cl == 0:
+ return 0.0
+
+ return max(0.0, min(abs(iq / cl), 1.0))
+
+ async def check_plate_gripped(self, num_attempts: int = 5) -> None:
+ for _ in range(num_attempts):
+ motor_status = await self.driver.query_int(
+ Axis.SERVO_GRIPPER, "MS", 1
+ )
+ logger.debug("Servo gripper motor status: %s", motor_status)
+
+ if motor_status in {0, 1}:
+ max_force_percentage = await self._get_servo_gripper_force_fraction()
+ if max_force_percentage > 90:
+ return
+ await asyncio.sleep(0.5)
+ max_force_percentage = await self._get_servo_gripper_force_fraction()
+ if max_force_percentage > 90:
+ return
+
+ current_position = await self.motor_get_current_position(Axis.SERVO_GRIPPER)
+ closed_position = 1
+ if abs(current_position - closed_position) < 2.0 / 625:
+ raise RuntimeError(
+ "Servo Gripper was able to move all the way to the closed position, which indicates the absence of an object in the gripper. The closed position value may need to be decreased."
+ )
+
+ return
+
+ elif motor_status == 2:
+ motor_fault = await self.driver.motor_get_fault(Axis.SERVO_GRIPPER)
+ if motor_fault is None:
+ raise RuntimeError("Error querying whether plate is gripped. Error querying motor fault.")
+ raise RuntimeError(
+ f"Servo Gripper may not have gripped the plate correctly. Motor fault: '{motor_fault}'"
+ )
+
+ await asyncio.sleep(0.05)
+
+ raise RuntimeError(
+ f"Servo Gripper was unable to confirm that the plate is gripped after {num_attempts} attempts."
+ )
+
+ async def servo_gripper_close(self, closed_position: int = 0, check_plate_gripped=True) -> None:
+ async with self._motion_guard():
+ await self._motors_move_joint_locked({Axis.SERVO_GRIPPER: closed_position})
+ if check_plate_gripped:
+ await self.check_plate_gripped()
+
+ async def servo_gripper_open(self, open_position: float) -> None:
+ async with self._motion_guard():
+ await self._motors_move_joint_locked({Axis.SERVO_GRIPPER: open_position})
+
+ async def drive_set_move_count_parameters(
+ self,
+ move_count: int,
+ travel: List[float],
+ last_maintenance_performed: float,
+ maintenance_required: bool,
+ last_maintenance_performed_date: int,
+ last_maintenance_performed_rail: float,
+ maintenance_required_rail: bool,
+ last_maintenance_performed_date_rail: int,
+ ) -> None:
+ z = Axis.Z
+
+ # MoveCount -> Z axis, UI index 22
+ await self.driver.write(z, "UI", 22, int(move_count))
+
+ # Travel[] -> each node, UF index 5
+ # The source looked 1-based for Travel and 0-based for NodeIDList; handle both cleanly.
+ if len(travel) == len(self._cfg.axes) + 1:
+ pairs = zip(self._cfg.axes, travel[1:])
+ else:
+ pairs = zip(self._cfg.axes, travel)
+
+ for axis_key, dist in pairs:
+ await self.driver.write(int(axis_key), "UF", 5, float(dist))
+
+ # LastMaintenancePerformed -> Z axis, UF index 6
+ await self.driver.write(z, "UF", 6, float(last_maintenance_performed))
+
+ # MaintenanceRequired -> Z axis, UI index 23
+ await self.driver.write(z, "UI", 23, 1 if maintenance_required else 0)
+
+ # LastMaintenancePerformedDate -> Z axis, UI index 21
+ await self.driver.write(z, "UI", 21, int(last_maintenance_performed_date))
+
+ # Rail (if present)
+ if self._cfg.robot_on_rail:
+ rail = Axis.RAIL
+ await self.driver.write(rail, "UF", 6, float(last_maintenance_performed_rail))
+ await self.driver.write(rail, "UI", 23, 1 if maintenance_required_rail else 0)
+ await self.driver.write(rail, "UI", 21, int(last_maintenance_performed_date_rail))
+
+ async def _read_config(self) -> KX2Config:
+ """Read the per-arm configuration from the drives.
+
+ Driver discovery has already populated `node_id_list` with everything
+ on the bus; here we just verify the required motion axes are present
+ and read each drive's parameters.
+ """
+ nodes = self.driver.node_id_list
+ for required in MOTION_AXES:
+ if required not in nodes:
+ raise CanError(f"Missing required axis {required}")
+ has_rail = Axis.RAIL in nodes
+ has_servo_gripper = Axis.SERVO_GRIPPER in nodes
+ if has_rail:
+ warnings.warn("Rails has not been tested for KX2 robots.")
+
+ axes: Dict[Axis, AxisConfig] = {}
+ for nid in nodes:
+ axes[Axis(nid)] = await self._read_axis_config(nid)
+
+ sh = Axis.SHOULDER
+ return KX2Config(
+ wrist_offset=await self.driver.query_float(sh, "UF", 8),
+ elbow_offset=await self.driver.query_float(sh, "UF", 9),
+ elbow_zero_offset=await self.driver.query_float(sh, "UF", 10),
+ axes=axes,
+ base_to_gripper_clearance_z=await self.driver.query_float(sh, "UF", 6),
+ base_to_gripper_clearance_arm=await self.driver.query_float(sh, "UF", 7),
+ robot_on_rail=has_rail,
+ servo_gripper=await self._read_servo_gripper_config() if has_servo_gripper else None,
+ )
+
+ async def _read_axis_config(self, nid: int) -> AxisConfig:
+ logger.debug("Reading parameters for axis %s", nid)
+
+ digital_inputs = await self._read_io_names(nid, 5, 11, _DIGITAL_INPUT_NAMES)
+ analog_inputs = await self._read_io_names(nid, 11, 13, {})
+ outputs = await self._read_io_names(nid, 13, 17, _OUTPUT_NAMES)
+
+ await self.driver.query_int(nid, "UI", 24) # serial — read for parity, unused
+
+ uf1 = await self.driver.query_float(nid, "UF", 1)
+ uf2 = await self.driver.query_float(nid, "UF", 2)
+ if uf1 == 0.0 or uf2 == 0.0:
+ raise CanError(f"Invalid motor conversion factor for axis {nid}: UF[1]={uf1}, UF[2]={uf2}")
+ motor_conversion_factor = uf1 / uf2
+
+ xm1 = await self.driver.query_int(nid, "XM", 1)
+ xm2 = await self.driver.query_int(nid, "XM", 2)
+ max_travel = await self.driver.query_float(nid, "UF", 3)
+ min_travel = await self.driver.query_float(nid, "UF", 4)
+ vh3 = await self.driver.query_int(nid, "VH", 3)
+ vl3 = await self.driver.query_int(nid, "VL", 3)
+
+ joint_move_direction = JointMoveDirection.Normal
+ if (xm1 == 0 and xm2 == 0) or (xm1 <= vl3 and xm2 >= vh3):
+ unlimited_travel = False
+ elif xm1 > vl3 and xm2 < vh3:
+ unlimited_travel = True
+ if Axis(nid).is_motion:
+ joint_move_direction = JointMoveDirection.ShortestWay
+ else:
+ raise CanError(
+ f"Invalid travel limits or modulo settings for axis {nid}: "
+ f"VH[3]={vh3}, VL[3]={vl3}, XM[1]={xm1}, XM[2]={xm2}"
+ )
+
+ ca45 = await self.driver.query_int(nid, "CA", 45)
+ if not (0 < ca45 <= 4):
+ raise CanError(f"Invalid encoder socket for axis {nid}: CA[45]={ca45}")
+ enc_type = await self.driver.query_int(nid, "CA", 40 + ca45)
+ if enc_type in (1, 2):
+ absolute_encoder = False
+ elif enc_type == 24:
+ absolute_encoder = True
+ else:
+ raise CanError(f"Unsupported encoder type for axis {nid}: CA[4{ca45}]={enc_type}")
+
+ ca46 = await self.driver.query_int(nid, "CA", 46)
+ num3 = 1.0 if ca45 == ca46 else await self.driver.query_float(nid, "FF", 3)
+ denom = motor_conversion_factor * num3
+
+ sp2 = await self.driver.query_int(nid, "SP", 2)
+ if sp2 == 100000:
+ max_vel = await self.driver.query_int(nid, "VH", 2) / 1.01 / denom
+ else:
+ max_vel = sp2 / denom
+ max_accel = await self.driver.query_int(nid, "SD", 0) / 1.01 / denom
+
+ return AxisConfig(
+ motor_conversion_factor=motor_conversion_factor,
+ max_travel=max_travel,
+ min_travel=min_travel,
+ unlimited_travel=unlimited_travel,
+ absolute_encoder=absolute_encoder,
+ max_vel=max_vel,
+ max_accel=max_accel,
+ joint_move_direction=joint_move_direction,
+ digital_inputs=digital_inputs,
+ analog_inputs=analog_inputs,
+ outputs=outputs,
+ )
+
+ async def _read_io_names(
+ self, nid: int, start: int, end: int, named: Dict[int, str]
+ ) -> Dict[int, str]:
+ """Read UI[start..end-1] as a channel -> human name map.
+
+ Channel index is 1-based. Codes in `named` map to fixed labels; positive
+ unknowns become "AuxPinN"; non-positive means unassigned.
+ """
+ out: Dict[int, str] = {}
+ for ui_idx in range(start, end):
+ code = await self.driver.query_int(nid, "UI", ui_idx)
+ ch = ui_idx - start + 1
+ if code in named:
+ out[ch] = named[code]
+ else:
+ out[ch] = "" if code <= 0 else f"AuxPin{code}"
+ return out
+
+ async def _read_servo_gripper_config(self) -> ServoGripperConfig:
+ sg = Axis.SERVO_GRIPPER
+ return ServoGripperConfig(
+ home_pos=int(await self.driver.query_float(sg, "UF", 6)),
+ home_search_vel=int(await self.driver.query_float(sg, "UF", 7)),
+ home_search_accel=int(await self.driver.query_float(sg, "UF", 8)),
+ home_default_position_error=int(await self.driver.query_float(sg, "UF", 9)),
+ home_hard_stop_position_error=int(await self.driver.query_float(sg, "UF", 10)),
+ home_hard_stop_offset=int(await self.driver.query_float(sg, "UF", 11)),
+ home_index_offset=int(await self.driver.query_float(sg, "UF", 12)),
+ home_offset_vel=int(await self.driver.query_float(sg, "UF", 13)),
+ home_offset_accel=int(await self.driver.query_float(sg, "UF", 14)),
+ home_timeout_msec=int(await self.driver.query_float(sg, "UF", 15)),
+ continuous_current=await self.driver.query_float(sg, "UF", 16),
+ peak_current=await self.driver.query_float(sg, "UF", 17),
+ )
+
+ @property
+ def _cfg(self) -> KX2Config:
+ if self._config is None:
+ raise RuntimeError("KX2 not set up — call setup() first")
+ return self._config
+
+ async def motor_get_current_position(self, axis: Axis) -> float:
+ raw = await self.driver.motor_get_current_position(
+ node_id=axis, pu=self._cfg.axes[axis].unlimited_travel,
+ )
+ c = self._cfg.axes[axis].motor_conversion_factor
+ if axis == Axis.ELBOW:
+ return kinematics.convert_elbow_angle_to_position(self._cfg, raw / c)
+ if c == 0:
+ logger.warning("Axis %s has conversion factor of 0", axis)
+ return 0.0
+ return raw / c
+
+ async def read_input(self, axis: Axis, input_num: int) -> bool:
+ return await self.driver.read_input(node_id=axis, input_num=0x10 + input_num)
+
+ # IR breakbeam between the gripper fingers, wired to the Z-drive's IO.
+ # True = beam interrupted (object present).
+ _PROXIMITY_SENSOR_AXIS: Axis = Axis.Z
+ _PROXIMITY_SENSOR_INPUT: int = 4
+
+ async def read_proximity_sensor(self) -> bool:
+ return await self.read_input(self._PROXIMITY_SENSOR_AXIS, self._PROXIMITY_SENSOR_INPUT)
+
+ async def wait_for_proximity_sensor(
+ self, state: bool = True, timeout: float = 5.0, poll: float = 0.01,
+ ) -> bool:
+ """Poll until the sensor reads `state`. Returns True on trip, False on timeout."""
+ deadline = time.monotonic() + timeout
+ while True:
+ if await self.read_proximity_sensor() == state:
+ return True
+ if time.monotonic() >= deadline:
+ return False
+ await asyncio.sleep(poll)
+
+ async def find_z_with_proximity_sensor(
+ self,
+ z_start: float,
+ z_end: float,
+ max_gripper_speed: float = 25.0,
+ max_gripper_acceleration: float = 100.0,
+ ) -> float:
+ """Pre-position to ``z_start``, then descend toward ``z_end``; halt when
+ the IR breakbeam trips.
+
+ ``z_start > z_end`` (search descends in world frame). The search
+ bounds are absolute Z so the caller never has to compute a delta.
+ ``max_gripper_speed`` / ``max_gripper_acceleration`` cap Z motion in
+ mm/s and mm/s² (Z is linear, so gripper speed equals |v_z|).
+
+ Arms IL[4]=StopForward so the drive halts the motor itself on the
+ input edge (sub-ms latency, no software in the loop). IL is restored
+ to GeneralPurpose afterwards even if the move raises. Returns the Z
+ position where the drive halted; raises RuntimeError if the beam
+ never tripped (descent ran the full ``z_start → z_end`` range).
+ """
+ if z_end >= z_start:
+ raise ValueError(
+ f"find_z_with_proximity_sensor: z_end ({z_end}) must be below "
+ f"z_start ({z_start}) — search descends."
+ )
+ move_params = KX2ArmBackend.JointMoveParams(
+ max_gripper_speed=max_gripper_speed,
+ max_gripper_acceleration=max_gripper_acceleration,
+ )
+ # Hold the motion guard for the whole find: a parallel caller's move
+ # could land between IL=StopForward and the descent, or between the
+ # descent and IL restore. The spawned move task uses
+ # `_motors_move_joint_locked` (no guard reacquire) since asyncio.Lock
+ # is task-aware and our reentrance check uses task identity — the
+ # spawned task isn't the owner.
+ async with self._motion_guard():
+ # Pre-flight: force the drive back to Op Enabled. A prior failed
+ # search could have left it in Fault/Quick Stop where new moves
+ # silently fail (Z barely moves).
+ await self.driver.motors_ensure_enabled([int(Axis.Z)])
+ await self._motors_move_joint_locked({Axis.Z: z_start}, params=move_params)
+ if await self.read_proximity_sensor():
+ return await self.motor_get_current_position(Axis.Z)
+ await self.driver.configure_input_logic(
+ self._PROXIMITY_SENSOR_AXIS, self._PROXIMITY_SENSOR_INPUT, _InputLogic.StopForward,
+ )
+ move_task = asyncio.create_task(
+ self._motors_move_joint_locked(
+ {Axis.Z: z_end}, params=move_params
+ )
+ )
+ tripped = False
+ try:
+ # The drive halts itself via IL the moment the beam breaks. We poll
+ # the sensor in parallel so we can stop waiting for "move done"
+ # (which never arrives — the drive halted short of target).
+ while not move_task.done():
+ if await self.read_proximity_sensor():
+ tripped = True
+ break
+ await asyncio.sleep(0.01)
+ move_task.cancel()
+ try:
+ await move_task
+ except (asyncio.CancelledError, CanError):
+ pass
+ finally:
+ # Match C# search cleanup (KX2RobotControl.cs:8650-8658): halt
+ # motor FIRST, then restore IL. Reverse order would let the drive
+ # surge toward the unreached target during the gap.
+ try:
+ await self.driver.motor_stop(Axis.Z)
+ except Exception as e:
+ logger.warning("find_z: motor_stop failed: %s", e)
+ try:
+ await self.driver.configure_input_logic(
+ self._PROXIMITY_SENSOR_AXIS, self._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose,
+ )
+ except Exception as e:
+ logger.warning("find_z: IL restore failed: %s", e)
+ # The IL-trip and the motor_stop fire EMCY frames that mark the
+ # move as a fault; the trip was *expected*, not a real fault, so
+ # clear the sticky state — otherwise the next motion call's
+ # `motor_check_if_move_done` raises immediately on a stale flag.
+ self.driver.clear_emcy_state(int(Axis.Z))
+ if not tripped:
+ z_actual_end = await self.motor_get_current_position(Axis.Z)
+ raise RuntimeError(
+ f"proximity sensor never tripped on Z {z_start:.2f} → {z_end:.2f} "
+ f"(stopped at {z_actual_end:.2f})"
+ )
+ return await self.motor_get_current_position(Axis.Z)
+
+ async def find_with_proximity_sensor(
+ self,
+ start: Coordinate,
+ end: Coordinate,
+ *,
+ max_gripper_speed: float = 25.0,
+ max_gripper_acceleration: float = 100.0,
+ ) -> Coordinate:
+ """Sweep the gripper from ``start`` to ``end`` along a straight Cartesian
+ line; halt when the IR breakbeam trips. Yaw is held at whatever the
+ gripper is currently at — proximity sensing doesn't care about
+ orientation, and asking the caller to specify a direction would force
+ them to think about wrist angle they don't otherwise need to. Returns
+ the gripper location at halt; raises ``RuntimeError`` if the beam
+ never tripped over the full path.
+
+ Generic Cartesian counterpart to :meth:`find_z_with_proximity_sensor`.
+ The Z-only descent has a hardware fast-path (``IL[4]=StopForward`` on
+ the Z drive halts the motor sub-ms on the input edge); X/Y motion can't
+ use that — the breakbeam is wired only to the Z drive's I/O — so this
+ method polls the sensor in software (~10 ms latency at 100 Hz). At the
+ default 25 mm/s sweep that's ~0.25 mm of overshoot before cancellation,
+ plus ~64 ms of post-cancel buffer-drain (8 PVT frames × 8 ms). For
+ Z-only descents where mm-precision into labware matters, prefer
+ ``find_z_with_proximity_sensor``.
+ """
+ move_params = KX2ArmBackend.CartesianMoveParams(
+ max_gripper_speed=max_gripper_speed,
+ max_gripper_acceleration=max_gripper_acceleration,
+ path="linear",
+ )
+
+ async with self._motion_guard():
+ await self.driver.motors_ensure_enabled([int(a) for a in MOTION_AXES])
+ current_yaw = (await self.request_gripper_pose()).rotation.z
+ start_pose = CartesianPose(location=start, rotation=Rotation(z=current_yaw))
+ end_pose = CartesianPose(location=end, rotation=Rotation(z=current_yaw))
+ # Pre-position to start (joint move — path doesn't matter, just get there).
+ pre_pos = await self._cart_to_joints(start_pose)
+ await self._motors_move_joint_locked(
+ cmd_pos=pre_pos,
+ params=KX2ArmBackend.JointMoveParams(
+ max_gripper_speed=max_gripper_speed,
+ max_gripper_acceleration=max_gripper_acceleration,
+ ),
+ )
+ if await self.read_proximity_sensor():
+ return (await self.request_gripper_pose()).location
+
+ sweep_task = asyncio.create_task(
+ self._run_linear_path(end_pose, move_params)
+ )
+ tripped = False
+ try:
+ # Drive runs the streamed PVT trajectory; we poll the sensor in
+ # parallel and cancel on trip. The sweep task's finally clause
+ # then sends ipm_stop + reverts to PPM, leaving the drive ready
+ # for the next motion call.
+ while not sweep_task.done():
+ if await self.read_proximity_sensor():
+ tripped = True
+ break
+ await asyncio.sleep(0.01)
+ sweep_task.cancel()
+ try:
+ await sweep_task
+ except (asyncio.CancelledError, CanError):
+ pass
+ finally:
+ # PVT-stop coasts up to 8 frames; halt() is too aggressive (drops
+ # all motion-axis MO=0). The sweep task's own cleanup already
+ # handled IPM teardown; clear sticky EMCY so next motion call's
+ # `motor_check_if_move_done` doesn't raise on the stop-induced frame.
+ self.driver.clear_emcy_state()
+ if not tripped:
+ end_loc = (await self.request_gripper_pose()).location
+ raise RuntimeError(
+ f"proximity sensor never tripped on "
+ f"({start.x:.1f},{start.y:.1f},{start.z:.1f}) → "
+ f"({end.x:.1f},{end.y:.1f},{end.z:.1f}) "
+ f"(stopped at ({end_loc.x:.1f},{end_loc.y:.1f},{end_loc.z:.1f}))"
+ )
+ return (await self.request_gripper_pose()).location
+
+ async def motors_move_joint(
+ self,
+ cmd_pos: Dict[Axis, float],
+ params: Optional["KX2ArmBackend.JointMoveParams"] = None,
+ ) -> None:
+ async with self._motion_guard():
+ await self._motors_move_joint_locked(cmd_pos, params)
+
+ async def _motors_move_joint_locked(
+ self,
+ cmd_pos: Dict[Axis, float],
+ params: Optional["KX2ArmBackend.JointMoveParams"] = None,
+ ) -> None:
+ """Caller MUST hold _motion_guard. Used by find_z's spawned poll/move
+ task, which can't re-enter the guard from a fresh asyncio Task."""
+ logger.debug("motors_move_joint cmd_pos=%s", cmd_pos)
+ if params is None:
+ params = KX2ArmBackend.JointMoveParams()
+ current = {Axis(k): v for k, v in (await self.request_joint_position()).items()}
+ plan = kinematics.plan_joint_move(
+ current=current,
+ target=cmd_pos,
+ cfg=self._cfg,
+ gripper_params=self._gripper_params,
+ max_gripper_speed=params.max_gripper_speed,
+ max_gripper_acceleration=params.max_gripper_acceleration,
+ )
+ if plan is None: # every axis a no-op
+ return
+ await self._motors_move_absolute_execute_locked(plan)
+
+ async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None:
+ async with self._motion_guard():
+ await self._motors_move_absolute_execute_locked(plan)
+
+ async def _motors_move_absolute_execute_locked(self, plan: MotorsMovePlan) -> None:
+ """Caller MUST hold _motion_guard."""
+ await self.driver.ipm_select_mode(False)
+
+ if logger.isEnabledFor(logging.DEBUG):
+ logger.debug(
+ "move plan: move_time=%.3fs, %d axes:", plan.move_time, len(plan.moves)
+ )
+ for move in plan.moves:
+ logger.debug(
+ " node=%d pos=%s vel=%s acc=%s dir=%s",
+ move.node_id, move.position, move.velocity,
+ move.acceleration, move.direction.name,
+ )
+
+ for move in plan.moves:
+ nid = move.node_id
+ await self.driver.motor_set_move_direction(nid, move.direction)
+ # 0x607A = Target Position (24698 decimal)
+ await self.driver.can_sdo_download_elmo_object(
+ nid, 24698, 0, int(move.position), _ElmoObjectDataType.INTEGER32,
+ )
+ # 0x6081 = Profile Velocity (24705 decimal)
+ await self.driver.can_sdo_download_elmo_object(
+ nid, 24705, 0, int(move.velocity), _ElmoObjectDataType.UNSIGNED32,
+ )
+ acc = max(int(move.acceleration), 100)
+ # 0x6083 = Profile Acceleration (24707 decimal)
+ await self.driver.can_sdo_download_elmo_object(
+ nid, 24707, 0, acc, _ElmoObjectDataType.UNSIGNED32
+ )
+ # 0x6084 = Profile Deceleration (24708 decimal)
+ await self.driver.can_sdo_download_elmo_object(
+ nid, 24708, 0, acc, _ElmoObjectDataType.UNSIGNED32
+ )
+
+ node_ids = [move.node_id for move in plan.moves]
+ await self.driver.ppm_begin_motion(node_ids)
+ await self.driver.wait_for_moves_done(node_ids, timeout=plan.move_time + 2)
+
+ async def _cart_to_joints(self, pose: CartesianPose) -> Dict[Axis, float]:
+ """Cartesian -> joints, snapping rotary axes to whichever 360° wrap is
+ closest to the current joint position."""
+ current = {Axis(k): v for k, v in (await self.request_joint_position()).items()}
+ ik_joints = kinematics.ik(pose, self._cfg, self._gripper_params)
+ return kinematics.snap_to_current(ik_joints, current)
+
+ async def _stream_samples(
+ self, samples: "List[kinematics.LinearPathSample]"
+ ) -> None:
+ """Stream a pre-built list of LinearPathSamples through the drive's IPM
+ buffer. Caller MUST hold ``_motion_guard``. The samples' encoder positions
+ and velocities are fed to each axis 8 frames ahead of the drive's read
+ pointer at the fixed IPM cadence (``_PVT_DT_MS``).
+
+ On cancel/exception the drive is brought back to PPM through a
+ ``finally`` block. Coast on a cancel can run up to ``_PVT_PRELOAD
+ * dt_ms`` (~64 ms) past the cancel — see ``halt()`` for zero-coast stop.
+ """
+ if len(samples) < 2:
+ await self.driver.ipm_select_mode(False)
+ return
+
+ # Skip axes with small total motion (Δ ≤ _SKIP_AXIS_COUNTS). Two reasons:
+ # (1) the drive idles on sub-threshold motion and leaves frames stuck in
+ # the IP buffer; (2) rotary axes (shoulder, wrist) wrap — IK gives
+ # angles in (-180, 180] but the drive's encoder counts up across
+ # revolutions, so streaming raw IK values for a rotary axis that's
+ # currently many revolutions in causes the drive to interpret each
+ # command as a huge multi-rotation move and tracking-error fault.
+ all_axes = [int(ax) for ax in (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST)]
+ active_axes: List[int] = []
+ for nid in all_axes:
+ seq = [s.encoder_position[Axis(nid)] for s in samples]
+ if max(seq) - min(seq) > self._SKIP_AXIS_COUNTS:
+ active_axes.append(nid)
+ if not active_axes:
+ return
+
+ # Clear any sticky fault on the involved drives before streaming.
+ await self.driver.motors_ensure_enabled(active_axes)
+
+ # Align the sample sequence to the drive's actual encoder positions.
+ # Rotary axes (shoulder, wrist) wrap — the drive's encoder counts up
+ # forever across full rotations, but IK gives joint angles in (-180,
+ # 180]; sample[0] therefore lands many revolutions away from the drive's
+ # actual position. Sending those raw triggers an immediate position-
+ # tracking fault (drive rejects RPDO3, looks like queue_full to us).
+ # Shift every sample's encoder_position by (actual_now - sample[0]) so
+ # the trajectory rides on top of the drive's current encoder, preserving
+ # all relative motion.
+ enc_now = {}
+ for nid in active_axes:
+ enc_now[nid] = await self.driver.motor_get_current_position(nid)
+ for nid in active_axes:
+ offset = enc_now[nid] - samples[0].encoder_position[Axis(nid)]
+ if offset == 0:
+ continue
+ for s in samples:
+ s.encoder_position[Axis(nid)] += offset
+
+ dt_s = self._PVT_DT_MS / 1000.0
+ preload = min(self._PVT_PRELOAD, len(samples))
+
+ await self.driver.ipm_select_mode(True)
+ try:
+ await self.driver.ipm_set_time_interval(self._PVT_DT_MS)
+ # Preload PRELOAD frames per axis. The drive starts consuming as soon
+ # as begin_motion fires; preload lets the producer fall behind by up
+ # to (PRELOAD-1) * dt without underflowing the queue.
+ for i in range(preload):
+ for nid in active_axes:
+ ax = Axis(nid)
+ self.driver.ipm_send_pvt_point(
+ nid,
+ samples[i].encoder_position[ax],
+ samples[i].encoder_velocity[ax],
+ )
+ self.driver.ipm_check_queue_fault(active_axes)
+
+ # Capture pacing reference *before* begin_motion (drive starts consuming
+ # on SYNC; capturing after underestimates elapsed drive-time and pace
+ # too late, eating into the underflow margin).
+ start = time.monotonic()
+ await self.driver.ipm_begin_motion(active_axes)
+
+ for i in range(preload, len(samples)):
+ target_t = (i - (preload - 1)) * dt_s
+ while time.monotonic() - start < target_t:
+ await asyncio.sleep(0)
+ for nid in active_axes:
+ ax = Axis(nid)
+ self.driver.ipm_send_pvt_point(
+ nid,
+ samples[i].encoder_position[ax],
+ samples[i].encoder_velocity[ax],
+ )
+ self.driver.ipm_check_queue_fault(active_axes)
+
+ # Drop ip-enable immediately after the last frame. Elmo drives don't
+ # latch SW bit-10 (target_reached) while ip-enable is high — polling
+ # for it inside IP mode hangs forever — and leaving ip-enable asserted
+ # past the buffer drain raises EMCY 0x8A on the next tick. Mirrors C#
+ # MotorsMovePathExecute. Trade-off: drive halts ~0.3 mm short of the
+ # trajectory end at typical speeds (matches the vendor behaviour).
+ await self.driver.ipm_stop(active_axes)
+ finally:
+ try:
+ await self.driver.ipm_stop(active_axes)
+ except Exception:
+ emcy_snap = {
+ ax: vars(self.driver._emcy[ax]) for ax in active_axes
+ if ax in self.driver._emcy
+ }
+ logger.exception(
+ "ipm_stop cleanup failed; drive may still be in IPM with ip-enable "
+ "high. EMCY state per axis: %s", emcy_snap,
+ )
+ try:
+ await self.driver.ipm_select_mode(False)
+ except Exception:
+ logger.exception(
+ "ipm_select_mode(False) cleanup failed; next motion call will "
+ "fresh-arm IPM via the re-arm path"
+ )
+
+ async def _run_linear_path(
+ self,
+ target_pose: CartesianPose,
+ params: "KX2ArmBackend.CartesianMoveParams",
+ ) -> None:
+ """Sample a straight tool-tip path to ``target_pose`` and stream it.
+ Caller MUST hold ``_motion_guard``."""
+ if params.max_gripper_speed is None or params.max_gripper_acceleration is None:
+ raise ValueError(
+ "CartesianMoveParams(path='linear') requires max_gripper_speed and "
+ "max_gripper_acceleration: the Cartesian profile is built from them "
+ "directly (no firmware fallback for streamed motion)."
+ )
+ current_joints = {
+ Axis(k): v for k, v in (await self.request_joint_position()).items()
+ }
+ start_pose = kinematics.fk(current_joints, self._cfg, self._gripper_params)
+ samples = kinematics.sample_linear_path(
+ cfg=self._cfg,
+ gripper_params=self._gripper_params,
+ start_pose=start_pose,
+ end_pose=target_pose,
+ vel_mm_per_s=params.max_gripper_speed,
+ accel_mm_per_s2=params.max_gripper_acceleration,
+ dt_s=self._PVT_DT_MS / 1000.0,
+ current_joints=current_joints,
+ )
+ await self._stream_samples(samples)
+
+ async def move_parametric(
+ self,
+ path_fn: "Callable[[float], CartesianPose]",
+ duration_s: float,
+ ) -> None:
+ """Stream a parametric Cartesian trajectory through IPM.
+
+ ``path_fn(t)`` is called at every IPM sample time ``t ∈ [0, duration_s]``
+ (seconds, evaluated at the drive's interpolation cadence, currently
+ 8 ms) and must return the absolute :class:`CartesianPose` the gripper
+ should occupy at that instant. Use the start pose if you need offsets:
+
+ start = await arm.request_gripper_pose()
+ def fig8(t):
+ s = t / duration_s
+ theta = 2 * math.pi * s
+ return CartesianPose(
+ location=Coordinate(start.location.x + 50 * math.sin(theta),
+ start.location.y + 12.5 * math.sin(2*theta),
+ start.location.z + 30 * math.sin(math.pi * s)),
+ rotation=start.rotation,
+ )
+ await arm.move_parametric(fig8, duration_s=8.0)
+
+ No speed/accel cap — the parametrization sets the velocity profile;
+ callers are responsible for keeping derivatives within drive limits.
+ Joint travel limits are enforced via IK; out-of-range raises IKError
+ before any motion. Path continuity is the caller's responsibility —
+ discontinuities surface as drive faults.
+ """
+ if duration_s <= 0:
+ raise ValueError(f"duration_s must be > 0, got {duration_s}")
+ async with self._motion_guard():
+ current_joints = {
+ Axis(k): v for k, v in (await self.request_joint_position()).items()
+ }
+ samples = kinematics.sample_parametric_path(
+ cfg=self._cfg,
+ gripper_params=self._gripper_params,
+ path_fn=path_fn,
+ duration_s=duration_s,
+ dt_s=self._PVT_DT_MS / 1000.0,
+ current_joints=current_joints,
+ )
+ await self._stream_samples(samples)
+
+ async def move_through_waypoints(
+ self,
+ waypoints: "List[CartesianPose]",
+ *,
+ speed: float,
+ accel: float,
+ ) -> None:
+ """Stream a smooth Catmull-Rom spline through ``waypoints``.
+
+ The curve passes through every waypoint with C¹ continuity (no stop at
+ intermediate waypoints). Tangents at each interior waypoint are derived
+ from neighbours; endpoint tangents are extrapolated from the first/last
+ segment. The whole spline is time-reparametrized into a trapezoidal
+ arc-length profile with peak Cartesian speed ``speed`` (mm/s) and peak
+ acceleration ``accel`` (mm/s²), then sampled at the IPM cadence.
+
+ Args:
+ waypoints: at least 2 absolute Cartesian poses. The first should
+ match the current pose closely (or the path will start with a
+ Cartesian-linear segment to get there).
+ speed: peak Cartesian speed along the spline, mm/s.
+ accel: peak Cartesian acceleration, mm/s².
+
+ Raises:
+ ValueError if waypoints < 2 or speed/accel <= 0.
+ IKError if any sample's joints are out of range.
+ """
+ if len(waypoints) < 2:
+ raise ValueError(f"need at least 2 waypoints, got {len(waypoints)}")
+ if speed <= 0 or accel <= 0:
+ raise ValueError(f"speed and accel must be > 0, got {speed}, {accel}")
+ async with self._motion_guard():
+ current_joints = {
+ Axis(k): v for k, v in (await self.request_joint_position()).items()
+ }
+ samples = kinematics.sample_waypoint_path(
+ cfg=self._cfg,
+ gripper_params=self._gripper_params,
+ waypoints=waypoints,
+ speed_mm_per_s=speed,
+ accel_mm_per_s2=accel,
+ dt_s=self._PVT_DT_MS / 1000.0,
+ current_joints=current_joints,
+ )
+ await self._stream_samples(samples)
+
+ # -- capability interface (OrientableGripperArmBackend + HasJoints + CanFreedrive) --
+
+ async def halt(self, backend_params: Optional[BackendParams] = None) -> None:
+ # Fire MO=0 on every motion axis concurrently — serial halts let later
+ # axes coast for the duration of the earlier SDOs.
+ #
+ # Deliberately NOT guarded by _motion_guard: emergency-stop must
+ # interrupt regardless of who's holding the lock.
+ await asyncio.gather(
+ *(self.driver.motor_emergency_stop(node_id=axis) for axis in MOTION_AXES)
+ )
+
+ # Park pose (centered, well inside workspace) and motion caps.
+ _PARK_JOINTS: Dict[Axis, float] = {
+ Axis.SHOULDER: 2.0, Axis.Z: 750.0, Axis.ELBOW: 1.0, Axis.WRIST: 356.0,
+ }
+ _PARK_SPEED: float = 80.0
+ _PARK_ACCEL: float = 400.0
+
+ async def park(self, backend_params: Optional[BackendParams] = None) -> None:
+ """Move the arm to a centered safe pose via a Cartesian-linear (IPM) move.
+
+ Uses IPM (`path='linear'`) rather than PPM (move_to_joint_position) so the
+ drives stay in mode 7 — back-to-back PPM → IPM transitions leave one or
+ more drives' IP buffers in a state where the next preload write hits
+ EMCY 0x34/0xBA (queue_full on first write). Sticking to IPM throughout
+ sidesteps that.
+ """
+ park_pose = kinematics.fk(self._PARK_JOINTS, self._cfg, self._gripper_params)
+ await self.move_to_location(
+ location=park_pose.location,
+ direction=park_pose.rotation.z,
+ backend_params=KX2ArmBackend.CartesianMoveParams(
+ max_gripper_speed=self._PARK_SPEED,
+ max_gripper_acceleration=self._PARK_ACCEL,
+ path="linear",
+ ),
+ )
+
+ async def request_gripper_pose(
+ self, backend_params: Optional[BackendParams] = None
+ ) -> CartesianPose:
+ joints = {Axis(k): v for k, v in (await self.request_joint_position()).items()}
+ return kinematics.fk(joints, self._cfg, self._gripper_params)
+
+ async def open_gripper(
+ self, gripper_width: float, backend_params: Optional[BackendParams] = None
+ ) -> None:
+ async with self._motion_guard():
+ await self._motors_move_joint_locked({Axis.SERVO_GRIPPER: gripper_width})
+
+ @dataclasses.dataclass
+ class GripParams(BackendParams):
+ check_plate_gripped: bool = True
+ # 10..100, fraction of the gripper's full current rating used as the
+ # CL/PL torque cap during the close. ``None`` keeps whatever the last
+ # close (or setup) left in place.
+ max_force_percent: Optional[int] = None
+
+ async def close_gripper(
+ self, gripper_width: float, backend_params: Optional[BackendParams] = None
+ ) -> None:
+ if not isinstance(backend_params, KX2ArmBackend.GripParams):
+ backend_params = KX2ArmBackend.GripParams()
+ async with self._motion_guard():
+ if backend_params.max_force_percent is not None:
+ await self._set_servo_gripper_force_limit(backend_params.max_force_percent)
+ await self._motors_move_joint_locked({Axis.SERVO_GRIPPER: gripper_width})
+ if backend_params.check_plate_gripped:
+ await self.check_plate_gripped()
+
+ async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool:
+ pos = await self.motor_get_current_position(Axis.SERVO_GRIPPER)
+ return abs(pos) < 1.0
+
+ @dataclasses.dataclass
+ class CartesianMoveParams(BackendParams):
+ """Cartesian gripper-speed/acceleration cap during the move (mm/s,
+ mm/s^2). ``None`` means run joints at firmware max with no Cartesian
+ cap. Joint speeds are scaled uniformly so the worst-case Cartesian
+ gripper speed along the trajectory stays at or under
+ ``max_gripper_speed``.
+
+ ``path`` selects the trajectory shape:
+ - ``"joint"`` (default): every axis ramps in parallel through its own
+ trapezoid. Tool tip traces a curvy path; speed is firmware-bounded
+ with optional gripper-speed cap.
+ - ``"linear"``: tool tip traces a straight line from current to
+ target pose, sampled and streamed via the drive's interpolation
+ buffer (PVT). Requires ``max_gripper_speed`` and
+ ``max_gripper_acceleration`` since they directly drive the
+ Cartesian profile (no cap == no speed to stream).
+ """
+ max_gripper_speed: Optional[float] = None
+ max_gripper_acceleration: Optional[float] = None
+ path: Literal["joint", "linear"] = "joint"
+
+ # PVT streaming cadence. 8 ms / 125 Hz: small enough that the 16-deep drive
+ # buffer holds ~125 ms of motion, long enough that producer scheduling
+ # jitter doesn't underflow the queue. Buffered up to PRELOAD frames ahead
+ # of the drive's read pointer.
+ #
+ # If you bump these, check: (a) DT_MS fits UINT8 — 0x60C2:01 is UNSIGNED8;
+ # `ipm_set_time_interval` validates this at the wire level. (b) PRELOAD <
+ # drive IP buffer depth (16, set by the 24772:2=16 write at setup) —
+ # otherwise the (PRELOAD+1)th frame queue_fulls immediately.
+ _PVT_DT_MS: int = 8
+ _PVT_PRELOAD: int = 8
+ # Below this trajectory delta (in encoder counts) an axis is considered
+ # "not moving" and skipped from IPM streaming — the drive idles on
+ # sub-threshold motion and leaves frames stuck in its IP buffer otherwise.
+ _SKIP_AXIS_COUNTS: int = 500
+
+ async def move_to_location(
+ self,
+ location: Coordinate,
+ direction: float,
+ backend_params: Optional[BackendParams] = None,
+ ) -> None:
+ if not isinstance(backend_params, KX2ArmBackend.CartesianMoveParams):
+ backend_params = KX2ArmBackend.CartesianMoveParams()
+ pose = CartesianPose(location=location, rotation=Rotation(z=direction))
+ if backend_params.path == "linear":
+ async with self._motion_guard():
+ await self._run_linear_path(pose, backend_params)
+ return
+ async with self._motion_guard():
+ joint_pos = await self._cart_to_joints(pose)
+ joint_params = KX2ArmBackend.JointMoveParams(
+ max_gripper_speed=backend_params.max_gripper_speed,
+ max_gripper_acceleration=backend_params.max_gripper_acceleration,
+ )
+ await self._motors_move_joint_locked(cmd_pos=joint_pos, params=joint_params)
+
+ @dataclasses.dataclass
+ class PickUpParams(BackendParams):
+ """Move-leg caps + grip-leg knob for the pick_up_* compounds."""
+ max_gripper_speed: Optional[float] = None
+ max_gripper_acceleration: Optional[float] = None
+ check_plate_gripped: bool = True
+
+ async def pick_up_at_location(
+ self,
+ location: Coordinate,
+ direction: float,
+ resource_width: float,
+ backend_params: Optional[BackendParams] = None,
+ ) -> None:
+ if not isinstance(backend_params, KX2ArmBackend.PickUpParams):
+ backend_params = KX2ArmBackend.PickUpParams()
+ move_params = KX2ArmBackend.CartesianMoveParams(
+ max_gripper_speed=backend_params.max_gripper_speed,
+ max_gripper_acceleration=backend_params.max_gripper_acceleration,
+ )
+ grip_params = KX2ArmBackend.GripParams(check_plate_gripped=backend_params.check_plate_gripped)
+ async with self._motion_guard():
+ await self.move_to_location(location, direction, backend_params=move_params)
+ await self.close_gripper(resource_width, backend_params=grip_params)
+
+ @dataclasses.dataclass
+ class DropParams(BackendParams):
+ """Move-leg caps for the drop_at_* compounds."""
+ max_gripper_speed: Optional[float] = None
+ max_gripper_acceleration: Optional[float] = None
+
+ async def drop_at_location(
+ self,
+ location: Coordinate,
+ direction: float,
+ resource_width: float,
+ backend_params: Optional[BackendParams] = None,
+ ) -> None:
+ if not isinstance(backend_params, KX2ArmBackend.DropParams):
+ backend_params = KX2ArmBackend.DropParams()
+ move_params = KX2ArmBackend.CartesianMoveParams(
+ max_gripper_speed=backend_params.max_gripper_speed,
+ max_gripper_acceleration=backend_params.max_gripper_acceleration,
+ )
+ async with self._motion_guard():
+ await self.move_to_location(location, direction, backend_params=move_params)
+ await self.open_gripper(resource_width)
+
+ @dataclasses.dataclass
+ class JointMoveParams(BackendParams):
+ """Same shape as ``CartesianMoveParams``'s speed/accel caps — see its
+ docstring."""
+ max_gripper_speed: Optional[float] = None
+ max_gripper_acceleration: Optional[float] = None
+
+ async def move_to_joint_position(
+ self,
+ position: Dict[int, float],
+ backend_params: Optional[BackendParams] = None,
+ ) -> None:
+ if not isinstance(backend_params, KX2ArmBackend.JointMoveParams):
+ backend_params = KX2ArmBackend.JointMoveParams()
+ async with self._motion_guard():
+ cmd_pos = {Axis(int(k)): float(v) for k, v in position.items()}
+ await self._motors_move_joint_locked(cmd_pos=cmd_pos, params=backend_params)
+
+ async def pick_up_at_joint_position(
+ self,
+ position: Dict[int, float],
+ resource_width: float,
+ backend_params: Optional[BackendParams] = None,
+ ) -> None:
+ if not isinstance(backend_params, KX2ArmBackend.PickUpParams):
+ backend_params = KX2ArmBackend.PickUpParams()
+ move_params = KX2ArmBackend.JointMoveParams(
+ max_gripper_speed=backend_params.max_gripper_speed,
+ max_gripper_acceleration=backend_params.max_gripper_acceleration,
+ )
+ grip_params = KX2ArmBackend.GripParams(check_plate_gripped=backend_params.check_plate_gripped)
+ async with self._motion_guard():
+ await self.move_to_joint_position(position, backend_params=move_params)
+ await self.close_gripper(resource_width, backend_params=grip_params)
+
+ async def drop_at_joint_position(
+ self,
+ position: Dict[int, float],
+ resource_width: float,
+ backend_params: Optional[BackendParams] = None,
+ ) -> None:
+ if not isinstance(backend_params, KX2ArmBackend.DropParams):
+ backend_params = KX2ArmBackend.DropParams()
+ move_params = KX2ArmBackend.JointMoveParams(
+ max_gripper_speed=backend_params.max_gripper_speed,
+ max_gripper_acceleration=backend_params.max_gripper_acceleration,
+ )
+ async with self._motion_guard():
+ await self.move_to_joint_position(position, backend_params=move_params)
+ await self.open_gripper(resource_width)
+
+ async def request_joint_position(
+ self, backend_params: Optional[BackendParams] = None
+ ) -> Dict[int, float]:
+ # Each motor_get_current_position is one BI query (one CAN round-trip,
+ # ~5ms). Distinct (node_id, msg_type, msg_index) keys mean concurrent
+ # in-flight requests can't collide in _send_bi's pending-future map, so
+ # gather pipelines all five into one round-trip's worth of latency.
+ axes = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST, Axis.SERVO_GRIPPER)
+ positions = await asyncio.gather(
+ *(self.motor_get_current_position(ax) for ax in axes)
+ )
+ return dict(zip(axes, positions))
+
+ def motion_limits(self) -> "_MotionLimits":
+ """Per-axis (max_speed, max_acceleration) read from the drives at setup.
+
+ Linear axes (Z, rail, servo gripper) are mm/s, mm/s^2; rotary axes
+ (shoulder, elbow, wrist) are deg/s, deg/s^2. These are the upper bounds
+ `JointMoveParams` / `CartesianMoveParams` get clamped to. Returned as a
+ dict subclass that renders as a table in Jupyter and plain-text columns
+ in a terminal.
+ """
+ return _MotionLimits(
+ {k: (cfg.max_vel, cfg.max_accel) for k, cfg in self._cfg.axes.items()},
+ )
+
+ async def start_freedrive_mode(
+ self,
+ free_axes: Optional[List[int]] = None,
+ backend_params: Optional[BackendParams] = None,
+ ) -> None:
+ # Default: free all motion axes (shoulder/Z/elbow/wrist) but never the
+ # gripper, so a held plate doesn't drop. Caller can override with an
+ # explicit list; [0] means "all motion axes" per CanFreedrive convention.
+ if free_axes is None or free_axes == [0]:
+ axes: List[int] = list(MOTION_AXES)
+ else:
+ axes = list(free_axes)
+ async with self._motion_guard():
+ for axis in axes:
+ await self.driver.motor_enable(node_id=axis, state=False, use_ds402=True)
+ self._freedrive_axes = axes
+
+ async def stop_freedrive_mode(self, backend_params: Optional[BackendParams] = None) -> None:
+ axes: List[int] = self._freedrive_axes or list(MOTION_AXES)
+ async with self._motion_guard():
+ await self.driver.motors_ensure_enabled([int(a) for a in axes])
+ self._freedrive_axes = []
+
+ async def very_dangerously_yeet(
+ self,
+ min_z: float = 400.0,
+ bump: float = 1.25,
+ force: bool = False,
+ ) -> None:
+ """Easter egg — swing the arm at firmware-max and open the gripper at
+ peak velocity to throw whatever is being held.
+
+ Call from your pickup pose. Sequence: auto-windup wrist to the inward
+ angle, swing shoulder 180° at firmware-max, fire gripper open near end
+ of cruise (with wrist flick at peak ω for extra tangential velocity),
+ return to pickup pose.
+
+ ``bump`` scales VH[2]/SP[2]/SD[0] on shoulder + wrist for the swing's
+ duration (restored in finally). 1.0 = stock; 1.25 confirmed safe;
+ higher risks tracking-error faults that need Elmo Composer recovery.
+
+ ``force=True`` skips the interactive 'y' prompt — for scripted use or
+ when the prompt path is broken (notebooks ran ``input`` in a thread
+ pool where ipykernel's stdin hook doesn't reach, returning '' and
+ aborting before the operator could type).
+ """
+ if not force:
+ warning = (
+ f"WARNING: very_dangerously_yeet: swing the arm at {bump:.2f}x firmware-max "
+ "and open the gripper mid-swing. Anything in the gripper will be "
+ "thrown. High bump can fault the drive. Type 'y' to continue: "
+ )
+ # Synchronous input(): python-can's reader thread keeps draining CAN
+ # frames while we block. Don't punt to asyncio.to_thread — ipykernel
+ # only services stdin requests on the main kernel thread.
+ answer = input(warning)
+ if answer.strip().lower() != "y":
+ raise RuntimeError("very_dangerously_yeet: aborted by user")
+
+ driver = self.driver
+ cfg = self._cfg
+
+ # Hold the motion guard for the whole yeet — bumped VH/SP/SD on
+ # SHOULDER+WRIST is a global drive-state mutation that mustn't overlap
+ # with another caller's move.
+ async with self._motion_guard():
+ z_now = await self.motor_get_current_position(Axis.Z)
+ if z_now < min_z:
+ raise RuntimeError(
+ f"yeet refused: Z={z_now:.0f}mm < min_z={min_z:.0f}mm; raise the arm first"
+ )
+
+ # Snapshot + bump VH[2]/SP[2]/SD[0] on swing axes; restore in finally.
+ saved_limits: Dict[Axis, dict] = {}
+ if bump != 1.0:
+ for ax in (Axis.SHOULDER, Axis.WRIST):
+ nid = ax
+ s = {
+ "VH2": await driver.query_int(nid, "VH", 2),
+ "SP2": await driver.query_int(nid, "SP", 2),
+ "SD0": await driver.query_int(nid, "SD", 0),
+ "max_vel": cfg.axes[ax].max_vel,
+ "max_accel": cfg.axes[ax].max_accel,
+ }
+ saved_limits[ax] = s
+ new_vh2 = int(s["VH2"] * bump)
+ new_sp2 = int(s["SP2"] * bump)
+ new_sd0 = int(s["SD0"] * bump)
+ await driver.write(nid, "VH", 2, new_vh2)
+ await driver.write(nid, "SP", 2, new_sp2)
+ await driver.write(nid, "SD", 0, new_sd0)
+ conv = abs(cfg.axes[ax].motor_conversion_factor)
+ cfg.axes[ax].max_vel = (new_sp2 / 1.01) / conv
+ cfg.axes[ax].max_accel = (new_sd0 / 1.01) / conv
+
+ try:
+ pickup_pose = await self.request_joint_position()
+
+ # Auto-windup: rotate wrist to the inward angle (opposite of outward).
+ wrist_inward = 0.0 if self._gripper_params.finger_side == "barcode_reader" else 180.0
+ while wrist_inward - pickup_pose[Axis.WRIST] > 180.0:
+ wrist_inward -= 360.0
+ while wrist_inward - pickup_pose[Axis.WRIST] < -180.0:
+ wrist_inward += 360.0
+ await self._motors_move_joint_locked(
+ cmd_pos={Axis.WRIST: wrist_inward},
+ params=KX2ArmBackend.JointMoveParams(
+ max_gripper_speed=_YEET_WINDUP_GRIPPER_SPEED,
+ max_gripper_acceleration=_YEET_WINDUP_GRIPPER_ACC,
+ ),
+ )
+
+ joints0 = await self.request_joint_position()
+
+ # Outward wrist = kinematic target (180° barcode_reader, 0° proximity).
+ wrist_outward = 180.0 if self._gripper_params.finger_side == "barcode_reader" else 0.0
+ while wrist_outward - joints0[Axis.WRIST] > 180.0:
+ wrist_outward -= 360.0
+ while wrist_outward - joints0[Axis.WRIST] < -180.0:
+ wrist_outward += 360.0
+
+ sh_move, sh_t_acc, sh_t_total, _ = await _yeet_build_axis_move(
+ self, Axis.SHOULDER,
+ joints0[Axis.SHOULDER], joints0[Axis.SHOULDER] + _YEET_SHOULDER_SWING_DEG,
+ )
+ wr_move, wr_t_acc, wr_t_total, _ = await _yeet_build_axis_move(
+ self, Axis.WRIST, joints0[Axis.WRIST], wrist_outward,
+ )
+ sh_plan = MotorsMovePlan(moves=[sh_move], move_time=sh_t_total)
+ wr_plan = MotorsMovePlan(moves=[wr_move], move_time=wr_t_total)
+
+ # Release fires inside shoulder cruise. Wrist trigger is delayed so its
+ # accel ramp finishes at release (peak ω at the gripper offset).
+ sh_cruise_dur = max(0.0, sh_t_total - 2 * sh_t_acc)
+ release_t = sh_t_acc + sh_cruise_dur * _YEET_RELEASE_FRACTION
+ wrist_trigger_t = max(0.0, release_t - wr_t_acc)
+
+ sg = Axis.SERVO_GRIPPER
+ sg_cfg = cfg.axes[Axis.SERVO_GRIPPER]
+ open_pos = min(_YEET_OPEN_POSITION, sg_cfg.max_travel - _YEET_OPEN_SAFETY_MARGIN)
+ gripper_plan = MotorsMovePlan(moves=[MotorMoveParam(
+ node_id=sg,
+ position=int(round(open_pos * sg_cfg.motor_conversion_factor)),
+ velocity=int(round(sg_cfg.max_vel * abs(sg_cfg.motor_conversion_factor))),
+ acceleration=int(round(sg_cfg.max_accel * abs(sg_cfg.motor_conversion_factor))),
+ direction=sg_cfg.joint_move_direction,
+ )])
+
+ # Pre-arm so triggers are pure control-word writes (sub-ms), not SDOs.
+ await _yeet_arm_plan(driver, sh_plan)
+ await _yeet_arm_plan(driver, wr_plan)
+
+ await driver.ppm_begin_motion([Axis.SHOULDER])
+ t0 = time.monotonic()
+ await asyncio.sleep(max(0.0, wrist_trigger_t - (time.monotonic() - t0)))
+ await driver.ppm_begin_motion([Axis.WRIST])
+
+ await asyncio.sleep(max(0.0, release_t - (time.monotonic() - t0)))
+ await self._motors_move_absolute_execute_locked(gripper_plan)
+
+ # Settle slack: at higher bump, drives overshoot + ring before asserting
+ # target-reached; tight margin trips a CanError even though throw was OK.
+ swing_finish_t = max(sh_t_total, wrist_trigger_t + wr_t_total)
+ await driver.wait_for_moves_done(
+ [Axis.SHOULDER, Axis.WRIST], timeout=swing_finish_t + 5,
+ )
+
+ await self._motors_move_joint_locked(
+ cmd_pos={
+ Axis.SHOULDER: pickup_pose[Axis.SHOULDER],
+ Axis.WRIST: pickup_pose[Axis.WRIST],
+ },
+ params=KX2ArmBackend.JointMoveParams(
+ max_gripper_speed=_YEET_RETURN_GRIPPER_SPEED,
+ max_gripper_acceleration=_YEET_RETURN_GRIPPER_ACC,
+ ),
+ )
+ finally:
+ for ax, s in saved_limits.items():
+ nid = ax
+ await driver.write(nid, "VH", 2, s["VH2"])
+ await driver.write(nid, "SP", 2, s["SP2"])
+ await driver.write(nid, "SD", 0, s["SD0"])
+ cfg.axes[ax].max_vel = s["max_vel"]
+ cfg.axes[ax].max_accel = s["max_accel"]
+
+
+class _MotionLimits(Dict[Axis, tuple]):
+ """Pretty-printing dict for `KX2ArmBackend.motion_limits()`. Dict access
+ still works (`limits[Axis.Z]` -> `(max_speed, max_accel)`); `__repr__`
+ formats it as an aligned ASCII table for both terminals and notebooks.
+ """
+
+ def __repr__(self) -> str:
+ rows = []
+ for ax, (v, a) in self.items():
+ unit = "mm" if ax.is_linear else "deg"
+ rows.append((ax.name, f"{v:.2f} {unit}/s", f"{a:.2f} {unit}/s^2"))
+ headers = ("axis", "max speed", "max acceleration")
+ widths = [max(len(headers[i]), *(len(r[i]) for r in rows)) for i in range(3)]
+ fmt = " ".join(f"{{:<{w}}}" for w in widths)
+ out = [fmt.format(*headers), fmt.format(*("-" * w for w in widths))]
+ out.extend(fmt.format(*r) for r in rows)
+ return "\n".join(out)
+
+# UI[5..10] code -> digital input role.
+_DIGITAL_INPUT_NAMES: Dict[int, str] = {
+ 101: "ProximitySensor",
+ 102: "TeachButton",
+}
+
+# UI[13..16] code -> output role.
+_OUTPUT_NAMES: Dict[int, str] = {
+ 101: "IndicatorLightRed",
+ 102: "IndicatorLightGreen",
+ 103: "IndicatorLightBlue",
+ 104: "IndicatorLight",
+ 105: "Buzzer",
+}
+
+
+# === very_dangerously_yeet helpers (easter egg) ============================
+# Constants and helpers for KX2ArmBackend.very_dangerously_yeet. Inlined
+# here on purpose; do not split into another module.
+
+_YEET_SHOULDER_SWING_DEG = 180.0
+_YEET_RELEASE_FRACTION = 0.85
+# Gripper open target (mm). Clamped at runtime to drive's max_travel - margin.
+_YEET_OPEN_POSITION = 30.0
+_YEET_OPEN_SAFETY_MARGIN = 1.0
+# Windup: arm holds the plate, don't whip.
+_YEET_WINDUP_GRIPPER_SPEED = 100.0 # mm/s
+_YEET_WINDUP_GRIPPER_ACC = 500.0 # mm/s^2
+# Return: plate is gone, but still keep it gentle.
+_YEET_RETURN_GRIPPER_SPEED = 100.0 # mm/s
+_YEET_RETURN_GRIPPER_ACC = 500.0 # mm/s^2
+
+
+async def _yeet_build_axis_move(
+ backend: "KX2ArmBackend", ax: Axis, cur: float, target: float,
+) -> tuple:
+ """Per-axis MotorMoveParam at firmware velocity limit (VH[2]/1.01).
+ Returns (move, t_acc, t_total, v_phys)."""
+ cfg = backend._cfg
+ ax_cfg = cfg.axes[ax]
+ conv = ax_cfg.motor_conversion_factor
+ vh2 = await backend.driver.query_int(ax, "VH", 2)
+ v_phys = vh2 / 1.01 / abs(conv)
+ a_phys = ax_cfg.max_accel
+ direction = ax_cfg.joint_move_direction
+
+ d = target - cur
+ span = ax_cfg.max_travel - ax_cfg.min_travel
+ if span > 0 and ax_cfg.unlimited_travel:
+ if direction == JointMoveDirection.Clockwise and d > 0.01:
+ d -= span
+ elif direction == JointMoveDirection.Counterclockwise and d < -0.01:
+ d += span
+ elif direction == JointMoveDirection.ShortestWay:
+ if d > 180.0:
+ d -= span
+ elif d < -180.0:
+ d += span
+ dist = abs(d)
+
+ if ax_cfg.unlimited_travel and direction != JointMoveDirection.Normal:
+ target = kinematics._wrap_to_range(target, ax_cfg.min_travel, ax_cfg.max_travel)
+
+ _, _, t_acc, t_total = kinematics._profile(dist, v_phys, a_phys)
+ move = MotorMoveParam(
+ node_id=ax,
+ position=int(round(target * conv)),
+ velocity=max(int(round(v_phys * abs(conv))), 1),
+ acceleration=max(int(round(a_phys * abs(conv))), 1),
+ direction=direction,
+ )
+ return move, t_acc, t_total, v_phys
+
+
+async def _yeet_arm_plan(driver: KX2Driver, plan: MotorsMovePlan) -> None:
+ """Pre-load a plan onto the drives without triggering it. Splits SDO
+ setup latency from the move start so the timer can be accurate."""
+ await driver.ipm_select_mode(False)
+ for move in plan.moves:
+ nid = move.node_id
+ await driver.motor_set_move_direction(nid, move.direction)
+ await driver.can_sdo_download_elmo_object(
+ nid, 24698, 0, int(move.position), _ElmoObjectDataType.INTEGER32,
+ )
+ await driver.can_sdo_download_elmo_object(
+ nid, 24705, 0, int(move.velocity), _ElmoObjectDataType.UNSIGNED32,
+ )
+ acc = max(int(move.acceleration), 100)
+ await driver.can_sdo_download_elmo_object(
+ nid, 24707, 0, acc, _ElmoObjectDataType.UNSIGNED32,
+ )
+ await driver.can_sdo_download_elmo_object(
+ nid, 24708, 0, acc, _ElmoObjectDataType.UNSIGNED32,
+ )
diff --git a/pylabrobot/paa/kx2/barcode_reader.py b/pylabrobot/paa/kx2/barcode_reader.py
new file mode 100644
index 00000000000..5824e4543c9
--- /dev/null
+++ b/pylabrobot/paa/kx2/barcode_reader.py
@@ -0,0 +1,340 @@
+"""PAA KX2 onboard barcode reader.
+
+The KX2's onboard barcode reader is a plain RS-232 device wired to the
+controller PC — entirely independent of the CAN bus that drives the motors.
+Lives in its own `Device` class alongside :class:`KX2`.
+
+The unit shipped with KX2 systems we've inspected is a **Denso MDI-4050**
+(1D laser scanner) — confirmed via ``Z4``::
+
+ MODEL = MDI-4050
+ ROM Ver. = BD01J09
+ I/F = RS-232C
+
+Denso's MDI series speaks an ESC-prefixed protocol close to (but not
+identical to) Microscan's. The vendor C# at ``KX2RobotControl.cs:15573–15848``
+covers the read-cycle subset.
+
+Factory defaults: **9600 8N1, no flow control**. Commands are framed as
+``ESC + + CR + LF``; responses are `` + CR``. Useful commands:
+
+ ``Z`` — fire trigger (start a single read window)
+ ``Y`` — stop trigger
+ ``Z1`` — query firmware version (handshake — single line response)
+ ``Z4`` — dump the entire NVRAM config (model, version, all symbology
+ enables, prefixes/suffixes, length limits — large multi-KB reply)
+ ``S0/S1/S2`` — set read mode: single / multiple / continuous
+ ``Y1..Y9`` / ``YM`` — read-time window (seconds, or M for indefinite)
+ ``+I/+F`` — auto-trigger on / off
+
+Symbology configuration
+-----------------------
+
+The MDI-4050 is a 1D laser, so PDF417 / DataMatrix / QR Code aren't supported.
+On a fresh unit, the typical enabled set is: UPC-A, UPC-E, EAN-13, EAN-8,
+Code 39, Tri-Optic, Codabar, Industrial 2/5, Interleaved 2/5, IATA, Code 128,
+Code 93, GS1 DataBar, GS1 DataBar Limited. Add-on (supplemental) codes,
+postal codes, MSI/Plessey, Telepen, UK/Plessey, and Code 11 are off by default.
+
+To re-enable / disable individual symbologies, use one of:
+
+1. **Denso's MDI Setup Windows utility** (free from Denso ADC) — checkbox UI
+ over the same RS-232 port, writes to the reader's NVRAM.
+2. **Configuration barcodes** printed in the MDI-4050 setting manual — scan
+ a Start → toggle → End sequence, no host needed.
+3. Direct register writes via the ESC protocol. The ``[NN]`` sections in the
+ ``Z4`` dump correspond to NVRAM registers but the per-symbology byte
+ layout is documented only in Denso's protocol manual; reverse-engineering
+ from a ``Z4`` dump alone is fragile.
+
+Use :meth:`KX2BarcodeReaderDriver.dump_config` to read the current state.
+
+============================================================================
+USB-to-serial driver setup (macOS)
+============================================================================
+
+The reader connects via a Prolific PL2303 USB-to-serial cable. macOS bundles a
+`AppleUSBPLCOM.dext` for older PL2303 silicon (TA/EA), but newer chips (GC/HXN,
+USB ID ``067b:23a3`` and ``067b:2303``) need Prolific's vendor DriverKit
+extension:
+
+ 1. ``brew install --cask prolific-pl2303``
+ 2. Launch ``/Applications/PL2303Serial.app`` once so macOS registers the
+ system extension.
+ 3. Open **System Settings → Privacy & Security**, scroll to the bottom, and
+ click **Allow** on the "System software from PL2303Serial was blocked"
+ prompt. macOS may require a restart.
+ 4. Replug the USB cable. The device shows up as
+ ``/dev/tty.PL2303G-USBtoUART`` (or ``/dev/cu.PL2303G-USBtoUART``).
+ Pass that path to :class:`KX2BarcodeReader`.
+
+To verify before instantiating, ``systemextensionsctl list`` should show
+``com.prolific.cdc.PLCdcFSDriver`` in state ``[activated enabled]``.
+
+On Linux, the PL2303 chip is supported by the in-tree ``pl2303`` driver and
+shows up as ``/dev/ttyUSB`` with no extra setup.
+"""
+
+from __future__ import annotations
+
+import asyncio
+import logging
+import time
+from typing import Literal, Optional
+
+from pylabrobot.capabilities.barcode_scanning import BarcodeScanner
+from pylabrobot.capabilities.barcode_scanning.backend import (
+ BarcodeScannerBackend,
+ BarcodeScannerError,
+)
+from pylabrobot.capabilities.capability import BackendParams
+from pylabrobot.device import Device, Driver
+from pylabrobot.io.serial import Serial
+from pylabrobot.resources.barcode import Barcode
+
+ReadMode = Literal["single", "multiple", "continuous"]
+
+try:
+ import serial as pyserial
+
+ _HAS_SERIAL = True
+except ImportError as _e:
+ _HAS_SERIAL = False
+ _SERIAL_IMPORT_ERROR = _e
+
+logger = logging.getLogger(__name__)
+
+_ESC = b"\x1b"
+_CR = b"\r"
+# Vendor C# uses `SerialPort.WriteLine(ESC + cmd + "\r")`, which appends the
+# default .NET `NewLine` of `"\n"` — so the actual frame on the wire is
+# ESC + cmd + CR + LF. Responses are split on CR alone (see
+# SerialPortBCR_DataReceived in KX2RobotControl.cs).
+_CMD_TERM = b"\r\n"
+
+
+class KX2BarcodeReaderDriver(Driver):
+ """Serial driver for the KX2's onboard Microscan-style barcode reader.
+
+ Factory defaults (per `KX2RobotControl.cs:1712–1741`): 9600 8N1, no flow
+ control. All commands are `ESC + + CR`; all responses are
+ ` + CR`.
+ """
+
+ default_baudrate = 9600
+
+ def __init__(self, port: str, baudrate: int = default_baudrate):
+ if not _HAS_SERIAL:
+ raise ImportError(
+ "pyserial is not installed. Install with `pip install pylabrobot[serial]` "
+ f"(import error: {_SERIAL_IMPORT_ERROR})"
+ )
+ super().__init__()
+ self.io = Serial(
+ human_readable_device_name="KX2 Barcode Reader",
+ port=port,
+ baudrate=baudrate,
+ bytesize=pyserial.EIGHTBITS,
+ parity=pyserial.PARITY_NONE,
+ stopbits=pyserial.STOPBITS_ONE,
+ write_timeout=1,
+ timeout=0.1, # short per-byte timeout; send_command handles the real deadline
+ rtscts=False,
+ )
+
+ async def setup(self, backend_params: Optional[BackendParams] = None) -> None:
+ await self.io.setup()
+ logger.info("[KX2 BCR %s] connected", self.io.port)
+
+ async def stop(self) -> None:
+ # Match the C# teardown: turn trigger off + restore default read time
+ # (KX2RobotControl.cs:15623–15624) so the reader is left in a known state.
+ await self.send_command("Y")
+ await self.send_command("Y2") # read time = 2s
+ await self.io.stop()
+ logger.info("[KX2 BCR %s] disconnected", self.io.port)
+
+ async def _read_until_cr(self, timeout: float) -> str:
+ """Read from the port until we see a CR, returning the decoded line.
+
+ Raises `BarcodeScannerError` on timeout.
+ """
+ deadline = time.monotonic() + timeout
+ buf = bytearray()
+ while time.monotonic() < deadline:
+ chunk = await self.io.read(32)
+ if chunk:
+ buf.extend(chunk)
+ if _CR in buf:
+ line, _, _ = buf.partition(_CR)
+ decoded = line.decode("ascii", errors="replace").lstrip("\x1b").rstrip()
+ return decoded
+ else:
+ await asyncio.sleep(0.01)
+ raise BarcodeScannerError(
+ f"KX2 barcode reader: timeout waiting for CR after {timeout}s (buffered={bytes(buf)!r})"
+ )
+
+ async def send_command(self, cmd: str) -> None:
+ """Fire-and-forget: send `ESC + cmd + CR + LF` and return immediately.
+
+ The reader doesn't echo most commands (S0/S1/S2, Y/Yn/YM, Z trigger,
+ +I/+F). Use :meth:`query` for commands that return a value (e.g. Z1).
+ Mirrors C# `BarcodeReaderSendCommand` with ``Response=""`` — see
+ `KX2RobotControl.cs:15639–15700`.
+ """
+ payload = _ESC + cmd.encode("ascii") + _CMD_TERM
+ await self.io.write(payload)
+ logger.debug("[KX2 BCR %s] %s (no wait)", self.io.port, cmd)
+
+ async def query(self, cmd: str, timeout: float = 5.0) -> str:
+ """Send a command and wait for a CR-terminated response.
+
+ Use only for commands that the reader actually replies to (Z1
+ software version). Other "set" commands time out here because the
+ reader is silent on success.
+ """
+ payload = _ESC + cmd.encode("ascii") + _CMD_TERM
+ await self.io.write(payload)
+ decoded = await self._read_until_cr(timeout)
+ logger.debug("[KX2 BCR %s] %s -> %r", self.io.port, cmd, decoded)
+ return decoded
+
+ async def read_decoded_barcode(self, timeout: float) -> str:
+ """Listen for an asynchronously-delivered decoded barcode line.
+
+ Used after firing `trigger(True)` — the reader emits the decoded data
+ followed by CR whenever it makes a successful read.
+ """
+ return await self._read_until_cr(timeout)
+
+ # --- typed command helpers (names mirror the C# API) --------------------
+
+ async def trigger(self, on: bool) -> None:
+ await self.send_command("Z" if on else "Y")
+
+ async def set_read_mode(self, mode: ReadMode) -> None:
+ """Maps to S0/S1/S2 on the wire."""
+ code = {"single": "S0", "multiple": "S1", "continuous": "S2"}[mode]
+ await self.send_command(code)
+
+ async def set_read_time(self, seconds: int) -> None:
+ """seconds: 1..9, or 0 for indefinite (mapped to YM)."""
+ if seconds == 0:
+ await self.send_command("YM")
+ elif 1 <= seconds <= 9:
+ await self.send_command(f"Y{seconds}")
+ else:
+ raise ValueError("read_time must be 0 (indefinite) or 1..9 seconds")
+
+ async def set_auto_trigger(self, on: bool) -> None:
+ await self.send_command("+I" if on else "+F")
+
+ async def get_software_version(self) -> str:
+ return await self.query("Z1")
+
+ async def dump_config(self, timeout: float = 8.0) -> str:
+ """Return the reader's full NVRAM dump (response to ``Z4``).
+
+ Includes the model string, firmware version, interface type, and the
+ complete symbology / prefix-suffix / length-limit table. The reply is
+ multi-KB and the reader streams it over a few hundred ms, so we read
+ until a quiet period rather than a single CR.
+ """
+ payload = _ESC + b"Z4" + _CMD_TERM
+ await self.io.write(payload)
+ deadline = time.monotonic() + timeout
+ last_byte = time.monotonic()
+ buf = bytearray()
+ while time.monotonic() < deadline:
+ chunk = await self.io.read(2048)
+ if chunk:
+ buf.extend(chunk)
+ last_byte = time.monotonic()
+ else:
+ if buf and time.monotonic() - last_byte > 1.0:
+ break
+ await asyncio.sleep(0.05)
+ return buf.decode("ascii", errors="replace")
+
+
+class KX2BarcodeReaderBackend(BarcodeScannerBackend):
+ """Adapts :class:`KX2BarcodeReaderDriver` to the BarcodeScanner capability."""
+
+ # Wait bound when the caller doesn't specify a read_time. Long enough that
+ # any reasonable on-device read window (Y1..Y9) finishes inside it.
+ _DEFAULT_SCAN_WAIT = 10.0
+
+ def __init__(self, driver: KX2BarcodeReaderDriver):
+ super().__init__()
+ self.driver = driver
+
+ async def _on_setup(self, backend_params: Optional[BackendParams] = None) -> None:
+ # Handshake: version query (mirrors KX2RobotControl.cs:15617).
+ version = await self.driver.get_software_version()
+ if not version:
+ raise BarcodeScannerError(
+ "KX2 barcode reader: empty software-version response during handshake. "
+ "Verify port, baud rate, and that the reader is powered on."
+ )
+ logger.info("[KX2 BCR %s] software version: %s", self.driver.io.port, version)
+ await self.driver.set_read_mode("single")
+
+ async def scan_barcode(self, read_time: Optional[float] = None) -> Optional[Barcode]:
+ # Reader's Y-command only takes integer seconds 1..9 (YM=indefinite). When
+ # the caller specifies read_time, push it to the device first so the
+ # on-device window matches our wait bound. Otherwise leave whatever's
+ # currently configured and wait long enough for any in-range setting.
+ if read_time is not None:
+ if read_time <= 0:
+ raise ValueError("read_time must be > 0")
+ await self.driver.set_read_time(int(round(read_time)))
+ await self.driver.trigger(True)
+ timeout = (read_time + 1.0) if read_time is not None else self._DEFAULT_SCAN_WAIT
+ try:
+ data = await self.driver.read_decoded_barcode(timeout=timeout)
+ except BarcodeScannerError:
+ # Driver raises on serial-read timeout. At this layer that's the
+ # "nothing decoded within the window" signal — return None instead of
+ # propagating. Real comms failures aren't reported via this path.
+ return None
+ if not data:
+ return None
+ return Barcode(data=data, symbology="ANY 1D", position_on_resource="front")
+
+
+class KX2BarcodeReader(Device):
+ """PAA KX2 onboard barcode reader (Microscan-style serial device).
+
+ Args:
+ port: Serial device path. On macOS this is typically
+ ``/dev/tty.PL2303G-USBtoUART`` (after the Prolific driver is
+ installed and approved — see module docstring). On Linux it's
+ usually ``/dev/ttyUSB``.
+ baudrate: Serial baud rate; the reader's factory default is 9600.
+
+ Usage::
+
+ bcr = KX2BarcodeReader(port="/dev/tty.PL2303G-USBtoUART11240")
+ await bcr.setup()
+ barcode = await bcr.barcode_scanning.scan(read_time=8)
+ print(barcode.data)
+ await bcr.stop()
+ """
+
+ def __init__(
+ self,
+ port: str,
+ baudrate: int = KX2BarcodeReaderDriver.default_baudrate,
+ ):
+ driver = KX2BarcodeReaderDriver(port=port, baudrate=baudrate)
+ super().__init__(driver=driver)
+ self.driver: KX2BarcodeReaderDriver = driver
+ self._backend = KX2BarcodeReaderBackend(driver)
+ self.barcode_scanning = BarcodeScanner(backend=self._backend)
+ self._capabilities = [self.barcode_scanning]
+
+ async def set_read_mode(self, mode: ReadMode) -> None:
+ """Set the trigger mode: ``"single"`` (default), ``"multiple"``, or
+ ``"continuous"``."""
+ await self.driver.set_read_mode(mode)
diff --git a/pylabrobot/paa/kx2/config.py b/pylabrobot/paa/kx2/config.py
new file mode 100644
index 00000000000..7ef960a9802
--- /dev/null
+++ b/pylabrobot/paa/kx2/config.py
@@ -0,0 +1,145 @@
+"""Per-arm configuration loaded from the KX2 drives during setup.
+
+The arm-specific calibration (link lengths, travel limits, motor conversion
+factors) lives on the drives themselves; the backend reads it once into a
+single `KX2Config` record at setup. The backend holds it as
+`Optional[KX2Config]` (None pre-setup) — but every field on the record
+itself is required, because once setup has run the whole record is filled.
+"""
+
+from dataclasses import dataclass
+from enum import IntEnum
+from typing import Dict, Literal, Optional
+
+from pylabrobot.paa.kx2.driver import JointMoveDirection
+
+
+GripperFingerSide = Literal["barcode_reader", "proximity_sensor"]
+
+
+class Axis(IntEnum):
+ """KX2 axis -> CANopen node-ID mapping."""
+
+ SHOULDER = 1
+ Z = 2
+ ELBOW = 3
+ WRIST = 4
+ RAIL = 5
+ SERVO_GRIPPER = 6
+
+ @property
+ def is_motion(self) -> bool:
+ """The four-axis arm proper. Excludes the rail (Cartesian carrier) and
+ the servo gripper (end-effector). Used for setup, halt, freedrive — any
+ operation that targets "the arm" without its peripherals."""
+ return self in (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST)
+
+ @property
+ def is_linear(self) -> bool:
+ """Axis travels in linear units (mm/s, mm/s^2). All others are rotary
+ (deg/s, deg/s^2). Used to pick the right speed/acceleration from the
+ linear/rotary split in JointMoveParams / CartesianMoveParams."""
+ return self in (Axis.Z, Axis.RAIL, Axis.SERVO_GRIPPER)
+
+
+@dataclass
+class AxisConfig:
+ """Per-axis params loaded from a single drive during setup.
+
+ Lives in `KX2Config.axes[axis]`. Order of fields here doesn't reflect
+ drive register order — they're grouped by what they describe.
+ """
+
+ motor_conversion_factor: float
+ max_travel: float
+ min_travel: float
+ unlimited_travel: bool
+ absolute_encoder: bool
+ max_vel: float
+ max_accel: float
+ joint_move_direction: JointMoveDirection
+
+ # I/O pin assignments — channel -> human-readable name ("GripperSensor",
+ # "Buzzer", "AuxPin42", or "" when unassigned). Probed from UI[5..16] but
+ # not currently consumed; kept for introspection.
+ digital_inputs: Dict[int, str]
+ analog_inputs: Dict[int, str]
+ outputs: Dict[int, str]
+
+
+@dataclass
+class GripperParams:
+ """User-supplied gripper tooling — known at construction, never read
+ from the drives. Lives on :class:`KX2ArmBackend`
+ (``self._gripper_params``) and is passed into kinematics alongside
+ :class:`KX2Config`. Distinct from :class:`ServoGripperConfig`, which
+ is drive-read motor calibration for the servo gripper itself.
+
+ Attributes:
+ length: Distance from the wrist axis to the gripper's *grip center*
+ (the geometric midpoint between the jaws, where a held plate
+ sits), in mm. Always non-negative — the "which side" choice is
+ encoded in :attr:`finger_side`, not the sign of this length.
+ The gripper assembly hangs off the wrist axis along its
+ extension direction, with both fingers clustered around the
+ grip center; FK/IK route ``location`` through this offset.
+ z_offset: Vertical offset from the wrist plate to the grip center,
+ in mm. Positive = grip center sits below the wrist plate.
+ finger_side: Which finger is treated as the gripper's "front" —
+ i.e., the one the reported world yaw (``rotation.z``) points at.
+ Flipping side is just a 180° relabel: for the same joints the
+ grip center is unchanged and only the reported yaw shifts by
+ 180°. For the same target ``(location, rotation.z)``, IK puts
+ the wrist axis on opposite sides of the grip center (separated
+ by ``2·length`` along the front-finger axis) — the gripper
+ assembly has to swing around the wrist motor to point the
+ chosen finger forward.
+ """
+
+ length: float = 0.0
+ z_offset: float = 0.0
+ finger_side: GripperFingerSide = "barcode_reader"
+
+
+@dataclass
+class ServoGripperConfig:
+ """Servo gripper params (axis 6 UF6..UF17). Only present when the
+ gripper is detected on the bus."""
+
+ home_pos: int
+ home_search_vel: int
+ home_search_accel: int
+ home_default_position_error: int
+ home_hard_stop_position_error: int
+ home_hard_stop_offset: int
+ home_index_offset: int
+ home_offset_vel: int
+ home_offset_accel: int
+ home_timeout_msec: int
+ continuous_current: float
+ peak_current: float
+
+
+@dataclass
+class KX2Config:
+ """Drive-read calibration. Strictly contents pulled off the bus at
+ setup; tooling (gripper geometry) lives separately on
+ :class:`GripperParams` and is owned by the backend."""
+
+ # Geometry (read from the shoulder drive's UF8/UF9/UF10).
+ wrist_offset: float
+ elbow_offset: float
+ elbow_zero_offset: float
+
+ # Per-axis params keyed by Axis. Iterating `axes` (or `axes.keys()`)
+ # gives the axes present on this arm.
+ axes: Dict[Axis, AxisConfig]
+
+ # Robot-level clearance limits (shoulder UF6/UF7).
+ base_to_gripper_clearance_z: float
+ base_to_gripper_clearance_arm: float
+
+ robot_on_rail: bool
+
+ # None if no servo gripper is present on the bus.
+ servo_gripper: Optional[ServoGripperConfig]
diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py
new file mode 100644
index 00000000000..712d3535862
--- /dev/null
+++ b/pylabrobot/paa/kx2/driver.py
@@ -0,0 +1,1734 @@
+"""Low-level CAN transport + CANopen/DS402 drive primitives for the PAA KX2.
+
+Uses the `canopen` library (python-can bus + CANopen SDO/PDO/NMT/EMCY).
+Paired with :class:`KX2ArmBackend` in ``arm_backend.py`` via the standard
+``Device`` + ``Driver`` + capability-backend split.
+
+This module is purely a CAN transport + Elmo interpreter layer. It knows only
+CANopen node IDs (ints). All axis-level / robot-topology concepts (axis names,
+motion-axis tuples, home status, move plans, joint-move direction, homing
+sequences) live in ``arm_backend``.
+"""
+
+from __future__ import annotations
+
+import asyncio
+import contextlib
+import logging
+import struct
+import time
+from dataclasses import dataclass, field
+from enum import IntEnum
+from typing import Callable, Dict, List, Optional, Tuple, Union
+
+from pylabrobot.capabilities.capability import BackendParams
+from pylabrobot.device import Driver
+
+try:
+ import canopen
+
+ _HAS_CANOPEN = True
+except ImportError as _e:
+ _HAS_CANOPEN = False
+ _CANOPEN_IMPORT_ERROR = _e
+
+
+def _u32_le(value: int) -> List[int]:
+ return list((value & 0xFFFFFFFF).to_bytes(4, byteorder="little", signed=False))
+
+
+class COBType(IntEnum):
+ NMT = 0
+ EMCY = 1
+ SYNC = 1
+ TIMESTAMP = 2
+ TPDO1 = 3
+ RPDO1 = 4
+ TPDO2 = 5
+ RPDO2 = 6
+ TPDO3 = 7
+ RPDO3 = 8
+ TPDO4 = 9
+ RPDO4 = 10
+ TSDO = 11
+ RSDO = 12
+ ERRCTRL = 14
+ HEARTBEAT = 14
+
+
+class RPDO(IntEnum):
+ RPDO1 = 1
+ RPDO3 = 3
+ RPDO4 = 4
+
+
+class PDOTransmissionType(IntEnum):
+ SynchronousAcyclic = 0
+ SynchronousCyclic = 1
+ EventDrivenManf = 254 # 0xFE
+ EventDrivenDev = 255 # 0xFF
+
+
+class RPDOMappedObject(IntEnum):
+ NotMapped = 0
+ ControlWord = 0x60400010
+ TargetTorque = 0x60710010
+ MaxTorque = 0x60720010
+ TargetPosition = 0x607A0020
+ VelocityOffset = 0x60B10020
+ TargetPositionIP = 0x60C10120
+ TargetVelocityIP = 0x60C10220
+ DigitalOutputs = 0x60FE0020
+ TargetVelocity = 0x60FF0020
+
+
+class TPDO(IntEnum):
+ TPDO1 = 1
+ TPDO3 = 3
+ TPDO4 = 4
+
+
+class TPDOTrigger(IntEnum):
+ MotionComplete = 0
+ MainHomingComplete = 1
+ AuxiliaryHomingComplete = 2
+ MotorShutDownByException = 3
+ MotorStarted = 4
+ UserProgramEmitCommand = 5
+ OSInterpreterExecutionComplete = 6
+ MotionStartedEvent = 8
+ PDODataChanged = 24
+ DigitalInputEvent = 26
+ StatusWordEvent = 27
+ BinaryInterpreterCommandComplete = 31
+
+
+class TPDOMappedObject(IntEnum):
+ NotMapped = 0
+ Timestamp = 0x20410020
+ PVTHeadPointer = 0x2F110010
+ PVTTailPointer = 0x2F120010
+ StatusWord = 0x60410010
+ PositionActualValue = 0x60640020
+ VelocityDemandValue = 0x606B0020
+ VelocityActualValue = 0x606C0020
+ TorqueDemandValue = 0x60740010
+ TorqueActualValue = 0x60770010
+ IPBufferPosition = 0x60C40410
+ DigitalInputs = 0x60FD0020
+
+
+class _ElmoObjectDataType(IntEnum):
+ UNSIGNED8 = 0
+ UNSIGNED16 = 1
+ UNSIGNED32 = 2
+ UNSIGNED64 = 3
+ INTEGER8 = 4
+ INTEGER16 = 5
+ INTEGER32 = 6
+ INTEGER64 = 7
+ STR = 8
+
+
+class CanError(Exception):
+ """Custom exception for CAN motor errors."""
+
+
+class _InputLogic(IntEnum):
+ """Elmo SimplIQ IL[N] codes. Even = active-low; odd (value+1) = active-high."""
+ GeneralPurpose = 0
+ StopForward = 2
+ StopReverse = 4
+ BeginMotion = 6
+ SoftStop = 8
+ MainHomeEnable = 10
+ AuxHomeEnable = 12
+ StopUnderControl = 14
+ AbortMotion = 16
+
+
+class JointMoveDirection(IntEnum):
+ """Move-direction hint used by the driver's move primitives.
+
+ Lives here (not in the backend) because the driver's
+ `motor_set_move_direction` primitive consumes it to program Elmo's modulo
+ mode register. Backend-side planning also uses it, but the canonical
+ definition is the driver's.
+ """
+
+ Normal = 0
+ Clockwise = 1
+ Counterclockwise = 2
+ ShortestWay = 3
+
+
+@dataclass
+class MotorMoveParam:
+ """One axis of a coordinated move, expressed purely in node-ID terms."""
+
+ # CANopen node ID for this axis. Backend passes `int(self.Axis.X)`.
+ node_id: int
+ position: int
+ velocity: int # encoder counts/sec (driver-internal; backend converts from mm/s or deg/s)
+ acceleration: int # encoder counts/sec^2
+ relative: bool = False
+ direction: JointMoveDirection = JointMoveDirection.ShortestWay
+
+
+@dataclass
+class MotorsMovePlan:
+ moves: List[MotorMoveParam] = field(default_factory=list)
+ move_time: float = 10.0
+
+
+# Vendor-specific Elmo binary interpreter rides on PDO2 COB-IDs (non-standard).
+# Request: RPDO2 = (6 << 7) | node_id = 0x300 + node_id
+# Response: TPDO2 = (5 << 7) | node_id = 0x280 + node_id
+_BI_REQUEST_COB_BASE = 0x300
+_BI_RESPONSE_COB_BASE = 0x280
+_EMCY_COB_BASE = 0x80
+_TPDO3_COB_BASE = 0x380
+_GROUP_NODE_ID = 10
+
+logger = logging.getLogger(__name__)
+
+
+@dataclass
+class EmcyFrame:
+ """Parsed CANopen EMCY frame with Elmo manufacturer-specific fields.
+
+ Layout matches the C# `sEmcy` struct (clscanmotor.cs:6966-6978):
+ bytes [0..2) ErrCode (u16 LE), [2] ErrReg (u8), [3] ElmoErrCode (u8),
+ [4..6) ErrCodeData1 (u16 LE), [6..8) ErrCodeData2 (u16 LE).
+ """
+
+ err_code: int
+ err_reg: int
+ elmo_err_code: int
+ data1: int
+ data2: int
+
+
+@dataclass
+class _NodeEmcyState:
+ """All EMCY state we track for one drive node — single source of truth.
+
+ Three flavors of consumer pull from this:
+ - **Streaming runtime** reads ``queue_full`` / ``underflow`` to detect
+ IPM backpressure. ``queue_low`` is a proactive warning (drive's IP
+ buffer approaching empty); ``underflow`` is post-fact (drive ran the
+ buffer dry and short-stopped the trajectory).
+ - **Motion-poll path** reads ``move_error`` to raise on the next
+ ``motor_check_if_move_done`` after a fault-class EMCY arrived. The
+ string is preformatted with axis context.
+ - **Diagnostics** read ``last_frame`` for the last raw EMCY received
+ from this node.
+
+ Reset (whole struct) on re-enable / fault clear / find_z post-IL-trip.
+ """
+
+ queue_low: bool = False
+ queue_low_write_pointer: int = 0
+ queue_low_read_pointer: int = 0
+ queue_full: bool = False
+ queue_full_failed_write_pointer: int = 0
+ underflow: bool = False
+ bad_head_pointer: bool = False
+ bad_mode_init_data: bool = False
+ motion_terminated: bool = False
+ out_of_modulo: bool = False
+ move_error: Optional[str] = None
+ last_frame: Optional["EmcyFrame"] = None
+
+
+# Standard CANopen error codes that don't depend on the Elmo byte.
+# Source: clscanmotor.cs:1108-1267. The (description, disable_motors) tuple
+# corresponds to (str1, flag1) in the C#.
+_EMCY_STANDARD: Dict[int, Tuple[str, bool]] = {
+ 0x8110: ("CAN message lost (corrupted or overrun)", False),
+ 0x8200: ("Protocol error (unrecognized NMT request)", False),
+ 0x8210: ("Attempt to access an unconfigured RPDO", False),
+ 0x8130: ("Heartbeat event", False),
+ 0x6180: ("Fatal CPU error: stack overflow", False),
+ 0x6200: ("User program aborted by an error", False),
+ 0xFF01: ("Request by user program 'emit' function", False),
+ 0x6300: (
+ "Object mapped to an RPDO returned an error during interpretation "
+ "or a referenced motion failed to be performed",
+ False,
+ ),
+ 0x7300: ("Resolver or Analog Encoder feedback failed", True),
+ 0x7380: ("Feedback loss: no match between encoder and Hall locations.", True),
+ 0x8311: (
+ "Peak current has been exceeded due to drive malfunction or badly-tuned "
+ "current controller",
+ True,
+ ),
+ 0x5441: ("E-stop button was pressed", True),
+ 0x5280: ("ECAM table problem", False),
+ 0x7381: (
+ "Two digital Hall sensors changed at once; only one sensor can be changed "
+ "at a time",
+ True,
+ ),
+ 0x8480: ("Speed tracking error", True),
+ 0x8611: ("Position tracking error", True),
+ 0x6320: ("Cannot start due to inconsistent database", False),
+ 0x8380: ("Cannot find electrical zero of motor when attempting to start motor", False),
+ 0x8481: ("Speed limit exceeded", True),
+ 0x6181: ("CPU exception: fatal exception", False),
+ 0x5281: ("Timing error", False),
+ 0x7121: ("Motor stuck: motor powered but not moving", True),
+ 0x8680: ("Position limit exceeded", True),
+ 0x8381: ("Cannot tune current offsets", False),
+ 0xFF10: ("Cannot start motor", False),
+ 0x5400: ("Cannot start motor", False),
+ 0x3120: (
+ "Under-voltage: power supply is shut down or it has too high an output impedance",
+ True,
+ ),
+ 0x3310: (
+ "Over-voltage: power supply voltage is too high or servo driver could not "
+ "absorb kinetic energy while braking a load",
+ True,
+ ),
+ 0x2340: (
+ "Short circuit: motor or its wiring may be defective, or drive is faulty",
+ True,
+ ),
+ 0x4310: ("Temperature: drive overheating", True),
+ 0xFF20: ("Safety switch is sensed - Drive in safety state", True),
+}
+
+
+def _decode_emcy(
+ frame: EmcyFrame, state: _NodeEmcyState
+) -> Tuple[str, bool, bool]:
+ """Decode an EMCY frame into (description, disable_motors, suppress_callback).
+
+ Mutates ``state`` for IPM queue events. Mirrors clscanmotor.cs:1070-1267
+ one-for-one. ``suppress_callback`` corresponds to the C# `flag2` and is only
+ set for the elmo 0x8A interpolation underflow under errCode 0xFF02 — that
+ case updates internal state but does not raise the user-visible event.
+ """
+ err = frame.err_code
+ elmo = frame.elmo_err_code
+
+ # DS301 §7.2.7.1: err_code=0 is the "no error / error reset" frame, emitted
+ # on bootup and after a fault clears. Suppress the user callback — re-enable
+ # is the explicit acknowledgment, not a spontaneous reset frame. The elmo
+ # byte is ignored: any frame with err_code=0 is a reset regardless.
+ if err == 0:
+ return ("Error reset", False, True)
+
+ if err == 0xFF00:
+ if elmo == 0x56:
+ state.queue_low = True
+ state.queue_low_write_pointer = frame.data1
+ state.queue_low_read_pointer = frame.data2
+ return ("Queue Low", False, False)
+ if elmo == 0x5B:
+ state.bad_head_pointer = True
+ return ("Bad Head Pointer", False, False)
+ if elmo == 0x34:
+ state.queue_full = True
+ state.queue_full_failed_write_pointer = frame.data1
+ return ("Queue Full", False, False)
+ if elmo == 0x07:
+ state.bad_mode_init_data = True
+ return ("Bad Mode Init Data", False, False)
+ if elmo == 0x08:
+ state.motion_terminated = True
+ return ("Motion Terminated", False, False)
+ if elmo == 0xA6:
+ state.out_of_modulo = True
+ return ("Out Of Modulo", False, False)
+ return (f"Unknown vendor EMCY 0xFF00/0x{elmo:02X}", False, False)
+
+ if err == 0xFF02:
+ if elmo == 0x07:
+ state.bad_mode_init_data = True
+ return ("Bad Mode Init Data", False, False)
+ if elmo == 0x08:
+ state.motion_terminated = True
+ return ("Motion Terminated", False, False)
+ if elmo == 0x34:
+ state.queue_full = True
+ state.queue_full_failed_write_pointer = frame.data1
+ return ("Queue Full", False, False)
+ if elmo == 0x56:
+ state.queue_low = True
+ state.queue_low_write_pointer = frame.data1
+ state.queue_low_read_pointer = frame.data2
+ return ("Queue Low", False, False)
+ if elmo == 0x5B:
+ state.bad_head_pointer = True
+ return ("Bad Head Pointer", False, False)
+ if elmo == 0x8A:
+ # State update only; user callback suppressed (C# flag2=true). Sets
+ # `underflow` (post-fact: drive already ran out of points) — distinct
+ # from `queue_low` (proactive: more data needed soon).
+ state.underflow = True
+ return ("Position Interpolation buffer underflow", False, True)
+ if elmo == 0xA6:
+ state.out_of_modulo = True
+ return ("Out Of Modulo", False, False)
+ if elmo == 0xBA:
+ state.queue_full = True
+ return ("Interpolation queue is full", False, False)
+ if elmo == 0xBB:
+ return ("Incorrect interpolation sub-mode", False, False)
+ return (f"DS402 IP Error 0x{elmo:02X}", False, False)
+
+ std = _EMCY_STANDARD.get(err)
+ if std is not None:
+ desc, disable = std
+ return (desc, disable, False)
+
+ return (f"Unknown EMCY 0x{err:04X}/0x{elmo:02X}", False, False)
+
+
+class KX2Driver(Driver):
+ """CANopen-library-backed KX2 drive transport.
+
+ Uses `canopen.Network` for bus ownership + NMT, `node.sdo` for SDO traffic,
+ raw SDO writes to 0x14xx/0x16xx/0x18xx/0x1Axx for PDO mapping, and
+ `network.send_message` / `network.subscribe` for the vendor-specific Elmo
+ binary interpreter (non-standard, rides on TPDO2/RPDO2 COB-IDs).
+
+ Pure CAN transport + Elmo interpreter primitives — takes node IDs (ints),
+ knows nothing about robot topology. Axis-level concepts live in
+ :class:`KX2ArmBackend`.
+ """
+
+ def __init__(
+ self,
+ has_rail: bool = False,
+ has_servo_gripper: bool = True,
+ interface: str = "pcan",
+ channel: Optional[str] = None,
+ bitrate: int = 500000,
+ ) -> None:
+ # The non-default topologies (rail-mounted KX2, gripper-less KX2)
+ # have shim code paths in this driver and the backend, but neither
+ # has been exercised against real hardware. KX2ArmBackend._on_setup
+ # also calls servo_gripper_initialize unconditionally. Refuse the
+ # configuration up front rather than letting users hit cryptic
+ # failures downstream.
+ if has_rail or not has_servo_gripper:
+ raise NotImplementedError(
+ "KX2 has only been tested with the default 4-axis arm + servo "
+ "gripper topology (has_rail=False, has_servo_gripper=True). "
+ "Other configurations have shim code paths but the setup / "
+ "homing layer needs work — see KX2ArmBackend._on_setup and "
+ "servo_gripper_initialize."
+ )
+
+ super().__init__()
+ self._interface = interface
+ self._channel = channel
+ self._bitrate = bitrate
+
+ self.has_rail = has_rail
+ self.has_servo_gripper = has_servo_gripper
+
+ self.node_id_list: List[int] = [1, 2, 3, 4]
+ if has_rail:
+ self.node_id_list.append(5)
+ if has_servo_gripper:
+ self.node_id_list.append(6)
+
+ # Motion axes = shoulder/Z/elbow/wrist. Driver only knows node IDs;
+ # axis-level names live in the backend's KX2ArmBackend.Axis enum.
+ self.motion_node_ids: List[int] = [1, 2, 3, 4]
+
+ self._network: Optional[canopen.Network] = None
+ self._nodes: Dict[int, canopen.RemoteNode] = {}
+ self._loop: Optional[asyncio.AbstractEventLoop] = None
+
+ # Pending binary-interpreter response futures keyed by
+ # (node_id, msg_type, msg_index). Set from the canopen listener thread
+ # via loop.call_soon_threadsafe; only the event-loop thread touches
+ # this dict directly.
+ self._pending_bi: Dict[Tuple[int, str, int], asyncio.Future] = {}
+
+ # Per-(node, cmd, idx) lock around the future-install + send + await
+ # cycle in _send_bi. Without it, two coroutines firing the same
+ # request would clobber each other's pending-future entry; with the
+ # motion lock above mitigating most of it but direct driver callers
+ # (notebook diagnostics) and gathered queries with overlapping keys
+ # still hit the race. setdefault(asyncio.Lock()) is fine because the
+ # garbage Lock from a missed-race insert is immediately discarded.
+ self._bi_locks: Dict[Tuple[int, str, int], asyncio.Lock] = {}
+
+ self._ipm_mode: bool = False
+
+ # EMCY (CANopen Emergency, COB-ID 0x80 + node_id) state. Subscribed in
+ # setup(); fires on the canopen listener thread, marshalled into the
+ # asyncio loop via _make_emcy_callback. Per-node `_NodeEmcyState` is
+ # the single source of truth (queue counters, sticky fault, last raw
+ # frame); `emcy_move_error` and `last_emcy` are derived properties.
+ self._emcy: Dict[int, _NodeEmcyState] = {}
+ # Tracks which node fired the most recent EMCY frame, so `last_emcy`
+ # can resolve to the right node's `last_frame`. Cheaper than scanning
+ # the dict on every read.
+ self._last_emcy_node_id: Optional[int] = None
+ self._emcy_callbacks: List[
+ Callable[[int, EmcyFrame, str, bool], None]
+ ] = []
+
+ # StatusWord (0x6041) push-cache from TPDO3, COB-ID 0x380+node_id. The
+ # canopen listener thread parses the 2-byte SW out of each TPDO3 frame and
+ # marshals (sw, set_event) onto the asyncio loop. _wait_setpoint_ack reads
+ # the cache + waits on the event instead of polling 0x6041 via SDO.
+ self._statusword: Dict[int, int] = {}
+ self._statusword_event: Dict[int, asyncio.Event] = {}
+
+ @property
+ def loop(self) -> asyncio.AbstractEventLoop:
+ """Event loop captured in setup(). Raises if accessed before setup()."""
+ if self._loop is None:
+ raise RuntimeError("KX2Driver event loop not initialized; call setup() first.")
+ return self._loop
+
+ @property
+ def emcy_move_error(self) -> Optional[str]:
+ """First pending fault across all nodes; ``None`` if no fault.
+
+ Pre-formatted with axis context (``"Axis {nid} {description}"``).
+ `motor_check_if_move_done` raises on this; recovery paths clear it
+ via `clear_emcy_state`.
+ """
+ for st in self._emcy.values():
+ if st.move_error is not None:
+ return st.move_error
+ return None
+
+ @property
+ def last_emcy(self) -> Optional[EmcyFrame]:
+ """Most recent EMCY frame received from any node; ``None`` if none yet.
+
+ Diagnostic only — motion logic uses `emcy_move_error` for fault
+ detection and `_emcy[nid]` for per-axis IPM queue state."""
+ if self._last_emcy_node_id is None:
+ return None
+ st = self._emcy.get(self._last_emcy_node_id)
+ return st.last_frame if st is not None else None
+
+ # --- lifecycle -----------------------------------------------------------
+
+ async def setup(self, backend_params: Optional[BackendParams] = None) -> None:
+ """Bring up the CAN bus, reset/start nodes, and configure PDO mapping."""
+ if not _HAS_CANOPEN:
+ raise ImportError(
+ "canopen is not installed. Install with `pip install pylabrobot[canopen]` "
+ f"(import error: {_CANOPEN_IMPORT_ERROR})"
+ )
+ if self._network is not None:
+ await self.stop()
+
+ self._loop = asyncio.get_running_loop()
+
+ network = canopen.Network()
+ network.connect(interface=self._interface, channel=self._channel, bitrate=self._bitrate)
+ self._network = network
+
+ # Subscribe to EMCY before Start All Nodes — bootup / fault frames
+ # emitted between NMT start and per-node setup would otherwise be lost
+ # (canopen's listener doesn't buffer pre-subscribe messages). Mirrors
+ # the C# event handler at KX2RobotControl.cs:15384-15425 /
+ # clscanmotor.cs:1057-1284.
+ for nid in self.node_id_list:
+ self._emcy[nid] = _NodeEmcyState()
+ network.subscribe(_EMCY_COB_BASE + nid, self._make_emcy_callback(nid))
+
+ # Reset all nodes, then start scanner so bootup messages populate it,
+ # then start all nodes.
+ network.nmt.send_command(0x82)
+ await asyncio.sleep(0.5)
+ network.scanner.search()
+ network.nmt.send_command(0x01)
+ await asyncio.sleep(0.5)
+
+ discovered = sorted(network.scanner.nodes)
+ if discovered != self.node_id_list:
+ raise CanError(
+ f"Node IDs on CAN bus do not match expected list: "
+ f"{discovered} != {self.node_id_list}"
+ )
+
+ for nid in self.node_id_list:
+ node = network.add_node(nid, canopen.ObjectDictionary())
+ # canopen's default SDO response timeout is 0.3s, which is tight for
+ # drives that queue vendor objects (Elmo 0x20xx/0x30xx). Match the 1s
+ # the legacy driver used for its own futures.
+ node.sdo.RESPONSE_TIMEOUT = 1.0
+ self._nodes[nid] = node
+ # Elmo binary-interpreter response subscription. BI traffic only
+ # happens after explicit user commands, so subscribing here is fine.
+ network.subscribe(_BI_RESPONSE_COB_BASE + nid, self._make_bi_callback(nid))
+
+ logger.info("canopen: connected, nodes=%s", discovered)
+
+ # TPDO3 push for StatusWord (0x6041) so _wait_setpoint_ack can wait on
+ # the bit-12 transition without an SDO round-trip per poll. Subscribe
+ # before remapping so we don't lose the first frame the drive emits
+ # when the new event-trigger arms. 1 ms inhibit (10 * 100 us) caps
+ # bus traffic — SW changes happen at the ~1-2 ms servo cycle, so the
+ # inhibit doesn't lose edges in practice and keeps the bus quiet
+ # during IPM streaming if anyone resurrects the IPM runtime.
+ for nid in self.motion_node_ids:
+ self._statusword_event[nid] = asyncio.Event()
+ network.subscribe(_TPDO3_COB_BASE + nid, self._make_tpdo3_callback(nid))
+
+ # Unmap TPDO1, map TPDO3 (StatusWord, triggered on any SW change) and
+ # TPDO4 (DigitalInputs, triggered on edge).
+ for node_id in self.node_id_list:
+ await self._can_tpdo_unmap(TPDO.TPDO1, node_id)
+ await self._tpdo_map(
+ TPDO.TPDO3, node_id, [TPDOMappedObject.StatusWord],
+ TPDOTrigger.StatusWordEvent, delay_100_us=10,
+ )
+ await self._tpdo_map(
+ TPDO.TPDO4, node_id, [TPDOMappedObject.DigitalInputs], TPDOTrigger.DigitalInputEvent
+ )
+
+ # Elmo vendor objects: interpolation config for IPM.
+ for nid in self.motion_node_ids:
+ await self.can_sdo_download_elmo_object(nid, 24768, 0, -1, _ElmoObjectDataType.INTEGER16)
+ await self.can_sdo_download_elmo_object(nid, 24772, 2, 16, _ElmoObjectDataType.UNSIGNED32)
+ await self.can_sdo_download_elmo_object(nid, 24772, 3, 0, _ElmoObjectDataType.UNSIGNED8)
+ await self.can_sdo_download_elmo_object(nid, 24772, 5, 8, _ElmoObjectDataType.UNSIGNED8)
+ await self.can_sdo_download_elmo_object(nid, 24770, 2, -3, _ElmoObjectDataType.INTEGER8)
+ await self.can_sdo_download_elmo_object(nid, 24669, 0, 1, _ElmoObjectDataType.INTEGER16)
+
+ # RPDO1 = ControlWord (for DS402 enable), RPDO3 = interpolated target.
+ for nid in self.motion_node_ids:
+ await self._rpdo_map(
+ RPDO.RPDO1, nid, [RPDOMappedObject.ControlWord],
+ PDOTransmissionType.SynchronousCyclic,
+ )
+ await self._rpdo_map(
+ RPDO.RPDO3, nid,
+ [RPDOMappedObject.TargetPositionIP, RPDOMappedObject.TargetVelocityIP],
+ PDOTransmissionType.EventDrivenDev,
+ )
+
+ self._ipm_mode = True
+ await self.ipm_select_mode(False)
+
+ async def stop(self) -> None:
+ if self._network is not None:
+ # Drop _loop first so racing listener-thread _cb()s no-op at their
+ # `if self._loop is None: return` guard before they try to schedule
+ # onto a torn-down loop. Network reference clears after disconnect.
+ self._loop = None
+ self._network.disconnect()
+ self._network = None
+ self._nodes = {}
+ self._emcy = {}
+ self._last_emcy_node_id = None
+ # Clear callbacks too: _on_setup re-registers on each retry, so leaving
+ # them would compound N copies of the same handler across attempts.
+ self._emcy_callbacks = []
+ self._statusword = {}
+ self._statusword_event = {}
+
+ # --- PDO configuration (pure SDO writes; no library-PDO machinery) ------
+
+ async def _can_tpdo_unmap(self, tpdo: TPDO, node_id: int) -> None:
+ cob_type_int = {
+ TPDO.TPDO1: COBType.TPDO1.value,
+ TPDO.TPDO3: COBType.TPDO3.value,
+ TPDO.TPDO4: COBType.TPDO4.value,
+ }[tpdo]
+ node_id &= 0x7F
+ num1 = ((cob_type_int & 0x01) << 7) | node_id
+ num2 = (cob_type_int >> 1) & 0x07
+ await self._can_sdo_download(node_id, 0x1800 + tpdo.value - 1, 1, [num1, num2, 0, 0xC0])
+ await self._can_sdo_download(node_id, 0x1A00 + tpdo.value - 1, 0, [0, 0, 0, 0])
+
+ async def _rpdo_map(
+ self,
+ rpdo: RPDO,
+ node_id: int,
+ mapped_objects: List[RPDOMappedObject],
+ transmission_type: PDOTransmissionType,
+ ) -> None:
+ rpdo_idx = (int(rpdo) - 1) & 0xFF
+ cob_type = {
+ RPDO.RPDO1: COBType.RPDO1, RPDO.RPDO3: COBType.RPDO3, RPDO.RPDO4: COBType.RPDO4,
+ }[rpdo]
+ cob_id_11 = ((int(cob_type) & 0x0F) << 7) | (node_id & 0x7F)
+
+ # Disable PDO (bit 31 set)
+ await self._can_sdo_download(node_id, 0x1400 + rpdo_idx, 1, _u32_le(0x80000000 | cob_id_11))
+ # Clear mapping count
+ await self._can_sdo_download(node_id, 0x1600 + rpdo_idx, 0, [0, 0, 0, 0])
+ # Transmission type
+ await self._can_sdo_download(
+ node_id, 0x1400 + rpdo_idx, 2, [int(transmission_type) & 0xFF, 0, 0, 0]
+ )
+ # Mapped objects
+ for i, mo in enumerate(mapped_objects):
+ await self._can_sdo_download(node_id, 0x1600 + rpdo_idx, i + 1, _u32_le(int(mo)))
+ # Mapping count
+ await self._can_sdo_download(
+ node_id, 0x1600 + rpdo_idx, 0, [len(mapped_objects) & 0xFF, 0, 0, 0]
+ )
+ # Re-enable (clear bit 31)
+ await self._can_sdo_download(node_id, 0x1400 + rpdo_idx, 1, _u32_le(cob_id_11))
+
+ async def _tpdo_map(
+ self,
+ tpdo: TPDO,
+ node_id: int,
+ mapped_objects: List[TPDOMappedObject],
+ event_trigger: TPDOTrigger,
+ event_timer_ms: int = 0,
+ delay_100_us: int = 0,
+ transmission_type: PDOTransmissionType = PDOTransmissionType.EventDrivenDev,
+ ) -> None:
+ tpdo_idx = (int(tpdo) - 1) & 0xFF
+ cob_type = {
+ TPDO.TPDO1: COBType.TPDO1, TPDO.TPDO3: COBType.TPDO3, TPDO.TPDO4: COBType.TPDO4,
+ }[tpdo]
+ cob_id_11 = ((int(cob_type) & 0x0F) << 7) | (node_id & 0x7F)
+ event_mask = 1 << int(event_trigger)
+
+ # Disable TPDO (bit 30 + 31)
+ await self._can_sdo_download(node_id, 0x1800 + tpdo_idx, 1, _u32_le(0xC0000000 | cob_id_11))
+ # Clear mapping count
+ await self._can_sdo_download(node_id, 0x1A00 + tpdo_idx, 0, [0, 0, 0, 0])
+ # Transmission type
+ await self._can_sdo_download(
+ node_id, 0x1800 + tpdo_idx, 2, [int(transmission_type) & 0xFF, 0, 0, 0]
+ )
+ # Inhibit / delay 100us
+ await self._can_sdo_download(node_id, 0x1800 + tpdo_idx, 3, [delay_100_us & 0xFF, 0, 0, 0])
+ # Event timer (ms)
+ await self._can_sdo_download(node_id, 0x1800 + tpdo_idx, 5, [event_timer_ms & 0xFF, 0, 0, 0])
+ # Vendor event mask at 0x2F20:
+ await self._can_sdo_download(node_id, 0x2F20, int(tpdo) & 0xFF, _u32_le(event_mask))
+ # Mapped objects
+ for i, mo in enumerate(mapped_objects):
+ await self._can_sdo_download(node_id, 0x1A00 + tpdo_idx, i + 1, _u32_le(int(mo)))
+ # Mapping count
+ await self._can_sdo_download(
+ node_id, 0x1A00 + tpdo_idx, 0, [len(mapped_objects) & 0xFF, 0, 0, 0]
+ )
+ # Re-enable (clear bits 30 + 31)
+ await self._can_sdo_download(node_id, 0x1800 + tpdo_idx, 1, _u32_le(cob_id_11))
+
+ # --- SDO -----------------------------------------------------------------
+
+ async def _can_sdo_upload(
+ self, node_id: int, index: int, sub_index: int,
+ ) -> bytes:
+ # node.sdo.upload is blocking I/O (library handles expedited + segmented
+ # transfers + abort codes); run off the event loop.
+ return await asyncio.to_thread(self._nodes[node_id].sdo.upload, index, sub_index)
+
+ async def _can_sdo_download(
+ self, node_id: int, index: int, sub_index: int, data: List[int],
+ ) -> None:
+ await asyncio.to_thread(
+ self._nodes[node_id].sdo.download, index, sub_index, bytes(data),
+ )
+
+ async def can_sdo_download_elmo_object(
+ self,
+ node_id: int,
+ elmo_object_int: int,
+ sub_index: int,
+ data: Union[int, float],
+ data_type: _ElmoObjectDataType,
+ ) -> None:
+ # Byte width + signedness derived from data_type; float inputs truncate to int.
+ _SDO_ELMO_PACK = {
+ _ElmoObjectDataType.UNSIGNED8: (1, False),
+ _ElmoObjectDataType.UNSIGNED16: (2, False),
+ _ElmoObjectDataType.UNSIGNED32: (4, False),
+ _ElmoObjectDataType.UNSIGNED64: (8, False),
+ _ElmoObjectDataType.INTEGER8: (1, True),
+ _ElmoObjectDataType.INTEGER16: (2, True),
+ _ElmoObjectDataType.INTEGER32: (4, True),
+ _ElmoObjectDataType.INTEGER64: (8, True),
+ }
+ spec = _SDO_ELMO_PACK.get(data_type)
+ if spec is None:
+ raise CanError(f"Unsupported data type for SDO Write: {data_type.name}")
+ width, signed = spec
+ data_bytes = list(int(data).to_bytes(width, "little", signed=signed))
+ await self._can_sdo_download(node_id, elmo_object_int, sub_index, data_bytes)
+
+ # --- EMCY (CANopen Emergency, COB-ID 0x80 + node_id) --------------------
+
+ def add_emcy_callback(
+ self, cb: Callable[[int, EmcyFrame, str, bool], None]
+ ) -> None:
+ """Register a callback fired on every (non-suppressed) EMCY frame.
+
+ Callback signature: ``cb(node_id, frame, description, disable_motors)``.
+ Always invoked on the asyncio loop thread captured in :meth:`setup`.
+ Exceptions raised by the callback are logged and swallowed so one bad
+ handler can't poison the rest.
+ """
+ self._emcy_callbacks.append(cb)
+
+ def clear_emcy_state(self, node_id: Optional[int] = None) -> None:
+ """Clear EMCY state.
+
+ With ``node_id``: reset that one node's `_NodeEmcyState` (queue
+ counters + sticky fault + last_frame). Without: clear the sticky
+ fault on every node — leaves queue counters intact since they're
+ stream-scoped and reset by `ipm_begin_motion`.
+ """
+ if node_id is not None:
+ if node_id in self._emcy:
+ self._emcy[node_id] = _NodeEmcyState()
+ return
+ for st in self._emcy.values():
+ st.move_error = None
+
+ def _make_emcy_callback(self, node_id: int):
+ """Return a `canopen.Network.subscribe` callback bound to a specific node."""
+
+ def _cb(cob_id: int, data: bytes, timestamp: float) -> None:
+ # Fires on canopen's listener thread. Marshal decoding into the loop.
+ if self._loop is None:
+ return
+ self._loop.call_soon_threadsafe(self._dispatch_emcy, node_id, bytes(data))
+
+ return _cb
+
+ def _dispatch_emcy(self, node_id: int, data: bytes) -> None:
+ if len(data) < 8:
+ logger.warning("EMCY frame too short from node %d: %s", node_id, data.hex())
+ return
+ err_code, err_reg, elmo_err, d1, d2 = struct.unpack(" None:
+ if self._loop is None:
+ return
+ if len(data) < 2:
+ # StatusWord is 2 bytes — a shorter frame means the drive's TPDO3
+ # mapping is wrong or a different sender is squatting on the COB-ID.
+ # _wait_setpoint_ack would silently fall back to SDO probing forever.
+ logger.warning(
+ "TPDO3 frame too short from node %d: %s (expected >=2 bytes)",
+ node_id, bytes(data).hex(),
+ )
+ return
+ sw = int.from_bytes(bytes(data[:2]), "little")
+ self._loop.call_soon_threadsafe(self._dispatch_statusword, node_id, sw)
+
+ return _cb
+
+ def _dispatch_statusword(self, node_id: int, sw: int) -> None:
+ self._statusword[node_id] = sw
+ ev = self._statusword_event.get(node_id)
+ if ev is not None:
+ ev.set()
+
+ # --- Elmo binary interpreter (vendor protocol on TPDO2/RPDO2) ------------
+
+ def _make_bi_callback(self, node_id: int):
+ """Return a `canopen.Network.subscribe` callback bound to a specific node."""
+
+ def _cb(cob_id: int, data: bytes, timestamp: float) -> None:
+ # Fires on canopen's listener thread. Marshal decoding into the loop.
+ if self._loop is None:
+ return
+ self._loop.call_soon_threadsafe(self._dispatch_bi_response, node_id, bytes(data))
+
+ return _cb
+
+ def _dispatch_bi_response(self, node_id: int, data: bytes) -> None:
+ if len(data) < 8:
+ logger.warning("Binary interpreter response too short from node %d: %s", node_id, data.hex())
+ return
+ msg_type = chr(data[0]) + chr(data[1])
+ msg_index = ((data[3] & 0x3F) << 8) | data[2]
+ is_int = (data[3] & 0x80) == 0
+ fmt = " List[Union[int, float]]:
+ """Frame + send an 8-byte binary-interpreter request; await one response
+ per target node. Each response is decoded to its native type (int or
+ float) by :meth:`_dispatch_bi_response`.
+ """
+ if self._network is None:
+ raise CanError("binary interpreter called before setup()")
+
+ timeout = 10.0 if cmd.upper() == "SV" else 1.0
+
+ byte0 = ord(cmd[0]) & 0xFF
+ byte1 = ord(cmd[-1]) & 0xFF
+ byte2 = cmd_index & 0xFF
+ byte3 = (cmd_index >> 8) & 0x3F
+ if is_query:
+ byte3 |= 0x40
+ if is_float:
+ byte3 |= 0x80
+
+ val_bytes = (
+ struct.pack(" int:
+ """Query an int-typed Elmo parameter. Returns the drive's current value."""
+ if node_id == _GROUP_NODE_ID:
+ raise CanError("Group queries are not supported")
+ resps = await self._send_bi(
+ node_id, cmd, cmd_index, is_query=True, is_execute=False, is_float=False,
+ )
+ return int(resps[0])
+
+ async def query_float(self, node_id: int, cmd: str, cmd_index: int) -> float:
+ """Query a float-typed Elmo parameter. Returns the drive's current value."""
+ if node_id == _GROUP_NODE_ID:
+ raise CanError("Group queries are not supported")
+ resps = await self._send_bi(
+ node_id, cmd, cmd_index, is_query=True, is_execute=False, is_float=True,
+ )
+ return float(resps[0])
+
+ async def write(
+ self, node_id: int, cmd: str, cmd_index: int, value: Union[int, float],
+ ) -> None:
+ """Write an Elmo parameter. The type of ``value`` selects int vs float
+ framing on the wire. The drive echoes the accepted value back, which we
+ verify — a mismatch raises :class:`CanError`.
+ """
+ is_float = isinstance(value, float)
+ resps = await self._send_bi(
+ node_id, cmd, cmd_index,
+ is_query=False, is_execute=False, is_float=is_float, value=value,
+ )
+ targets = (
+ list(self.motion_node_ids) if node_id == _GROUP_NODE_ID else [node_id]
+ )
+ for nid, resp in zip(targets, resps):
+ if is_float:
+ # Elmo stores floats as float32; the echo may drift slightly relative
+ # to our float64 input — accept within ~1% ratio.
+ exp, act = float(value), float(resp)
+ ok = exp == act or (act != 0.0 and 0.99 < exp / act < 1.01)
+ else:
+ ok = int(resp) == int(value)
+ if not ok:
+ raise CanError(
+ f"Unexpected CAN response: sent {cmd}[{cmd_index}]={value}, "
+ f"got {resp} from node {nid}"
+ )
+
+ async def execute(self, node_id: int, cmd: str, cmd_index: int = 0) -> None:
+ """Fire-and-forget execute (e.g. ``BG``). Awaits the drive's response so
+ the caller sees the command completed on the wire, but no echo-check."""
+ await self._send_bi(
+ node_id, cmd, cmd_index, is_query=False, is_execute=True, is_float=False,
+ )
+
+ async def _os_interpreter(
+ self,
+ node_id: int,
+ cmd: str,
+ *,
+ query: bool = False,
+ ) -> str:
+ """Run an OS interpreter command via standard CiA-301 OS Command objects.
+
+ Uses 0x1024 (OS Command Mode) + 0x1023 (OSCommand record) — the library
+ handles the expedited vs. segmented SDO choice and toggle-bit dance
+ automatically, replacing ~260 lines of hand-rolled segmented SDO in the
+ legacy driver.
+ """
+ if node_id not in self._nodes:
+ raise CanError(f"os_interpreter: unknown node {node_id}")
+ node = self._nodes[node_id]
+
+ # 0x1024:0 = OS Command Mode. Elmo/legacy code sets this to 0 ("evaluate
+ # immediately") before each command.
+ await asyncio.to_thread(node.sdo.download, 0x1024, 0, bytes([0]))
+
+ # 0x1023:1 = OSCommand.Command. ASCII-encoded; library segments if >4 bytes.
+ await asyncio.to_thread(node.sdo.download, 0x1023, 1, cmd.encode("ascii"))
+
+ # 0x1023:2 = OSCommand.Status (U8). This is the CiA-301 OS-command lifecycle
+ # byte, not an error flag:
+ # 0x00 no reply yet / no error 0x01 command is being executed
+ # 0x02 completed, no reply 0x03 completed with reply
+ # 0xFF no command
+ # For async `XQ##` dispatches the drive returns 0x01 immediately, which is
+ # expected — the caller (e.g. `user_program_run`) polls PS/UI afterward for
+ # completion. SDO abort codes surface as `SdoAbortedError` from the upload
+ # itself; we don't need to inspect the byte. Log at debug for diagnostics.
+ status_bytes = await asyncio.to_thread(node.sdo.upload, 0x1023, 2)
+ logger.debug(
+ "os_interpreter node=%d cmd=%r status=0x%02X",
+ node_id, cmd, int.from_bytes(status_bytes[:1], "little"),
+ )
+
+ if not query:
+ return ""
+
+ # 0x1023:3 = OSCommand.Reply (DOMAIN / string). Library handles segmented.
+ reply: bytes = await asyncio.to_thread(node.sdo.upload, 0x1023, 3)
+ return reply.decode("ascii", errors="replace").rstrip("\x00").rstrip()
+
+ # --- raw CANopen sends (SYNC + RPDO1 controlword) -----------------------
+
+ async def _can_sync(self) -> None:
+ if self._network is None:
+ raise CanError("_can_sync called before setup()")
+ # SYNC object (0x080), no data.
+ self._network.send_message(0x80, b"")
+
+ async def _control_word_set(self, node_id: int, value: int, sync: bool = True) -> None:
+ if self._network is None:
+ raise CanError("_control_word_set called before setup()")
+ val_bytes = value.to_bytes(2, byteorder="little")
+ # RPDO1 COB-ID = (4 << 7) | node_id = 0x200 + node_id
+ self._network.send_message(0x200 + node_id, val_bytes)
+ if sync:
+ await self._can_sync()
+
+ async def request_drive_version(self, node_id: int) -> str:
+ """Query Elmo drive firmware version (VR) via the OS interpreter."""
+ return await self._os_interpreter(node_id, "VR", query=True)
+
+ # --- DS402 / motor control ----------------------------------------------
+
+ async def motor_emergency_stop(self, node_id: int) -> None:
+ await self.write(node_id, "MO", 0, 0)
+
+ async def motor_is_enabled(self, node_id: int) -> bool:
+ """Return True if the motor is energized (Elmo MO=1).
+
+ Faulted drives report MO=0 — use motor_get_fault to distinguish a
+ plain disable from a fault.
+ """
+ return await self.query_int(node_id, "MO", 0) == 1
+
+ async def motor_get_current_position(self, node_id: int, pu: bool = False) -> int:
+ cmd = "PU" if pu else "PX"
+ return await self.query_int(node_id, cmd, 0)
+
+ async def motor_get_motion_status(self, node_id: int) -> int:
+ return await self.query_int(node_id, "MS", 0)
+
+ async def motor_set_move_direction(
+ self, node_id: int, direction: JointMoveDirection
+ ) -> None:
+ # Elmo modulo mode register: bit0 enables modulo; bits6..7 encode the
+ # direction (0=Normal, 1=CW, 2=CCW, 3=Shortest). Packs to 1 + 64*direction
+ # = 1/65/129/193.
+ val = 1 + 64 * int(direction)
+ await self.can_sdo_download_elmo_object(node_id, 24818, 0, val, _ElmoObjectDataType.UNSIGNED16)
+
+ async def motor_check_if_move_done(self, node_id: int) -> bool:
+ # E-stop and some fault paths leave MS pinned at 2 ("stopping in
+ # progress") indefinitely, so gating fault-surfacing on ms==1 misses
+ # them — the poll loop times out before ever consulting EMCY state.
+ # Check sticky EMCY first so any fatal frame raises on the next poll
+ # iteration regardless of MS.
+ pending = self.emcy_move_error
+ if pending is not None:
+ raise RuntimeError(f"Motor Fault: {pending}")
+ ms_val = await self.query_int(node_id, "MS", 0)
+ if ms_val == 0:
+ return True
+ if ms_val == 1:
+ mo_val = await self.query_int(node_id, "MO", 0)
+ if mo_val == 1:
+ return True
+ fault = await self.motor_get_fault(node_id)
+ if fault is not None:
+ raise RuntimeError(f"Motor Fault: Axis {node_id} {fault}")
+ raise RuntimeError(f"Motor Fault: Axis {node_id} (Unknown)")
+ return False
+
+ async def motor_get_fault(self, node_id: int) -> Optional[str]:
+ val = await self.query_int(node_id, "MF", 0)
+ if val == 0:
+ return None
+ # Elmo MF register: most faults are independent single bits. Bits 13/14/15
+ # are different — they form a 3-bit selector (b15<<2 | b14<<1 | b13) where
+ # only four combinations are real faults; the rest mean nothing.
+ bit_msgs = {
+ 0x0001: "Motor Hall sensor feedback angle not found yet.",
+ 0x0004: "Feedback loss: no match between encoder and Hall location.",
+ 0x0008: "The peak current has been exceeded.",
+ 0x0010: "Inhibit.",
+ 0x0040: "Two digital Hall sensors were changed at the same time.",
+ 0x0080: "Speed tracking error.",
+ 0x0100: "Position tracking error.",
+ 0x0200: "Inconsistent drive database.",
+ 0x0400: "Too large a difference in ECAM table.",
+ 0x0800: "CAN heartbeat failure.",
+ 0x1000: "Servo drive fault.",
+ 0x010000: "Failed to find the electrical zero of the motor during startup.",
+ 0x020000: "Speed limit exceeded.",
+ 0x040000: "Drive CPU stack overflow.",
+ 0x080000: "Drive CPU exception.",
+ 0x200000: "Motor stuck.",
+ 0x400000: "Position limit exceeded.",
+ 0x20000000: "Cannot start motor.",
+ }
+ triplet_msgs = {
+ 0b001: "Power supply under voltage.", # b13 only
+ 0b010: "Power supply over voltage.", # b14 only
+ 0b101: "Motor lead short circuit or faulty drive.", # b13 + b15
+ 0b110: "Drive overheated.", # b14 + b15
+ }
+ faults = [msg for bit, msg in bit_msgs.items() if val & bit]
+ triplet = (val >> 13) & 0b111
+ if triplet in triplet_msgs:
+ faults.append(triplet_msgs[triplet])
+ if not faults:
+ return f"Unknown fault code: {val} (0x{val:08X})"
+ return " ".join(faults)
+
+ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> None:
+ """Enable or disable a single drive.
+
+ - ``use_ds402=True``: DS402 controlword sequence over RPDO1 (Fault ->
+ Shutdown -> Switched On -> Op Enabled on enable; reverse on disable).
+ Used for the four motion axes (shoulder/Z/elbow/wrist).
+ - ``use_ds402=False``: vendor binary-interpreter ``MO=1/0``. Used for the
+ rail and the servo gripper.
+
+ Caller picks the path; the driver does not know about robot topology.
+
+ Drives sometimes need several seconds after a fault / power-rail bounce
+ before they accept enable, and disable can lag past a single 100 ms
+ settle for the same reason — the retry budget covers both directions
+ so a slow drive doesn't leave the arm half-enabled mid-freedrive.
+ """
+ if state:
+ # Clear sticky EMCY state from any prior fault on this drive so the
+ # post-enable motion path doesn't re-surface stale errors. Mirrors
+ # clscanmotor.cs:4481 ("EmcyMoveErrorReceived = false" before re-enable)
+ # plus the per-axis IPM-queue clear at clscanmotor.cs:4050-4051.
+ self.clear_emcy_state(node_id=node_id)
+
+ want = 1 if state else 0
+ max_attempts = 20
+ inter_attempt_sleep_s = 0.5
+ for attempt in range(1, max_attempts + 1):
+ if not use_ds402:
+ await self.write(node_id, "MO", 0, want)
+ elif state:
+ # DS402 enable: edge-pulsed CW writes with SW confirmation between
+ # transitions. CiA 402 §6.1: Fault -> Switch on disabled fires on
+ # the rising edge of CW bit 7 (Fault Reset). RPDO1 is mapped
+ # SynchronousCyclic, so back-to-back writes within one servo cycle
+ # can be coalesced and the drive never sees the edge — fault never
+ # clears and the retry loop spins. Polling SW between transitions
+ # forces each CW write to land on the wire before the next one.
+ await self._control_word_set(node_id=node_id, value=0x00) # clear bits
+ await self._control_word_set(node_id=node_id, value=0x80) # fault reset
+ await self._wait_sw_bit(node_id, bit_mask=1 << 3, want_high=False)
+ await self._control_word_set(node_id=node_id, value=0x06) # Shutdown
+ await self._wait_sw_bit(node_id, bit_mask=1 << 0, want_high=True)
+ await self._control_word_set(node_id=node_id, value=0x07) # Switch on
+ await self._wait_sw_bit(node_id, bit_mask=1 << 1, want_high=True)
+ await self._control_word_set(node_id=node_id, value=0x0F) # Enable op
+ # Bit 2 (Operation enabled) is confirmed by the MO query below.
+ else:
+ # DS402 disable: Op Enabled -> Switched On -> Ready to Switch On.
+ # Matches C# (clscanmotor.cs:4540-4543) — back-to-back, no inter-CW sleep.
+ await self._control_word_set(node_id=node_id, value=7)
+ await self._control_word_set(node_id=node_id, value=6)
+ await asyncio.sleep(0.1)
+ mo = await self.query_int(node_id, "MO", 0)
+ if mo == want:
+ return
+ logger.warning(
+ "motor_enable(state=%s) attempt %d/%d failed for node %d (MO=%s); retrying",
+ state, attempt, max_attempts, node_id, mo,
+ )
+ await asyncio.sleep(inter_attempt_sleep_s)
+ verb = "enable" if state else "disable"
+ raise CanError(f"Motor failed to {verb} (node_id = {node_id}) after {max_attempts} attempts")
+
+ async def motors_ensure_enabled(
+ self, node_ids: List[int], *, use_ds402: bool = True,
+ ) -> None:
+ """Enable every drive in ``node_ids`` that isn't already enabled.
+
+ One ``motor_is_enabled`` SDO read per axis (~5 ms); only drives reading
+ MO=0 pay the full DS402 enable cycle. Per-axis work runs concurrently —
+ each node ID has its own SDO channel so they don't serialize on the bus.
+
+ The cheap path (drive already enabled) is the common case after the
+ first move; the slow path covers post-halt, post-fault, post-freedrive
+ where the drive deliberately landed in Switch On Disabled. Used by
+ every motion-trigger site (PPM, IPM begin) and lifecycle transitions
+ (setup, stop_freedrive_mode, find_z preflight) so they all share one
+ recovery contract.
+ """
+ # Sequential, not gather: motor_enable's DS402 ladder writes intermediate
+ # CW values (0x06/0x07/0x0F) interleaved with SW reads. If one axis
+ # raises mid-cycle, gather cancels the others mid-sequence — leaving
+ # them in an indeterminate state (e.g. 0x07 written but 0x0F never sent)
+ # that the per-call retry budget can't reliably recover from. Sequential
+ # finishes one axis fully before touching the next.
+ for nid in node_ids:
+ nid_int = int(nid)
+ if await self.motor_is_enabled(nid_int):
+ continue
+ logger.warning("node %d: re-enabling (was disabled)", nid_int)
+ await self.motor_enable(node_id=nid_int, state=True, use_ds402=use_ds402)
+
+ # --- motion primitives --------------------------------------------------
+
+ async def _set_op_mode(self, node_id: int, mode: int, timeout_s: float = 0.05) -> None:
+ """Write 0x6060 (modes_of_operation) and poll 0x6061 (modes_of_operation_display)
+ until the drive acknowledges. CiA 402 §6.2: 0x6060 is the request, 0x6061 is
+ the actual mode — issuing a move (0x607A or 0x60C1 write) before the drive
+ flips reads the actual mode races the mode change. Drives typically ack in
+ <5 ms; timeout is generous so a busy bus doesn't false-fail.
+
+ See https://www.stober.jp/manual/manual-commissioning-instruction-cia402-443080-01-en.pdf
+ for a CiA 402 commissioning reference (object table, mode codes, state machine).
+ """
+ await self._can_sdo_download(node_id, 0x6060, 0x00, [mode])
+ deadline = asyncio.get_event_loop().time() + timeout_s
+ while True:
+ raw = await self._can_sdo_upload(node_id, 0x6061, 0x00)
+ actual = struct.unpack("= deadline:
+ raise CanError(
+ f"node {node_id}: 0x6061 modes_of_operation_display = {actual}, "
+ f"expected {mode} after {timeout_s * 1000:.0f}ms — drive didn't ack mode change"
+ )
+ await asyncio.sleep(0.005)
+
+ async def ipm_select_mode(self, enable: bool) -> None:
+ """Enable/disable IPM (Interpolated Position Mode, 0x6060=7) on all motion
+ axes via standard SDO writes.
+
+ The ``_ipm_mode`` bookkeeping flag is set *pessimistically*: True before
+ the SDO writes on enable, False after them on disable. So a partial
+ failure mid-sequence still leaves the next caller with an accurate
+ "we tried to be in IPM" signal — they can re-arm rather than assume
+ we cleanly stayed in PPM.
+ """
+ if enable:
+ self._ipm_mode = True
+ # First, latch CW=0x0F on every axis (op-enabled, NO ip-enable/new-
+ # setpoint trigger). PPM leaves CW bit 4 high; in mode 7 bit 4 means
+ # "interpolation enabled" so an unreset CW makes the drive try to
+ # interpolate an empty buffer the moment we flip to mode 7, and the next
+ # RPDO3 preload hits EMCY 0x34 / 0xBA (queue_full on first write).
+ # RPDO1 is SynchronousCyclic so the writes need a SYNC to take effect.
+ for nid in self.motion_node_ids:
+ await self._control_word_set(nid, 0x0F, sync=False)
+ await self._can_sync()
+ # Now do the mode-bounce + buffer clear per axis.
+ for nid in self.motion_node_ids:
+ await self._set_op_mode(nid, 1)
+ await self.ipm_clear_queue(nid)
+ await self._set_op_mode(nid, 7)
+ # Let drives finish ingesting mode 7 + buffer reset before any RPDO3
+ # write — without this, back-to-back IPM moves intermittently see the
+ # first preload write hit EMCY 0x34 / 0xBA (queue_full) against a
+ # buffer the drive still treats as "previous state".
+ await asyncio.sleep(0.05)
+ else:
+ # Always attempt to revert — the cheap mode-display poll inside
+ # _set_op_mode is the authoritative confirmation. Skip the writes only
+ # if we know we never armed (cleaner test semantics; idempotent on
+ # the wire either way).
+ if self._ipm_mode:
+ try:
+ for nid in self.motion_node_ids:
+ await self._set_op_mode(nid, 1) # profile position mode
+ finally:
+ # Clear bookkeeping even on partial failure — the alternative is
+ # leaving _ipm_mode=True after a teardown attempt, which would
+ # cause the next select_mode(True) to take the re-arm branch
+ # against drives possibly already in PPM.
+ self._ipm_mode = False
+
+ async def ipm_clear_queue(self, node_id: int) -> None:
+ """Reset the drive's interpolation buffer head/tail (0x60C4 sub 6 = 0).
+ Used by `ipm_select_mode` re-arm and post-cancel cleanup so a stale tail
+ pointer doesn't replay old points on the next IPM enable."""
+ await self._can_sdo_download(int(node_id), 0x60C4, 0x06, [0])
+
+ async def ipm_set_time_interval(self, ms: int) -> None:
+ """Program 0x60C2:01 = ms on every motion axis. The 0x60C2:02 = -3 written
+ at setup means the unit is milliseconds; this just sets the count.
+
+ Drive-level: integer ms only (UNSIGNED8). Caller picks dt; runtime passes
+ the same value used to build the trajectory so position/velocity scaling
+ matches what the drive integrates between points."""
+ if not 0 <= int(ms) <= 255:
+ raise ValueError(f"ipm_set_time_interval: ms must fit in UINT8, got {ms}")
+ for nid in self.motion_node_ids:
+ await self.can_sdo_download_elmo_object(
+ nid, 24770, 1, int(ms), _ElmoObjectDataType.UNSIGNED8,
+ )
+
+ def ipm_send_pvt_point(
+ self, node_id: int, position_enc: int, velocity_enc_per_s: int,
+ ) -> None:
+ """Append one PVT (P, V) data point to the drive's interpolation buffer
+ via RPDO3.
+
+ Wire layout (8 bytes, little-endian, RPDO3 COB-ID = 0x400 + node_id):
+ [0..3] TargetPositionIP (INT32 encoder counts)
+ [4..7] TargetVelocityIP (INT32 encoder counts/sec)
+
+ Synchronous — `network.send_message` queues to the kernel CAN buffer in
+ microseconds with no I/O wait, so there's nothing to await. Marking it
+ `async` would mislead callers about the cost (no event-loop yield) and
+ add a coroutine-frame allocation per send.
+
+ RPDO3 is mapped EventDriven (no SYNC needed) at setup. Caller paces the
+ feed; sending faster than the buffer drains raises EMCY 0x34 from the
+ drive (queue-full)."""
+ if self._network is None:
+ raise CanError("ipm_send_pvt_point called before setup()")
+ payload = (
+ int(position_enc).to_bytes(4, "little", signed=True)
+ + int(velocity_enc_per_s).to_bytes(4, "little", signed=True)
+ )
+ cob_id = (int(COBType.RPDO3) << 7) | (int(node_id) & 0x7F)
+ self._network.send_message(cob_id, payload)
+
+ async def ipm_begin_motion(self, node_ids: List[int]) -> None:
+ """Start IPM streaming on the listed axes. CW=0x1F (op-enabled +
+ ip-enable) per axis via RPDO1; one SYNC at the end so the
+ SynchronousCyclic-mapped RPDO1s latch together.
+
+ Auto-recovers from a prior disable (post-halt, post-fault) via
+ ``motors_ensure_enabled``. Single-shot 0x1F to a drive in Switch On
+ Disabled is silently dropped — the state machine needs the 6→7→15
+ transitions visible to its poll loop.
+
+ Resets the per-axis IPM queue counters BEFORE issuing the CW edge so a
+ queue_full/underflow fired between preload and begin (e.g. malformed
+ first point) is preserved for the runtime to inspect and surface."""
+ ids = [int(n) for n in node_ids]
+ if not ids:
+ return
+ await self.motors_ensure_enabled(ids)
+ for nid in ids:
+ if nid in self._emcy:
+ self._emcy[nid] = _NodeEmcyState()
+ for nid in ids[:-1]:
+ await self._control_word_set(nid, 0x1F, sync=False)
+ await self._control_word_set(ids[-1], 0x1F, sync=True)
+
+ async def ipm_stop(self, node_ids: Optional[List[int]] = None) -> None:
+ """Drop ip-enable on the listed axes (CW=0x0F via RPDO1, SYNC on last).
+ The drive consumes any already-buffered points before halting — coast
+ can run up to (queued_points * dt_ms) ms past the request. Defaults to
+ every motion axis when ``node_ids`` is None."""
+ ids = [int(n) for n in node_ids] if node_ids is not None else list(self.motion_node_ids)
+ if not ids:
+ return
+ for nid in ids[:-1]:
+ await self._control_word_set(nid, 0x0F, sync=False)
+ await self._control_word_set(ids[-1], 0x0F, sync=True)
+
+ def ipm_check_queue_fault(self, node_ids: List[int]) -> None:
+ """Raise CanError if any axis has a fatal IPM queue condition.
+
+ Inspects the EMCY-driven ``_emcy`` state for ``queue_full`` (drive
+ rejected our point) or ``underflow`` (drive ran the buffer dry).
+ Either condition means the trajectory is no longer trustworthy — the
+ streaming runtime calls this after each send-batch to surface the fault
+ promptly instead of letting the move silently degrade.
+ """
+ bad: List[str] = []
+ for nid in node_ids:
+ st = self._emcy.get(int(nid))
+ if st is None:
+ continue
+ if st.queue_full:
+ bad.append(f"Axis {nid} queue_full (failed write @ {st.queue_full_failed_write_pointer})")
+ if st.underflow:
+ bad.append(f"Axis {nid} underflow")
+ if bad:
+ raise CanError("IPM queue fault: " + "; ".join(bad))
+
+ async def ipm_wait_motion_complete(
+ self, node_ids: List[int], timeout_s: float,
+ ) -> None:
+ """Wait until SW bit-10 (motion_complete / target_reached) goes high on
+ every listed axis. The bit goes high once the IP buffer drains and the
+ drive's trajectory generator settles on the last commanded position.
+
+ Polls via the TPDO3-backed StatusWord cache; falls back to SDO probe
+ if the cache hasn't seen a frame within 5 ms. Raises CanError on
+ timeout, naming the offending axis."""
+ async def _wait_one(nid: int) -> None:
+ ok = await self._wait_sw_bit(
+ int(nid), bit_mask=1 << 10, want_high=True, timeout_s=timeout_s,
+ )
+ if not ok:
+ raise CanError(
+ f"Axis {nid}: SW bit-10 (target reached) never went high within "
+ f"{timeout_s:.1f}s — IPM trajectory did not complete"
+ )
+ await asyncio.gather(*(_wait_one(int(n)) for n in node_ids))
+
+ async def wait_for_moves_done(
+ self, node_ids: List[int], timeout: float
+ ) -> None:
+ # Poll MS every 30ms after a 50ms warm-up. The warm-up avoids reading
+ # MS=0 in the window between CW=63 and motion actually starting.
+ assert self._loop is not None
+ loop = self._loop
+
+ async def _poll_axis(nid: int) -> None:
+ deadline = loop.time() + timeout
+ await asyncio.sleep(0.05)
+ while loop.time() < deadline:
+ try:
+ if await self.motor_check_if_move_done(int(nid)):
+ return
+ except CanError as e:
+ # Transient bus error — keep polling. Visible at DEBUG so a wedged
+ # bus shows up in logs instead of just burning the full timeout.
+ logger.debug("wait_for_moves_done node %d: transient CAN error: %s", nid, e)
+ await asyncio.sleep(0.03)
+ # Final authoritative check; propagates CanError / motor-fault.
+ if not await self.motor_check_if_move_done(int(nid)):
+ raise CanError(f"Node {nid} move did not complete within {timeout}s")
+
+ await asyncio.gather(*(_poll_axis(n) for n in node_ids))
+
+ async def ppm_begin_motion(
+ self, node_ids: List[int], *, relative: bool = False
+ ) -> None:
+ # CiA 402 Profile Position Mode trigger handshake. Per drive:
+ # 1. CW bit 4 = 0 (new_setpoint cleared) -- bit 5 stays high so the
+ # drive treats the trigger as "change set immediately".
+ # 2. wait SW bit 12 (setpoint_ack) low -- drive ack of step 1.
+ # 3. CW bit 4 = 1 -- rising edge latches 0x607A; motion starts.
+ # 4. wait SW bit 12 high -- drive ack of step 3. If it doesn't go
+ # high, the rising edge was missed (RPDO/SDO race or drive busy)
+ # and we retry the cycle.
+ # Without (4) the failure rate is ~5-10% on this Elmo firmware: bit 4
+ # falls and rises within milliseconds and the drive doesn't always see
+ # the edge. Polling bit 12 high is the only authoritative confirmation
+ # that the new setpoint was actually latched.
+ relative_bit = 0x40 if relative else 0
+ cw_low = 47 + relative_bit
+ cw_high = 47 + 0x10 + relative_bit
+ # Auto-recover from prior disable (post-E-stop, post-find_z IL halt,
+ # post-freedrive). A disabled drive never raises SW bit 12, so the PPM
+ # trigger spins all 10 attempts before failing.
+ await self.motors_ensure_enabled([int(n) for n in node_ids])
+ for nid in node_ids:
+ await self._trigger_new_setpoint(int(nid), cw_low, cw_high)
+
+ async def _trigger_new_setpoint(
+ self,
+ node_id: int,
+ cw_low: int,
+ cw_high: int,
+ *,
+ max_attempts: int = 10,
+ ) -> None:
+ """Run the CiA 402 PPM new-setpoint handshake on one drive.
+
+ Each attempt: drop CW bit 4, wait SW bit 12 low, set CW bit 4, wait
+ SW bit 12 high. Retries up to ``max_attempts`` if bit 12 doesn't go
+ high (= drive missed the rising edge). Raises on persistent failure
+ rather than letting motion silently drop."""
+ for attempt in range(1, max_attempts + 1):
+ await self._control_word_set(node_id, cw_low, sync=True)
+ cleared = await self._wait_setpoint_ack(node_id, want_high=False)
+ if not cleared:
+ logger.debug(
+ "node %d: setpoint_ack didn't clear (attempt %d/%d)",
+ node_id, attempt, max_attempts,
+ )
+ continue
+ await self._control_word_set(node_id, cw_high, sync=True)
+ raised = await self._wait_setpoint_ack(node_id, want_high=True)
+ if raised:
+ if attempt > 1:
+ logger.debug(
+ "node %d: new setpoint accepted on attempt %d", node_id, attempt
+ )
+ return
+ logger.debug(
+ "node %d: setpoint_ack didn't go high (attempt %d/%d); retrying",
+ node_id, attempt, max_attempts,
+ )
+ raise CanError(
+ f"Axis {node_id}: drive did not accept new PPM setpoint after "
+ f"{max_attempts} attempts (SW bit 12 never went high after CW bit 4 "
+ f"rising edge)"
+ )
+
+ async def _wait_sw_bit(
+ self,
+ node_id: int,
+ *,
+ bit_mask: int,
+ want_high: bool,
+ timeout_s: float = 0.05,
+ ) -> bool:
+ """Wait until ``self._statusword[node_id] & bit_mask`` matches ``want_high``.
+
+ TPDO3 maps StatusWord (0x6041) with the StatusWordEvent trigger; the
+ canopen listener thread parses each frame into self._statusword[node_id]
+ and signals self._statusword_event[node_id]. We wait on the event, with
+ a 5 ms grace before falling back to an SDO probe — covers the case
+ where the drive's event-trigger config didn't take and TPDO3 is silent.
+ Returns True on a match, False on timeout.
+ """
+ assert self._loop is not None
+ ev = self._statusword_event.get(node_id)
+ deadline = self._loop.time() + timeout_s
+ while self._loop.time() < deadline:
+ sw = self._statusword.get(node_id)
+ if sw is not None and bool(sw & bit_mask) == want_high:
+ return True
+ if ev is None:
+ # No subscription (drive outside motion_node_ids); SDO poll only.
+ raw = await self._can_sdo_upload(node_id, 0x6041, 0x00)
+ sw = int.from_bytes(raw[:2], "little")
+ self._statusword[node_id] = sw
+ if bool(sw & bit_mask) == want_high:
+ return True
+ await asyncio.sleep(0.001)
+ continue
+ ev.clear()
+ try:
+ remaining = max(0.0, deadline - self._loop.time())
+ await asyncio.wait_for(ev.wait(), timeout=min(remaining, 0.005))
+ except asyncio.TimeoutError:
+ # TPDO3 didn't fire within 5 ms — probe via SDO and update the
+ # cache so subsequent waits start from the latest known SW.
+ raw = await self._can_sdo_upload(node_id, 0x6041, 0x00)
+ sw = int.from_bytes(raw[:2], "little")
+ self._statusword[node_id] = sw
+ return False
+
+ async def _wait_setpoint_ack(
+ self, node_id: int, *, want_high: bool, timeout: float = 0.05
+ ) -> bool:
+ """Wait until 0x6041 bit 12 (setpoint_ack) matches ``want_high``.
+
+ Thin specialization of :meth:`_wait_sw_bit` for the PPM trigger
+ handshake. 50 ms total is plenty: bit 12 flips within a servo cycle
+ (~1-2 ms) once the drive sees the CW bit-4 edge.
+ """
+ return await self._wait_sw_bit(
+ node_id, bit_mask=1 << 12, want_high=want_high, timeout_s=timeout
+ )
+
+ async def user_program_run(
+ self,
+ node_id: int,
+ user_function: str,
+ params: Optional[List[Union[int, float]]] = None,
+ timeout_sec: int = 0,
+ wait_until_done: bool = False,
+ ) -> int:
+ if node_id < 0 or node_id > 255:
+ raise ValueError("node_id must be in [0, 255]")
+
+ ps = await self.query_int(node_id, "PS", 0)
+ if ps == -2:
+ raise CanError(f"Node {node_id}: controller reported PS=-2 (not ready / unavailable)")
+
+ if ps != -1:
+ await self.write(node_id, "UI", 1, 0)
+ t0 = time.monotonic()
+ while (time.monotonic() - t0) < 3.0:
+ ps = await self.query_int(node_id, "PS", 0)
+ if ps == -1:
+ break
+ await asyncio.sleep(0.01)
+ else:
+ raise CanError(f"Node {node_id}: did not reach idle state (PS=-1) within 3s (last PS={ps})")
+
+ arg_str = f"({','.join(str(p) for p in params)})" if params else ""
+
+ await self.write(node_id, "UI", 1, 1)
+
+ cmd = f"XQ##{user_function}{arg_str}"
+ logger.debug("user_program_run: %s", cmd)
+ await self._os_interpreter(node_id, cmd, query=False)
+
+ last_line_completed = 0
+ if wait_until_done:
+ t0 = time.monotonic()
+ ps = 1
+ ui1 = 1
+ while ps == 1 and ui1 == 1 and (time.monotonic() - t0) < timeout_sec:
+ ps = await self.query_int(node_id, "PS", 0)
+ ui1 = await self.query_int(node_id, "UI", 1)
+ await asyncio.sleep(0.01)
+
+ last_line_completed = await self.query_int(node_id, "UI", 2)
+
+ if ps == 1 and ui1 == 1:
+ raise CanError(
+ f"Node {node_id}: timeout waiting for '{user_function}' after {timeout_sec}s, "
+ f"last_line={last_line_completed}"
+ )
+ if ui1 != 0:
+ raise CanError(
+ f"Node {node_id}: user program ended with UI[1]={ui1} (expected 0), "
+ f"last_line={last_line_completed}"
+ )
+
+ return 0
+
+ # --- I/O -----------------------------------------------------------------
+
+ async def read_input(self, node_id: int, input_num: int) -> bool:
+ return await self.query_int(node_id, "IB", input_num) == 1
+
+ async def read_output(self, node_id: int, output_num: int) -> bool:
+ val = await self.query_int(node_id, "OP", 0)
+ mask = 1 << (output_num - 1)
+ return (val & mask) == mask
+
+ async def set_output(self, node_id: int, output_num: int, state: bool) -> None:
+ await self.write(node_id, "OB", output_num, 1 if state else 0)
+
+ async def motor_stop(self, node_id: int, settle: float = 0.1) -> None:
+ """Controlled halt of one axis (port of C# MotorStop, clscanmotor.cs:5517).
+
+ Sends CW=271 (Op Enabled + Halt — controlled deceleration, no power drop),
+ waits `settle` seconds for the drive to come to rest, then writes 0x6060 = 7
+ then = 1 to clear the post-halt status-word state. Used after an IL-induced
+ auto-halt so the next move doesn't see a hung MS register.
+
+ The C# version polls a TPDO-event flag with a 2.5s timeout. We can't reuse
+ `wait_for_moves_done` here because MS never goes to 0 after a halt — the
+ poll would just burn the full timeout. Drive deceleration is sub-100ms for
+ the search velocities used here, so a fixed sleep is fine.
+ """
+ await self._control_word_set(node_id, 271)
+ await asyncio.sleep(settle)
+ await self._can_sdo_download(node_id, 0x6060, 0x00, [7])
+ await self._can_sdo_download(node_id, 0x6060, 0x00, [1])
+
+ async def read_input_logic(self, node_id: int, input_num: int) -> int:
+ return await self.query_int(node_id, "IL", input_num)
+
+ async def configure_input_logic(
+ self, node_id: int, input_num: int, logic: int, logic_high: bool = False,
+ ) -> None:
+ """Set IL[input_num]: drive auto-acts on input edges (e.g. halt motion).
+
+ Pass an `_InputLogic` member or raw int for `logic`. With `StopForward` the
+ drive halts the motor itself the instant the input trips during forward
+ motion — no software in the loop. Skips the write if value already matches;
+ settles 250ms after a real change (Elmo IL needs time to apply).
+ """
+ value = int(logic) + (1 if logic_high else 0)
+ if await self.read_input_logic(node_id, input_num) == value:
+ return
+ await self.write(node_id, "IL", input_num, value)
+ await asyncio.sleep(0.25)
diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py
new file mode 100644
index 00000000000..6308d51dcc1
--- /dev/null
+++ b/pylabrobot/paa/kx2/kinematics.py
@@ -0,0 +1,916 @@
+"""
+KX2 kinematics: FK and IK.
+
+The KX2 is *not* a dual-link SCARA. The elbow is a prismatic radial slide
+(not a revolute J3), Z is a separate prismatic axis, and the rail is
+outside this kinematic model. So the math is closed-form and trivially
+cheap — no two-link cosine law, no elbow-up/elbow-down branch, no
+unreachable-pose check beyond "rotation must be about +Z". The wrist is
+a continuous-rotation drive with no winding, so J4 has no preferred sign
+either — `ik` returns one canonical value, `snap_to_current` then pulls
+it to whichever 360° wrap is closest to the current J4.
+
+Joint dict keys match the drive node-IDs and the `KX2ArmBackend.Axis` enum:
+ 1: shoulder [deg]
+ 2: Z [mm]
+ 3: elbow [mm] (radial extension)
+ 4: wrist [deg]
+
+Task pose is a `CartesianPose`. ``location`` is the gripper's *grip
+center* (the geometric midpoint between the two jaws, where a held
+plate sits) in world coordinates; rotation.z is yaw in degrees about
+world +Z, and rotation.x/y must be 0. Sign convention follows right-hand
+rule about +Z (CCW positive looking down).
+
+`rotation.z` is the world direction in which the *front* finger
+of the gripper points — where "front" is whichever finger
+:attr:`GripperParams.finger_side` names. The gripper assembly hangs
+``t.length`` away from the wrist axis along its extension direction;
+the *grip center* is at that offset, and the two fingers cluster
+around it. Flipping `finger_side` is just a 180° relabel of which
+finger is the "front", so for the same joint state the grip center
+stays put and only the reported yaw flips by 180°. For the same
+(grip center, yaw) target, the wrist *axis* lands on opposite sides
+of the grip center for the two side choices (separated by
+``2·t.length`` along the front-finger axis), because the gripper
+assembly has to swing around the wrist motor to point the chosen
+finger forward.
+"""
+
+from dataclasses import dataclass, field
+from math import asin, atan2, ceil, cos, degrees, hypot, pi, radians, sin, sqrt, trunc
+from typing import Callable, Dict, List, Optional, Tuple
+
+from pylabrobot.capabilities.arms import kinematics as arm_kinematics
+from pylabrobot.capabilities.arms.standard import CartesianPose
+from pylabrobot.paa.kx2.config import Axis, GripperParams, KX2Config
+from pylabrobot.paa.kx2.driver import (
+ JointMoveDirection,
+ MotorMoveParam,
+ MotorsMovePlan,
+)
+from pylabrobot.resources import Coordinate, Rotation
+
+
+class IKError(ValueError):
+ """Target pose is unreachable (for now: non-Z rotation requested)."""
+
+
+# Floating-point fudge for boundary checks (e.g. snap shoulder to ±180°).
+_EPS = 1e-6
+
+
+def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperParams) -> CartesianPose:
+ """Forward kinematics.
+
+ Args:
+ joints: {Axis.SHOULDER: deg, Axis.Z: mm, Axis.ELBOW: mm, Axis.WRIST: deg}.
+ c: arm configuration (drive-read calibration).
+ t: gripper tooling (user-supplied geometry).
+ Returns:
+ CartesianPose where ``location`` is the gripper's *grip center*
+ (geometric midpoint of the jaws) and ``rotation.z`` is the world
+ yaw of the *front* finger (the one named by ``t.finger_side``).
+ Yaw is wrapped to ``[-180, 180]``. The grip center depends only
+ on the joint state — flipping ``t.finger_side`` for fixed joints
+ leaves it unchanged and shifts only the reported yaw by 180°.
+ """
+ r = c.wrist_offset + c.elbow_offset + c.elbow_zero_offset + joints[Axis.ELBOW]
+ sh_deg = joints[Axis.SHOULDER]
+ sh = radians(sh_deg)
+
+ wrist_x = -r * sin(sh)
+ wrist_y = r * cos(sh)
+ wrist_z = joints[Axis.Z]
+
+ # Gripper assembly hangs t.length off the wrist axis along the
+ # extension direction (= world angle of the wrist motor = WRIST+SHOULDER).
+ # finger_side just relabels which physical finger is "front", so it
+ # only shifts the reported yaw by 180°; the grip center is fixed.
+ ext_deg = joints[Axis.WRIST] + sh_deg
+ ext = radians(ext_deg)
+
+ yaw_deg = ext_deg
+ if t.finger_side == "proximity_sensor":
+ yaw_deg += 180.0
+ while yaw_deg > 180.0:
+ yaw_deg -= 360.0
+ while yaw_deg < -180.0:
+ yaw_deg += 360.0
+
+ return CartesianPose(
+ location=Coordinate(
+ x=wrist_x + t.length * sin(ext),
+ y=wrist_y - t.length * cos(ext),
+ z=wrist_z - t.z_offset,
+ ),
+ rotation=Rotation(z=yaw_deg),
+ )
+
+
+def ik(pose: CartesianPose, c: KX2Config, t: GripperParams) -> Dict[Axis, float]:
+ """Inverse kinematics.
+
+ Args:
+ pose: target gripper pose. ``location`` is the *grip center*
+ (geometric midpoint of the jaws). ``rotation.z`` is the world
+ direction the *front* finger should face (per ``t.finger_side``).
+ ``rotation.x/y`` must be 0.
+ c: arm configuration (drive-read calibration).
+ t: gripper tooling (user-supplied geometry).
+ Returns:
+ joints dict {Axis.SHOULDER: deg, Axis.Z: mm, Axis.ELBOW: mm, Axis.WRIST: deg}.
+ Shoulder and elbow differ between the two finger-side choices for
+ the same target — the gripper assembly swings around the wrist
+ motor, so the wrist axis lands on opposite sides of the grip
+ center (separated by ``2·t.length`` along the front-finger axis).
+ J4 is the canonical (-180°, 180°] solution; `snap_to_current`
+ then pulls it to whichever 360° wrap is closest to the current J4.
+ Raises:
+ IKError if the requested rotation has an x or y component.
+ """
+ if pose.rotation.x != 0 or pose.rotation.y != 0:
+ raise IKError("Only Z rotation is supported for KX2")
+
+ # The incoming pose describes the grip center; the joint-space math
+ # operates on the wrist axis. The gripper assembly hangs ``t.length``
+ # off the wrist axis along its *extension direction*, so the grip
+ # center sits at wrist + t.length·. For barcode_reader
+ # the extension direction == the front-finger direction; for
+ # proximity_sensor the front finger has been swung 180° to the other
+ # side, so the extension (and therefore grip-center offset) points
+ # 180° opposite the front finger.
+ ext_deg = pose.rotation.z
+ if t.finger_side == "proximity_sensor":
+ ext_deg -= 180.0
+ ext = radians(ext_deg)
+ x = pose.location.x - t.length * sin(ext)
+ y = pose.location.y + t.length * cos(ext)
+ wrist_z = pose.location.z + t.z_offset
+
+ # atan2 returns (-π, π]; on the -Y axis it yields -180°. Snap to +180°
+ # so the boundary lands on the "in-range" side of an exclusive max
+ # convention and downstream sign comparisons are stable.
+ shoulder = -degrees(atan2(x, y))
+ if abs(shoulder + 180.0) < _EPS:
+ shoulder = 180.0
+
+ elbow = hypot(x, y) - c.wrist_offset - c.elbow_offset - c.elbow_zero_offset
+ # Wrist motor's world angle == extension direction. Joint space:
+ # wrist_joint = ext_world - shoulder.
+ wrist = ext_deg - shoulder
+
+ joints = {Axis.SHOULDER: shoulder, Axis.Z: wrist_z, Axis.ELBOW: elbow, Axis.WRIST: wrist}
+
+ # Enforce per-axis joint travel limits. Rotary axes (shoulder, wrist) are
+ # 360°-periodic and a downstream `snap_to_current` may wrap them — check
+ # only the *non-wrappable* representative (the canonical solution in this
+ # call). Linear axes (Z, elbow) get a strict check. Raising here aborts
+ # the caller before any motion command leaves the host.
+ for ax in (Axis.Z, Axis.ELBOW):
+ cfg_ax = c.axes[ax]
+ val = joints[ax]
+ if val < cfg_ax.min_travel or val > cfg_ax.max_travel:
+ raise IKError(
+ f"{ax.name} out of range: {val:.3f} not in "
+ f"[{cfg_ax.min_travel:.3f}, {cfg_ax.max_travel:.3f}] "
+ f"for pose location={pose.location}, yaw={pose.rotation.z}"
+ )
+ for ax in (Axis.SHOULDER, Axis.WRIST):
+ cfg_ax = c.axes[ax]
+ if cfg_ax.unlimited_travel:
+ continue
+ # Pull the canonical solution into the range straddling the limits before
+ # checking, so a -180° solution against a [-90, 270] axis doesn't false-fail.
+ val = joints[ax]
+ mid = 0.5 * (cfg_ax.min_travel + cfg_ax.max_travel)
+ val += 360.0 * round((mid - val) / 360.0)
+ if val < cfg_ax.min_travel or val > cfg_ax.max_travel:
+ raise IKError(
+ f"{ax.name} out of range: {val:.3f}° not in "
+ f"[{cfg_ax.min_travel:.3f}, {cfg_ax.max_travel:.3f}]° "
+ f"for pose location={pose.location}, yaw={pose.rotation.z}"
+ )
+
+ return joints
+
+
+def snap_to_current(
+ joints: Dict[Axis, float], current: Dict[Axis, float]
+) -> Dict[Axis, float]:
+ """Shift rotary joints by 360° multiples toward `current`. Z and elbow
+ are prismatic and pass through unchanged."""
+ out = dict(joints)
+ for axis in (Axis.SHOULDER, Axis.WRIST):
+ out[axis] += 360 * round((current[axis] - out[axis]) / 360)
+ return out
+
+
+# --- elbow encoder/joint frame conversion ----------------------------------
+#
+# The elbow is exposed to FK/IK as a linear radial extension (mm), but the
+# physical motor is a rotary actuator driving a sine linkage — so the encoder
+# count is sin-related to the mm-space joint value. These two functions sit
+# at the encoder/joint boundary; FK/IK above and the planner below both
+# operate in the linear (mm) domain. Mirrors C# `ConvertElbowPositionToAngle`
+# / `ConvertElbowAngleToPosition` (KX2RobotControl.cs:2944, 2974).
+
+def convert_elbow_position_to_angle(cfg: KX2Config, elbow_pos: float) -> float:
+ max_travel = cfg.axes[Axis.ELBOW].max_travel
+ denom = max_travel + cfg.elbow_zero_offset
+ # Clamp to [-1, 1]: floating-point overshoot at the joint limit (e.g.
+ # encoder reads max_travel + 1e-12) would otherwise raise ValueError
+ # from asin even though the position is physically reachable.
+ if elbow_pos > max_travel:
+ x = max(-1.0, min(1.0, (2.0 * max_travel - elbow_pos + cfg.elbow_zero_offset) / denom))
+ return 90.0 + asin(x) * (180.0 / pi)
+ x = max(-1.0, min(1.0, (elbow_pos + cfg.elbow_zero_offset) / denom))
+ return asin(x) * (180.0 / pi)
+
+
+def convert_elbow_angle_to_position(cfg: KX2Config, elbow_angle_deg: float) -> float:
+ max_travel = cfg.axes[Axis.ELBOW].max_travel
+ denom = max_travel + cfg.elbow_zero_offset
+ if elbow_angle_deg > 90.0:
+ # Inverse of `90 + asin((2·max − pos + zero)/(max + zero))`. The `+ zero`
+ # term has to appear on both sides of the reflection or the round-trip
+ # drifts by ~zero·(max+zero)/max — silent encoder-read miscalibration
+ # when the joint is past peak extension.
+ return 2.0 * max_travel + cfg.elbow_zero_offset - denom * sin((elbow_angle_deg - 90.0) * (pi / 180.0))
+ return denom * sin(elbow_angle_deg * (pi / 180.0)) - cfg.elbow_zero_offset
+
+
+# --- trajectory planning ---------------------------------------------------
+
+def _wrap_to_range(x: float, lo: float, hi: float) -> float:
+ span = hi - lo
+ if span == 0:
+ return lo
+ k = trunc(x / span)
+ x = x - k * span
+ if x < lo:
+ x += span
+ if x == hi:
+ x -= span
+ return x
+
+
+def _profile(dist: float, v: float, a: float) -> tuple:
+ """Return (v, a, t_acc, t_total) with triangular fallback. If the
+ distance is short, you can't reach v before you must decelerate."""
+ if dist <= 0:
+ return v, a, 0.0, 0.0
+ if a <= 0:
+ # degenerate; avoid crash
+ return max(v, 1e-9), 1e-9, 0.0, dist / max(v, 1e-9)
+ t_acc = v / a
+ d_acc = 0.5 * a * t_acc * t_acc
+ if 2.0 * d_acc > dist: # triangular
+ t_acc = sqrt(dist / a)
+ v = a * t_acc
+ return v, a, t_acc, 2.0 * t_acc
+ d_cruise = dist - 2.0 * d_acc
+ t_cruise = d_cruise / max(v, 1e-9)
+ return v, a, t_acc, t_cruise + 2.0 * t_acc
+
+
+def _directional_delta(target: float, current: float, ax_cfg) -> float:
+ """Signed joint delta honoring `unlimited_travel` direction modes.
+
+ For non-`unlimited_travel` axes the delta is the literal `target − current`.
+ For unlimited axes the delta is rewritten to walk the direction the config
+ asks for (CW=negative, CCW=positive, ShortestWay=≤180°)."""
+ d = target - current
+ if not ax_cfg.unlimited_travel:
+ return d
+ span = ax_cfg.max_travel - ax_cfg.min_travel
+ dir_ = ax_cfg.joint_move_direction
+ if dir_ == JointMoveDirection.Clockwise and d > 0.01:
+ d -= span
+ elif dir_ == JointMoveDirection.Counterclockwise and d < -0.01:
+ d += span
+ elif dir_ == JointMoveDirection.ShortestWay:
+ if d > 180.0:
+ d -= span
+ elif d < -180.0:
+ d += span
+ return d
+
+
+def _stretch_to_time(dist: float, v: float, a: float, T: float) -> tuple:
+ """Slow a single-axis trapezoidal/triangular profile so it lands at time T.
+
+ Given the pass-1 profile (v, a) for `dist`, scale (v, a) by the same factor
+ k so the new profile takes time T. Preserving the v/a ratio keeps the
+ acceleration-time fixed; only the cruise leg stretches. Returns (v, a) for
+ the stretched profile."""
+ if dist <= 0 or a <= 0 or v <= 0:
+ return v, a
+ denom = v * (T - v / a)
+ if abs(denom) < 1e-12:
+ return v, a
+ k = dist / denom
+ return v * k, a * k
+
+
+def plan_joint_move(
+ current: Dict[Axis, float],
+ target: Dict[Axis, float],
+ cfg: KX2Config,
+ gripper_params: GripperParams,
+ *,
+ max_gripper_speed: Optional[float] = None,
+ max_gripper_acceleration: Optional[float] = None,
+) -> Optional[MotorsMovePlan]:
+ """Pure planner: joint-space target -> per-axis encoder plan.
+
+ Caller owns the driver round-trip — pass ``current`` from
+ ``request_joint_position`` (linear-extension units for elbow). Returns
+ ``None`` if every axis would be a no-op (within 0.01 of current).
+
+ ``target`` may be a subset of axes (e.g. ``{Axis.Z: 100}``) — unspecified
+ axes don't move.
+
+ ``current`` must include all four arm axes (SHOULDER/Z/ELBOW/WRIST) when
+ a gripper-speed/accel cap is given: the cap helper evaluates the FK
+ Jacobian at the start pose, and the column for any moving axis depends
+ on the absolute position of every other arm axis (the radius from the
+ shoulder enters the shoulder Jacobian, etc.). The orchestrator always
+ passes a full ``current`` from ``request_joint_position``; tests calling
+ this function directly must do the same.
+
+ ``max_gripper_speed`` / ``max_gripper_acceleration`` cap joint velocities
+ so the worst-case Cartesian gripper speed/accel along the trajectory
+ stays at or under the cap.
+ """
+ if max_gripper_speed is not None and max_gripper_speed <= 0.0:
+ raise ValueError(f"max_gripper_speed must be positive, got {max_gripper_speed}")
+ if max_gripper_acceleration is not None and max_gripper_acceleration <= 0.0:
+ raise ValueError(f"max_gripper_acceleration must be positive, got {max_gripper_acceleration}")
+
+ target = dict(target)
+ axes = list(target.keys())
+
+ # Travel-limit bounds check. Mirrors C# MoveAbsoluteSingleAxisPrivate
+ # (KX2RobotControl.cs:4624-4649): snap if within 0.1 of the limit, raise
+ # otherwise. Without this, an out-of-range target (e.g. gripper width 600
+ # when max_travel ~30) parks the drive trying to reach an unreachable
+ # position — MS never returns to 0 and every subsequent command on that
+ # axis hangs until full re-setup. Run before the elbow position->angle
+ # conversion so max/min_travel are compared in the units the user passed.
+ for ax in axes:
+ ax_cfg = cfg.axes[ax]
+ if ax_cfg.unlimited_travel:
+ continue
+ t = target[ax]
+ if t > ax_cfg.max_travel:
+ if t - ax_cfg.max_travel < 0.1:
+ target[ax] = ax_cfg.max_travel
+ else:
+ raise ValueError(f"Axis {ax.name} target {t} exceeds max_travel {ax_cfg.max_travel}")
+ elif t < ax_cfg.min_travel:
+ if ax_cfg.min_travel - t < 0.1:
+ target[ax] = ax_cfg.min_travel
+ else:
+ raise ValueError(f"Axis {ax.name} target {t} below min_travel {ax_cfg.min_travel}")
+
+ # Snapshot in cmd_pos units (elbow as linear extension) for the gripper-
+ # speed cap helper, which iterates the path in `fk`'s natural units.
+ target_cmd_units = dict(target)
+ curr_cmd_units = dict(current)
+
+ # Convert elbow target+current from position->angle for planning math —
+ # the motor's vel/accel limits are encoder-rate, which is angle-rate, not
+ # mm-rate. Time math is done in angle units; we convert back at the end.
+ if Axis.ELBOW in axes:
+ target[Axis.ELBOW] = convert_elbow_position_to_angle(cfg, target[Axis.ELBOW])
+ curr = dict(current)
+ if Axis.ELBOW in curr:
+ curr[Axis.ELBOW] = convert_elbow_position_to_angle(cfg, curr[Axis.ELBOW])
+
+ # Clearance check (in angle space for elbow, since base_to_gripper_clearance_arm
+ # is defined in the same domain in C#).
+ if Axis.Z in axes:
+ if (
+ target[Axis.Z] < cfg.axes[Axis.Z].min_travel + cfg.base_to_gripper_clearance_z
+ and target[Axis.ELBOW] < cfg.base_to_gripper_clearance_arm
+ ):
+ raise ValueError("Base-to-gripper clearance violated")
+
+ # Unlimited-travel normalization for non-NORMAL direction modes.
+ for ax in axes:
+ ax_cfg = cfg.axes[ax]
+ if ax_cfg.unlimited_travel and ax_cfg.joint_move_direction != JointMoveDirection.Normal:
+ target[ax] = _wrap_to_range(target[ax], ax_cfg.min_travel, ax_cfg.max_travel)
+
+ # Direction-aware deltas in cmd_pos units (elbow as linear extension), for
+ # the gripper-speed cap helper that walks the path in `fk`'s natural units.
+ cap_deltas: Dict[Axis, float] = {
+ ax: _directional_delta(target_cmd_units[ax], curr_cmd_units.get(ax, 0.0), cfg.axes[ax])
+ for ax in axes
+ }
+
+ # Per-axis caps from the gripper-speed/accel limits. Servo gripper isn't in
+ # fk, so it always runs at firmware max regardless of cap.
+ arm_axes = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST)
+ cap_requested = max_gripper_speed is not None or max_gripper_acceleration is not None
+ cap_relevant = any(ax in arm_axes for ax in axes)
+ if cap_requested and cap_relevant:
+ missing = [ax for ax in arm_axes if ax not in curr_cmd_units]
+ if missing:
+ raise ValueError(
+ f"max_gripper_speed/acceleration requires `current` to include all "
+ f"four arm axes (SHOULDER/Z/ELBOW/WRIST); missing: "
+ f"{[ax.name for ax in missing]}"
+ )
+ fk_start = {ax: curr_cmd_units[ax] for ax in arm_axes if ax in curr_cmd_units}
+ fk_deltas = {ax: cap_deltas.get(ax, 0.0) for ax in arm_axes if ax in fk_start}
+ fk_loc = lambda j: fk(j, cfg, gripper_params).location
+ capped: Dict[str, Dict[Axis, float]] = {"v": {}, "a": {}}
+ for kind, cap, field in (
+ ("v", max_gripper_speed, "max_vel"),
+ ("a", max_gripper_acceleration, "max_accel"),
+ ):
+ if cap is None or not fk_start:
+ continue
+ result = arm_kinematics.joint_velocities_for_max_gripper_speed(
+ fk=fk_loc,
+ joints_start=fk_start,
+ joint_deltas=fk_deltas,
+ joint_max_velocities={ax: getattr(cfg.axes[ax], field) for ax in fk_start},
+ max_gripper_speed=cap,
+ num_samples=1000,
+ eps=1e-3,
+ )
+ capped[kind] = {ax: abs(x) for ax, x in result.items()}
+ capped_v, capped_a = capped["v"], capped["a"]
+
+ # Per-axis trajectory (planning units = angle for elbow). Three-pass sync:
+ # (1) profile each axis at its firmware max; (2) match accel-times to the
+ # slowest-accel axis (shrink `a` so all ramps end together); (3) scale (v,a)
+ # together so all axes finish together at the slowest total time.
+ dist: Dict[Axis, float] = {}
+ v: Dict[Axis, float] = {}
+ a: Dict[Axis, float] = {}
+ ta: Dict[Axis, float] = {}
+ tt: Dict[Axis, float] = {}
+ for ax in axes:
+ ax_cfg = cfg.axes[ax]
+ dist[ax] = abs(_directional_delta(target[ax], curr[ax], ax_cfg))
+ v[ax] = capped_v.get(ax, ax_cfg.max_vel)
+ a[ax] = capped_a.get(ax, ax_cfg.max_accel)
+ if dist[ax] >= 0.01 and a[ax] > 0:
+ v[ax], a[ax], ta[ax], tt[ax] = _profile(dist[ax], v[ax], a[ax])
+ else:
+ ta[ax] = tt[ax] = 0.0
+ moving = [ax for ax in axes if tt[ax] > 0.0]
+ if not moving:
+ return None
+
+ lead_acc_t = max(ta[ax] for ax in moving)
+ for ax in moving:
+ if ta[ax] < lead_acc_t:
+ a[ax] = v[ax] / lead_acc_t
+ v[ax], a[ax], _, tt[ax] = _profile(dist[ax], v[ax], a[ax])
+
+ lead_T = max(tt[ax] for ax in moving)
+ for ax in moving:
+ if tt[ax] < lead_T:
+ v[ax], a[ax] = _stretch_to_time(dist[ax], v[ax], a[ax], lead_T)
+ v[ax], a[ax], _, tt[ax] = _profile(dist[ax], v[ax], a[ax])
+
+ move_time = max(tt[ax] for ax in moving)
+
+ # Convert back to encoder units. Elbow target is still in angle space here
+ # — the encoder count for an elbow joint is angle * conv, not mm * conv.
+ moves = []
+ for ax in axes:
+ ax_cfg = cfg.axes[ax]
+ conv = ax_cfg.motor_conversion_factor
+ enc_pos = target[ax] * conv
+ # Skipped axes get firmware max — same formula as moving axes. They
+ # don't move (dist < 0.01), so vel/accel are nominal; the previous
+ # 1000.0 constant was 0.03–4% of firmware max across axes, leaving
+ # the drive's profile registers in a pathologically slow state if a
+ # follow-up step ever picked them up.
+ enc_vel = max(v[ax] * abs(conv), 1.0)
+ enc_accel = max(a[ax] * abs(conv), 1.0)
+ moves.append(MotorMoveParam(
+ node_id=int(ax),
+ position=int(round(enc_pos)),
+ velocity=int(round(enc_vel)),
+ acceleration=int(round(enc_accel)),
+ direction=ax_cfg.joint_move_direction,
+ ))
+ return MotorsMovePlan(moves=moves, move_time=move_time)
+
+
+# --- Cartesian-linear path sampling (for IPM/PVT streaming) ----------------
+#
+# `plan_joint_move` generates a *joint*-space trapezoid: every axis ramps in
+# parallel through its own (v, a) profile, which gives a curvy tool-tip path.
+# `sample_linear_path` instead generates a *Cartesian*-linear path: the
+# gripper travels the straight line from start to end, sampled at fixed dt,
+# and IK at each sample yields the joint-space trajectory. The KX2ArmBackend
+# streams the result into the drive's interpolation buffer (PVT mode) when
+# `CartesianMoveParams(path='linear')` is requested.
+
+# Arm axes that show up in FK/IK and therefore have an entry in every
+# `LinearPathSample.joints`. Servo gripper and rail are not Cartesian-driven.
+_LINEAR_PATH_AXES: Tuple[Axis, ...] = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST)
+
+
+@dataclass
+class LinearPathSample:
+ """One frame of a Cartesian-linear trajectory.
+
+ ``joints`` is in planning units (mm for Z, deg for SHOULDER/WRIST,
+ *linear extension mm* for ELBOW). ``encoder_position`` /
+ ``encoder_velocity`` are post-conversion: ELBOW is converted through
+ ``convert_elbow_position_to_angle`` first because the encoder counts are
+ driven by the rotary actuator, not the linear projection. The runtime
+ feeds these straight into ``ipm_send_pvt_point``.
+ """
+
+ time_s: float
+ joints: Dict[Axis, float] = field(default_factory=dict)
+ encoder_position: Dict[Axis, int] = field(default_factory=dict)
+ encoder_velocity: Dict[Axis, int] = field(default_factory=dict)
+
+
+def _arc_length_trapezoid_times(
+ length: float, vel: float, accel: float, dt_s: float,
+) -> List[float]:
+ """Return arc-length s(t_i) at evenly-spaced t_i = i*dt covering a trapezoidal
+ profile from 0 to ``length`` with peak speed ``vel`` and peak accel ``accel``.
+
+ Falls back to triangular when the path is too short to reach ``vel``.
+
+ The trajectory is **dt-aligned**: the last sample lands at exactly
+ ``ceil(t_total / dt) * dt`` with s = length, and one extra "hold" sample
+ at length is appended so the central-difference velocity at the final
+ point is exactly zero. This matters for IPM streaming — the drive
+ integrates a cubic Hermite spline through (P_i, V_i) → (P_{i+1}, V_{i+1})
+ over each dt; without the hold, the FD-derived V at the last sample
+ would be small-but-nonzero, and the cubic would overshoot the target
+ before snapping back.
+ """
+ if length <= 0 or dt_s <= 0:
+ return [0.0]
+ v, a, t_acc, t_total = _profile(length, vel, accel)
+ if t_total <= 0:
+ return [0.0]
+ d_acc = 0.5 * a * t_acc * t_acc
+ n_motion = max(2, ceil(t_total / dt_s) + 1)
+ out: List[float] = []
+ for i in range(n_motion):
+ t = i * dt_s
+ if t >= t_total:
+ out.append(length)
+ elif t < t_acc:
+ out.append(0.5 * a * t * t)
+ elif t < t_total - t_acc:
+ out.append(d_acc + v * (t - t_acc))
+ else:
+ td = t_total - t
+ out.append(length - 0.5 * a * td * td)
+ out.append(length) # trailing hold so FD-derived final V is exactly 0
+ return out
+
+
+def _yaw_lerp(yaw_a_deg: float, yaw_b_deg: float, alpha: float) -> float:
+ """Interpolate yaw the short way around the circle. Avoids a 359° unwind
+ when start=+179°, end=-179° (true delta = 2°, naive lerp delta = 358°)."""
+ delta = (yaw_b_deg - yaw_a_deg + 540.0) % 360.0 - 180.0
+ return yaw_a_deg + alpha * delta
+
+
+def sample_linear_path(
+ cfg: KX2Config,
+ gripper_params: GripperParams,
+ start_pose: CartesianPose,
+ end_pose: CartesianPose,
+ *,
+ vel_mm_per_s: float,
+ accel_mm_per_s2: float,
+ dt_s: float,
+ current_joints: Optional[Dict[Axis, float]] = None,
+) -> List[LinearPathSample]:
+ """Sample a straight tool-tip path from ``start_pose`` to ``end_pose`` at dt.
+
+ Args:
+ cfg: drive-read calibration.
+ gripper_params: tooling geometry.
+ start_pose, end_pose: Cartesian endpoints (gripper clamp point + yaw).
+ vel_mm_per_s: peak Cartesian speed along the path.
+ accel_mm_per_s2: peak Cartesian acceleration along the path.
+ dt_s: sample period (matches the dt fed to ``ipm_set_time_interval``).
+ current_joints: pre-move joint snapshot. If supplied, the first sample's
+ WRIST/SHOULDER are 360°-snapped toward ``current_joints`` so the
+ trajectory doesn't begin with a full unwind. ELBOW position passes
+ through unchanged (linear axis).
+
+ Returns:
+ Dense list of samples, one per dt step. The last sample lands exactly
+ on ``end_pose``. ``encoder_position`` / ``encoder_velocity`` are ready
+ to feed straight into ``ipm_send_pvt_point``. Velocity is the central
+ finite difference of encoder positions; the endpoints use one-sided
+ differences and are clamped to zero at the very last sample so the
+ drive ends stationary.
+ """
+ if vel_mm_per_s <= 0 or accel_mm_per_s2 <= 0 or dt_s <= 0:
+ raise ValueError(
+ f"sample_linear_path: vel/accel/dt must be positive (got "
+ f"{vel_mm_per_s}, {accel_mm_per_s2}, {dt_s})"
+ )
+
+ sx, sy, sz = start_pose.location.x, start_pose.location.y, start_pose.location.z
+ ex, ey, ez = end_pose.location.x, end_pose.location.y, end_pose.location.z
+ start_yaw, end_yaw = start_pose.rotation.z, end_pose.rotation.z
+ length = sqrt((ex - sx) ** 2 + (ey - sy) ** 2 + (ez - sz) ** 2)
+
+ # Pure rotation in place is unsupported here: the speed/accel caps are
+ # mm/s and mm/s², so reusing them as deg/s for a wrist spin would silently
+ # command rotational rates the caller didn't intend. Use a joint-space
+ # move for orientation-only changes.
+ rot_delta = abs(((end_yaw - start_yaw + 540.0) % 360.0) - 180.0)
+ if length <= _EPS and rot_delta > _EPS:
+ raise NotImplementedError(
+ "sample_linear_path: pure rotation (no translation) is not supported. "
+ f"Translation length={length:.4f} mm, rotation delta={rot_delta:.2f}°. "
+ "Use move_to_joint_position with the target wrist angle for "
+ "orientation-only changes."
+ )
+
+ # Cartesian arc-length trapezoid. Yaw rides the same s/length ratio so
+ # rotation lands together with translation.
+ s_seq = _arc_length_trapezoid_times(length, vel_mm_per_s, accel_mm_per_s2, dt_s)
+
+ prev_for_snap: Optional[Dict[Axis, float]] = (
+ dict(current_joints) if current_joints is not None else None
+ )
+ poses: List[Dict[Axis, float]] = []
+ for s in s_seq:
+ alpha = (s / length) if length > 0 else 1.0
+ pose_i = CartesianPose(
+ location=Coordinate(
+ x=sx + alpha * (ex - sx),
+ y=sy + alpha * (ey - sy),
+ z=sz + alpha * (ez - sz),
+ ),
+ rotation=Rotation(z=_yaw_lerp(start_yaw, end_yaw, alpha)),
+ )
+ joints = ik(pose_i, cfg, gripper_params)
+ if prev_for_snap is not None:
+ joints = snap_to_current(joints, prev_for_snap)
+ poses.append(joints)
+ prev_for_snap = joints
+
+ # Encoder-space sequence per axis. Elbow's encoder is angle-driven (sine
+ # linkage), so convert position to angle before scaling by motor factor.
+ enc_pos: Dict[Axis, List[int]] = {ax: [] for ax in _LINEAR_PATH_AXES}
+ for joints in poses:
+ for ax in _LINEAR_PATH_AXES:
+ conv = cfg.axes[ax].motor_conversion_factor
+ val = joints[ax]
+ if ax is Axis.ELBOW:
+ val = convert_elbow_position_to_angle(cfg, val)
+ enc_pos[ax].append(int(round(val * conv)))
+
+ # Central finite difference for velocity; one-sided at endpoints. The
+ # trajectory is dt-aligned with a trailing hold sample (s = length),
+ # so the one-sided FD at the last point is naturally zero — no need
+ # to force it. Forcing zero against a non-trivial v[n-2] would create
+ # the cubic-Hermite overshoot the trailing hold is meant to avoid.
+ n = len(poses)
+ # V[0] forced to 0 (matches C# MotorsMovePath line 4242). Some drives reject
+ # the first preload frame if it has non-zero velocity.
+ enc_vel: Dict[Axis, List[int]] = {ax: [0] * n for ax in _LINEAR_PATH_AXES}
+ for ax in _LINEAR_PATH_AXES:
+ seq = enc_pos[ax]
+ if n >= 2:
+ for i in range(1, n - 1):
+ enc_vel[ax][i] = int(round((seq[i + 1] - seq[i - 1]) / (2.0 * dt_s)))
+ enc_vel[ax][n - 1] = int(round((seq[n - 1] - seq[n - 2]) / dt_s))
+
+ out: List[LinearPathSample] = []
+ for i in range(n):
+ out.append(LinearPathSample(
+ time_s=i * dt_s,
+ joints=poses[i],
+ encoder_position={ax: enc_pos[ax][i] for ax in _LINEAR_PATH_AXES},
+ encoder_velocity={ax: enc_vel[ax][i] for ax in _LINEAR_PATH_AXES},
+ ))
+ return out
+
+
+def _build_samples_from_joints(
+ joints_seq: List[Dict[Axis, float]],
+ cfg: KX2Config,
+ dt_s: float,
+) -> List[LinearPathSample]:
+ """Convert a joint-space sequence into encoder-domain PVT samples.
+
+ Mirrors the encoder + central-diff velocity block of ``sample_linear_path``.
+ Shared by every sampler so they all produce identically-formed samples.
+ """
+ n = len(joints_seq)
+ enc_pos: Dict[Axis, List[int]] = {ax: [] for ax in _LINEAR_PATH_AXES}
+ for joints in joints_seq:
+ for ax in _LINEAR_PATH_AXES:
+ conv = cfg.axes[ax].motor_conversion_factor
+ val = joints[ax]
+ if ax is Axis.ELBOW:
+ val = convert_elbow_position_to_angle(cfg, val)
+ enc_pos[ax].append(int(round(val * conv)))
+ # V[0]=0 (matches sample_linear_path and C# MotorsMovePath line 4242).
+ # Forward-diff initial velocity makes drives reject the first preload frame
+ # in some configurations (queue_full on first write).
+ enc_vel: Dict[Axis, List[int]] = {ax: [0] * n for ax in _LINEAR_PATH_AXES}
+ for ax in _LINEAR_PATH_AXES:
+ seq = enc_pos[ax]
+ if n >= 2:
+ for i in range(1, n - 1):
+ enc_vel[ax][i] = int(round((seq[i + 1] - seq[i - 1]) / (2.0 * dt_s)))
+ enc_vel[ax][n - 1] = int(round((seq[n - 1] - seq[n - 2]) / dt_s))
+ return [
+ LinearPathSample(
+ time_s=i * dt_s,
+ joints=joints_seq[i],
+ encoder_position={ax: enc_pos[ax][i] for ax in _LINEAR_PATH_AXES},
+ encoder_velocity={ax: enc_vel[ax][i] for ax in _LINEAR_PATH_AXES},
+ ) for i in range(n)
+ ]
+
+
+def _ik_pose_sequence(
+ poses: List[CartesianPose],
+ cfg: KX2Config,
+ gripper_params: GripperParams,
+ current_joints: Optional[Dict[Axis, float]],
+) -> List[Dict[Axis, float]]:
+ """IK every pose, snap rotary axes to the previous solution (avoiding 360°
+ unwinds across the trajectory). Raises IKError on the first out-of-range
+ pose with the trajectory-relative index in the message."""
+ prev: Optional[Dict[Axis, float]] = (
+ dict(current_joints) if current_joints is not None else None
+ )
+ out: List[Dict[Axis, float]] = []
+ for i, p in enumerate(poses):
+ try:
+ joints = ik(p, cfg, gripper_params)
+ except IKError as e:
+ raise IKError(f"sample {i}/{len(poses)}: {e}") from e
+ if prev is not None:
+ joints = snap_to_current(joints, prev)
+ out.append(joints)
+ prev = joints
+ return out
+
+
+def sample_parametric_path(
+ cfg: KX2Config,
+ gripper_params: GripperParams,
+ path_fn: "Callable[[float], CartesianPose]",
+ duration_s: float,
+ dt_s: float,
+ current_joints: Optional[Dict[Axis, float]] = None,
+) -> List[LinearPathSample]:
+ """Evaluate ``path_fn`` at every ``i * dt_s`` for ``i = 0..N`` (N chosen so
+ the last sample lands at or just past ``duration_s``), IK each pose, and
+ return encoder-domain samples ready for streaming.
+
+ Caller owns the velocity profile — ``path_fn`` defines both shape and pacing.
+ IK enforces joint limits; any out-of-range pose raises IKError with the
+ sample index.
+ """
+ if duration_s <= 0 or dt_s <= 0:
+ raise ValueError(f"duration_s and dt_s must be > 0 (got {duration_s}, {dt_s})")
+ n = max(2, int(duration_s / dt_s) + 1)
+ poses = [path_fn(min(i * dt_s, duration_s)) for i in range(n)]
+ joints_seq = _ik_pose_sequence(poses, cfg, gripper_params, current_joints)
+ return _build_samples_from_joints(joints_seq, cfg, dt_s)
+
+
+def sample_waypoint_path(
+ cfg: KX2Config,
+ gripper_params: GripperParams,
+ waypoints: List[CartesianPose],
+ speed_mm_per_s: float,
+ accel_mm_per_s2: float,
+ dt_s: float,
+ current_joints: Optional[Dict[Axis, float]] = None,
+) -> List[LinearPathSample]:
+ """Sample a Catmull-Rom spline through ``waypoints`` at the IPM cadence.
+
+ Curve geometry: centripetal Catmull-Rom through the location component of
+ every waypoint, with C¹ continuity at interior knots. Yaw is interpolated
+ linearly between knot rotations (short-way around the circle). Endpoint
+ tangents are extrapolated from the first/last segment.
+
+ Time parametrization: trapezoidal velocity profile over the spline's total
+ arc length, with peak speed ``speed_mm_per_s`` and peak acceleration
+ ``accel_mm_per_s2``. Returns to a triangular profile when the spline is too
+ short to reach peak speed.
+
+ The spline is dense-sampled internally (50 sub-steps per segment) to build
+ an arc-length lookup table; the trajectory is then sampled by stepping
+ through that table at dt-spaced arc-length increments.
+ """
+ if len(waypoints) < 2:
+ raise ValueError(f"need >= 2 waypoints, got {len(waypoints)}")
+ if speed_mm_per_s <= 0 or accel_mm_per_s2 <= 0 or dt_s <= 0:
+ raise ValueError(
+ f"speed, accel, dt_s must be > 0 (got {speed_mm_per_s}, "
+ f"{accel_mm_per_s2}, {dt_s})"
+ )
+
+ # Build extended control point list with mirrored endpoint tangents so the
+ # Catmull-Rom evaluator can use four consecutive points at every segment.
+ pts = [(w.location.x, w.location.y, w.location.z) for w in waypoints]
+ yaws = [w.rotation.z for w in waypoints]
+ ext = [tuple(2 * a - b for a, b in zip(pts[0], pts[1]))]
+ ext.extend(pts)
+ ext.append(tuple(2 * a - b for a, b in zip(pts[-1], pts[-2])))
+
+ def cr_eval(seg_idx: int, t: float) -> "Tuple[float, float, float]":
+ # Centripetal Catmull-Rom at local parameter t in [0, 1] within segment
+ # seg_idx (which goes between waypoints[seg_idx] and waypoints[seg_idx+1]).
+ p0, p1, p2, p3 = ext[seg_idx], ext[seg_idx + 1], ext[seg_idx + 2], ext[seg_idx + 3]
+ t2 = t * t
+ t3 = t2 * t
+ return tuple(
+ 0.5 * (
+ (2 * p1[k])
+ + (-p0[k] + p2[k]) * t
+ + (2 * p0[k] - 5 * p1[k] + 4 * p2[k] - p3[k]) * t2
+ + (-p0[k] + 3 * p1[k] - 3 * p2[k] + p3[k]) * t3
+ ) for k in range(3)
+ )
+
+ # Build (cumulative arc length → (segment, local t)) table by densely
+ # sampling the spline.
+ SUBSTEPS = 50
+ arc_table: List[Tuple[float, int, float]] = [] # (s, seg, local_t)
+ total_len = 0.0
+ prev_pt = pts[0]
+ arc_table.append((0.0, 0, 0.0))
+ for seg in range(len(pts) - 1):
+ for k in range(1, SUBSTEPS + 1):
+ t_local = k / SUBSTEPS
+ p = cr_eval(seg, t_local)
+ total_len += sqrt(sum((p[i] - prev_pt[i]) ** 2 for i in range(3)))
+ arc_table.append((total_len, seg, t_local))
+ prev_pt = p
+
+ if total_len <= _EPS:
+ # All waypoints colinear at one point — bail out with a hold-in-place
+ # 2-sample trajectory so the streamer no-ops cleanly.
+ one_pose = waypoints[0]
+ return sample_parametric_path(
+ cfg=cfg, gripper_params=gripper_params,
+ path_fn=lambda _t: one_pose, duration_s=dt_s, dt_s=dt_s,
+ current_joints=current_joints,
+ )
+
+ # Trapezoidal time → arc-length profile.
+ s_seq = _arc_length_trapezoid_times(total_len, speed_mm_per_s, accel_mm_per_s2, dt_s)
+ n = len(s_seq)
+
+ # Resolve each arc-length s into (seg, local_t) via binary search in the table.
+ poses: List[CartesianPose] = []
+ for s in s_seq:
+ s_clamped = min(max(s, 0.0), total_len)
+ lo, hi = 0, len(arc_table) - 1
+ while hi - lo > 1:
+ mid = (lo + hi) // 2
+ if arc_table[mid][0] <= s_clamped:
+ lo = mid
+ else:
+ hi = mid
+ s0, seg0, t0 = arc_table[lo]
+ s1, seg1, t1 = arc_table[hi]
+ span = s1 - s0
+ alpha = 0.0 if span <= _EPS else (s_clamped - s0) / span
+ # Linear interpolation in local-t space within the same segment; if the
+ # table spans a segment boundary, evaluate using the higher one.
+ if seg0 == seg1:
+ seg = seg0
+ tl = t0 + alpha * (t1 - t0)
+ else:
+ seg = seg1
+ tl = t1 * alpha
+ x, y, z = cr_eval(seg, tl)
+ # Yaw interpolation: chord-length parameter along the full polyline so
+ # yaw lerps across segment boundaries naturally.
+ yaw_alpha = s_clamped / total_len
+ # Map yaw_alpha (0..1 over whole polyline) onto waypoint indices linearly.
+ yaw_pos = yaw_alpha * (len(yaws) - 1)
+ yi = min(int(yaw_pos), len(yaws) - 2)
+ yf = yaw_pos - yi
+ yaw = _yaw_lerp(yaws[yi], yaws[yi + 1], yf)
+ poses.append(CartesianPose(location=Coordinate(x=x, y=y, z=z),
+ rotation=Rotation(z=yaw)))
+
+ joints_seq = _ik_pose_sequence(poses, cfg, gripper_params, current_joints)
+ return _build_samples_from_joints(joints_seq, cfg, dt_s)
diff --git a/pylabrobot/paa/kx2/kx2.py b/pylabrobot/paa/kx2/kx2.py
new file mode 100644
index 00000000000..b7953298eca
--- /dev/null
+++ b/pylabrobot/paa/kx2/kx2.py
@@ -0,0 +1,110 @@
+import logging
+from typing import Optional
+
+from pylabrobot.capabilities.arms.orientable_arm import OrientableArm
+from pylabrobot.capabilities.barcode_scanning import BarcodeScanner
+from pylabrobot.capabilities.capability import BackendParams
+from pylabrobot.device import Device
+from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend
+from pylabrobot.paa.kx2.barcode_reader import (
+ KX2BarcodeReaderBackend,
+ KX2BarcodeReaderDriver,
+)
+from pylabrobot.paa.kx2.config import GripperFingerSide
+from pylabrobot.paa.kx2.driver import KX2Driver
+from pylabrobot.resources.resource import Resource
+
+logger = logging.getLogger(__name__)
+
+
+class KX2(Device):
+ """PAA KX2 robotic plate handler."""
+
+ def __init__(
+ self,
+ gripper_length: float = 0.0,
+ gripper_z_offset: float = 0.0,
+ gripper_finger_side: GripperFingerSide = "barcode_reader",
+ has_rail: bool = False,
+ has_servo_gripper: bool = True,
+ barcode_port: Optional[str] = None,
+ barcode_baudrate: int = KX2BarcodeReaderDriver.default_baudrate,
+ ) -> None:
+ """
+ Args:
+ gripper_length: distance from the wrist axis to the gripper's
+ *grip center* (geometric midpoint of the jaws) in mm.
+ Non-negative; ``gripper_finger_side`` selects which side, not
+ the sign of ``gripper_length``. For the same target
+ ``(location, direction)``, IK puts the wrist on opposite sides
+ of the grip center for the two finger-side choices.
+ gripper_z_offset: vertical offset from the wrist plate to the
+ grip center in mm. Positive = grip center below the wrist plate.
+ gripper_finger_side: which finger is treated as the gripper's
+ "front". The world yaw returned by
+ :meth:`arm.request_gripper_pose` (and the ``direction``
+ argument to :meth:`arm.move_to_location`) points at this
+ finger. Flipping side is a 180° relabel of which finger is
+ "front" — for the same joints the grip center is unchanged
+ and only the reported yaw shifts by 180°.
+ has_rail: True if the arm is mounted on the optional linear
+ rail (axis 5).
+ has_servo_gripper: True if a servo-driven plate gripper is on
+ the bus (axis 6).
+ barcode_port: optional serial port of the onboard barcode reader.
+ When set, exposes :attr:`barcode_scanning`.
+ barcode_baudrate: barcode-reader serial baud rate.
+ """
+ driver = KX2Driver(has_rail=has_rail, has_servo_gripper=has_servo_gripper)
+ super().__init__(driver=driver)
+ self.driver: KX2Driver = driver
+ backend = KX2ArmBackend(
+ driver=driver,
+ gripper_length=gripper_length,
+ gripper_z_offset=gripper_z_offset,
+ gripper_finger_side=gripper_finger_side,
+ )
+ self.reference = Resource(name="KX2", size_x=200, size_y=200, size_z=200)
+ self.arm = OrientableArm(backend=backend, reference_resource=self.reference)
+ self._capabilities = [self.arm]
+
+ # Onboard barcode reader is a serial device on a separate USB-serial port,
+ # entirely independent of the CAN bus that drives the motors. Wire it in
+ # only when a port is given; the standalone `KX2BarcodeReader` Device
+ # remains available for users who prefer to manage it as a sibling.
+ self._bcr_driver: Optional[KX2BarcodeReaderDriver] = None
+ self.barcode_scanning: Optional[BarcodeScanner] = None
+ if barcode_port is not None:
+ self._bcr_driver = KX2BarcodeReaderDriver(
+ port=barcode_port, baudrate=barcode_baudrate,
+ )
+ bcr_backend = KX2BarcodeReaderBackend(self._bcr_driver)
+ self.barcode_scanning = BarcodeScanner(backend=bcr_backend)
+ self._capabilities.append(self.barcode_scanning)
+
+ async def setup(self, backend_params: Optional[BackendParams] = None):
+ # Idempotent: re-running setup on a live KX2 used to crash partway through
+ # the homing/PDO sequence. If already up, stay up — call stop() first to
+ # force a fresh init.
+ if self.setup_finished:
+ logger.info("KX2.setup: already set up; skipping. Call stop() first to re-init.")
+ return
+ # Bring the barcode reader's serial port up before super().setup() so the
+ # barcode_scanning capability's _on_setup (version handshake) has an open
+ # port. If the BCR fails, abort before touching the CAN bus.
+ if self._bcr_driver is not None:
+ await self._bcr_driver.setup()
+ try:
+ await super().setup(backend_params=backend_params)
+ except BaseException:
+ if self._bcr_driver is not None:
+ try:
+ await self._bcr_driver.stop()
+ except Exception:
+ logger.exception("KX2.setup cleanup: BCR driver stop failed; ignoring")
+ raise
+
+ async def stop(self):
+ await super().stop()
+ if self._bcr_driver is not None:
+ await self._bcr_driver.stop()
diff --git a/pylabrobot/paa/kx2/tests/__init__.py b/pylabrobot/paa/kx2/tests/__init__.py
new file mode 100644
index 00000000000..e69de29bb2d
diff --git a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py
new file mode 100644
index 00000000000..8eff9df7ede
--- /dev/null
+++ b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py
@@ -0,0 +1,408 @@
+"""Unit tests for ``KX2ArmBackend.find_z_with_proximity_sensor``.
+
+Covers the four interesting paths through the descent + IL-arm + cancel-on-trip
+state machine:
+
+* Sensor trips mid-descent -> task is cancelled, motor_stop + IL-restore run.
+* Sensor never trips -> RuntimeError mentioning the descent range.
+* Move task raises a CanError mid-descent -> cleanup still runs, no exception
+ escapes from the swallow at ``await move_task``.
+* Sensor already tripped at start -> short-circuit returns z0; no IL change,
+ no descent task spawned.
+
+The fake backend stubs the driver methods the function actually calls
+(``motor_enable``, ``motor_stop``, ``configure_input_logic``,
+``motor_get_current_position`` via ``read_input``-style scripts) and replaces
+``_motors_move_joint_locked`` with an async sleeper so the test controls
+when the descent "finishes". The motion guard is initialised so
+``async with self._motion_guard()`` works without a real lock setup race.
+"""
+import asyncio
+import unittest
+from typing import Any, Dict, List, NamedTuple, Optional, Tuple
+
+from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend
+from pylabrobot.paa.kx2.config import Axis
+from pylabrobot.paa.kx2.driver import CanError, _InputLogic
+
+
+class _FakeDriver:
+ """Records every method ``find_z_with_proximity_sensor`` calls. Each
+ method is async so ``await`` works as in production."""
+
+ def __init__(self) -> None:
+ self.motor_enable_calls: List[Tuple[Any, bool, bool]] = []
+ self.motor_stop_calls: List[Axis] = []
+ self.configure_il_calls: List[Tuple[Any, int, _InputLogic]] = []
+ self.motor_stop_should_raise: Optional[Exception] = None
+ self.ensure_enabled_calls: List[Tuple[int, ...]] = []
+ self.clear_emcy_calls: List[Optional[int]] = []
+
+ async def motor_enable(self, *, node_id: Any, state: bool, use_ds402: bool) -> None:
+ self.motor_enable_calls.append((node_id, state, use_ds402))
+
+ async def motors_ensure_enabled(
+ self, node_ids: List[int], *, use_ds402: bool = True,
+ ) -> None:
+ self.ensure_enabled_calls.append(tuple(int(n) for n in node_ids))
+
+ def clear_emcy_state(self, node_id: Optional[int] = None) -> None:
+ self.clear_emcy_calls.append(node_id)
+
+ async def motor_stop(self, axis: Axis) -> None:
+ self.motor_stop_calls.append(axis)
+ if self.motor_stop_should_raise is not None:
+ raise self.motor_stop_should_raise
+
+ async def configure_input_logic(
+ self, axis: Any, input_num: int, logic: _InputLogic,
+ ) -> None:
+ self.configure_il_calls.append((axis, input_num, logic))
+
+
+class _FindZHarness(NamedTuple):
+ """Bundle of test handles. Returning a typed tuple keeps mypy happy
+ (fake_driver is _FakeDriver, not KX2Driver) without polluting the
+ KX2ArmBackend instance with ad-hoc attributes."""
+ backend: KX2ArmBackend
+ fake_driver: _FakeDriver
+ move_calls: List[Dict[str, Any]]
+
+
+def _build_harness(
+ *,
+ proximity_script: List[bool],
+ z_positions: List[float],
+ move_behavior: str = "long_sleep",
+ move_exception: Optional[Exception] = None,
+ pre_position_completes: bool = False,
+) -> _FindZHarness:
+ """Construct a KX2ArmBackend wired for find_z testing.
+
+ Args:
+ proximity_script: Sequence of bools returned by ``read_proximity_sensor``;
+ one popped per call. The last element is sticky once exhausted.
+ z_positions: Sequence returned by ``motor_get_current_position`` calls.
+ Same exhaust-then-sticky behavior.
+ move_behavior: ``"long_sleep"`` (default; awaits ~10 s so the test cancels
+ it via the trip path) or ``"completes_immediately"`` (returns as soon
+ as the task is scheduled — used for the "sensor never trips" test).
+ move_exception: If set, ``_motors_move_joint_locked`` raises this from
+ inside the spawned task before it would otherwise sleep.
+ pre_position_completes: If True, the first move call (the pre-position
+ to ``z_start``) completes immediately regardless of ``move_behavior``;
+ subsequent moves (the descent) follow ``move_behavior``. Without this,
+ a ``long_sleep`` behavior would block on the awaited pre-position.
+ """
+ backend = KX2ArmBackend.__new__(KX2ArmBackend)
+ fake_driver = _FakeDriver()
+ backend.driver = fake_driver # type: ignore[assignment]
+ backend._motion_lock = asyncio.Lock()
+ backend._motion_owner = None
+ backend._gripper_params = None # type: ignore[assignment] # not consumed by find_z
+
+ prox = list(proximity_script)
+ zs = list(z_positions)
+
+ async def _read_proximity() -> bool:
+ # Yield once so any concurrently scheduled task (the descent move) gets
+ # a chance to run before we observe the next sensor value. Without this
+ # an immediate-True script can race the move task and cancel it before
+ # it has a chance to record its call.
+ await asyncio.sleep(0)
+ val = prox[0] if len(prox) == 1 else prox.pop(0)
+ return val
+
+ async def _get_z(axis: Axis) -> float:
+ val = zs[0] if len(zs) == 1 else zs.pop(0)
+ return float(val)
+
+ move_calls: List[Dict[str, Any]] = []
+
+ async def _fake_move(cmd_pos: Dict[Axis, float], params: Any = None) -> None:
+ move_calls.append({"cmd_pos": dict(cmd_pos), "params": params})
+ is_first = len(move_calls) == 1
+ if pre_position_completes and is_first:
+ return # Pre-position always completes when this flag is set.
+ if move_exception is not None:
+ raise move_exception
+ if move_behavior == "completes_immediately":
+ return
+ # "long_sleep": stays awaitable until cancelled by find_z's trip handler.
+ await asyncio.sleep(10.0)
+
+ backend.read_proximity_sensor = _read_proximity # type: ignore[assignment]
+ backend.motor_get_current_position = _get_z # type: ignore[assignment]
+ backend._motors_move_joint_locked = _fake_move # type: ignore[assignment]
+ return _FindZHarness(backend=backend, fake_driver=fake_driver, move_calls=move_calls)
+
+
+class FindZSensorTripsCleanupTests(unittest.TestCase):
+ def test_trip_cancels_descent_and_runs_cleanup(self):
+ """Sensor returns False on the pre-check, then True after one poll
+ cycle. Verify: pre-position move ran, descent task spawned then
+ cancelled, motor_stop ran, IL restored to GeneralPurpose."""
+ h = _build_harness(
+ # Pre-position move (completes_immediately so the descent path is
+ # reached), pre-check sensor=False, poll sensor=True (trip).
+ proximity_script=[False, True],
+ # Z read once at the end after cleanup, returns the stop position.
+ z_positions=[95.5],
+ pre_position_completes=True,
+ )
+
+ z = asyncio.run(h.backend.find_z_with_proximity_sensor(
+ z_start=100.0, z_end=80.0,
+ ))
+
+ self.assertAlmostEqual(z, 95.5, places=9)
+ arm = (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.StopForward)
+ restore = (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose)
+ self.assertEqual(h.fake_driver.configure_il_calls, [arm, restore])
+ self.assertEqual(h.fake_driver.motor_stop_calls, [Axis.Z])
+ self.assertEqual(h.fake_driver.ensure_enabled_calls, [(int(Axis.Z),)])
+ # Two moves: pre-position to z_start, descent to z_end.
+ self.assertEqual(len(h.move_calls), 2)
+ self.assertAlmostEqual(h.move_calls[0]["cmd_pos"][Axis.Z], 100.0, places=9)
+ self.assertAlmostEqual(h.move_calls[1]["cmd_pos"][Axis.Z], 80.0, places=9)
+
+
+class FindZSensorNeverTripsTests(unittest.TestCase):
+ def test_never_trips_raises_runtime_error_with_range(self):
+ """Descent completes without a trip → raises with z_start, z_end, and
+ the actual stop position in the message."""
+ h = _build_harness(
+ proximity_script=[False],
+ z_positions=[79.5],
+ move_behavior="completes_immediately", # both moves complete immediately
+ )
+
+ with self.assertRaises(RuntimeError) as ctx:
+ asyncio.run(h.backend.find_z_with_proximity_sensor(
+ z_start=100.0, z_end=80.0,
+ ))
+
+ msg = str(ctx.exception)
+ self.assertIn("never tripped", msg)
+ self.assertIn("100.00", msg)
+ self.assertIn("80.00", msg)
+ self.assertIn("79.50", msg)
+ self.assertEqual(h.fake_driver.motor_stop_calls, [Axis.Z])
+ self.assertIn(
+ (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose),
+ h.fake_driver.configure_il_calls,
+ )
+
+
+class FindZMoveRaisesCanErrorTests(unittest.TestCase):
+ def test_canerror_in_descent_is_swallowed_cleanup_runs(self):
+ """Descent task raises CanError. ``await move_task`` swallows it; sensor
+ stayed False, so the function raises RuntimeError *after* cleanup.
+ motor_stop + IL restore still ran."""
+ h = _build_harness(
+ proximity_script=[False],
+ z_positions=[100.0],
+ move_exception=CanError("synthetic descent failure"),
+ pre_position_completes=True, # Only the descent should raise, not the pre-position.
+ )
+
+ with self.assertRaises(RuntimeError) as ctx:
+ asyncio.run(h.backend.find_z_with_proximity_sensor(
+ z_start=100.0, z_end=80.0,
+ ))
+
+ self.assertIn("never tripped", str(ctx.exception))
+ self.assertEqual(h.fake_driver.motor_stop_calls, [Axis.Z])
+ self.assertEqual(
+ h.fake_driver.configure_il_calls[-1],
+ (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose),
+ )
+
+ def test_motor_stop_failure_is_logged_not_raised(self):
+ """Even if motor_stop fails during cleanup, IL restore still runs and
+ the function still completes its happy-path return (sensor tripped)."""
+ h = _build_harness(
+ proximity_script=[False, True],
+ z_positions=[95.5],
+ pre_position_completes=True,
+ )
+ h.fake_driver.motor_stop_should_raise = CanError("synthetic stop failure")
+
+ z = asyncio.run(h.backend.find_z_with_proximity_sensor(
+ z_start=100.0, z_end=80.0,
+ ))
+
+ self.assertAlmostEqual(z, 95.5, places=9)
+ self.assertEqual(
+ h.fake_driver.configure_il_calls[-1],
+ (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose),
+ )
+
+
+class FindZAlreadyTrippedTests(unittest.TestCase):
+ def test_pre_check_already_tripped_skips_descent(self):
+ """After the pre-position move, the sensor reads tripped: returns
+ current Z without arming IL or spawning a descent task."""
+ h = _build_harness(
+ proximity_script=[True],
+ z_positions=[55.0],
+ move_behavior="completes_immediately", # pre-position completes, then trip
+ )
+
+ z = asyncio.run(h.backend.find_z_with_proximity_sensor(
+ z_start=80.0, z_end=20.0,
+ ))
+
+ self.assertAlmostEqual(z, 55.0, places=9)
+ # Pre-position ran, but no descent — only one move call.
+ self.assertEqual(len(h.move_calls), 1)
+ self.assertAlmostEqual(h.move_calls[0]["cmd_pos"][Axis.Z], 80.0, places=9)
+ # No IL configure (short-circuit hits before the StopForward arm),
+ # no motor_stop. ensure_enabled still ran as pre-flight.
+ self.assertEqual(h.fake_driver.configure_il_calls, [])
+ self.assertEqual(h.fake_driver.motor_stop_calls, [])
+ self.assertEqual(h.fake_driver.ensure_enabled_calls, [(int(Axis.Z),)])
+
+
+class FindZArgValidationTests(unittest.TestCase):
+ def test_z_end_above_z_start_raises(self):
+ """Search must descend (z_end < z_start). Surface the typo before any
+ drive interaction."""
+ h = _build_harness(proximity_script=[False], z_positions=[0.0])
+ with self.assertRaises(ValueError):
+ asyncio.run(h.backend.find_z_with_proximity_sensor(
+ z_start=80.0, z_end=100.0,
+ ))
+ # Nothing happened on the drive side.
+ self.assertEqual(h.fake_driver.ensure_enabled_calls, [])
+ self.assertEqual(h.move_calls, [])
+
+
+class _FindGenericHarness(NamedTuple):
+ backend: KX2ArmBackend
+ fake_driver: _FakeDriver
+ joint_move_calls: List[Dict[str, Any]]
+ linear_move_calls: List[Dict[str, Any]]
+
+
+def _build_generic_harness(
+ *,
+ proximity_script: List[bool],
+ gripper_locations: List[Any],
+ linear_behavior: str = "long_sleep",
+) -> _FindGenericHarness:
+ """Harness for ``find_with_proximity_sensor``. Fakes _run_linear_path
+ (the IPM streaming runtime), _cart_to_joints (IK), and
+ request_gripper_pose (FK) so the test exercises the polling-and-
+ cancel logic without spinning up a real driver."""
+ from pylabrobot.capabilities.arms.standard import CartesianPose
+ from pylabrobot.resources import Rotation
+
+ backend = KX2ArmBackend.__new__(KX2ArmBackend)
+ fake_driver = _FakeDriver()
+ backend.driver = fake_driver # type: ignore[assignment]
+ backend._motion_lock = asyncio.Lock()
+ backend._motion_owner = None
+ backend._gripper_params = None # type: ignore[assignment]
+
+ prox = list(proximity_script)
+ locs = list(gripper_locations)
+
+ async def _read_proximity() -> bool:
+ await asyncio.sleep(0)
+ return prox[0] if len(prox) == 1 else prox.pop(0)
+
+ async def _request_gripper_pose():
+ val = locs[0] if len(locs) == 1 else locs.pop(0)
+ if isinstance(val, CartesianPose):
+ return val
+ return CartesianPose(location=val, rotation=Rotation(z=0))
+
+ async def _cart_to_joints(pose):
+ return {Axis.SHOULDER: 0.0, Axis.Z: pose.location.z, Axis.ELBOW: 100.0,
+ Axis.WRIST: pose.rotation.z}
+
+ joint_calls: List[Dict[str, Any]] = []
+ linear_calls: List[Dict[str, Any]] = []
+
+ async def _fake_joint_move(cmd_pos, params=None):
+ joint_calls.append({"cmd_pos": dict(cmd_pos), "params": params})
+
+ async def _fake_run_linear(target_pose, params):
+ linear_calls.append({"pose": target_pose, "params": params})
+ if linear_behavior == "completes_immediately":
+ return
+ try:
+ await asyncio.sleep(10.0)
+ finally:
+ # Mirror the real method's finally-block behavior.
+ pass
+
+ backend.read_proximity_sensor = _read_proximity # type: ignore[assignment]
+ backend.request_gripper_pose = _request_gripper_pose # type: ignore[assignment]
+ backend._cart_to_joints = _cart_to_joints # type: ignore[assignment]
+ backend._motors_move_joint_locked = _fake_joint_move # type: ignore[assignment]
+ backend._run_linear_path = _fake_run_linear # type: ignore[assignment]
+ return _FindGenericHarness(backend, fake_driver, joint_calls, linear_calls)
+
+
+class FindGenericTests(unittest.TestCase):
+ def test_trip_cancels_sweep_returns_gripper_location(self):
+ """Sensor trips during the sweep: linear-path task is cancelled,
+ EMCY state cleared, and the gripper location at halt is returned."""
+ from pylabrobot.resources import Coordinate
+ h = _build_generic_harness(
+ proximity_script=[False, True],
+ gripper_locations=[Coordinate(x=110.0, y=200.0, z=50.0)],
+ )
+
+ result = asyncio.run(h.backend.find_with_proximity_sensor(
+ start=Coordinate(x=100, y=200, z=50),
+ end=Coordinate(x=200, y=200, z=50),
+ ))
+
+ self.assertAlmostEqual(result.x, 110.0, places=6)
+ self.assertEqual(len(h.joint_move_calls), 1) # Pre-position only.
+ self.assertEqual(len(h.linear_move_calls), 1) # Sweep itself.
+ # Sticky EMCY cleared so the next motion call doesn't raise on a
+ # halt-induced frame.
+ self.assertEqual(h.fake_driver.clear_emcy_calls, [None])
+
+ def test_never_trips_raises_with_endpoints(self):
+ from pylabrobot.resources import Coordinate
+ h = _build_generic_harness(
+ proximity_script=[False],
+ gripper_locations=[Coordinate(x=200.0, y=200.0, z=50.0)],
+ linear_behavior="completes_immediately",
+ )
+
+ with self.assertRaises(RuntimeError) as ctx:
+ asyncio.run(h.backend.find_with_proximity_sensor(
+ start=Coordinate(x=100, y=200, z=50),
+ end=Coordinate(x=200, y=200, z=50),
+ ))
+
+ msg = str(ctx.exception)
+ self.assertIn("never tripped", msg)
+ self.assertIn("100.0", msg)
+ self.assertIn("200.0", msg)
+
+ def test_pre_check_already_tripped_skips_sweep(self):
+ from pylabrobot.resources import Coordinate
+ h = _build_generic_harness(
+ proximity_script=[True],
+ gripper_locations=[Coordinate(x=100.0, y=200.0, z=50.0)],
+ )
+
+ result = asyncio.run(h.backend.find_with_proximity_sensor(
+ start=Coordinate(x=100, y=200, z=50),
+ end=Coordinate(x=200, y=200, z=50),
+ ))
+
+ self.assertAlmostEqual(result.x, 100.0, places=6)
+ self.assertEqual(len(h.joint_move_calls), 1) # Pre-position ran.
+ self.assertEqual(len(h.linear_move_calls), 0) # No sweep.
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/pylabrobot/paa/kx2/tests/arm_backend_pvt_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_pvt_tests.py
new file mode 100644
index 00000000000..8b3aeb1ae32
--- /dev/null
+++ b/pylabrobot/paa/kx2/tests/arm_backend_pvt_tests.py
@@ -0,0 +1,305 @@
+"""Unit tests for ``KX2ArmBackend._run_linear_path``.
+
+Exercises the IPM streaming runtime that backs ``CartesianMoveParams(path='linear')``:
+
+* Preload + begin_motion call ordering. Preload writes 8 frames into the
+ drive's interpolation buffer *before* CW=0x1F is issued so the first sync
+ doesn't underflow the queue.
+* Per-sample (P, V) frames go to all four arm axes (SHOULDER/Z/ELBOW/WRIST)
+ in lockstep.
+* Cancellation mid-stream runs the finally-block cleanup: ``ipm_stop`` then
+ ``ipm_select_mode(False)``, both even if the cancel arrives before the
+ trapezoid is exhausted.
+* Validation: requesting ``path='linear'`` without ``max_gripper_speed`` or
+ ``max_gripper_acceleration`` raises ValueError.
+
+Driver methods are stubbed on a fake recorder; ``request_joint_position`` is
+shimmed so the runtime computes start_pose without going to hardware. Motion
+guard is initialised the same way the find_z tests do it (manual Lock +
+``__new__``).
+"""
+import asyncio
+import unittest
+from typing import Any, Dict, List, Optional, Tuple
+
+from pylabrobot.capabilities.arms.standard import CartesianPose
+from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend
+from pylabrobot.paa.kx2.config import (
+ Axis, AxisConfig, GripperParams, KX2Config,
+)
+from pylabrobot.paa.kx2.driver import JointMoveDirection
+from pylabrobot.resources import Coordinate, Rotation
+
+
+def _axis() -> AxisConfig:
+ return AxisConfig(
+ motor_conversion_factor=1000.0,
+ max_travel=180.0,
+ min_travel=-180.0,
+ unlimited_travel=False,
+ absolute_encoder=True,
+ max_vel=100.0,
+ max_accel=100.0,
+ joint_move_direction=JointMoveDirection.Normal,
+ digital_inputs={},
+ analog_inputs={},
+ outputs={},
+ )
+
+
+def _config() -> KX2Config:
+ return KX2Config(
+ wrist_offset=10.0,
+ elbow_offset=20.0,
+ elbow_zero_offset=5.0,
+ axes={a: _axis() for a in (
+ Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST,
+ )},
+ base_to_gripper_clearance_z=0.0,
+ base_to_gripper_clearance_arm=0.0,
+ robot_on_rail=False,
+ servo_gripper=None,
+ )
+
+
+class _FakeDriver:
+ """Records every IPM-related call. Order matters: tests assert the
+ preload-then-begin-motion sequencing."""
+
+ def __init__(self) -> None:
+ self.calls: List[Tuple[str, Tuple[Any, ...]]] = []
+ self.send_calls: List[Tuple[int, int, int]] = []
+ self.cancel_after_sends: Optional[int] = None
+
+ async def ipm_select_mode(self, enable: bool) -> None:
+ self.calls.append(("ipm_select_mode", (enable,)))
+
+ async def ipm_set_time_interval(self, ms: int) -> None:
+ self.calls.append(("ipm_set_time_interval", (ms,)))
+
+ def ipm_send_pvt_point(
+ self, node_id: int, position_enc: int, velocity_enc_per_s: int,
+ ) -> None:
+ self.send_calls.append((node_id, position_enc, velocity_enc_per_s))
+ self.calls.append((
+ "ipm_send_pvt_point", (node_id, position_enc, velocity_enc_per_s),
+ ))
+ if (
+ self.cancel_after_sends is not None
+ and len(self.send_calls) >= self.cancel_after_sends
+ ):
+ raise asyncio.CancelledError("test injection")
+
+ async def ipm_begin_motion(self, node_ids: List[int]) -> None:
+ self.calls.append(("ipm_begin_motion", (tuple(node_ids),)))
+
+ async def ipm_stop(self, node_ids: Optional[List[int]] = None) -> None:
+ self.calls.append((
+ "ipm_stop", (tuple(node_ids) if node_ids is not None else None,),
+ ))
+
+ def ipm_check_queue_fault(self, node_ids: List[int]) -> None:
+ # Synchronous helper; record the call so tests can verify backpressure
+ # checks happen at the expected points.
+ self.calls.append(("ipm_check_queue_fault", (tuple(int(n) for n in node_ids),)))
+
+ async def ipm_wait_motion_complete(
+ self, node_ids: List[int], timeout_s: float,
+ ) -> None:
+ self.calls.append((
+ "ipm_wait_motion_complete", (tuple(int(n) for n in node_ids), timeout_s),
+ ))
+
+ async def motors_ensure_enabled(
+ self, node_ids: List[int], *, use_ds402: bool = True,
+ ) -> None:
+ self.calls.append(("motors_ensure_enabled", (tuple(int(n) for n in node_ids),)))
+
+ def clear_emcy_state(self, node_id=None):
+ self.calls.append(("clear_emcy_state", (node_id,)))
+
+ async def motor_get_current_position(self, node_id, pu=False):
+ return 0
+
+
+def _build_backend(
+ fake_driver: _FakeDriver,
+ *,
+ current_joints: Optional[Dict[int, float]] = None,
+) -> KX2ArmBackend:
+ backend = KX2ArmBackend.__new__(KX2ArmBackend)
+ backend.driver = fake_driver # type: ignore[assignment]
+ backend._motion_lock = asyncio.Lock()
+ backend._motion_owner = None
+ backend._config = _config()
+ backend._gripper_params = GripperParams(length=15.0, z_offset=3.0)
+
+ joints = current_joints if current_joints is not None else {
+ int(Axis.SHOULDER): 30.0,
+ int(Axis.Z): 50.0,
+ int(Axis.ELBOW): 60.0,
+ int(Axis.WRIST): 0.0,
+ int(Axis.SERVO_GRIPPER): 0.0,
+ }
+
+ async def _request_joint_position() -> Dict[int, float]:
+ return dict(joints)
+
+ backend.request_joint_position = _request_joint_position # type: ignore[assignment]
+ return backend
+
+
+def _target_pose() -> CartesianPose:
+ """A pose ~30 mm away from the harness's current_joints — long enough that
+ the arc-length trapezoid produces well over the 8-frame preload."""
+ return CartesianPose(
+ location=Coordinate(x=20.0, y=110.0, z=80.0),
+ rotation=Rotation(z=15.0),
+ )
+
+
+class LinearPathHappyPath(unittest.TestCase):
+ def test_preload_then_begin_then_stream(self):
+ """Preload 8 frames per axis (32 sends), then ipm_begin_motion, then
+ further per-frame sends in lockstep across the four arm axes."""
+ fake = _FakeDriver()
+ backend = _build_backend(fake)
+ params = KX2ArmBackend.CartesianMoveParams(
+ max_gripper_speed=20.0,
+ max_gripper_acceleration=200.0,
+ path="linear",
+ )
+ asyncio.run(backend._run_linear_path(_target_pose(), params))
+
+ # Find begin_motion in the call log
+ begin_idx = next(
+ i for i, (n, _) in enumerate(fake.calls) if n == "ipm_begin_motion"
+ )
+ pre = fake.calls[:begin_idx]
+ pre_sends = [c for c in pre if c[0] == "ipm_send_pvt_point"]
+ # 4 axes * 8 preload frames = 32 sends before begin_motion
+ self.assertEqual(len(pre_sends), 4 * backend._PVT_PRELOAD)
+
+ # begin_motion received the four arm axes
+ _, (axes,) = fake.calls[begin_idx]
+ self.assertEqual(set(axes), {
+ int(Axis.SHOULDER), int(Axis.Z), int(Axis.ELBOW), int(Axis.WRIST),
+ })
+
+ def test_dt_set_to_8ms_default(self):
+ fake = _FakeDriver()
+ backend = _build_backend(fake)
+ params = KX2ArmBackend.CartesianMoveParams(
+ max_gripper_speed=20.0, max_gripper_acceleration=200.0, path="linear",
+ )
+ asyncio.run(backend._run_linear_path(_target_pose(), params))
+ dt_calls = [c for c in fake.calls if c[0] == "ipm_set_time_interval"]
+ self.assertEqual(len(dt_calls), 1)
+ self.assertEqual(dt_calls[0][1], (backend._PVT_DT_MS,))
+
+ def test_select_mode_true_then_false(self):
+ """Mode is flipped on at start, off in cleanup. The drive must end in
+ PPM so subsequent joint moves don't try to issue PPM triggers in IPM."""
+ fake = _FakeDriver()
+ backend = _build_backend(fake)
+ params = KX2ArmBackend.CartesianMoveParams(
+ max_gripper_speed=20.0, max_gripper_acceleration=200.0, path="linear",
+ )
+ asyncio.run(backend._run_linear_path(_target_pose(), params))
+ selects = [c[1] for c in fake.calls if c[0] == "ipm_select_mode"]
+ self.assertEqual(selects[0], (True,))
+ self.assertEqual(selects[-1], (False,))
+
+ def test_stream_end_drops_ip_enable_no_blocking_wait(self):
+ """After the last frame, the runtime must drop ip-enable (ipm_stop) and
+ NOT poll for any wait condition. The Elmo drive doesn't latch SW bit-10
+ until ip-enable goes low — polling it inside IP mode hangs forever — and
+ leaving ip-enable asserted past the buffer drain raises EMCY 0x8A
+ (underflow). Mirrors C# MotorsMovePathExecute which proceeds straight to
+ PVTBeginMotion(false) once streaming completes."""
+ fake = _FakeDriver()
+ backend = _build_backend(fake)
+ params = KX2ArmBackend.CartesianMoveParams(
+ max_gripper_speed=20.0, max_gripper_acceleration=200.0, path="linear",
+ )
+ asyncio.run(backend._run_linear_path(_target_pose(), params))
+ self.assertFalse([c for c in fake.calls if c[0] == "wait_for_moves_done"])
+ self.assertFalse([c for c in fake.calls if c[0] == "ipm_wait_motion_complete"])
+ stops = [c for c in fake.calls if c[0] == "ipm_stop"]
+ self.assertGreaterEqual(len(stops), 1)
+ (axes,) = stops[0][1]
+ self.assertEqual(set(axes), {
+ int(Axis.SHOULDER), int(Axis.Z), int(Axis.ELBOW), int(Axis.WRIST),
+ })
+
+ def test_queue_fault_checked_after_preload_and_each_send(self):
+ """After preload and after every streamed send, the runtime must
+ inspect _emcy via ipm_check_queue_fault — otherwise queue_full /
+ underflow EMCYs are silently swallowed and the move appears to
+ succeed even when the drive rejected our points."""
+ fake = _FakeDriver()
+ backend = _build_backend(fake)
+ params = KX2ArmBackend.CartesianMoveParams(
+ max_gripper_speed=20.0, max_gripper_acceleration=200.0, path="linear",
+ )
+ asyncio.run(backend._run_linear_path(_target_pose(), params))
+ fault_checks = [c for c in fake.calls if c[0] == "ipm_check_queue_fault"]
+ # At least one after preload, then one after each streamed send, plus
+ # one after the wait — total ≥ 3.
+ self.assertGreaterEqual(len(fault_checks), 3)
+
+
+class LinearPathCancellationCleanup(unittest.TestCase):
+ def test_cancel_mid_stream_runs_ipm_stop_and_ipm_select_false(self):
+ """Inject a CancelledError after a few sends. Both cleanup steps must
+ execute regardless: ipm_stop dropping ip-enable, then ipm_select_mode
+ flipping back to PPM."""
+ fake = _FakeDriver()
+ fake.cancel_after_sends = 12 # 8 preload + a few streamed
+ backend = _build_backend(fake)
+ params = KX2ArmBackend.CartesianMoveParams(
+ max_gripper_speed=20.0, max_gripper_acceleration=200.0, path="linear",
+ )
+ with self.assertRaises(asyncio.CancelledError):
+ asyncio.run(backend._run_linear_path(_target_pose(), params))
+
+ names = [c[0] for c in fake.calls]
+ self.assertIn("ipm_stop", names)
+ self.assertEqual(names[-1], "ipm_select_mode")
+ self.assertEqual(fake.calls[-1][1], (False,))
+
+
+class LinearPathValidation(unittest.TestCase):
+ def test_missing_caps_raises(self):
+ """No max_gripper_speed -> we can't build the Cartesian profile.
+ Surface a clear ValueError before any drive interaction."""
+ fake = _FakeDriver()
+ backend = _build_backend(fake)
+ params_no_speed = KX2ArmBackend.CartesianMoveParams(
+ max_gripper_speed=None,
+ max_gripper_acceleration=200.0,
+ path="linear",
+ )
+ with self.assertRaises(ValueError):
+ asyncio.run(backend._run_linear_path(_target_pose(), params_no_speed))
+ self.assertEqual(len(fake.calls), 0)
+
+ params_no_accel = KX2ArmBackend.CartesianMoveParams(
+ max_gripper_speed=20.0,
+ max_gripper_acceleration=None,
+ path="linear",
+ )
+ with self.assertRaises(ValueError):
+ asyncio.run(backend._run_linear_path(_target_pose(), params_no_accel))
+
+
+class LinearPathDispatchDefault(unittest.TestCase):
+ def test_default_path_is_joint(self):
+ """``CartesianMoveParams()`` defaults to joint-space planning. Verify
+ the field is unset (no surprise switch to linear)."""
+ p = KX2ArmBackend.CartesianMoveParams()
+ self.assertEqual(p.path, "joint")
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/pylabrobot/paa/kx2/tests/arm_backend_read_axis_config_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_read_axis_config_tests.py
new file mode 100644
index 00000000000..5658b23b529
--- /dev/null
+++ b/pylabrobot/paa/kx2/tests/arm_backend_read_axis_config_tests.py
@@ -0,0 +1,243 @@
+"""Unit tests for ``KX2ArmBackend._read_axis_config``.
+
+Covers the per-axis Elmo register read and its validation branches:
+
+* UF1 / UF2 -> motor conversion factor (zero on either is invalid).
+* XM[1..2], VH[3], VL[3] -> bounded vs unlimited travel decision.
+* CA[45] -> encoder socket index in 1..4.
+* CA[40+ca45] -> encoder type code (1 / 2 / 24 are the only supported).
+
+The test populates a per-(cmd, idx) dict that a fake driver reads from.
+No CAN, no asyncio loop concerns beyond a one-shot ``asyncio.run``.
+"""
+import asyncio
+import unittest
+from typing import Any, Dict, Tuple
+
+from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend
+from pylabrobot.paa.kx2.config import Axis, AxisConfig
+from pylabrobot.paa.kx2.driver import CanError, JointMoveDirection
+
+
+def _baseline_register_map() -> Dict[Tuple[str, int], Any]:
+ """Plausible "all-good, bounded-travel" register snapshot for one axis.
+
+ Values picked so:
+ * UF1/UF2 give a non-zero conv factor (UF1=10000, UF2=1 -> 10000).
+ * XM/VH/VL satisfy the bounded-travel branch (xm1==xm2==0).
+ * CA[45]==1 (valid socket), CA[41]==24 (absolute encoder).
+ * SP[2] != 100000 so max_vel takes the SP[2]/denom branch.
+ """
+ reg: Dict[Tuple[str, int], Any] = {}
+ # _read_io_names: UI[5..10] (digital), UI[11..12] (analog), UI[13..16] (outputs)
+ for idx in range(5, 17):
+ reg[("UI", idx)] = 0 # all-unassigned -> "" entries
+ reg[("UI", 24)] = 12345 # serial, value irrelevant
+ # Conversion factor: UF1/UF2.
+ reg[("UF", 1)] = 10000.0
+ reg[("UF", 2)] = 1.0
+ # Bounded travel: xm1==xm2==0 hits the "(xm1==0 and xm2==0)" branch.
+ reg[("XM", 1)] = 0
+ reg[("XM", 2)] = 0
+ reg[("UF", 3)] = 180.0 # max_travel
+ reg[("UF", 4)] = -180.0 # min_travel
+ reg[("VH", 3)] = 1000
+ reg[("VL", 3)] = -1000
+ # Encoder socket + type: socket 1, type 24 (absolute).
+ reg[("CA", 45)] = 1
+ reg[("CA", 41)] = 24
+ reg[("CA", 46)] = 1 # equals ca45 -> num3 = 1.0 (skips FF[3] read)
+ # Velocity / accel.
+ reg[("SP", 2)] = 50000
+ reg[("VH", 2)] = 200000
+ reg[("SD", 0)] = 100000
+ reg[("FF", 3)] = 1.0 # only used if ca45 != ca46
+ return reg
+
+
+class _FakeDriver:
+ """Minimal stand-in for KX2Driver; only resolves query_int / query_float
+ against a (cmd, idx) -> value dict. Raises KeyError on unmapped reads so
+ test misconfig surfaces loudly."""
+
+ def __init__(self, reg: Dict[Tuple[str, int], Any]):
+ self.reg = reg
+ self.calls: list = []
+
+ async def query_int(self, node_id: int, cmd: str, idx: int) -> int:
+ self.calls.append(("int", node_id, cmd, idx))
+ return int(self.reg[(cmd, idx)])
+
+ async def query_float(self, node_id: int, cmd: str, idx: int) -> float:
+ self.calls.append(("float", node_id, cmd, idx))
+ return float(self.reg[(cmd, idx)])
+
+
+def _build_backend(reg: Dict[Tuple[str, int], Any]) -> KX2ArmBackend:
+ """Bypass __init__ — _read_axis_config only touches self.driver."""
+ backend = KX2ArmBackend.__new__(KX2ArmBackend)
+ backend.driver = _FakeDriver(reg) # type: ignore[assignment]
+ return backend
+
+
+class ReadAxisConfigHappyPathTests(unittest.TestCase):
+ def test_returns_axis_config_with_expected_fields(self):
+ """Full register snapshot -> AxisConfig with the values derived from
+ the snapshot."""
+ reg = _baseline_register_map()
+ backend = _build_backend(reg)
+ cfg = asyncio.run(backend._read_axis_config(Axis.SHOULDER))
+
+ self.assertIsInstance(cfg, AxisConfig)
+ self.assertEqual(cfg.motor_conversion_factor, 10000.0)
+ self.assertEqual(cfg.max_travel, 180.0)
+ self.assertEqual(cfg.min_travel, -180.0)
+ self.assertFalse(cfg.unlimited_travel)
+ self.assertTrue(cfg.absolute_encoder)
+ self.assertEqual(cfg.joint_move_direction, JointMoveDirection.Normal)
+ # max_vel = SP[2] / (conv * num3) = 50000 / (10000 * 1.0) = 5.0
+ self.assertAlmostEqual(cfg.max_vel, 5.0, places=9)
+ # max_accel = SD[0] / 1.01 / (conv * num3) = 100000 / 1.01 / 10000
+ self.assertAlmostEqual(cfg.max_accel, 100000 / 1.01 / 10000.0, places=9)
+ # Default I/O dicts: all-empty channel labels (codes were 0).
+ self.assertEqual(cfg.digital_inputs, {1: "", 2: "", 3: "", 4: "", 5: "", 6: ""})
+ self.assertEqual(cfg.analog_inputs, {1: "", 2: ""})
+ self.assertEqual(cfg.outputs, {1: "", 2: "", 3: "", 4: ""})
+
+ def test_unlimited_travel_branch_sets_shortest_way_for_motion_axis(self):
+ """xm1 > vl3 and xm2 < vh3 -> unlimited travel; for motion axes, the
+ joint_move_direction flips to ShortestWay."""
+ reg = _baseline_register_map()
+ # xm range strictly inside vl/vh range.
+ reg[("XM", 1)] = -500
+ reg[("XM", 2)] = 500
+ reg[("VH", 3)] = 1000
+ reg[("VL", 3)] = -1000
+ backend = _build_backend(reg)
+ cfg = asyncio.run(backend._read_axis_config(Axis.SHOULDER))
+ self.assertTrue(cfg.unlimited_travel)
+ self.assertEqual(cfg.joint_move_direction, JointMoveDirection.ShortestWay)
+
+ def test_unlimited_travel_keeps_normal_direction_on_non_motion_axis(self):
+ """Same unlimited-travel path on a non-motion axis (e.g. RAIL) keeps
+ the joint_move_direction at Normal."""
+ reg = _baseline_register_map()
+ reg[("XM", 1)] = -500
+ reg[("XM", 2)] = 500
+ backend = _build_backend(reg)
+ cfg = asyncio.run(backend._read_axis_config(Axis.RAIL))
+ self.assertTrue(cfg.unlimited_travel)
+ self.assertEqual(cfg.joint_move_direction, JointMoveDirection.Normal)
+
+ def test_sp2_is_100000_uses_vh2_path_for_max_vel(self):
+ """SP[2] == 100000 sentinel -> max_vel comes from VH[2]/1.01/denom."""
+ reg = _baseline_register_map()
+ reg[("SP", 2)] = 100000
+ reg[("VH", 2)] = 200000
+ backend = _build_backend(reg)
+ cfg = asyncio.run(backend._read_axis_config(Axis.SHOULDER))
+ # 200000 / 1.01 / (10000 * 1.0)
+ self.assertAlmostEqual(cfg.max_vel, 200000 / 1.01 / 10000.0, places=9)
+
+ def test_ca45_neq_ca46_pulls_ff3_into_denom(self):
+ """When the position encoder (CA[46]) differs from the commutation
+ encoder (CA[45]), num3 is read from FF[3] instead of defaulting to 1."""
+ reg = _baseline_register_map()
+ reg[("CA", 46)] = 2 # different from CA[45]=1
+ reg[("FF", 3)] = 4.0
+ backend = _build_backend(reg)
+ cfg = asyncio.run(backend._read_axis_config(Axis.SHOULDER))
+ # denom = 10000 * 4.0; max_vel = 50000 / 40000 = 1.25
+ self.assertAlmostEqual(cfg.max_vel, 1.25, places=9)
+
+
+class ReadAxisConfigValidationTests(unittest.TestCase):
+ """One test per validation branch: a future contributor flipping a guard
+ by mistake should fail at least one of these."""
+
+ def test_uf1_zero_raises(self):
+ reg = _baseline_register_map()
+ reg[("UF", 1)] = 0.0
+ backend = _build_backend(reg)
+ with self.assertRaises(CanError) as ctx:
+ asyncio.run(backend._read_axis_config(Axis.SHOULDER))
+ self.assertIn("Invalid motor conversion factor", str(ctx.exception))
+ self.assertIn(f"axis {int(Axis.SHOULDER)}", str(ctx.exception))
+ self.assertIn("UF[1]=0.0", str(ctx.exception))
+
+ def test_uf2_zero_raises(self):
+ reg = _baseline_register_map()
+ reg[("UF", 2)] = 0.0
+ backend = _build_backend(reg)
+ with self.assertRaises(CanError) as ctx:
+ asyncio.run(backend._read_axis_config(Axis.SHOULDER))
+ self.assertIn("Invalid motor conversion factor", str(ctx.exception))
+ self.assertIn("UF[2]=0.0", str(ctx.exception))
+
+ def test_invalid_travel_limits_raises(self):
+ """Neither bounded-travel nor unlimited-travel branch matches: e.g.
+ xm1 > vl3 but xm2 >= vh3 -> hits the else."""
+ reg = _baseline_register_map()
+ reg[("XM", 1)] = -500 # > VL[3] = -1000
+ reg[("XM", 2)] = 2000 # >= VH[3] = 1000 -> not unlimited, not bounded
+ backend = _build_backend(reg)
+ with self.assertRaises(CanError) as ctx:
+ asyncio.run(backend._read_axis_config(Axis.SHOULDER))
+ msg = str(ctx.exception)
+ self.assertIn("Invalid travel limits or modulo settings", msg)
+ self.assertIn("VH[3]=1000", msg)
+ self.assertIn("VL[3]=-1000", msg)
+ self.assertIn("XM[1]=-500", msg)
+ self.assertIn("XM[2]=2000", msg)
+
+ def test_ca45_zero_raises(self):
+ """CA[45] == 0 fails the (0 < ca45 <= 4) guard."""
+ reg = _baseline_register_map()
+ reg[("CA", 45)] = 0
+ backend = _build_backend(reg)
+ with self.assertRaises(CanError) as ctx:
+ asyncio.run(backend._read_axis_config(Axis.SHOULDER))
+ self.assertIn("Invalid encoder socket", str(ctx.exception))
+ self.assertIn("CA[45]=0", str(ctx.exception))
+
+ def test_ca45_too_large_raises(self):
+ """CA[45] > 4 also fails the guard (only sockets 1..4 wired)."""
+ reg = _baseline_register_map()
+ reg[("CA", 45)] = 5
+ backend = _build_backend(reg)
+ with self.assertRaises(CanError) as ctx:
+ asyncio.run(backend._read_axis_config(Axis.SHOULDER))
+ self.assertIn("Invalid encoder socket", str(ctx.exception))
+ self.assertIn("CA[45]=5", str(ctx.exception))
+
+ def test_unsupported_encoder_type_raises(self):
+ """enc_type not in {1, 2, 24} -> raises with the offending CA register."""
+ reg = _baseline_register_map()
+ reg[("CA", 45)] = 1
+ reg[("CA", 41)] = 7 # neither incremental (1/2) nor absolute (24)
+ backend = _build_backend(reg)
+ with self.assertRaises(CanError) as ctx:
+ asyncio.run(backend._read_axis_config(Axis.SHOULDER))
+ msg = str(ctx.exception)
+ self.assertIn("Unsupported encoder type", msg)
+ self.assertIn("CA[41]=7", msg)
+
+ def test_incremental_encoder_type_1_sets_absolute_false(self):
+ reg = _baseline_register_map()
+ reg[("CA", 45)] = 1
+ reg[("CA", 41)] = 1
+ backend = _build_backend(reg)
+ cfg = asyncio.run(backend._read_axis_config(Axis.SHOULDER))
+ self.assertFalse(cfg.absolute_encoder)
+
+ def test_incremental_encoder_type_2_sets_absolute_false(self):
+ reg = _baseline_register_map()
+ reg[("CA", 45)] = 1
+ reg[("CA", 41)] = 2
+ backend = _build_backend(reg)
+ cfg = asyncio.run(backend._read_axis_config(Axis.SHOULDER))
+ self.assertFalse(cfg.absolute_encoder)
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py b/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py
new file mode 100644
index 00000000000..cc6fc0e748e
--- /dev/null
+++ b/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py
@@ -0,0 +1,235 @@
+"""Lifecycle tests for the KX2 driver's EMCY callback registration / shutdown.
+
+Covers the post-PR-880 fixes for:
+- ``stop()`` clearing ``_emcy_callbacks`` (no leak across setup retries).
+- ``stop()`` setting ``_loop = None`` before ``network.disconnect()`` so racing
+ listener-thread ``_cb``s no-op at their guard.
+- ``setup()`` subscribing EMCY before sending the NMT start command.
+- ``KX2ArmBackend._on_emcy`` scheduling via the driver's captured loop, not the
+ deprecated ``asyncio.get_event_loop()``.
+"""
+import asyncio
+import struct
+import unittest
+from typing import List, Tuple
+from unittest import mock
+
+from pylabrobot.paa.kx2 import driver as driver_mod
+from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend
+from pylabrobot.paa.kx2.driver import (
+ EmcyFrame,
+ KX2Driver,
+ _EMCY_COB_BASE,
+ _NodeEmcyState,
+)
+
+
+def _frame(
+ err_code: int, elmo: int = 0, err_reg: int = 0, data1: int = 0, data2: int = 0
+) -> bytes:
+ return struct.pack(" bytes:
+ return struct.pack(" bytes:
+ return struct.pack(" clear bit 12 immediately
+ ("set_high",) -> raise bit 12 immediately
+ ("hang",) -> leave SW unchanged so _wait_setpoint_ack times out
+
+ The script is consumed in order on each ``_control_word_set`` call.
+ """
+
+ def __init__(self, steps: List[Tuple[str, ...]]):
+ self.steps = list(steps)
+ self.calls: List[int] = [] # values passed to _control_word_set
+
+ def consume(self) -> Tuple[str, ...]:
+ if not self.steps:
+ raise AssertionError("script exhausted but _control_word_set was called again")
+ return self.steps.pop(0)
+
+
+def _build_driver(nid: int, script: _SwScript) -> KX2Driver:
+ drv = KX2Driver()
+ loop = asyncio.get_event_loop()
+ drv._loop = loop
+ drv._statusword = {nid: 0}
+ drv._statusword_event = {nid: asyncio.Event()}
+
+ async def _fake_cw_set(node_id: int, value: int, sync: bool = True) -> None:
+ script.calls.append(value)
+ step = script.consume()
+ tag = step[0]
+ if tag == "set_low":
+ drv._statusword[node_id] = drv._statusword.get(node_id, 0) & ~SW_BIT_12
+ drv._statusword_event[node_id].set()
+ elif tag == "set_high":
+ drv._statusword[node_id] = drv._statusword.get(node_id, 0) | SW_BIT_12
+ drv._statusword_event[node_id].set()
+ elif tag == "hang":
+ # Leave SW unchanged. _wait_sw_bit will exhaust its 50 ms timeout
+ # waiting on the event + falling back to SDO; we also stub the SDO
+ # path below to return the cached SW (= still wrong) so it always
+ # times out.
+ pass
+ else:
+ raise AssertionError(f"unknown step {tag!r}")
+
+ async def _fake_sdo_upload(node_id: int, idx: int, sub: int) -> bytes:
+ return drv._statusword.get(node_id, 0).to_bytes(2, "little") + b"\x00\x00"
+
+ drv._control_word_set = _fake_cw_set # type: ignore[assignment]
+ drv._can_sdo_upload = _fake_sdo_upload # type: ignore[assignment]
+ return drv
+
+
+class TriggerNewSetpointHappyPathTests(unittest.TestCase):
+ def test_first_attempt_succeeds(self):
+ """SW bit 12 clears, then rises promptly -> returns, no raise.
+ `_control_word_set` is called exactly twice (cw_low then cw_high)."""
+ nid = 1
+ script = _SwScript([("set_low",), ("set_high",)])
+
+ async def _go():
+ drv = _build_driver(nid, script)
+ await drv._trigger_new_setpoint(nid, cw_low=47, cw_high=63)
+
+ asyncio.new_event_loop().run_until_complete(_go())
+ self.assertEqual(script.calls, [47, 63])
+ self.assertEqual(script.steps, []) # script fully consumed
+
+
+class TriggerNewSetpointRetryThenSuccessTests(unittest.TestCase):
+ def test_second_attempt_succeeds(self):
+ """First attempt: bit 12 clears but never rises (timeout). Second
+ attempt: clears + rises -> returns. CW is set 4 times total."""
+ nid = 1
+ # Attempt 1: cw_low clears bit 12, cw_high hangs (bit 12 doesn't rise)
+ # Attempt 2: cw_low clears, cw_high raises.
+ script = _SwScript([
+ ("set_low",), ("hang",),
+ ("set_low",), ("set_high",),
+ ])
+
+ async def _go():
+ drv = _build_driver(nid, script)
+ # Use a shorter wait timeout indirectly via the script: ("hang",) just
+ # means "leave SW unchanged"; the wait will spin its 50 ms timeout.
+ await drv._trigger_new_setpoint(nid, cw_low=47, cw_high=63)
+
+ asyncio.new_event_loop().run_until_complete(_go())
+ self.assertEqual(script.calls, [47, 63, 47, 63])
+ self.assertEqual(script.steps, [])
+
+
+class TriggerNewSetpointAllAttemptsFailTests(unittest.TestCase):
+ def test_raises_after_max_attempts(self):
+ """SW bit 12 never rises -> raises CanError tagged with the axis nid.
+ With max_attempts=2 the handshake runs twice (4 CW writes) then gives up."""
+ nid = 3
+ script = _SwScript([
+ ("set_low",), ("hang",),
+ ("set_low",), ("hang",),
+ ])
+
+ async def _go():
+ drv = _build_driver(nid, script)
+ await drv._trigger_new_setpoint(nid, cw_low=47, cw_high=63, max_attempts=2)
+
+ with self.assertRaises(CanError) as ctx:
+ asyncio.new_event_loop().run_until_complete(_go())
+ msg = str(ctx.exception)
+ self.assertIn(f"Axis {nid}", msg)
+ self.assertIn("did not accept new PPM setpoint", msg)
+ self.assertIn("after 2 attempts", msg)
+ self.assertEqual(script.calls, [47, 63, 47, 63])
+
+
+class TriggerNewSetpointBit12StuckHighTests(unittest.TestCase):
+ def test_recovers_when_bit12_does_not_clear_on_first_cw_low(self):
+ """Pre-existing bit 12 high. Attempt 1 cw_low fails to clear it -> the
+ inner ``cleared`` check is False, the attempt restarts (continues the
+ loop) without writing cw_high. Attempt 2's cw_low clears, cw_high raises.
+
+ State machine recovery proof: cw_high is NOT written on attempt 1
+ (script only sees [cw_low, cw_low, cw_high])."""
+ nid = 2
+ # Attempt 1: cw_low call -> "hang" (bit 12 stays high; cleared==False).
+ # Attempt 2: cw_low call -> set_low (clears), cw_high call -> set_high.
+ script = _SwScript([
+ ("hang",),
+ ("set_low",), ("set_high",),
+ ])
+
+ async def _go():
+ drv = _build_driver(nid, script)
+ drv._statusword[nid] = SW_BIT_12 # bit 12 high before any handshake
+ await drv._trigger_new_setpoint(nid, cw_low=47, cw_high=63)
+
+ asyncio.new_event_loop().run_until_complete(_go())
+ # Attempt 1 wrote cw_low only; attempt 2 wrote cw_low + cw_high.
+ self.assertEqual(script.calls, [47, 47, 63])
+ self.assertEqual(script.steps, [])
+
+
+class TriggerNewSetpointMaxAttemptsOneTests(unittest.TestCase):
+ def test_single_attempt_failure_raises_immediately(self):
+ """With ``max_attempts=1`` the handshake gets exactly one shot. If
+ SW bit 12 doesn't rise, raises straight away (no second attempt)."""
+ nid = 4
+ script = _SwScript([("set_low",), ("hang",)])
+
+ async def _go():
+ drv = _build_driver(nid, script)
+ await drv._trigger_new_setpoint(nid, cw_low=47, cw_high=63, max_attempts=1)
+
+ with self.assertRaises(CanError) as ctx:
+ asyncio.new_event_loop().run_until_complete(_go())
+ self.assertIn("after 1 attempts", str(ctx.exception))
+ self.assertEqual(script.calls, [47, 63])
+
+ def test_single_attempt_success_returns(self):
+ """``max_attempts=1`` happy path: still works on a clean handshake."""
+ nid = 4
+ script = _SwScript([("set_low",), ("set_high",)])
+
+ async def _go():
+ drv = _build_driver(nid, script)
+ await drv._trigger_new_setpoint(nid, cw_low=47, cw_high=63, max_attempts=1)
+
+ asyncio.new_event_loop().run_until_complete(_go())
+ self.assertEqual(script.calls, [47, 63])
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/pylabrobot/paa/kx2/tests/kinematics_tests.py b/pylabrobot/paa/kx2/tests/kinematics_tests.py
new file mode 100644
index 00000000000..81c81b3ca8c
--- /dev/null
+++ b/pylabrobot/paa/kx2/tests/kinematics_tests.py
@@ -0,0 +1,551 @@
+import math
+import unittest
+
+from pylabrobot.capabilities.arms.standard import CartesianPose
+from pylabrobot.paa.kx2 import kinematics
+from pylabrobot.paa.kx2.config import Axis, AxisConfig, GripperParams, KX2Config
+from pylabrobot.paa.kx2.driver import JointMoveDirection
+from pylabrobot.paa.kx2.kinematics import IKError
+from pylabrobot.resources import Coordinate, Rotation
+
+
+def _axis(
+ *, max_travel: float = 1e6, min_travel: float = -1e6, unlimited: bool = False,
+) -> AxisConfig:
+ return AxisConfig(
+ motor_conversion_factor=1.0,
+ max_travel=max_travel,
+ min_travel=min_travel,
+ unlimited_travel=unlimited,
+ absolute_encoder=True,
+ max_vel=100.0,
+ max_accel=100.0,
+ joint_move_direction=JointMoveDirection.Normal,
+ digital_inputs={},
+ analog_inputs={},
+ outputs={},
+ )
+
+
+def _config(
+ wrist_offset: float = 10.0,
+ elbow_offset: float = 20.0,
+ elbow_zero_offset: float = 5.0,
+) -> KX2Config:
+ return KX2Config(
+ wrist_offset=wrist_offset,
+ elbow_offset=elbow_offset,
+ elbow_zero_offset=elbow_zero_offset,
+ axes={
+ Axis.SHOULDER: _axis(unlimited=True),
+ Axis.Z: _axis(min_travel=0.0, max_travel=10000.0),
+ Axis.ELBOW: _axis(min_travel=-10000.0, max_travel=10000.0),
+ Axis.WRIST: _axis(unlimited=True),
+ },
+ base_to_gripper_clearance_z=0.0,
+ base_to_gripper_clearance_arm=0.0,
+ robot_on_rail=False,
+ servo_gripper=None,
+ )
+
+
+class FKIKRoundTrip(unittest.TestCase):
+ def test_roundtrip(self):
+ c = _config()
+ g = GripperParams(length=15.0, z_offset=3.0)
+ pose = CartesianPose(
+ location=Coordinate(x=100, y=200, z=50), rotation=Rotation(z=30)
+ )
+ joints = kinematics.ik(pose, c, g)
+ back = kinematics.fk(joints, c, g)
+ self.assertAlmostEqual(back.location.x, pose.location.x, places=9)
+ self.assertAlmostEqual(back.location.y, pose.location.y, places=9)
+ self.assertAlmostEqual(back.location.z, pose.location.z, places=9)
+ self.assertAlmostEqual(back.rotation.z, pose.rotation.z, places=9)
+
+ def test_roundtrip_at_origin_yaw_zero(self):
+ """Sanity: a pose at (0, R, Z, 0°) lands shoulder=0°."""
+ c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0)
+ g = GripperParams()
+ pose = CartesianPose(
+ location=Coordinate(x=0, y=300, z=10), rotation=Rotation(z=0)
+ )
+ joints = kinematics.ik(pose, c, g)
+ self.assertAlmostEqual(joints[Axis.SHOULDER], 0.0, places=9)
+ self.assertAlmostEqual(joints[Axis.ELBOW], 300.0, places=9)
+ self.assertAlmostEqual(joints[Axis.Z], 10.0, places=9)
+ self.assertAlmostEqual(joints[Axis.WRIST], 0.0, places=9)
+
+ def test_z_offset_sign_convention(self):
+ """Pin the documented convention: positive z_offset = clamp sits below
+ the wrist plate (in world frame, +Z up). FK with the same Z joint and
+ a larger z_offset must produce a *lower* gripper z; IK must require a
+ *higher* wrist Z to reach the same gripper z."""
+ c = _config()
+ joints = {Axis.SHOULDER: 0.0, Axis.Z: 100.0, Axis.ELBOW: 50.0, Axis.WRIST: 0.0}
+ g_low = GripperParams(z_offset=0.0)
+ g_high = GripperParams(z_offset=10.0)
+ self.assertGreater(
+ kinematics.fk(joints, c, g_low).location.z,
+ kinematics.fk(joints, c, g_high).location.z,
+ "FK: increasing z_offset should lower the gripper",
+ )
+ pose = CartesianPose(location=Coordinate(x=0, y=300, z=50), rotation=Rotation(z=0))
+ self.assertGreater(
+ kinematics.ik(pose, c, g_high)[Axis.Z],
+ kinematics.ik(pose, c, g_low)[Axis.Z],
+ "IK: increasing z_offset should raise the wrist target",
+ )
+
+ def test_ik_shoulder_branch_convention(self):
+ """Pin shoulder = -degrees(atan2(x, y)) (CW from +Y, standard for KX2).
+
+ The C# original computes this with four quadrant cases hand-rolling
+ atan2 (KX2RobotControl.cs:7195-7268); Python uses the standard
+ library `atan2`. The two are identical at every reachable pose modulo
+ the -Y boundary, where Python snaps -180 -> +180 (kinematics.py:113)
+ to match C#. This test pins one point per quadrant + every axis
+ crossing so a future drive-by simplification can't silently flip a
+ sign or drop the boundary snap.
+ """
+ c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0)
+ g = GripperParams()
+ cases = [
+ ("+Y axis", 0.0, 300.0, 0.0),
+ ("Q1 (+x, +y)", 100.0, 100.0, -45.0),
+ ("+X axis", 300.0, 0.0, -90.0),
+ ("Q4 (+x, -y)", 100.0, -100.0, -135.0),
+ ("-Y axis", 0.0, -300.0, 180.0), # boundary: snapped from -180
+ ("Q3 (-x, -y)", -100.0, -100.0, 135.0),
+ ("-X axis", -300.0, 0.0, 90.0),
+ ("Q2 (-x, +y)", -100.0, 100.0, 45.0),
+ ]
+ for label, x, y, expected_shoulder in cases:
+ pose = CartesianPose(
+ location=Coordinate(x=x, y=y, z=0), rotation=Rotation(z=0),
+ )
+ joints = kinematics.ik(pose, c, g)
+ self.assertAlmostEqual(
+ joints[Axis.SHOULDER], expected_shoulder, places=9,
+ msg=f"{label}: x={x}, y={y}",
+ )
+
+
+class FKIKAnchors(unittest.TestCase):
+ """Anchor points pinning FK and IK output values for representative
+ joints/poses. Values were snapshotted from the current implementation —
+ if you change the FK or IK formula and these tests fail, the new
+ output is provably different from the old one and needs explicit
+ hardware verification."""
+
+ def _cfg(self):
+ def ax(min_travel=-180.0, max_travel=180.0, unlimited_travel=False):
+ return AxisConfig(
+ motor_conversion_factor=1.0, max_travel=max_travel, min_travel=min_travel,
+ unlimited_travel=unlimited_travel, absolute_encoder=True,
+ max_vel=100.0, max_accel=100.0,
+ joint_move_direction=JointMoveDirection.Normal,
+ digital_inputs={}, analog_inputs={}, outputs={},
+ )
+ return KX2Config(
+ wrist_offset=10.0, elbow_offset=20.0, elbow_zero_offset=5.0,
+ axes={
+ Axis.SHOULDER: ax(),
+ Axis.Z: ax(min_travel=0.0, max_travel=750.0),
+ Axis.ELBOW: ax(min_travel=0.0, max_travel=300.0),
+ Axis.WRIST: ax(unlimited_travel=True),
+ },
+ base_to_gripper_clearance_z=0.0, base_to_gripper_clearance_arm=0.0,
+ robot_on_rail=False, servo_gripper=None,
+ )
+
+ def _gripper(self):
+ return GripperParams(length=15.0, z_offset=3.0, finger_side="barcode_reader")
+
+ def test_fk_zero_pose(self):
+ """All joints at 0 → gripper at (0, 20, -3, 0°) for the default
+ barcode-reader gripper (15 mm length, 3 mm z_offset)."""
+ p = kinematics.fk(
+ {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0},
+ self._cfg(), self._gripper(),
+ )
+ self.assertAlmostEqual(p.location.x, 0.0, places=6)
+ self.assertAlmostEqual(p.location.y, 20.0, places=6)
+ self.assertAlmostEqual(p.location.z, -3.0, places=6)
+ self.assertAlmostEqual(p.rotation.z, 0.0, places=6)
+
+ def test_fk_q1_mid(self):
+ p = kinematics.fk(
+ {Axis.SHOULDER: 30.0, Axis.Z: 100.0, Axis.ELBOW: 80.0, Axis.WRIST: 0.0},
+ self._cfg(), self._gripper(),
+ )
+ self.assertAlmostEqual(p.location.x, -50.0, places=6)
+ self.assertAlmostEqual(p.location.y, 86.6025, places=4)
+ self.assertAlmostEqual(p.location.z, 97.0, places=6)
+ self.assertAlmostEqual(p.rotation.z, 30.0, places=6)
+
+ def test_fk_q2_mid(self):
+ p = kinematics.fk(
+ {Axis.SHOULDER: -45.0, Axis.Z: 50.0, Axis.ELBOW: 120.0, Axis.WRIST: 30.0},
+ self._cfg(), self._gripper(),
+ )
+ self.assertAlmostEqual(p.location.x, 105.7193, places=4)
+ self.assertAlmostEqual(p.location.y, 95.1127, places=4)
+ self.assertAlmostEqual(p.location.z, 47.0, places=6)
+ self.assertAlmostEqual(p.rotation.z, -15.0, places=6)
+
+ def test_fk_max_extension_with_wrist(self):
+ p = kinematics.fk(
+ {Axis.SHOULDER: 90.0, Axis.Z: 200.0, Axis.ELBOW: 250.0, Axis.WRIST: -45.0},
+ self._cfg(), self._gripper(),
+ )
+ self.assertAlmostEqual(p.location.x, -274.3934, places=4)
+ self.assertAlmostEqual(p.location.y, -10.6066, places=4)
+ self.assertAlmostEqual(p.location.z, 197.0, places=6)
+ self.assertAlmostEqual(p.rotation.z, 45.0, places=6)
+
+ def test_fk_wrist_180_keeps_yaw_in_range(self):
+ """Wrist at 180° + shoulder at 0° gives yaw=180° (not -180° via the
+ boundary snap)."""
+ p = kinematics.fk(
+ {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 100.0, Axis.WRIST: 180.0},
+ self._cfg(), self._gripper(),
+ )
+ self.assertAlmostEqual(p.location.x, 0.0, places=6)
+ self.assertAlmostEqual(p.location.y, 150.0, places=6)
+ self.assertAlmostEqual(p.rotation.z, 180.0, places=6)
+
+ def test_ik_on_plus_y_axis(self):
+ """Target on +Y axis → shoulder = 0; elbow = y - sum_of_offsets."""
+ pose = CartesianPose(location=Coordinate(x=0, y=300, z=50), rotation=Rotation(z=0))
+ j = kinematics.ik(pose, self._cfg(), self._gripper())
+ self.assertAlmostEqual(j[Axis.SHOULDER], 0.0, places=6)
+ self.assertAlmostEqual(j[Axis.Z], 53.0, places=6)
+ self.assertAlmostEqual(j[Axis.ELBOW], 280.0, places=4)
+ self.assertAlmostEqual(j[Axis.WRIST], 0.0, places=6)
+
+ def test_ik_q1_target(self):
+ pose = CartesianPose(location=Coordinate(x=100, y=200, z=100), rotation=Rotation(z=30))
+ j = kinematics.ik(pose, self._cfg(), self._gripper())
+ self.assertAlmostEqual(j[Axis.SHOULDER], -23.474916, places=4)
+ self.assertAlmostEqual(j[Axis.Z], 103.0, places=6)
+ self.assertAlmostEqual(j[Axis.ELBOW], 197.209286, places=4)
+ self.assertAlmostEqual(j[Axis.WRIST], 53.474916, places=4)
+
+ def test_ik_q2_target(self):
+ pose = CartesianPose(location=Coordinate(x=-150, y=150, z=75), rotation=Rotation(z=-45))
+ j = kinematics.ik(pose, self._cfg(), self._gripper())
+ self.assertAlmostEqual(j[Axis.SHOULDER], 40.955309, places=4)
+ self.assertAlmostEqual(j[Axis.Z], 78.0, places=6)
+ self.assertAlmostEqual(j[Axis.ELBOW], 177.661703, places=4)
+ self.assertAlmostEqual(j[Axis.WRIST], -85.955309, places=4)
+
+ def test_ik_negative_quadrant(self):
+ """Target in negative-Y region — shoulder rotates past 90°."""
+ pose = CartesianPose(location=Coordinate(x=-50, y=-200, z=120), rotation=Rotation(z=90))
+ j = kinematics.ik(pose, self._cfg(), self._gripper())
+ self.assertAlmostEqual(j[Axis.SHOULDER], 161.995838, places=4)
+ self.assertAlmostEqual(j[Axis.Z], 123.0, places=6)
+ self.assertAlmostEqual(j[Axis.ELBOW], 175.297408, places=4)
+ self.assertAlmostEqual(j[Axis.WRIST], -71.995838, places=4)
+
+
+class IKErrors(unittest.TestCase):
+ def test_x_rotation_raises_ikerror(self):
+ c = _config()
+ g = GripperParams()
+ pose = CartesianPose(
+ location=Coordinate(x=0, y=100, z=0), rotation=Rotation(x=10, z=0)
+ )
+ with self.assertRaises(IKError):
+ kinematics.ik(pose, c, g)
+
+ def test_y_rotation_raises_ikerror(self):
+ c = _config()
+ g = GripperParams()
+ pose = CartesianPose(
+ location=Coordinate(x=0, y=100, z=0), rotation=Rotation(y=10, z=0)
+ )
+ with self.assertRaises(IKError):
+ kinematics.ik(pose, c, g)
+
+
+class SnapToCurrent(unittest.TestCase):
+ def test_shifts_wrist_toward_current(self):
+ """J4=20 with current=380 should snap to 380 (closer than 20)."""
+ j = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 20.0}
+ cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 380.0}
+ out = kinematics.snap_to_current(j, cur)
+ self.assertAlmostEqual(out[Axis.WRIST], 380.0)
+
+ def test_shifts_shoulder_toward_current(self):
+ """Shoulder is rotary too — also snaps."""
+ j = {Axis.SHOULDER: -170.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0}
+ cur = {Axis.SHOULDER: 200.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0}
+ out = kinematics.snap_to_current(j, cur)
+ self.assertAlmostEqual(out[Axis.SHOULDER], 190.0) # -170 + 360
+
+ def test_prismatic_axes_pass_through(self):
+ """Z and elbow are not 360°-modulo; they should be untouched."""
+ j = {Axis.SHOULDER: 0.0, Axis.Z: 100.0, Axis.ELBOW: 50.0, Axis.WRIST: 0.0}
+ cur = {Axis.SHOULDER: 0.0, Axis.Z: 999.0, Axis.ELBOW: 999.0, Axis.WRIST: 0.0}
+ out = kinematics.snap_to_current(j, cur)
+ self.assertEqual(out[Axis.Z], 100.0)
+ self.assertEqual(out[Axis.ELBOW], 50.0)
+
+ def test_pure_closest_no_re_enforce(self):
+ """The wrist drive wraps freely — snap pulls to whichever 360° wrap is closest."""
+ j = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 350.0}
+ cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0}
+ out = kinematics.snap_to_current(j, cur)
+ self.assertAlmostEqual(out[Axis.WRIST], -10.0)
+
+ def test_no_shift_when_already_close(self):
+ j = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 30.0}
+ cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 25.0}
+ out = kinematics.snap_to_current(j, cur)
+ self.assertAlmostEqual(out[Axis.WRIST], 30.0)
+
+
+class GripperFingerSide(unittest.TestCase):
+ def test_proximity_side_keeps_grip_center_flips_yaw(self):
+ """Same joints, opposite finger side -> identical grip center (the
+ location field is the geometric midpoint between the jaws, not a
+ finger tip), but reported yaw flips 180° because the front finger
+ swaps to the opposite physical sensor."""
+ c = _config()
+ g_bc = GripperParams(length=15.0, z_offset=3.0, finger_side="barcode_reader")
+ g_pr = GripperParams(length=15.0, z_offset=3.0, finger_side="proximity_sensor")
+ j = {Axis.SHOULDER: 30.0, Axis.Z: 50.0, Axis.ELBOW: 100.0, Axis.WRIST: 15.0}
+ p_bc = kinematics.fk(j, c, g_bc)
+ p_pr = kinematics.fk(j, c, g_pr)
+
+ self.assertAlmostEqual(p_bc.location.x, p_pr.location.x, places=9)
+ self.assertAlmostEqual(p_bc.location.y, p_pr.location.y, places=9)
+ self.assertAlmostEqual(p_bc.location.z, p_pr.location.z, places=9)
+ # Yaws are normalized to [-180, 180]; flipping finger_side rotates
+ # the *labelled* front by 180°, so the absolute difference is 180.
+ self.assertAlmostEqual(abs(p_bc.rotation.z - p_pr.rotation.z), 180.0, places=9)
+
+ def test_proximity_roundtrip(self):
+ c = _config()
+ g = GripperParams(length=15.0, z_offset=3.0, finger_side="proximity_sensor")
+ pose = CartesianPose(
+ location=Coordinate(x=100, y=200, z=50), rotation=Rotation(z=30)
+ )
+ joints = kinematics.ik(pose, c, g)
+ back = kinematics.fk(joints, c, g)
+ self.assertAlmostEqual(back.location.x, pose.location.x, places=9)
+ self.assertAlmostEqual(back.location.y, pose.location.y, places=9)
+ self.assertAlmostEqual(back.location.z, pose.location.z, places=9)
+ self.assertAlmostEqual(back.rotation.z, pose.rotation.z, places=9)
+
+ def test_ik_wrist_axis_lands_on_opposite_sides_of_grip_center(self):
+ """Same target ``(location, direction)``, opposite finger side ->
+ wrist axis lands on opposite sides of the grip center along the
+ front-finger axis, separated by ``2·t.length``. The gripper assembly
+ has to swing around the wrist motor so the chosen finger faces the
+ target direction; both solutions still place the grip center exactly
+ at the requested location (verified by FK round-trip)."""
+ pose = CartesianPose(
+ location=Coordinate(x=0, y=300, z=0), rotation=Rotation(z=0)
+ )
+ c = _config()
+ g_bc = GripperParams(length=15.0, z_offset=3.0, finger_side="barcode_reader")
+ g_pr = GripperParams(length=15.0, z_offset=3.0, finger_side="proximity_sensor")
+ j_bc = kinematics.ik(pose, c, g_bc)
+ j_pr = kinematics.ik(pose, c, g_pr)
+
+ # Both solutions land the grip center on the requested point.
+ back_bc = kinematics.fk(j_bc, c, g_bc)
+ back_pr = kinematics.fk(j_pr, c, g_pr)
+ for back in (back_bc, back_pr):
+ self.assertAlmostEqual(back.location.x, pose.location.x, places=9)
+ self.assertAlmostEqual(back.location.y, pose.location.y, places=9)
+ self.assertAlmostEqual(back.location.z, pose.location.z, places=9)
+ self.assertAlmostEqual(back.rotation.z, pose.rotation.z, places=9)
+
+ # Wrist axes (= grip center − t.length·) sit on opposite
+ # sides of the grip center → distance between them is 2·t.length.
+ def wrist_xy(j):
+ r = (
+ c.wrist_offset + c.elbow_offset + c.elbow_zero_offset + j[Axis.ELBOW]
+ )
+ sh = math.radians(j[Axis.SHOULDER])
+ return (-r * math.sin(sh), r * math.cos(sh))
+
+ wx_bc, wy_bc = wrist_xy(j_bc)
+ wx_pr, wy_pr = wrist_xy(j_pr)
+ sep = math.hypot(wx_bc - wx_pr, wy_bc - wy_pr)
+ self.assertAlmostEqual(sep, 2.0 * g_bc.length, places=6)
+
+
+class ShoulderSnapAt180(unittest.TestCase):
+ def test_negative_180_snaps_to_positive(self):
+ """A pose pointing exactly along -y has shoulder = ±180; we snap to +180."""
+ c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0)
+ g = GripperParams()
+ pose = CartesianPose(
+ location=Coordinate(x=0, y=-100, z=0), rotation=Rotation(z=180)
+ )
+ joints = kinematics.ik(pose, c, g)
+ self.assertAlmostEqual(joints[Axis.SHOULDER], 180.0, places=9)
+
+
+def _linear_pose(x: float, y: float, z: float, yaw: float = 0.0) -> CartesianPose:
+ return CartesianPose(
+ location=Coordinate(x=x, y=y, z=z), rotation=Rotation(z=yaw),
+ )
+
+
+class SampleLinearPath(unittest.TestCase):
+ """Sampler-level checks for `kinematics.sample_linear_path`. The straight
+ Cartesian line is the contract; we verify endpoints land exact, intermediate
+ samples stay on the line, dt drives sample density, and finite-difference
+ velocity respects the central-difference rule."""
+
+ def setUp(self) -> None:
+ self.cfg = _config()
+ self.gp = GripperParams(length=15.0, z_offset=3.0)
+ self.start = _linear_pose(50.0, 100.0, 30.0, yaw=10.0)
+ self.end = _linear_pose(80.0, 130.0, 60.0, yaw=40.0)
+
+ def test_endpoints_exact_via_fk_roundtrip(self):
+ """First sample's joints FK back to start_pose; last sample's to end_pose.
+ Tightest correctness check — the streamed start/end land where requested."""
+ samples = kinematics.sample_linear_path(
+ self.cfg, self.gp, self.start, self.end,
+ vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008,
+ )
+ self.assertGreater(len(samples), 2)
+ first_back = kinematics.fk(samples[0].joints, self.cfg, self.gp)
+ last_back = kinematics.fk(samples[-1].joints, self.cfg, self.gp)
+ for got, want in (
+ (first_back.location.x, self.start.location.x),
+ (first_back.location.y, self.start.location.y),
+ (first_back.location.z, self.start.location.z),
+ (last_back.location.x, self.end.location.x),
+ (last_back.location.y, self.end.location.y),
+ (last_back.location.z, self.end.location.z),
+ ):
+ self.assertAlmostEqual(got, want, places=6)
+
+ def test_samples_stay_on_straight_line(self):
+ """FK every sample. Cross-product of (sample - start) with (end - start)
+ is zero iff the sample is colinear; tolerance handles floating-point."""
+ samples = kinematics.sample_linear_path(
+ self.cfg, self.gp, self.start, self.end,
+ vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008,
+ )
+ sx, sy, sz = self.start.location.x, self.start.location.y, self.start.location.z
+ ex, ey, ez = self.end.location.x, self.end.location.y, self.end.location.z
+ dx, dy, dz = ex - sx, ey - sy, ez - sz
+ L = math.sqrt(dx * dx + dy * dy + dz * dz)
+ for sample in samples:
+ back = kinematics.fk(sample.joints, self.cfg, self.gp)
+ px, py, pz = back.location.x - sx, back.location.y - sy, back.location.z - sz
+ cx = py * dz - pz * dy
+ cy = pz * dx - px * dz
+ cz = px * dy - py * dx
+ cross_norm = math.sqrt(cx * cx + cy * cy + cz * cz)
+ self.assertLess(cross_norm / L, 1e-6,
+ msg=f"sample at t={sample.time_s:.4f}s off line by {cross_norm:.6f}")
+
+ def test_dt_halves_doubles_sample_count(self):
+ """Two-fold dt change should ~halve sample count. ±2 slop for the trapezoid
+ rounding where the trailing partial step varies."""
+ fast = kinematics.sample_linear_path(
+ self.cfg, self.gp, self.start, self.end,
+ vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.004,
+ )
+ slow = kinematics.sample_linear_path(
+ self.cfg, self.gp, self.start, self.end,
+ vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008,
+ )
+ self.assertAlmostEqual(len(fast) / len(slow), 2.0, delta=0.2)
+
+ def test_last_sample_velocity_zero(self):
+ """Drive integrates V over dt; non-zero final V would push past target.
+ Last sample's encoder velocity must be exactly zero on every axis."""
+ samples = kinematics.sample_linear_path(
+ self.cfg, self.gp, self.start, self.end,
+ vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008,
+ )
+ for ax, v in samples[-1].encoder_velocity.items():
+ self.assertEqual(v, 0, msg=f"axis {ax.name} final velocity {v} != 0")
+
+ def test_zero_length_path_returns_short(self):
+ """Same start and end pose: no motion. Sampler returns a degenerate
+ list (≤1 sample). Runtime guards on len(samples) < 2 and bails."""
+ samples = kinematics.sample_linear_path(
+ self.cfg, self.gp, self.start, self.start,
+ vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008,
+ )
+ self.assertLessEqual(len(samples), 1)
+
+ def test_invalid_caps_raise(self):
+ """Non-positive vel/accel/dt is a programmer error — fail loud."""
+ with self.assertRaises(ValueError):
+ kinematics.sample_linear_path(
+ self.cfg, self.gp, self.start, self.end,
+ vel_mm_per_s=0.0, accel_mm_per_s2=100.0, dt_s=0.008,
+ )
+ with self.assertRaises(ValueError):
+ kinematics.sample_linear_path(
+ self.cfg, self.gp, self.start, self.end,
+ vel_mm_per_s=20.0, accel_mm_per_s2=-1.0, dt_s=0.008,
+ )
+ with self.assertRaises(ValueError):
+ kinematics.sample_linear_path(
+ self.cfg, self.gp, self.start, self.end,
+ vel_mm_per_s=20.0, accel_mm_per_s2=100.0, dt_s=0.0,
+ )
+
+ def test_velocity_finite_difference_matches_position(self):
+ """Interior samples use central difference: v[i] ≈ (p[i+1] - p[i-1]) / 2dt.
+ Verify the relationship holds for shoulder over an interior sample."""
+ samples = kinematics.sample_linear_path(
+ self.cfg, self.gp, self.start, self.end,
+ vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008,
+ )
+ if len(samples) < 4:
+ self.skipTest("not enough samples for interior check")
+ i = len(samples) // 2
+ expected_v = (
+ samples[i + 1].encoder_position[Axis.SHOULDER]
+ - samples[i - 1].encoder_position[Axis.SHOULDER]
+ ) / (2.0 * 0.008)
+ self.assertAlmostEqual(
+ samples[i].encoder_velocity[Axis.SHOULDER], int(round(expected_v)), delta=1,
+ )
+
+ def test_yaw_lerp_takes_short_way(self):
+ """Yaw 179° -> -179° while ALSO translating is a 2° rotation, not 358°.
+ Sampler must interpolate yaw via the short arc, otherwise wrist angles
+ spin a full unwind alongside the linear translation."""
+ start = _linear_pose(50.0, 100.0, 30.0, yaw=179.0)
+ end = _linear_pose(80.0, 130.0, 60.0, yaw=-179.0)
+ samples = kinematics.sample_linear_path(
+ self.cfg, self.gp, start, end,
+ vel_mm_per_s=10.0, accel_mm_per_s2=100.0, dt_s=0.008,
+ )
+ self.assertGreaterEqual(len(samples), 3)
+ mid_back = kinematics.fk(samples[len(samples) // 2].joints, self.cfg, self.gp)
+ # Short way through ±180° -> midpoint near ±180°, not near 0°.
+ self.assertGreater(abs(mid_back.rotation.z), 170.0)
+
+ def test_pure_rotation_in_place_raises(self):
+ """No translation + non-zero rotation: caps are mm/s, repurposing them
+ as deg/s is a footgun. Surface clearly so the caller switches to
+ move_to_joint_position."""
+ start = _linear_pose(50.0, 100.0, 30.0, yaw=10.0)
+ end = _linear_pose(50.0, 100.0, 30.0, yaw=40.0)
+ with self.assertRaises(NotImplementedError):
+ kinematics.sample_linear_path(
+ self.cfg, self.gp, start, end,
+ vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008,
+ )
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/pylabrobot/paa/kx2/tests/planner_tests.py b/pylabrobot/paa/kx2/tests/planner_tests.py
new file mode 100644
index 00000000000..52a15bd1aa9
--- /dev/null
+++ b/pylabrobot/paa/kx2/tests/planner_tests.py
@@ -0,0 +1,740 @@
+"""Unit tests for the KX2 trajectory planner and elbow conversions in
+``pylabrobot.paa.kx2.kinematics``.
+
+Covers planner-level behaviour (``plan_joint_move``, sync passes, encoder
+unit conversion, gripper-speed cap, clearance) and the elbow position<->
+angle round-trip. FK/IK/snap_to_current already have coverage in
+``kinematics_tests.py`` -- this file deliberately stays disjoint.
+
+The planner is pure Python: no driver round-trip, no async, no CAN. We
+build :class:`KX2Config` directly with synthetic per-axis configs.
+"""
+
+import math
+import unittest
+from typing import Optional
+
+from pylabrobot.paa.kx2 import kinematics
+from pylabrobot.paa.kx2.config import Axis, AxisConfig, GripperParams, KX2Config
+from pylabrobot.paa.kx2.driver import JointMoveDirection, MotorsMovePlan
+
+
+def _axis(
+ motor_conversion_factor: float = 1.0,
+ max_travel: float = 180.0,
+ min_travel: float = -180.0,
+ unlimited_travel: bool = False,
+ max_vel: float = 100.0,
+ max_accel: float = 100.0,
+ joint_move_direction: JointMoveDirection = JointMoveDirection.Normal,
+) -> AxisConfig:
+ return AxisConfig(
+ motor_conversion_factor=motor_conversion_factor,
+ max_travel=max_travel,
+ min_travel=min_travel,
+ unlimited_travel=unlimited_travel,
+ absolute_encoder=True,
+ max_vel=max_vel,
+ max_accel=max_accel,
+ joint_move_direction=joint_move_direction,
+ digital_inputs={},
+ analog_inputs={},
+ outputs={},
+ )
+
+
+def _config(
+ shoulder: Optional[AxisConfig] = None,
+ z: Optional[AxisConfig] = None,
+ elbow: Optional[AxisConfig] = None,
+ wrist: Optional[AxisConfig] = None,
+ servo_gripper: Optional[AxisConfig] = None,
+ wrist_offset: float = 10.0,
+ elbow_offset: float = 20.0,
+ elbow_zero_offset: float = 5.0,
+ base_to_gripper_clearance_z: float = 0.0,
+ base_to_gripper_clearance_arm: float = 0.0,
+) -> KX2Config:
+ axes = {
+ Axis.SHOULDER: shoulder if shoulder is not None else _axis(),
+ Axis.Z: z if z is not None else _axis(min_travel=0.0, max_travel=400.0),
+ Axis.ELBOW: elbow if elbow is not None else _axis(min_travel=0.0, max_travel=300.0),
+ Axis.WRIST: wrist if wrist is not None else _axis(),
+ }
+ if servo_gripper is not None:
+ axes[Axis.SERVO_GRIPPER] = servo_gripper
+ return KX2Config(
+ wrist_offset=wrist_offset,
+ elbow_offset=elbow_offset,
+ elbow_zero_offset=elbow_zero_offset,
+ axes=axes,
+ base_to_gripper_clearance_z=base_to_gripper_clearance_z,
+ base_to_gripper_clearance_arm=base_to_gripper_clearance_arm,
+ robot_on_rail=False,
+ servo_gripper=None,
+ )
+
+
+def _move_for(plan: MotorsMovePlan, axis: Axis):
+ for m in plan.moves:
+ if m.node_id == int(axis):
+ return m
+ raise AssertionError(f"axis {axis.name} not in plan")
+
+
+# --- 1. happy paths ---------------------------------------------------------
+
+class PlanJointMoveHappyPath(unittest.TestCase):
+ def test_single_axis_returns_plan_with_one_axis_entry(self):
+ cfg = _config()
+ g = GripperParams()
+ cur = {Axis.SHOULDER: 0.0}
+ plan = kinematics.plan_joint_move(cur, {Axis.SHOULDER: 30.0}, cfg, g)
+ self.assertIsInstance(plan, MotorsMovePlan)
+ assert plan is not None # type narrowing for mypy
+ self.assertEqual(len(plan.moves), 1)
+
+ def test_single_axis_encoder_position_uses_conv_factor(self):
+ cfg = _config(shoulder=_axis(motor_conversion_factor=2.0))
+ g = GripperParams()
+ plan = kinematics.plan_joint_move({Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g)
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ m = _move_for(plan, Axis.SHOULDER)
+ self.assertEqual(m.position, 60)
+
+ def test_single_axis_velocity_floored_at_one(self):
+ """If conv * v < 1 (would round to 0), it's floored at 1."""
+ cfg = _config(shoulder=_axis(motor_conversion_factor=1e-6, max_vel=0.001))
+ g = GripperParams()
+ plan = kinematics.plan_joint_move({Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g)
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ m = _move_for(plan, Axis.SHOULDER)
+ self.assertGreaterEqual(m.velocity, 1)
+
+ def test_single_axis_velocity_uses_abs_conv(self):
+ cfg = _config(shoulder=_axis(motor_conversion_factor=-3.0))
+ g = GripperParams()
+ plan = kinematics.plan_joint_move({Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g)
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ m = _move_for(plan, Axis.SHOULDER)
+ self.assertGreater(m.velocity, 0)
+
+ def test_single_axis_move_time_positive(self):
+ cfg = _config()
+ g = GripperParams()
+ plan = kinematics.plan_joint_move({Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g)
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ self.assertGreater(plan.move_time, 0.0)
+
+ def test_multi_axis_all_axes_in_plan(self):
+ cfg = _config()
+ g = GripperParams()
+ cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0}
+ tgt = {Axis.SHOULDER: 30.0, Axis.Z: 50.0}
+ plan = kinematics.plan_joint_move(cur, tgt, cfg, g)
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ nodes = {m.node_id for m in plan.moves}
+ self.assertEqual(nodes, {int(Axis.SHOULDER), int(Axis.Z)})
+
+ def test_multi_axis_move_time_equals_slowest(self):
+ """move_time should equal the slowest axis's total time."""
+ cfg = _config(
+ shoulder=_axis(max_vel=10.0, max_accel=100.0),
+ z=_axis(min_travel=0.0, max_travel=400.0, max_vel=1000.0, max_accel=1000.0),
+ )
+ g = GripperParams()
+ cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0}
+ tgt = {Axis.SHOULDER: 30.0, Axis.Z: 50.0}
+ plan = kinematics.plan_joint_move(cur, tgt, cfg, g)
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ # shoulder is the slow lead axis; with v=10 deg/s and 30 deg, dominated time > 1s.
+ self.assertGreater(plan.move_time, 1.0)
+
+ def test_no_op_returns_none(self):
+ cfg = _config()
+ g = GripperParams()
+ cur = {Axis.SHOULDER: 5.0, Axis.Z: 10.0}
+ tgt = {Axis.SHOULDER: 5.0, Axis.Z: 10.0}
+ self.assertIsNone(kinematics.plan_joint_move(cur, tgt, cfg, g))
+
+ def test_no_op_within_threshold_returns_none(self):
+ """Targets within 0.01 of current count as no-op."""
+ cfg = _config()
+ g = GripperParams()
+ cur = {Axis.SHOULDER: 5.0}
+ tgt = {Axis.SHOULDER: 5.005}
+ self.assertIsNone(kinematics.plan_joint_move(cur, tgt, cfg, g))
+
+
+# --- 2. travel limit snap and raise ----------------------------------------
+
+class TravelLimitSnap(unittest.TestCase):
+ def test_target_above_max_within_tolerance_snapped(self):
+ cfg = _config(shoulder=_axis(min_travel=-180.0, max_travel=180.0))
+ g = GripperParams()
+ plan = kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 180.05}, cfg, g
+ )
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ m = _move_for(plan, Axis.SHOULDER)
+ # snapped to 180.0 -> position = 180 * 1.0 = 180
+ self.assertEqual(m.position, 180)
+
+ def test_target_above_max_out_of_tolerance_raises(self):
+ cfg = _config(shoulder=_axis(min_travel=-180.0, max_travel=180.0))
+ g = GripperParams()
+ with self.assertRaises(ValueError) as ctx:
+ kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 180.5}, cfg, g
+ )
+ self.assertIn("SHOULDER", str(ctx.exception))
+
+ def test_target_below_min_within_tolerance_snapped(self):
+ cfg = _config(shoulder=_axis(min_travel=-180.0, max_travel=180.0))
+ g = GripperParams()
+ plan = kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: -180.05}, cfg, g
+ )
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ m = _move_for(plan, Axis.SHOULDER)
+ self.assertEqual(m.position, -180)
+
+ def test_target_below_min_out_of_tolerance_raises(self):
+ cfg = _config(shoulder=_axis(min_travel=-180.0, max_travel=180.0))
+ g = GripperParams()
+ with self.assertRaises(ValueError) as ctx:
+ kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: -180.5}, cfg, g
+ )
+ self.assertIn("SHOULDER", str(ctx.exception))
+
+ def test_unlimited_travel_axis_accepts_out_of_range(self):
+ """An unlimited_travel axis (e.g. wrist) shouldn't raise on a 540° target."""
+ cfg = _config(
+ wrist=_axis(min_travel=-180.0, max_travel=180.0, unlimited_travel=True),
+ )
+ g = GripperParams()
+ plan = kinematics.plan_joint_move(
+ {Axis.WRIST: 0.0}, {Axis.WRIST: 540.0}, cfg, g
+ )
+ self.assertIsNotNone(plan)
+
+
+# --- 3. validation ----------------------------------------------------------
+
+class GripperCapValidation(unittest.TestCase):
+ def test_negative_speed_raises(self):
+ cfg = _config()
+ g = GripperParams()
+ with self.assertRaises(ValueError) as ctx:
+ kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g,
+ max_gripper_speed=-1.0,
+ )
+ self.assertIn("must be positive", str(ctx.exception))
+
+ def test_zero_speed_raises(self):
+ cfg = _config()
+ g = GripperParams()
+ with self.assertRaises(ValueError) as ctx:
+ kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g,
+ max_gripper_speed=0.0,
+ )
+ self.assertIn("must be positive", str(ctx.exception))
+
+ def test_negative_acceleration_raises(self):
+ cfg = _config()
+ g = GripperParams()
+ with self.assertRaises(ValueError) as ctx:
+ kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g,
+ max_gripper_acceleration=-2.0,
+ )
+ self.assertIn("must be positive", str(ctx.exception))
+
+ def test_zero_acceleration_raises(self):
+ cfg = _config()
+ g = GripperParams()
+ with self.assertRaises(ValueError) as ctx:
+ kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g,
+ max_gripper_acceleration=0.0,
+ )
+ self.assertIn("must be positive", str(ctx.exception))
+
+ def test_both_none_runs_at_firmware_max(self):
+ cfg = _config()
+ g = GripperParams(length=15.0, z_offset=3.0)
+ # Need a full arm joint state for fk (the cap helper invokes fk).
+ cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 100.0, Axis.WRIST: 0.0}
+ tgt = {Axis.SHOULDER: 30.0, Axis.Z: 0.0, Axis.ELBOW: 100.0, Axis.WRIST: 0.0}
+ plan_uncapped = kinematics.plan_joint_move(cur, tgt, cfg, g)
+ self.assertIsNotNone(plan_uncapped)
+ assert plan_uncapped is not None # type narrowing for mypy
+ # Uncapped move time should match a profile run with the cap set very
+ # high (effectively no cap). Both should produce identical move_time.
+ plan_high_cap = kinematics.plan_joint_move(
+ cur, tgt, cfg, g, max_gripper_speed=1e9, max_gripper_acceleration=1e9
+ )
+ self.assertIsNotNone(plan_high_cap)
+ assert plan_high_cap is not None # type narrowing for mypy
+ self.assertAlmostEqual(plan_uncapped.move_time, plan_high_cap.move_time, places=6)
+
+
+# --- 4. elbow conversions ---------------------------------------------------
+
+class ElbowConversion(unittest.TestCase):
+ def test_max_travel_lands_at_90_deg(self):
+ cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0)
+ self.assertAlmostEqual(
+ kinematics.convert_elbow_position_to_angle(cfg, 300.0), 90.0, places=9
+ )
+
+ def test_zero_pos_lands_at_asin_of_zero_offset(self):
+ cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0)
+ expected = math.asin(5.0 / 305.0) * (180.0 / math.pi)
+ self.assertAlmostEqual(
+ kinematics.convert_elbow_position_to_angle(cfg, 0.0), expected, places=9
+ )
+
+ def test_round_trip_below_90(self):
+ """pos -> angle -> pos must be identity for points where angle < 90."""
+ cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0)
+ for pos in (0.0, 50.0, 100.0, 150.0, 200.0, 250.0, 299.0):
+ angle = kinematics.convert_elbow_position_to_angle(cfg, pos)
+ back = kinematics.convert_elbow_angle_to_position(cfg, angle)
+ self.assertAlmostEqual(back, pos, places=6, msg=f"pos={pos}")
+
+ def test_round_trip_above_90(self):
+ """Inverse identity for the piecewise-reflected branch (angle > 90).
+
+ Forward maps pos > max via ``90 + asin((2*max - pos + zero) / (max + zero))``;
+ the inverse must include the matching ``+ zero`` term so that pos -> angle
+ -> pos is the identity for any pos in the second-half domain.
+ """
+ cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0)
+ for pos in (300.5, 320.0, 380.0, 450.0, 550.0, 600.0):
+ angle = kinematics.convert_elbow_position_to_angle(cfg, pos)
+ back = kinematics.convert_elbow_angle_to_position(cfg, angle)
+ self.assertAlmostEqual(back, pos, places=6, msg=f"pos={pos}")
+
+ def test_above_90_branch_returns_angle_above_90(self):
+ """At least confirm the piecewise branch is taken: pos > max_travel
+ => angle > 90°."""
+ cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0)
+ for pos in (300.5, 320.0, 380.0, 450.0, 550.0, 599.0):
+ angle = kinematics.convert_elbow_position_to_angle(cfg, pos)
+ self.assertGreater(angle, 90.0, msg=f"pos={pos}")
+
+ def test_angle_round_trip_at_90(self):
+ cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0)
+ pos = kinematics.convert_elbow_angle_to_position(cfg, 90.0)
+ self.assertAlmostEqual(pos, 300.0, places=9)
+
+ def test_open_clamp_at_max_travel_plus_epsilon_does_not_raise(self):
+ """Floating-point overshoot at the joint limit must not raise. The asin
+ argument is clamped to [-1, 1] before the call."""
+ cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0)
+ for eps in (1e-15, 1e-14, 1e-12, 1e-9):
+ kinematics.convert_elbow_position_to_angle(cfg, 300.0 + eps)
+
+
+# --- 5. direction-aware delta in plan_joint_move ----------------------------
+
+class DirectionAwareDelta(unittest.TestCase):
+ def _wrist_cfg(self, direction: JointMoveDirection) -> KX2Config:
+ return _config(
+ wrist=_axis(
+ min_travel=-180.0,
+ max_travel=180.0,
+ unlimited_travel=True,
+ joint_move_direction=direction,
+ max_vel=360.0,
+ max_accel=720.0,
+ )
+ )
+
+ def test_clockwise_takes_long_way_when_target_ahead(self):
+ """Wrist at 0, target 10. Clockwise => long way around (-350).
+
+ move_time should reflect the long-way distance, not 10.
+ """
+ cfg_cw = self._wrist_cfg(JointMoveDirection.Clockwise)
+ cfg_short = self._wrist_cfg(JointMoveDirection.ShortestWay)
+ g = GripperParams()
+ cur = {Axis.WRIST: 0.0}
+ tgt = {Axis.WRIST: 10.0}
+ plan_cw = kinematics.plan_joint_move(cur, tgt, cfg_cw, g)
+ plan_short = kinematics.plan_joint_move(cur, tgt, cfg_short, g)
+ self.assertIsNotNone(plan_cw)
+ self.assertIsNotNone(plan_short)
+ assert plan_cw is not None and plan_short is not None # type narrowing for mypy
+ # Long way (350°) takes much longer than short way (10°).
+ self.assertGreater(plan_cw.move_time, plan_short.move_time * 5)
+
+ def test_counterclockwise_takes_long_way_when_target_behind(self):
+ """Wrist at 0, target -10. Counterclockwise => long way (+350)."""
+ cfg_ccw = self._wrist_cfg(JointMoveDirection.Counterclockwise)
+ cfg_short = self._wrist_cfg(JointMoveDirection.ShortestWay)
+ g = GripperParams()
+ cur = {Axis.WRIST: 0.0}
+ tgt = {Axis.WRIST: -10.0}
+ plan_ccw = kinematics.plan_joint_move(cur, tgt, cfg_ccw, g)
+ plan_short = kinematics.plan_joint_move(cur, tgt, cfg_short, g)
+ self.assertIsNotNone(plan_ccw)
+ self.assertIsNotNone(plan_short)
+ assert plan_ccw is not None and plan_short is not None # type narrowing for mypy
+ self.assertGreater(plan_ccw.move_time, plan_short.move_time * 5)
+
+ def test_shortest_way_wraps_at_180(self):
+ """A target +190 is reachable via -170 under ShortestWay -- the
+ abs distance traveled should equal 170, not 190."""
+ cfg_short = self._wrist_cfg(JointMoveDirection.ShortestWay)
+ cfg_normal = self._wrist_cfg(JointMoveDirection.Normal)
+ g = GripperParams()
+ cur = {Axis.WRIST: 0.0}
+ plan_short = kinematics.plan_joint_move(cur, {Axis.WRIST: 190.0}, cfg_short, g)
+ # ShortestWay first wraps target into (-180, 180]: 190 -> -170.
+ plan_normal = kinematics.plan_joint_move(cur, {Axis.WRIST: -170.0}, cfg_normal, g)
+ self.assertIsNotNone(plan_short)
+ self.assertIsNotNone(plan_normal)
+ assert plan_short is not None and plan_normal is not None # type narrowing for mypy
+ self.assertAlmostEqual(plan_short.move_time, plan_normal.move_time, places=6)
+
+ def test_normal_direction_uses_literal_delta(self):
+ """Normal mode = no wrap. delta is target - current, no shortcut."""
+ cfg_normal = self._wrist_cfg(JointMoveDirection.Normal)
+ g = GripperParams()
+ plan_long = kinematics.plan_joint_move(
+ {Axis.WRIST: 0.0}, {Axis.WRIST: 170.0}, cfg_normal, g
+ )
+ plan_short = kinematics.plan_joint_move(
+ {Axis.WRIST: 0.0}, {Axis.WRIST: 10.0}, cfg_normal, g
+ )
+ self.assertIsNotNone(plan_long)
+ self.assertIsNotNone(plan_short)
+ assert plan_long is not None and plan_short is not None # type narrowing for mypy
+ # Move time roughly proportional to distance for the same v/a.
+ self.assertGreater(plan_long.move_time, plan_short.move_time)
+
+
+# --- 6. accel sync ----------------------------------------------------------
+
+class AccelSync(unittest.TestCase):
+ def test_slower_accel_axis_drives_accel_for_others(self):
+ """Two axes with very different max_accel should sync accel-time.
+
+ The axis with the longer accel ramp (lower a, higher v) becomes the
+ lead; the other axis's acceleration should be reduced so the two
+ ramp together.
+ """
+ # Same dist, same v, but very different a.
+ cfg = _config(
+ shoulder=_axis(max_vel=100.0, max_accel=10.0), # slow accel -> long ramp -> lead
+ z=_axis(min_travel=0.0, max_travel=400.0, max_vel=100.0, max_accel=1000.0),
+ )
+ g = GripperParams()
+ cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0}
+ tgt = {Axis.SHOULDER: 50.0, Axis.Z: 50.0}
+ plan = kinematics.plan_joint_move(cur, tgt, cfg, g)
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ z_move = _move_for(plan, Axis.Z)
+ # z's effective acceleration should be reduced from 1000 toward
+ # something far smaller after sync. Encoder accel = a * |conv|, conv=1.
+ self.assertLess(z_move.acceleration, 1000)
+
+
+# --- 7. time sync -----------------------------------------------------------
+
+class TimeSync(unittest.TestCase):
+ def test_shorter_time_axis_velocity_scaled_down(self):
+ """Two axes; one is much slower (long total time), the other should
+ have its v and a scaled by k = dist/denom < 1, never exceeding the
+ lead's time."""
+ cfg = _config(
+ shoulder=_axis(max_vel=10.0, max_accel=20.0), # slow
+ z=_axis(min_travel=0.0, max_travel=400.0, max_vel=100.0, max_accel=200.0),
+ )
+ g = GripperParams()
+ cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0}
+ tgt = {Axis.SHOULDER: 100.0, Axis.Z: 100.0}
+ plan = kinematics.plan_joint_move(cur, tgt, cfg, g)
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ z_move = _move_for(plan, Axis.Z)
+ # z's commanded velocity should be far less than its firmware max
+ # because it's been time-synced down to the slow shoulder.
+ self.assertLess(z_move.velocity, 100)
+
+
+# --- 8. encoder unit conversion at plan exit --------------------------------
+
+class EncoderUnitConversion(unittest.TestCase):
+ def test_conv_factor_scales_position(self):
+ cfg = _config(shoulder=_axis(motor_conversion_factor=1000.0))
+ g = GripperParams()
+ plan = kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g
+ )
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ m = _move_for(plan, Axis.SHOULDER)
+ # 30 deg * 1000 enc/deg = 30000
+ self.assertEqual(m.position, 30000)
+
+ def test_negative_conv_preserves_position_sign(self):
+ """Position uses the signed conv (so -1.0 flips sign), but velocity
+ and acceleration use abs(conv) (always positive)."""
+ cfg = _config(shoulder=_axis(motor_conversion_factor=-2.0))
+ g = GripperParams()
+ plan = kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g
+ )
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ m = _move_for(plan, Axis.SHOULDER)
+ self.assertEqual(m.position, -60)
+ self.assertGreater(m.velocity, 0)
+ self.assertGreater(m.acceleration, 0)
+
+ def test_skip_axis_emits_firmware_max_velocity_acceleration(self):
+ """Axes the planner skips (no-op) still write vel/accel to the drive's
+ profile registers — set them to firmware max so a stale register can't
+ leave a follow-up move pathologically slow."""
+ cfg = _config() # default _axis: max_vel=100, max_accel=100, conv=1.0
+ g = GripperParams()
+ cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0}
+ tgt = {Axis.SHOULDER: 30.0, Axis.Z: 0.0} # Z is no-op
+ plan = kinematics.plan_joint_move(cur, tgt, cfg, g)
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ z_move = _move_for(plan, Axis.Z)
+ self.assertEqual(z_move.velocity, 100) # max_vel * |conv|
+ self.assertEqual(z_move.acceleration, 100)
+
+
+# --- 9. gripper-speed cap path ---------------------------------------------
+
+class GripperSpeedCapPath(unittest.TestCase):
+ def test_small_cap_reduces_velocity(self):
+ """A tight gripper-speed cap should drop joint velocities below the
+ uncapped baseline."""
+ cfg = _config()
+ g = GripperParams(length=15.0, z_offset=3.0)
+ cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 100.0, Axis.WRIST: 0.0}
+ tgt = {Axis.SHOULDER: 30.0, Axis.Z: 50.0, Axis.ELBOW: 200.0, Axis.WRIST: 15.0}
+
+ plan_uncapped = kinematics.plan_joint_move(cur, tgt, cfg, g)
+ plan_capped = kinematics.plan_joint_move(
+ cur, tgt, cfg, g, max_gripper_speed=10.0
+ )
+ self.assertIsNotNone(plan_uncapped)
+ self.assertIsNotNone(plan_capped)
+ assert plan_uncapped is not None and plan_capped is not None # type narrowing
+ # Capped plan should take strictly longer than uncapped.
+ self.assertGreater(plan_capped.move_time, plan_uncapped.move_time)
+
+ def test_servo_gripper_axis_not_in_arm_axes_passes_through(self):
+ """The servo gripper axis (Axis.SERVO_GRIPPER) is not part of fk's
+ arm_axes set, so the gripper-speed cap helper shouldn't touch it."""
+ cfg = _config(
+ servo_gripper=_axis(min_travel=0.0, max_travel=30.0, max_vel=50.0, max_accel=100.0),
+ )
+ g = GripperParams()
+ cur = {Axis.SERVO_GRIPPER: 0.0}
+ tgt = {Axis.SERVO_GRIPPER: 20.0}
+ # Even with a tight gripper-speed cap, the servo gripper should still
+ # produce a plan -- the cap helper sees an empty fk_start and doesn't run.
+ plan = kinematics.plan_joint_move(
+ cur, tgt, cfg, g, max_gripper_speed=0.001
+ )
+ self.assertIsNotNone(plan)
+ assert plan is not None # type narrowing for mypy
+ self.assertEqual(len(plan.moves), 1)
+ self.assertEqual(plan.moves[0].node_id, int(Axis.SERVO_GRIPPER))
+
+
+# --- 10. clearance check ----------------------------------------------------
+
+class ClearanceCheck(unittest.TestCase):
+ def test_violation_raises(self):
+ """Z below clearance AND elbow angle below arm clearance -> raise."""
+ cfg = _config(
+ z=_axis(min_travel=0.0, max_travel=400.0),
+ elbow=_axis(min_travel=0.0, max_travel=300.0),
+ base_to_gripper_clearance_z=50.0,
+ base_to_gripper_clearance_arm=80.0, # 80 deg threshold (angle space)
+ )
+ g = GripperParams()
+ # target Z = 10 < (0 + 50). Elbow position 50 -> angle ~asin(55/305) ~ 10° < 80°.
+ cur = {Axis.Z: 100.0, Axis.ELBOW: 200.0}
+ tgt = {Axis.Z: 10.0, Axis.ELBOW: 50.0}
+ with self.assertRaises(ValueError) as ctx:
+ kinematics.plan_joint_move(cur, tgt, cfg, g)
+ self.assertIn("clearance", str(ctx.exception).lower())
+
+ def test_no_violation_when_only_z_low(self):
+ """Z low but elbow angle above arm clearance -> no raise."""
+ cfg = _config(
+ z=_axis(min_travel=0.0, max_travel=400.0),
+ elbow=_axis(min_travel=0.0, max_travel=300.0),
+ base_to_gripper_clearance_z=50.0,
+ base_to_gripper_clearance_arm=5.0, # very low threshold so any pose passes
+ )
+ g = GripperParams()
+ cur = {Axis.Z: 100.0, Axis.ELBOW: 200.0}
+ tgt = {Axis.Z: 10.0, Axis.ELBOW: 200.0}
+ plan = kinematics.plan_joint_move(cur, tgt, cfg, g)
+ self.assertIsNotNone(plan)
+
+ def test_no_violation_when_only_elbow_low(self):
+ """Elbow angle below threshold but Z high -> no raise."""
+ cfg = _config(
+ z=_axis(min_travel=0.0, max_travel=400.0),
+ elbow=_axis(min_travel=0.0, max_travel=300.0),
+ base_to_gripper_clearance_z=50.0,
+ base_to_gripper_clearance_arm=80.0,
+ )
+ g = GripperParams()
+ cur = {Axis.Z: 100.0, Axis.ELBOW: 200.0}
+ tgt = {Axis.Z: 200.0, Axis.ELBOW: 50.0}
+ plan = kinematics.plan_joint_move(cur, tgt, cfg, g)
+ self.assertIsNotNone(plan)
+
+
+# --- 11. exact-value pins for the sync algorithm ----------------------------
+# These tests pin the *exact* encoder velocity, acceleration, and move_time
+# the planner produces for representative cases. They're the contract for any
+# refactor of the per-axis profile + accel-sync + time-sync chain — directional
+# asserts (assertLess/Greater) are too loose to catch a value-shifting change.
+# Tolerance: ±1 on int encoder values (rounding noise from float ops); 4
+# decimals on move_time.
+
+class SyncAlgorithmExactValues(unittest.TestCase):
+ def _assert_enc_close(self, actual, expected):
+ self.assertLessEqual(
+ abs(actual - expected), 1, f"{actual} not within 1 of {expected}"
+ )
+
+ def test_single_axis_trapezoidal(self):
+ """dist=100, v_max=100, a_max=200; t_acc=0.5, d_acc=25, t_cruise=0.5."""
+ cfg = _config(shoulder=_axis(max_vel=100.0, max_accel=200.0))
+ plan = kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 100.0}, cfg, GripperParams(),
+ )
+ self.assertIsNotNone(plan)
+ assert plan is not None
+ m = _move_for(plan, Axis.SHOULDER)
+ self.assertEqual(m.position, 100)
+ self.assertEqual(m.velocity, 100)
+ self.assertEqual(m.acceleration, 200)
+ self.assertAlmostEqual(plan.move_time, 1.5, places=4)
+
+ def test_single_axis_triangular(self):
+ """dist=10, v_max=100, a_max=200; can't reach v_max -> triangular.
+ t_acc = sqrt(10/200) = 0.2236, v_actual = a*t_acc = 44.72 -> 45."""
+ cfg = _config(shoulder=_axis(max_vel=100.0, max_accel=200.0))
+ plan = kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 10.0}, cfg, GripperParams(),
+ )
+ self.assertIsNotNone(plan)
+ assert plan is not None
+ m = _move_for(plan, Axis.SHOULDER)
+ self.assertEqual(m.position, 10)
+ self._assert_enc_close(m.velocity, 45)
+ self.assertEqual(m.acceleration, 200)
+ self.assertAlmostEqual(plan.move_time, 0.4472136, places=4)
+
+ def test_accel_sync_two_axis_exact(self):
+ """Shoulder is the slow-accel lead (v=100, a=10); Z's accel scales down."""
+ cfg = _config(
+ shoulder=_axis(max_vel=100.0, max_accel=10.0),
+ z=_axis(min_travel=0.0, max_travel=400.0, max_vel=100.0, max_accel=1000.0),
+ )
+ plan = kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0, Axis.Z: 0.0},
+ {Axis.SHOULDER: 50.0, Axis.Z: 50.0},
+ cfg, GripperParams(),
+ )
+ self.assertIsNotNone(plan)
+ assert plan is not None
+ sh = _move_for(plan, Axis.SHOULDER)
+ z = _move_for(plan, Axis.Z)
+ self._assert_enc_close(sh.velocity, 22)
+ self.assertEqual(sh.acceleration, 10)
+ self._assert_enc_close(z.velocity, 15)
+ self._assert_enc_close(z.acceleration, 14)
+ self.assertAlmostEqual(plan.move_time, 4.472136, places=4)
+
+ def test_time_sync_two_axis_exact(self):
+ """Shoulder is the slow-vel lead (v=10, a=20); Z's v scales down to match
+ move_time = dist/v + v/a = 100/10 + 10/20 = 10.5s."""
+ cfg = _config(
+ shoulder=_axis(max_vel=10.0, max_accel=20.0),
+ z=_axis(min_travel=0.0, max_travel=400.0, max_vel=100.0, max_accel=200.0),
+ )
+ plan = kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0, Axis.Z: 0.0},
+ {Axis.SHOULDER: 100.0, Axis.Z: 100.0},
+ cfg, GripperParams(),
+ )
+ self.assertIsNotNone(plan)
+ assert plan is not None
+ sh = _move_for(plan, Axis.SHOULDER)
+ z = _move_for(plan, Axis.Z)
+ self.assertEqual(sh.velocity, 10)
+ self.assertEqual(sh.acceleration, 20)
+ self.assertEqual(z.velocity, 10)
+ self.assertEqual(z.acceleration, 20)
+ self.assertAlmostEqual(plan.move_time, 10.5, places=4)
+
+ def test_four_axis_real_conv_exact(self):
+ """Full 4-axis coordinated move with realistic KX2 conv factors. Pins
+ the entire pipeline (elbow asin + sync + encoder packing) end-to-end."""
+ cfg = _config(
+ shoulder=_axis(max_vel=145.0, max_accel=300.0, motor_conversion_factor=23301.694),
+ z=_axis(min_travel=0.0, max_travel=750.0, max_vel=750.0, max_accel=1000.0,
+ motor_conversion_factor=3997.838),
+ elbow=_axis(min_travel=0.0, max_travel=300.0, max_vel=80.0, max_accel=180.0,
+ motor_conversion_factor=18204.444),
+ wrist=_axis(min_travel=-180.0, max_travel=180.0, max_vel=500.0, max_accel=1000.0,
+ motor_conversion_factor=45.511, unlimited_travel=True),
+ )
+ plan = kinematics.plan_joint_move(
+ {Axis.SHOULDER: 0.0, Axis.Z: 50.0, Axis.ELBOW: 100.0, Axis.WRIST: 0.0},
+ {Axis.SHOULDER: 30.0, Axis.Z: 200.0, Axis.ELBOW: 150.0, Axis.WRIST: 90.0},
+ cfg, GripperParams(),
+ )
+ self.assertIsNotNone(plan)
+ assert plan is not None
+ sh = _move_for(plan, Axis.SHOULDER)
+ z = _move_for(plan, Axis.Z)
+ el = _move_for(plan, Axis.ELBOW)
+ wr = _move_for(plan, Axis.WRIST)
+ self._assert_enc_close(sh.position, 699051)
+ self._assert_enc_close(sh.velocity, 1646247)
+ self._assert_enc_close(sh.acceleration, 4704052)
+ self._assert_enc_close(z.position, 799568)
+ self._assert_enc_close(z.velocity, 1548356)
+ self._assert_enc_close(z.acceleration, 3997838)
+ self._assert_enc_close(el.position, 556033)
+ self._assert_enc_close(el.velocity, 403583)
+ self._assert_enc_close(el.acceleration, 1322501)
+ self._assert_enc_close(wr.position, 4096)
+ self._assert_enc_close(wr.velocity, 9444)
+ self._assert_enc_close(wr.acceleration, 27705)
+ self.assertAlmostEqual(plan.move_time, 0.774597, places=3)
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/pylabrobot/ufactory/xarm6/backend.py b/pylabrobot/ufactory/xarm6/backend.py
index 02a0f7f2633..92079e6a68c 100644
--- a/pylabrobot/ufactory/xarm6/backend.py
+++ b/pylabrobot/ufactory/xarm6/backend.py
@@ -123,17 +123,26 @@ async def _set_gripper_units(self, units: int) -> None:
# -- CanGrip ---------------------------------------------------------------
- async def open_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = None
- ) -> None:
- """Open the bio-gripper to the specified width (mm)."""
- await self._set_gripper_units(self._mm_to_gripper_units(gripper_width))
+ @property
+ def min_gripper_width(self) -> float:
+ return self.gripper_min_mm
+
+ @property
+ def max_gripper_width(self) -> float:
+ return self.gripper_max_mm
- async def close_gripper(
- self, gripper_width: float, backend_params: Optional[BackendParams] = None
+ async def move_gripper(
+ self,
+ width: float,
+ force_sensing: bool = False,
+ backend_params: Optional[BackendParams] = None,
) -> None:
- """Close the bio-gripper to the specified width (mm)."""
- await self._set_gripper_units(self._mm_to_gripper_units(gripper_width))
+ """Move the bio-gripper jaws to ``width`` mm.
+
+ The xArm bio-gripper is position-controlled; ``force_sensing`` has no
+ effect on the SDK call.
+ """
+ await self._set_gripper_units(self._mm_to_gripper_units(width))
async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool:
"""Return True if the gripper width is at or below ``closed_threshold_mm``."""
@@ -166,7 +175,7 @@ async def park(self, backend_params: Optional[BackendParams] = None) -> None:
num_retries=1,
)
- async def request_gripper_location(
+ async def request_gripper_pose(
self, backend_params: Optional[BackendParams] = None
) -> CartesianPose:
"""Get the current gripper location and rotation."""
@@ -209,7 +218,7 @@ async def pick_up_at_location(
) -> None:
"""Move to ``location`` and close the gripper to ``resource_width``."""
await self.move_to_location(location, rotation, backend_params=backend_params)
- await self.close_gripper(resource_width)
+ await self.move_gripper(width=resource_width, force_sensing=True)
async def drop_at_location(
self,
@@ -262,7 +271,7 @@ async def pick_up_at_joint_position(
) -> None:
"""Move to the joint target and close the gripper to ``resource_width``."""
await self.move_to_joint_position(position, backend_params=backend_params)
- await self.close_gripper(resource_width)
+ await self.move_gripper(width=resource_width, force_sensing=True)
async def drop_at_joint_position(
self,
@@ -277,12 +286,14 @@ async def drop_at_joint_position(
# -- CanFreedrive ----------------------------------------------------------
async def start_freedrive_mode(
- self, free_axes: List[int], backend_params: Optional[BackendParams] = None
+ self,
+ free_axes: Optional[List[int]] = None,
+ backend_params: Optional[BackendParams] = None,
) -> None:
"""Enter freedrive (manual teaching) mode.
- The xArm SDK only supports freeing all axes at once, so ``free_axes`` is
- accepted for interface compatibility but ignored.
+ The xArm SDK only supports freeing all axes at once, so ``free_axes`` has
+ no effect.
"""
await self._driver._call_sdk(self._driver._arm.set_mode, 2, op="set_mode")
await self._driver._call_sdk(self._driver._arm.set_state, 0, op="set_state")
diff --git a/pylabrobot/ufactory/xarm6/backend_tests.py b/pylabrobot/ufactory/xarm6/backend_tests.py
index 9977d277d5d..20db6cc0b57 100644
--- a/pylabrobot/ufactory/xarm6/backend_tests.py
+++ b/pylabrobot/ufactory/xarm6/backend_tests.py
@@ -35,31 +35,33 @@ def _sdk_calls_for(self, func) -> list:
# -- Gripper ---------------------------------------------------------------
- async def test_open_gripper_mm_to_units(self):
- await self.backend.open_gripper(gripper_width=85)
+ async def test_move_gripper_mm_to_units(self):
+ # Default range is 71..150 mm mapped to 0..850 units, so 85 mm → ~151 units.
+ await self.backend.move_gripper(width=85, force_sensing=False)
calls = self._sdk_calls_for(self.driver._arm.set_gripper_position)
self.assertEqual(len(calls), 1)
- self.assertEqual(calls[0].args[1], 850)
+ self.assertEqual(calls[0].args[1], 151)
self.assertEqual(calls[0].kwargs["wait"], True)
self.assertEqual(calls[0].kwargs["speed"], 0)
- async def test_open_gripper_half(self):
- await self.backend.open_gripper(gripper_width=42.5)
+ async def test_move_gripper_midpoint(self):
+ # Midpoint of the 71..150 range is 110.5 mm → 425 units.
+ await self.backend.move_gripper(width=110.5, force_sensing=False)
calls = self._sdk_calls_for(self.driver._arm.set_gripper_position)
self.assertEqual(calls[0].args[1], 425)
- async def test_close_gripper(self):
- await self.backend.close_gripper(gripper_width=0)
+ async def test_move_gripper_force_sensing(self):
+ await self.backend.move_gripper(width=71, force_sensing=True)
calls = self._sdk_calls_for(self.driver._arm.set_gripper_position)
self.assertEqual(calls[0].args[1], 0)
- async def test_open_gripper_clamped_high(self):
- await self.backend.open_gripper(gripper_width=200)
+ async def test_move_gripper_clamped_high(self):
+ await self.backend.move_gripper(width=200, force_sensing=False)
calls = self._sdk_calls_for(self.driver._arm.set_gripper_position)
self.assertEqual(calls[0].args[1], 850)
- async def test_open_gripper_clamped_low(self):
- await self.backend.open_gripper(gripper_width=-5)
+ async def test_move_gripper_clamped_low(self):
+ await self.backend.move_gripper(width=-5, force_sensing=False)
calls = self._sdk_calls_for(self.driver._arm.set_gripper_position)
self.assertEqual(calls[0].args[1], 0)
@@ -104,8 +106,8 @@ async def test_park_with_location(self):
self.assertEqual(set_pos_calls[0].kwargs["y"], 0)
self.assertEqual(set_pos_calls[0].kwargs["z"], 300)
- async def test_request_gripper_location(self):
- location = await self.backend.request_gripper_location()
+ async def test_request_gripper_pose(self):
+ location = await self.backend.request_gripper_pose()
self.assertEqual(location.location.x, 100)
self.assertEqual(location.location.y, 200)
self.assertEqual(location.location.z, 300)
@@ -150,7 +152,8 @@ async def test_pick_up_at_location_move_then_close(self):
grip_calls = self._sdk_calls_for(self.driver._arm.set_gripper_position)
self.assertEqual(len(grip_calls), 1)
- self.assertEqual(grip_calls[0].args[1], 800) # 80 mm → 800 units
+ # 80 mm in 71..150 mm range → (80-71)/(150-71) * 850 ≈ 97 units.
+ self.assertEqual(grip_calls[0].args[1], 97)
async def test_drop_at_location_move_then_open_max(self):
loc = Coordinate(x=300, y=100, z=50)
@@ -195,7 +198,7 @@ async def test_pick_up_at_joint_position(self):
self.assertEqual(len(self._sdk_calls_for(self.driver._arm.set_servo_angle)), 1)
grip_calls = self._sdk_calls_for(self.driver._arm.set_gripper_position)
self.assertEqual(len(grip_calls), 1)
- self.assertEqual(grip_calls[0].args[1], 800)
+ self.assertEqual(grip_calls[0].args[1], 97)
async def test_drop_at_joint_position(self):
await self.backend.drop_at_joint_position({1: 0, 2: 0}, resource_width=80)
@@ -222,8 +225,9 @@ async def test_stop_freedrive_mode(self):
# -- Custom configuration --------------------------------------------------
- async def test_custom_mm_per_gripper_unit(self):
- backend = XArm6ArmBackend(driver=self.driver, mm_per_gripper_unit=0.2)
- await backend.open_gripper(gripper_width=85)
+ async def test_custom_gripper_range(self):
+ backend = XArm6ArmBackend(driver=self.driver, gripper_min_mm=50.0, gripper_max_mm=100.0)
+ await backend.move_gripper(width=75.0, force_sensing=False)
calls = self._sdk_calls_for(self.driver._arm.set_gripper_position)
+ # Midpoint of 50..100 mm range → 0.5 * 850 = 425 units.
self.assertEqual(calls[0].args[1], 425)
diff --git a/pylabrobot/ufactory/xarm6/xarm6.py b/pylabrobot/ufactory/xarm6/xarm6.py
index ce0fa858967..7c16a4d0a65 100644
--- a/pylabrobot/ufactory/xarm6/xarm6.py
+++ b/pylabrobot/ufactory/xarm6/xarm6.py
@@ -1,4 +1,4 @@
-from pylabrobot.capabilities.arms.articulated_arm import ArticulatedArm
+from pylabrobot.capabilities.arms.articulated_arm import ArticulatedGripperArm
from pylabrobot.device import Device
from pylabrobot.resources import Resource
from pylabrobot.ufactory.xarm6.backend import XArm6ArmBackend
@@ -9,7 +9,7 @@ class XArm6(Device):
"""UFACTORY xArm 6 robotic arm with bio-gripper.
Composes an :class:`XArm6Driver`, a default :class:`XArm6ArmBackend`, and an
- :class:`ArticulatedArm` frontend. The arm capability is exposed as
+ :class:`ArticulatedGripperArm` frontend. The arm capability is exposed as
``self.arm``; joint-space and freedrive operations live on
``self.arm.backend``.
@@ -23,5 +23,5 @@ def __init__(self, driver: XArm6Driver) -> None:
self.driver: XArm6Driver = driver
backend = XArm6ArmBackend(driver=driver)
self.reference = Resource(name="XArm6", size_x=200, size_y=200, size_z=200)
- self.arm = ArticulatedArm(backend=backend, reference_resource=self.reference)
+ self.arm = ArticulatedGripperArm(backend=backend, reference_resource=self.reference)
self._capabilities = [self.arm]
diff --git a/pyproject.toml b/pyproject.toml
index c873d4096bd..87f7a8a4fbe 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -22,7 +22,8 @@ sila = ["zeroconf>=0.131.0", "grpcio"]
cytation-microscopy = ["numpy>=1.26", "opencv-python", "PyGObject"]
pico = ["PyLabRobot[sila]", "opencv-python", "numpy"]
xarm = ["xarm-python-sdk"]
-all = ["PyLabRobot[serial,usb,ftdi,hid,modbus,websockets,visualizer,opentrons,sila,pico,xarm]"]
+canopen = ["canopen"]
+all = ["PyLabRobot[serial,usb,ftdi,hid,modbus,websockets,visualizer,opentrons,sila,pico,xarm,canopen]"]
test = [
"pytest",
"pytest-timeout",