diff --git a/docs/_static/devices.json b/docs/_static/devices.json index 53b04127422..9b963ffef4a 100644 --- a/docs/_static/devices.json +++ b/docs/_static/devices.json @@ -1835,7 +1835,11 @@ "arm" ], "status": "mostly", - "api_version": "v0", + "api_version": "v1", + "api": "pylabrobot.paa.KX2", + "code_slug": "paa/kx2", + "doc_slug": "paa/kx2/hello-world", + "needs_hardware_testing": false, "manager": "https://discuss.pylabrobot.org/u/rickwierenga" }, { diff --git a/docs/api/pylabrobot.paa.rst b/docs/api/pylabrobot.paa.rst new file mode 100644 index 00000000000..63287d32aeb --- /dev/null +++ b/docs/api/pylabrobot.paa.rst @@ -0,0 +1,24 @@ +.. currentmodule:: pylabrobot.paa + +pylabrobot.paa package +====================== + +KX2 +--- + +.. currentmodule:: pylabrobot.paa.kx2 + +.. autosummary:: + :toctree: _autosummary + :nosignatures: + :recursive: + + KX2 + KX2Gripper + KX2BarcodeReader + KX2Config + Axis + CartesianPose + CanError + IKError + KX2BarcodeReaderError diff --git a/docs/api/pylabrobot.rst b/docs/api/pylabrobot.rst index 93a83ba709e..c324082e0eb 100644 --- a/docs/api/pylabrobot.rst +++ b/docs/api/pylabrobot.rst @@ -40,6 +40,7 @@ Manufacturers pylabrobot.micronic pylabrobot.molecular_devices pylabrobot.opentrons + pylabrobot.paa pylabrobot.qinstruments pylabrobot.revvity pylabrobot.sartorius diff --git a/docs/user_guide/index.md b/docs/user_guide/index.md index 624d37f53eb..16657b31a39 100644 --- a/docs/user_guide/index.md +++ b/docs/user_guide/index.md @@ -48,6 +48,7 @@ mettler_toledo/index micronic/index molecular_devices/index opentrons/index +paa/index qinstruments/index revvity/index sartorius/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..e8b1bb4d17c --- /dev/null +++ b/docs/user_guide/paa/kx2/barcode-reader.ipynb @@ -0,0 +1,127 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "a23709b6", + "metadata": {}, + "source": [ + "# KX2 barcode reader\n", + "\n", + "The optional barcode reader is an independent serial device. Configure the reader for external triggering before connecting; PLR leaves its stored mode, symbologies and firmware read window unchanged. See [installation](installation.md) for finding the serial port." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1d1e367c", + "metadata": {}, + "outputs": [], + "source": [ + "from pylabrobot.paa.kx2 import KX2BarcodeReader\n", + "\n", + "reader = KX2BarcodeReader(port=\"/dev/ttyUSB0\")" + ] + }, + { + "cell_type": "markdown", + "id": "a19f2be0", + "metadata": {}, + "source": [ + "## Connect\n", + "\n", + "Setup opens 9600-baud, 8-N-1 serial with no RTS/CTS and requests the firmware version." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "27ce8e67", + "metadata": {}, + "outputs": [], + "source": [ + "await reader.setup()" + ] + }, + { + "cell_type": "markdown", + "id": "cd0ebce3", + "metadata": {}, + "source": [ + "## Scan\n", + "\n", + "Place the label in the reading area. `read_time` is a host deadline in seconds, not a change to the scanner's firmware read window. A no-read returns `None`; errors propagate. The trigger is switched off on success, timeout, failure and cancellation." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "274f8d7a", + "metadata": {}, + "outputs": [], + "source": [ + "barcode = await reader.scan_barcode(read_time=5)\n", + "barcode" + ] + }, + { + "cell_type": "markdown", + "id": "464ad870", + "metadata": {}, + "source": [ + "## Read firmware version" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1ea3c392", + "metadata": {}, + "outputs": [], + "source": [ + "await reader.request_software_version()" + ] + }, + { + "cell_type": "markdown", + "id": "2cdefb98", + "metadata": {}, + "source": [ + "## Disconnect\n", + "\n", + "Stop joins any scan's trigger cleanup and closes the port." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "0bf89968", + "metadata": {}, + "outputs": [], + "source": [ + "await reader.stop()" + ] + }, + { + "cell_type": "markdown", + "id": "d7fecf0d", + "metadata": {}, + "source": [ + "## Use the reader attached to an arm\n", + "\n", + "You can instead construct `KX2(barcode_port=\"/dev/ttyUSB0\")`; the arm then manages the reader lifecycle and exposes `await kx2.scan_barcode(read_time=5)`. Do not open the same serial port with both objects." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/docs/user_guide/paa/kx2/continuous-motion.ipynb b/docs/user_guide/paa/kx2/continuous-motion.ipynb new file mode 100644 index 00000000000..40c4cd6e467 --- /dev/null +++ b/docs/user_guide/paa/kx2/continuous-motion.ipynb @@ -0,0 +1,166 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "6f6bd53c", + "metadata": {}, + "source": [ + "# KX2 continuous motion\n", + "\n", + "Complete [hello world](hello-world.ipynb) through setup first. These cells assume a connected `kx2`.\n", + "\n", + "Streaming sends CANopen interpolation points at 8 ms intervals. Setup configures the drive buffers and synchronized motion channels." + ] + }, + { + "cell_type": "markdown", + "id": "13a5e9fa", + "metadata": {}, + "source": [ + "## Capture a starting pose\n", + "\n", + "All locations are absolute grip-center coordinates in millimeters, with the KX2 yaw convention described in hello world." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9ee496b4", + "metadata": {}, + "outputs": [], + "source": [ + "from pylabrobot.paa.kx2 import CartesianPose\n", + "from pylabrobot.resources import Coordinate, Rotation\n", + "\n", + "start = await kx2.request_gripper_pose()" + ] + }, + { + "cell_type": "markdown", + "id": "d15f5cc3", + "metadata": {}, + "source": [ + "## Linear Cartesian path\n", + "\n", + "After verifying a clear 5 mm vertical segment, run this move. Both speed and acceleration must be provided. The planner checks joint travel at the sampled poses; it does not check collisions." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "de87d1b2", + "metadata": {}, + "outputs": [], + "source": [ + "target = Coordinate(start.location.x, start.location.y, start.location.z + 5)\n", + "await kx2.move_to_location(target, start.rotation.z, path=\"linear\",\n", + " max_gripper_speed=5, max_gripper_acceleration=20)" + ] + }, + { + "cell_type": "markdown", + "id": "74892bb5", + "metadata": {}, + "source": [ + "## Rotate at a fixed grip center\n", + "\n", + "A linear path can change yaw while keeping the grip center in place. Set the tool offsets for your gripper and verify clearance for the rotation. Angular speed and acceleration use degrees per second and degrees per second squared; their defaults are 30 and 100." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e9119ca5", + "metadata": {}, + "outputs": [], + "source": [ + "current = await kx2.request_gripper_pose()\n", + "await kx2.move_to_location(\n", + " current.location, current.rotation.z + 1, path=\"linear\",\n", + " max_gripper_speed=5, max_gripper_acceleration=20,\n", + " max_gripper_angular_speed=1, max_gripper_angular_acceleration=2,\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "02325b67", + "metadata": {}, + "source": [ + "## Smooth waypoints\n", + "\n", + "`move_through_waypoints` fits a smooth spline, so the curve can overshoot the line segments joining your points. Check the whole curve. Keep the first waypoint at the measured current pose." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "354d1fed", + "metadata": {}, + "outputs": [], + "source": [ + "current = await kx2.request_gripper_pose()\n", + "waypoints = [current, CartesianPose(\n", + " Coordinate(current.location.x, current.location.y, current.location.z + 5), current.rotation\n", + ")]\n", + "await kx2.move_through_waypoints(waypoints, speed=5, acceleration=20)" + ] + }, + { + "cell_type": "markdown", + "id": "c41f93b8", + "metadata": {}, + "source": [ + "## Parametric paths\n", + "\n", + "`move_parametric(path_fn, duration)` samples absolute poses at times in seconds. It does not impose a speed or acceleration cap. The path must start at the measured pose and remain continuous and within travel and motion limits; this holds the measured pose without a displacement." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b0a6e8bc", + "metadata": {}, + "outputs": [], + "source": [ + "held = await kx2.request_gripper_pose()\n", + "\n", + "def hold_pose(t: float) -> CartesianPose:\n", + " \"\"\"Return the same absolute pose throughout the requested interval.\"\"\"\n", + " return held\n", + "\n", + "await kx2.move_parametric(hold_pose, duration=1)" + ] + }, + { + "cell_type": "markdown", + "id": "6f93ae2e", + "metadata": {}, + "source": [ + "## Disconnect" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "6d320012", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.stop()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb new file mode 100644 index 00000000000..1c5bef5f152 --- /dev/null +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -0,0 +1,376 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "0abf6c95", + "metadata": {}, + "source": [ + "# PAA KX2\n", + "\n", + "```{device-card} paa-kx2\n", + "```\n", + "\n", + "| Property | Value |\n", + "|---|---|\n", + "| Connection | CANopen, 500 kbit/s; optional serial barcode reader |\n", + "| Supported topology | Shoulder, vertical Z, radial elbow, wrist, servo gripper |\n", + "| Position units | Millimeters for Z, elbow and jaws; degrees for shoulder and wrist |\n", + "| Lifecycle | `setup()` reads calibration; `stop()` restores saved configuration |" + ] + }, + { + "cell_type": "markdown", + "id": "3838ce09", + "metadata": {}, + "source": [ + "## How it talks\n", + "\n", + "The arm's drives share a CAN bus. The barcode reader uses a separate serial connection. Install `pylabrobot[canopen,serial]` and configure the adapter as described in [installation](installation.md). No vendor application should control the arm at the same time." + ] + }, + { + "cell_type": "markdown", + "id": "641f56ea", + "metadata": {}, + "source": [ + "## Physical setup\n", + "\n", + "Connect the arm's CAN USB connection and power according to its manual. Check the adapter's channel name and 500 kbit/s configuration. Keep the e-stop accessible, clear the arm's travel, and support any load before entering freedrive. Measure the installed gripper's grip-center length and vertical offset; the default zero offsets describe the wrist reference point.\n", + "\n", + "Choose the interface and channel that actually enumerate on your host. This Linux example uses SocketCAN; other adapters use their python-can interface names." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "724aa34a", + "metadata": {}, + "outputs": [], + "source": [ + "from pylabrobot.paa import KX2\n", + "from pylabrobot.paa.kx2 import Axis\n", + "\n", + "kx2 = KX2(interface=\"socketcan\", channel=\"can0\", bitrate=500000,\n", + " gripper_length=0, gripper_z_offset=0)" + ] + }, + { + "cell_type": "markdown", + "id": "d82df712", + "metadata": {}, + "source": [ + "## Connect\n", + "\n", + "`setup()` reads the arm's calibration and saves the configuration it changes. It does not automatically home, enable motors or close the jaws. Homing is an explicit operation below. Always finish with `stop()`, including after an exception." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b156aa4a", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.setup()" + ] + }, + { + "cell_type": "markdown", + "id": "95044170", + "metadata": {}, + "source": [ + "## Read joint state\n", + "\n", + "The device-local `Axis` keys and `move_to_joint_state` / `request_joint_state` names follow the PreciseFlex convention. The KX2 elbow is a radial slide in millimeters. Its geometry and calibration belong to this driver." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "1a81f7c6", + "metadata": {}, + "outputs": [], + "source": [ + "joint_state = await kx2.request_joint_state()\n", + "joint_state" + ] + }, + { + "cell_type": "markdown", + "id": "1bd9b714", + "metadata": {}, + "source": [ + "## Read Cartesian pose\n", + "\n", + "The location is the grip center. KX2 yaw is counterclockwise about +Z, with zero pointing along -Y for the barcode-reader finger. Shoulder zero places the radial slide along +Y. Selecting the proximity-sensor finger shifts reported yaw by 180 degrees without moving the grip center. Do not reuse Cartesian poses from another arm without transforming its coordinate frame." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "66a2e77b", + "metadata": {}, + "outputs": [], + "source": [ + "pose = await kx2.request_gripper_pose()\n", + "pose" + ] + }, + { + "cell_type": "markdown", + "id": "48894f6f", + "metadata": {}, + "source": [ + "## Move to an absolute joint state\n", + "\n", + "Use taught, checked targets within this arm's travel limits. Omitted axes are unchanged. This first example commands the measured Z position, making it a no-op until you deliberately change the target. Speed and acceleration settings apply to this call." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c6b323b4", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.move_to_joint_state(\n", + " {Axis.Z: joint_state[Axis.Z]}, max_gripper_speed=20, max_gripper_acceleration=100\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "17669818", + "metadata": {}, + "source": [ + "## Move to a Cartesian pose\n", + "\n", + "The default joint path can curve through space. Check the entire path for clearance. This example commands the measured pose; edit it only after establishing your coordinate frame. Streamed linear motion is described in [continuous motion](continuous-motion.ipynb)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "da3cfda1", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.move_to_location(\n", + " pose.location, pose.rotation.z, max_gripper_speed=20, max_gripper_acceleration=100\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "809deec6", + "metadata": {}, + "source": [ + "## Home the gripper\n", + "\n", + "Remove any plate and clear the jaws before running this cell. Homing moves to the hard stop and encoder index and updates the jaw calibration. It restores temporary current, homing and motion settings afterwards." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c79a23d9", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.gripper.home()" + ] + }, + { + "cell_type": "markdown", + "id": "d9656b49", + "metadata": {}, + "source": [ + "## Read the gripper\n", + "\n", + "The physical jaws are available as `kx2.gripper`, as on current PreciseFlex drivers. Their travel limits come from this arm's calibration." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2432cf12", + "metadata": {}, + "outputs": [], + "source": [ + "jaw_width = await kx2.gripper.request_jaw_position()\n", + "jaw_width" + ] + }, + { + "cell_type": "markdown", + "id": "f2755c20", + "metadata": {}, + "source": [ + "## Position the jaws\n", + "\n", + "Use an absolute width in millimeters. This example leaves the jaws at their measured width. An ordinary coordinated joint move refuses a closing jaw target unless `close_gripper_without_force_sensing=True` is explicitly passed." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "71f69099", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.gripper.move_to_jaw_position(jaw_width, force_sensing=False)" + ] + }, + { + "cell_type": "markdown", + "id": "151c6676", + "metadata": {}, + "source": [ + "## Grip a plate\n", + "\n", + "Choose a target within the calibrated jaw travel and a current limit suitable for the plate and tooling. `max_force_percent` is a percentage of motor current, not newtons. `max_speed` and `max_acceleration` limit jaw motion in mm/s and mm/s².\n", + "\n", + "A force-sensed close requires measurable closing travel and plate detection before accepting sustained stationary current as contact. `check_plate_gripped=True` also checks the held grip. The closing target and current limit remain active until release or disconnection. Opening ends the closing target, uses normal opening current, and restores the current limits saved before the grip.\n", + "\n", + "The following cell reads the plate sensor and grip evidence without moving." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2f7fba72", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.gripper.sense_plate_gripped()" + ] + }, + { + "cell_type": "markdown", + "id": "ca1f3c66", + "metadata": {}, + "source": [ + "## Teach and park\n", + "\n", + "Freedrive disables the selected positioning motors. Support the arm or load before running this cell; choose only axes that can be moved safely by hand." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d26ad9ef", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.start_freedrive_mode(free_axes=[Axis.SHOULDER, Axis.ELBOW, Axis.WRIST])" + ] + }, + { + "cell_type": "markdown", + "id": "a39d8aa6", + "metadata": {}, + "source": [ + "Move the arm by hand to a clear parking pose, then read the positioning joints." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "eaa6e539", + "metadata": {}, + "outputs": [], + "source": [ + "taught = await kx2.request_joint_state()\n", + "kx2.parking_position = {axis: taught[axis] for axis in (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST)}" + ] + }, + { + "cell_type": "markdown", + "id": "22b97552", + "metadata": {}, + "source": [ + "Restore the motor enable states saved when freedrive began. This refuses to re-enable after a fault or e-stop." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b782a31c", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.stop_freedrive_mode()" + ] + }, + { + "cell_type": "markdown", + "id": "fc64e7db", + "metadata": {}, + "source": [ + "`park()` uses your configured target and leaves the jaws unchanged. There is no universal default parking pose. Check clearance from the current position before moving." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "84dbaaa8", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.park()" + ] + }, + { + "cell_type": "markdown", + "id": "1326fcbe", + "metadata": {}, + "source": [ + "## Stop an operation\n", + "\n", + "`halt()` cancels arm motion and stops the positioning axes while preserving gripper holding power. It blocks subsequent motion in this connection. Resolve the cause, then stop and reconnect before continuing. Use the physical e-stop for an emergency; host software depends on the CAN connection." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "18612304", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.halt()" + ] + }, + { + "cell_type": "markdown", + "id": "ed124b3c", + "metadata": {}, + "source": [ + "## Disconnect\n", + "\n", + "`stop()` joins active work, restores saved registers, PDO mappings and node states, and closes the connections. It reports restoration failures. Faulted motors are not automatically re-enabled. If the gripper was disabled when connecting, stop disables it again; remove or support a held plate before disconnecting." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "b0324325", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.stop()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/docs/user_guide/paa/kx2/installation.md b/docs/user_guide/paa/kx2/installation.md new file mode 100644 index 00000000000..ea9385afd97 --- /dev/null +++ b/docs/user_guide/paa/kx2/installation.md @@ -0,0 +1,39 @@ +# KX2 installation + +Install from a PyLabRobot checkout containing the KX2 driver: + +```bash +pip install -e ".[canopen,serial]" +``` + +The arm uses CANopen at 500 kbit/s through its CAN USB connection. The optional barcode reader uses a separate USB-to-serial connection; omit `serial` if unused. The Python driver uses portable `python-can` adapter interfaces through `pylabrobot.io.CANopen`. + +## Limitations + +- The optional rail is not supported. +- Configurations without the servo gripper are not supported. + +## CAN adapter + +Install the driver for the adapter fitted to your arm and identify its channel. On Linux, a SocketCAN adapter normally appears as `can0`. Configure the interface before opening PLR: + +```bash +sudo ip link set can0 up type can bitrate 500000 +sudo ip link set can0 txqueuelen 1000 +``` + +Pass `interface="socketcan", channel="can0"` to `KX2`. On Windows or macOS, choose the interface and channel supported by your adapter's python-can backend (for example `pcan` with its enumerated channel). OS driver installation is adapter-specific; the device driver itself does not call a vendor DLL. Use exclusive control of the bus while operating the arm. + +The arm must expose nodes 1, 2, 3, 4 and 6 and support NMT state readback through node guarding or producer heartbeat. For Gold drives without node guarding, the transport briefly enables heartbeat and restores the exact original period. Setup saves the NMT state before bringing nodes operational and restores it on teardown. + +## Barcode reader + +The serial connection uses 9600 baud, 8 data bits, no parity, one stop bit, and no RTS/CTS. Identify the actual port: + +```bash +python -m serial.tools.list_ports -v +``` + +Use that port (such as `/dev/ttyUSB0`, an enumerated `/dev/cu.*` device, or `COM3`) with `KX2BarcodeReader(port=...)` or `KX2(barcode_port=...)`. Install the USB-to-serial adapter manufacturer's driver if your operating system does not enumerate it. Configure the scanner for external triggering; PLR does not overwrite its mode or read-window settings. + +Continue with [hello world](hello-world.ipynb). 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..7b34e0beffc --- /dev/null +++ b/docs/user_guide/paa/kx2/proximity-sensor.ipynb @@ -0,0 +1,111 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "05930479", + "metadata": {}, + "source": [ + "# KX2 proximity sensor\n", + "\n", + "Complete [hello world](hello-world.ipynb) through setup first. These cells assume a connected `kx2`. The beam between the fingers is wired to input 4 on the Z drive. `True` means the beam is interrupted." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "e2cc9529", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.sense_proximity()" + ] + }, + { + "cell_type": "markdown", + "id": "ac48affc", + "metadata": {}, + "source": [ + "## Wait for the beam\n", + "\n", + "This observes the sensor without moving any axis. It returns `False` if the deadline expires." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "38fec4bd", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.wait_for_proximity_sensor(state=True, timeout=5)" + ] + }, + { + "cell_type": "markdown", + "id": "bc78c5c7", + "metadata": {}, + "source": [ + "## Search downward\n", + "\n", + "Measure the search range and clear the approach path. This example searches 5 mm below the measured Z. The search first moves to `z_start`, then descends towards `z_end`. It stops when the beam trips and returns the measured Z. The input's original logic is restored even on cancellation; unrelated drive faults propagate." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4f2fbf37", + "metadata": {}, + "outputs": [], + "source": [ + "from pylabrobot.paa.kx2 import Axis\n", + "\n", + "position = await kx2.request_joint_state()\n", + "found_z = await kx2.find_z_with_proximity_sensor(\n", + " z_start=position[Axis.Z], z_end=position[Axis.Z] - 5,\n", + " max_gripper_speed=5, max_gripper_acceleration=20,\n", + ")\n", + "found_z" + ] + }, + { + "cell_type": "markdown", + "id": "f8b0e70f", + "metadata": {}, + "source": [ + "## Cartesian search\n", + "\n", + "`find_with_proximity_sensor(start, end, max_gripper_speed=..., max_gripper_acceleration=...)` follows a streamed straight segment while retaining the measured yaw. It polls the sensor on the host, so it has different stopping latency from the drive-local input stop used by the Z search. Read the [streaming limitations](continuous-motion.ipynb) and verify clearance and stopping distance before using it." + ] + }, + { + "cell_type": "markdown", + "id": "2cc94248", + "metadata": {}, + "source": [ + "## Disconnect" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ad21588a", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.stop()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "name": "python" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/pylabrobot/io/__init__.py b/pylabrobot/io/__init__.py index 9464cba4870..709f16bbafb 100644 --- a/pylabrobot/io/__init__.py +++ b/pylabrobot/io/__init__.py @@ -1,3 +1,4 @@ +from .canopen import CANopen from .capture import start_capture, stop_capture from .command_line import CommandLineResult, CommandLineTransport, CommandLineValidator from .http import HTTP, HTTPError diff --git a/pylabrobot/io/canopen.py b/pylabrobot/io/canopen.py new file mode 100644 index 00000000000..05314d9d164 --- /dev/null +++ b/pylabrobot/io/canopen.py @@ -0,0 +1,297 @@ +"""Asynchronous CANopen transport with serialized SDO channels and frame subscriptions.""" + +import asyncio +import logging +from dataclasses import dataclass +from typing import Callable, Dict, List, Optional, Sequence, TypeVar + +from pylabrobot.io.capture import Command, capturer + +try: + import can + import canopen +except ImportError: + can = None # type: ignore[assignment] + canopen = None # type: ignore[assignment] + +logger = logging.getLogger(__name__) +T = TypeVar("T") +FrameCallback = Callable[[int, bytes, float], None] + + +async def finish_io(awaitable: "asyncio.Future[T]") -> T: + """Join a pending transfer or cleanup despite cancellation, then propagate cancellation. + + A cancelled coroutine must not release an SDO channel while its worker is still using it. + """ + cancelled = False + while not awaitable.done(): + try: + await asyncio.shield(awaitable) + except asyncio.CancelledError: + cancelled = True + result = awaitable.result() + if cancelled: + raise asyncio.CancelledError() + return result + + +@dataclass +class CANopenCommand(Command): + """One CAN frame or SDO transaction, including its complete payload.""" + + node_id: int + index: int + subindex: int + data: str + dlc: Optional[int] = None + + +class CANopen: + """A CANopen network; device topology and register meanings belong to its caller. + + ``interface`` and ``channel`` use python-can's adapter names. Receiving callbacks run on + the asyncio loop that opened the connection. SDO transfers on one node are serialized. + """ + + def __init__( + self, interface: str, channel: Optional[str] = None, bitrate: int = 500000, timeout: float = 1.0 + ) -> None: + """Store adapter parameters without opening a connection.""" + self.interface = interface + self.channel = channel + self.bitrate = bitrate + self.timeout = timeout + self._network: Optional[canopen.Network] = None + self._nodes: Dict[int, canopen.RemoteNode] = {} + self._locks: Dict[int, asyncio.Lock] = {} + self._nmt_locks: Dict[int, asyncio.Lock] = {} + self._callbacks: Dict[int, List[FrameCallback]] = {} + self._loop: Optional[asyncio.AbstractEventLoop] = None + self._generation = 0 + self._workers: set[asyncio.Task] = set() + self._closing = False + + @property + def connected(self) -> bool: + """Whether this transport owns an open network.""" + return self._network is not None + + def _record( + self, + action: str, + node_id: int, + index: int, + subindex: int, + data: bytes, + dlc: Optional[int] = None, + ) -> None: + """Record bytes and their addressing without interpreting device registers.""" + capturer.record( + CANopenCommand( + "canopen", + f"{self.interface}:{self.channel}", + action, + node_id, + index, + subindex, + data.hex(), + dlc, + ) + ) + logger.debug( + "CANopen %s node=%d index=0x%04x:%d %s", action, node_id, index, subindex, data.hex() + ) + + def _dispatch(self, generation: int, cob_id: int, data: bytes, timestamp: float) -> None: + """Discard callbacks from a closed session and deliver current frames.""" + if generation != self._generation or self._loop is None: + return + self._record("receive", 0, cob_id, 0, data) + for callback in tuple(self._callbacks.get(cob_id, [])): + try: + callback(cob_id, data, timestamp) + except Exception: + logger.exception("CANopen callback failed for COB-ID 0x%x", cob_id) + + def subscribe(self, cob_id: int, callback: FrameCallback) -> None: + """Subscribe before sending a request; callback delivery stays on the owning loop.""" + network = self._network + if network is None or self._loop is None: + raise RuntimeError("CANopen is not connected") + if cob_id not in self._callbacks: + generation, loop = self._generation, self._loop + + def receive(received_id: int, data: bytes, timestamp: float) -> None: + """Marshal the library listener thread onto the connection's event loop.""" + if not loop.is_closed(): + loop.call_soon_threadsafe(self._dispatch, generation, received_id, bytes(data), timestamp) + + self._callbacks[cob_id] = [] + network.subscribe(cob_id, receive) + self._callbacks[cob_id].append(callback) + + async def _transfer(self, operation: Callable[..., T], *args, **kwargs) -> T: + """Join a worker before the connection or its SDO lock can be released.""" + if self._closing or self._network is None: + raise RuntimeError("CANopen is not connected") + task = asyncio.create_task(asyncio.to_thread(operation, *args, **kwargs)) + self._workers.add(task) + try: + return await finish_io(task) + finally: + self._workers.discard(task) + + async def _disconnect(self) -> None: + """Wait for all accepted transfers, then release the adapter.""" + self._closing = True + self._generation += 1 + self._loop = None + network = self._network + try: + if self._workers: + await asyncio.gather(*self._workers, return_exceptions=True) + if network is not None: + await asyncio.to_thread(network.disconnect) + finally: + self._network = None + self._nodes.clear() + self._locks.clear() + self._nmt_locks.clear() + self._callbacks.clear() + + async def stop(self) -> None: + """Disconnect after outstanding workers finish, even if the caller is cancelled.""" + await finish_io(asyncio.create_task(self._disconnect())) + + async def setup(self, node_ids: Sequence[int]) -> None: + """Open the adapter and prepare SDO channels, without resetting or starting any node.""" + if self.connected: + raise RuntimeError("CANopen is already connected") + if canopen is None: + raise ImportError("Install CANopen support with pip install 'pylabrobot[canopen]'") + if len(set(node_ids)) != len(node_ids) or any(not 1 <= nid <= 127 for nid in node_ids): + raise ValueError("CANopen node IDs must be unique integers in 1..127") + self._closing = False + self._loop = asyncio.get_running_loop() + network = canopen.Network() + self._network = network + try: + await self._transfer( + network.connect, interface=self.interface, channel=self.channel, bitrate=self.bitrate + ) + for nid in node_ids: + node = network.add_node(nid, canopen.ObjectDictionary()) + node.sdo.RESPONSE_TIMEOUT = self.timeout + self._nodes[nid] = node + self._locks[nid] = asyncio.Lock() + self._nmt_locks[nid] = asyncio.Lock() + except BaseException: + await finish_io(asyncio.ensure_future(self.stop())) + raise + + def _send_remote(self, network: "canopen.Network", cob_id: int, dlc: int) -> None: + """Send an RTR frame with its requested response length preserved. + + canopen.Network.send_message drops the DLC when python-can clears an RTR payload. + """ + if network.bus is None or can is None: + raise RuntimeError("CANopen is not connected") + message = can.Message( + arbitration_id=cob_id, is_extended_id=False, is_remote_frame=True, dlc=dlc + ) + with network.send_lock: + network.bus.send(message) + network.check() + + async def write( + self, + cob_id: int, + data: bytes, + *, + remote: bool = False, + dlc: Optional[int] = None, + ) -> None: + """Send one CAN frame; RTR frames declare their requested response length with dlc.""" + network = self._network + if network is None: + raise RuntimeError("CANopen is not connected") + if not 0 <= cob_id <= 0x7FF or len(data) > 8: + raise ValueError("CANopen requires an 11-bit identifier and at most eight bytes") + if remote: + if data or dlc is None or not 0 <= dlc <= 8: + raise ValueError("An RTR frame requires empty data and a DLC in 0..8") + await self._transfer(self._send_remote, network, cob_id, dlc) + else: + if dlc is not None and dlc != len(data): + raise ValueError("Data-frame DLC must match its payload") + await self._transfer(network.send_message, cob_id, data) + self._record("remote" if remote else "send", 0, cob_id, 0, data, dlc if remote else len(data)) + + async def upload(self, node_id: int, index: int, subindex: int) -> bytes: + """Read an object, including segmented replies, on the node's SDO channel.""" + if not self.connected or self._closing: + raise RuntimeError("CANopen is not connected") + async with self._locks[node_id]: + if self._closing: + raise RuntimeError("CANopen is closing") + result = bytes(await self._transfer(self._nodes[node_id].sdo.upload, index, subindex)) + self._record("upload", node_id, index, subindex, result) + return result + + async def download(self, node_id: int, index: int, subindex: int, data: bytes) -> None: + """Write an object; wait for the acknowledgement before releasing its SDO channel.""" + if not self.connected or self._closing: + raise RuntimeError("CANopen is not connected") + async with self._locks[node_id]: + if self._closing: + raise RuntimeError("CANopen is closing") + await self._transfer(self._nodes[node_id].sdo.download, index, subindex, data) + self._record("download", node_id, index, subindex, data) + + async def _request_nmt_state(self, node_id: int) -> int: + """Query one node while holding its NMT configuration lock.""" + future: asyncio.Future[int] = asyncio.get_running_loop().create_future() + + def receive(cob_id: int, data: bytes, timestamp: float) -> None: + """Read the state while ignoring the node-guard toggle bit.""" + if data and not future.done(): + future.set_result(data[0] & 0x7F) + + cob_id = 0x700 + node_id + self.subscribe(cob_id, receive) + try: + await self.write(cob_id, b"", remote=True, dlc=1) + try: + return await asyncio.wait_for(asyncio.shield(future), self.timeout) + except asyncio.TimeoutError: + original = await self.upload(node_id, 0x1017, 0) + if len(original) != 2: + raise ValueError(f"Node {node_id} returned an invalid heartbeat period") + try: + await self.download(node_id, 0x1017, 0, (100).to_bytes(2, "little")) + return await asyncio.wait_for(future, self.timeout) + finally: + await finish_io(asyncio.create_task(self.download(node_id, 0x1017, 0, original))) + finally: + future.cancel() + callbacks = self._callbacks.get(cob_id, []) + if receive in callbacks: + callbacks.remove(receive) + + async def request_nmt_state(self, node_id: int) -> int: + """Read NMT state, temporarily enabling heartbeat if node guarding is unsupported. + + Restore the exact producer heartbeat period before returning, including on cancellation. + """ + async with self._nmt_locks[node_id]: + return await self._request_nmt_state(node_id) + + async def set_nmt_state(self, node_id: int, state: int) -> None: + """Move one node to operational, stopped, or pre-operational and confirm it.""" + commands = {5: 1, 4: 2, 127: 128} + if state not in commands: + raise ValueError(f"Cannot restore NMT state {state}") + await self.write(0, bytes([commands[state], node_id])) + if await self.request_nmt_state(node_id) != state: + raise RuntimeError(f"Node {node_id} did not enter NMT state {state}") diff --git a/pylabrobot/io/canopen_tests.py b/pylabrobot/io/canopen_tests.py new file mode 100644 index 00000000000..0e605182f03 --- /dev/null +++ b/pylabrobot/io/canopen_tests.py @@ -0,0 +1,144 @@ +"""CANopen transport tests use mocked adapters; no CAN interface is opened.""" + +import asyncio +import threading +import unittest +from unittest.mock import MagicMock, patch + +from pylabrobot.io.canopen import CANopen, canopen + + +@unittest.skipIf(canopen is None, "optional canopen dependency is not installed") +class CANopenTests(unittest.IsolatedAsyncioTestCase): + """Verify worker lifetime, cancellation, subscriptions and NMT framing.""" + + async def test_cancelled_sdo_keeps_channel_until_worker_finishes(self) -> None: + """Cancellation cannot let a second SDO overtake an unfinished transfer.""" + network = MagicMock() + entered, release = threading.Event(), threading.Event() + calls = [] + + def upload(index: int, subindex: int) -> bytes: + """Block the first transaction while a competing transaction queues.""" + calls.append(index) + if index == 1: + entered.set() + if not release.wait(2): + raise TimeoutError("test worker was not released") + return b"\x05" + + network.add_node.return_value.sdo.upload.side_effect = upload + io = CANopen("virtual") + with patch("pylabrobot.io.canopen.canopen.Network", return_value=network): + await io.setup([1]) + first = asyncio.create_task(io.upload(1, 1, 0)) + await asyncio.to_thread(entered.wait, 2) + first.cancel() + second = asyncio.create_task(io.upload(1, 2, 0)) + await asyncio.sleep(0.01) + self.assertEqual(calls, [1]) + first.cancel() + release.set() + with self.assertRaises(asyncio.CancelledError): + await first + self.assertEqual(await second, b"\x05") + self.assertEqual(calls, [1, 2]) + await io.stop() + network.disconnect.assert_called_once() + + async def test_nmt_request_masks_toggle_and_discards_stale_callbacks(self) -> None: + """Node guarding uses RTR and queued callbacks from a closed session are ignored.""" + network = MagicMock() + callbacks = {} + + def subscribe(cob_id, callback) -> None: + """Retain the library-thread callback.""" + callbacks[cob_id] = callback + + def send(message) -> None: + """Supply a node guard response from the worker thread.""" + if message.is_remote_frame: + callbacks[message.arbitration_id](message.arbitration_id, b"\x85", 0.0) + + network.subscribe.side_effect = subscribe + network.bus.send.side_effect = send + io = CANopen("virtual") + with patch("pylabrobot.io.canopen.canopen.Network", return_value=network): + await io.setup([1]) + self.assertEqual(await io.request_nmt_state(1), 5) + frame = network.bus.send.call_args.args[0] + self.assertEqual(frame.arbitration_id, 0x701) + self.assertEqual(frame.dlc, 1) + self.assertTrue(frame.is_remote_frame) + self.assertEqual(frame.data, b"") + seen = [] + io.subscribe(0x181, lambda cob, data, timestamp: seen.append(data)) + callback = callbacks[0x181] + await io.stop() + callback(0x181, b"late", 0.0) + await asyncio.sleep(0) + self.assertEqual(seen, []) + + async def test_heartbeat_fallback_restores_period(self) -> None: + """Gold-style heartbeat readback restores the exact period on every exit path.""" + for outcome in ("success", "timeout", "cancel", "write_error"): + with self.subTest(outcome=outcome): + network = MagicMock() + callbacks = {} + period = b"\x34\x12" + changed = asyncio.Event() + loop = asyncio.get_running_loop() + + def subscribe(cob_id, callback) -> None: + """Retain the receive callback.""" + callbacks[cob_id] = callback + + def download(index, subindex, data) -> None: + """Emulate a period write, including errors after applying it.""" + nonlocal period + self.assertEqual((index, subindex), (0x1017, 0)) + period = data + if data == b"\x64\x00": + loop.call_soon_threadsafe(changed.set) + if outcome == "success": + callbacks[0x701](0x701, b"\x05", 0.0) + elif outcome == "write_error": + raise OSError("lost acknowledgement") + + network.subscribe.side_effect = subscribe + network.add_node.return_value.sdo.upload.side_effect = lambda *_: period + network.add_node.return_value.sdo.download.side_effect = download + io = CANopen("virtual", timeout=0.02) + with patch("pylabrobot.io.canopen.canopen.Network", return_value=network): + await io.setup([1]) + request = asyncio.create_task(io.request_nmt_state(1)) + if outcome == "cancel": + await changed.wait() + request.cancel() + with self.assertRaises(asyncio.CancelledError): + await request + elif outcome == "timeout": + with self.assertRaises(asyncio.TimeoutError): + await request + elif outcome == "write_error": + with self.assertRaisesRegex(OSError, "lost acknowledgement"): + await request + else: + self.assertEqual(await request, 5) + self.assertEqual( + await asyncio.gather(io.request_nmt_state(1), io.request_nmt_state(1)), [5, 5] + ) + self.assertEqual(period, b"\x34\x12") + self.assertEqual(io._callbacks[0x701], []) + await io.stop() + + async def test_partial_connect_failure_disconnects(self) -> None: + """A partially allocated adapter is released when connect raises.""" + network = MagicMock() + network.connect.side_effect = OSError("adapter error") + io = CANopen("virtual") + with patch("pylabrobot.io.canopen.canopen.Network", return_value=network): + with self.assertRaisesRegex(OSError, "adapter error"): + await io.setup([1]) + network.disconnect.assert_called_once() + self.assertFalse(io.connected) diff --git a/pylabrobot/paa/__init__.py b/pylabrobot/paa/__init__.py new file mode 100644 index 00000000000..422a4916c23 --- /dev/null +++ b/pylabrobot/paa/__init__.py @@ -0,0 +1,3 @@ +"""PAA laboratory devices.""" + +from .kx2 import KX2 diff --git a/pylabrobot/paa/kx2/__init__.py b/pylabrobot/paa/kx2/__init__.py new file mode 100644 index 00000000000..a87ac775dae --- /dev/null +++ b/pylabrobot/paa/kx2/__init__.py @@ -0,0 +1,15 @@ +"""PAA KX2 arm and its independently addressable servo gripper and barcode reader.""" + +from .barcode_reader import KX2BarcodeReader, KX2BarcodeReaderError +from .config import ( + Axis, + AxisConfig, + GripperFingerSide, + GripperParams, + KX2Config, + ServoGripperConfig, +) +from .gripper import KX2Gripper +from .kinematics import CartesianPose, IKError +from .kx2 import KX2 +from .protocol import CanError diff --git a/pylabrobot/paa/kx2/barcode_reader.py b/pylabrobot/paa/kx2/barcode_reader.py new file mode 100644 index 00000000000..8e5855cd46c --- /dev/null +++ b/pylabrobot/paa/kx2/barcode_reader.py @@ -0,0 +1,154 @@ +"""The KX2 serial barcode reader, without changes to its stored scanner configuration.""" + +import asyncio +import logging +import math +import time +from typing import Optional + +from pylabrobot.io.canopen import finish_io +from pylabrobot.io.serial import Serial +from pylabrobot.resources.barcode import Barcode + +logger = logging.getLogger(__name__) + + +class KX2BarcodeReaderError(RuntimeError): + """A malformed reply or an unavailable barcode reader.""" + + +class KX2BarcodeReader: + """An independently connected reader using ESC commands and CR-terminated replies. + + The reader must be configured for external triggering. Setup, scanning and teardown leave + its mode, symbologies and firmware read window unchanged. ``read_time`` is a host deadline; + the reader can finish its own read window earlier. A scan always switches its trigger off. + """ + + default_baudrate = 9600 + + def __init__(self, port: str, baudrate: int = default_baudrate) -> None: + """Configure a serial connection without opening it.""" + self.io = Serial( + human_readable_device_name="KX2 Barcode Reader", + port=port, + baudrate=baudrate, + bytesize=8, + parity="N", + stopbits=1, + write_timeout=1, + timeout=0.1, + rtscts=False, + ) + self._lock = asyncio.Lock() + self._connected = False + self._buffer = bytearray() + self._scan_task: Optional[asyncio.Task] = None + self._stopping = False + + async def _send_command(self, command: str) -> None: + """Send the manufacturer's ESC + ASCII + CR LF frame.""" + await self.io.write(b"\x1b" + command.encode("ascii") + b"\r\n") + + async def _read_line(self, timeout: float) -> str: + """Read one CR-terminated reply, preserving any following bytes.""" + deadline = time.monotonic() + timeout + while True: + if b"\r" in self._buffer: + line, _, tail = self._buffer.partition(b"\r") + self._buffer[:] = tail + return line.decode("ascii", errors="strict").lstrip("\x1b\n").rstrip() + if time.monotonic() >= deadline: + raise TimeoutError("KX2 barcode reader did not reply before the deadline") + self._buffer.extend(await self.io.read(32)) + if len(self._buffer) > 65536: + raise KX2BarcodeReaderError("Barcode reply exceeds 64 KiB without a terminator") + await asyncio.sleep(0) + + async def _request_version(self) -> str: + """Query Z1 without changing scanner settings.""" + await self._send_command("Z1") + version = await self._read_line(5.0) + if not version: + raise KX2BarcodeReaderError("Empty software-version response") + return version + + async def setup(self) -> None: + """Connect and verify the version; close the port after a partial setup failure.""" + async with self._lock: + if self._connected: + return + self._stopping = False + self._buffer.clear() + try: + await self.io.setup() + version = await self._request_version() + except BaseException: + await finish_io(asyncio.ensure_future(self.io.stop())) + raise + self._connected = True + logger.info("KX2 barcode reader connected: version=%s", version) + + async def request_software_version(self) -> str: + """Read the scanner firmware version.""" + async with self._lock: + if not self._connected or self._stopping: + raise KX2BarcodeReaderError("Barcode reader is not connected") + return await self._request_version() + + async def scan_barcode(self, read_time: Optional[float] = None) -> Optional[Barcode]: + """Return one barcode or None by the host deadline, preserving the firmware read window.""" + timeout = 10.0 if read_time is None else read_time + if not math.isfinite(timeout) or timeout <= 0: + raise ValueError("read_time must be finite and positive") + async with self._lock: + if not self._connected or self._stopping: + raise KX2BarcodeReaderError("Barcode reader is not connected") + self._scan_task = asyncio.current_task() + try: + self._buffer.clear() + await self.io.reset_input_buffer() + try: + await self._send_command("Z") + try: + data = await self._read_line(timeout) + except TimeoutError: + return None + return ( + Barcode(data=data, symbology="ANY 1D", position_on_resource="front") if data else None + ) + finally: + await finish_io(asyncio.ensure_future(self._send_command("Y"))) + finally: + self._scan_task = None + + async def _join_scan(self, task: asyncio.Task) -> None: + """Join trigger cleanup without turning task cancellation into a stop failure.""" + try: + await task + except asyncio.CancelledError: + pass + + async def _close(self) -> None: + """Close after pending scanner operations release the port.""" + async with self._lock: + try: + if self._connected: + await self.io.stop() + finally: + self._connected = False + self._buffer.clear() + + async def stop(self) -> None: + """Join any active scan's trigger cleanup and close the port.""" + self._stopping = True + try: + task = self._scan_task + if task is not None and task is not asyncio.current_task(): + task.cancel() + try: + await finish_io(asyncio.ensure_future(self._join_scan(task))) + except asyncio.CancelledError: + pass + finally: + await finish_io(asyncio.ensure_future(self._close())) diff --git a/pylabrobot/paa/kx2/config.py b/pylabrobot/paa/kx2/config.py new file mode 100644 index 00000000000..d14159b57bc --- /dev/null +++ b/pylabrobot/paa/kx2/config.py @@ -0,0 +1,144 @@ +"""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.ELBOW, 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/gripper.py b/pylabrobot/paa/kx2/gripper.py new file mode 100644 index 00000000000..20bfba5ad55 --- /dev/null +++ b/pylabrobot/paa/kx2/gripper.py @@ -0,0 +1,249 @@ +"""The KX2 servo gripper: absolute jaw motion and independent contact verification.""" + +import asyncio +import math +import time +from dataclasses import replace +from typing import TYPE_CHECKING, Optional + +from pylabrobot.io.canopen import finish_io + +from . import kinematics +from .config import Axis +from .protocol import CanError, MotorsMovePlan + +if TYPE_CHECKING: + from .kx2 import KX2, _Registers + + +class KX2Gripper: + """The physical servo gripper, reached as ``kx2.gripper``.""" + + def __init__(self, driver: "KX2") -> None: + """Use the owning arm's transport, calibration, and motion lock.""" + self._driver = driver + self._hold_settings: Optional["_Registers"] = None + + @property + def jaw_width_range(self) -> tuple[float, float]: + """Calibrated jaw travel in millimeters.""" + cfg = self._driver.configuration.axes[Axis.SERVO_GRIPPER] + return cfg.min_travel, cfg.max_travel + + async def request_jaw_position(self) -> float: + """Read the jaw width in millimeters.""" + return await self._driver.request_axis_position(Axis.SERVO_GRIPPER) + + async def sense_fully_closed(self) -> bool: + """Whether the measured jaws are within 0.1 mm of their calibrated closed limit.""" + return abs(await self.request_jaw_position() - self.jaw_width_range[0]) <= 0.1 + + async def sense_plate_gripped(self) -> bool: + """Require plate detection, stationary current, and jaws away from both travel limits.""" + fault = await self._driver.request_axis_fault(Axis.SERVO_GRIPPER) + if fault is not None: + raise CanError(fault) + if not await self._driver.request_axis_enabled(Axis.SERVO_GRIPPER): + return False + status = await self._driver._motor_get_motion_status(Axis.SERVO_GRIPPER) + if status not in (0, 1) or not await self._driver.sense_proximity(): + return False + position = await self.request_jaw_position() + low, high = self.jaw_width_range + if not low + 0.1 < position < high - 0.1: + return False + for _ in range(3): + if await self._driver._get_servo_gripper_force_fraction() < 0.9: + return False + await asyncio.sleep(0.05) + measured = await self.request_jaw_position() + if abs(measured - position) > 0.02: + return False + return ( + await self._driver.request_axis_enabled(Axis.SERVO_GRIPPER) + and await self._driver.request_axis_fault(Axis.SERVO_GRIPPER) is None + and await self._driver.sense_proximity() + ) + + async def check_plate_gripped(self, num_attempts: int = 5) -> None: + """Require measured contact, independently of reaching the requested width.""" + if num_attempts < 1: + raise ValueError("num_attempts must be positive") + for _ in range(num_attempts): + if await self.sense_plate_gripped(): + return + await asyncio.sleep(0.05) + raise CanError("Gripper did not confirm a held plate") + + async def home(self) -> None: + """Calibrate the jaws against the hard stop and encoder index.""" + await self._driver._home_gripper() + + async def _restore_current_limits(self) -> None: + """Restore the limits saved before a grip, after release or when the session ends.""" + if self._hold_settings is not None: + await self._driver._restore_registers(self._hold_settings) + self._hold_settings = None + + async def _wait_for_contact( + self, plan: MotorsMovePlan, width: float, start_width: float, already_gripped: bool + ) -> bool: + """Wait for settled contact or the requested position, keeping the closing target active.""" + deadline = time.monotonic() + plan.move_time + 2.0 + contact_samples = 0 + contact_position: Optional[float] = None + await asyncio.sleep(0.05) + while time.monotonic() < deadline: + fault = await self._driver.request_axis_fault(Axis.SERVO_GRIPPER) + if fault is not None: + raise CanError(fault) + if not await self._driver.request_axis_enabled(Axis.SERVO_GRIPPER): + raise CanError("Gripper disabled during closing motion") + status = await self._driver._motor_get_motion_status(Axis.SERVO_GRIPPER) + position = await self.request_jaw_position() + force = await self._driver._get_servo_gripper_force_fraction() + at_limit = abs(position - self.jaw_width_range[0]) <= 0.1 + if force < 0.9 or at_limit or status not in (0, 1): + contact_position, contact_samples = None, 0 + elif contact_position is None or abs(position - contact_position) > 0.02: + contact_position, contact_samples = position, 1 + else: + contact_samples += 1 + if contact_samples >= 5: + if not already_gripped and start_width - position < 0.1: + raise CanError( + "Current limit reached before measurable jaw closure; check the limit and obstruction" + ) + if not await self._driver.sense_proximity(): + raise CanError("Jaws encountered resistance but the plate sensor is clear") + return True + if status in (0, 1) and abs(position - width) <= 0.01 and (force < 0.9 or at_limit): + return False + await asyncio.sleep(0.05) + raise CanError("Gripper did not reach its target or establish stationary contact") + + async def _wait_for_position(self, width: float, timeout: float) -> None: + """Confirm settled encoder feedback, including profiler-done status with residual error.""" + deadline = time.monotonic() + timeout + settled = 0 + position = await self.request_jaw_position() + await asyncio.sleep(0.05) + while time.monotonic() < deadline: + fault = await self._driver.request_axis_fault(Axis.SERVO_GRIPPER) + if fault is not None: + raise CanError(fault) + if not await self._driver.request_axis_enabled(Axis.SERVO_GRIPPER): + raise CanError("Gripper disabled during jaw motion") + status = await self._driver._motor_get_motion_status(Axis.SERVO_GRIPPER) + position = await self.request_jaw_position() + settled = settled + 1 if status in (0, 1) and abs(position - width) <= 0.01 else 0 + if settled >= 3: + return + await asyncio.sleep(0.05) + raise CanError(f"Gripper did not reach {width:.3f} mm; measured {position:.3f} mm") + + async def _move_jaws( + self, + width: float, + *, + closing: bool, + max_speed: Optional[float], + max_acceleration: Optional[float], + ) -> bool: + """Run a jaw-only profile with scoped speed settings and optional contact detection.""" + sg = Axis.SERVO_GRIPPER + cfg = self._driver.configuration + axis = cfg.axes[sg] + limits = replace( + axis, + max_vel=axis.max_vel if max_speed is None else min(axis.max_vel, max_speed), + max_accel=axis.max_accel + if max_acceleration is None + else min(axis.max_accel, max_acceleration), + ) + start_width = await self.request_jaw_position() + already_gripped = await self.sense_plate_gripped() if closing else False + plan = kinematics.plan_joint_move( + current={sg: start_width}, + target={sg: width}, + cfg=replace(cfg, axes={**cfg.axes, sg: limits}), + gripper_params=self._driver._gripper_params, + ) + if plan is None: + if closing: + return await self.sense_plate_gripped() + if self._hold_settings is not None: + await self._driver._stop_axes([sg]) + return False + registers = [(sg, index, 0) for index in (0x60F2, 0x6081, 0x6082, 0x6083, 0x6084, 0x6060)] + async with self._driver._preserve_settings(sdo=registers): + try: + await self._driver._start_joint_plan(plan) + if closing: + return await self._wait_for_contact(plan, width, start_width, already_gripped) + await self._wait_for_position(width, timeout=plan.move_time + 2) + return False + except BaseException: + await finish_io(asyncio.create_task(self._driver._stop_axes([sg]))) + raise + + async def move_to_jaw_position( + self, + width: float, + force_sensing: bool = True, + *, + max_force_percent: Optional[int] = None, + check_plate_gripped: bool = False, + max_speed: Optional[float] = None, + max_acceleration: Optional[float] = None, + ) -> None: + """Move the jaws in mm, retaining the closing target and current limit while gripping. + + ``max_force_percent`` is a percentage of motor current, not a force in newtons. + A confirmed grip holds that limit until the jaws open or the connection closes; + release restores the exact limits saved before the grip. Speed (mm/s) and + acceleration (mm/s²) apply only to the jaws and are restored after each move. + """ + cfg = self._driver.configuration.axes[Axis.SERVO_GRIPPER] + if not math.isfinite(width) or not cfg.min_travel <= width <= cfg.max_travel: + raise ValueError("Jaw width is outside the calibrated travel") + if max_force_percent is not None and not 10 <= max_force_percent <= 100: + raise ValueError("max_force_percent must be in 10..100") + for value in (max_speed, max_acceleration): + if value is not None and (not math.isfinite(value) or value <= 0): + raise ValueError("Jaw speed and acceleration must be finite and positive") + if not force_sensing and (check_plate_gripped or max_force_percent is not None): + raise ValueError("Current limiting and grip verification require force_sensing") + async with self._driver._motion_guard(): + current = await self.request_jaw_position() + closing = force_sensing and width < current + opening = width > current + holding = False + try: + if closing: + if self._hold_settings is None: + self._hold_settings = await self._driver._snapshot_registers( + floats=[(Axis.SERVO_GRIPPER, "PL", 1), (Axis.SERVO_GRIPPER, "CL", 1)] + ) + await self._driver._set_servo_gripper_force_limit( + 100 if max_force_percent is None else max_force_percent + ) + if opening: + # End the closing reference before increasing the current available for release. + await self._driver._stop_axes([Axis.SERVO_GRIPPER]) + async with self._driver._preserve_settings( + floats=[(Axis.SERVO_GRIPPER, "PL", 1), (Axis.SERVO_GRIPPER, "CL", 1)] + ): + await self._driver._set_servo_gripper_force_limit(100) + await self._move_jaws( + width, closing=False, max_speed=max_speed, max_acceleration=max_acceleration + ) + else: + holding = await self._move_jaws( + width, closing=closing, max_speed=max_speed, max_acceleration=max_acceleration + ) + if check_plate_gripped: + await self.check_plate_gripped() + finally: + if not holding: + await finish_io(asyncio.create_task(self._restore_current_limits())) diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py new file mode 100644 index 00000000000..ce9c70100ef --- /dev/null +++ b/pylabrobot/paa/kx2/kinematics.py @@ -0,0 +1,952 @@ +""" +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, measured from -Y at zero; 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, isfinite, pi, radians, sin, sqrt, trunc +from typing import Callable, Dict, List, Optional, Tuple + +from pylabrobot.paa.kx2 import motion as arm_kinematics +from pylabrobot.paa.kx2.config import Axis, GripperParams, KX2Config +from pylabrobot.paa.kx2.protocol import ( + JointMoveDirection, + MotorMoveParam, + MotorsMovePlan, +) +from pylabrobot.resources import Coordinate, Rotation + + +@dataclass(frozen=True) +class CartesianPose: + """Grip center in millimeters and gripper yaw in degrees about +Z.""" + + location: Coordinate + rotation: 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: + """Convert radial extension in mm to the elbow actuator angle.""" + 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: + """Convert elbow actuator angle to radial extension in mm.""" + 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: + """Wrap an angle into the requested interval.""" + 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_state`` (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_state``; 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 ( + not isfinite(max_gripper_speed) or 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 ( + not isfinite(max_gripper_acceleration) or 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.get(Axis.ELBOW, curr[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} + + def fk_loc(j: Dict[Axis, float]) -> Coordinate: + """Evaluate the calibrated grip center for speed estimation.""" + return fk(j, cfg, gripper_params).location + + capped: Dict[str, Dict[Axis, float]] = {"v": {}, "a": {}} + for kind, cap in ( + ("v", max_gripper_speed), + ("a", max_gripper_acceleration), + ): + 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: (cfg.axes[ax].max_vel if kind == "v" else cfg.axes[ax].max_accel) 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, + angular_speed: float = 30.0, + angular_acceleration: float = 100.0, +) -> 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). + angular_speed: maximum yaw speed in degrees per second. + angular_acceleration: maximum yaw acceleration in degrees per second squared. + + 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 not all( + isfinite(v) and v > 0 + for v in (vel_mm_per_s, accel_mm_per_s2, dt_s, angular_speed, angular_acceleration) + ): + 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) + + rot_delta = abs(((end_yaw - start_yaw + 540.0) % 360.0) - 180.0) + # Translation and yaw follow the same normalized progress, with independent units. + speeds, accelerations = [], [] + if length > _EPS: + speeds.append(vel_mm_per_s / length) + accelerations.append(accel_mm_per_s2 / length) + if rot_delta > _EPS: + speeds.append(angular_speed / rot_delta) + accelerations.append(angular_acceleration / rot_delta) + progress = ( + _arc_length_trapezoid_times(1.0, min(speeds), min(accelerations), dt_s) if speeds else [1.0] + ) + + prev_for_snap: Optional[Dict[Axis, float]] = ( + dict(current_joints) if current_joints is not None else None + ) + poses: List[Dict[Axis, float]] = [] + for alpha in progress: + 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}, {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]). + """Evaluate the Catmull-Rom segment at a local parameter.""" + 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 + result = 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) + ) + return result[0], result[1], result[2] + + # 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) + + # 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..89a305745fd --- /dev/null +++ b/pylabrobot/paa/kx2/kx2.py @@ -0,0 +1,3128 @@ +"""PAA KX2 arm control, calibrated kinematics, and optional serial barcode reader.""" + +from __future__ import annotations + +import asyncio +import contextlib +import logging +import math +import struct +import sys +import time +import warnings +from collections.abc import AsyncIterator +from contextlib import asynccontextmanager +from dataclasses import dataclass, field +from enum import IntEnum +from typing import Callable, Dict, List, Literal, Optional, Sequence, Tuple, Union + +from pylabrobot.io.canopen import CANopen, FrameCallback, finish_io +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.gripper import KX2Gripper +from pylabrobot.paa.kx2.kinematics import CartesianPose +from pylabrobot.paa.kx2.protocol import ( + _BI_REQUEST_COB_BASE, + _BI_RESPONSE_COB_BASE, + _EMCY_COB_BASE, + _GROUP_NODE_ID, + _TPDO3_COB_BASE, + RPDO, + TPDO, + CanError, + COBType, + EmcyFrame, + JointMoveDirection, + MotorMoveParam, + MotorsMovePlan, + PDOTransmissionType, + RPDOMappedObject, + TPDOMappedObject, + TPDOTrigger, + _decode_emcy, + _ElmoObjectDataType, + _NodeEmcyState, + _u32_le, +) +from pylabrobot.resources import Coordinate, Rotation +from pylabrobot.resources.barcode import Barcode + +logger = logging.getLogger(__name__) + + +@dataclass +class _Registers: + """Exact values to restore after an operation or connection lifecycle.""" + + sdo: Dict[Tuple[int, int, int], bytes] = field(default_factory=dict) + integers: Dict[Tuple[int, str, int], int] = field(default_factory=dict) + floats: Dict[Tuple[int, str, int], float] = field(default_factory=dict) + + +@dataclass +class _PDOSettings: + """A PDO's original communication fields and mapping entries.""" + + node: int + communication: int + mapping: int + fields: Dict[int, bytes] + entries: Dict[int, bytes] + event_mask: Optional[bytes] = None + + +class HomeStatus(IntEnum): + """Jaw calibration state stored in the gripper drive.""" + + 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 _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: + """Format calibrated motion limits with units for each axis.""" + 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", +} + + +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()``. + """ + + # 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 + + _PARK_SPEED: float = 80.0 + _PARK_ACCEL: float = 400.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 + + _parking_position: Optional[Dict[Axis, float]] + + def _validate_parking_position(self, position: Dict[Axis, float]) -> None: + """Require finite positioning-axis targets inside discovered travel limits.""" + if not position: + raise ValueError("parking_position must contain at least one arm axis") + for axis, value in position.items(): + if not isinstance(axis, Axis) or not axis.is_motion or not math.isfinite(value): + raise ValueError("parking_position requires motion Axis keys and finite values") + if self._config is not None: + cfg = self._config.axes[axis] + if not cfg.unlimited_travel and not cfg.min_travel <= value <= cfg.max_travel: + raise ValueError(f"Parking target for {axis.name} is outside its travel") + + @property + def parking_position(self) -> Optional[Dict[Axis, float]]: + """An explicit park target in this arm's calibrated joint coordinates.""" + return None if self._parking_position is None else dict(self._parking_position) + + @parking_position.setter + def parking_position(self, position: Optional[Dict[Axis, float]]) -> None: + """Store a validated, arm-specific parking target.""" + if position is not None: + self._validate_parking_position(position) + self._parking_position = None if position is None else dict(position) + + 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, + parking_position: Optional[Dict[Axis, float]] = None, + ) -> 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 adapter name. Defaults to SocketCAN on Linux + and PCAN on other platforms. + 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. + """ + if has_rail or not has_servo_gripper: + raise NotImplementedError("Only the four-axis KX2 with a servo gripper is supported") + if not all(math.isfinite(v) for v in (gripper_length, gripper_z_offset)) or gripper_length < 0: + raise ValueError("Tool offsets must be finite and gripper_length non-negative") + if gripper_finger_side not in ("barcode_reader", "proximity_sensor"): + raise ValueError("Unknown gripper_finger_side") + 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.io = CANopen(interface=interface, channel=channel, bitrate=bitrate) + self._lifecycle_lock = asyncio.Lock() + self._session_registers = _Registers() + self._session_pdos: List[_PDOSettings] = [] + self._session_configured = False + self._session_nmt: Dict[int, int] = {} + self._session_enabled: Dict[int, bool] = {} + self._faulted = False + self._stopping = False + self._parking_position = None + self._background_tasks: set[asyncio.Task] = set() + 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] = {} + + # Serialize requests on each drive's binary-interpreter channel. + self._bi_locks: Dict[int, asyncio.Lock] = {} + self._invalid_bi_keys: set[Tuple[int, str, int]] = set() + + self._ipm_mode: bool = False + self._ipm_axes: List[int] = [] + + # 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.parking_position = parking_position + self._freedrive_axes: Dict[int, bool] = {} + # 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 + self.gripper = KX2Gripper(self) + + # --- SDO ----------------------------------------------------------------- + + async def _can_sdo_upload(self, node_id: int, index: int, sub_index: int) -> bytes: + """Read the exact object bytes over the node's serialized SDO channel.""" + return await self.io.upload(node_id, index, sub_index) + + # --- 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 + + async def _send_bi( + self, + node_id: int, + cmd: str, + cmd_index: int, + *, + is_query: bool, + is_execute: bool, + is_float: bool, + value: Union[int, float] = 0, + ) -> 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 not self.io.connected: + 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(" List[Union[int, float]]: + """Drain the response before allowing another request with the same address.""" + try: + await self.io.write(_BI_REQUEST_COB_BASE + node_id, data_to_send) + return await asyncio.wait_for(asyncio.gather(*futures), timeout=timeout) + except BaseException: + self._invalid_bi_keys.update(keys) + self._faulted = True + raise + + try: + return await finish_io(asyncio.create_task(exchange())) + except asyncio.TimeoutError: + raise CanError(f"Timeout waiting for response to {cmd}[{cmd_index}] from node {node_id}") + finally: + # Defensive: clear pending futures on any exit (success, timeout, + # other exception). Without finally a non-Timeout exception could + # leave stale futures keyed on (nid, cmd, cmd_index) that the next + # caller would await indefinitely — the dispatch resolves only the + # most recent future at a given key. + for key in keys: + self._pending_bi.pop(key, None) + + async def _query_int(self, node_id: int, cmd: str, cmd_index: int) -> 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 _snapshot_registers( + self, + *, + sdo: Sequence[Tuple[int, int, int]] = (), + integers: Sequence[Tuple[int, str, int]] = (), + floats: Sequence[Tuple[int, str, int]] = (), + ) -> _Registers: + """Read all requested settings before the caller changes any of them.""" + saved = _Registers() + for sdo_key in sdo: + saved.sdo[sdo_key] = await self._can_sdo_upload(*sdo_key) + for key in integers: + saved.integers[key] = await self._query_int(*key) + for key in floats: + saved.floats[key] = await self._query_float(*key) + return saved + + async def _can_sdo_download( + self, node_id: int, index: int, sub_index: int, data: List[int] + ) -> None: + """Write an object and await acknowledgement.""" + await self.io.download(node_id, index, sub_index, bytes(data)) + + 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}, got {resp} from node {nid}" + ) + + async def _restore_registers(self, saved: _Registers) -> None: + """Restore exact values, trying all writes even when a prior write fails.""" + errors: List[BaseException] = [] + for key, value in reversed(tuple(saved.integers.items())): + try: + await self._write(*key, value) + except BaseException as exc: + errors.append(exc) + # CL must stay below PL throughout restoration, including a partially failed close. + currents = {nid for nid, cmd, index in saved.floats if cmd in ("CL", "PL")} + for nid in currents: + try: + cl, pl = saved.floats[(nid, "CL", 1)], saved.floats[(nid, "PL", 1)] + current_cl = await self._query_float(nid, "CL", 1) + await self._write(nid, "PL", 1, max(pl, cl, current_cl)) + await self._write(nid, "CL", 1, cl) + await self._write(nid, "PL", 1, pl) + except BaseException as exc: + errors.append(exc) + for key, float_value in saved.floats.items(): + if key[1] in ("CL", "PL"): + continue + try: + await self._write(*key, float_value) + except BaseException as exc: + errors.append(exc) + # Interpolation type determines the permitted record size (60C4:5). + sdo_items = sorted(saved.sdo.items(), key=lambda item: item[0][1] != 0x60C0) + for sdo_key, data in sdo_items: + try: + actual = await self._can_sdo_upload(*sdo_key) + # Zero is an unselectable power-on state. Preserve it while untouched; + # after motion, leave the drive in idle profile-position mode. + if sdo_key[1:] == (0x6060, 0) and data == b"\x00" and actual != data: + data = b"\x01" + if actual != data: + await self._can_sdo_download(*sdo_key, list(data)) + restored = await self._can_sdo_upload(*sdo_key) + if restored != data: + node, index, sub = sdo_key + raise CanError( + f"Node {node} did not restore 0x{index:04X}:{sub}: " + f"expected {data.hex()}, read {restored.hex()}" + ) + except BaseException as exc: + errors.append(exc) + if errors: + self._faulted = True + logger.error("KX2 could not restore register settings: %s", errors) + raise errors[0] + + @asynccontextmanager + async def _preserve_settings( + self, + *, + sdo: Sequence[Tuple[int, int, int]] = (), + integers: Sequence[Tuple[int, str, int]] = (), + floats: Sequence[Tuple[int, str, int]] = (), + ) -> AsyncIterator[None]: + """Restore temporary settings on success, failure, and cancellation.""" + saved = await self._snapshot_registers(sdo=sdo, integers=integers, floats=floats) + try: + yield + finally: + await finish_io(asyncio.ensure_future(self._restore_registers(saved))) + + async def _snapshot_pdo(self, node: int, number: int, *, transmit: bool) -> _PDOSettings: + """Read communication fields and all mapping entries that setup can overwrite.""" + communication = (0x1800 if transmit else 0x1400) + number - 1 + mapping = (0x1A00 if transmit else 0x1600) + number - 1 + fields = {} + for sub in (1, 2, 3, 5) if transmit else (1, 2): + fields[sub] = await self._can_sdo_upload(node, communication, sub) + count = await self._can_sdo_upload(node, mapping, 0) + entries = {0: count} + for sub in range( + 1, max(int.from_bytes(count, "little"), 2 if number == 3 and not transmit else 1) + 1 + ): + entries[sub] = await self._can_sdo_upload(node, mapping, sub) + event_mask = await self._can_sdo_upload(node, 0x2F20, number) if transmit else None + return _PDOSettings(node, communication, mapping, fields, entries, event_mask) + + def _has_session_pdo(self, node: int, number: int, *, transmit: bool) -> bool: + """Whether setup owns a reversible mapping for this PDO.""" + communication = (0x1800 if transmit else 0x1400) + number - 1 + return any( + pdo.node == node and pdo.communication == communication for pdo in self._session_pdos + ) + + async def _save_restorable_pdo(self, node: int, number: int, *, transmit: bool) -> None: + """Save active PDO configuration and writable descriptor contents.""" + saved = await self._snapshot_pdo(node, number, transmit=transmit) + self._session_pdos.append(saved) + + async def _restore_pdo(self, saved: _PDOSettings) -> None: + """Disable a PDO, restore its mapping and timing, then restore its original COB-ID.""" + cob = int.from_bytes(saved.fields[1], "little") + await self._can_sdo_download(saved.node, saved.communication, 1, _u32_le(cob | 0x80000000)) + await self._can_sdo_download(saved.node, saved.mapping, 0, [0]) + for sub, data in saved.entries.items(): + # Unused zero descriptors are not writable on Gold firmware. Restoring + # the original mapping count keeps these descriptors inactive. + if sub and any(data): + await self._can_sdo_download(saved.node, saved.mapping, sub, list(data)) + await self._can_sdo_download(saved.node, saved.mapping, 0, list(saved.entries[0])) + for sub, data in saved.fields.items(): + if sub != 1: + await self._can_sdo_download(saved.node, saved.communication, sub, list(data)) + if saved.event_mask is not None: + await self._can_sdo_download( + saved.node, 0x2F20, saved.communication - 0x1800 + 1, list(saved.event_mask) + ) + await self._can_sdo_download(saved.node, saved.communication, 1, list(saved.fields[1])) + + 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 _motor_get_motion_status(self, node_id: int) -> int: + """Read the drive motion-status code.""" + return await self._query_int(node_id, "MS", 0) + + async def _stop_axes(self, axes: List[int]) -> None: + """Stop without dropping a held load; confirm each axis has ceased motion.""" + errors: List[BaseException] = [] + for axis in axes: + try: + await self._execute(axis, "ST") + except BaseException as exc: + errors.append(exc) + try: + # A disabled drive may retain an enabled controlword, so do not replay it. + if await self.request_axis_enabled(axis): + controlword = int.from_bytes(await self._can_sdo_upload(axis, 0x6040, 0), "little") + await self._control_word_set(axis, (controlword | 0x100) & ~0x10) + except BaseException as exc: + errors.append(exc) + deadline = time.monotonic() + 3.0 + for axis in axes: + try: + reference: Optional[int] = None + settled_since: Optional[float] = None + while await self._motor_get_motion_status(axis) not in (0, 1, 3): + # A reverse-limit stop can leave MS=2 latched. Require a stationary + # position reference, zero profiler velocity and matching feedback. + desired = await self._query_int(axis, "DV", 3) + velocity = await self._query_int(axis, "DV", 5) + actual = await self._query_int(axis, "PX", 0) + tolerance = self._encoder_tolerance(Axis(axis)) if self._config is not None else 1 + if velocity == 0 and abs(actual - desired) <= tolerance: + if desired != reference or settled_since is None: + reference, settled_since = desired, time.monotonic() + elif time.monotonic() - settled_since >= 0.15: + break + else: + reference, settled_since = None, None + if time.monotonic() >= deadline: + raise CanError(f"Axis {axis} did not stop") + await asyncio.sleep(0.02) + except BaseException as exc: + errors.append(exc) + if errors: + self._faulted = True + raise errors[0] + + @property + def _cfg(self) -> KX2Config: + """Return discovered calibration or reject use before setup.""" + if self._config is None: + raise RuntimeError("KX2 not set up — call setup() first") + return self._config + + @property + def configuration(self) -> KX2Config: + """Calibration and limits read from this arm during setup.""" + return self._cfg + + @property + def barcode_reader(self) -> Optional[KX2BarcodeReader]: + """The independent serial reader, if fitted.""" + return self._bcr + + 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. + """Encode a vendor object value using its declared wire width.""" + _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) + + 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. + """Set modulo positioning direction for a rotary drive.""" + val = 1 + 64 * int(direction) + await self._can_sdo_download_elmo_object(node_id, 24818, 0, val, _ElmoObjectDataType.UNSIGNED16) + + # --- 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) + + @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 + + async def request_axis_fault(self, node_id: int) -> Optional[str]: + """Read and decode the drive motor-fault register.""" + 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_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. + """Check completion while surfacing pending motor faults.""" + 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 in (1, 3): + mo_val = await self._query_int(node_id, "MO", 0) + if mo_val == 1: + # MS=1 means the profiler stopped; MS=0 also confirms feedback settled. + return False + fault = await self.request_axis_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 _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. + """Poll axis completion and propagate drive faults.""" + assert self._loop is not None + loop = self._loop + + async def _poll_axis(nid: int) -> None: + """Poll a single axis until completion or its deadline.""" + 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: + if self._faulted: + raise + # 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 request_axis_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 + + # --- raw CANopen sends (SYNC + RPDO1 controlword) ----------------------- + + async def _can_sync(self) -> None: + """Send the synchronous PDO latch frame.""" + if not self.io.connected: + raise CanError("_can_sync called before setup()") + await self.io.write(0x80, b"") + + async def _control_word_set(self, node_id: int, value: int, sync: bool = True) -> None: + """Write the controlword through the mapped RPDO, or SDO for the gripper.""" + payload = value.to_bytes(2, byteorder="little") + if not self._has_session_pdo(node_id, 1, transmit=False): + self._statusword.pop(node_id, None) + await self.io.download(node_id, 0x6040, 0, payload) + return + await self.io.write(0x200 + node_id, payload) + if sync: + await self._can_sync() + + 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 + + # ====================================================================== + # Robot-level logic: homing, gripper, kinematics, motion + # ====================================================================== + + # -- robot-level homing / estop --------------------------------------- + + async def request_estop(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", 0) + return (r & 0x4000) == 0 or (r & 0x8000) == 0 + + 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: + fault = await self.request_axis_fault(node_id) + if self._faulted or self.emcy_move_error is not None or fault is not None: + raise CanError( + f"Cannot enable node {node_id} while faulted: {fault or self.emcy_move_error}" + ) + if await self.request_estop(): + raise CanError("Cannot enable a motor while e-stop is active") + if await self.request_axis_enabled(node_id): + return + + 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: + # Move through the enable states, confirming each statusword transition. + await self._control_word_set(node_id=node_id, value=0x00) # clear bits + 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. + 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. + if self._faulted or self._stopping: + raise CanError("KX2 stopped or faulted; reconnect before commanding motion") + for nid in node_ids: + nid_int = int(nid) + if await self.request_axis_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 and nid_int in self.motion_node_ids + ) + + 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 _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 _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. + """Enable participating drives and latch their absolute profile targets.""" + relative_bit = 0x40 if relative else 0 + cw_low = 47 + relative_bit + cw_high = 47 + 0x10 + relative_bit + # A disabled drive cannot acknowledge a setpoint. Faults prohibit enabling. + 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 _start_joint_plan(self, plan: MotorsMovePlan) -> None: + """Configure and start the profile; caller holds the motion guard and owns completion.""" + for nid in {move.node_id for move in plan.moves}: + await self._set_op_mode(nid, 1) + self._ipm_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, + ) + # A preceding JP jog can leave a nonzero end velocity: point moves must stop. + await self._can_sdo_download_elmo_object(nid, 0x6082, 0, 0, _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) + + async def _execute_joint_plan(self, plan: MotorsMovePlan) -> None: + """Run a profile to completion while the caller holds the motion guard.""" + await self._start_joint_plan(plan) + await self._wait_for_moves_done( + [move.node_id for move in plan.moves], timeout=plan.move_time + 2 + ) + + async def _motors_move_absolute_execute_locked(self, plan: MotorsMovePlan) -> None: + """Execute a plan with scoped velocity, acceleration, direction and operation mode.""" + axes = [move.node_id for move in plan.moves] + registers = [ + (nid, index, 0) for nid in axes for index in (0x60F2, 0x6081, 0x6082, 0x6083, 0x6084) + ] + registers += [(nid, 0x6060, 0) for nid in dict.fromkeys(axes)] + async with self._preserve_settings(sdo=registers): + try: + await self._execute_joint_plan(plan) + except BaseException: + await finish_io(asyncio.ensure_future(self._stop_axes(axes))) + raise + + async def _motor_read_position_raw(self, node_id: int, pu: bool = False) -> int: + """Read signed encoder counts from the drive.""" + cmd = "PU" if pu else "PX" + return await self._query_int(node_id, cmd, 0) + + 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_select_mode(self, enable: bool, node_ids: Optional[List[int]] = None) -> None: + """Enable/disable interpolation on the participating motion axes. + + 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: + ids = list(self.motion_node_ids if node_ids is None else node_ids) + if any( + not self._has_session_pdo(nid, number, transmit=False) for nid in ids for number in (1, 3) + ): + raise CanError( + "Linear motion requires initialized RPDO1/RPDO3 mappings that can be restored" + ) + self._ipm_mode = True + self._ipm_axes = ids + # 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 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 ids: + await self._set_op_mode(nid, 1) + await self._ipm_clear_queue(nid) + await self._set_op_mode(nid, 7) + # Clearing disables buffer access; re-enable it before loading points. + await self._can_sdo_download(nid, 0x60C4, 0x06, [1]) + # 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._ipm_axes: + 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 + self._ipm_axes = [] + + async def _ipm_set_time_interval(self, ms: int, node_ids: Optional[List[int]] = None) -> None: + """Program 0x60C2:01 = ms on participating axes. 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 if node_ids is None else node_ids: + await self._can_sdo_download_elmo_object( + nid, + 24770, + 1, + int(ms), + _ElmoObjectDataType.UNSIGNED8, + ) + + async def _ipm_send_pvt_point( + self, + node_id: int, + position_enc: int, + velocity_enc_per_s: int, + ) -> None: + """Append a position/velocity record through 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) + + RPDO3 is event driven; the caller paces records to match the interpolation period. + """ + if not self.io.connected: + 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) + await self.io.write(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. + + Queue errors collected during preload remain available to the runtime.""" + ids = [int(n) for n in node_ids] + if not ids: + return + await self._motors_ensure_enabled(ids) + 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], *, allow_empty: bool = False) -> 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 and not allow_empty: + bad.append(f"Axis {nid} underflow") + if bad: + raise CanError("IPM queue fault: " + "; ".join(bad)) + + async def _finish_stream(self, active_axes: List[int]) -> None: + """Try both streaming cleanup steps and surface any failure.""" + try: + await self._ipm_stop(active_axes) + finally: + await self._ipm_select_mode(False) + + def _encoder_tolerance(self, axis: Axis) -> int: + """Allow 0.01 drive units of settling error, or one encoder count.""" + return max(1, round(abs(self._cfg.axes[axis].motor_conversion_factor) * 0.01)) + + async def _check_encoder_targets(self, targets: Dict[Axis, int]) -> None: + """Reject a settled move that did not reach its absolute encoder targets.""" + for axis, target in targets.items(): + actual = await self._motor_read_position_raw(axis) + if abs(actual - target) > self._encoder_tolerance(axis): + raise CanError( + f"{axis.name} did not reach its target: expected {target}, measured {actual} counts" + ) + + async def _wait_for_encoder_targets(self, targets: Dict[Axis, int], timeout: float = 3.0) -> None: + """Keep the final position reference active until feedback reaches the endpoint.""" + deadline = time.monotonic() + timeout + while True: + try: + await self._check_encoder_targets(targets) + return + except CanError: + if time.monotonic() >= deadline: + raise + await asyncio.sleep(0.02) + + async def _stream_samples_unscoped(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 + + 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): + active_axes.append(nid) + + # Only complete revolutions are equivalent absolute rotary positions. + # A stale start position must not translate the requested path or endpoint. + offsets: Dict[Axis, int] = {} + for nid in all_axes: + axis = Axis(nid) + actual = await self._motor_read_position_raw(nid) + planned = samples[0].encoder_position[axis] + cfg = self._cfg.axes[axis] + offset = 0 + if axis in (Axis.SHOULDER, Axis.WRIST) and cfg.unlimited_travel: + revolution = cfg.motor_conversion_factor * 360 + offset = round(round((actual - planned) / revolution) * revolution) + if abs(actual - planned - offset) > self._encoder_tolerance(axis): + raise ValueError(f"Streamed path does not start at the measured {axis.name} position") + offsets[axis] = offset + if not active_axes: + return + await self._motors_ensure_enabled(active_axes) + + interval = self._PVT_DT_MS + dt_s = interval / 1000.0 + preload = min(self._PVT_PRELOAD, len(samples)) + + # The interpolation period is latched when the drive enters mode 7. + for nid in active_axes: + state = self._emcy.get(nid) + if state is not None: + state.queue_full = False + state.underflow = False + await self._ipm_set_time_interval(interval, active_axes) + await self._ipm_select_mode(True, active_axes) + try: + # 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) + await self._ipm_send_pvt_point( + nid, + samples[i].encoder_position[ax] + offsets[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) + await self._ipm_send_pvt_point( + nid, + samples[i].encoder_position[ax] + offsets[ax], + samples[i].encoder_velocity[ax], + ) + self._ipm_check_queue_fault(active_axes) + + # Let the terminal zero-velocity point settle before disabling interpolation. + await self._wait_for_moves_done(active_axes, timeout=3.0) + # An exhausted FIFO disables interpolation and reports underflow even at + # the terminal zero-velocity point. Only accept it after feeding the whole + # path, waiting for settled feedback, and verifying every endpoint below. + self._ipm_check_queue_fault(active_axes, allow_empty=True) + await self._wait_for_encoder_targets( + { + Axis(nid): samples[-1].encoder_position[Axis(nid)] + offsets[Axis(nid)] + for nid in all_axes + } + ) + await self._ipm_stop(active_axes) + finally: + await finish_io(asyncio.ensure_future(self._finish_stream(active_axes))) + + async def _stream_samples(self, samples: List[kinematics.LinearPathSample]) -> None: + """Stream with the caller's operation modes and interpolation interval restored afterwards.""" + registers = [ + (nid, index, sub) + for nid in self.motion_node_ids + for index, sub in ( + (0x6060, 0), + (0x60C2, 1), + (0x60C2, 2), + ) + ] + async with self._preserve_settings(sdo=registers): + try: + await self._stream_samples_unscoped(samples) + except BaseException: + await finish_io(asyncio.ensure_future(self._stop_axes(list(MOTION_AXES)))) + raise + + @property + def setup_finished(self) -> bool: + """Whether connection and calibration discovery completed.""" + return self._setup_finished + + async def _bus_stop(self) -> None: + """Restore configuration before disconnecting, without re-enabling a faulted motor.""" + errors: List[BaseException] = [] + for task in tuple(self._background_tasks): + try: + await task + except BaseException as exc: + errors.append(exc) + if self.io.connected: + for nid, enabled in self._session_enabled.items(): + try: + current_enabled = await self.request_axis_enabled(nid) + if current_enabled != enabled and (not enabled or not self._faulted): + if enabled and await self.request_estop(): + raise CanError("Cannot restore enabled motor while e-stop is active") + await self._motor_enable(nid, enabled, use_ds402=False) + except BaseException as exc: + errors.append(exc) + try: + if self.gripper._hold_settings is not None: + await self._stop_axes([Axis.SERVO_GRIPPER]) + await self.gripper._restore_current_limits() + except BaseException as exc: + errors.append(exc) + try: + await self._restore_registers(self._session_registers) + except BaseException as exc: + errors.append(exc) + for pdo in reversed(self._session_pdos if self._session_configured else []): + try: + await self._restore_pdo(pdo) + except BaseException as exc: + errors.append(exc) + for nid, state in self._session_nmt.items(): + try: + if state != 5: + await self.io.set_nmt_state(nid, state) + except BaseException as exc: + errors.append(exc) + self._loop = None + for future in self._pending_bi.values(): + if not future.done(): + future.set_exception(CanError("KX2 connection closed")) + try: + await self.io.stop() + finally: + self._pending_bi.clear() + self._bi_locks.clear() + self._invalid_bi_keys.clear() + self._emcy.clear() + self._last_emcy_node_id = None + self._emcy_callbacks.clear() + self._statusword.clear() + self._statusword_event.clear() + self._session_nmt.clear() + self._session_enabled.clear() + self._session_pdos.clear() + self._session_configured = False + self._session_registers = _Registers() + self._ipm_mode = False + self._ipm_axes = [] + if errors: + logger.error("KX2 restoration failed: %s", errors) + raise errors[0] + + async def _stop_session(self) -> None: + """Try every cleanup step, retaining the first failure for the caller.""" + error: Optional[BaseException] = None + try: + await self._bus_stop() + except BaseException as exc: + error = exc + try: + if self._bcr is not None: + await self._bcr.stop() + except BaseException as exc: + error = error or exc + finally: + self._setup_finished = False + self._config = None + self._freedrive_axes.clear() + self._stopping = False + if error is not None: + raise error + + 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_axis_config(self, nid: int) -> AxisConfig: + """Read one axis calibration, travel limits and I/O assignments.""" + 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_servo_gripper_config(self) -> ServoGripperConfig: + """Read homing and current settings for the servo gripper.""" + 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), + ) + + 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 _arm_setup(self) -> None: + """Read the arm's calibration without enabling or moving it.""" + self._config = await self._read_config() + if self.parking_position is not None: + self._validate_parking_position(self.parking_position) + + # --- DS402 / motor control ---------------------------------------------- + + async def _motor_emergency_stop(self, node_id: int) -> None: + """Disable one motor immediately through the binary interpreter.""" + await self._write(node_id, "MO", 0, 0) + + # --- EMCY auto-disable --------------------------------------------------- + + def _on_emcy( + self, node_id: int, frame: EmcyFrame, description: str, disable_motors: bool + ) -> None: + """Latch fatal faults and stop positioning motors without acquiring the motion lock.""" + logger.error("KX2 EMCY axis=%d code=0x%04X: %s", node_id, frame.err_code, description) + if not disable_motors: + return + self._faulted = True + owner = self._motion_owner + if owner is not None: + owner.cancel() + + async def disable_all() -> None: + """Attempt every positioning motor even if one node is unavailable.""" + for axis in MOTION_AXES: + try: + await self._motor_emergency_stop(axis) + except Exception: + logger.exception("EMCY auto-disable failed for axis %s", axis.name) + + task = self.loop.create_task(disable_all()) + self._background_tasks.add(task) + task.add_done_callback(self._background_tasks.discard) + + # ====================================================================== + # 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: + """Disable a transmit PDO and clear its mapping.""" + 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]) + + async def _rpdo_map( + self, + rpdo: RPDO, + node_id: int, + mapped_objects: List[RPDOMappedObject], + transmission_type: PDOTransmissionType, + ) -> None: + """Configure the objects received by a drive in one PDO.""" + 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]) + # Transmission type + await self._can_sdo_download(node_id, 0x1400 + rpdo_idx, 2, [int(transmission_type)]) + # 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)]) + # 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: + """Configure the objects and events transmitted by a drive.""" + 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]) + # Transmission type + await self._can_sdo_download(node_id, 0x1800 + tpdo_idx, 2, [int(transmission_type)]) + # Inhibit / delay 100us + await self._can_sdo_download( + node_id, 0x1800 + tpdo_idx, 3, list(delay_100_us.to_bytes(2, "little")) + ) + # Event timer (ms) + await self._can_sdo_download( + node_id, 0x1800 + tpdo_idx, 5, list(event_timer_ms.to_bytes(2, "little")) + ) + # 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)]) + # Re-enable (clear bits 30 + 31) + await self._can_sdo_download(node_id, 0x1800 + tpdo_idx, 1, _u32_le(cob_id_11)) + + # --- 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. + """ + if cb not in self._emcy_callbacks: + self._emcy_callbacks.append(cb) + + def _dispatch_emcy(self, node_id: int, data: bytes) -> None: + """Decode an emergency frame and notify registered fault observers.""" + 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(" FrameCallback: + """Return a `canopen.Network.subscribe` callback bound to a specific node.""" + + def _cb(cob_id: int, data: bytes, timestamp: float) -> None: + # PLR CANopen has already delivered this frame to the owning loop. + """Deliver a received frame on the owning event loop.""" + if self._loop is None: + return + self._dispatch_emcy(node_id, bytes(data)) + + return _cb + + def _dispatch_statusword(self, node_id: int, sw: int) -> None: + """Publish a received statusword and wake transition waiters.""" + self._statusword[node_id] = sw + ev = self._statusword_event.get(node_id) + if ev is not None: + ev.set() + + def _make_tpdo3_callback(self, node_id: int) -> FrameCallback: + """Return a callback that decodes TPDO3 (StatusWord) and signals waiters.""" + + def _cb(cob_id: int, data: bytes, timestamp: float) -> None: + """Deliver a received frame on the owning event loop.""" + 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._dispatch_statusword(node_id, sw) + + return _cb + + def _dispatch_bi_response(self, node_id: int, data: bytes) -> None: + """Decode a typed interpreter reply and resolve its pending request.""" + 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 = " FrameCallback: + """Return a `canopen.Network.subscribe` callback bound to a specific node.""" + + def _cb(cob_id: int, data: bytes, timestamp: float) -> None: + # PLR CANopen has already delivered this frame to the owning loop. + """Deliver a received frame on the owning event loop.""" + if self._loop is None: + return + self._dispatch_bi_response(node_id, bytes(data)) + + return _cb + + async def _bus_setup(self) -> None: + """Connect, save node state, and configure reversible PDO mappings.""" + self._loop = asyncio.get_running_loop() + await self.io.setup(self.node_id_list) + self.add_emcy_callback(self._on_emcy) + for nid in self.node_id_list: + self._emcy[nid] = _NodeEmcyState() + self.io.subscribe(_EMCY_COB_BASE + nid, self._make_emcy_callback(nid)) + self.io.subscribe(_BI_RESPONSE_COB_BASE + nid, self._make_bi_callback(nid)) + state = await self.io.request_nmt_state(nid) + if state not in (4, 5, 127): + raise CanError(f"Node {nid} has unsupported NMT state {state}") + self._session_nmt[nid] = state + if state != 5: + await self.io.set_nmt_state(nid, 5) + # Gold drives report 3 when disabled, with no active motion profiler. + if await self._motor_get_motion_status(nid) not in (0, 1, 3): + raise CanError(f"Node {nid} is moving; setup will not interrupt it") + self._session_enabled[nid] = await self.request_axis_enabled(nid) + + # Snapshot every changed setting before writing any mappings. + for nid in self.node_id_list: + for number in (1, 3, 4): + await self._save_restorable_pdo(nid, number, transmit=True) + for nid in self.motion_node_ids: + for number in (1, 3): + await self._save_restorable_pdo(nid, number, transmit=False) + self._session_registers = await self._snapshot_registers( + sdo=[ + (nid, index, sub) + for nid in self.motion_node_ids + for index, sub in ( + (0x60C0, 0), + (0x60C4, 2), + (0x60C4, 3), + (0x60C4, 5), + (0x60C2, 2), + (0x605D, 0), + (0x6060, 0), + ) + ], + ) + + self._session_configured = True + # 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. + for nid in self.motion_node_ids: + if not self._has_session_pdo(nid, 3, transmit=True): + continue + self._statusword_event[nid] = asyncio.Event() + self.io.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: + if self._has_session_pdo(node_id, 1, transmit=True): + await self._can_tpdo_unmap(TPDO.TPDO1, node_id) + if self._has_session_pdo(node_id, 3, transmit=True): + await self._tpdo_map( + TPDO.TPDO3, + node_id, + [TPDOMappedObject.StatusWord], + TPDOTrigger.StatusWordEvent, + delay_100_us=10, + ) + if self._has_session_pdo(node_id, 4, transmit=True): + 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) + + # Map synchronous controlwords and event-driven interpolation records. + for nid in self.motion_node_ids: + if self._has_session_pdo(nid, 1, transmit=False): + await self._rpdo_map( + RPDO.RPDO1, nid, [RPDOMappedObject.ControlWord], PDOTransmissionType.SynchronousCyclic + ) + if self._has_session_pdo(nid, 3, transmit=False): + await self._rpdo_map( + RPDO.RPDO3, + nid, + [RPDOMappedObject.TargetPositionIP, RPDOMappedObject.TargetVelocityIP], + PDOTransmissionType.EventDrivenDev, + ) + + # --- lifecycle ----------------------------------------------------------- + + async def setup(self, skip_home: bool = True) -> None: + """Connect and read calibration. Homing is explicit unless ``skip_home=False``. + + Setup does not reset the network, enable motors, or close the jaws. The optional + gripper homing moves hardware and updates its encoder calibration. + """ + async with self._lifecycle_lock: + if self._setup_finished: + return + self._faulted = False + self._stopping = False + try: + if self._bcr is not None: + await self._bcr.setup() + await self._bus_setup() + await self._arm_setup() + self._setup_finished = True + if not skip_home: + await self.gripper.home() + except BaseException: + await finish_io(asyncio.ensure_future(self._stop_session())) + raise + logger.info( + "KX2 connected on %s:%s; axes=%s", self._interface, self._channel, self.node_id_list + ) + + async def _join_motion(self, task: asyncio.Task) -> None: + """Join cancelled work before its transports are closed.""" + try: + await task + except asyncio.CancelledError: + pass + + async def stop(self) -> None: + """Finish active work, restore session settings, and close both transports. + + Gold firmware cannot restore unused mapping descriptors to zero; their original + mapping counts and communication settings are restored, leaving them inactive. + An untouched power-on mode 0 is preserved. After motion, it becomes idle profile + position mode 1 because mode 0 cannot be selected through the drive protocol. + """ + async with self._lifecycle_lock: + self._stopping = True + try: + owner = self._motion_owner + if owner is not None and owner is not asyncio.current_task(): + owner.cancel() + await finish_io(asyncio.ensure_future(self._join_motion(owner))) + finally: + await finish_io(asyncio.ensure_future(self._stop_session())) + + async def scan_barcode(self, read_time: Optional[float] = None) -> Optional[Barcode]: + """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) -> AsyncIterator[None]: + """Serialize complete operations and allow their same-task suboperations.""" + current = asyncio.current_task() + if self._motion_owner is current: + yield + return + async with self._motion_lock: + if self._stopping or self._faulted: + raise CanError("KX2 is stopped or faulted; reconnect after resolving the fault") + self._motion_owner = current + try: + yield + finally: + self._motion_owner = 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 + + 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 + + async def request_drive_version(self, node_id: int) -> str: + """Read the standard firmware-version object without changing interpreter settings.""" + async with self._lifecycle_lock: + reply = await self.io.upload(node_id, 0x100A, 0) + return reply.decode("ascii", errors="strict").rstrip("\x00").rstrip() + + 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: + """Wait for one drive to report its target reached.""" + 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)) + + # --- I/O ----------------------------------------------------------------- + + async def _read_digital_input(self, node_id: int, input_num: int) -> bool: + """Read one drive input bit.""" + return await self._query_int(node_id, "IB", input_num) == 1 + + async def request_output(self, node_id: int, output_num: int) -> bool: + """Read the current state of an output bit.""" + 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: + """Set an output to the requested state.""" + 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: + """Stop one drive without changing its operation mode.""" + await self._stop_axes([node_id]) + + async def _read_input_logic(self, node_id: int, input_num: int) -> int: + """Read the action assigned to an input.""" + 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). + + ``logic`` is the drive's active-low IL code; ``logic_high`` selects high polarity. + Skips the write if the 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) + + async def _gripper_get_homed_status(self) -> HomeStatus: + """Read the jaw calibration state from the drive.""" + return HomeStatus(await self._query_int(Axis.SERVO_GRIPPER, "UI", 3)) + + async def _gripper_set_homed_status(self, status: HomeStatus) -> None: + """Store the jaw calibration state.""" + await self._write(Axis.SERVO_GRIPPER, "UI", 3, int(status)) + + async def _gripper_reset_encoder_position(self, position: float) -> None: + """Set the calibrated encoder datum using the homing latch.""" + 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) + # The encoder datum is a signed integer count. + 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: + """Jog to the hard stop, observing position error and stopping on the host.""" + 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 index in (1, 2, 3, 4, 5): + await self._write(sg, "HM", index, 0) + # Gold JV uses velocity control, where PE is always zero. JP retains position control. + await self._write(sg, "JP", 0, srch_vel) + try: + await self._execute(sg, "BG") + deadline = time.monotonic() + timeout + contact_since: Optional[float] = None + contact_position = 0 + while True: + fault = await self.request_axis_fault(sg) + if fault is not None: + raise CanError(fault) + position = await self._motor_read_position_raw(sg) + if abs(await self._query_int(sg, "PE", 0)) >= abs(hs_pe): + if contact_since is None or abs(position - contact_position) > max(1, abs(hs_pe) // 10): + contact_since, contact_position = time.monotonic(), position + elif time.monotonic() - contact_since >= 0.1: + break + else: + contact_since = None + if time.monotonic() >= deadline: + raise TimeoutError("Gripper did not reach its hard stop") + await asyncio.sleep(0.01) + finally: + await finish_io(asyncio.ensure_future(self._stop_axes([sg]))) + await self._can_sdo_download(sg, 0x6082, 0, [0, 0, 0, 0]) + # The following absolute backoff is relative to this hard-stop datum. + await self._gripper_reset_encoder_position(0) + + async def _gripper_index_search( + self, + srch_vel: int, + srch_acc: int, + positive_direction: bool, + timeout: float, + ) -> tuple: + """Search for the encoder index and return its captured position.""" + 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.request_axis_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.Normal, + ) + ] + ) + ) + + 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.Normal, + ) + ] + ) + ) + await self._gripper_reset_encoder_position(sgc.home_pos) + await self._gripper_set_homed_status(HomeStatus.Homed) + + 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) + + # -- servo gripper ------------------------------------------------------ + + async def _home_gripper(self) -> None: + """Calibrate the jaw encoder against the hard stop and index; restore temporary settings.""" + sg = Axis.SERVO_GRIPPER + sgc = self._cfg.servo_gripper + if sgc is None: + raise RuntimeError("Servo gripper not present") + async with self._motion_guard(): + if await self._query_int(sg, "HM", 1) != 0: + raise CanError("Another gripper homing sequence is armed") + integer_keys = [ + (sg, cmd, index) + for cmd, index in ( + ("ER", 3), + ("AC", 0), + ("DC", 0), + ("JP", 0), + ("PR", 1), + ("HM", 1), + ("HM", 2), + ("HM", 3), + ("HM", 4), + ("HM", 5), + ) + ] + async with self._preserve_settings( + sdo=[(sg, 0x6081, 0), (sg, 0x6082, 0), (sg, 0x6060, 0)], + integers=integer_keys, + floats=[(sg, "PL", 1), (sg, "CL", 1)], + ): + await self._gripper_set_homed_status(HomeStatus.NotHomed) + await self._motor_enable(sg, True, use_ds402=False) + await self._set_servo_gripper_force_limit(100) + try: + await self._home_servo_gripper(sgc) + finally: + await finish_io(asyncio.ensure_future(self._stop_axes([sg]))) + # Reinitialize the position reference after restoring interpreter settings + # so subsequent CANopen targets use the calibrated encoder datum. + await self._motor_enable(sg, False, use_ds402=False) + await self._motor_enable(sg, True, use_ds402=False) + logger.info("KX2 gripper homed") + + 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 request_axis_position(self, axis: Axis) -> float: + """Read one calibrated axis position in millimeters or degrees.""" + 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 request_input(self, axis: Axis, input_num: int) -> bool: + """Read the assigned digital input on an axis.""" + return await self._read_digital_input(node_id=axis, input_num=0x10 + input_num) + + async def sense_proximity(self) -> bool: + """Read the interrupted-beam sensor between the gripper fingers.""" + return await self.request_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.sense_proximity() == state: + return True + if time.monotonic() >= deadline: + return False + await asyncio.sleep(poll) + + async def _finish_search(self, task: Optional[asyncio.Task], axes: List[int]) -> None: + """Join the motion producer before stopping its axes and releasing temporary settings.""" + try: + if task is not None: + if not task.done(): + task.cancel() + try: + await task + except asyncio.CancelledError: + pass + finally: + await self._stop_axes(axes) + + async def request_joint_state(self) -> Dict[Axis, float]: + """Read all calibrated axis positions in millimeters or degrees.""" + # 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.request_axis_position(ax) for ax in axes)) + return dict(zip(axes, positions)) + + 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_state()).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 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: + """Descend between absolute Z bounds, stopping on the beam and restoring its input logic.""" + if not math.isfinite(z_start) or not math.isfinite(z_end) or z_end >= z_start: + raise ValueError("z_end must be below finite z_start") + async with self._motion_guard(): + 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.sense_proximity(): + return await self.request_axis_position(Axis.Z) + async with self._preserve_settings( + integers=[(self._PROXIMITY_SENSOR_AXIS, "IL", self._PROXIMITY_SENSOR_INPUT)] + ): + task: Optional[asyncio.Task] = None + tripped = False + try: + await self._configure_input_logic( + # Active-low reverse limit: stop descent while allowing upward withdrawal. + self._PROXIMITY_SENSOR_AXIS, + self._PROXIMITY_SENSOR_INPUT, + 8, + ) + 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, + ) + ) + while not task.done(): + if await self.sense_proximity(): + tripped = True + break + await asyncio.sleep(0.01) + if task.done(): + await task + tripped = tripped or await self.sense_proximity() + finally: + await finish_io(asyncio.ensure_future(self._finish_search(task, [Axis.Z]))) + if not tripped: + raise RuntimeError(f"Proximity sensor never tripped on Z {z_start} to {z_end}") + return await self.request_axis_position(Axis.Z) + + async def _run_linear_path( + self, + target_pose: CartesianPose, + *, + max_gripper_speed: Optional[float], + max_gripper_acceleration: Optional[float], + max_gripper_angular_speed: float = 30.0, + max_gripper_angular_acceleration: float = 100.0, + ) -> 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_state()).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, + angular_speed=max_gripper_angular_speed, + angular_acceleration=max_gripper_angular_acceleration, + ) + await self._stream_samples(samples) + + async def request_gripper_pose(self) -> CartesianPose: + """Compute the grip-center pose from measured calibrated joints.""" + joints = {Axis(k): v for k, v in (await self.request_joint_state()).items()} + return kinematics.fk(joints, self._cfg, self._gripper_params) + + 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_state()).items()} + ik_joints = kinematics.ik(pose, self._cfg, self._gripper_params) + return kinematics.snap_to_current(ik_joints, current) + + 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", + max_gripper_angular_speed: Optional[float] = None, + max_gripper_angular_acceleration: Optional[float] = None, + ) -> 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). + Yaw is independently limited by ``max_gripper_angular_speed`` (deg/s, + default 30) and ``max_gripper_angular_acceleration`` (deg/s², default 100). + Rotation-only moves hold the grip center fixed. Angular limits apply + only to ``path="linear"``. + """ + if path not in ("joint", "linear") or not all( + math.isfinite(v) for v in (location.x, location.y, location.z, direction) + ): + raise ValueError("A finite Cartesian target and path='joint' or 'linear' are required") + if path == "joint" and ( + max_gripper_angular_speed is not None or max_gripper_angular_acceleration is not None + ): + raise ValueError("Angular limits require path='linear'") + 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, + max_gripper_angular_speed=( + 30.0 if max_gripper_angular_speed is None else max_gripper_angular_speed + ), + max_gripper_angular_acceleration=( + 100.0 if max_gripper_angular_acceleration is None else max_gripper_angular_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 find_with_proximity_sensor( + self, + start: Coordinate, + end: Coordinate, + *, + max_gripper_speed: float = 25.0, + max_gripper_acceleration: float = 100.0, + ) -> Coordinate: + """Sweep a straight Cartesian segment until the beam trips; retain the measured yaw. + + Sensor polling and adapter latency affect stopping distance. Use the Z search when the + drive's local input stop is needed. No unrelated fault is cleared after a search. + """ + async with self._motion_guard(): + yaw = (await self.request_gripper_pose()).rotation.z + await self.move_to_location( + start, + yaw, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + if await self.sense_proximity(): + return (await self.request_gripper_pose()).location + task = asyncio.create_task( + self._run_linear_path( + CartesianPose(end, Rotation(z=yaw)), + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + ) + tripped = False + try: + while not task.done(): + if await self.sense_proximity(): + tripped = True + break + await asyncio.sleep(0.01) + if task.done(): + await task + tripped = tripped or await self.sense_proximity() + finally: + await finish_io(asyncio.ensure_future(self._finish_search(task, list(MOTION_AXES)))) + if not tripped: + raise RuntimeError("Proximity sensor never tripped along the Cartesian search") + 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: + """Serialize and execute absolute calibrated joint targets.""" + 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_absolute_execute(self, plan: MotorsMovePlan) -> None: + """Serialize and execute a prepared encoder-space motion plan.""" + async with self._motion_guard(): + await self._motors_move_absolute_execute_locked(plan) + + async def move_parametric( + self, + path_fn: "Callable[[float], CartesianPose]", + duration: float, + ) -> None: + """Stream a parametric Cartesian trajectory through IPM. + + ``path_fn(t)`` is called at every IPM sample time ``t ∈ [0, duration]`` + (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 + 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=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 not math.isfinite(duration) or duration <= 0: + raise ValueError(f"duration must be > 0, got {duration}") + async with self._motion_guard(): + current_joints = {Axis(k): v for k, v in (await self.request_joint_state()).items()} + samples = kinematics.sample_parametric_path( + cfg=self._cfg, + gripper_params=self._gripper_params, + path_fn=path_fn, + duration_s=duration, + 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, + acceleration: 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 ``acceleration`` (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. + acceleration: peak Cartesian acceleration, mm/s². + + Raises: + ValueError if waypoints < 2 or speed/acceleration <= 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 not all(math.isfinite(v) and v > 0 for v in (speed, acceleration)): + raise ValueError(f"speed and accel must be > 0, got {speed}, {acceleration}") + async with self._motion_guard(): + current_joints = {Axis(k): v for k, v in (await self.request_joint_state()).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=acceleration, + 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: + """Cancel active motion and stop the positioning axes, preserving gripper holding power.""" + self._faulted = True + try: + owner = self._motion_owner + if owner is not None and owner is not asyncio.current_task(): + owner.cancel() + await finish_io(asyncio.ensure_future(self._join_motion(owner))) + finally: + await finish_io(asyncio.ensure_future(self._stop_axes(list(MOTION_AXES)))) + + async def move_to_joint_state( + self, + joint_state: Dict[Axis, float], + *, + max_gripper_speed: Optional[float] = None, + max_gripper_acceleration: Optional[float] = None, + close_gripper_without_force_sensing: bool = False, + ) -> None: + """Move absolute joint targets; omitted axes stay at their measured positions. + + Z and elbow use millimeters; shoulder and wrist use degrees. A jaw-closing target + requires an explicit override because coordinated joint moves do not sense contact. + """ + if any( + not isinstance(axis, Axis) or axis == Axis.RAIL or not math.isfinite(value) + for axis, value in joint_state.items() + ): + raise ValueError("joint_state requires Axis keys and finite values") + async with self._motion_guard(): + current = await self.request_joint_state() + target = {**current, **joint_state} + if ( + not close_gripper_without_force_sensing + and target[Axis.SERVO_GRIPPER] < current[Axis.SERVO_GRIPPER] + ): + raise ValueError( + "Close the jaws using gripper.move_to_jaw_position, or explicitly allow a position close" + ) + await self._motors_move_joint_locked( + joint_state, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + + async def park(self) -> None: + """Move to the configured park target, leaving the gripper unchanged.""" + target = self.parking_position + if target is None: + raise ValueError("Set parking_position to a calibrated, clear pose before calling park()") + await self.move_to_joint_state( + target, max_gripper_speed=self._PARK_SPEED, max_gripper_acceleration=self._PARK_ACCEL + ) + + 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, + max_force_percent: Optional[int] = None, + max_jaw_speed: Optional[float] = None, + max_jaw_acceleration: Optional[float] = None, + ) -> None: + """Move to the Cartesian target and grip using the supplied jaw width.""" + 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.gripper.move_to_jaw_position( + resource_width, + max_force_percent=max_force_percent, + check_plate_gripped=check_plate_gripped, + max_speed=max_jaw_speed, + max_acceleration=max_jaw_acceleration, + ) + + 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, + max_jaw_speed: Optional[float] = None, + max_jaw_acceleration: Optional[float] = None, + ) -> None: + """Move to the Cartesian target and release to the supplied jaw width.""" + 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.gripper.move_to_jaw_position( + resource_width, + force_sensing=False, + max_speed=max_jaw_speed, + max_acceleration=max_jaw_acceleration, + ) + + async def pick_up_at_joint_state( + self, + joint_state: Dict[Axis, float], + resource_width: float, + *, + max_gripper_speed: Optional[float] = None, + max_gripper_acceleration: Optional[float] = None, + check_plate_gripped: bool = True, + max_force_percent: Optional[int] = None, + max_jaw_speed: Optional[float] = None, + max_jaw_acceleration: Optional[float] = None, + ) -> None: + """Move to the joint target and grip using the supplied jaw width.""" + async with self._motion_guard(): + await self.move_to_joint_state( + joint_state, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + await self.gripper.move_to_jaw_position( + resource_width, + max_force_percent=max_force_percent, + check_plate_gripped=check_plate_gripped, + max_speed=max_jaw_speed, + max_acceleration=max_jaw_acceleration, + ) + + async def drop_at_joint_state( + self, + joint_state: Dict[Axis, float], + resource_width: float, + *, + max_gripper_speed: Optional[float] = None, + max_gripper_acceleration: Optional[float] = None, + max_jaw_speed: Optional[float] = None, + max_jaw_acceleration: Optional[float] = None, + ) -> None: + """Move to the joint target and release to the supplied jaw width.""" + async with self._motion_guard(): + await self.move_to_joint_state( + joint_state, + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) + await self.gripper.move_to_jaw_position( + resource_width, + force_sensing=False, + max_speed=max_jaw_speed, + max_acceleration=max_jaw_acceleration, + ) + + 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 _restore_freedrive(self) -> None: + """Restore motors affected by teaching without clearing a fault or e-stop.""" + if self._faulted or await self.request_estop(): + raise CanError("Cannot restore freedrive while stopped or faulted") + for axis, enabled in tuple(self._freedrive_axes.items()): + await self._motor_enable(axis, enabled, use_ds402=True) + del self._freedrive_axes[axis] + + async def start_freedrive_mode(self, free_axes: Optional[List[Axis]] = None) -> None: + """Disable the selected positioning motors for teaching, retaining their prior enable states.""" + axes = list(MOTION_AXES) if free_axes is None else free_axes + if any(axis not in MOTION_AXES for axis in axes): + raise ValueError("Freedrive accepts only the four positioning axes") + async with self._motion_guard(): + try: + for axis in axes: + if axis not in self._freedrive_axes: + self._freedrive_axes[axis] = await self.request_axis_enabled(axis) + await self._motor_enable(axis, False, use_ds402=True) + except BaseException: + await finish_io(asyncio.ensure_future(self._restore_freedrive())) + raise + + async def stop_freedrive_mode(self) -> None: + """Return each taught axis to its enable state from before freedrive.""" + async with self._motion_guard(): + if self._freedrive_axes: + await self._restore_freedrive() diff --git a/pylabrobot/paa/kx2/motion.py b/pylabrobot/paa/kx2/motion.py new file mode 100644 index 00000000000..1abee10569e --- /dev/null +++ b/pylabrobot/paa/kx2/motion.py @@ -0,0 +1,157 @@ +"""Pure gripper-speed calculations for KX2 trajectory planning. + +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/paa/kx2/protocol.py b/pylabrobot/paa/kx2/protocol.py new file mode 100644 index 00000000000..bfb54755180 --- /dev/null +++ b/pylabrobot/paa/kx2/protocol.py @@ -0,0 +1,383 @@ +"""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]: + """Encode an unsigned 32-bit little-endian word.""" + return list((value & 0xFFFFFFFF).to_bytes(4, byteorder="little", signed=False)) + + +class COBType(IntEnum): + """CANopen function codes in the 11-bit identifier.""" + + 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): + """Receive PDO numbers used by the drive.""" + + RPDO1 = 1 + RPDO3 = 3 + RPDO4 = 4 + + +class PDOTransmissionType(IntEnum): + """CANopen synchronous and event-driven transmission codes.""" + + SynchronousAcyclic = 0 + SynchronousCyclic = 1 + EventDrivenManf = 254 # 0xFE + EventDrivenDev = 255 # 0xFF + + +class RPDOMappedObject(IntEnum): + """Drive object mappings accepted in receive PDOs.""" + + NotMapped = 0 + ControlWord = 0x60400010 + TargetTorque = 0x60710010 + MaxTorque = 0x60720010 + TargetPosition = 0x607A0020 + VelocityOffset = 0x60B10020 + TargetPositionIP = 0x60C10120 + TargetVelocityIP = 0x60C10220 + DigitalOutputs = 0x60FE0020 + TargetVelocity = 0x60FF0020 + + +class TPDO(IntEnum): + """Transmit PDO numbers used by the drive.""" + + TPDO1 = 1 + TPDO3 = 3 + TPDO4 = 4 + + +class TPDOTrigger(IntEnum): + """Elmo event-mask bit positions for transmit PDO triggers.""" + + 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): + """Drive object mappings available in transmit PDOs.""" + + 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): + """Native integer and string encoding selectors.""" + + 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: + """Coordinated encoder-space targets and estimated completion time.""" + + 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/barcode_tests.py b/pylabrobot/paa/kx2/tests/barcode_tests.py new file mode 100644 index 00000000000..058a9bc33d0 --- /dev/null +++ b/pylabrobot/paa/kx2/tests/barcode_tests.py @@ -0,0 +1,83 @@ +"""Byte-level scanner commands and cleanup, without a serial connection.""" + +import asyncio +import unittest +from unittest.mock import AsyncMock + +from pylabrobot.paa.kx2 import KX2BarcodeReader + + +def reader() -> KX2BarcodeReader: + """Create a scanner with a mocked PLR Serial transport.""" + scanner = KX2BarcodeReader("test-port") + scanner.io.setup = AsyncMock() # type: ignore[method-assign] + scanner.io.stop = AsyncMock() # type: ignore[method-assign] + scanner.io.write = AsyncMock() # type: ignore[method-assign] + scanner.io.reset_input_buffer = AsyncMock() # type: ignore[method-assign] + scanner.io.read = AsyncMock(return_value=b"FW 1.0\r") # type: ignore[method-assign] + return scanner + + +class BarcodeTests(unittest.IsolatedAsyncioTestCase): + """Verify trigger cleanup and preserve scanner configuration.""" + + async def test_version_scan_and_close_exact_bytes(self) -> None: + """Send only Z1, Z and Y; leave mode and firmware read-window settings alone.""" + scanner = reader() + await scanner.setup() + scanner.io.read = AsyncMock(return_value=b"PLATE123\r") # type: ignore[method-assign] + barcode = await scanner.scan_barcode() + self.assertIsNotNone(barcode) + assert barcode is not None + self.assertEqual(barcode.data, "PLATE123") + await scanner.stop() + self.assertEqual( + [call.args[0] for call in scanner.io.write.call_args_list], # type: ignore[attr-defined] + [b"\x1bZ1\r\n", b"\x1bZ\r\n", b"\x1bY\r\n"], + ) + scanner.io.stop.assert_awaited_once() # type: ignore[attr-defined] + + async def test_scan_timeout_turns_trigger_off(self) -> None: + """A no-read returns None and still sends trigger-off.""" + scanner = reader() + await scanner.setup() + scanner.io.read = AsyncMock(return_value=b"") # type: ignore[method-assign] + self.assertIsNone(await scanner.scan_barcode(read_time=0.001)) + scanner.io.write.assert_awaited_with(b"\x1bY\r\n") # type: ignore[attr-defined] + await scanner.stop() + + async def test_stop_joins_scan_cleanup_and_closes(self) -> None: + """Teardown interrupts an active scan and waits for trigger-off before closing.""" + scanner = reader() + await scanner.setup() + started = asyncio.Event() + + async def read_wait(num_bytes: int = 1) -> bytes: + """Block until stop cancels the scan.""" + started.set() + await asyncio.Future() + return b"" + + scanner.io.read = read_wait # type: ignore[method-assign] + task = asyncio.create_task(scanner.scan_barcode()) + await asyncio.wait_for(started.wait(), 2) + await scanner.stop() + self.assertTrue(task.cancelled()) + scanner.io.write.assert_awaited_with(b"\x1bY\r\n") # type: ignore[attr-defined] + scanner.io.stop.assert_awaited_once() # type: ignore[attr-defined] + + async def test_setup_failure_closes_port(self) -> None: + """A bad version reply cannot leak the serial connection.""" + scanner = reader() + scanner.io.read = AsyncMock(side_effect=OSError("read failed")) # type: ignore[method-assign] + with self.assertRaises(OSError): + await scanner.setup() + scanner.io.stop.assert_awaited_once() # type: ignore[attr-defined] + + async def test_buffer_retains_next_line(self) -> None: + """Two replies in one serial read remain separately readable.""" + scanner = reader() + scanner.io.read = AsyncMock(return_value=b"ONE\rTWO\r") # type: ignore[method-assign] + self.assertEqual(await scanner._read_line(1), "ONE") + self.assertEqual(await scanner._read_line(1), "TWO") + scanner.io.read.assert_awaited_once() # type: ignore[attr-defined] diff --git a/pylabrobot/paa/kx2/tests/configuration_tests.py b/pylabrobot/paa/kx2/tests/configuration_tests.py new file mode 100644 index 00000000000..001c6ba9edf --- /dev/null +++ b/pylabrobot/paa/kx2/tests/configuration_tests.py @@ -0,0 +1,250 @@ +"""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 explicit protocol primitives to the test recorder.""" + backend._query_int = fake._query_int + backend._query_float = fake._query_float + + +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() + _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/emcy_logging_tests.py b/pylabrobot/paa/kx2/tests/emcy_logging_tests.py new file mode 100644 index 00000000000..12866dba2cd --- /dev/null +++ b/pylabrobot/paa/kx2/tests/emcy_logging_tests.py @@ -0,0 +1,103 @@ +"""Polish-pass tests for the KX2 EMCY pipeline. + +Covers three follow-ups to the main EMCY decoder/dispatcher behavior tested +in ``driver_emcy_tests.py``: + +* DS301 §7.2.7.1 "error reset" frame (err_code=0). +* Tiered log levels in ``_dispatch_emcy``. +* ``home_motor`` preserves rich EMCY descriptions on motor-fault re-raise. +""" + +import logging +import struct +import unittest + +from pylabrobot.paa.kx2.kx2 import KX2 +from pylabrobot.paa.kx2.protocol import ( + EmcyFrame, + _decode_emcy, + _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(" 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_keeps_grip_center_fixed_with_angular_limits(self): + """Yaw changes in place at bounded angular speed and acceleration.""" + start = _linear_pose(50.0, 100.0, 30.0, yaw=10.0) + end = _linear_pose(50.0, 100.0, 30.0, yaw=40.0) + dt = 0.008 + samples = kinematics.sample_linear_path( + self.cfg, + self.gp, + start, + end, + vel_mm_per_s=20.0, + accel_mm_per_s2=200.0, + dt_s=dt, + angular_speed=5.0, + angular_acceleration=10.0, + ) + poses = [kinematics.fk(sample.joints, self.cfg, self.gp) for sample in samples] + for pose in poses: + self.assertAlmostEqual(pose.location.x, start.location.x) + self.assertAlmostEqual(pose.location.y, start.location.y) + self.assertAlmostEqual(pose.location.z, start.location.z) + self.assertAlmostEqual(poses[-1].rotation.z, 40.0) + velocities = [(b.rotation.z - a.rotation.z) / dt for a, b in zip(poses, poses[1:])] + self.assertLessEqual(max(abs(v) for v in velocities), 5.0 + 1e-8) + accelerations = [(b - a) / dt for a, b in zip(velocities, velocities[1:])] + self.assertLessEqual(max(abs(a) for a in accelerations), 10.0 + 1e-7) + self.assertTrue(all(v == 0 for v in samples[-1].encoder_velocity.values())) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/paa/kx2/tests/lifecycle_tests.py b/pylabrobot/paa/kx2/tests/lifecycle_tests.py new file mode 100644 index 00000000000..25f4d6c86f5 --- /dev/null +++ b/pylabrobot/paa/kx2/tests/lifecycle_tests.py @@ -0,0 +1,390 @@ +"""Connection and operation restoration through a stateful CANopen transport double.""" + +import asyncio +import struct +import unittest +from typing import Dict, List, Sequence, Tuple +from unittest.mock import AsyncMock + +from pylabrobot.io.canopen import CANopen, FrameCallback +from pylabrobot.paa.kx2 import KX2, Axis +from pylabrobot.paa.kx2.config import AxisConfig, KX2Config, ServoGripperConfig +from pylabrobot.paa.kx2.protocol import CanError, JointMoveDirection + + +def configuration() -> KX2Config: + """Use explicit synthetic calibration, unrelated to a real arm's limits.""" + return KX2Config( + wrist_offset=10, + elbow_offset=20, + elbow_zero_offset=5, + axes={ + axis: AxisConfig( + 1000, 200, -200, False, True, 100, 200, JointMoveDirection.Normal, {}, {}, {} + ) + for axis in Axis + if axis != Axis.RAIL + }, + base_to_gripper_clearance_z=0, + base_to_gripper_clearance_arm=0, + robot_on_rail=False, + servo_gripper=ServoGripperConfig(0, 1000, 2000, 100, 500, 100, 50, 500, 1000, 5000, 1.2, 2.4), + ) + + +class MemoryCAN(CANopen): + """Simulate register persistence, NMT, BI replies and a one-shot failing write.""" + + def __init__(self) -> None: + """Seed non-default registers so restoring assumed defaults cannot pass.""" + super().__init__("virtual") + self.is_open = False + self.states = {node: 127 if node % 2 else 5 for node in (1, 2, 3, 4, 6)} + self.registers: Dict[Tuple[int, int, int], bytes] = {} + self.values: Dict[Tuple[int, str, int], float] = {} + self.listeners: Dict[int, List[FrameCallback]] = {} + self.frames: List[Tuple[int, bytes]] = [] + self.events: List[Tuple[str, int]] = [] + self.write_count = 0 + self.fail_at = -1 + for node in self.states: + self.values.update({(node, "MS", 0): 0, (node, "MO", 0): 0, (node, "SR", 0): 0xC000}) + for transmit, numbers in ((True, (1, 3, 4)), (False, (1, 3))): + for number in numbers: + comm = (0x1800 if transmit else 0x1400) + number - 1 + mapping = (0x1A00 if transmit else 0x1600) + number - 1 + for sub, value in { + 1: bytes([node, number, 0, 0xC0]), + 2: b"\xfe", + 3: b"\x09\x00", + 5: b"\x33\x00", + }.items(): + self.registers[node, comm, sub] = value + self.registers[node, mapping, 0] = b"\x01" + self.registers[node, mapping, 1] = b"\x20\x00\x64\x60" + self.registers[node, mapping, 2] = b"\x20\x00\x6c\x60" + self.registers[node, 0x2F20, number] = b"\x01\x00\x00\x00" + for index, sub, size in ( + (0x6040, 0, 2), + (0x60C0, 0, 2), + (0x60C4, 2, 4), + (0x60C4, 3, 1), + (0x60C4, 5, 1), + (0x60C2, 1, 1), + (0x60C2, 2, 1), + (0x605D, 0, 2), + (0x6060, 0, 1), + (0x6061, 0, 1), + (0x60F2, 0, 2), + (0x6081, 0, 4), + (0x6082, 0, 4), + (0x6083, 0, 4), + (0x6084, 0, 4), + ): + self.registers[node, index, sub] = (3).to_bytes(size, "little") + + @property + def connected(self) -> bool: + """Whether the simulated adapter is open.""" + return self.is_open + + async def setup(self, node_ids: Sequence[int]) -> None: + """Open without changing any drive state.""" + self.is_open = True + + async def stop(self) -> None: + """Close the simulated adapter.""" + self.is_open = False + + def subscribe(self, cob_id: int, callback: FrameCallback) -> None: + """Record subscription order and retain reply consumers.""" + self.events.append(("subscribe", cob_id)) + self.listeners.setdefault(cob_id, []).append(callback) + + async def request_nmt_state(self, node_id: int) -> int: + """Return the simulated node state.""" + return self.states[node_id] + + async def set_nmt_state(self, node_id: int, state: int) -> None: + """Record transitions without broadcasting a reset.""" + self.events.append(("nmt", node_id)) + self.states[node_id] = state + + async def upload(self, node_id: int, index: int, subindex: int) -> bytes: + """Read an initialized object; unexpected objects fail the test.""" + return self.registers[node_id, index, subindex] + + async def download(self, node_id: int, index: int, subindex: int, data: bytes) -> None: + """Persist each write and optionally fail after a partial operation.""" + self.registers[node_id, index, subindex] = bytes(data) + if index == 0x6060: + self.registers[node_id, 0x6061, subindex] = bytes(data) + self.write_count += 1 + if self.write_count == self.fail_at: + raise CanError("injected write failure") + + async def write( + self, cob_id: int, data: bytes, *, remote: bool = False, dlc: int | None = None + ) -> None: + """Reply to binary-interpreter requests with typed register values.""" + self.frames.append((cob_id, data)) + if not 0x301 <= cob_id <= 0x306: + return + node = cob_id - 0x300 + command = data[:2].decode("ascii") + index = data[2] | ((data[3] & 0x3F) << 8) + key = (node, command, index) + floating = bool(data[3] & 0x80) + if not data[3] & 0x40 and len(data) == 8: + self.values[key] = struct.unpack(" Tuple[KX2, MemoryCAN]: + """Construct a KX2 with simulated transport and explicit calibration.""" + arm, io = KX2(), MemoryCAN() + arm.io = io + arm._read_config = AsyncMock(return_value=configuration()) # type: ignore[method-assign] + return arm, io + + +class RestorationTests(unittest.IsolatedAsyncioTestCase): + """Exercise production setup, teardown and configuration scopes.""" + + async def test_empty_pdo_slots_support_streaming_and_restore_active_configuration(self) -> None: + """Unused descriptors may stay initialized; mapping counts and communication restore.""" + arm, io = driver() + for node in io.states: + for mapping in (0x1A02, 0x1A03, 0x1602): + io.registers[node, mapping, 0] = b"\x00" + for sub in (1, 2): + io.registers[node, mapping, sub] = bytes(4) + io.registers[node, 0x6060, 0] = b"\x00" + before = dict(io.registers) + await arm.setup() + self.assertEqual(io.registers[1, 0x6060, 0], b"\x00") + self.assertTrue(arm._has_session_pdo(1, 3, transmit=True)) + self.assertTrue(arm._has_session_pdo(1, 3, transmit=False)) + await arm._ipm_select_mode(True) + await arm._ipm_select_mode(False) + await arm.stop() + for key, value in before.items(): + node, index, sub = key + if index in (0x1A02, 0x1A03, 0x1602) and sub: + continue + if index in (0x6060, 0x6061) and node in arm.motion_node_ids: + self.assertEqual(io.registers[key], b"\x01") + else: + self.assertEqual(io.registers[key], value, key) + + async def test_disabled_gold_drives_are_stationary(self) -> None: + """Gold MS=3 permits setup without enabling the motors.""" + arm, io = driver() + for node in io.states: + io.values[node, "MS", 0] = 3 + await arm.setup() + await arm._stop_axes([1]) + await arm.stop() + self.assertTrue(all(io.values[node, "MO", 0] == 0 for node in io.states)) + + async def test_interpreter_serializes_different_commands_on_the_same_drive(self) -> None: + """Input polling cannot overwrite a pending motion-status reply on the same node.""" + arm, io = driver() + await arm.setup() + started, release = asyncio.Event(), asyncio.Event() + original_write = io.write + commands = [] + + async def delayed_write( + cob_id: int, data: bytes, *, remote: bool = False, dlc: int | None = None + ) -> None: + """Hold one drive's status reply while other requests contend for its channel.""" + commands.append((cob_id, data[:2])) + if cob_id == 0x302 and data[:2] == b"MS": + started.set() + await release.wait() + await original_write(cob_id, data, remote=remote, dlc=dlc) + + io.write = delayed_write # type: ignore[method-assign] + status = asyncio.create_task(arm._query_int(2, "MS", 0)) + await started.wait() + sensor = asyncio.create_task(arm._query_int(2, "IB", 20)) + await arm._query_int(1, "PX", 0) + await asyncio.sleep(0.01) + self.assertNotIn((0x302, b"IB"), commands) + self.assertIn((0x301, b"PX"), commands) + release.set() + await asyncio.gather(status, sensor) + self.assertIn((0x302, b"IB"), commands) + await arm.stop() + + async def test_binary_interpreter_error_is_not_a_value(self) -> None: + """A rejected command carries an error flag, even for a floating-point request.""" + arm, _ = driver() + future = asyncio.get_running_loop().create_future() + arm._pending_bi[1, "AC", 0] = future + arm._dispatch_bi_response(1, b"AC\x00\xc0\x15\x00\x00\x00") + with self.assertRaisesRegex(CanError, r"Node 1 rejected AC\[0\]: Elmo error 21"): + await future + + async def test_lifecycle_restores_nondefault_registers_and_nmt(self) -> None: + """Every changed register, PDO map and node state returns byte-for-byte.""" + arm, io = driver() + before, states = dict(io.registers), dict(io.states) + await arm.setup() + self.assertTrue(arm.setup_finished) + self.assertNotEqual(io.registers, before) + self.assertFalse(any(cmd[:2] in (b"MO", b"BG") and not cmd[3] & 0x40 for _, cmd in io.frames)) + for node in (1, 3): + self.assertLess(io.events.index(("subscribe", 0x80 + node)), io.events.index(("nmt", node))) + await arm.stop() + self.assertEqual(io.registers, before) + self.assertEqual(io.states, states) + self.assertFalse(io.connected) + await arm.stop() + + async def test_partial_setup_failure_restores_and_disconnects(self) -> None: + """A write that reaches a drive before failing still gets restored.""" + for failing_write in (1, 23, 110): + arm, io = driver() + before, states = dict(io.registers), dict(io.states) + io.fail_at = failing_write + with self.assertRaisesRegex(CanError, "injected"): + await arm.setup() + self.assertEqual(io.registers, before) + self.assertEqual(io.states, states) + self.assertFalse(io.connected) + + async def test_acknowledged_but_ignored_restore_is_reported(self) -> None: + """An SDO acknowledgement does not prove the requested setting was restored.""" + arm, io = driver() + await arm.setup() + write = io.download + + async def ignore_mode_restore(node, index, sub, data): + """Model a drive acknowledging a mode value it cannot select.""" + if index == 0x6060 and data == b"\x03": + return + await write(node, index, sub, data) + + io.download = ignore_mode_restore # type: ignore[method-assign] + io.registers[1, 0x6060, 0] = b"\x03" + with self.assertRaisesRegex(CanError, "did not restore 0x6060"): + async with arm._preserve_settings(sdo=[(1, 0x6060, 0), (1, 0x6081, 0)]): + await io.download(1, 0x6060, 0, b"\x01") + await io.download(1, 0x6081, 0, b"\x04\x00\x00\x00") + self.assertEqual(io.registers[1, 0x6081, 0], b"\x03\x00\x00\x00") + self.assertTrue(arm._faulted) + with self.assertRaisesRegex(CanError, "did not restore 0x6060"): + await arm.stop() + self.assertFalse(io.connected) + + async def test_interpolation_buffer_access_enabled_after_clear_and_mode_selection(self) -> None: + """A cleared but disabled buffer discards the preload and underflows on start.""" + arm, io = driver() + await arm.setup() + writes = AsyncMock(wraps=io.download) + io.download = writes # type: ignore[method-assign] + await arm._ipm_select_mode(True) + for node in arm.motion_node_ids: + sequence = [ + (call.args[1], call.args[2], call.args[3]) + for call in writes.call_args_list + if call.args[0] == node and call.args[1] in (0x6060, 0x60C4) + ] + self.assertEqual( + sequence, + [ + (0x6060, 0, b"\x01"), + (0x60C4, 6, b"\x00"), + (0x6060, 0, b"\x07"), + (0x60C4, 6, b"\x01"), + ], + ) + await arm._ipm_select_mode(False) + await arm.stop() + + async def test_interpolation_leaves_nonparticipating_drives_untouched(self) -> None: + """A Z-only stream cannot enable or select interpolation on the wrist.""" + arm, io = driver() + await arm.setup() + before = dict(io.registers) + io.frames.clear() + await arm._ipm_set_time_interval(8, [Axis.Z]) + await arm._ipm_select_mode(True, [Axis.Z]) + await arm._ipm_select_mode(False) + for key, data in before.items(): + if key[0] != Axis.Z: + self.assertEqual(io.registers[key], data, key) + self.assertEqual([cob for cob, _ in io.frames if 0x201 <= cob <= 0x204], [0x202]) + await arm.stop() + + async def test_cancelled_scope_restores_current_interlock_and_settings(self) -> None: + """Restore exact current limits after cancellation without violating CL <= PL.""" + arm, io = driver() + await arm.setup() + io.values[6, "CL", 1], io.values[6, "PL", 1] = 0.4, 0.8 + entered = asyncio.Event() + + async def operation() -> None: + """Cancel while temporary settings are active.""" + async with arm._preserve_settings(floats=[(6, "PL", 1), (6, "CL", 1)]): + await arm._set_servo_gripper_force_limit(100) + entered.set() + await asyncio.Future() + + task = asyncio.create_task(operation()) + await entered.wait() + task.cancel() + with self.assertRaises(asyncio.CancelledError): + await task + self.assertAlmostEqual(io.values[6, "CL", 1], 0.4) + self.assertAlmostEqual(io.values[6, "PL", 1], 0.8) + current_writes = [ + (frame[:2], struct.unpack(" None: + """Do not strand transports when joining a failed motion raises.""" + arm, io = driver() + await arm.setup() + entered = asyncio.Event() + + async def motion() -> None: + """Fail during cancellation cleanup.""" + async with arm._motion_guard(): + entered.set() + try: + await asyncio.Future() + finally: + raise CanError("motion cleanup failed") + + task = asyncio.create_task(motion()) + await entered.wait() + with self.assertRaisesRegex(CanError, "cleanup failed"): + await arm.stop() + self.assertTrue(task.done()) + self.assertFalse(io.connected) + + async def test_concurrent_version_reads_do_not_change_configuration(self) -> None: + """Version queries use the standard read-only software-version object.""" + arm, io = driver() + await arm.setup() + io.registers[1, 0x100A, 0] = b"Elmo firmware\x00" + before = dict(io.registers) + versions = await asyncio.gather(arm.request_drive_version(1), arm.request_drive_version(1)) + self.assertEqual(versions, ["Elmo firmware", "Elmo firmware"]) + self.assertEqual(io.registers, before) + await arm.stop() diff --git a/pylabrobot/paa/kx2/tests/operations_tests.py b/pylabrobot/paa/kx2/tests/operations_tests.py new file mode 100644 index 00000000000..2b3a3339a30 --- /dev/null +++ b/pylabrobot/paa/kx2/tests/operations_tests.py @@ -0,0 +1,357 @@ +"""Device-facing arm, gripper and homing behavior through simulated I/O.""" + +import asyncio +import unittest +from unittest.mock import AsyncMock + +from pylabrobot.paa.kx2 import Axis +from pylabrobot.paa.kx2.protocol import CanError, JointMoveDirection, MotorMoveParam, MotorsMovePlan +from pylabrobot.paa.kx2.tests.lifecycle_tests import driver + + +class OperationsTests(unittest.IsolatedAsyncioTestCase): + """Check partial targets, contact evidence, homing and deliberate parking.""" + + async def test_estop_requires_both_safety_channels(self) -> None: + """Either open safety channel blocks enabling a motor.""" + arm, io = driver() + await arm.setup() + for status in (0, 0x4000, 0x8000): + io.values[1, "SR", 0] = status + self.assertTrue(await arm.request_estop()) + with self.assertRaisesRegex(CanError, "e-stop is active"): + await arm._motor_enable(Axis.Z, True, use_ds402=True) + io.values[1, "SR", 0] = 0xC000 + self.assertFalse(await arm.request_estop()) + await arm.stop() + + async def test_stop_halts_canopen_profile_without_changing_enable_state(self) -> None: + """An interpreter stop alone cannot terminate a CANopen position profile.""" + arm, io = driver() + await arm.setup() + original_write = io.write + + async def respond_to_halt(cob_id, data, **kwargs): + await original_write(cob_id, data, **kwargs) + if cob_id == 0x202 and int.from_bytes(data, "little") & 0x100: + io.values[2, "MS", 0] = 1 + + io.write = respond_to_halt # type: ignore[method-assign] + for controlword in (0x3F, 0x06): + io.values[2, "MO", 0] = 1 + io.registers[2, 0x6040, 0] = controlword.to_bytes(2, "little") + io.values[2, "MS", 0] = 2 + await arm._stop_axes([Axis.Z]) + expected = ((controlword | 0x100) & ~0x10).to_bytes(2, "little") + self.assertIn((0x202, expected), io.frames) + io.frames.clear() + io.values[2, "MO", 0], io.values[2, "MS", 0] = 0, 3 + io.registers[2, 0x6040, 0] = b"\x3f\x00" + await arm._stop_axes([Axis.Z]) + self.assertFalse(any(cob == 0x202 for cob, _ in io.frames)) + await arm.stop() + + async def test_limit_stop_requires_stationary_reference_and_matching_feedback(self) -> None: + """A latched MS=2 is acceptable only after the reference and feedback settle.""" + arm, io = driver() + await arm.setup() + io.values[2, "MS", 0] = 2 + io.values[2, "PX", 0] = io.values[2, "DV", 3] = 50000 + io.values[2, "DV", 5] = 10 + original_query = arm._query_int + velocity_samples = 0 + + async def query(node_id, command, index=0): + nonlocal velocity_samples + if command == "DV" and index == 5: + velocity_samples += 1 + if velocity_samples == 4: + io.values[2, "DV", 5] = 0 + io.values[2, "PX", 0] = 49900 + if velocity_samples == 8: + io.values[2, "PX", 0] = 50000 + return await original_query(node_id, command, index) + + arm._query_int = query # type: ignore[method-assign] + await arm._stop_axes([Axis.Z]) + self.assertGreater(velocity_samples, 8) + self.assertEqual(io.values[2, "MS", 0], 2) + await arm.stop() + + async def test_point_move_stops_then_restores_jog_end_velocity(self) -> None: + """A nonzero prior end velocity cannot carry a point move beyond its target.""" + arm, io = driver() + await arm.setup() + original = (50000).to_bytes(4, "little") + io.registers[6, 0x6082, 0] = original + + async def begin(node_ids: list[int], *, relative: bool = False) -> None: + """Inspect the profile immediately before its start command.""" + self.assertEqual(io.registers[6, 0x6082, 0], bytes(4)) + + arm._ppm_begin_motion = begin # type: ignore[method-assign] + arm._wait_for_moves_done = AsyncMock() # type: ignore[method-assign] + await arm._motors_move_absolute_execute_locked( + MotorsMovePlan( + moves=[MotorMoveParam(6, -280, 10000, 10000, False, JointMoveDirection.Normal)] + ) + ) + self.assertEqual(io.registers[6, 0x6082, 0], original) + await arm.stop() + + async def test_completion_waits_for_feedback_to_settle(self) -> None: + """Profiler completion alone does not imply the axis reached its target.""" + arm, io = driver() + await arm.setup() + io.values[4, "MO", 0] = 1 + io.values[4, "MS", 0] = 1 + self.assertFalse(await arm._motor_check_if_move_done(4)) + io.values[4, "MS", 0] = 0 + self.assertTrue(await arm._motor_check_if_move_done(4)) + io.values[4, "MS", 0], io.values[4, "MO", 0] = 3, 0 + with self.assertRaisesRegex(RuntimeError, "Motor Fault"): + await arm._motor_check_if_move_done(4) + await arm.stop() + + async def test_partial_target_and_position_only_close_guard(self) -> None: + """A partial arm move preserves its keys and a jaw close needs explicit consent.""" + arm, _ = driver() + await arm.setup() + arm.request_joint_state = AsyncMock( # type: ignore[method-assign] + return_value={ + Axis.SHOULDER: 0, + Axis.Z: 50, + Axis.ELBOW: 20, + Axis.WRIST: 0, + Axis.SERVO_GRIPPER: 30, + } + ) # type: ignore[method-assign] + move = AsyncMock() + arm._motors_move_joint_locked = move # type: ignore[method-assign] + await arm.move_to_joint_state({Axis.Z: 60}) + self.assertEqual(move.call_args.args[0], {Axis.Z: 60}) + with self.assertRaisesRegex(ValueError, "Close the jaws"): + await arm.move_to_joint_state({Axis.SERVO_GRIPPER: 20}) + await arm.move_to_joint_state( + {Axis.SERVO_GRIPPER: 20}, close_gripper_without_force_sensing=True + ) + await arm.stop() + + async def test_grip_detection_uses_current_fraction_and_closed_limit(self) -> None: + """High current on a stationary jaw away from the hard stop counts as contact.""" + arm, io = driver() + await arm.setup() + io.values[2, "IB", 20] = 1 + io.values[6, "CL", 1], io.values[6, "IQ", 0] = 1.0, 0.95 + io.values[6, "MO", 0] = 1 + io.values[6, "PX", 0] = 20000 + self.assertTrue(await arm.gripper.sense_plate_gripped()) + io.values[6, "MO", 0] = 0 + self.assertFalse(await arm.gripper.sense_plate_gripped()) + io.values[6, "MO", 0] = 1 + io.values[6, "IQ", 0] = 0.1 + self.assertFalse(await arm.gripper.sense_plate_gripped()) + io.values[6, "IQ", 0] = 0.95 + io.values[6, "PX", 0] = -200000 + self.assertFalse(await arm.gripper.sense_plate_gripped()) + await arm.stop() + + async def test_gripper_force_is_scoped_on_failure(self) -> None: + """A rejected jaw move restores the current limits it found.""" + arm, io = driver() + await arm.setup() + io.values[6, "PL", 1], io.values[6, "CL", 1] = 0.8, 0.4 + io.values[6, "PX", 0] = 30000 + arm._start_joint_plan = AsyncMock(side_effect=CanError("blocked")) # type: ignore[method-assign] + with self.assertRaisesRegex(CanError, "blocked"): + await arm.gripper.move_to_jaw_position(20, max_force_percent=50) + self.assertAlmostEqual(io.values[6, "CL", 1], 0.4) + self.assertAlmostEqual(io.values[6, "PL", 1], 0.8) + await arm.stop() + + async def test_current_during_jaw_motion_is_not_a_grip(self) -> None: + """High motor current while the jaw is still moving cannot confirm a held plate.""" + arm, io = driver() + await arm.setup() + io.values[6, "MO", 0] = 1 + io.values[6, "CL", 1], io.values[6, "IQ", 0] = 1.0, 0.95 + arm.gripper.request_jaw_position = AsyncMock( # type: ignore[method-assign] + side_effect=[10.0, 10.0, 10.1] + ) + self.assertFalse(await arm.gripper.sense_plate_gripped()) + await arm.stop() + + async def test_contact_retains_target_and_current_until_release(self) -> None: + """A grip holds its limited current; opening restores the original limits.""" + arm, io = driver() + await arm.setup() + io.values[2, "IB", 20] = 1 + io.values[6, "CL", 1], io.values[6, "PL", 1] = 0.4, 0.8 + io.values[6, "PX", 0], io.values[6, "IQ", 0] = 30000, 0.57 + io.values[6, "MO", 0], io.values[6, "MS", 0] = 1, 1 + start = AsyncMock() + arm._start_joint_plan = start # type: ignore[method-assign] + await arm.gripper.move_to_jaw_position( + 20, max_force_percent=50, check_plate_gripped=True, max_speed=1, max_acceleration=2 + ) + self.assertAlmostEqual(io.values[6, "CL", 1], 0.6) + self.assertAlmostEqual(io.values[6, "PL", 1], 1.2) + self.assertNotIn((0x306, b"ST\x00\x00"), io.frames) + self.assertEqual(start.call_args.args[0].moves[0].velocity, 1000) + self.assertEqual(start.call_args.args[0].moves[0].acceleration, 2000) + self.assertEqual(arm.configuration.axes[Axis.SERVO_GRIPPER].max_vel, 100) + io.values[6, "MS", 0] = 0 + + async def open_jaws(plan): + self.assertAlmostEqual(io.values[6, "CL", 1], 1.2) + io.values[6, "PX", 0] = plan.moves[0].position + + arm._start_joint_plan = open_jaws # type: ignore[method-assign] + await arm.gripper.move_to_jaw_position(40, force_sensing=False) + self.assertAlmostEqual(io.values[6, "CL", 1], 0.4) + self.assertAlmostEqual(io.values[6, "PL", 1], 0.8) + await arm.stop() + + async def test_current_without_plate_detection_is_not_a_grip(self) -> None: + """Motor resistance alone cannot establish plate presence.""" + arm, io = driver() + await arm.setup() + io.values[6, "MO", 0], io.values[6, "PX", 0] = 1, 30000 + io.values[6, "CL", 1], io.values[6, "IQ", 0] = 0.6, 0.59 + self.assertFalse(await arm.gripper.sense_plate_gripped()) + io.values[2, "IB", 20] = 1 + io.values[6, "PX", 0] = 199980 + self.assertFalse(await arm.gripper.sense_plate_gripped()) + await arm.stop() + + async def test_stall_before_closure_is_not_reported_as_a_grip(self) -> None: + """Insufficient current at the open limit must fail rather than claim contact.""" + arm, io = driver() + await arm.setup() + io.values[6, "CL", 1], io.values[6, "PL", 1] = 0.4, 0.8 + io.values[6, "MO", 0], io.values[6, "MS", 0] = 1, 1 + io.values[6, "PX", 0], io.values[6, "IQ", 0] = 199980, 0.24 + io.values[2, "IB", 20] = 1 + arm._start_joint_plan = AsyncMock() # type: ignore[method-assign] + with self.assertRaisesRegex(CanError, "before measurable jaw closure"): + await arm.gripper.move_to_jaw_position(196, max_force_percent=20) + self.assertAlmostEqual(io.values[6, "CL", 1], 0.4) + self.assertIn((0x306, b"ST\x00\x00"), io.frames) + await arm.stop() + + async def test_release_accepts_settled_feedback_with_profiler_done(self) -> None: + """MS=1 does not invalidate feedback within the jaw tolerance.""" + arm, io = driver() + await arm.setup() + io.values[6, "CL", 1], io.values[6, "PL", 1] = 0.4, 0.8 + io.values[6, "PX", 0], io.values[6, "MO", 0], io.values[6, "MS", 0] = 30000, 1, 1 + + async def open_jaws(plan): + self.assertAlmostEqual(io.values[6, "CL", 1], 1.2) + io.values[6, "PX", 0] = plan.moves[0].position - 5 + + arm._start_joint_plan = open_jaws # type: ignore[method-assign] + await arm.gripper.move_to_jaw_position(40, force_sensing=False, max_speed=1) + self.assertAlmostEqual(io.values[6, "CL", 1], 0.4) + self.assertAlmostEqual(io.values[6, "PL", 1], 0.8) + await arm.stop() + + async def test_cancelled_close_stops_before_restoring_current(self) -> None: + """Cancelling a closing profile cannot leave a producer or closing target running.""" + arm, io = driver() + await arm.setup() + io.values[6, "CL", 1], io.values[6, "PL", 1] = 0.4, 0.8 + io.values[6, "PX", 0], io.values[6, "MO", 0] = 30000, 1 + begun = asyncio.Event() + + async def begin(plan): + begun.set() + + arm._start_joint_plan = begin # type: ignore[method-assign] + task = asyncio.create_task(arm.gripper.move_to_jaw_position(20, max_force_percent=50)) + await begun.wait() + task.cancel() + with self.assertRaises(asyncio.CancelledError): + await task + self.assertIn((0x306, b"ST\x00\x00"), io.frames) + self.assertAlmostEqual(io.values[6, "CL", 1], 0.4) + self.assertAlmostEqual(io.values[6, "PL", 1], 0.8) + await arm.stop() + + async def test_disconnect_restores_current_after_held_grip(self) -> None: + """The connection owns cleanup for a grip that has not been released.""" + arm, io = driver() + await arm.setup() + io.values[2, "IB", 20] = 1 + io.values[6, "CL", 1], io.values[6, "PL", 1] = 0.4, 0.8 + io.values[6, "PX", 0], io.values[6, "IQ", 0] = 30000, 0.57 + io.values[6, "MO", 0], io.values[6, "MS", 0] = 1, 1 + arm._start_joint_plan = AsyncMock() # type: ignore[method-assign] + await arm.gripper.move_to_jaw_position(20, max_force_percent=50) + await arm.stop() + self.assertAlmostEqual(io.values[6, "CL", 1], 0.4) + self.assertAlmostEqual(io.values[6, "PL", 1], 0.8) + + async def test_hard_stop_uses_jog_begin_error_query_and_stop(self) -> None: + """Homing decisions run in Python, with a stop after the observed hard-stop error.""" + arm, io = driver() + await arm.setup() + io.values[6, "PE", 0] = 600 + await arm._gripper_hard_stop_search(1000, 2000, 100, 500, 1) + commands = [frame[:2] for cob, frame in io.frames if cob == 0x306] + self.assertLess(commands.index(b"JP"), commands.index(b"BG")) + self.assertLess(commands.index(b"BG"), commands.index(b"PE")) + self.assertLess(commands.index(b"PE"), commands.index(b"ST")) + self.assertNotIn(b"JV", commands) + datum = [ + frame + for cob, frame in io.frames + if cob == 0x306 and frame[:3] == b"HM\x02" and not frame[3] & 0x40 + ] + self.assertEqual(datum[-1][4:], b"\x00\x00\x00\x00") + self.assertNotIn(b"XQ", commands) + await arm.stop() + + async def test_jaw_target_after_homing_uses_calibrated_reference(self) -> None: + """The interpreter homing reference must not carry into the next CANopen move.""" + arm, io = driver() + await arm.setup() + needs_reference_reset = False + original_write = io.write + + async def home(config): + nonlocal needs_reference_reset + needs_reference_reset = True + io.values[6, "PX", 0] = 30000 + + async def write(cob_id: int, data: bytes, *, remote: bool = False, dlc: int | None = None): + nonlocal needs_reference_reset + if cob_id == 0x306 and data == b"MO\x00\x00\x00\x00\x00\x00": + needs_reference_reset = False + await original_write(cob_id, data, remote=remote, dlc=dlc) + + async def move(plan): + self.assertFalse(needs_reference_reset) + io.values[6, "PX", 0] = plan.moves[0].position + + io.write = write # type: ignore[method-assign] + arm._home_servo_gripper = home # type: ignore[method-assign] + arm._start_joint_plan = move # type: ignore[method-assign] + await arm.gripper.home() + await arm.gripper.move_to_jaw_position(29, force_sensing=False, max_speed=1) + self.assertEqual(await arm.gripper.request_jaw_position(), 29) + await arm.stop() + + async def test_park_requires_calibrated_explicit_target(self) -> None: + """There is no universal parking pose across different KX2 installations.""" + arm, _ = driver() + await arm.setup() + with self.assertRaisesRegex(ValueError, "parking_position"): + await arm.park() + with self.assertRaises(ValueError): + arm.parking_position = {Axis.Z: 1000} + target = {Axis.Z: 100.0} + arm.parking_position = target + target[Axis.Z] = 999 + self.assertEqual(arm.parking_position, {Axis.Z: 100}) + await arm.stop() diff --git a/pylabrobot/paa/kx2/tests/planner_tests.py b/pylabrobot/paa/kx2/tests/planner_tests.py new file mode 100644 index 00000000000..5d8f16264ac --- /dev/null +++ b/pylabrobot/paa/kx2/tests/planner_tests.py @@ -0,0 +1,772 @@ +"""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_z_only_move_checks_measured_elbow(self) -> None: + """An omitted elbow keeps its measured angle for the base-clearance check.""" + cfg = _config(base_to_gripper_clearance_z=50, base_to_gripper_clearance_arm=30) + clear = {Axis.Z: 10.0, Axis.ELBOW: 200.0} + plan = kinematics.plan_joint_move(clear, {Axis.Z: 10.5}, cfg, GripperParams()) + self.assertIsNotNone(plan) + assert plan is not None + self.assertEqual([move.node_id for move in plan.moves], [Axis.Z]) + obstructed = {Axis.Z: 10.0, Axis.ELBOW: 50.0} + with self.assertRaisesRegex(ValueError, "clearance"): + kinematics.plan_joint_move(obstructed, {Axis.Z: 10.5}, cfg, GripperParams()) + + def test_violation_raises(self): + """Z below clearance AND elbow angle below arm clearance -> raise.""" + cfg = _config( + z=_axis(min_travel=0.0, max_travel=400.0), + elbow=_axis(min_travel=0.0, max_travel=300.0), + base_to_gripper_clearance_z=50.0, + base_to_gripper_clearance_arm=80.0, # 80 deg threshold (angle space) + ) + g = GripperParams() + # target Z = 10 < (0 + 50). Elbow position 50 -> angle ~asin(55/305) ~ 10° < 80°. + cur = {Axis.Z: 100.0, Axis.ELBOW: 200.0} + tgt = {Axis.Z: 10.0, Axis.ELBOW: 50.0} + with self.assertRaises(ValueError) as ctx: + kinematics.plan_joint_move(cur, tgt, cfg, g) + self.assertIn("clearance", str(ctx.exception).lower()) + + def test_no_violation_when_only_z_low(self): + """Z low but elbow angle above arm clearance -> no raise.""" + cfg = _config( + z=_axis(min_travel=0.0, max_travel=400.0), + elbow=_axis(min_travel=0.0, max_travel=300.0), + base_to_gripper_clearance_z=50.0, + base_to_gripper_clearance_arm=5.0, # very low threshold so any pose passes + ) + g = GripperParams() + cur = {Axis.Z: 100.0, Axis.ELBOW: 200.0} + tgt = {Axis.Z: 10.0, Axis.ELBOW: 200.0} + plan = kinematics.plan_joint_move(cur, tgt, cfg, g) + self.assertIsNotNone(plan) + + def test_no_violation_when_only_elbow_low(self): + """Elbow angle below threshold but Z high -> no raise.""" + cfg = _config( + z=_axis(min_travel=0.0, max_travel=400.0), + elbow=_axis(min_travel=0.0, max_travel=300.0), + base_to_gripper_clearance_z=50.0, + base_to_gripper_clearance_arm=80.0, + ) + g = GripperParams() + cur = {Axis.Z: 100.0, Axis.ELBOW: 200.0} + tgt = {Axis.Z: 200.0, Axis.ELBOW: 50.0} + plan = kinematics.plan_joint_move(cur, tgt, cfg, g) + self.assertIsNotNone(plan) + + +# --- 11. exact-value pins for the sync algorithm ---------------------------- +# These tests pin the *exact* encoder velocity, acceleration, and move_time +# the planner produces for representative cases. They're the contract for any +# refactor of the per-axis profile + accel-sync + time-sync chain — directional +# asserts (assertLess/Greater) are too loose to catch a value-shifting change. +# Tolerance: ±1 on int encoder values (rounding noise from float ops); 4 +# decimals on move_time. + + +class SyncAlgorithmExactValues(unittest.TestCase): + def _assert_enc_close(self, actual, expected): + self.assertLessEqual(abs(actual - expected), 1, f"{actual} not within 1 of {expected}") + + def test_single_axis_trapezoidal(self): + """dist=100, v_max=100, a_max=200; t_acc=0.5, d_acc=25, t_cruise=0.5.""" + cfg = _config(shoulder=_axis(max_vel=100.0, max_accel=200.0)) + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, + {Axis.SHOULDER: 100.0}, + cfg, + GripperParams(), + ) + self.assertIsNotNone(plan) + assert plan is not None + m = _move_for(plan, Axis.SHOULDER) + self.assertEqual(m.position, 100) + self.assertEqual(m.velocity, 100) + self.assertEqual(m.acceleration, 200) + self.assertAlmostEqual(plan.move_time, 1.5, places=4) + + def test_single_axis_triangular(self): + """dist=10, v_max=100, a_max=200; can't reach v_max -> triangular. + t_acc = sqrt(10/200) = 0.2236, v_actual = a*t_acc = 44.72 -> 45.""" + cfg = _config(shoulder=_axis(max_vel=100.0, max_accel=200.0)) + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, + {Axis.SHOULDER: 10.0}, + cfg, + GripperParams(), + ) + self.assertIsNotNone(plan) + assert plan is not None + m = _move_for(plan, Axis.SHOULDER) + self.assertEqual(m.position, 10) + self._assert_enc_close(m.velocity, 45) + self.assertEqual(m.acceleration, 200) + self.assertAlmostEqual(plan.move_time, 0.4472136, places=4) + + def test_accel_sync_two_axis_exact(self): + """Shoulder is the slow-accel lead (v=100, a=10); Z's accel scales down.""" + cfg = _config( + shoulder=_axis(max_vel=100.0, max_accel=10.0), + z=_axis(min_travel=0.0, max_travel=400.0, max_vel=100.0, max_accel=1000.0), + ) + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0, Axis.Z: 0.0}, + {Axis.SHOULDER: 50.0, Axis.Z: 50.0}, + cfg, + GripperParams(), + ) + self.assertIsNotNone(plan) + assert plan is not None + sh = _move_for(plan, Axis.SHOULDER) + z = _move_for(plan, Axis.Z) + self._assert_enc_close(sh.velocity, 22) + self.assertEqual(sh.acceleration, 10) + self._assert_enc_close(z.velocity, 15) + self._assert_enc_close(z.acceleration, 14) + self.assertAlmostEqual(plan.move_time, 4.472136, places=4) + + def test_time_sync_two_axis_exact(self): + """Shoulder is the slow-vel lead (v=10, a=20); Z's v scales down to match + move_time = dist/v + v/a = 100/10 + 10/20 = 10.5s.""" + cfg = _config( + shoulder=_axis(max_vel=10.0, max_accel=20.0), + z=_axis(min_travel=0.0, max_travel=400.0, max_vel=100.0, max_accel=200.0), + ) + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0, Axis.Z: 0.0}, + {Axis.SHOULDER: 100.0, Axis.Z: 100.0}, + cfg, + GripperParams(), + ) + self.assertIsNotNone(plan) + assert plan is not None + sh = _move_for(plan, Axis.SHOULDER) + z = _move_for(plan, Axis.Z) + self.assertEqual(sh.velocity, 10) + self.assertEqual(sh.acceleration, 20) + self.assertEqual(z.velocity, 10) + self.assertEqual(z.acceleration, 20) + self.assertAlmostEqual(plan.move_time, 10.5, places=4) + + def test_four_axis_real_conv_exact(self): + """Full 4-axis coordinated move with realistic KX2 conv factors. Pins + the entire pipeline (elbow asin + sync + encoder packing) end-to-end.""" + cfg = _config( + shoulder=_axis(max_vel=145.0, max_accel=300.0, motor_conversion_factor=23301.694), + z=_axis( + min_travel=0.0, + max_travel=750.0, + max_vel=750.0, + max_accel=1000.0, + motor_conversion_factor=3997.838, + ), + elbow=_axis( + min_travel=0.0, + max_travel=300.0, + max_vel=80.0, + max_accel=180.0, + motor_conversion_factor=18204.444, + ), + wrist=_axis( + min_travel=-180.0, + max_travel=180.0, + max_vel=500.0, + max_accel=1000.0, + motor_conversion_factor=45.511, + unlimited_travel=True, + ), + ) + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0, Axis.Z: 50.0, Axis.ELBOW: 100.0, Axis.WRIST: 0.0}, + {Axis.SHOULDER: 30.0, Axis.Z: 200.0, Axis.ELBOW: 150.0, Axis.WRIST: 90.0}, + cfg, + GripperParams(), + ) + self.assertIsNotNone(plan) + assert plan is not None + sh = _move_for(plan, Axis.SHOULDER) + z = _move_for(plan, Axis.Z) + el = _move_for(plan, Axis.ELBOW) + wr = _move_for(plan, Axis.WRIST) + self._assert_enc_close(sh.position, 699051) + self._assert_enc_close(sh.velocity, 1646247) + self._assert_enc_close(sh.acceleration, 4704052) + self._assert_enc_close(z.position, 799568) + self._assert_enc_close(z.velocity, 1548356) + self._assert_enc_close(z.acceleration, 3997838) + self._assert_enc_close(el.position, 556033) + self._assert_enc_close(el.velocity, 403583) + self._assert_enc_close(el.acceleration, 1322501) + self._assert_enc_close(wr.position, 4096) + self._assert_enc_close(wr.velocity, 9444) + self._assert_enc_close(wr.acceleration, 27705) + self.assertAlmostEqual(plan.move_time, 0.774597, places=3) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/paa/kx2/tests/proximity_tests.py b/pylabrobot/paa/kx2/tests/proximity_tests.py new file mode 100644 index 00000000000..732b6bb7ad8 --- /dev/null +++ b/pylabrobot/paa/kx2/tests/proximity_tests.py @@ -0,0 +1,110 @@ +"""Proximity searches join motion and restore input logic on every exit path.""" + +import asyncio +import unittest +from unittest.mock import AsyncMock + +from pylabrobot.paa.kx2 import Axis +from pylabrobot.paa.kx2.protocol import CanError +from pylabrobot.paa.kx2.tests.lifecycle_tests import driver +from pylabrobot.resources import Coordinate + + +class ProximityTests(unittest.IsolatedAsyncioTestCase): + """Exercise sensor trips, drive faults, cancellation and cleanup failures.""" + + async def test_trip_joins_descent_before_restoring_logic(self) -> None: + """A trip stops and joins the producer before restoring non-default IL.""" + arm, io = driver() + await arm.setup() + io.values[2, "IL", 4] = 23 + joined = asyncio.Event() + started = asyncio.Event() + + async def move(target, **kwargs) -> None: + """Only the descent remains in flight until the sensor interrupts it.""" + if target[Axis.Z] == 80: + self.assertEqual(io.values[2, "IL", 4], 8) + started.set() + try: + await asyncio.Future() + finally: + joined.set() + + async def sensor() -> bool: + """Trip only after the descent producer has started.""" + return started.is_set() + + arm._motors_move_joint_locked = move # type: ignore[method-assign,assignment] + arm.sense_proximity = sensor # type: ignore[method-assign,assignment] + arm.request_axis_position = AsyncMock(return_value=95.5) # type: ignore[method-assign,assignment] + self.assertEqual(await arm.find_z_with_proximity_sensor(100, 80), 95.5) + self.assertTrue(joined.is_set()) + self.assertEqual(io.values[2, "IL", 4], 23) + self.assertIn((0x302, b"ST\x00\x00"), io.frames) + await arm.stop() + + async def test_drive_error_propagates_and_restores_logic(self) -> None: + """Unexpected drive failures retain their error instead of becoming a no-trip result.""" + arm, io = driver() + await arm.setup() + io.values[2, "IL", 4] = 17 + arm.sense_proximity = AsyncMock(return_value=False) # type: ignore[method-assign,assignment] + arm._motors_move_joint_locked = AsyncMock(side_effect=[None, CanError("descent fault")]) # type: ignore[method-assign,assignment] + with self.assertRaisesRegex(CanError, "descent fault"): + await arm.find_z_with_proximity_sensor(100, 80) + self.assertEqual(io.values[2, "IL", 4], 17) + await arm.stop() + + async def test_cancel_joins_cartesian_sweep(self) -> None: + """Cancelling the caller cannot leave an unowned sweep emitting motion commands.""" + arm, _ = driver() + await arm.setup() + started, joined = asyncio.Event(), asyncio.Event() + arm.sense_proximity = AsyncMock(return_value=False) # type: ignore[method-assign,assignment] + arm.request_gripper_pose = AsyncMock() # type: ignore[method-assign,assignment] + + async def move(*args, **kwargs) -> None: + """Block only the second, linear move.""" + if True: + started.set() + try: + await asyncio.Future() + finally: + joined.set() + + arm.move_to_location = AsyncMock() # type: ignore[method-assign,assignment] + arm._run_linear_path = move # type: ignore[method-assign,assignment] + task = asyncio.create_task( + arm.find_with_proximity_sensor(Coordinate(0, 0, 0), Coordinate(1, 0, 0)) + ) + await asyncio.wait_for(started.wait(), 2) + task.cancel() + with self.assertRaises(asyncio.CancelledError): + await task + self.assertTrue(joined.is_set()) + self.assertIsNone(arm._motion_owner) + await arm.stop() + + async def test_no_trip_reports_search_bounds(self) -> None: + """Reaching the endpoint without a sensor trip is a failed search.""" + arm, _ = driver() + await arm.setup() + arm.sense_proximity = AsyncMock(return_value=False) # type: ignore[method-assign,assignment] + arm._motors_move_joint_locked = AsyncMock() # type: ignore[method-assign,assignment] + with self.assertRaisesRegex(RuntimeError, "100 to 80"): + await arm.find_z_with_proximity_sensor(100, 80) + await arm.stop() + + async def test_stop_failure_is_reported_and_input_restored(self) -> None: + """Failed stopping is not reported as a successful sensor search.""" + arm, io = driver() + await arm.setup() + io.values[2, "IL", 4] = 19 + arm.sense_proximity = AsyncMock(side_effect=[False, True]) # type: ignore[method-assign,assignment] + arm._motors_move_joint_locked = AsyncMock() # type: ignore[method-assign,assignment] + arm._stop_axes = AsyncMock(side_effect=CanError("stop failed")) # type: ignore[method-assign,assignment] + with self.assertRaisesRegex(CanError, "stop failed"): + await arm.find_z_with_proximity_sensor(100, 80) + self.assertEqual(io.values[2, "IL", 4], 19) + await arm.stop() diff --git a/pylabrobot/paa/kx2/tests/setpoint_tests.py b/pylabrobot/paa/kx2/tests/setpoint_tests.py new file mode 100644 index 00000000000..a462484644f --- /dev/null +++ b/pylabrobot/paa/kx2/tests/setpoint_tests.py @@ -0,0 +1,206 @@ +"""Unit tests for the KX2 driver's CiA 402 PPM new-setpoint handshake. + +Covers ``KX2._trigger_new_setpoint``: the four-step CW/SW dance +(drop bit 4, wait bit 12 low, raise bit 4, wait bit 12 high) and its +retry-on-missed-edge behavior. The drive's SW state is faked by setting +``self._statusword[nid]`` directly and signalling +``self._statusword_event[nid]`` — same shape as the real TPDO3 callback, +without any CAN traffic. +""" + +import asyncio +import unittest +from typing import List, Tuple + +from pylabrobot.paa.kx2.kx2 import KX2 +from pylabrobot.paa.kx2.protocol import CanError + +SW_BIT_12 = 1 << 12 + + +class _SwScript: + """Drives the fake SW state machine. + + Each step is one of: + ("set_low",) -> 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/streaming_tests.py b/pylabrobot/paa/kx2/tests/streaming_tests.py new file mode 100644 index 00000000000..d73648e3327 --- /dev/null +++ b/pylabrobot/paa/kx2/tests/streaming_tests.py @@ -0,0 +1,542 @@ +"""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_state`` 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 copy +import inspect +import unittest +from typing import Any, Dict, List, Optional, Tuple + +from pylabrobot.paa.kx2.config import ( + Axis, + AxisConfig, + GripperParams, + KX2Config, +) +from pylabrobot.paa.kx2.kinematics import CartesianPose +from pylabrobot.paa.kx2 import kinematics +from pylabrobot.paa.kx2.kx2 import KX2, _NodeEmcyState +from pylabrobot.paa.kx2.protocol import CanError, 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 + self.registers: Dict[Tuple[int, int, int], bytes] = {} + self.positions: Dict[int, int] = {} + self.endpoint_error = 0 + self.mode_nodes: List[List[int]] = [] + + async def _ipm_select_mode(self, enable: bool, node_ids=None) -> None: + self.calls.append(("_ipm_select_mode", (enable,))) + if enable: + self.mode_nodes.append(list(node_ids)) + for node in (1, 2, 3, 4): + self.registers[node, 0x6060, 0] = bytes([7 if enable else 1]) + + async def _ipm_set_time_interval(self, ms: int, node_ids=None) -> None: + self.calls.append(("_ipm_set_time_interval", (ms,))) + for node in (1, 2, 3, 4): + self.registers[node, 0x60C2, 1] = bytes([ms]) + + async 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], *, allow_empty: bool = False) -> 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 request_axis_position(self, node_id, pu=False): + return self.positions[node_id] + + async def _can_sdo_upload(self, nid, index, sub): + return self.registers.get((nid, index, sub), b"\x03") + + async def _can_sdo_download(self, nid, index, sub, value): + self.calls.append(("restore", (nid, index, sub, value))) + self.registers[nid, index, sub] = bytes(value) + + async def _execute(self, nid, cmd, index=0): + self.calls.append((cmd, (nid,))) + + async def _control_word_set(self, nid, value, sync=True): + self.calls.append(("controlword", (nid, value))) + + async def _motor_get_motion_status(self, nid): + return 0 + + async def request_axis_enabled(self, nid): + return True + + async def _wait_for_moves_done(self, nodes, timeout): + for nid, position, velocity in self.send_calls: + self.positions[nid] = position + self.endpoint_error + self.calls.append(("wait_done", (tuple(nodes), timeout))) + + +def _attach_fake(backend, fake, *, rename=None): + """Bind explicit protocol primitives to the test recorder.""" + backend._can_sdo_upload = fake._can_sdo_upload + backend._can_sdo_download = fake._can_sdo_download + backend._execute = fake._execute + backend._control_word_set = fake._control_word_set + backend._motor_get_motion_status = fake._motor_get_motion_status + backend.request_axis_enabled = fake.request_axis_enabled + backend._wait_for_moves_done = fake._wait_for_moves_done + backend._ipm_select_mode = fake._ipm_select_mode + backend._ipm_set_time_interval = fake._ipm_set_time_interval + backend._ipm_send_pvt_point = fake._ipm_send_pvt_point + backend._ipm_begin_motion = fake._ipm_begin_motion + backend._ipm_stop = fake._ipm_stop + backend._ipm_check_queue_fault = fake._ipm_check_queue_fault + backend._ipm_wait_motion_complete = fake._ipm_wait_motion_complete + backend._motors_ensure_enabled = fake._motors_ensure_enabled + backend._clear_emcy_state = fake._clear_emcy_state + backend._motor_read_position_raw = fake.request_axis_position + backend._has_session_pdo = lambda node, number, *, transmit: True + + +def _build_backend( + fake_driver: _FakeDriver, + *, + current_joints: Optional[Dict[int, float]] = None, +) -> KX2: + backend = KX2() + # The fake's request_axis_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={"request_axis_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_state = _request_joint_position # type: ignore[assignment] + for axis in backend.configuration.axes: + position = joints[axis] + if axis == Axis.ELBOW: + position = kinematics.convert_elbow_position_to_angle(backend.configuration, position) + fake_driver.positions[int(axis)] = round( + position * backend.configuration.axes[axis].motor_conversion_factor + ) + 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,)) + names = [name for name, _ in fake.calls] + self.assertLess(names.index("_ipm_set_time_interval"), names.index("_ipm_select_mode")) + + 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_waits_for_buffered_motion_before_disabling_interpolation(self): + """Submitting the terminal point does not mean the look-ahead buffer has drained.""" + fake = _FakeDriver() + backend = _build_backend(fake) + asyncio.run( + backend._run_linear_path( + _target_pose(), + max_gripper_speed=20.0, + max_gripper_acceleration=200.0, + ) + ) + names = [name for name, _ in fake.calls] + self.assertLess(names.index("wait_done"), names.index("_ipm_stop")) + 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], "restore") + self.assertEqual( + [args for name, args in fake.calls if name == "_ipm_select_mode"][-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 StreamedEndpoints(unittest.IsolatedAsyncioTestCase): + """Exercise encoder resolution, absolute coordinates, and measured completion.""" + + def _samples(self, fake: _FakeDriver) -> List[kinematics.LinearPathSample]: + """A small wrist move with the other axes stationary.""" + positions = {Axis(nid): value for nid, value in fake.positions.items()} + return [ + kinematics.LinearPathSample( + time_s=i * 0.008, + joints={}, + encoder_position={ + axis: value + (46 * i if axis == Axis.WRIST else 0) for axis, value in positions.items() + }, + encoder_velocity={axis: 0 for axis in positions}, + ) + for i in range(3) + ] + + async def test_motion_below_500_counts_is_sent(self): + fake = _FakeDriver() + backend = _build_backend(fake) + samples = self._samples(fake) + await backend._stream_samples(samples) + self.assertEqual([node for node, _, _ in fake.send_calls], [Axis.WRIST] * 3) + self.assertEqual(fake.mode_nodes, [[Axis.WRIST]]) + self.assertEqual(fake.positions[Axis.WRIST], samples[-1].encoder_position[Axis.WRIST]) + + async def test_missed_endpoint_raises_and_cleans_up(self): + fake = _FakeDriver() + backend = _build_backend(fake) + fake.endpoint_error = 30 + with self.assertRaisesRegex(CanError, "WRIST did not reach"): + await backend._stream_samples(self._samples(fake)) + self.assertIn(("_ipm_select_mode", (False,)), fake.calls) + self.assertIn(("ST", (Axis.WRIST,)), fake.calls) + + async def test_terminal_empty_fifo_requires_the_measured_endpoint(self): + for error in (0, 30): + with self.subTest(endpoint_error=error): + fake = _FakeDriver() + fake.endpoint_error = error + backend = _build_backend(fake) + + async def wait_done(nodes, timeout): + await fake._wait_for_moves_done(nodes, timeout) + backend._emcy[Axis.WRIST] = _NodeEmcyState(underflow=True) + + backend._wait_for_moves_done = wait_done # type: ignore[method-assign] + backend._ipm_check_queue_fault = lambda nodes, **kw: KX2._ipm_check_queue_fault( # type: ignore[method-assign] + backend, nodes, **kw + ) + if error: + with self.assertRaisesRegex(CanError, "WRIST did not reach"): + await backend._stream_samples(self._samples(fake)) + else: + await backend._stream_samples(self._samples(fake)) + + async def test_empty_fifo_during_feeding_is_rejected(self): + fake = _FakeDriver() + backend = _build_backend(fake) + + async def send(nid, position, velocity): + await fake._ipm_send_pvt_point(nid, position, velocity) + backend._emcy[Axis.WRIST] = _NodeEmcyState(underflow=True) + + backend._ipm_send_pvt_point = send # type: ignore[method-assign] + backend._ipm_check_queue_fault = lambda nodes, **kw: KX2._ipm_check_queue_fault( # type: ignore[method-assign] + backend, nodes, **kw + ) + with self.assertRaisesRegex(CanError, "underflow"): + await backend._stream_samples(self._samples(fake)) + + async def test_previous_stream_empty_fifo_does_not_poison_the_next_move(self): + fake = _FakeDriver() + backend = _build_backend(fake) + backend._emcy[Axis.WRIST] = _NodeEmcyState(underflow=True) + backend._ipm_check_queue_fault = lambda nodes, **kw: KX2._ipm_check_queue_fault( # type: ignore[method-assign] + backend, nodes, **kw + ) + await backend._stream_samples(self._samples(fake)) + + def test_full_fifo_is_fatal_even_after_feeding(self): + backend = KX2() + backend._emcy[Axis.WRIST] = _NodeEmcyState(queue_full=True) + with self.assertRaisesRegex(CanError, "queue_full"): + backend._ipm_check_queue_fault([Axis.WRIST], allow_empty=True) + + async def test_stale_start_is_rejected_before_enabling(self): + fake = _FakeDriver() + backend = _build_backend(fake) + samples = self._samples(fake) + fake.positions[Axis.Z] += 2000 + with self.assertRaisesRegex(ValueError, "measured Z"): + await backend._stream_samples(samples) + self.assertFalse(fake.send_calls) + self.assertFalse([name for name, _ in fake.calls if name == "_motors_ensure_enabled"]) + + async def test_rotary_wrap_keeps_caller_samples_unchanged(self): + fake = _FakeDriver() + backend = _build_backend(fake) + backend.configuration.axes[Axis.WRIST].unlimited_travel = True + samples = self._samples(fake) + original = copy.deepcopy(samples) + fake.positions[Axis.WRIST] += 360000 + await backend._stream_samples(samples) + self.assertEqual(samples, original) + self.assertEqual(fake.positions[Axis.WRIST], 360092) + + +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/pyproject.toml b/pyproject.toml index 09b9285fc39..69ed1920c21 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -16,6 +16,7 @@ plr = "pylabrobot.cli:main" [project.optional-dependencies] serial = ["pyserial"] +canopen = ["canopen>=2.4,<3"] usb = ["pyusb", "libusb-package"] ftdi = ["pylibftdi", "pyusb"] hid = ["hid"] @@ -37,7 +38,7 @@ pico = ["PyLabRobot[sila]", "opencv-python", "numpy"] precise-flex-vision = ["numpy", "pillow"] xarm = ["xarm-python-sdk"] odyssey = ["Pillow"] -all = ["PyLabRobot[serial,usb,ftdi,hid,btx,modbus,websockets,visualizer,opentrons,sila,pico,precise-flex-vision,xarm,odyssey]"] +all = ["PyLabRobot[canopen,serial,usb,ftdi,hid,btx,modbus,websockets,visualizer,opentrons,sila,pico,precise-flex-vision,xarm,odyssey]"] test = [ "pytest>=9.1", "pytest-xdist",