add kx2 backend#880
Conversation
# 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>
…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).
…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>
…convention so 0°=+X (#1034) Co-authored-by: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
…hy refactor (#1044) Co-authored-by: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
Verified IRL: park() via IPM/Cartesian-linear lands within 0.1 of the new targets. ELBOW=1 keeps a comfortable margin above the planner's min_travel=0 so the linear path's IK doesn't underflow at the boundary. Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
|
Hi @rickwierenga! First off thanks for the incredible work, we are big fans :). Tested at HEAD 1.
-from pylabrobot.capabilities.arms.orientable_arm import OrientableArm
+from pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm
@@
- self.arm = OrientableArm(backend=backend, reference_resource=self.reference)
+ self.arm = OrientableGripperArm(backend=backend, reference_resource=self.reference)2.
@property
def min_gripper_width(self) -> Optional[float]:
if self._config is None:
return None
axis_cfg = self._config.axes.get(Axis.SERVO_GRIPPER)
return None if axis_cfg is None else axis_cfg.min_travel
@property
def max_gripper_width(self) -> Optional[float]:
if self._config is None:
return None
axis_cfg = self._config.axes.get(Axis.SERVO_GRIPPER)
return None if axis_cfg is None else axis_cfg.max_travel
async def move_gripper(
self,
width: float,
force_sensing: bool = False,
backend_params: Optional[BackendParams] = None,
) -> None:
if force_sensing:
await self.close_gripper(gripper_width=width, backend_params=backend_params)
else:
await self.open_gripper(gripper_width=width, backend_params=backend_params)The properties intentionally read A note while you're in this area: the capability-level wrappers on 3. Our KX-2 has the rail axis on the bus (node 5).
So - if has_rail or not has_servo_gripper:
- raise NotImplementedError(
- "KX2 has only been tested with the default 4-axis arm + servo "
- "gripper topology (has_rail=False, has_servo_gripper=True). "
- ...
- )
+ if not has_servo_gripper:
+ raise NotImplementedError(
+ "KX2 has only been tested with the default servo-gripper "
+ "topology (has_servo_gripper=True). The gripper-less path has "
+ "shim code but the setup / homing layer needs work — see "
+ "KX2ArmBackend._on_setup and servo_gripper_initialize."
+ )
+ if has_rail:
+ warnings.warn(
+ "has_rail=True: rail (node 5) is included in CAN bus discovery "
+ "and PDO remapping, but the rail motor is NOT enabled and "
+ "kinematics / motion code paths ignore it. Rail position is "
+ "not queried. Verify your task does not need to move the rail."
+ )( What's working on real hardware as of today
Multi-axis moves and rail-axis support are next on our side. Happy to |
No description provided.