diff --git a/docs/api/pylabrobot.paa.rst b/docs/api/pylabrobot.paa.rst new file mode 100644 index 00000000000..cfd86b3e2d6 --- /dev/null +++ b/docs/api/pylabrobot.paa.rst @@ -0,0 +1,19 @@ +.. currentmodule:: pylabrobot.paa + +pylabrobot.paa package +====================== + +KX2 +--- + +.. currentmodule:: pylabrobot.paa.kx2 + +.. autosummary:: + :toctree: _autosummary + :nosignatures: + :recursive: + + KX2 + KX2BarcodeReader + KX2Config + Axis 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/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..59c84e8627f --- /dev/null +++ b/docs/user_guide/paa/index.md @@ -0,0 +1,11 @@ +# PAA + +```{toctree} +:maxdepth: 1 + +kx2/installation +kx2/hello-world +kx2/continuous-motion +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..0e7a775802b --- /dev/null +++ b/docs/user_guide/paa/kx2/barcode-reader.ipynb @@ -0,0 +1,181 @@ +{ + "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 — 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 → Privacy & Security → 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** — 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 — model, firmware, every per-symbology enable flag, prefixes/suffixes, and length limits — 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) — 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 — scan a Start → toggle → End sequence; no host required.\n", + "3. Direct register writes via the ESC protocol — 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 — the reader doesn't emit a symbology ID unless prefix/suffix bytes are configured to include one." + ] + }, + { + "cell_type": "markdown", + "id": "e7a9907a", + "source": "## Integrated with the arm\n\nWhen you construct the arm with a `barcode_port`, the `KX2` owns the reader and manages its lifecycle: `kx2.setup()` opens the port and runs the handshake, `kx2.scan_barcode(read_time=8)` returns a {class}`~pylabrobot.resources.barcode.Barcode` (or `None` on a no-read), and `kx2.stop()` tears it down.\n\n```python\nfrom pylabrobot.paa.kx2 import KX2\n\nkx2 = KX2(barcode_port=\"/dev/tty.PL2303G-USBtoUART11240\")\nawait kx2.setup()\nbarcode = await kx2.scan_barcode(read_time=8)\nawait kx2.stop()\n```\n\nThe standalone {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` below drives the reader on its own — useful when the arm is e-stopped, or when there's no arm at all.", + "metadata": {} + }, + { + "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 — 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 — 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 +} \ No newline at end of file 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..e83cd1deb65 --- /dev/null +++ b/docs/user_guide/paa/kx2/continuous-motion.ipynb @@ -0,0 +1,372 @@ +{ + "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", + "| `kx2.move_to_location(..., path='linear')` | one target pose | a single straight-line Cartesian move |\n", + "| `kx2.move_parametric(fn, duration_s)` | a callable `t → pose` | math-defined paths (figure-8, helix, Lissajous) |\n", + "| `kx2.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.kx2 - INFO - canopen: connected, nodes=[1, 2, 3, 4, 6]\n" + ] + } + ], + "source": [ + "import math\n", + "from pylabrobot.paa.kx2 import KX2\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": null, + "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.request_gripper_pose()\ntarget = Coordinate(x=start.location.x, y=start.location.y, z=start.location.z + 20)\nawait kx2.move_to_location(\n location=target,\n direction=start.rotation.z,\n max_gripper_speed=50.0, max_gripper_acceleration=200.0, path='linear',\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.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.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.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.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.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.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.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.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.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.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.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 +} \ No newline at end of file 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..dd938c3a80c --- /dev/null +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -0,0 +1,432 @@ +{ + "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", + "- Pick and place, Cartesian and joint-space movement, and 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.kx2 - INFO - canopen: connected, nodes=[1, 2, 3, 4, 6]\n", + "2026-05-03 19:57:01,871 - pylabrobot.paa.kx2.kx2 - WARNING - node 1: re-enabling (was disabled)\n", + "2026-05-03 19:57:02,082 - pylabrobot.paa.kx2.kx2 - 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.kx2 - WARNING - node 3: re-enabling (was disabled)\n", + "2026-05-03 19:57:02,835 - pylabrobot.paa.kx2.kx2 - 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 control\n", + "\n", + "All arm, gripper, and drive methods live directly on the `KX2` object — there's no separate arm or driver handle. Move the tool with `kx2.move_to_location(...)` / `kx2.move_to_joint_position(...)`, work the gripper with `kx2.open_gripper(...)` / `kx2.close_gripper(...)`, read state with `kx2.request_gripper_pose()` / `kx2.request_joint_position()`, and reach the low-level drive primitives (motor commands, diagnostics, binary interpreter) as methods on the same object. Gripper yaw is controlled via a single `direction` float (degrees; 0° = front)." + ] + }, + { + "cell_type": "markdown", + "id": "kx2-gripper-header", + "metadata": {}, + "source": "### Gripper control\n\nThe servo gripper is force-aware. `close_gripper` moves to the target width and then verifies the plate is gripped; pass `check_plate_gripped=False` 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.open_gripper(gripper_width=25)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-gripper-close", + "metadata": {}, + "outputs": [], + "source": "await kx2.close_gripper(gripper_width=0, check_plate_gripped=False)" + }, + { + "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.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 — 10..100, clamped) when closing on fragile labware:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-gripper-force", + "metadata": {}, + "outputs": [], + "source": "await kx2.close_gripper(gripper_width=0, max_force_percent=30)" + }, + { + "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. The move methods 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 — 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.motion_limits()" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-cartesian-header", + "metadata": {}, + "source": "### Cartesian movement\n\nMove the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only — the KX2 cannot roll or pitch). Pass `max_gripper_speed` / `max_gripper_acceleration` to cap 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.request_gripper_pose()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-cartesian-code", + "metadata": {}, + "outputs": [], + "source": "from pylabrobot.resources import Coordinate\n\nawait kx2.move_to_location(\n location=Coordinate(x=330.124, y=210.552, z=320.0441),\n direction=198,\n max_gripper_speed=200.0, max_gripper_acceleration=1000.0,\n)" + }, + { + "cell_type": "code", + "execution_count": null, + "id": "0a41fece", + "metadata": {}, + "outputs": [], + "source": "from pylabrobot.resources import Coordinate\n\nawait kx2.move_to_location(\n location=Coordinate(x=130.124, y=210.552, z=420.0441),\n direction=100,\n max_gripper_speed=200.0,\n max_gripper_acceleration=1000.0,\n path=\"joint\",\n)" + }, + { + "cell_type": "markdown", + "id": "kx2-joint-header", + "metadata": {}, + "source": [ + "### Joint movement\n", + "\n", + "Move in joint space using the {class}`~pylabrobot.paa.kx2.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": null, + "id": "kx2-joint-code", + "metadata": {}, + "outputs": [], + "source": "from pylabrobot.paa.kx2 import Axis\n\nposition = {\n Axis.SHOULDER: 0.0,\n Axis.Z: 150.0,\n Axis.ELBOW: 90.0,\n Axis.WRIST: 0.0,\n}\nawait kx2.move_to_joint_position(\n position,\n max_gripper_speed=200.0, max_gripper_acceleration=1000.0,\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", + " Axis.SHOULDER: 0.0,\n", + " Axis.Z: 150.0,\n", + " Axis.ELBOW: 90.0,\n", + " Axis.WRIST: 0.0,\n", + "}\n", + "place_joints = {\n", + " Axis.SHOULDER: 30.0,\n", + " Axis.Z: 150.0,\n", + " Axis.ELBOW: 90.0,\n", + " Axis.WRIST: 0.0,\n", + "}\n", + "\n", + "await kx2.pick_up_at_joint_position(pick_joints, resource_width=0)\n", + "await kx2.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.request_joint_position()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-query-cart", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.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 — 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)\nplace = Coordinate(x=450.0, y=-200.0, z=80.0)\nabove = Coordinate(x=0, y=0, z=60.0)\nyaw = 0.0\n\n# Approach/retract are the caller's responsibility. pick_up_at_location and\n# drop_at_location each take the resource (plate) width to close / open to.\nawait kx2.move_to_location(pick + above, direction=yaw)\nawait kx2.pick_up_at_location(location=pick, direction=yaw, resource_width=0)\nawait kx2.move_to_location(pick + above, direction=yaw)\n\nawait kx2.move_to_location(place + above, direction=yaw)\nawait kx2.drop_at_location(location=place, direction=yaw, resource_width=30)\nawait kx2.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 — 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.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.request_gripper_pose()\n", + "print(taught)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-free-off", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.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.halt()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-estop", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.get_estop_state()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-fault", + "metadata": {}, + "outputs": [], + "source": [ + "from pylabrobot.paa.kx2 import Axis\n", + "\n", + "await kx2.motor_get_fault(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 +} \ No newline at end of file diff --git a/docs/user_guide/paa/kx2/installation.md b/docs/user_guide/paa/kx2/installation.md new file mode 100644 index 00000000000..8e99c96cb79 --- /dev/null +++ b/docs/user_guide/paa/kx2/installation.md @@ -0,0 +1,51 @@ +# Installation + +The KX2 arm talks over CAN bus (CANopen / CiA-402); the optional onboard barcode reader is a separate USB-to-serial device. Install the pieces for whichever you use. + +## Dependencies + +The arm needs the `canopen` extra; the barcode reader needs `serial`: + +```bash +pip install "pylabrobot[canopen,serial]" +``` + +Drop `serial` if you don't have the barcode reader. + +## CAN bus (the arm) + +The arm connects to the host over its USB-B port, which carries the CAN bus — no separate CAN adapter needed. Just plug the USB-B cable into the host. PLR speaks CANopen (CiA-301 + CiA-402 drive profile) over it via `python-can` (pulled in by the `canopen` extra). + +### On Linux + +Bring the interface up: + +```bash +sudo ip link set can0 up type can bitrate 500000 +sudo ip link set can0 txqueuelen 1000 +``` + +## Barcode reader USB-to-serial driver (optional) + +The onboard barcode reader connects via a Prolific PL2303 USB-to-serial cable. Find the port it enumerates on with `python -m serial.tools.list_ports -v`. + +### On Linux + +Nothing to install — the in-tree `pl2303` kernel module claims the cable automatically and it shows up as `/dev/ttyUSB`. + +### On macOS + +macOS bundles a driver for older PL2303 variants but not the newer GC/HXN chip (USB ID `067b:23a3`). If the device doesn't appear at `/dev/tty.PL2303G-USBtoUART` after plugging it in, install Prolific's vendor driver: + +```bash +brew install --cask prolific-pl2303 +open -a PL2303Serial # registers the system extension +``` + +Then go to **System Settings → Privacy & Security** and click **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]`. + +### On Windows + +```{note} +**TODO.** The PL2303 driver setup on Windows hasn't been written up yet. In general it means installing Prolific's driver so the cable enumerates as a `COM` port, which you then pass as the reader's `port`. If you've done this on Windows, contributions to this section are welcome — see [CONTRIBUTING.md](/contributor_guide/contributing). +``` 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..e520fe6ec42 --- /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 — 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.kx2 - INFO - canopen: connected, nodes=[1, 2, 3, 4, 6]\n", + "2026-05-03 19:52:45,964 - pylabrobot.paa.kx2.kx2 - WARNING - node 1: re-enabling (was disabled)\n", + "2026-05-03 19:52:46,081 - pylabrobot.paa.kx2.kx2 - WARNING - node 3: re-enabling (was disabled)\n", + "2026-05-03 19:52:46,246 - pylabrobot.paa.kx2.kx2 - 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.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 → 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.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/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/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..bedec193cab --- /dev/null +++ b/pylabrobot/paa/kx2/__init__.py @@ -0,0 +1,23 @@ +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", + "KX2BarcodeReader", + "KX2Config", + "ServoGripperConfig", +] diff --git a/pylabrobot/paa/kx2/barcode_reader.py b/pylabrobot/paa/kx2/barcode_reader.py new file mode 100644 index 00000000000..11e768c754b --- /dev/null +++ b/pylabrobot/paa/kx2/barcode_reader.py @@ -0,0 +1,319 @@ +"""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 class alongside :class:`~pylabrobot.paa.kx2.kx2.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:`KX2BarcodeReader.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.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 KX2BarcodeReaderError(Exception): + """A KX2 barcode-reader comms failure (port error or read timeout).""" + + +class KX2BarcodeReader: + """PAA KX2 onboard barcode reader (Denso MDI-4050, Microscan-style serial). + + A single class spanning the RS-232 transport and the read-cycle logic. It's a + plain serial device with no dependency on the CAN bus that drives the arm, so + it works even with the arm e-stopped. The :class:`~pylabrobot.paa.kx2.kx2.KX2` + owns one internally when constructed with a ``barcode_port``; construct this + directly to drive the reader on its own. + + Factory defaults (per ``KX2RobotControl.cs:1712–1741``): 9600 8N1, no flow + control. All commands are ``ESC + + CR + LF``; all responses are + `` + CR``. + + 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.scan_barcode(read_time=8) + print(barcode.data) + await bcr.stop() + """ + + default_baudrate = 9600 + + # 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, 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})" + ) + 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; the read loop handles the real deadline + rtscts=False, + ) + + # --- lifecycle ----------------------------------------------------------- + + async def setup(self) -> None: + """Open the serial port, handshake, and arm for single-read scans. + + The handshake is a version query (mirrors ``KX2RobotControl.cs:15617``); an + empty reply means the reader isn't answering, so raise rather than let the + first scan time out cryptically. + """ + await self.io.setup() + version = await self.get_software_version() + if not version: + raise KX2BarcodeReaderError( + "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] connected, software version: %s", self.io.port, version) + await self.set_read_mode("single") + + 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) + + # --- serial protocol ----------------------------------------------------- + + async def _read_until_cr(self, timeout: float) -> str: + """Read from the port until we see a CR, returning the decoded line. + + Raises :class:`KX2BarcodeReaderError` 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 KX2BarcodeReaderError( + 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 + + # --- 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: + """Set the trigger mode: ``"single"`` (default), ``"multiple"``, or + ``"continuous"``. 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") + + # --- read cycle ---------------------------------------------------------- + + async def scan_barcode(self, read_time: Optional[float] = None) -> Optional[Barcode]: + """Fire a read cycle and return the decoded :class:`~pylabrobot.resources.barcode.Barcode`, + or ``None`` on a no-read. + + Args: + read_time: on-device read window in seconds. The reader's Y-command only + takes integer seconds 1..9 (YM=indefinite), so this is rounded and + pushed to the device so its window matches our wait bound. When + omitted, whatever window is currently configured stays, and we 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.set_read_time(int(round(read_time))) + await self.trigger(True) + timeout = (read_time + 1.0) if read_time is not None else self._DEFAULT_SCAN_WAIT + try: + data = await self._read_until_cr(timeout=timeout) + except KX2BarcodeReaderError: + # Serial-read timeout here means "nothing decoded within the window" — + # a no-read, not a comms failure. Return None. (Port errors surface as + # different exceptions from self.io.) + return None + if not data: + return None + return Barcode(data=data, symbology="ANY 1D", position_on_resource="front") diff --git a/pylabrobot/paa/kx2/config.py b/pylabrobot/paa/kx2/config.py new file mode 100644 index 00000000000..58425c82b47 --- /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.protocol 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 units when applying the move + methods' max_gripper_speed / max_gripper_acceleration caps.""" + 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:`KX2` + (``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/kinematics.py b/pylabrobot/paa/kx2/kinematics.py new file mode 100644 index 00000000000..6d65c856cba --- /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 `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.protocol 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 KX2 +# streams the result into the drive's interpolation buffer (PVT mode) when +# `move_to_location(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..11d4e7a3b47 --- /dev/null +++ b/pylabrobot/paa/kx2/kx2.py @@ -0,0 +1,3139 @@ +"""PAA KX2 robotic plate handler — single self-contained hardware class. + +One class, :class:`KX2`, owns the whole robot: the CANopen/DS402 CAN transport +and Elmo binary-interpreter primitives (formerly ``KX2Driver``), the +robot-level motion/homing/gripper logic and kinematics glue (formerly +``KX2ArmBackend``), and the top-level ``setup``/``stop`` lifecycle. It is not a +PLR ``Device``/``Driver``/capability backend — it is a plain class written +specifically for this hardware. + +Pure wire-protocol definitions live in :mod:`~pylabrobot.paa.kx2.protocol`; +frame/joint kinematics in :mod:`~pylabrobot.paa.kx2.kinematics`; drive-read +calibration in :mod:`~pylabrobot.paa.kx2.config`. The onboard barcode reader is +a separate serial device (:mod:`~pylabrobot.paa.kx2.barcode_reader`); when a +port is given, :class:`KX2` owns one and manages its lifecycle. +""" + +from __future__ import annotations + +import asyncio +import contextlib +import logging +import struct +import sys +import time +import warnings +from contextlib import asynccontextmanager +from enum import IntEnum +from typing import Callable, Dict, List, Literal, Optional, Tuple, Union + +from pylabrobot.capabilities.arms.standard import CartesianPose +from pylabrobot.paa.kx2 import kinematics +from pylabrobot.paa.kx2.barcode_reader import KX2BarcodeReader +from pylabrobot.paa.kx2.config import ( + Axis, + AxisConfig, + GripperFingerSide, + GripperParams, + KX2Config, + ServoGripperConfig, +) +from pylabrobot.paa.kx2.protocol import ( + COBType, + CanError, + EmcyFrame, + JointMoveDirection, + MotorMoveParam, + MotorsMovePlan, + PDOTransmissionType, + RPDO, + RPDOMappedObject, + TPDO, + TPDOMappedObject, + TPDOTrigger, + _BI_REQUEST_COB_BASE, + _BI_RESPONSE_COB_BASE, + _ElmoObjectDataType, + _EMCY_COB_BASE, + _GROUP_NODE_ID, + _InputLogic, + _NodeEmcyState, + _TPDO3_COB_BASE, + _decode_emcy, + _u32_le, +) +from pylabrobot.resources import Coordinate, Rotation + +try: + import canopen + + _HAS_CANOPEN = True +except ImportError as _e: + _HAS_CANOPEN = False + _CANOPEN_IMPORT_ERROR = _e + +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 KX2: + """PAA KX2 robotic plate handler. + + A single class spanning CAN transport + Elmo drive primitives and the + robot-level arm/gripper/homing/kinematics logic. Construct it, ``await + setup()``, drive it, ``await stop()``. + """ + + def __init__( + self, + has_rail: bool = False, + has_servo_gripper: bool = True, + interface: Optional[str] = None, + channel: Optional[str] = None, + bitrate: int = 500000, + gripper_length: float = 0.0, + gripper_z_offset: float = 0.0, + gripper_finger_side: GripperFingerSide = "barcode_reader", + barcode_port: Optional[str] = None, + barcode_baudrate: int = KX2BarcodeReader.default_baudrate, + ) -> None: + """ + Args: + 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). + interface: python-can interface name for the CANopen bus (default + ``"pcan"``). + channel: python-can channel; ``None`` lets the interface pick its default. + bitrate: CAN bitrate in bit/s (default 500 kbit/s). + 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. + 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°. + barcode_port: optional serial port of the onboard barcode reader. When + set, :meth:`setup` brings it up and :meth:`scan_barcode` is usable. + barcode_baudrate: barcode-reader serial baud rate. + """ + # The non-default topologies (rail-mounted KX2, gripper-less KX2) + # have shim code paths in this class, but neither + # has been exercised against real hardware. KX2._arm_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 KX2._arm_setup and " + "servo_gripper_initialize." + ) + if interface is None: + interface = "socketcan" if sys.platform.startswith("linux") else "pcan" + if channel is None and interface == "socketcan": + channel = "can0" + 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, keyed by CANopen node ID; + # axis-level names live in the `Axis` enum (config.py). + 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] = {} + + # --- tooling + arm-level state --- + # 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 + + # --- optional onboard barcode reader (independent serial device) --- + # The 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` class + # remains available for users who prefer to manage it as a sibling. + self._bcr: Optional[KX2BarcodeReader] = None + if barcode_port is not None: + self._bcr = KX2BarcodeReader(port=barcode_port, baudrate=barcode_baudrate) + + self._setup_finished = False + + @property + def setup_finished(self) -> bool: + return self._setup_finished + + # --- lifecycle ----------------------------------------------------------- + + async def setup(self) -> None: + """Bring the whole KX2 up: barcode reader (if any) -> CAN bus -> arm. + + Idempotent: re-running setup on a live KX2 stays 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 up first (open port + version handshake) so a + # bad reader aborts before we touch the CAN bus. It rides on a separate + # serial port with no dependency on the drives. + if self._bcr is not None: + await self._bcr.setup() + try: + await self._bus_setup() + await self._arm_setup() + except BaseException: + if self._bcr is not None: + try: + await self._bcr.stop() + except Exception: + logger.exception("KX2.setup cleanup: barcode reader stop failed; ignoring") + raise + self._setup_finished = True + + async def stop(self) -> None: + """Tear down the CAN bus and the barcode reader (if any).""" + try: + await self._bus_stop() + finally: + if self._bcr is not None: + await self._bcr.stop() + self._setup_finished = False + + async def _arm_setup(self) -> None: + """Robot bring-up after the CAN bus is live: read per-drive config, enable + the motion axes, and home + close the servo gripper. + + If anything below raises, tear the CAN bus 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.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.motors_ensure_enabled([int(a) for a in MOTION_AXES]) + await self.servo_gripper_initialize() + except BaseException: + try: + await self._bus_stop() + except Exception: + logger.exception("KX2 setup cleanup: _bus_stop() failed; ignoring") + raise + + async def scan_barcode(self, read_time: Optional[float] = None): + """Fire the onboard barcode reader and return the decoded ``Barcode`` (or + ``None`` on a no-read). Raises if the KX2 was constructed without a + ``barcode_port``. + """ + if self._bcr is None: + raise RuntimeError( + "KX2 was constructed without barcode_port; no barcode reader configured." + ) + return await self._bcr.scan_barcode(read_time) + + # --- reentrant motion guard ---------------------------------------------- + + @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 + + # --- EMCY auto-disable --------------------------------------------------- + + 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.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.loop.create_task(_disable_all()) + + # --- drive-read properties ----------------------------------------------- + + @property + def loop(self) -> asyncio.AbstractEventLoop: + """Event loop captured in setup(). Raises if accessed before setup().""" + if self._loop is None: + raise RuntimeError("KX2 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 + + async def _bus_setup(self) -> 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._bus_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 _bus_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 = {} + + + # ====================================================================== + # CAN transport + Elmo drive primitives + # ====================================================================== + + # --- 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_read_position_raw(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_digital_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) + + + # ====================================================================== + # Robot-level logic: homing, gripper, kinematics, motion + # ====================================================================== + + # -- 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.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.query_int(Axis.SERVO_GRIPPER, "UI", 3)) + + async def _gripper_set_homed_status(self, status: HomeStatus) -> None: + await self.write(Axis.SERVO_GRIPPER, "UI", 3, int(status)) + + async def _gripper_reset_encoder_position(self, position: float) -> None: + sg = Axis.SERVO_GRIPPER + await self.write(sg, "HM", 1, 0) + await self.write(sg, "HM", 3, 0) + await self.write(sg, "HM", 4, 0) + await self.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.write(sg, "HM", 2, int(round(position))) + await self.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.write(sg, "ER", 3, max_pe * 10) + await self.write(sg, "AC", 0, srch_acc) + await self.write(sg, "DC", 0, srch_acc) + for i in [3, 4, 5, 2]: + await self.write(sg, "HM", i, 0) + await self.write(sg, "JV", 0, srch_vel) + + try: + params: List[Union[int, float]] = [int(hs_pe), int(timeout * 1000)] + last_line = await self.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._motor_read_position_raw(sg) + await self.write(sg, "PA", 0, curr_pos) + await self.write(sg, "SP", 0, srch_vel) + await self.write(sg, "AC", 0, srch_acc) + await self.write(sg, "DC", 0, srch_acc) + finally: + await asyncio.sleep(0.3) + await self.execute(sg, "BG", 0) + await asyncio.sleep(0.3) + await self.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.write(sg, "HM", 1, 0) + + one_revolution = await self.query_int(sg, "CA", 18) + if not positive_direction: + one_revolution *= -1 + + await self.write(sg, "PR", 1, one_revolution) + await self.write(sg, "SP", 0, srch_vel) + await self.write(sg, "AC", 0, srch_acc) + await self.write(sg, "DC", 0, srch_acc) + + await self.write(sg, "HM", 3, 3) # index only + await self.write(sg, "HM", 4, 0) + await self.write(sg, "HM", 5, 0) + await self.write(sg, "HM", 2, 0) + await self.write(sg, "HM", 1, 1) # arm + + await self.execute(sg, "BG", 0) + await self.wait_for_moves_done([sg], timeout) + + left = await self.query_int(sg, "HM", 1) + if left != 0: + raise RuntimeError("Homing Failure: Failed to finish index pulse search.") + + captured_position = await self.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.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.motor_get_fault(sg) + if fault is not None: + raise RuntimeError(fault) from e + raise + + await self.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.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.write(sg, "PL", 1, sgc.peak_current) + await self.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.write(sg, "PL", 1, sgc.peak_current) + await self.write(sg, "CL", 1, cont_current) + await self.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.query_float(sg, "CL", 1) + iq = await self.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.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.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.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.write(int(axis_key), "UF", 5, float(dist)) + + # LastMaintenancePerformed -> Z axis, UF index 6 + await self.write(z, "UF", 6, float(last_maintenance_performed)) + + # MaintenanceRequired -> Z axis, UI index 23 + await self.write(z, "UI", 23, 1 if maintenance_required else 0) + + # LastMaintenancePerformedDate -> Z axis, UI index 21 + await self.write(z, "UI", 21, int(last_maintenance_performed_date)) + + # Rail (if present) + if self._cfg.robot_on_rail: + rail = Axis.RAIL + await self.write(rail, "UF", 6, float(last_maintenance_performed_rail)) + await self.write(rail, "UI", 23, 1 if maintenance_required_rail else 0) + await self.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.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.query_float(sh, "UF", 8), + elbow_offset=await self.query_float(sh, "UF", 9), + elbow_zero_offset=await self.query_float(sh, "UF", 10), + axes=axes, + base_to_gripper_clearance_z=await self.query_float(sh, "UF", 6), + base_to_gripper_clearance_arm=await self.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.query_int(nid, "UI", 24) # serial — read for parity, unused + + uf1 = await self.query_float(nid, "UF", 1) + uf2 = await self.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.query_int(nid, "XM", 1) + xm2 = await self.query_int(nid, "XM", 2) + max_travel = await self.query_float(nid, "UF", 3) + min_travel = await self.query_float(nid, "UF", 4) + vh3 = await self.query_int(nid, "VH", 3) + vl3 = await self.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.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.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.query_int(nid, "CA", 46) + num3 = 1.0 if ca45 == ca46 else await self.query_float(nid, "FF", 3) + denom = motor_conversion_factor * num3 + + sp2 = await self.query_int(nid, "SP", 2) + if sp2 == 100000: + max_vel = await self.query_int(nid, "VH", 2) / 1.01 / denom + else: + max_vel = sp2 / denom + max_accel = await self.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.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.query_float(sg, "UF", 6)), + home_search_vel=int(await self.query_float(sg, "UF", 7)), + home_search_accel=int(await self.query_float(sg, "UF", 8)), + home_default_position_error=int(await self.query_float(sg, "UF", 9)), + home_hard_stop_position_error=int(await self.query_float(sg, "UF", 10)), + home_hard_stop_offset=int(await self.query_float(sg, "UF", 11)), + home_index_offset=int(await self.query_float(sg, "UF", 12)), + home_offset_vel=int(await self.query_float(sg, "UF", 13)), + home_offset_accel=int(await self.query_float(sg, "UF", 14)), + home_timeout_msec=int(await self.query_float(sg, "UF", 15)), + continuous_current=await self.query_float(sg, "UF", 16), + peak_current=await self.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._motor_read_position_raw( + 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._read_digital_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." + ) + # 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.motors_ensure_enabled([int(Axis.Z)]) + await self._motors_move_joint_locked( + {Axis.Z: z_start}, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + if await self.read_proximity_sensor(): + return await self.motor_get_current_position(Axis.Z) + await self.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}, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + ) + 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.motor_stop(Axis.Z) + except Exception as e: + logger.warning("find_z: motor_stop failed: %s", e) + try: + await self.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.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``. + """ + async with self._motion_guard(): + await self.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, + 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, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + ) + 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.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], + *, + max_gripper_speed: Optional[float] = None, + max_gripper_acceleration: Optional[float] = None, + ) -> None: + async with self._motion_guard(): + await self._motors_move_joint_locked( + cmd_pos, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + + async def _motors_move_joint_locked( + self, + cmd_pos: Dict[Axis, float], + *, + max_gripper_speed: Optional[float] = None, + max_gripper_acceleration: Optional[float] = 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) + 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=max_gripper_speed, + max_gripper_acceleration=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.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.motor_set_move_direction(nid, move.direction) + # 0x607A = Target Position (24698 decimal) + await self.can_sdo_download_elmo_object( + nid, 24698, 0, int(move.position), _ElmoObjectDataType.INTEGER32, + ) + # 0x6081 = Profile Velocity (24705 decimal) + await self.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.can_sdo_download_elmo_object( + nid, 24707, 0, acc, _ElmoObjectDataType.UNSIGNED32 + ) + # 0x6084 = Profile Deceleration (24708 decimal) + await self.can_sdo_download_elmo_object( + nid, 24708, 0, acc, _ElmoObjectDataType.UNSIGNED32 + ) + + node_ids = [move.node_id for move in plan.moves] + await self.ppm_begin_motion(node_ids) + await self.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.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.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._motor_read_position_raw(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.ipm_select_mode(True) + try: + await self.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.ipm_send_pvt_point( + nid, + samples[i].encoder_position[ax], + samples[i].encoder_velocity[ax], + ) + self.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.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.ipm_send_pvt_point( + nid, + samples[i].encoder_position[ax], + samples[i].encoder_velocity[ax], + ) + self.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.ipm_stop(active_axes) + finally: + try: + await self.ipm_stop(active_axes) + except Exception: + emcy_snap = { + ax: vars(self._emcy[ax]) for ax in active_axes + if ax in self._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.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, + *, + max_gripper_speed: Optional[float], + max_gripper_acceleration: Optional[float], + ) -> None: + """Sample a straight tool-tip path to ``target_pose`` and stream it. + Caller MUST hold ``_motion_guard``.""" + if max_gripper_speed is None or max_gripper_acceleration is None: + raise ValueError( + "move_to_location(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=max_gripper_speed, + accel_mm_per_s2=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) + + # -- high-level arm API -- + + async def halt(self) -> 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.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) -> 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, + max_gripper_speed=self._PARK_SPEED, + max_gripper_acceleration=self._PARK_ACCEL, + path="linear", + ) + + async def request_gripper_pose(self) -> 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) -> None: + async with self._motion_guard(): + await self._motors_move_joint_locked({Axis.SERVO_GRIPPER: gripper_width}) + + async def close_gripper( + self, + gripper_width: float, + *, + check_plate_gripped: bool = True, + max_force_percent: Optional[int] = None, + ) -> None: + """Close the servo gripper to ``gripper_width``. + + Args: + check_plate_gripped: after the close, verify the gripper stalled on a + plate rather than reaching the fully-closed position. + max_force_percent: 10..100, fraction of the gripper's full current + rating used as the CL/PL torque cap during this close. ``None`` keeps + whatever the last close (or setup) left in place. + """ + async with self._motion_guard(): + if max_force_percent is not None: + await self._set_servo_gripper_force_limit(max_force_percent) + await self._motors_move_joint_locked({Axis.SERVO_GRIPPER: gripper_width}) + if check_plate_gripped: + await self.check_plate_gripped() + + async def is_gripper_closed(self) -> bool: + pos = await self.motor_get_current_position(Axis.SERVO_GRIPPER) + return abs(pos) < 1.0 + + # 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, + *, + max_gripper_speed: Optional[float] = None, + max_gripper_acceleration: Optional[float] = None, + path: Literal["joint", "linear"] = "joint", + ) -> None: + """Move the gripper to a Cartesian ``location`` with yaw ``direction`` (deg). + + ``max_gripper_speed`` / ``max_gripper_acceleration`` cap the worst-case + Cartesian speed/acceleration at the gripper (mm/s, mm/s^2). ``None`` runs + joints at firmware max with no Cartesian cap; otherwise joint speeds are + scaled uniformly so the gripper stays under the cap. + + ``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 the 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`` — they + directly drive the Cartesian profile (no cap == no speed to stream). + """ + pose = CartesianPose(location=location, rotation=Rotation(z=direction)) + if path == "linear": + async with self._motion_guard(): + await self._run_linear_path( + pose, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + return + async with self._motion_guard(): + joint_pos = await self._cart_to_joints(pose) + await self._motors_move_joint_locked( + cmd_pos=joint_pos, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + + async def pick_up_at_location( + self, + location: Coordinate, + direction: float, + resource_width: float, + *, + max_gripper_speed: Optional[float] = None, + max_gripper_acceleration: Optional[float] = None, + check_plate_gripped: bool = True, + ) -> None: + async with self._motion_guard(): + await self.move_to_location( + location, direction, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + await self.close_gripper(resource_width, check_plate_gripped=check_plate_gripped) + + async def drop_at_location( + self, + location: Coordinate, + direction: float, + resource_width: float, + *, + max_gripper_speed: Optional[float] = None, + max_gripper_acceleration: Optional[float] = None, + ) -> None: + async with self._motion_guard(): + await self.move_to_location( + location, direction, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + await self.open_gripper(resource_width) + + async def move_to_joint_position( + self, + position: Dict[int, float], + *, + max_gripper_speed: Optional[float] = None, + max_gripper_acceleration: Optional[float] = None, + ) -> None: + 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, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + + async def pick_up_at_joint_position( + self, + position: Dict[int, float], + resource_width: float, + *, + max_gripper_speed: Optional[float] = None, + max_gripper_acceleration: Optional[float] = None, + check_plate_gripped: bool = True, + ) -> None: + async with self._motion_guard(): + await self.move_to_joint_position( + position, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + await self.close_gripper(resource_width, check_plate_gripped=check_plate_gripped) + + async def drop_at_joint_position( + self, + position: Dict[int, float], + resource_width: float, + *, + max_gripper_speed: Optional[float] = None, + max_gripper_acceleration: Optional[float] = None, + ) -> None: + async with self._motion_guard(): + await self.move_to_joint_position( + position, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + await self.open_gripper(resource_width) + + async def request_joint_position(self) -> 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 the + move methods' ``max_gripper_speed`` / ``max_gripper_acceleration`` 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) -> 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" (freedrive 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.motor_enable(node_id=axis, state=False, use_ds402=True) + self._freedrive_axes = axes + + async def stop_freedrive_mode(self) -> None: + axes: List[int] = self._freedrive_axes or list(MOTION_AXES) + async with self._motion_guard(): + await self.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 + 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}, + 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], + }, + 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 `KX2.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 KX2.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: "KX2", 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.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: KX2, 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/protocol.py b/pylabrobot/paa/kx2/protocol.py new file mode 100644 index 00000000000..2750eea535b --- /dev/null +++ b/pylabrobot/paa/kx2/protocol.py @@ -0,0 +1,367 @@ +"""Pure CANopen/Elmo protocol layer for the PAA KX2. + +Constants, enums, frame dataclasses, and the EMCY decoder — no hardware, no +transport, no ``self``. Everything here is data + pure functions shared by +:class:`~pylabrobot.paa.kx2.kx2.KX2` and the config/kinematics modules. + +Split out of the KX2 hardware class so the wire-protocol definitions can be +imported (and unit-tested) without pulling in the CAN transport. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from enum import IntEnum +from typing import Dict, List, Optional, Tuple + + +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 KX2's move primitives. + + Lives in the protocol layer (not the hardware class) because the + `motor_set_move_direction` primitive consumes it to program Elmo's modulo + mode register, and both config-reading and kinematics planning reference it. + """ + + 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. Callers pass `int(self.Axis.X)`. + node_id: int + position: int + velocity: int # encoder counts/sec (drive-internal; converted 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 + + +@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) 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..2626090c443 --- /dev/null +++ b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py @@ -0,0 +1,436 @@ +"""Unit tests for ``KX2.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.config import Axis +from pylabrobot.paa.kx2.kx2 import KX2 +from pylabrobot.paa.kx2.protocol import CanError, _InputLogic + + +def _attach_fake(backend, fake, *, rename=None): + """Bind the fake recorder's methods onto the KX2 instance, standing in for + the transport primitives the code under test calls. ``rename`` maps a fake + method name to the (possibly private) KX2 method name it replaces.""" + rename = rename or {} + for name in dir(fake): + if name.startswith("__"): + continue + attr = getattr(fake, name) + if callable(attr): + setattr(backend, rename.get(name, name), attr) + + +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 KX2) without polluting the + KX2 instance with ad-hoc attributes.""" + backend: KX2 + 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 KX2 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 = KX2.__new__(KX2) + fake_driver = _FakeDriver() + _attach_fake(backend, fake_driver) + 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], *, + max_gripper_speed: Any = None, max_gripper_acceleration: Any = None, + ) -> None: + move_calls.append({ + "cmd_pos": dict(cmd_pos), + "max_gripper_speed": max_gripper_speed, + "max_gripper_acceleration": max_gripper_acceleration, + }) + 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: KX2 + 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 = KX2.__new__(KX2) + fake_driver = _FakeDriver() + _attach_fake(backend, fake_driver) + 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, *, max_gripper_speed=None, max_gripper_acceleration=None): + joint_calls.append({ + "cmd_pos": dict(cmd_pos), + "max_gripper_speed": max_gripper_speed, + "max_gripper_acceleration": max_gripper_acceleration, + }) + + async def _fake_run_linear(target_pose, *, max_gripper_speed=None, max_gripper_acceleration=None): + linear_calls.append({ + "pose": target_pose, + "max_gripper_speed": max_gripper_speed, + "max_gripper_acceleration": max_gripper_acceleration, + }) + 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..717075ff04f --- /dev/null +++ b/pylabrobot/paa/kx2/tests/arm_backend_pvt_tests.py @@ -0,0 +1,310 @@ +"""Unit tests for ``KX2._run_linear_path``. + +Exercises the IPM streaming runtime that backs ``move_to_location(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 inspect +import unittest +from typing import Any, Dict, List, Optional, Tuple + +from pylabrobot.capabilities.arms.standard import CartesianPose +from pylabrobot.paa.kx2.config import ( + Axis, AxisConfig, GripperParams, KX2Config, +) +from pylabrobot.paa.kx2.kx2 import KX2 +from pylabrobot.paa.kx2.protocol 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 _attach_fake(backend, fake, *, rename=None): + """Bind the fake recorder's methods onto the KX2 instance, standing in for + the transport primitives the code under test calls. ``rename`` maps a fake + method name to the (possibly private) KX2 method name it replaces.""" + rename = rename or {} + for name in dir(fake): + if name.startswith("__"): + continue + attr = getattr(fake, name) + if callable(attr): + setattr(backend, rename.get(name, name), attr) + + +def _build_backend( + fake_driver: _FakeDriver, + *, + current_joints: Optional[Dict[int, float]] = None, +) -> KX2: + backend = KX2.__new__(KX2) + # The fake's motor_get_current_position stands in for the low-level encoder + # read, which the merged class exposes as _motor_read_position_raw. + _attach_fake( + backend, fake_driver, + rename={"motor_get_current_position": "_motor_read_position_raw"}, + ) + 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) + asyncio.run(backend._run_linear_path( + _target_pose(), max_gripper_speed=20.0, max_gripper_acceleration=200.0, + )) + + # 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) + asyncio.run(backend._run_linear_path( + _target_pose(), max_gripper_speed=20.0, max_gripper_acceleration=200.0, + )) + 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) + asyncio.run(backend._run_linear_path( + _target_pose(), max_gripper_speed=20.0, max_gripper_acceleration=200.0, + )) + 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) + asyncio.run(backend._run_linear_path( + _target_pose(), max_gripper_speed=20.0, max_gripper_acceleration=200.0, + )) + 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) + asyncio.run(backend._run_linear_path( + _target_pose(), max_gripper_speed=20.0, max_gripper_acceleration=200.0, + )) + 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) + with self.assertRaises(asyncio.CancelledError): + asyncio.run(backend._run_linear_path( + _target_pose(), max_gripper_speed=20.0, max_gripper_acceleration=200.0, + )) + + 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) + with self.assertRaises(ValueError): + asyncio.run(backend._run_linear_path( + _target_pose(), max_gripper_speed=None, max_gripper_acceleration=200.0, + )) + self.assertEqual(len(fake.calls), 0) + + with self.assertRaises(ValueError): + asyncio.run(backend._run_linear_path( + _target_pose(), max_gripper_speed=20.0, max_gripper_acceleration=None, + )) + + +class LinearPathDispatchDefault(unittest.TestCase): + def test_default_path_is_joint(self): + """move_to_location defaults to joint-space planning (no surprise switch + to the linear IPM streamer).""" + default = inspect.signature(KX2.move_to_location).parameters["path"].default + self.assertEqual(default, "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..a981e0b180c --- /dev/null +++ b/pylabrobot/paa/kx2/tests/arm_backend_read_axis_config_tests.py @@ -0,0 +1,256 @@ +"""Unit tests for ``KX2._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.config import Axis, AxisConfig +from pylabrobot.paa.kx2.kx2 import KX2 +from pylabrobot.paa.kx2.protocol import CanError, JointMoveDirection + + +def _attach_fake(backend, fake, *, rename=None): + """Bind the fake recorder's methods onto the KX2 instance, standing in for + the transport primitives the code under test calls. ``rename`` maps a fake + method name to the (possibly private) KX2 method name it replaces.""" + rename = rename or {} + for name in dir(fake): + if name.startswith("__"): + continue + attr = getattr(fake, name) + if callable(attr): + setattr(backend, rename.get(name, name), attr) + + +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 KX2; 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]) -> KX2: + """Bypass __init__ — _read_axis_config only touches the query primitives.""" + backend = KX2.__new__(KX2) + _attach_fake(backend, _FakeDriver(reg)) + 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..70c097f6614 --- /dev/null +++ b/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py @@ -0,0 +1,232 @@ +"""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. +- ``KX2._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 kx2 as kx2_mod +from pylabrobot.paa.kx2.kx2 import KX2 +from pylabrobot.paa.kx2.protocol import ( + EmcyFrame, + _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) -> KX2: + drv = KX2() + 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..3f078afcd9e --- /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.protocol 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..5e65b1b8be0 --- /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.protocol 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/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",