add kx2 backend#880
Draft
rickwierenga wants to merge 96 commits into
Draft
Conversation
1 task
# Conflicts: # pylabrobot/arms/__init__.py # pylabrobot/arms/backend.py # pylabrobot/arms/standard.py # pylabrobot/legacy/arms/precise_flex/coords.py # pylabrobot/legacy/arms/scara.py # setup.py
Split KX2 into a transport driver + arm-capability backend, matching the
pattern used by PreciseFlex and xArm6:
- `KX2Driver(Driver)` (was `KX2Can`): owns the CAN bus, CANopen/DS402
primitives, SDO/PDO/NMT, and the Elmo binary interpreter. Adds
`setup()`/`stop()` wrapping the existing `connect()`/`disconnect()`.
- `KX2ArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive)`
(was `KX2Backend`): owns the driver, the IK, and the arm-semantic
methods. Implements the capability interface directly — `halt`,
`open/close/is_gripper_closed`, `move_to_location`, `pick_up/drop_at_*`,
joint variants, `start/stop_freedrive_mode`, `request_{joint,gripper}`.
`_on_setup` runs drive-param read, PDO mapping, motor enable, and
gripper init after the driver's CAN bring-up.
- `KX2(Device)` wires the two into an `OrientableArm` capability.
Moved the module from `pylabrobot/arms/kx2/` to `pylabrobot/paa/kx2/`.
Dropped redundant helpers subsumed by the capability methods
(`get_joint_position`, `get_cartesian_position`, `move_to_cartesian_position`,
`activate_free_mode`, `deactivate_free_mode`). Promoted `MOTION_AXES` to
a module-level constant and retired the cross-class `KX2Backend.` refs.
Added a hello-world notebook at `docs/user_guide/paa/kx2/`.
The underlying CAN protocol code is unchanged; this change is pure
restructuring so the KX2 plugs into the capability-based arm frontend.
Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
Drop ~20 bare `print()` calls across `KX2Driver` and `KX2ArmBackend` in favor of a module-level `logger = logging.getLogger(__name__)`: - errors/exceptions (CAN read/write, catch-all in read task) use `logger.error` / `logger.exception`. - recoverable anomalies (EMCY malformed, unknown COB type, motor-enable failure during setup, unexpected SR register, zero conversion factor) use `logger.warning`. - lifecycle signal (EMCY summary, raise_event) uses `logger.info`. - per-message trace (NMT ignore, digital-input move-done, servo-gripper status, per-axis param read, `motor_send_command`, `motors_move_joint`, `user_program_run` XQ command, EMCY payload dump) uses `logger.debug`. - dev-noise dumps with no diagnostic value (`"EMCY received!!"`, raw EventData/emcy repr, arg-list debug prints, `"node_id not int:"` coercion warning, commented-out TPDO read print) are deleted. No behavior change; these were all fire-and-forget stdout writes. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
New file at `pylabrobot/paa/kx2/kx2_canopen_driver.py` with a skeleton of the canopen-library-backed driver, built side-by-side with the existing `KX2Driver` so the legacy transport stays working throughout development. First phase only — network bring-up + node discovery: - `setup()` connects a `canopen.Network`, broadcasts NMT reset, runs the scanner, broadcasts NMT start, verifies the discovered node set matches the expected `node_id_list`, and attaches a `RemoteNode` per node. - `stop()` disconnects the network. - All other public methods (SDO, PDO, DS402 control, binary/OS interpreter, motor helpers, I/O) are stubbed with `NotImplementedError`. Public method surface mirrors `KX2Driver` so `KX2ArmBackend` will be able to point at either driver without further caller changes. `kx2.py` is not switched over yet — this change is code-only, no behavior change. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
`can_sdo_upload` / `can_sdo_download` dispatch to `node.sdo.upload` / `node.sdo.download`, wrapped in `asyncio.to_thread` because the library's SDO client is blocking I/O (handles expedited + segmented transfers + abort codes on its own). Public signature (`node_id`, `object_byte0`, `object_byte1`, `sub_index`) is preserved for `KX2ArmBackend` parity; internally we reassemble `index = (byte0 << 8) | byte1`. `can_sdo_upload_elmo_object` / `can_sdo_download_elmo_object` port the type-dispatched byte packing from the legacy driver verbatim — they only depend on the low-level SDO primitives above, so no library-specific logic. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
Elmo's binary interpreter is a vendor protocol that rides on PDO2 COB-IDs (0x300+nid for request, 0x280+nid for response) and is not part of CANopen proper, so it cannot go through `node.pdo`. We send via `network.send_message` and receive via `network.subscribe` per node. Plumbing: - `setup()` subscribes to each node's TPDO2 cob_id with a per-node callback that marshals decoding into the asyncio loop via `loop.call_soon_threadsafe` (the canopen listener runs on its own thread). - Incoming frames are decoded into `(msg_type, msg_index, value_str)` and used to resolve a pending future in `self._pending_bi`, keyed by (node_id, msg_type, msg_index). - `binary_interpreter()` builds the 8-byte (or 4-byte for execute) payload identically to the legacy driver, registers futures, sends via `network.send_message`, and awaits with 1s (or 10s for SV). - Group path (node_id=10): fans out to futures for each motion axis and gathers; query returns comma-joined values as the legacy did. Low-priority flag is ignored — `network.send_message` has no priority queuing analogue. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
…Driver Ports the motor/motion primitives from the legacy driver to the canopen class. Logic is unchanged — only the transport underneath differs. Raw CANopen sends (no library PDO machinery — the drive side just has to be mapped to receive them, which connect_part_two will handle): - `can_sync()` → `network.send_message(0x80, b"")`. - `control_word_set(nid, value, sync)` → `network.send_message(0x200+nid, <u16 LE>)` then optional SYNC. Motor helpers, all delegating to `binary_interpreter` / `can_sdo_*`: - `motor_emergency_stop`, `motor_get_current_position`, `motor_get_fault` (full bitfield decode), `motor_get_motion_status`, `motor_check_if_move_done`, `motor_set_move_direction`, `motor_set_homed_status`, `motor_get_homed_status`, `motor_reset_encoder_position`, `read_input`, `read_output`, `set_output`. `motor_enable`: DS402 controlword sequence (0→128→6→7→15 enable, 7→6 disable) for motion axes; MO binary-interpreter command for gripper/non- motion axes. `pvt_select_mode`: standard SDOs on 0x60C4:06 and 0x6060 across all motion axes. `motors_move_absolute_execute`: SDO writes for target position (0x607A), velocity (0x6081), accel/decel (0x6083/4), then `_motors_move_start` (controlword 47 → 63), then `_wait_for_moves_done`. The wait currently falls back to polling via `motor_check_if_move_done` because TPDO4 digital-input callbacks aren't wired yet; PDO mapping lands next. `home_motor`: still `NotImplementedError` — depends on `os_interpreter` (segmented SDO over 0x1023/0x1024) and motor_hard_stop_search, both pending. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
Ports the drive initialization sequence that runs after the CAN bus is up. Logic is identical to the legacy driver — the transport for every SDO underneath is just `node.sdo.download` via our earlier `can_sdo_download` implementation. Per-node mapping: - Unmap TPDO1. - Map TPDO3 to `StatusWord`, triggered on `MotionComplete`. - Map TPDO4 to `DigitalInputs`, triggered on `DigitalInputEvent`. Per motion axis: - Write Elmo vendor DS402 configuration (objects 24768, 24772:2/3/5, 24770:2, 24669:0) to enable interpolated position mode defaults. - Map RPDO1 to `ControlWord` (synchronous cyclic — the DS402 state machine writes we send via `control_word_set`). - Map RPDO3 to `TargetPositionIP` + `TargetVelocityIP` (event-driven; used by `motors_move_absolute_execute`). Move-done plumbing: subscribe each node's TPDO3 cob_id via `network.subscribe`. Because TPDO3 fires on the `MotionComplete` event, any TPDO3 frame arriving for an axis with a pending `_waiting_moves` future resolves it — replacing the legacy `_process_tpdo_message` path. The digital-input TPDO4 handler (edge-detected inputs for hard-stop search) isn't wired yet; that path is only needed for homing. PDO mapping helpers (`_tpdo_map`, `_rpdo_map`, `can_tpdo_unmap`) use raw SDO writes into 0x14xx/0x16xx/0x18xx/0x1Axx + the vendor 0x2F20 event mask — same byte-level sequence as the legacy `can_tpdo_map`/ `can_rpdo_map`, but built from plain `can_sdo_download` calls. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
`os_interpreter` now uses plain `node.sdo` against 0x1024 (OS Command Mode) and 0x1023 (OSCommand record). The canopen library transparently handles expedited vs. segmented SDO transfers and the toggle-bit dance, replacing ~260 lines of hand-rolled SDO framing in the legacy driver with ~20 lines that delegate to `node.sdo.download` / `node.sdo.upload`. Error decoding via the command status byte is unchanged. `user_program_run`, `motor_hard_stop_search`, `motor_index_search`, and `home_motor` are ported verbatim from the legacy driver. They're just compositions of `binary_interpreter`, `os_interpreter`, `motors_move_absolute_execute`, and `_wait_for_moves_done` — no transport-specific logic — so the logic is identical. Full `KX2Driver` public surface is now covered on `KX2CanopenDriver`; `KX2ArmBackend` can be pointed at either driver. `kx2.py` isn't switched over yet — that's the final step, once the hello-world notebook runs clean against this driver on hardware. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
`KX2Canopen` is a drop-in replacement for `KX2` that wraps `KX2CanopenDriver` underneath. Both devices share the exact same `KX2ArmBackend` capability frontend, so the hello-world notebook can switch between them by importing `KX2Canopen` instead of `KX2`. The legacy `KX2` stays in place until the new driver is validated on hardware; then `KX2` → `KX2Canopen`, the legacy driver + class are deleted, and `KX2Canopen` is renamed back to `KX2`. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
canopen defaults `SdoClient.RESPONSE_TIMEOUT` to 0.3s, which is tight for Elmo drives replying to vendor-object SDOs. Match the 1s the legacy driver waited on its own futures. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
Setup cell now imports `KX2Canopen` (canopen-library driver) instead of `KX2` (legacy hand-rolled transport). All other cells are unchanged — both devices share the same `KX2ArmBackend` frontend, so the arm-level API (`kx2.arm.*`) is identical. Added a note in the setup section pointing at the legacy class in case we need to A/B test on hardware. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
The byte at 0x1023:2 is the CiA-301 OS-command lifecycle state, not an
error flag. For an async dispatch like \`XQ##Home(...)\` the drive
correctly returns 0x01 ("command is being executed") immediately — the
caller (\`user_program_run\`) then polls PS/UI for completion. The
legacy driver never inspected this byte; it only looked for "ABORT" in
the response text, which canopen surfaces via \`SdoAbortedError\` from
the SDO upload itself. Log the status at debug level for diagnostics
and move on.
Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
Hardware-validated the canopen-backed path end-to-end (setup, homing, joint & cartesian moves, gripper). Promoting it to the default and deleting the legacy hand-rolled CAN transport. Renames: - \`KX2CanopenDriver\` → \`KX2Driver\`, file \`kx2_canopen_driver.py\` → \`kx2_driver.py\` (git-tracked rename). - \`KX2Canopen\` → \`KX2\` (the \`KX2Canopen\` transitional class and the legacy \`KX2\` class are both gone). Deletions in \`kx2_backend.py\`: - The entire legacy \`KX2Driver\` (née \`KX2Can\`) class — ~2560 LOC of hand-rolled CAN transport, queue-backed read/write tasks, raw SDO/PDO framing, segmented-SDO toggle-bit dance, EMCY processing, heartbeat tracking, DS402 controlword dispatch. - Supporting types that only the legacy transport used: \`_u32_le\` helper, \`InputLogic\`, \`EventType\`, \`MoveType\`, \`EventData\`, \`ErrCtrl\`, \`PVT_EMCY\` / \`PVT_EMCY_QueueLow\` / \`PVT_EMCY_QueueFull\`, \`Emcy\`, \`Query\`, \`CAN_Msg\`, \`NodeInputConfig\`, \`ElmoObject\` (the 120-entry enum). - Now-unused imports (\`can\`, \`struct\`, \`time\`, \`field\`, \`Any\`, \`Tuple\`, \`Union\`, \`Driver\`). Driver moved behind \`TYPE_CHECKING\` to avoid a circular import. Kept: enums the canopen driver still uses (\`COBType\`, \`RPDO\`, \`TPDO\`, \`PDOTransmissionType\`, \`RPDOMappedObject\`, \`TPDOMappedObject\`, \`TPDOTrigger\`), plus arm-side helpers (\`_is_number\`, \`_to_float\`) and public types (\`KX2Axis\`, \`MOTION_AXES\`, \`JointMoveDirection\`, \`HomeStatus\`, \`CmdType\`, \`ValType\`, \`ElmoObjectDataType\`, \`CanError\`, \`MotorMoveParam\`, \`MotorsMovePlan\`). Notebook updated to import \`KX2\` directly; setup note removed since there is no longer an alternative driver to opt between. Net: -2,850 LOC across the module. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
- \`motor_enable\` (DS402 path): add 10ms sleep between controlwords so the drive's state machine can settle between Fault → Shutdown → Switched On → Op Enabled transitions. With the legacy driver this gap came from queue drainage; \`network.send_message\` is synchronous and fires back-to-back, so \`stop_freedrive_mode\` was observing motors still in an intermediate state when it queried MO and raising \"Motor failed to enable\". - Notebook pick/drop cell: remove spurious \`resource_width=30\` from \`drop_at_location\` — the OrientableArm frontend reuses the width captured during the matching \`pick_up_at_location\`, so passing it twice fails with a TypeError. - Notebook fault-diagnostic cell: import \`KX2Axis\` so the cell runs standalone without depending on the joint-move cell above it. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
Prefix 18 internal-only methods on \`KX2Driver\` with \`_\` so they don't show up in tab-completion as if they were user-facing. Arm-backend callers (\`self.driver.X\`) updated to match. Privatized (internal plumbing — called only from \`KX2Driver\` or \`KX2ArmBackend\` internals): - Lifecycle init: \`_connect_part_two\`. - SDO transport: \`_can_sdo_upload\`, \`_can_sdo_download\`, \`_can_sdo_upload_elmo_object\`, \`_can_sdo_download_elmo_object\`, \`_can_tpdo_unmap\`. - DS402 state-machine internals: \`_can_sync\`, \`_control_word_set\`, \`_motor_enable\`, \`_pvt_select_mode\`. - Motion plumbing: \`_motors_move_absolute_execute\`, \`_motor_set_move_direction\`, \`_motor_set_homed_status\`, \`_motor_reset_encoder_position\`. - Homing orchestration: \`_user_program_run\`, \`_motor_hard_stop_search\`, \`_motor_index_search\`, \`_home_motor\`. Kept public (genuine user-facing API + diagnostics): - \`setup\`, \`stop\`, \`motor_emergency_stop\`, \`motor_get_current_position\`, \`motor_get_motion_status\`, \`motor_check_if_move_done\`, \`motor_get_homed_status\`, \`motor_get_fault\`, \`read_input\`, \`read_output\`, \`set_output\`, \`binary_interpreter\` + \`os_interpreter\` (power-user escape hatches). Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
- \`binary_interpreter\`, \`os_interpreter\`: raw Elmo vendor-command escape hatches. Users invoke them via the arm backend (or the \`motor_*\` diagnostic wrappers) rather than directly. - \`read_input\`, \`read_output\`, \`set_output\`: wire-level digital I/O on the drive. \`KX2ArmBackend.read_input\` already wraps this with the correct \`0x10 +\` offset convention; direct use would bypass that. Public driver surface is now just \`setup\`, \`stop\`, and the six diagnostic \`motor_*\` queries (position, motion status, move-done, homed status, fault, emergency stop). Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
Both methods were pure protocol-level concerns — auto-routing between
the binary and OS interpreters, and decoding a drive's status register —
with no dependency on arm-level state. They belonged on the driver, not
the arm backend.
- Add \`motor_send_command\` + \`get_estop_state\` to \`KX2Driver\` (public).
\`motor_send_command\` also bundles the \`_OS_INTERPRETER_CMDS\` and
\`_NO_QUERY_CMDS\` classification tables as class constants.
- Delete them from \`KX2ArmBackend\`.
- Rewrite ~60 internal calls: \`self.motor_send_command(...)\` →
\`self.driver.motor_send_command(...)\`.
Also fix two stale references exposed by the interpreter privatization:
- \`self.driver.read_input\` → \`self.driver._read_input\` in the arm
backend's \`read_input\` wrapper.
- Six \`self.binary_interpreter\` / \`self.os_interpreter\` internal calls
in the driver that sed missed earlier (all under \`_home_motor\` /
\`_user_program_run\` / \`_motor_hard_stop_search\` /
\`_motor_reset_encoder_position\` etc.) → \`self._binary_interpreter\` /
\`self._os_interpreter\`.
Notebook: \`kx2.arm.backend.get_estop_state()\` → \`kx2.driver.get_estop_state()\`.
Post-split public surfaces:
- Driver: \`setup\`, \`stop\`, \`motor_send_command\`, \`get_estop_state\`,
\`motor_emergency_stop\`, \`motor_get_{current_position,motion_status,
fault,homed_status}\`, \`motor_check_if_move_done\`.
- Arm backend: 15 capability methods, kinematics helpers, servo-gripper
orchestration, motion planning — no protocol-level logic.
Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
Five methods were privatized in the earlier pass but are legitimately called from the arm backend across the class boundary — that's a layering violation (\`self.driver._X\` from a consumer). Restore them as public driver API: - \`connect_part_two\` (called from \`_on_setup\`) - \`motor_enable\` (called from \`_on_setup\` + freedrive toggles) - \`home_motor\` (called from \`servo_gripper_home\`) - \`motors_move_absolute_execute\` (called from \`motors_move_joint\`) - \`read_input\` (called from the arm backend's wrapper) Truly internal helpers that only other driver methods call (SDO, PDO mapping, DS402 controlword, SYNC, PVT mode, homing orchestration pieces, binary/OS interpreter primitives, \`read_output\`/\`set_output\`) stay private. Post-change: no \`self.driver._*\` call appears anywhere in \`kx2_backend.py\`. The driver/backend boundary now matches the prefix convention. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
The KX2's onboard barcode reader is a plain RS-232 device wired to the controller PC, completely independent of the CAN motor stack. Evidence from the vendor C# sources (\`KX2RobotControl.cs:15573–15848\`): it's opened via \`System.IO.Ports.SerialPort\` at 9600 8N1, driven with ESC-prefixed, CR-terminated commands (\`Z\` trigger, \`Y\` stop, \`S0/S1/S2\` read modes, \`Y1..Y9/YM\` read time, \`Z1\` version handshake), and delivers decoded data asynchronously as \`<data>\\r\` on the same port. Added \`pylabrobot/paa/kx2/kx2_barcode_reader.py\` containing: - \`KX2BarcodeReaderDriver(Driver)\` — owns the \`Serial\` I/O. Public \`send_command(cmd, timeout) -> str\` builds \`ESC + cmd + CR\`, reads until CR. Named helpers: \`trigger(on)\`, \`set_read_mode(mode)\`, \`set_read_time(seconds)\`, \`set_auto_trigger(on)\`, \`get_software_version()\`. Teardown sends \`Y\` + \`Y2\` to leave the reader in trigger-off / 2 s-readtime state (mirrors C# 15623–15624). - \`KX2BarcodeReaderBackend(BarcodeScannerBackend)\` — capability adapter. \`_on_setup\` does the \`Z1\` version handshake, sets single-read mode, configures read time. \`scan_barcode()\` fires \`Z\` trigger then listens for the next CR-terminated line (no command-response — the read is an asynchronous event on the same port). - \`KX2BarcodeReader(Device)\` — standalone device, wired with a single \`BarcodeScanner\` capability exposed as \`bcr.barcode_scanning\`. Lives separately from \`KX2\` (the motor device) because \`Device\` owns one driver; users instantiate both as needed. Notebook: added a "Barcode reader" section demonstrating setup, scan, teardown. Open item before shipping to hardware: confirm port discovery (likely \`/dev/ttyUSB1\` on Linux or similar) and verify the factory-default baud via the \`Z1\` handshake. The plan-agent output and the C# reference both point to 9600 8N1 as the starting guess. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
The vendor C# (\`KX2RobotControl.cs:15641\`) builds the command string as \`ESC + cmd + "\r"\` and sends it via \`SerialPort.WriteLine\`, which appends \`SerialPort.NewLine\` (default \`"\n"\`) — so the actual frame on the wire is \`ESC + cmd + "\r" + "\n"\`, not just \`ESC + cmd + "\r"\`. Fix the driver to match. Other settings confirmed correct against the C# (which uses bare \`new SerialPort()\` + only \`BaudRate\`/\`PortName\` assignments): 9600 8N1, Handshake.None, DTR=false, RTS=false. Responses are split on CR alone by the DataReceived handler. Notebook: expand the "Barcode reader" section with supported symbology list (Microscan MS-3/MS-4/QX-830 standard 1D set), a setup cell pointed at \`/dev/tty.usbserial-FTE1YWTI\`, separate cells for scan + driver-level controls (read mode, read time, auto-trigger, version), and teardown. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
…ontract driver.py: - _wait_setpoint_ack now waits on a per-node asyncio.Event fed by TPDO3 pushes instead of polling 0x6041 via SDO every cycle. TPDO3 trigger flipped from MotionComplete to StatusWordEvent (Elmo trigger 27) with 1 ms inhibit; canopen listener thread parses 2-byte SW and marshals it onto the loop, the cache + event are cleared in stop(). 5 ms grace before the SDO fallback covers a stuck event-trigger config so a silent drive can't hang motion. Hardware-verified. - _can_sdo_upload / _can_sdo_download take a 16-bit `index` argument instead of (object_byte0, object_byte1). The split was a Python-side artifact — every call site reconstructed the same number. 27 sites updated; PDO-config sites use 0x1400 / 0x1600 / 0x1800 / 0x1A00 + idx for readability. - pvt_select_mode(True) when already in PVT now matches the C# 3-step re-arm (clscanmotor.cs:6014-6031): PPM (0x6060=1) -> IP-buffer reset (0x60C4:6=0) -> IPM (0x6060=7). The previous Python single-write left the drive in PPM with _pvt_mode lying. Dead path today (IPM runtime not ported) but cheap insurance. - _make_tpdo3_callback warns on <2-byte frames so a misconfigured drive doesn't fail silent (cache empty -> SDO fallback forever). kinematics.py: - plan_joint_move with a gripper-speed/accel cap requires `current` to include all four arm axes; raises ValueError naming the missing ones instead of the bare KeyError that fk used to throw. The cap helper evaluates the FK Jacobian at the start pose, and each moving axis's Jacobian column depends on every other arm axis's absolute position — filling missing axes with 0 would give wrong velocity bounds. Production never trips this (orchestrator passes full current); the improved error is for direct callers. tests/kinematics_tests.py: - test_z_offset_sign_convention pins the documented "positive z_offset = clamp sits below the wrist plate" convention. FK with same joints + larger z_offset must give a lower gripper z; IK for the same gripper pose must give a higher wrist Z. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
…S, tests driver.py: - _send_bi takes a per-(node, cmd, idx) asyncio.Lock around the install + send + await cycle, acquired via contextlib.AsyncExitStack so a group broadcast holds all four locks in LIFO order. Two concurrent callers with the same key now serialize instead of cancelling each other's pending futures. Motion lock already covered the common path; this closes the race for direct driver callers (notebook diagnostics) and any gathered queries with overlapping keys. The cancel-old-future defensive code is gone — with the lock, no second caller can install a future until the first completes — and the cleanup moved to a finally so non-Timeout exceptions don't leak pending futures. - _poll_axis swallowed CanError silently; now logs at DEBUG so a wedged bus shows up in logs instead of just burning the full timeout. kinematics.py: - convert_elbow_position_to_angle clamps the asin argument to [-1, 1]. Floating-point overshoot at the joint limit (e.g. encoder reads max_travel + 1e-12) used to raise ValueError on a physically reachable pose. xfail test in planner_tests.py flips to a real assertion. - Skip-axis encoder vel/accel use max_vel * |conv| / max_accel * |conv| instead of a hardcoded 1000.0. Hardware data showed 1000 was 0.03–4% of firmware max across axes — a stale profile register would leave a follow-up move pathologically slow. Skipped axes don't move so the value is nominal in normal operation; consistent with non-skip axes is the principled choice. - KX2Config.eps moved to a module-level kinematics._EPS. Only kinematics read it; carrying it as a config field made it look drive-supplied. tests/kinematics_tests.py: - test_ik_shoulder_branch_convention pins shoulder = -degrees(atan2(x, y)) across all four quadrants and the axis crossings. C# uses four hand- rolled atan branches with quadrant offsets (KX2RobotControl.cs:7195- 7268); Python's atan2 + the boundary snap at line 113 (-180 -> 180) produces an identical result for every reachable pose. Test exists so a future drive-by simplification can't silently flip a sign. config.py: drop KX2Config.eps. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
motor_check_if_move_done's MF-bit-polling path was raising "Motor Fault:
{decoded}" with no node identifier — wait_for_moves_done collects faults
from N axes in parallel and the operator couldn't tell which one tripped.
_trigger_new_setpoint already named the node but used "node {nid}" instead
of the EMCY path's "Axis {nid}" format. Both now match the EMCY format
("Axis {nid}") for consistent error parsing across the fault paths.
kinematics.py: shoulder boundary-snap comment trimmed to the operative
fact (atan2 returns -180 on the -Y axis; we want it on the +180 side
of an exclusive max convention).
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
Existing AccelSync / TimeSync tests used directional asserts (assertLess / Greater) which won't catch a refactor that produces different-but-still- monotone values. Adding 5 tests that pin exact encoder vel/accel/position and move_time across: - single-axis trapezoidal (dist=100, v=100, a=200 -> t=1.5s) - single-axis triangular (dist=10, v=100, a=200 -> v_actual=45, t=0.4472) - accel-sync 2-axis (shoulder slow lead -> z's accel scales 1000 -> 14) - time-sync 2-axis (shoulder slow vel lead -> both at v=10, a=20, t=10.5) - 4-axis with real KX2 conv factors (full pipeline incl. elbow asin) Tolerance: ±1 on int encoder values (rounding noise from float ops); 4 decimals on move_time. These pin the per-axis profile + accel-sync + time-sync chain so the upcoming closed-form rewrite has a numeric contract. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
…er calls plan_joint_move had two near-identical direction-aware delta blocks (one for cap_deltas in cmd-units, one for dist in angle-units) and two near-identical 9-line gripper-cap helper invocations. Both collapse: - _directional_delta(target, current, ax_cfg) -> float: one helper, two one-line callsites. Behavior preserved (CW / CCW / ShortestWay / Normal + literal-delta default for non-unlimited axes). - The two arm_kinematics.joint_velocities_for_max_gripper_speed calls parameterize over (cap_value, max_field) pairs in a single loop. Net -25 lines. All 92 tests + 1 xfail pass; numeric pins in SyncAlgorithmExactValues confirm the algorithm output is unchanged within ±1 encoder count. The three-pass _profile recompute stays — two passes are load-bearing (baseline → accel-sync feeds time-sync's lead picker), the third catches edge cases where time-sync's k-scale slips into triangular or hits the denom-near-zero short-circuit. The closed-form quadratic solve I sketched (v = (aT − √(a²T² − 4·a·dist))/2) doesn't capture accel-sync's a-reduction, so it would change observable trajectories — caught by the value-pinning tests added in d6dd639. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
motor_enable used to write the controlword sequence (0, 128, 6, 7, 15) back-to-back over RPDO1 (mapped SynchronousCyclic). The drive transitions out of Fault on the rising edge of CW bit 7 (= 128). When consecutive RPDO frames coalesce within one servo cycle, the edge is invisible and the fault never clears — visible symptom: 1-in-N motor_enable failures on busy buses or after a hard fault. Now the enable sequence pauses after each transition until the statusword confirms the new state: - CW=0x00 -> clear bits - CW=0x80 -> fault reset (rising edge of bit 7) - wait SW bit 3 (Fault) cleared - CW=0x06 -> Shutdown, wait SW bit 0 (Ready to switch on) - CW=0x07 -> Switch on, wait SW bit 1 (Switched on) - CW=0x0F -> Enable op (bit 2 confirmed by the existing MO query) The 20-attempt retry loop wraps the whole sequence as before. Side cleanup: the TPDO3-push helper that _wait_setpoint_ack used got generalized over an arbitrary SW bit_mask. _wait_setpoint_ack is now a one-line wrapper around the new _wait_sw_bit; the SW polls in the enable sequence use the same helper. The SDO fallback branch now writes back into self._statusword[node_id] instead of discarding the read. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
…ction
convert_elbow_position_to_angle (forward) for pos > max_travel was:
angle = 90 + asin((2*max - pos + zero_offset) / (max + zero_offset))
convert_elbow_angle_to_position (inverse) for angle > 90 was:
pos = (max + zero_offset) * sin(angle°) - zero_offset
pos = 2*max - pos # <- reflection without + zero_offset
The reflection dropped the matching `+ zero_offset` term, so the round-trip
drifted by ~zero_offset · (max + zero_offset) / max — at zero_offset=5,
max=300: ~287 mm error at pos=300.5. Latent in normal use because the
planner only calls pos -> angle, never angle -> pos for >90° values, but
motor_get_current_position calls the buggy direction on encoder reads. A
homing miscalibration that put the elbow past 90° in encoder space would
have silently misreported joint position.
The correct inverse derives directly from the forward formula by solving
for pos:
pos = 2*max + zero_offset - (max + zero_offset) * sin((angle - 90)°)
test_round_trip_above_90 (xfail since the bug was first pinned) now
passes. Verified pre-fix: AssertionError 587.54 != 300.5 within 6 places.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
home_motor's only caller (servo_gripper_home) always passed Axis.SERVO_GRIPPER. The four motion axes use absolute encoders and don't need this hard-stop + index-pulse routine; the rail isn't homed by the driver in any current setup. Refactor to make the gripper-only contract explicit: home_motor(axis, sgc) -> _home_servo_gripper(sgc) _motor_hard_stop_search(axis, …) -> _gripper_hard_stop_search(…) _motor_index_search(axis, …) -> _gripper_index_search(…) _motor_reset_encoder_position(…) -> _gripper_reset_encoder_position(…) _motor_set_homed_status(…) -> _gripper_set_homed_status(…) motor_get_homed_status(axis) -> gripper_get_homed_status() `nid = axis` indirection gone in all four helpers — they hardcode Axis.SERVO_GRIPPER as `sg`. The "Error 43" magic-number RuntimeError became a descriptive message that names what the drive is reporting (CA[41] value) and what to try (power-cycle / re-check homing config). Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
FKIKRoundTrip already tested fk(ik(p)) == p, but pure round-trip can't catch a sign flip or unit error that's symmetric across the inverse pair. Adding 9 anchor tests that pin specific (joints) -> Cartesian and (Cartesian, yaw) -> joints values for representative poses across all quadrants and the axis crossings. Values were snapshotted from the current implementation; if you change FK or IK and these fail, the new output is provably different from the old one and needs explicit hardware verification. Coverage: zero pose, Q1/Q2 mid, max extension, wrist sign flip; IK on +Y axis, Q1/Q2 targets, negative quadrant (shoulder past 90°), gripper finger-side flip handling. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
…nd_z Closes the §2.5 #1 driver-test gap with three new files (+25 tests, total 127 passing). Pure-Python with stubbed driver/backend instances; no CAN bus. driver_setpoint_trigger_tests.py — 6 tests pinning _trigger_new_setpoint: - happy path on attempt 1 - retry then success on attempt 2 - all max_attempts exhausted -> CanError with "Axis {nid}" and "after N attempts" substrings - bit-12-stays-high after cw_low restarts the state machine - max_attempts=1 boundary Fake-driver approach: scripted state machine, _control_word_set steps it, mutating self._statusword[nid] + signalling self._statusword_event[nid]. arm_backend_read_axis_config_tests.py — 13 tests pinning every validation branch in _read_axis_config: - UF[1]/UF[2] == 0 -> "Invalid motor conversion factor" - travel-limit / modulo combo not in {bounded, unlimited} -> "Invalid travel limits or modulo settings" - CA[45] not in (0, 4] -> "Invalid encoder socket" - enc_type not in {1, 2, 24} -> "Unsupported encoder type" Plus happy-path coverage: bounded vs unlimited travel, motion vs non-motion ShortestWay flip, SP[2] sentinel, CA[45] != CA[46] FF[3] denom, encoder type 1/2 setting absolute_encoder=False. arm_backend_find_z_proximity_tests.py — 6 tests pinning find_z behavior: - sensor trips mid-descent -> motor_stop + IL restore (cleanup order) - sensor never trips -> RuntimeError naming descent range and z0/z_end - move raises CanError mid-descent -> cleanup still runs - already-tripped at start -> returns z0 without spawning the descent Subtle: _read_proximity needs asyncio.sleep(0) so the spawned task gets scheduled before the trip check observes True. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
CartesianMoveParams gains a `path` field; default 'joint' keeps today's
joint-trapezoid behavior, 'linear' streams a straight tool-tip path
through the drive's interpolation buffer.
Driver primitives (driver.py): ipm_select_mode (0x6060=7), ipm_set_time_interval
(0x60C2:01), ipm_send_pvt_point (sync — network.send_message doesn't block),
ipm_begin_motion (CW=0x1F, SYNC on last), ipm_stop (CW=0x0F),
ipm_clear_queue (0x60C4:6=0), ipm_check_queue_fault (reads _ipm_emcy
queue_full/underflow), ipm_wait_motion_complete (polls SW bit-10, the
authoritative IPM-done signal — MS goes to 0 transiently between
buffered points). motors_ensure_enabled re-enables disabled drives in
parallel; shared by every motion-trigger site (PPM, IPM, setup,
freedrive-exit, find_z preflight). motors_move_start renamed
ppm_begin_motion for symmetry with ipm_begin_motion.
Sampler (kinematics.py sample_linear_path): builds a dt-aligned
trapezoid in arc-length, IKs each waypoint, emits per-axis (P, V)
encoder samples. Trailing hold sample makes the FD-derived final
velocity exactly 0 — without it, cubic-Hermite interpolation through
(P_{n-1}, V≈cruise) → (P_n, V=0) would overshoot before snapping back.
Pure rotation in place raises NotImplementedError instead of
repurposing mm/s caps as deg/s.
Runtime (arm_backend.py _run_linear_path): preload 8 frames, capture
pacing reference *before* begin_motion (drive starts consuming on
SYNC; capture-after underestimates elapsed drive-time), stream
remainder with 8-ahead time-based pacing, check queue-fault EMCY
after preload and after each send, wait on bit-10, finally-block
cleanup that always reverts to PPM. Dispatched from move_to_location
when path='linear'. Requires max_gripper_speed and max_gripper_acceleration
since the Cartesian profile is built from them directly.
EMCY (_IpmEmcyState): adds `underflow` field so post-fact 0x8A is
distinct from proactive 0x56 queue_low. _ipm_mode bookkeeping now
pessimistic — partial-failure paths leave consistent state for the
next call's re-arm logic.
Tests: 144 total (was 142). New arm_backend_linear_path_tests.py
(7 tests covering preload/begin order, dt, mode flip, ipm_check_queue_fault
hooks, ipm_wait_motion_complete in place of MS-based wait, cancel
cleanup, validation). 8 new sampler tests in kinematics_tests.py
(endpoints exact via FK roundtrip, samples colinear, dt scales sample
count, last sample velocity 0, FD matches central diff, yaw short-way,
pure rotation raises). Driver EMCY tests updated for underflow split;
find_z tests updated to use ensure_enabled.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
Pass `barcode_port` to KX2() and the onboard scanner shows up as `kx2.barcode_scanning`. No port → no capability, unchanged behavior. Standalone KX2BarcodeReader Device still works for users who prefer to manage the scanner sibling-style. Setup opens the serial port before the CAN bus so the version handshake during cap._on_setup has a live port. Setup failure tears the BCR back down before propagating, so a CAN init crash doesn't leak the serial port. stop() reverses. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
KX2BarcodeReader and KX2BarcodeReaderBackend no longer take a read_time constructor arg. The capability's scan(read_time=...) is the only public surface for it; KX2BarcodeReaderBackend pushes the requested window to the device per call (Y1..Y9 on the wire). KX2.__init__ loses barcode_read_time too. The Device-level set_read_time wrapper is gone; the wire-protocol helper on KX2BarcodeReaderDriver stays for power users.
The driver's read_decoded_barcode raises BarcodeScannerError on serial read timeout; at the capability layer that's the "nothing seen in window" signal, so swallow it and return None to match the abstract Optional[Barcode] contract.
The driver tracked EMCY state via four scattered fields — `_ipm_emcy[nid]`
(IPM queue counters), `emcy_move_error_received: bool`,
`emcy_move_error: str`, and `emcy_move_error_node_id: Optional[int]` —
plus a separate `last_emcy: Optional[EmcyFrame]`. Three concerns, one
source of truth: per-node `_NodeEmcyState` carrying queue counters,
sticky `move_error` (preformatted with axis context, None when no
fault), and `last_frame`. Global views are derived properties.
`motor_check_if_move_done` now reads:
pending = self.emcy_move_error
if pending is not None:
raise RuntimeError(f"Motor Fault: {pending}")
`clear_emcy_state(node_id)` resets that node's whole struct;
`clear_emcy_state()` clears `move_error` across all nodes but preserves
queue counters (stream-scoped, reset by ipm_begin_motion).
`find_z_with_proximity_sensor` now calls `clear_emcy_state(Axis.Z)` in
its finally block. The IL-trip and motor_stop fire EMCY frames that set
move_error; the trip is expected, not a fault, so without clearing the
next motion call's `motor_check_if_move_done` raised on a stale flag.
Renames: `_IpmEmcyState` -> `_NodeEmcyState` (broader scope now);
`self._ipm_emcy` -> `self._emcy`. Tests updated.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
The breakbeam read + find_z descent is a self-contained workflow with its own setup pattern (position the arm above an object, open gripper wider). Pulling it out of the main hello-world notebook keeps that one focused on the arm motion API. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
The old `find_z_with_proximity_sensor(max_descent, z_start=None)` mixed a relative descent distance with an optional absolute start. Callers think in absolute Z, not deltas, so they kept computing `max_descent = z_top - z_bottom` at every site. New signature takes both bounds directly: `find_z_with_proximity_sensor(z_start, z_end)`, both required, both absolute, search descends. z_start is now always the pre-position target — the conditional "only pre-move if z_start given" branch is gone, and the "search from current Z" mode goes away with it. If you want to start from the current Z, pass it explicitly. Validates `z_end < z_start` upfront so a transposed-args bug raises before any drive interaction. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
`find_with_proximity_sensor(start, end, direction)` sweeps the gripper along a straight Cartesian line and halts when the IR breakbeam trips. Yaw held constant at `direction` for the sweep. Returns the gripper location at halt; raises if the beam never tripped. The existing `find_z_with_proximity_sensor` keeps its IL[4]=StopForward fast path (sub-ms drive-side halt on the input edge) — that only works on Z because the breakbeam is wired to the Z drive's I/O. X/Y motion software-polls at 100 Hz; ~0.25 mm overshoot at 25 mm/s, plus ~64 ms of PVT buffer-drain coast after cancel. Documented on the method. Pre-position with a joint move to `start` (path doesn't matter for the approach), then run a linear (PVT) sweep to `end` while a parallel poll loop watches the sensor. On trip, cancel the sweep task — its finally sends ipm_stop + reverts to PPM. Sticky EMCY cleared so the next motion call doesn't raise on the halt-induced frame. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
Mirrors the proximity-sensor split. Hello-world drops from 46 to 35 cells; new `barcode-reader.ipynb` carries the scan + config + driver sections, plus its own setup/teardown so it runs standalone. Toctree gets one new entry. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
Yaw doesn't matter for proximity sensing — the breakbeam doesn't care which way the gripper is pointed. Asking for a `direction` forced callers to think about wrist angle they otherwise wouldn't, just to sweep an X/Y/Z line. Now reads the current gripper yaw at the start of the call and holds it constant for the sweep. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
…ip center) The old kinematics treated `location` as a finger tip equidistant from the wrist axis on both finger sides, so flipping `finger_side` for the same target left shoulder/elbow/Z untouched and only rotated the wrist 180°. That's wrong for the actual gripper, where the assembly hangs `t.length` off the wrist axis along its extension direction: physically swapping which finger faces forward requires the wrist motor to rotate 180° AND the wrist axis to swing to the other side of the grip center. FK/IK now route `location` through the extension direction (not the front-finger direction), so the same `(grip-center, yaw)` target gives wrist-axis solutions 2·t.length apart for the two finger-side choices. Verified against a real calibration: two recorded coords for the same plate, 200 mm apart along the front-finger axis, agree on t.length ≈ 100 mm to within 0.07 mm on both axes — exactly what the new model predicts. For barcode_reader the formulas reduce identically to the old ones, so all existing FK/IK anchor tests still pass; the two GripperFingerSide tests that encoded the old symmetric model are replaced. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
The method returns a CartesianPose (location + rotation), so 'location' was a misnomer. Renamed across the ABC, the Arm wrapper, and all backends (xArm6, Hamilton STAR/iSwap/core, Brooks PreciseFlex, legacy adapters), plus user-guide notebooks/markdown.
Brings the ABC-level rename from v1b1 and applies the same rename to the KX2 backend (override, internal callers, test mocks, notebook). Conflict resolution: v1b1 renamed the method; kx2-backend's preceding GripperLocation → CartesianPose work renamed the return type. Both renames land in the merged tree (method = request_gripper_pose, return = CartesianPose).
9 tasks
…ints
Adds two streamed-motion APIs on KX2ArmBackend backed by the same IPM
(Position-Velocity-Time) transport as the existing path='linear' move:
arm.backend.move_parametric(path_fn, duration_s)
Evaluates path_fn(t) at every IPM sample tick (8 ms) and streams
the resulting Cartesian poses. Useful for math-defined paths
(figure 8, helix, Lissajous).
arm.backend.move_through_waypoints(waypoints, speed, accel)
Smooth centripetal Catmull-Rom spline through a list of poses,
time-reparametrized to a trapezoidal arc-length profile capped at
speed (mm/s) and accel (mm/s²). No stop at intermediate waypoints.
Backend cleanup behind these APIs:
* _run_linear_path refactored: trajectory sampling stays in kinematics
(sample_linear_path / sample_parametric_path / sample_waypoint_path,
sharing _build_samples_from_joints + _ik_pose_sequence), and the IPM
dance is extracted into _stream_samples.
* _stream_samples replaces the SW bit-10 wait at end-of-stream with an
immediate ipm_stop. Elmo drives don't latch bit-10 until ip-enable
goes low, so polling it inside IP mode hung forever; mirrors C#
MotorsMovePathExecute / PVTBeginMotion(false). Trade-off: drive halts
~0.3 mm short of the trajectory end at typical speeds.
* _stream_samples aligns sample[0] to each drive's actual encoder
position. Rotary axes (shoulder, wrist) wrap — IK gives angles in
(-180, 180] but the encoder counts up across revolutions, so raw
IK values for a multi-revolution axis triggered immediate tracking
faults. The shift preserves all relative motion.
* _stream_samples skips axes with sub-threshold motion
(_SKIP_AXIS_COUNTS = 500). Drives idle on near-static (P, V=0)
frames and leave the buffer full for the next move's preload to
collide with. ensure_enabled clears sticky faults before streaming.
* ipm_select_mode always mode-bounces through PPM (mode 1 → buffer
clear → mode 7) on enable; drops the "first-armed fast path" that
intermittently left stale frames between runs. CW also force-reset
to 0x0F before the bounce (PPM leaves bit 4 high; in mode 7 that
means "interpolation enabled" → empty-buffer interpolation on entry).
* sampler V[0] = 0 on every axis (matches C# line 4242). Drives reject
the first preload frame in some configurations otherwise.
* park() is implemented as a Cartesian-linear (IPM) move to a centered
pose instead of NotImplementedError, so a single sequence stays in
IPM throughout and avoids the PPM → IPM transition path.
* IK enforces per-axis joint travel limits — out-of-range poses raise
IKError before any motion. Test config widened to realistic ranges.
Docs: new continuous-motion.ipynb walkthrough (linear, figure 8, helix,
Lissajous, waypoint L-shape, zig-zag scan), end-to-end verified on
hardware. Renames _LINEAR_PATH_* constants to _PVT_* and test file to
arm_backend_pvt_tests.py to reflect the broader use.
Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
No description provided.