From a34e085f0a64c5b890a4aabaf98da6197d5431b9 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Fri, 6 Feb 2026 19:14:54 -0500 Subject: [PATCH 01/93] kx2 --- pylabrobot/arms/__init__.py | 1 + pylabrobot/arms/backend.py | 23 +- pylabrobot/arms/kx2/kx2_backend.py | 4035 ++++++++++++++++++++++++ pylabrobot/arms/precise_flex/coords.py | 4 +- pylabrobot/arms/scara.py | 4 +- pylabrobot/arms/standard.py | 2 +- setup.py | 6 +- 7 files changed, 4062 insertions(+), 13 deletions(-) create mode 100644 pylabrobot/arms/kx2/kx2_backend.py diff --git a/pylabrobot/arms/__init__.py b/pylabrobot/arms/__init__.py index 60125af2e6e..875b1f2b0c0 100644 --- a/pylabrobot/arms/__init__.py +++ b/pylabrobot/arms/__init__.py @@ -1,3 +1,4 @@ +from .kx2 import * from .precise_flex import * from .scara import * from .standard import * diff --git a/pylabrobot/arms/backend.py b/pylabrobot/arms/backend.py index 6be521095ac..ee10def97e5 100644 --- a/pylabrobot/arms/backend.py +++ b/pylabrobot/arms/backend.py @@ -2,8 +2,7 @@ from dataclasses import dataclass from typing import Optional, Union -from pylabrobot.arms.precise_flex.coords import PreciseFlexCartesianCoords -from pylabrobot.arms.standard import JointCoords +from pylabrobot.arms.standard import GripperPose, JointCoords from pylabrobot.machines.backend import MachineBackend @@ -78,7 +77,7 @@ async def move_to_safe(self) -> None: @abstractmethod async def approach( self, - position: Union[PreciseFlexCartesianCoords, JointCoords], + position: Union[GripperPose, JointCoords], access: Optional[AccessPattern] = None, ) -> None: """Move the arm to an approach position (offset from target). @@ -91,7 +90,7 @@ async def approach( @abstractmethod async def pick_up_resource( self, - position: Union[PreciseFlexCartesianCoords, JointCoords], + position: Union[GripperPose, JointCoords], plate_width: float, access: Optional[AccessPattern] = None, ) -> None: @@ -105,7 +104,7 @@ async def pick_up_resource( @abstractmethod async def drop_resource( self, - position: Union[PreciseFlexCartesianCoords, JointCoords], + position: Union[GripperPose, JointCoords], access: Optional[AccessPattern] = None, ) -> None: """Place a plate at the specified position. @@ -116,13 +115,23 @@ async def drop_resource( """ @abstractmethod - async def move_to(self, position: Union[PreciseFlexCartesianCoords, JointCoords]) -> None: + async def move_to(self, position: Union[GripperPose, JointCoords]) -> None: """Move the arm to a specified position in 3D space or in joint space.""" + # TODO: move_to_cartesian and move_to_joint + @abstractmethod async def get_joint_position(self) -> JointCoords: """Get the current position of the arm in joint space.""" @abstractmethod - async def get_cartesian_position(self) -> PreciseFlexCartesianCoords: + async def get_cartesian_position(self) -> GripperPose: """Get the current position of the arm in 3D space.""" + + async def activate_free_mode(self) -> None: + """Activates free / "drag to teach mode.""" + raise NotImplementedError("Free mode not implemented for this backend.") + + async def deactivate_free_mode(self) -> None: + """Deactivates free mode.""" + raise NotImplementedError("Free mode not implemented for this backend.") diff --git a/pylabrobot/arms/kx2/kx2_backend.py b/pylabrobot/arms/kx2/kx2_backend.py new file mode 100644 index 00000000000..c904fff8aa2 --- /dev/null +++ b/pylabrobot/arms/kx2/kx2_backend.py @@ -0,0 +1,4035 @@ +import asyncio +import math +import struct +import time +import warnings +from dataclasses import dataclass, field +from enum import IntEnum +from typing import Any, Dict, List, Optional, Sequence, Tuple, Union + +import can + +from pylabrobot.arms.standard import GripperPose +from pylabrobot.resources import Coordinate, Rotation + + +class KX2Axis(IntEnum): + SHOULDER = 1 + Z = 2 + ELBOW = 3 + WRIST = 4 + RAIL = 5 + SERVO_GRIPPER = 6 + + +def _is_number(s: str) -> bool: + try: + float(str(s).strip()) + return True + except Exception: + return False + + +def _to_float(s: str, default: float = 0.0) -> float: + try: + return float(str(s).strip()) + except Exception: + warnings.warn(f"Error converting '{s}' to float, returning default") + return default + + +def _u32_le(value: int) -> List[int]: + return list((value & 0xFFFFFFFF).to_bytes(4, byteorder="little", signed=False)) + + +class JointMoveDirection(IntEnum): + Normal = 0 + Clockwise = 1 + Counterclockwise = 2 + ShortestWay = 3 + + +class HomeStatus(IntEnum): + NotHomed = 0 + Homed = 1 + InitializedWithoutHoming = 2 + + +class InputLogic(IntEnum): + GeneralPurpose = 0 + EnableForwardOnly = 1 + EnableReverseOnly = 2 + + +class EventType(IntEnum): + MotorPositionChanged = 1 + MotionStatusReceived = 2 + MotorEnabledStatusReceived = 3 + DigitalInputChangedState = 4 + CANEmergencyMessageReceived = 5 + MoveError = 6 + MotorMoveDone = 7 + MotorsMoveDone = 8 + MotorsMovePathDone = 9 + + +class MoveType(IntEnum): + MotorMove = 0 + MotorsMoveAbsolute = 1 + MotorsMovePath = 2 + + +class CmdType(IntEnum): + ValQuery = 1 + ValSet = 2 + Execute = 3 + + +class ValType(IntEnum): + Int = 1 + Float = 2 + + +class COBType(IntEnum): + NMT = 0 + EMCY = 1 + SYNC = 1 + TIMESTAMP = 2 + TPDO1 = 3 + RPDO1 = 4 + TPDO2 = 5 + RPDO2 = 6 + TPDO3 = 7 + RPDO3 = 8 + TPDO4 = 9 + RPDO4 = 10 + TSDO = 11 + RSDO = 12 + ERRCTRL = 14 + HEARTBEAT = 14 + + +class RPDO(IntEnum): + RPDO1 = 1 + RPDO3 = 3 + RPDO4 = 4 + + +class PDOTransmissionType(IntEnum): + SynchronousAcyclic = 0 + SynchronousCyclic = 1 + EventDrivenManf = 254 # 0xFE + EventDrivenDev = 255 # 0xFF + + +class RPDOMappedObject(IntEnum): + NotMapped = 0 + ControlWord = 0x60400010 + TargetTorque = 0x60710010 + MaxTorque = 0x60720010 + TargetPosition = 0x607A0020 + VelocityOffset = 0x60B10020 + TargetPositionIP = 0x60C10120 + TargetVelocityIP = 0x60C10220 + DigitalOutputs = 0x60FE0020 + TargetVelocity = 0x60FF0020 + + +class TPDO(IntEnum): + TPDO1 = 1 + TPDO3 = 3 + TPDO4 = 4 + + +class TPDOTrigger(IntEnum): + MotionComplete = 0 + MainHomingComplete = 1 + AuxiliaryHomingComplete = 2 + MotorShutDownByException = 3 + MotorStarted = 4 + UserProgramEmitCommand = 5 + OSInterpreterExecutionComplete = 6 + MotionStartedEvent = 8 + PDODataChanged = 24 + DigitalInputEvent = 26 + StatusWordEvent = 27 + BinaryInterpreterCommandComplete = 31 + + +class TPDOMappedObject(IntEnum): + NotMapped = 0 + Timestamp = 0x20410020 + PVTHeadPointer = 0x2F110010 + PVTTailPointer = 0x2F120010 + StatusWord = 0x60410010 + PositionActualValue = 0x60640020 + VelocityDemandValue = 0x606B0020 + VelocityActualValue = 0x606C0020 + TorqueDemandValue = 0x60740010 + TorqueActualValue = 0x60770010 + IPBufferPosition = 0x60C40410 + DigitalInputs = 0x60FD0020 + + +class ElmoObject(IntEnum): + DeviceType = 0x1000 + ErrorRegister = 0x1001 + ManfStatusRegister = 0x1002 + ErrorField = 0x1003 + CommCyclePeriod = 0x1006 + ManfDeviceName = 0x1008 + ManfHWVersion = 0x1009 + ManfSWVersion = 0x100A + NodeID = 0x100B + StoreParameters = 0x1010 + RestoreParameters = 0x1011 + CustomerHeartbeatTime = 0x1016 + ProducerHeartbeatTime = 0x1017 + IdentityObject = 0x1018 + OSCommand = 0x1023 + OSCommandMode = 0x1024 + ErrorBehavior = 0x1029 + RPDO1CommParam = 0x1400 + RPDO2CommParam = 0x1401 + RPDO3CommParam = 0x1402 + RPDO4CommParam = 0x1403 + RPDO1Mapping = 0x1600 + RPDO2Mapping = 0x1601 + RPDO3Mapping = 0x1602 + RPDO4Mapping = 0x1603 + TPDO1CommParam = 0x1800 + TPDO2CommParam = 0x1801 + TPDO3CommParam = 0x1802 + TPDO4CommParam = 0x1803 + TPDO1Mapping = 0x1A00 + TPDO2Mapping = 0x1A01 + TPDO3Mapping = 0x1A02 + TPDO4Mapping = 0x1A03 + FastReference = 0x2005 + BinaryInterpreterInput = 0x2012 + BinaryInterpreterOutput = 0x2013 + FilteredRMSCurrent = 0x201B + HomeOnBlockLimitParams = 0x2020 + RecorderData = 0x2030 + TimeStamp = 0x2040 + DriveParamsChecksum = 0x2060 + AdditionalPositionRangeLimit = 0x207B + ExtendedErrorCode = 0x2081 + CANControllerStatus = 0x2082 + SerialEncoderStatus = 0x2084 + ExtraStatusRegister = 0x2085 + STOStatusRegister = 0x2086 + PALVersion = 0x2087 + AuxPositionActualValue = 0x20A0 + SocketAdditionalFunction = 0x20B0 + AbsoluteSensorFunctions = 0x20FC + DigitalInputs = 0x20FD + DigitalInputLowByte = 0x2201 + ExtendedInputs = 0x2202 + Application = 0x2203 + AnalogInputs = 0x2205 + Supply5VDC = 0x2206 + DigitalOutputs = 0x22A0 + ExtendedOutputs = 0x22A1 + DriveTemperature = 0x22A2 + DriveTemperature2 = 0x22A3 + MotorTemperature = 0x22A4 + GainSchedulingIndex = 0x2E00 + TorqueWindow = 0x2E06 + TorqueWindowTime = 0x2E07 + HomeOnTouchProbe = 0x2E10 + GantryYawOffset = 0x2E15 + UserInteger = 0x2F00 + UserFloat = 0x2F01 + GetCtrlBoardType = 0x2F05 + TPDOAsyncEvents = 0x2F20 + EmergencyEvents = 0x2F21 + DS402Config = 0x2F41 + ThresholdParam = 0x2F45 + CANEncoderRange = 0x2F70 + ExtrapolationCyclesTimeout = 0x2F75 + ElmoParamBG = 0x3020 + ElmoParamCA = 0x3034 + ElmoParamCL = 0x303F + ElmoParamDC = 0x3050 + ElmoParamEC = 0x306A + ElmoParamER = 0x3079 + ElmoParamHL = 0x30C1 + ElmoParamHP = 0x30C5 + ElmoParamHT = 0x30C9 + ElmoParamIB = 0x30D1 + ElmoParamID = 0x30D3 + ElmoParamIF = 0x30D5 + ElmoParamIL = 0x30DB + ElmoParamIP = 0x30DF + ElmoParamIQ = 0x30E0 + ElmoParamJV = 0x30FF + ElmoParamKI = 0x310C + ElmoParamKL = 0x310F + ElmoParamKP = 0x3113 + ElmoParamKV = 0x3119 + ElmoParamLL = 0x3129 + ElmoParamMC = 0x313A + ElmoParamMO = 0x3146 + ElmoParamMS = 0x314A + ElmoParamOB = 0x316D + ElmoParamOL = 0x3177 + ElmoParamOP = 0x317B + ElmoParamPA = 0x3186 + ElmoParamPE = 0x318A + ElmoParamPL = 0x3191 + ElmoParamPP = 0x3195 + ElmoParamPR = 0x3197 + ElmoParamPS = 0x3198 + ElmoParamPX = 0x319D + ElmoParamSD = 0x31D7 + ElmoParamSF = 0x31D9 + ElmoParamSR = 0x31E5 + ElmoParamSV = 0x31E9 + ElmoParamSW = 0x31EA + ElmoParamTC = 0x31F0 + ElmoParamTM = 0x31FA + ElmoParamUC = 0x320D + ElmoParamUI = 0x3210 + ElmoParamVE = 0x3226 + ElmoParamVH = 0x3229 + ElmoParamVL = 0x322D + + +class ElmoObjectDataType(IntEnum): + UNSIGNED8 = 0 + UNSIGNED16 = 1 + UNSIGNED32 = 2 + UNSIGNED64 = 3 + INTEGER8 = 4 + INTEGER16 = 5 + INTEGER32 = 6 + INTEGER64 = 7 + STR = 8 + + +@dataclass +class EventData: + event_type: EventType = EventType.MotorPositionChanged + pending: bool = False + node_id: int = 0 + cmr_msg_type: str = "" + cmr_data_type: str = "" + cmr_data: str = "" + mpc_position: Optional[List[int]] = None # Array + msr_status: bool = False + msr_status_word: int = 0 + mesr_status: int = 0 + dics_state: Optional[List[bool]] = None # Array + cemr_emcy_msg: Any = None # sEmcy + cemr_description: str = "" + cemr_disable_motors: bool = False + me_error_code: int = 0 + me_index: int = 0 + mmd_all_moves_done: bool = False + + +@dataclass +class ErrCtrl: + data_byte: Optional[List[int]] = None # Array + + +@dataclass +class PVT_EMCY_QueueLow: + state: bool = False + write_pointer: int = 0 + read_pointer: int = 0 + + +@dataclass +class PVT_EMCY_QueueFull: + state: bool = False + failed_write_pointer: int = 0 + + +@dataclass +class PVT_EMCY: + queue_low: PVT_EMCY_QueueLow = field(default_factory=PVT_EMCY_QueueLow) + queue_full: PVT_EMCY_QueueFull = field(default_factory=PVT_EMCY_QueueFull) + bad_head_pointer: bool = False + bad_mode_init_data: bool = False + motion_terminated: bool = False + out_of_modulo: bool = False + + +@dataclass +class Emcy: + err_code: int = 0 + err_reg: int = 0 + elmo_err_code: int = 0 + err_code_data1: int = 0 + err_code_data2: int = 0 + + +@dataclass +class Query: + node_id: int = 0 + object_byte0: int = 0 + object_byte1: int = 0 + sub_index: int = 0 + msg_type: str = "" # "SDODI" "CHR" "STAT" "SDOUA" "SDOSU" + msg_index: int = 0 + + +@dataclass +class CAN_Msg: + cob: COBType = COBType.NMT + node_id: int = 0 + byte0: int = 0 + byte1: int = 0 + byte2: int = 0 + byte3: int = 0 + byte4: int = 0 + byte5: int = 0 + byte6: int = 0 + byte7: int = 0 + execute: bool = False + data_length: int = 8 + error_msg: str = "" + pending: bool = False + time_stamp: int = 0 + fut: Optional[asyncio.Future] = None + + +@dataclass +class NodeInputConfig: + logic: InputLogic = InputLogic.GeneralPurpose + logic_high: bool = True + + +class CanError(Exception): + """Custom exception for CAN motor errors.""" + + +@dataclass +class MotorMoveParam: + axis: "KX2Axis" + position: int + velocity: int + acceleration: int + relative: bool = False + direction: JointMoveDirection = JointMoveDirection.ShortestWay + + +@dataclass +class MotorsMovePlan: + moves: List[MotorMoveParam] + move_time: float = 10.0 + + +class KX2Can: + def __init__(self, has_rail: bool = False, has_servo_gripper: bool = True) -> None: + self.connecting: bool = False + self.grp_id: int = 0 + + # Error control + self.err_ctrl: List[ErrCtrl] = [ErrCtrl() for _ in range(8)] + + self.move_error_code: int = 0 + + self.pvt_time_interval_msec: int = 0 + self.pvt_stop: bool = False + + self.pvt_emcy: List[PVT_EMCY] = [PVT_EMCY() for _ in range(128)] + self._pvt_mode: bool = False + + self._can_device: Optional[can.BusABC] = None + + # Threading flags (for asyncio tasks) + self._can_write_task_running = False + self._can_read_task_running = True + self.b_pvt_thread_started = False + + # Can message queues + self._can_msg_queue_hp: asyncio.Queue[CAN_Msg] = asyncio.Queue() + self._can_msg_queue_lp: asyncio.Queue[CAN_Msg] = asyncio.Queue() + + self._waiting_moves: Dict[KX2Axis, asyncio.Future] = {} + + self.node_id_list = [1, 2, 3, 4] + if has_rail: + self.node_id_list.append(5) + if has_servo_gripper: + self.node_id_list.append(6) + + self.input_state: Dict[int, int] = {i: 0 for i in range(len(self.node_id_list))} + + # initialize based on maximum possible node_id (assume 127 + 1 + 1, so 129), 5, 1 + self.tpdo_mapped_object: List[List[List[TPDOMappedObject]]] = [ + [[TPDOMappedObject.NotMapped for _ in range(1)] for _ in range(5)] for _ in range(129) + ] + self.node_input_config: List[List[NodeInputConfig]] = [ + [NodeInputConfig() for _ in range(7)] for _ in range(129) + ] + + # Wait buffers + self._os_query_wait_buffer: List[Tuple[Query, asyncio.Future]] = [] + self._os_query_wait_buffer_lock = asyncio.Lock() + + self._query_wait_buffer: List[Tuple[Query, asyncio.Future]] = [] + self._query_wait_buffer_lock = asyncio.Lock() + + @property + def can_device(self) -> can.BusABC: + if self._can_device is None: + raise CanError("CAN device is not initialized.") + return self._can_device + + async def _can_write_task(self): + self._can_write_task_running = True + + while self._can_write_task_running: + message: Optional[CAN_Msg] = None + try: + message = self._can_msg_queue_hp.get_nowait() + except asyncio.QueueEmpty: + try: + message = self._can_msg_queue_lp.get_nowait() + except asyncio.QueueEmpty: # No messages to send + await asyncio.sleep(0.001) + continue + + if message.fut is not None and message.fut.done(): + continue # Skip if future is already done + + msg_id = (message.cob.value << 7) + message.node_id + data_bytes = [ + message.byte0, + message.byte1, + message.byte2, + message.byte3, + message.byte4, + message.byte5, + message.byte6, + message.byte7, + ][: message.data_length] + + try: + can_msg = can.Message( + arbitration_id=msg_id, + data=data_bytes, + is_extended_id=False, # Assuming standard IDs for CANopen + ) + + await asyncio.to_thread(self.can_device.send, can_msg) + if message.fut and not message.fut.done(): + message.fut.set_result(None) + except Exception as e: + error_msg = f"CAN Write Error: {e}" + print(f"CAN Write Error: {e}") + if message.fut and not message.fut.done(): + message.fut.set_exception(CanError(error_msg)) + + async def _process_emcy_message(self, node_id: int, message: can.Message) -> None: + print("EMCY received!!") + + data = message.data + if not data or len(data) < 8: + print(f"EMCY malformed: expected 8 bytes, got {0 if not data else len(data)}") + return + + def u16_le(i: int) -> int: + return int(data[i]) | (int(data[i + 1]) << 8) + + emcy = Emcy() + emcy.err_code = u16_le(0) + emcy.err_reg = int(data[2]) + emcy.elmo_err_code = int(data[3]) + emcy.err_code_data1 = u16_le(4) + emcy.err_code_data2 = u16_le(6) + + self.last_emcy = emcy + + desc = "" + disable_motors = False + suppress_event = False + + err = emcy.err_code + elmo = emcy.elmo_err_code + + # Simple (err_code -> (description, disable_motors)) + ERR_MAP: dict[int, tuple[str, bool]] = { + 0x8110: ("CAN message lost (corrupted or overrun)", False), + 0x8200: ("Protocol error (unrecognized NMT request)", False), + 0x8210: ("Attempt to access an unconfigured RPDO", False), + 0x8130: ("Heartbeat event", False), + 0x6180: ("Fatal CPU error: stack overflow", False), + 0x6181: ("CPU exception: fatal exception", False), + 0x6200: ("User program aborted by an error", False), + 0xFF01: ("Request by user program 'emit' function", False), + 0x6300: ( + "Object mapped to an RPDO returned an error during interpretation or a referenced motion failed to be performed", + False, + ), + 0x7300: ("Resolver or Analog Encoder feedback failed", True), + 0x7380: ("Feedback loss: no match between encoder and Hall locations.", True), + 0x8311: ( + "Peak current has been exceeded due to drive malfunction or badly-tuned current controller", + True, + ), + 0x5441: ("E-stop button was pressed", True), + 0x5280: ("ECAM table problem", False), + 0x7381: ( + "Two digital Hall sensors changed at once; only one sensor can be changed at a time", + True, + ), + 0x8480: ("Speed tracking error", True), + 0x8611: ("Position tracking error", True), + 0x6320: ("Cannot start due to inconsistent database", False), + 0x8380: ("Cannot find electrical zero of motor when attempting to start motor", False), + 0x8481: ("Speed limit exceeded", True), + 0x5281: ("Timing error", False), + 0x7121: ("Motor stuck: motor powered but not moving", True), + 0x8680: ("Position limit exceeded", True), + 0x8381: ("Cannot tune current offsets", False), + 0xFF10: ("Cannot start motor", False), + 0x5400: ("Cannot start motor", False), + 0x3120: ( + "Under-voltage: power supply is shut down or it has too high an output impedance", + True, + ), + 0x3310: ( + "Over-voltage: power supply voltage is too high or servo driver could not absorb kinetic energy while braking a load", + True, + ), + 0x2340: ("Short circuit: motor or its wiring may be defective, or drive is faulty", True), + 0x4310: ("Temperature: drive overheating", True), + 0xFF20: ("Safety switch is sensed - Drive in safety state", True), + } + + if err == 0xFF00: + if elmo == 0x56: + st = self.pvt_emcy[node_id] + st.queue_low.state = True + st.queue_low.write_pointer = emcy.err_code_data1 + st.queue_low.read_pointer = emcy.err_code_data2 + desc = "Queue Low" + elif elmo == 0x5B: + self.pvt_emcy[node_id].bad_head_pointer = True + desc = "Bad Head Pointer" + elif elmo == 0x34: + st = self.pvt_emcy[node_id] + st.queue_full.state = True + st.queue_full.failed_write_pointer = emcy.err_code_data1 + desc = "Queue Full" + elif elmo == 0x07: + self.pvt_emcy[node_id].bad_mode_init_data = True + desc = "Bad Mode Init Data" + elif elmo == 0x08: + self.pvt_emcy[node_id].motion_terminated = True + desc = "Motion Terminated" + elif elmo == 0xA6: + self.pvt_emcy[node_id].out_of_modulo = True + desc = "Out Of Modulo" + else: + desc = f"DS402 IP Error {elmo}" + + elif err == 0xFF02: + if elmo == 0x07: + self.pvt_emcy[node_id].bad_mode_init_data = True + desc = "Bad Mode Init Data" + elif elmo == 0x08: + self.pvt_emcy[node_id].motion_terminated = True + desc = "Motion Terminated" + elif elmo == 0x34: + st = self.pvt_emcy[node_id] + st.queue_full.state = True + st.queue_full.failed_write_pointer = emcy.err_code_data1 + desc = "Queue Full" + elif elmo == 0x56: + st = self.pvt_emcy[node_id] + st.queue_low.state = True + st.queue_low.write_pointer = emcy.err_code_data1 + st.queue_low.read_pointer = emcy.err_code_data2 + desc = "Queue Low" + elif elmo == 0x5B: + self.pvt_emcy[node_id].bad_head_pointer = True + desc = "Bad Head Pointer" + elif elmo == 0x8A: + self.pvt_emcy[node_id].queue_low.state = True + desc = "Position Interpolation buffer underflow" + suppress_event = True + elif elmo == 0xA6: + self.pvt_emcy[node_id].out_of_modulo = True + desc = "Out Of Modulo" + elif elmo == 0xBA: + self.pvt_emcy[node_id].queue_full.state = True + desc = "Interpolation queue is full" + elif elmo == 0xBB: + desc = "Incorrect interpolation sub-mode" + else: + desc = f"DS402 IP Error {elmo}" + + else: + mapped = ERR_MAP.get(err) + if mapped: + desc, disable_motors = mapped + else: + desc = f"Unknown EMCY err={err} elmo={elmo}" + + if disable_motors: + for fut in self._waiting_moves.values(): + if not fut.done(): + fut.set_exception(CanError(f"Motor move error on node {node_id}: {desc}")) + + if not suppress_event: + event = EventData() + event.event_type = EventType.CANEmergencyMessageReceived + event.node_id = node_id + event.cemr_description = desc + event.cemr_emcy_msg = emcy + event.cemr_disable_motors = disable_motors + print(event) + + print(emcy) + print( + f"EMCY node={node_id} desc='{desc}' disable_motors={disable_motors} suppress_event={suppress_event}" + ) + + async def _process_tpdo_message(self, node_id: int, message: can.Message, response_type: int): + tpdo_index = {0: TPDO.TPDO1, 4: TPDO.TPDO3, 6: TPDO.TPDO4}[response_type - 3] + + if self.node_id_list is None or node_id not in self.node_id_list: + return + + if self.tpdo_mapped_object is None: + return + + num2 = 0 + + node_idx = self.node_id_list.index(node_id) + + for i in range(len(self.tpdo_mapped_object[node_id][tpdo_index])): + mapped = self.tpdo_mapped_object[node_id][tpdo_index][i] + + # read 16-bit or 32-bit value depending on flags + num4_: Optional[int] = None + if mapped.value & 0x10 == 0x10: # 16 bit + num4_ = (message.data[num2 + 1] << 8) | message.data[num2] + num2 += 2 + if mapped.value & 0x20 == 0x20: # 32 bit + num4_ = ( + (message.data[num2 + 3] << 24) + | (message.data[num2 + 2] << 16) + | (message.data[num2 + 1] << 8) + | message.data[num2] + ) + num2 += 4 + if num4_ is None: + # print("Failed to read TPDO mapped object value, probably fine....") + # raise CanError("Failed to read TPDO mapped object value.") + continue + num4 = num4_ + + if mapped == TPDOMappedObject.StatusWord: + if KX2Axis(node_id) in self._waiting_moves: + self._waiting_moves[KX2Axis(node_id)].set_result(None) + + event_data = EventData( + event_type=EventType.MotionStatusReceived, + node_id=node_id, + msr_status=True, + msr_status_word=int(num4 & 0xFFFF), # short + ) + await self._raise_an_event(event_data) + elif mapped == TPDOMappedObject.PositionActualValue: + pass # we don't need to mirror the encoder position on the client side + elif mapped == TPDOMappedObject.DigitalInputs: + # temp array for digital inputs (indices 16..21 used) + num_array = [0] * 22 # 0..21 + + # clear bits 16..21?? + for i in range(16, 21 + 1): + num_array[i] = 0 + + # set bits from num4 + if num4 & (1 << 16): + num_array[16] = 1 + if num4 & (1 << 17): + num_array[17] = 1 + if num4 & (1 << 18): + num_array[18] = 1 + if num4 & (1 << 19): + num_array[19] = 1 + if num4 & (1 << 20): + num_array[20] = 1 + if num4 & (1 << 21): + num_array[21] = 1 + + # num6: packed 6-bit value + num6 = ( + num_array[16] + + num_array[17] * 2 + + num_array[18] * 4 + + num_array[19] * 8 + + num_array[20] * 16 + + num_array[21] * 32 + ) + + if self.input_state[node_idx] != num6: + # check for transitions that imply move done + for index4 in range(1, 7): # 1..6 + # previous bit for this input (0/1) + prev_bit = 1 if (self.input_state[node_idx] & (1 << (index4 - 1))) else 0 + new_bit = num_array[15 + index4] + + cfg = self.node_input_config[node_idx][index4] + logic = cfg.logic + + edge = prev_bit != new_bit + logic_enabled = logic in ( + InputLogic.EnableForwardOnly, + InputLogic.EnableReverseOnly, + ) + + if edge and logic_enabled and new_bit == 1: + self._waiting_moves[KX2Axis(node_id)].set_result(None) + print(f"Digital input {index4} enabled motor move done for node {node_id}") + + self.input_state[node_idx] = num6 + + # index 0 unused, 1..6 valid + dics_state = [False] * 7 + dics_state[1] = num_array[16] > 0 + dics_state[2] = num_array[17] > 0 + dics_state[3] = num_array[18] > 0 + dics_state[4] = num_array[19] > 0 + dics_state[5] = num_array[20] > 0 + dics_state[6] = num_array[21] > 0 + + # raise digital input change event + event_data = EventData( + event_type=EventType.DigitalInputChangedState, + node_id=node_id, + dics_state=dics_state, + ) + + await self._raise_an_event(event_data) + + async def _process_binary_interpreter_response(self, node_id: int, message: can.Message) -> None: + data = message.data + if not data or len(data) < 8: + raise CanError("Invalid binary interpreter response data.") + + msg_type = chr(data[0]) + chr(data[1]) + + # Build index: lower 8 bits from DATA[2], upper 6 bits from DATA[3] + msg_index = ((data[3] & 0b0011_1111) << 8) | data[2] + + is_int = (data[3] & 0x80) == 0 + + if is_int: + raw = bytes(data[4:8]) + value_int = struct.unpack(" None: + data = message.data + if not data: + return + + cmd = data[0] + length = len(data) + + async with self._os_query_wait_buffer_lock: + # ---- 1) SDO abort (0x80) ---- + if cmd == 0x80 and length >= 8: + abort_code = data[4] | (data[5] << 8) | (data[6] << 16) | (data[7] << 24) + + msg = self.get_sdo_abort_message(abort_code) + + for query, fut in self._get_os_queries(node_id): + if ( + query.object_byte0 == data[2] + and query.object_byte1 == data[1] + and query.sub_index == data[3] + ): + fut.set_exception(CanError(msg)) + + # ---- 2) SDO download initiate response (0x60) -> "SDODI" ---- + elif cmd == 0x60 and length >= 4: + for query, fut in self._get_os_queries(node_id): + if ( + query.object_byte0 == data[2] + and query.object_byte1 == data[1] + and query.sub_index == data[3] + and query.msg_type == "SDODI" + ): + fut.set_result("SDODI") + + # ---- 3) "CHR" response: 0x20/0x30 and index bytes zero ---- + elif cmd in (0x20, 0x30) and length >= 4 and data[1] == 0 and data[2] == 0 and data[3] == 0: + for query, fut in self._get_os_queries(node_id): + if query.msg_type == "CHR": + fut.set_result("CHR") + + # ---- 4) "STAT" response ---- + elif length >= 5 and cmd == 0x4F and data[1] == 0x23 and data[2] == 0x10 and data[3] == 2: + stat_value = data[4] + for query, fut in self._get_os_queries(node_id): + if query.msg_type == "STAT": + fut.set_result(f"STAT{stat_value}") + + # ---- 5) "VAL" response ---- + elif length >= 4 and cmd == 0x43 and data[1] == 0x23 and data[2] == 0x10 and data[3] == 3: + if length > 4: + tail = ",".join(str(b) for b in data[4:length]) + val_response = f"VAL,{tail}" + else: + val_response = "VAL" + + for query, fut in self._get_os_queries(node_id): + if query.msg_type == "VAL": + fut.set_result(val_response) + + # ---- 6) SDO upload (expedited) "SDOUA" ---- + elif (cmd & 0x40) == 0x40 and length >= 4: + # Decode bits according to CANopen-style layout: + # n = number of unused bytes in 4-byte data field + # e = (usually) expedited bit + n = (cmd >> 2) & 0x03 + e = (cmd >> 1) & 0x01 + # s = cmd & 0x01 # not needed here + + used = max(0, min(4 - n, length - 4)) + raw_bytes = data[4 : 4 + used] + + for query, fut in self._get_os_queries(node_id): + if ( + query.msg_type == "SDOUA" + and query.object_byte0 == data[2] + and query.object_byte1 == data[1] + and query.sub_index == data[3] + ): + if e == 0: + # Numeric value: sum(data[i] * 256^(i-1)) + value = 0 + for i, b in enumerate(raw_bytes): + value |= int(b) << (8 * i) + fut.set_result(f"SDOUAN{value}") + else: + # String value: concatenate chars from data bytes + text = "".join(chr(b) for b in raw_bytes) + fut.set_result(f"SDOUAE{text}") + + # ---- 7) SDO segmented upload "SDOSU" ---- + elif (cmd & 0x20) == 0 and (cmd & 0x40) == 0 and (cmd & 0x80) == 0 and length >= 1: + # Again, follow CANopen-ish semantics: + # toggle bit, n = number of unused bytes (among 7), last-segment bit. + toggle = (cmd >> 4) & 0x01 + n = (cmd >> 1) & 0x07 + last = cmd & 0x01 + + used = max(0, min(7 - n, length - 1)) + payload = data[1 : 1 + used] + payload_str = "".join(chr(b) for b in payload) + + for query, fut in self._get_os_queries(node_id): + if query.msg_type == "SDOSU": + prefix = "SDOSUC" if last == 0 else "SDOSUD" + fut.set_result(f"{prefix}{toggle}{payload_str}") + + # ---- Cleanup: drop finished futures from buffer ---- + self._os_query_wait_buffer = [ + (query, fut) + for query, fut in self._os_query_wait_buffer + if query.node_id != node_id or not fut.done() + ] + + async def _process_motor_drive_restarted(self, node_id: int, message: can.Message): + self.err_ctrl[node_id].data_byte = list(message.data) + if not self.connecting: + event = EventData( + event_type=EventType.CANEmergencyMessageReceived, + node_id=node_id, + cemr_description="Node Guarding error. Motor drive restarted spontaneously.", + cemr_disable_motors=True, + ) + await self._raise_an_event(event) + + await self.can_write(cob=COBType.NMT, node_id=0, byte0=0x01, byte1=node_id) + await self.can_tpdo_unmap(tpdo=TPDO.TPDO1, node_id=node_id) + await self.can_tpdo3_map(node_id=node_id) + await self.can_tpdo4_map(node_id=node_id) + + async def _can_read_task( + self, + ): + while self._can_read_task_running: + try: + message = await asyncio.to_thread(self.can_device.recv, timeout=1.0) + except Exception as e: + print(f"CAN Read Error: {e}") + raise CanError(f"CAN Read Error: {e}") + + if message is None: # timeout + continue + + response_type = message.arbitration_id >> 7 + response_type_c = round(message.arbitration_id / 128) + assert ( + response_type == response_type_c + ), f"Response type calculation mismatch: {response_type} != {response_type_c}" + node_id = message.arbitration_id & 0x7F + node_id_c = message.arbitration_id - (response_type * 128) + assert node_id == node_id_c, f"Node index calculation mismatch: {node_id} != {node_id_c}" + + if response_type == 0: + print("NMT message received, ignoring") + elif response_type == 1: + await self._process_emcy_message(node_id=node_id, message=message) + elif response_type in {3, 7, 9}: + await self._process_tpdo_message( + node_id=node_id, message=message, response_type=response_type + ) + elif response_type == 5: + await self._process_binary_interpreter_response(node_id=node_id, message=message) + elif response_type == 11: + await self._process_sdo_response(node_id=node_id, message=message) + elif response_type == 14: + await self._process_motor_drive_restarted(node_id=node_id, message=message) + else: + print(f"Unknown CAN message type received: {response_type}") + + async def can_write( + self, + cob: COBType, + node_id: int, + byte0: int = 0, + byte1: int = 0, + byte2: int = 0, + byte3: int = 0, + byte4: int = 0, + byte5: int = 0, + byte6: int = 0, + byte7: int = 0, + execute: bool = False, + data_length: int = 8, + low_priority: bool = False, + time_stamp: int = 0, + ) -> None: + """Queues a CAN message for transmission.""" + + fut = asyncio.get_event_loop().create_future() + + msg_entry = CAN_Msg( + cob=cob, + node_id=node_id, + byte0=byte0, + byte1=byte1, + byte2=byte2, + byte3=byte3, + byte4=byte4, + byte5=byte5, + byte6=byte6, + byte7=byte7, + execute=execute, + data_length=data_length, + error_msg="", + pending=True, # Still use pending for internal state if needed elsewhere, but fut is primary + time_stamp=time_stamp, + fut=fut, + ) + + if low_priority: + await self._can_msg_queue_lp.put(msg_entry) + else: + await self._can_msg_queue_hp.put(msg_entry) + + timeout_sec = 5.0 # 5000ms + try: + await asyncio.wait_for(fut, timeout=timeout_sec) + except asyncio.TimeoutError: + raise CanError( + f"Failed to send CAN message {cob.name},{node_id},... Low Priority = {low_priority} (timeout)" + ) + except Exception as e: + raise CanError(str(e)) + + async def connect( + self, + baud_rate: int = 500000, + ) -> None: + self.sending_sv_command = False + self.move_error_code = 0 + self.pvt_time_interval_msec = 0 + + # Determine max_node_id for array sizing if dynamic behavior is needed + max_node_id = 6 + + # Resize buffers based on max_node_id + buffer_size = max_node_id + 1 if max_node_id > 0 else 8 + + self.node_input_config = [[NodeInputConfig() for _ in range(7)] for _ in range(buffer_size + 1)] + for i in range(buffer_size + 1): + for j in range(1, 7): + self.node_input_config[i][j].logic = InputLogic.GeneralPurpose + self.node_input_config[i][j].logic_high = True + + # Initialize python-can bus + self._can_device = can.Bus( + interface="pcan", # Or 'usbcan', 'kvaser', etc. based on setup + channel=None, # e.g., 'PCAN_USBBUS1' or int 0 for default + bitrate=baud_rate, + is_extended_id=False, + ) + + # Start asyncio tasks + asyncio.create_task(self._can_read_task()) + asyncio.create_task(self._can_write_task()) + + await asyncio.sleep(0.01) + + # --- CANopen Initialization Sequence --- + self.connecting = True + # NMT Reset Nodes (0x80) + await self.can_write(cob=COBType.NMT, node_id=0, byte0=0x82) + + await asyncio.sleep(0.5) + + # NMT Start Nodes (0x01) + await self.can_write(cob=COBType.NMT, node_id=0, byte0=0x01) + await asyncio.sleep(0.1) + self.connecting = False + + discovered_nodes = [ + i for i in range(len(self.err_ctrl)) if self.err_ctrl[i].data_byte is not None + ] + if discovered_nodes != self.node_id_list: + raise CanError( + f"Node IDs on CAN bus do not match expected list: {discovered_nodes} != {self.node_id_list}" + ) + + async def connect_part_two(self): + """After setting up the threads and can connection, and receiving node IDs, map the things. this is flag=True""" + + max_node_id = max(self.node_id_list) + + self.tpdo_mapped_object = [ + [[TPDOMappedObject.NotMapped for _ in range(1)] for _ in range(5)] + for _ in range(max_node_id + 2) + ] + + for node_id in self.node_id_list: + await self.can_tpdo_unmap(TPDO.TPDO1, node_id) + + await self.can_tpdo3_map(node_id) + await self.can_tpdo4_map(node_id) + + # Configure Elmo objects if group_node_id is set + # This section involves can_sdo_download_ElmoObject which needs to be translated first. + for axis in KX2Backend.MOTION_AXES: + await self.can_sdo_download_elmo_object( + node_id=int(axis), + elmo_object_int=24768, + sub_index=0, + data="-1", + data_type=ElmoObjectDataType.INTEGER16, + ) + + await self.can_sdo_download_elmo_object( + node_id=int(axis), + elmo_object_int=24772, + sub_index=2, + data="16", + data_type=ElmoObjectDataType.UNSIGNED32, + ) + + await self.can_sdo_download_elmo_object( + node_id=int(axis), + elmo_object_int=24772, + sub_index=3, + data="0", + data_type=ElmoObjectDataType.UNSIGNED8, + ) + + await self.can_sdo_download_elmo_object( + node_id=int(axis), + elmo_object_int=24772, + sub_index=5, + data="8", + data_type=ElmoObjectDataType.UNSIGNED8, + ) + + await self.can_sdo_download_elmo_object( + node_id=int(axis), + elmo_object_int=24770, + sub_index=2, + data="-3", + data_type=ElmoObjectDataType.INTEGER8, + ) + + await self.can_sdo_download_elmo_object( + node_id=int(axis), + elmo_object_int=24669, + sub_index=0, + data="1", + data_type=ElmoObjectDataType.INTEGER16, + ) + + for axis in KX2Backend.MOTION_AXES: + await self.can_rpdo1_map(int(axis)) + await self.can_rpdo3_map(int(axis)) + + # PVT Mode setup + self._pvt_mode = True + await self.pvt_select_mode(False) + + async def disconnect(self) -> None: + self._can_read_task_running = False + self._can_write_task_running = False + + if self._can_device is not None: + self._can_device.shutdown() + self._can_device = None + + async def _raise_an_event(self, event_data: EventData): + print(f"Raising event: {event_data}") + + # TODO: on move error / emergency, we should set the indicator light and disable motors + + # --- helper methods and core functionalities --- + + def get_sdo_abort_message(self, code: int) -> str: + sdo_default_abort_message = f"Unknown error {code:#010x}." + return { + 0x05030000: "Toggle bit not alternated.", + 0x05040001: "Invalid or unknown client/server command specifier.", + 0x05040002: "Invalid block size.", + 0x05040003: "Invalid sequence number in SDO block upload.", + 0x05040005: "Out of memory.", + 0x06010000: "Unsupported access to an object.", + 0x06010001: "Attempt to read a write-only object.", + 0x06010002: "Attempt to write a read-only object.", + 0x06020000: "Object does not exist in object dictionary.", + 0x06040041: "Object cannot be mapped to PDO.", + 0x06040042: "Number and length of objects to be mapped exceeds PDO length.", + 0x06040043: "General parameter incompatibility.", + 0x06060000: "Access failed due to hardware error.", + 0x06070012: "Data type does not match. Service parameter too long.", + 0x06090011: "Sub-index does not exist.", + 0x06090030: "Value range of parameter exceeded (only for write access).", + 0x06090031: "Value of parameter written too high.", + 0x06090032: "Value of parameter written too low.", + 0x06090036: "Maximum value is less than minimum value.", + 0x08000000: "General error. Use the EC command to retrieve the actual error.", + 0x08000020: "Data cannot be transferred to or stored in application.", + 0x08000022: "Data cannot be transferred to or stored in application due to present device state.", + 0x08000024: "There is no data available to transmit.", + }.get(code, sdo_default_abort_message) + + async def _add_os_query_wait_buffer(self, query: Query): + """Adds a new query to the OS query wait buffer and returns a Future for awaiting the response. + The Future will be set by the reading thread when the response is received. + """ + fut = asyncio.get_event_loop().create_future() + async with self._os_query_wait_buffer_lock: + self._os_query_wait_buffer.append((query, fut)) + return fut + + def _get_os_queries(self, node_id: int) -> List[Tuple[Query, asyncio.Future]]: + if not self._os_query_wait_buffer_lock.locked(): + raise RuntimeError("Lock must be held to access OS queries.") + return [ + (query, fut) + for query, fut in self._os_query_wait_buffer + if not fut.done() and query.node_id == node_id + ] + + async def _add_query_wait_buffer(self, query): + """Adds a new query to the query wait buffer and returns a Future for awaiting the response. + The Future will be set by the reading thread when the response is received. + """ + fut = asyncio.get_event_loop().create_future() + async with self._query_wait_buffer_lock: + self._query_wait_buffer.append((query, fut)) + return fut + + def _get_queries(self, node_id: int) -> List[Tuple[Query, asyncio.Future]]: + if not self._query_wait_buffer_lock.locked(): + raise RuntimeError("Lock must be held to access queries.") + return [ + (query, fut) + for query, fut in self._query_wait_buffer + if not fut.done() and query.node_id == node_id + ] + + async def can_sdo_upload( + self, + node_id: int, + object_byte0: int, + object_byte1: int, + sub_index: int, + ) -> bytes: + """can_sdo_upload (read). Sends an SDO Upload request and waits for the response.""" + + query = Query( + node_id=node_id, + object_byte0=object_byte0, + object_byte1=object_byte1, + sub_index=sub_index, + msg_type="SDOUA", # SDO Upload Acknowledge + ) + fut = await self._add_os_query_wait_buffer(query) + + # Command for SDO_UPLOAD_INITIATE (0x40) + await self.can_write( + cob=COBType.RSDO, + node_id=node_id, + byte0=0x40, + byte1=object_byte1, + byte2=object_byte0, + byte3=sub_index, + ) + + # Wait for response + resp = await asyncio.wait_for(fut, timeout=1.0) + + if "SDOUA" not in resp: + raise CanError( + f"Failed to receive Initiate SDO Upload acknowledgement from motor drive {node_id}" + ) + + if "ABORT" in resp: + abort_code_start = resp.index("ABORT") + abort_msg = resp[abort_code_start + 4 :] + raise CanError( + f"SDO command {hex(object_byte0 << 8 | object_byte1)} sub-index {sub_index}. Abort code received from motor drive {node_id}. {abort_msg}" + ) + + # At this point, `resp` should contain SDOUANxxx or SDOUAExxx or actual data + # This is where segmented transfer or direct values are extracted. + # This part is highly complex and depends on `SDOUA` command. + # TODO: more work here. + + # Simplified conversion from query.response to data_byte_ref + if "SDOUAN" in resp: + val_str = resp[len("SDOUAN") :] + try: + return int(val_str).to_bytes(8, "little")[:4] # Max 4 bytes for expedited + except ValueError: + raise CanError( + f"Failed to receive numeric response to {hex(object_byte0 << 8 | object_byte1)} sub-index {sub_index} from motor drive {node_id}" + ) + if "SDOUAE" in resp: + data_str = resp[len("SDOUAE") :] + return [ord(c) for c in data_str] + raise CanError(f"Failed to interpret SDO Upload response: {resp}") + + async def can_sdo_download( + self, + node_id: int, + object_byte0: int, + object_byte1: int, + sub_index: int, + data_byte: List[int], + ) -> None: + """ + can_sdo_download (write). + Sends an SDO message and waits for a response. + """ + if len(data_byte) <= 4: # Expedited SDO Download + query = Query( + node_id=node_id, + object_byte0=object_byte0, + object_byte1=object_byte1, + sub_index=sub_index, + msg_type="SDODI", # SDO Download Initiate Acknowledge + ) + fut = await self._add_os_query_wait_buffer(query) + + # 0x21: initiate download, data length indicated by n = 4 - len(data) in bytes + # If len 1 => 0x2F, len 2 => 0x2B, len 3 => 0x27, len 4 => 0x23 + cmd_byte = 0x23 | ((4 - len(data_byte)) << 2) | 0x01 # 0x01 is probably a fixed bit + filled_data = list(data_byte) + [0] * (4 - len(data_byte)) # Pad with zeros to 4 bytes + + await self.can_write( + COBType.RSDO, + node_id, + byte0=cmd_byte, + byte1=object_byte1, + byte2=object_byte0, + byte3=sub_index, + byte4=filled_data[0], + byte5=filled_data[1], + byte6=filled_data[2], + byte7=filled_data[3], + ) + + resp = await asyncio.wait_for(fut, timeout=1.0) + + if "SDODI" not in resp: + raise CanError( + f"Failed to receive Expedited SDO Download acknowledgement from motor drive {node_id}" + ) + + if "ABORT" in resp: + abort_code_start = resp.index("ABORT") + abort_msg = resp[abort_code_start + 5 :] + raise CanError( + f"SDO command {hex(object_byte0 << 8 | object_byte1)} sub-index {sub_index} abort code received from motor drive {node_id}. {abort_msg}" + ) + + else: # Segmented SDO Download (more than 4 bytes) + # This is much more involved, involving multiple CAN messages for data transfer. + query = Query( + node_id=node_id, + object_byte0=object_byte0, + object_byte1=object_byte1, + sub_index=sub_index, + msg_type="SDODI", # SDO Download Initiate Acknowledge + ) + fut = await self._add_os_query_wait_buffer(query) + + # Initiate Segmented SDO Download (0x21): Object, Subindex, Data size (total len in bytes) + # Data payload for this initiate message: object bytes, subindex, and total data len. + # `CAN_Write(eCOBType.RSDO, NodeID, (byte) 33, ObjectByte1, ObjectByte0, SubIndex)` + # Byte0=0x21 (initiate segment download), then object/subindex for the SDO. + # The data length is sent with this message if known. + # For `segmented` method, the total size is determined. + + # Byte0 = (byte)33 -- (CSID = 001, E=0, S=0) -- initiate Segmented SDO Download + # Followed by Object and Subindex + await self.can_write( + COBType.RSDO, node_id, byte0=0x21, byte1=object_byte1, byte2=object_byte0, byte3=sub_index + ) # Missing length + + # Wait for response on initiated segmented download. + resp = await asyncio.wait_for(fut, timeout=1.0) # 1000ms timeout + + if "SDODI" not in resp: + raise CanError( + f"Failed to receive Segmented SDO Download acknowledgement from motor drive {node_id}" + ) + + if "ABORT" in resp: + abort_code_start = resp.index("ABORT") + abort_msg = resp[abort_code_start + 5 :] + raise CanError( + f"SDO command {hex(object_byte0 << 8 | object_byte1)} sub-index {sub_index} abort code received from motor drive {node_id}. {abort_msg}" + ) + + # Token byte toggling and sending segments. This is a loops over data_byte_ref. + toggle_bit = 0 + bytes_sent = 0 + while bytes_sent < len(data_byte): + query_frag = Query( + node_id=node_id, + object_byte0=object_byte0, + object_byte1=object_byte1, + sub_index=sub_index, + msg_type="CHR", + ) + fut_frag = await self._add_os_query_wait_buffer(query_frag) + + remaining_data = len(data_byte) - bytes_sent + segment_len = min(remaining_data, 7) # Max 7 bytes payload for SDO segment + + # `num1` is toggle bit, `num10` is len_indicator. Last bit set if last fragment. + # Base is 0x00, toggle bit is 0x10. N=num_bytes_not_used. + # If last segment: add 0x01. + cmd_sdo_seg = (toggle_bit << 4) | ((7 - segment_len) << 1) + if bytes_sent + segment_len >= len(data_byte): # This is the last segment + cmd_sdo_seg |= 0x01 # Set last segment bit (C=1) + + segment_data = data_byte[bytes_sent : bytes_sent + segment_len] + + # Pad segment_data to 7 bytes for can_write + padded_segment = segment_data + [0] * (7 - len(segment_data)) + + await self.can_write( + COBType.RSDO, + node_id, + byte0=cmd_sdo_seg, + byte1=padded_segment[0], + byte2=padded_segment[1], + byte3=padded_segment[2], + byte4=padded_segment[3], + byte5=padded_segment[4], + byte6=padded_segment[5], + byte7=padded_segment[6], + data_length=segment_len + 1, + ) # cmd_sdo_seg itself + data + + resp = await asyncio.wait_for(fut_frag, timeout=1.0) # 1000ms timeout + + if "CHR" not in resp: + raise CanError( + f"Failed to receive OS Character Transfer acknowledgement for segment from drive {node_id}" + ) + + bytes_sent += segment_len + toggle_bit = 1 - toggle_bit # Toggle + + async def os_interpreter( + self, + node_id: int, + cmd: str, + *, + query: bool = False, + ) -> str: + def _abort_detail(resp_txt: str) -> Optional[str]: + i = resp_txt.upper().find("ABORT") + if i < 0: + return None + return resp_txt[i + 5 :].strip() + + async def send_and_wait( + *, + object_byte0: int, + object_byte1: int, + sub_index: int, + msg_type: str, + write_kwargs: dict, + timeout_s: float = 1.0, + ) -> str: + q = Query( + node_id=node_id, + object_byte0=object_byte0, + object_byte1=object_byte1, + sub_index=sub_index, + msg_type=msg_type, + ) + fut = await self._add_os_query_wait_buffer(query=q) + await self.can_write(**write_kwargs) + return await asyncio.wait_for(fut, timeout=timeout_s) + + # --- 1) "Evaluate Immediately mode" acknowledge (SDODI), write: 35,36,0x10 + try: + r = await send_and_wait( + object_byte0=0x10, + object_byte1=0x24, # 36 + sub_index=0, + msg_type="SDODI", + write_kwargs=dict( + cob=COBType.RSDO, + node_id=node_id, + byte0=0x23, # 35 + byte1=0x24, # 36 + byte2=0x10, + ), + ) + except Exception as e: + raise CanError( + f"Failed to send OS evaluate-immediately command to motor drive {node_id}: {e}" + ) + + if "SDODI" not in r: + raise CanError( + f"Failed to receive OS Evaluate Immediately mode acknowledgement from motor drive {node_id}" + ) + + ab = _abort_detail(r) + if ab is not None: + raise CanError( + f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}" + ) + + # --- 2) Initiate segmented SDO download, sub=1, write: 33,35,0x10,1 + r = await send_and_wait( + object_byte0=0x10, + object_byte1=0x23, # 35 + sub_index=1, + msg_type="SDODI", + write_kwargs=dict( + cob=COBType.RSDO, + node_id=node_id, + byte0=0x21, # 33 + byte1=0x23, # 35 + byte2=0x10, + byte3=0x01, + ), + ) + + if "SDODI" not in r: + raise CanError( + f"Failed to receive Segmented SDO Download acknowledgement from motor drive {node_id}" + ) + + ab = _abort_detail(r) + if ab is not None: + raise CanError( + f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}" + ) + + # --- 3) Send command as 7-byte segments with CANopen toggle/unused/last bits. + cmd_bytes = cmd.encode("ascii", errors="strict") + chunks: List[bytes] = [cmd_bytes[i : i + 7] for i in range(0, len(cmd_bytes), 7)] + if not chunks: + chunks = [b""] + + toggle = 1 + + for idx, chunk in enumerate(chunks): + toggle = 0 if toggle != 0 else 1 # flip each segment + unused = 7 - len(chunk) + last = idx == len(chunks) - 1 + + # Byte0: [toggle in bit4] + [unused in bits1-3] + [last in bit0] + byte0 = (toggle << 4) | ((unused & 0x07) << 1) | (1 if last else 0) + + padded = chunk + (b"0" * unused) + b1, b2, b3, b4, b5, b6, b7 = (padded[i] for i in range(7)) + + r = await send_and_wait( + object_byte0=0, + object_byte1=0, + sub_index=0, + msg_type="CHR", + write_kwargs=dict( + cob=COBType.RSDO, + node_id=node_id, + byte0=byte0, + byte1=b1, + byte2=b2, + byte3=b3, + byte4=b4, + byte5=b5, + byte6=b6, + byte7=b7, + ), + ) + + if "CHR" not in r: + raise CanError( + f"Failed to receive OS Character Transfer acknowledgement from motor drive {node_id}" + ) + + ab = _abort_detail(r) + if ab is not None: + raise CanError( + f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}" + ) + + # --- 4) Read OS command status (STAT), sub=2, write: 0x40,35,0x10,2 + r = await send_and_wait( + object_byte0=0x10, + object_byte1=0x23, # 35 + sub_index=2, + msg_type="STAT", + write_kwargs=dict( + cob=COBType.RSDO, + node_id=node_id, + byte0=0x40, + byte1=0x23, + byte2=0x10, + byte3=0x02, + ), + ) + + if "STAT" not in r: + raise CanError(f"Failed to receive OS command status from motor drive {node_id}") + + ab = _abort_detail(r) + if ab is not None: + raise CanError( + f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}" + ) + + # If not querying a response, done. + if not query: + return "" + + # --- 5) Initiate SDO upload (SDOUA), sub=3, write: 0x40,35,0x10,3 + r = await send_and_wait( + object_byte0=0x10, + object_byte1=0x23, + sub_index=3, + msg_type="SDOUA", + write_kwargs=dict( + cob=COBType.RSDO, + node_id=node_id, + byte0=0x40, + byte1=0x23, + byte2=0x10, + byte3=0x03, + ), + ) + + if "SDOUA" not in r: + raise CanError( + f"Failed to receive Initiate SDO Upload acknowledgement from motor drive {node_id}" + ) + + ab = _abort_detail(r) + if ab is not None: + raise CanError( + f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}" + ) + + # expects r like: "SDOUA" + ("N" or "E") + payload + response_out = "" + + if len(r) >= 6 and r.startswith("SDOUA"): + kind = r[5:6] # 6th char (1-based) + payload = r[6:] # everything after that char + + if kind == "E": + response_out = payload + return response_out + + if kind == "N": + # if non-numeric => error + if not payload.strip().lstrip("+-").isdigit(): + raise CanError(f"Failed to receive a response to '{cmd}' from motor drive {node_id}") + # else fall through to segmented upload reading below + + # --- 6) Segmented SDO upload (SDOSU) + seg_idx = 0 + response_out = "" + + while True: + # client request byte0 toggles between 0x60 and 0x70 + req0 = 0x60 if (seg_idx % 2) == 0 else 0x70 + + r = await send_and_wait( + object_byte0=0, + object_byte1=0, + sub_index=0, + msg_type="SDOSU", + write_kwargs=dict( + cob=COBType.RSDO, + node_id=node_id, + byte0=req0, + ), + ) + + if "SDOSU" not in r: + raise CanError(f"Failed to receive OS response value from motor drive {node_id}") + + ab = _abort_detail(r) + if ab is not None: + raise CanError( + f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}" + ) + + if len(r) <= 7: + raise CanError(f"Failed to extract data response for '{cmd}' from motor drive {node_id}") + + # r: "SDOSU" + (pos6: 'C'/'D') + (pos7: toggle bit '0'/'1') + data... + cd_flag = r[5:6] + toggle_bit = r[6:7] + data_part = r[7:] + + response_out += data_part + + if (req0 == 0x60 and toggle_bit == "1") or (req0 == 0x70 and toggle_bit == "0"): + raise CanError(f"Toggle bit mismatch in response for '{cmd}' from motor drive {node_id}") + + if cd_flag == "C": + seg_idx += 1 + continue + + if cd_flag == "D": + return response_out + + raise CanError(f"Failed to receive data response for '{cmd}' from motor drive {node_id}") + + async def can_sync(self) -> None: + await self.can_write(cob=COBType.SYNC, node_id=0) + + async def can_sdo_download_elmo_object( + self, + node_id: int, + elmo_object_int: int, + sub_index: int, + data: str, + data_type: ElmoObjectDataType, + ) -> None: + """Wrapper around can_sdo_download for Elmo specific objects, handling data type conversions.""" + data_bytes: List[int] = [] + + if data_type == ElmoObjectDataType.UNSIGNED8: + data_bytes = list(int(data).to_bytes(1, "little")) + elif data_type == ElmoObjectDataType.UNSIGNED16: + data_bytes = list(int(data).to_bytes(2, "little")) + elif data_type == ElmoObjectDataType.UNSIGNED32: + data_bytes = list(int(float(data)).to_bytes(4, "little")) + elif data_type == ElmoObjectDataType.UNSIGNED64: + data_bytes = list(int(data).to_bytes(8, "little")) + elif data_type == ElmoObjectDataType.INTEGER8: + data_bytes = list(int(data).to_bytes(1, "little", signed=True)) + elif data_type == ElmoObjectDataType.INTEGER16: + data_bytes = list(int(data).to_bytes(2, "little", signed=True)) + elif data_type == ElmoObjectDataType.INTEGER32: + data_bytes = list(int(float(data)).to_bytes(4, "little", signed=True)) + elif data_type == ElmoObjectDataType.INTEGER64: + data_bytes = list(int(data).to_bytes(8, "little", signed=True)) + elif data_type == ElmoObjectDataType.STR: + data_bytes = [ord(c) for c in data] + else: + raise CanError(f"Unsupported data type for SDO Write: {data_type.name}") + + obj_byte0 = elmo_object_int >> 8 + obj_byte1 = elmo_object_int & 0xFF + await self.can_sdo_download(node_id, obj_byte0, obj_byte1, sub_index, data_bytes) + + async def can_sdo_upload_elmo_object( + self, + node_id: int, + elmo_object_int: int, + sub_index: int, + data_type: ElmoObjectDataType, + ) -> str: + """Wrapper around can_sdo_upload for Elmo specific objects, handling data type conversions.""" + + obj_byte0 = elmo_object_int >> 8 + obj_byte1 = elmo_object_int & 0xFF + data_bytes = await self.can_sdo_upload(node_id, obj_byte0, obj_byte1, sub_index) + + if len(data_bytes) == 0: + return "" + if data_type == ElmoObjectDataType.UNSIGNED8: + return str(int.from_bytes(data_bytes[:1], "little", signed=False)) + if data_type == ElmoObjectDataType.UNSIGNED16: + return str(int.from_bytes(data_bytes[:2], "little", signed=False)) + if data_type == ElmoObjectDataType.UNSIGNED32: + return str(int.from_bytes(data_bytes[:4], "little", signed=False)) + if data_type == ElmoObjectDataType.UNSIGNED64: + return str(int.from_bytes(data_bytes[:8], "little", signed=False)) + if data_type == ElmoObjectDataType.INTEGER16: + return str(int.from_bytes(data_bytes[:2], "little", signed=True)) + if data_type == ElmoObjectDataType.INTEGER32: + return str(int.from_bytes(data_bytes[:4], "little", signed=True)) + if data_type == ElmoObjectDataType.INTEGER64: + return str(int.from_bytes(data_bytes[:8], "little", signed=True)) + if data_type == ElmoObjectDataType.STR: + return "".join([chr(b) for b in data_bytes]) + raise CanError(f"Unsupported data type for SDO Read conversion: {data_type.name}") + + # --- PDO Mapping methods --- + + async def can_tpdo_unmap(self, tpdo: TPDO, node_id: int): + cob_type_int = { + TPDO.TPDO1: COBType.TPDO1.value, + TPDO.TPDO3: COBType.TPDO3.value, + TPDO.TPDO4: COBType.TPDO4.value, + }[tpdo] + + if not (0 <= node_id <= 0x7F): + raise ValueError(f"node_id must be 0..127, got {node_id}") + + node_id = node_id & 0x7F + num1 = ((cob_type_int & 0x01) << 7) | node_id + num2 = (cob_type_int >> 1) & 0x07 + + await self.can_sdo_download( + node_id=node_id, + object_byte0=24, + object_byte1=tpdo.value - 1, + sub_index=1, + data_byte=[ + num1, + num2, + 0, + 0xC0, + ], + ) + + await self.can_sdo_download( + node_id=node_id, + object_byte0=26, + object_byte1=tpdo.value - 1, + sub_index=0, + data_byte=[0, 0, 0, 0], + ) + + for index in range(len(self.tpdo_mapped_object[node_id][tpdo])): + self.tpdo_mapped_object[node_id][tpdo][index] = TPDOMappedObject.NotMapped + + async def can_tpdo3_map(self, node_id: int) -> None: + mapped_objects = [TPDOMappedObject.StatusWord] + # EventTimerMs=0, Delay100us=0, TransmissionType=EventDrivenDev + await self.can_tpdo_map( + tpdo=TPDO.TPDO3, + node_id=node_id, + mapped_objects=mapped_objects, + event_trigger=TPDOTrigger.MotionComplete, + ) + + async def can_tpdo4_map(self, node_id: int) -> None: + mapped_objects = [TPDOMappedObject.DigitalInputs] + await self.can_tpdo_map( + tpdo=TPDO.TPDO4, + node_id=node_id, + mapped_objects=mapped_objects, + event_trigger=TPDOTrigger.DigitalInputEvent, + ) + + async def can_rpdo1_map(self, node_id: int) -> None: + mapped_objects = [RPDOMappedObject.ControlWord] + await self.can_rpdo_map( + rpdo=RPDO.RPDO1, + node_id=node_id, + mapped_objects=mapped_objects, + transmission_type=PDOTransmissionType.SynchronousCyclic, + ) + + async def can_rpdo3_map(self, node_id: int) -> None: + mapped_objects = [RPDOMappedObject.TargetPositionIP, RPDOMappedObject.TargetVelocityIP] + await self.can_rpdo_map( + rpdo=RPDO.RPDO3, + node_id=node_id, + mapped_objects=mapped_objects, + transmission_type=PDOTransmissionType.EventDrivenDev, + ) + + async def can_rpdo_map( + self, + rpdo: RPDO, + node_id: int, + mapped_objects: List[RPDOMappedObject], + transmission_type: PDOTransmissionType, + ) -> None: + """Maps RPDOs for incoming messages.""" + + rpdo_num = int(rpdo) # expects 1..4 + rpdo_idx = (rpdo_num - 1) & 0xFF # original passes (byte)(RPDO - 1) + + # Map RPDO -> COB function code (4-bit) used to build the 11-bit COB-ID + # (original decompilation only showed 1,3,4; RPDO2 is included here) + if rpdo == RPDO.RPDO1: + cob_type = COBType.RPDO1 + elif rpdo == RPDO.RPDO3: + cob_type = COBType.RPDO3 + elif rpdo == RPDO.RPDO4: + cob_type = COBType.RPDO4 + else: + raise ValueError(f"Unsupported RPDO: {rpdo!r}") + + # CANopen 11-bit COB-ID: (function_code << 7) | node_id(7 bits) + cob_id_11 = ((int(cob_type) & 0x0F) << 7) | (node_id & 0x7F) + + # 1) Disable PDO (set bit 31) while configuring: 0x80000000 | cob_id + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x14, + object_byte1=rpdo_idx, + sub_index=0x01, + data_byte=_u32_le(0x80000000 | cob_id_11), + ) + + # 2) Clear mapping count (sub 0) at 0x1600 + rpdo_idx + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x16, + object_byte1=rpdo_idx, + sub_index=0x00, + data_byte=[0, 0, 0, 0], + ) + + # 3) Set transmission type (sub 2) at 0x1400 + rpdo_idx + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x14, + object_byte1=rpdo_idx, + sub_index=0x02, + data_byte=[int(transmission_type) & 0xFF, 0, 0, 0], + ) + + # 4) Write each mapped object (sub 1..n) at 0x1600 + rpdo_idx + for i, mo in enumerate(mapped_objects): + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x16, + object_byte1=rpdo_idx, + sub_index=(i + 1) & 0xFF, + data_byte=_u32_le(int(mo)), + ) + + # 5) Set mapping count (sub 0) + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x16, + object_byte1=rpdo_idx, + sub_index=0x00, + data_byte=[len(mapped_objects) & 0xFF, 0, 0, 0], + ) + + # 6) Enable PDO (clear bit 31): write cob_id only + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x14, + object_byte1=rpdo_idx, + sub_index=0x01, + data_byte=_u32_le(cob_id_11), + ) + + async def can_tpdo_map( + self, + tpdo: TPDO, + node_id: int, + mapped_objects: List[TPDOMappedObject], + event_trigger: TPDOTrigger, + event_timer_ms: int = 0, + delay_100_us: int = 0, + transmission_type: PDOTransmissionType = PDOTransmissionType.EventDrivenDev, + ) -> None: + """Maps TPDOs for outgoing messages.""" + + tpdo_num = int(tpdo) # expects 1..4 + tpdo_idx = (tpdo_num - 1) & 0xFF # (byte)(TPDO - 1) + + if tpdo == TPDO.TPDO1: + cob_type = COBType.TPDO1 + elif tpdo == TPDO.TPDO3: + cob_type = COBType.TPDO3 + elif tpdo == TPDO.TPDO4: + cob_type = COBType.TPDO4 + else: + raise ValueError(f"Unsupported TPDO: {tpdo!r}") + + # CANopen 11-bit COB-ID + cob_id_11 = ((int(cob_type) & 0x0F) << 7) | (node_id & 0x7F) + + # Event trigger mask: 2**EventTrigger, stored as u32 + event_mask = 1 << int(event_trigger) + + # 1) Disable TPDO while configuring: 0xC0000000 | cob_id + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x18, + object_byte1=tpdo_idx, + sub_index=0x01, + data_byte=_u32_le(0xC0000000 | cob_id_11), + ) + + # 2) Clear mapping count: 0x1A00 + tpdo_idx, sub 0 + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x1A, + object_byte1=tpdo_idx, + sub_index=0x00, + data_byte=[0, 0, 0, 0], + ) + + # 3) Transmission type: 0x1800 + tpdo_idx, sub 2 + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x18, + object_byte1=tpdo_idx, + sub_index=0x02, + data_byte=[int(transmission_type) & 0xFF, 0, 0, 0], + ) + + # 4) Inhibit time / delay (100us units): sub 3 + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x18, + object_byte1=tpdo_idx, + sub_index=0x03, + data_byte=[delay_100_us & 0xFF, 0, 0, 0], + ) + + # 5) Event timer (ms): sub 5 + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x18, + object_byte1=tpdo_idx, + sub_index=0x05, + data_byte=[event_timer_ms & 0xFF, 0, 0, 0], + ) + + # 6) Write event trigger mask to 0x2F20 sub = TPDO (matches can_sdo_download(NodeID, 0x2F, 0x20, TPDO, ...)) + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x2F, + object_byte1=0x20, + sub_index=tpdo_num & 0xFF, + data_byte=_u32_le(event_mask), + ) + + # 7) Write mapped objects into 0x1A00 + tpdo_idx, sub 1..n + for i, mo in enumerate(mapped_objects): + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x1A, + object_byte1=tpdo_idx, + sub_index=(i + 1) & 0xFF, + data_byte=_u32_le(int(mo)), + ) + + # 8) Set mapping count (sub 0) + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x1A, + object_byte1=tpdo_idx, + sub_index=0x00, + data_byte=[len(mapped_objects) & 0xFF, 0, 0, 0], + ) + + # 9) Re-enable TPDO: write 0x40000000 | cob_id + await self.can_sdo_download( + node_id=node_id, + object_byte0=0x18, + object_byte1=tpdo_idx, + sub_index=0x01, + data_byte=_u32_le(0x40000000 | cob_id_11), + ) + + # 10) Mirror the original side-effect: store mapping in self.tpdo_mapped_object + self.tpdo_mapped_object[node_id][tpdo] = mapped_objects + + async def pvt_select_mode(self, enable: bool) -> None: + """Enables or disables PVT mode on all motors in the group.""" + + if enable: + if not self._pvt_mode: + for axis in KX2Backend.MOTION_AXES: + await self.can_sdo_download( + node_id=int(axis), + object_byte0=0x60, + object_byte1=0xC4, + sub_index=0x06, + data_byte=[0], + ) + await self.can_sdo_download( + node_id=int(axis), + object_byte0=0x60, + object_byte1=0x60, + sub_index=0x00, + data_byte=[7], + ) + self._pvt_mode = True + else: + for axis in KX2Backend.MOTION_AXES: + await self.can_sdo_download( + node_id=int(axis), + object_byte0=0x60, + object_byte1=0x60, + sub_index=0x00, + data_byte=[1], + ) + await self.can_sdo_download( + node_id=int(axis), + object_byte0=0x60, + object_byte1=0xC4, + sub_index=0x06, + data_byte=[0], + ) + await self.can_sdo_download( + node_id=int(axis), + object_byte0=0x60, + object_byte1=0x60, + sub_index=0x00, + data_byte=[7], + ) + elif self._pvt_mode: + for axis in KX2Backend.MOTION_AXES: + await self.can_sdo_download( + node_id=int(axis), + object_byte0=0x60, + object_byte1=0x60, + sub_index=0x00, + data_byte=[1], + ) + self._pvt_mode = False + + async def binary_interpreter( + self, + node_id: int, + cmd: str, + cmd_index: int, + cmd_type: CmdType, + value: str = "0", + val_type: ValType = ValType.Int, + low_priority: bool = False, + ) -> Union[str, float]: + timeout = 10.0 if cmd.upper() == "SV" else 1.0 + + is_float = val_type == ValType.Float + is_query = cmd_type == CmdType.ValQuery + is_execute = cmd_type == CmdType.Execute + + # Helper: build command bytes + def _build_bytes() -> tuple[int, int, int, int, int, int, int, int]: + # byte0, byte1: ASCII of first and last char of cmd + byte0 = ord(cmd[0]) + byte1 = ord(cmd[-1]) + + # Encode cmd_index into 14 bits: + # - low 8 bits -> byte2 + # - high 6 bits -> lower bits of byte3 + byte2 = cmd_index & 0xFF + byte3 = (cmd_index >> 8) & 0x3F # keep only 6 bits + + # Set flags in bits 6 and 7 of byte3 + if is_query: + byte3 |= 0x40 # bit 6 + if is_float: + byte3 |= 0x80 # bit 7 + + # Encode value + if is_float: + byte4, byte5, byte6, byte7 = struct.pack(" bool: + try: + expected = float(expected_str) + actual = float(actual_str) + except ValueError: + return False + if actual == 0.0: + return expected == 0.0 + ratio = expected / actual + return expected == actual or (0.99 < ratio < 1.01) + + max_attempts = 1 + + # Single-node path (NodeID != 10) + if node_id != 10: + for attempt in range(1, max_attempts + 1): + if value == "": + value = "0" + + ( + byte0, + byte1, + byte2, + byte3, + byte4, + byte5, + byte6, + byte7, + ) = _build_bytes() + + # Build a minimal query object compatible with your buffer/reader + # We don't know your exact type, so SimpleNamespace gives us attributes. + query = Query( + node_id=node_id, + msg_index=cmd_index, + msg_type=cmd, + ) + + fut = await self._add_query_wait_buffer(query) + + await self.can_write( + COBType.RPDO2, + node_id, + byte0, + byte1, + byte2, + byte3, + byte4, + byte5, + byte6, + byte7, + execute=is_execute, + low_priority=low_priority, + data_length=4 if is_execute else 8, + ) + + try: + resp = await asyncio.wait_for(fut, timeout=timeout) + except asyncio.TimeoutError: + if attempt == max_attempts: + raise CanError( + f"Timeout waiting for response to {cmd}[{cmd_index}] from node {node_id}" + ) + # retry + continue + + # Query: just return the response + if is_query: + value = str(resp) + return float(value) if is_float else int(float(value)) + + # Execute: only care that we got *some* response + if is_execute: + if resp == "" and attempt == max_attempts: + raise CanError( + f"No response for execute command {cmd}[{cmd_index}] from node {node_id}" + ) + if resp != "": + return float(value) if is_float else int(float(value)) + # else retry + continue + + # Write: verify echoed value + if is_float: + ok = _float_matches(value, str(resp)) + else: + ok = (int(float(resp))) == (int(float(value))) + + if ok: + return float(value) if is_float else int(float(value)) + + if attempt == max_attempts: + raise CanError( + f"Unexpected CAN response: attempted to send {cmd}[{cmd_index}]={value}, " + f"but received response={resp} from node {node_id}" + ) + # else retry + + # Should never get here + raise CanError("Internal error in binary_interpreter (single-node)") + + # Group path (NodeID == 10) + grp_ids = [int(axis) for axis in KX2Backend.MOTION_AXES] + + for attempt in range(1, max_attempts + 1): + if value == "": + value = "0" + + ( + byte0, + byte1, + byte2, + byte3, + byte4, + byte5, + byte6, + byte7, + ) = _build_bytes() + + # One query per node in group + queries = [] + futures = [] + for gid in grp_ids: + q = Query( + node_id=gid, + msg_index=cmd_index, + msg_type=cmd, + ) + queries.append(q) + fut = await self._add_query_wait_buffer(q) + futures.append(fut) + + await self.can_write( + COBType.RPDO2, + node_id, # broadcast/group node (10) + byte0, + byte1, + byte2, + byte3, + byte4, + byte5, + byte6, + byte7, + execute=is_execute, + low_priority=low_priority, + data_length=4 if is_execute else 8, + ) + + try: + # Wait for *all* group responses + resps = await asyncio.wait_for( + asyncio.gather(*futures, return_exceptions=False), + timeout=timeout, + ) + except asyncio.TimeoutError: + if attempt == max_attempts: + raise CanError( + f"Timeout waiting for group response to {cmd}[{cmd_index}] " f"from nodes {grp_ids}" + ) + # retry + continue + + # Query: concatenate responses with commas + if is_query: + if any(r == "" for r in resps): + if attempt == max_attempts: + raise CanError( + f"Incomplete group query response for {cmd}[{cmd_index}] " f"from nodes {grp_ids}" + ) + # retry + continue + + value = ",".join(str(r) for r in resps) + return float(value) if is_float else int(float(value)) + + # Execute: just require all responses non-empty + if is_execute: + if all(r != "" for r in resps): + return float(value) if is_float else int(float(value)) + if attempt == max_attempts: + missing_nodes = [gid for gid, r in zip(grp_ids, resps) if r == ""] + raise CanError( + f"No execute response from nodes {missing_nodes} " f"for {cmd}[{cmd_index}]" + ) + # retry + continue + + # Write: verify each node's echoed value + mismatch_node = None + mismatch_resp = None + + for gid, resp in zip(grp_ids, resps): + if is_float: + ok = _float_matches(value, str(resp)) + else: + ok = str(resp) == str(value) + + if not ok: + mismatch_node = gid + mismatch_resp = resp + break + + if mismatch_node is None: + # everyone matched + return float(value) if is_float else int(float(value)) + + if attempt == max_attempts: + raise CanError( + f"Unexpected CAN response: attempted to send {cmd}[{cmd_index}]={value}, " + f"but received response={mismatch_resp} from node {mismatch_node}" + ) + # else retry + + # Should never get here + raise CanError("Internal error in binary_interpreter (group)") + + # --- Functions --- + + async def configure_input_logic( + self, + node_id: int, + input_num: int, + logic_high: bool, + logic_type: InputLogic, + ) -> None: + val_to_set = logic_type.value + if logic_high: + val_to_set += 1 + + right = await self.binary_interpreter(node_id, "IL", input_num, CmdType.ValQuery) + + if val_to_set != right: + await self.binary_interpreter(node_id, "IL", input_num, CmdType.ValSet, str(val_to_set)) + await asyncio.sleep(0.25) + + async def read_input(self, node_id: int, input_num: int) -> bool: + """Returns the State (bool).""" + left = await self.binary_interpreter(node_id, "IB", input_num, CmdType.ValQuery) + return left == 1 + + async def read_output(self, node_id: int, output_num: int) -> bool: + """Returns the State.""" + expression = await self.binary_interpreter(node_id, "OP", 0, CmdType.ValQuery) + val = int(expression) + + mask = int(round(math.pow(2, output_num - 1))) + return (val & mask) == mask + + async def set_output(self, node_id: int, output_num: int, state: bool) -> str: + val = "1" if state else "0" + return await self.binary_interpreter(node_id, "OB", output_num, CmdType.ValSet, val) + + async def motor_get_current_position(self, node_id: int, pu: bool = False) -> int: + cmd = "PU" if pu else "PX" + val_str = await self.binary_interpreter(int(node_id), cmd, 0, CmdType.ValQuery) + return int(round(float(val_str))) + + async def motor_get_motion_status(self, node_id: int) -> int: + val = await self.binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) + return int(round(float(val))) + + async def motor_enable(self, axis: KX2Axis, state: bool) -> None: + if not isinstance(axis, KX2Axis): + raise + + flag = not (axis in KX2Backend.MOTION_AXES or int(axis) == self.grp_id) + + if state: + self.EmcyMoveErrorReceived = False + if flag: + await self.binary_interpreter(axis, "MO", 0, CmdType.ValSet, "1") + else: + # Standard DS402 Enable Sequence + await self.control_word_set(node_id=int(axis), value=0) + await self.control_word_set(node_id=int(axis), value=128) + await self.control_word_set(node_id=int(axis), value=6) + await self.control_word_set(node_id=int(axis), value=7) + await self.control_word_set(node_id=int(axis), value=15) + + await asyncio.sleep(0.1) + + left = await self.binary_interpreter( + node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery, val_type=ValType.Int + ) + if left != 1: + raise CanError(f"Motor failed to enable (axis = {axis})") + else: + if flag: + try: + await self.binary_interpreter( + node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValSet, value="0" + ) + except Exception as e: + pass + else: + await self.control_word_set(node_id=int(axis), value=7) + await self.control_word_set(node_id=int(axis), value=6) + + await asyncio.sleep(0.1) + left = await self.binary_interpreter( + node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery + ) + if left != 0: + raise RuntimeError(f"Motor failed to disable (axis = {axis}") + + async def motor_set_move_direction(self, node_id: int, direction: JointMoveDirection) -> None: + val_str = "1" + if direction == JointMoveDirection.Clockwise: + val_str = "65" + elif direction == JointMoveDirection.Counterclockwise: + val_str = "129" + elif direction == JointMoveDirection.ShortestWay: + val_str = "193" + + await self.can_sdo_download_elmo_object( + node_id, 24818, 0, val_str, ElmoObjectDataType.UNSIGNED16 + ) + + async def motor_emergency_stop(self, node_id: int) -> None: + await self.binary_interpreter(node_id, "MO", 0, CmdType.ValSet, "0") + + async def user_program_run( + self, + axis: KX2Axis, + user_function: str, + params=None, + timeout_sec: int = 0, + wait_until_done: bool = False, + ) -> int: + """ + Runs a user program on `axis` and optionally waits for completion. + + Returns: + last_line_completed (0 if unknown / not provided by controller) + Raises: + CanError on any controller/protocol/runtime failure or timeout. + """ + if not isinstance(axis, int): + raise ValueError("axis must be int") + if axis < 0 or axis > 255: + raise ValueError("axis must be in [0, 255]") + + node_id = int(axis) + + # PS query + ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + + if ps == -2: + raise CanError(f"Axis {axis}: controller reported PS=-2 (not ready / unavailable)") + + # If not idle (-1), request idle by setting UI[1]=0 and wait up to 3s for PS=-1 + if ps != -1: + await self.binary_interpreter( + node_id, + "UI", + 1, + CmdType.ValSet, + value=0, # don't stringify bytes; pass normal ints + val_type=ValType.Int, + ) + + t0 = time.monotonic() + while (time.monotonic() - t0) < 3.0: + ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + if ps == -1: + break + await asyncio.sleep(0.01) + else: + raise CanError(f"Axis {axis}: did not reach idle state (PS=-1) within 3s (last PS={ps})") + + # Build "(a,b,c)" argument list + arg_str = "" + if params: + parts = [str(p) for p in params] + print(parts) + if parts: + arg_str = f"({','.join(parts)})" + print(arg_str) + + # Arm UI[1]=1 then execute XQ + await self.binary_interpreter( + node_id, + "UI", + 1, + CmdType.ValSet, + value="1", + val_type=ValType.Int, + ) + + cmd = f"XQ##{user_function}{arg_str}" + print(cmd) + await self.os_interpreter(node_id, cmd, query=False) + + last_line_completed = 0 + + if wait_until_done: + # Wait while PS==1 and UI[1]==1, or until timeout + t0 = time.monotonic() + ps = 1 + ui1 = 1 + while ps == 1 and ui1 == 1 and (time.monotonic() - t0) < float(timeout_sec): + ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + ui1 = int(await self.binary_interpreter(node_id, "UI", 1, CmdType.ValQuery)) + await asyncio.sleep(0.01) + + # Grab UI[2] (last line completed) after wait loop (matches original behavior) + expr_raw = await self.binary_interpreter(node_id, "UI", 2, CmdType.ValQuery) + try: + last_line_completed = int(str(expr_raw).strip()) + except Exception: + last_line_completed = 0 + + # UI[1] should be "0" on successful completion + if ui1 != 0: + raise CanError( + f"Axis {axis}: user program ended with UI[1]={ui1} (expected 0), last_line={last_line_completed}" + ) + + # Timeout condition: still stuck in running state + if ps == 1 and ui1 == 1: + raise CanError( + f"Axis {axis}: timeout waiting for '{user_function}' after {timeout_sec}s, last_line={last_line_completed}" + ) + + return 0 + + async def home_motor( + self, + axis: "KX2Axis", + hs_offset: int, + ind_offset: int, + home_pos: int, + srch_vel: int, + srch_acc: int, + max_pe: int, + hs_pe: int, + offset_vel: int, + offset_acc: int, + timeout: float, + ) -> None: + left = await self.binary_interpreter(int(axis), "CA", 41, CmdType.ValQuery) + if left == 24: + raise RuntimeError("Error 43") + + try: + await self.motor_hard_stop_search(axis, srch_vel, srch_acc, max_pe, hs_pe, timeout) + except Exception as e: + # Check fault + fault = await self.motor_get_fault(axis) + if fault is not None: + raise RuntimeError(fault) + raise e + + await self.motor_enable(axis=axis, state=True) + + await self.motors_move_absolute_execute( + plan=MotorsMovePlan( + moves=[ + MotorMoveParam( + axis=KX2Axis(axis), + position=hs_offset, + velocity=offset_vel, + acceleration=offset_acc, + relative=False, + direction=JointMoveDirection.ShortestWay, + ) + ], + ) + ) + + is_positive = hs_offset > 0 + await self.motor_index_search(axis, abs(srch_vel), srch_acc, is_positive, timeout) + + await self.motors_move_absolute_execute( + plan=MotorsMovePlan( + moves=[ + MotorMoveParam( + axis=KX2Axis(axis), + position=ind_offset, + velocity=offset_vel, + acceleration=offset_acc, + relative=False, + direction=JointMoveDirection.ShortestWay, + ) + ] + ) + ) + await self.motor_reset_encoder_position(axis, home_pos) + await self.motor_set_homed_status(axis, HomeStatus.Homed) + + async def motor_hard_stop_search( + self, axis: KX2Axis, srch_vel: int, srch_acc: int, max_pe: int, hs_pe: int, timeout: float + ) -> None: + await self.binary_interpreter(int(axis), "ER", 3, CmdType.ValSet, str(max_pe * 10)) + await self.binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) + await self.binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) + # Clear homing params + for i in [3, 4, 5, 2]: + await self.binary_interpreter(int(axis), "HM", i, CmdType.ValSet, "0") + + await self.binary_interpreter( + node_id=int(axis), cmd="JV", cmd_index=0, cmd_type=CmdType.ValSet, value=str(srch_vel) + ) + + try: + params = [str(int(hs_pe)), str(int(timeout * 1000))] + try: + last_line = await self.user_program_run(axis, "Home", params, int(timeout), True) + if last_line in [1, 2, 3]: + raise RuntimeError(f"Homing Script Error {34 + last_line}") + except Exception as e: + # Re-raise unless specific handling needed + raise e + + curr_pos = await self.motor_get_current_position(axis) + + await self.binary_interpreter( + node_id=int(axis), cmd="PA", cmd_index=0, cmd_type=CmdType.ValSet, value=str(curr_pos) + ) + await self.binary_interpreter( + node_id=int(axis), cmd="SP", cmd_index=0, cmd_type=CmdType.ValSet, value=str(srch_vel) + ) + await self.binary_interpreter( + node_id=int(axis), cmd="AC", cmd_index=0, cmd_type=CmdType.ValSet, value=str(srch_acc) + ) + await self.binary_interpreter( + node_id=int(axis), cmd="DC", cmd_index=0, cmd_type=CmdType.ValSet, value=str(srch_acc) + ) + finally: + # Stop any lingering motion/scripts + await asyncio.sleep(0.3) + await self.binary_interpreter(int(axis), "BG", 0, CmdType.Execute, value="0") + await asyncio.sleep(0.3) + + # Restore Error Range to normal safety limits + await self.binary_interpreter(int(axis), "ER", 3, CmdType.ValSet, str(int(max_pe))) + + # Force Motor Off to kill any zombie scripts holding the state machine + # if self.move_error_code != 0: # Only if we had an error + # await self.binary_interpreter(int(axis), "MO", 0, eCmdType.ValSet, "0") + + async def motor_index_search( + self, axis: KX2Axis, srch_vel: int, srch_acc: int, positive_direction: bool, timeout: float + ) -> Tuple[int, int]: + """Returns (OneRevolution, CapturedPosition).""" + await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") + + rev = await self.binary_interpreter(int(axis), "CA", 18, CmdType.ValQuery) + one_revolution = int(float(rev)) + if not positive_direction: + one_revolution *= -1 + + await self.binary_interpreter(int(axis), "PR", 1, CmdType.ValSet, str(one_revolution)) + await self.binary_interpreter(int(axis), "SP", 0, CmdType.ValSet, str(srch_vel)) + await self.binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) + await self.binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) + + await self.binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "3") # Index only + await self.binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") # Arm + + self._waiting_moves = {axis: asyncio.get_event_loop().create_future()} + await self.binary_interpreter(int(axis), "BG", 0, CmdType.Execute) + await self._wait_for_moves_done(timeout) + + left = await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValQuery) + if left != 0: + raise RuntimeError("Homing Failure: Failed to finish index pulse search.") + + cap = await self.binary_interpreter(int(axis), "HM", 7, CmdType.ValQuery) + captured_position = int(float(cap)) + + return one_revolution, captured_position + + async def motor_reset_encoder_position(self, axis: KX2Axis, position: float) -> None: + await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, str(position)) + await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") + + async def motor_set_homed_status(self, axis: KX2Axis, status: HomeStatus) -> None: + val = "0" + if status == HomeStatus.Homed: + val = "1" + elif status == HomeStatus.InitializedWithoutHoming: + val = "2" + await self.binary_interpreter(int(axis), "UI", 3, CmdType.ValSet, val) + + async def motor_get_homed_status(self, node_id: int) -> HomeStatus: + left = await self.binary_interpreter(node_id, "UI", 3, CmdType.ValQuery) + if left == 1: + return HomeStatus.Homed + if left == 2: + return HomeStatus.InitializedWithoutHoming + return HomeStatus.NotHomed + + async def motor_check_if_move_done(self, node_id: int) -> bool: + """Returns Done status. Raises error on fault.""" + ms_val = await self.binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) + + if ms_val == 0: + return True + if ms_val == 1: + mo_val = await self.binary_interpreter(node_id, "MO", 0, CmdType.ValQuery) + if mo_val == 1: + return True + fault = await self.motor_get_fault(node_id) + if fault is not None: + raise RuntimeError(f"Motor Fault: {fault}") + raise RuntimeError("Motor Fault (Unknown)") + if ms_val == 2: + return False + + return False + + async def motor_get_fault(self, axis: KX2Axis) -> Optional[str]: + val = await self.binary_interpreter(int(axis), "MF", 0, CmdType.ValQuery) + if val == 0: + return None + assert isinstance(val, int) + + faults: list[str] = [] + + # Simple bit flags + bit_msgs = { + 0x0001: "Motor Hall sensor feedback angle not found yet.", + 0x0004: "Feedback loss: no match between encoder and Hall location.", + 0x0008: "The peak current has been exceeded.", + 0x0010: "Inhibit.", + 0x0040: "Two digital Hall sensors were changed at the same time.", + 0x0080: "Speed tracking error.", + 0x0100: "Position tracking error.", + 0x0200: "Inconsistent drive database.", + 0x0400: "Too large a difference in ECAM table.", + 0x0800: "CAN heartbeat failure.", + 0x1000: "Servo drive fault.", + 0x010000: "Failed to find the electrical zero of the motor during startup.", + 0x020000: "Speed limit exceeded.", + 0x040000: "Drive CPU stack overflow.", + 0x080000: "Drive CPU exception.", + 0x200000: "Motor stuck.", + 0x400000: "Position limit exceeded.", + 0x20000000: "Cannot start motor.", + } + + for bit, msg in bit_msgs.items(): + if val & bit: + faults.append(msg) + + # 0x2000/0x4000/0x8000 triad + b13 = bool(val & 0x2000) + b14 = bool(val & 0x4000) + b15 = bool(val & 0x8000) + + if (not b15) and (not b14) and b13: + faults.append("Power supply under voltage.") + if (not b15) and b14 and (not b13): + faults.append("Power supply over voltage.") + if b15 and (not b14) and b13: + faults.append("Motor lead short circuit or faulty drive.") + if b15 and b14 and (not b13): + faults.append("Drive overheated.") + + if len(faults) == 0: + return f"Unknown fault code: {val} (0x{val:08X})" + return " ".join(faults) + + async def control_word_set(self, node_id: int, value: int, sync: bool = True) -> None: + val_bytes = value.to_bytes(2, byteorder="little") + await self.can_write(COBType.RPDO1, node_id, val_bytes[0], val_bytes[1], data_length=2) + if sync: + await self.can_sync() + + async def _wait_for_moves_done(self, timeout: float) -> None: + async def wait_for_move_done(axis: KX2Axis) -> None: + try: + await asyncio.wait_for(self._waiting_moves[axis], timeout=timeout) + except asyncio.TimeoutError: + pass + + # if not set on time, make a query + await self.motor_check_if_move_done(axis) + + await asyncio.gather(*(wait_for_move_done(axis) for axis in self._waiting_moves.keys())) + + async def _motors_move_start( + self, + axes: List[KX2Axis], + *, + relative: bool = False, + ) -> None: + # Create futures to wait on + self._waiting_moves = {ax: asyncio.get_event_loop().create_future() for ax in axes} + + # send control word 47 to group; SYNC only on last + relative_bit = 0x40 if relative else 0 + for i, nid in enumerate(axes): + last = i == (len(axes) - 1) + await self.control_word_set(nid, 47 + relative_bit, sync=last) + + # send control word 63 to group; SYNC only on last + for i, nid in enumerate(axes): + last = i == (len(axes) - 1) + await self.control_word_set(nid, 47 + 0x10 + relative_bit, sync=last) + + async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: + await self.pvt_select_mode(False) + + # Send per-axis parameters + for move in plan.moves: + await self.motor_set_move_direction(move.axis.value, move.direction) + + await self.can_sdo_download_elmo_object( + node_id=move.axis.value, + elmo_object_int=24698, + sub_index=0, + data=str(int(move.position)), + data_type=ElmoObjectDataType.INTEGER32, + ) + + await self.can_sdo_download_elmo_object( + node_id=move.axis.value, + elmo_object_int=24705, + sub_index=0, + data=str(int(move.velocity)), + data_type=ElmoObjectDataType.UNSIGNED32, + ) + + acc = max(int(move.acceleration), 100) + await self.can_sdo_download_elmo_object( + node_id=move.axis.value, + elmo_object_int=24707, + sub_index=0, + data=str(acc), + data_type=ElmoObjectDataType.UNSIGNED32, + ) + await self.can_sdo_download_elmo_object( + node_id=move.axis.value, + elmo_object_int=24708, + sub_index=0, + data=str(acc), + data_type=ElmoObjectDataType.UNSIGNED32, + ) + + await self._motors_move_start([move.axis for move in plan.moves]) + await self._wait_for_moves_done(timeout=plan.move_time + 2) + + +class KX2Backend: + MOTION_AXES = (KX2Axis.SHOULDER, KX2Axis.Z, KX2Axis.ELBOW, KX2Axis.WRIST) + + def __init__(self): + self.can = KX2Can() + + self.digital_input_assignment = {} # TODO: just cache? + self.AnalogInputAssignment = {} + self.output_assignment = {} + self.motor_conversion_factor_ax = {} + self.max_travel_ax = {} + self.min_travel_ax = {} + self.unlimited_travel_ax = {} + self.absolute_encoder_ax = {} + self.max_vel_ax = {} + self.max_accel_ax = {} + + self.g_joint_move_direction = { + 1: JointMoveDirection.Normal, + 2: JointMoveDirection.Normal, + 3: JointMoveDirection.Normal, + 4: JointMoveDirection.Normal, + 6: JointMoveDirection.Normal, + } + + self.node_id_list = [1, 2, 3, 4, 6] + + async def initialize(self): + await self.can.connect() # just to get the node IDs + await self.drive_get_parameters(self.can.node_id_list) + await self.can.connect_part_two() + + await asyncio.sleep(2) + + for axis in KX2Backend.MOTION_AXES: + if self.unlimited_travel_ax[axis]: + self.g_joint_move_direction[axis] = JointMoveDirection.ShortestWay + + for axis in KX2Backend.MOTION_AXES: + try: + await self.can.motor_enable(axis=axis, state=True) + except Exception as e: + print(f"Error enabling motor on axis {axis}: {e}") + + await self.servo_gripper_initialize() + + async def servo_gripper_initialize(self): + try: + await self.can.motor_enable(axis=KX2Axis.SERVO_GRIPPER, state=True) + except Exception as e: + print(f"Error enabling servo gripper motor on node {KX2Axis.SERVO_GRIPPER}: {e}") + + await self.servo_gripper_home() + + await self.servo_gripper_close() + + async def servo_gripper_home(self) -> None: + await self.motor_send_command( + node_id=int(KX2Axis.SERVO_GRIPPER), + motor_command="PL", + index=1, + value=str(self.servo_gripper_peak_current), + val_type=ValType.Float, + ) + + await self.motor_send_command( + node_id=int(KX2Axis.SERVO_GRIPPER), + motor_command="CL", + index=1, + value=str(self.servo_gripper_continuous_current), + val_type=ValType.Float, + ) + + await self.can.home_motor( + axis=KX2Axis.SERVO_GRIPPER, + hs_offset=self.servo_gripper_home_hard_stop_offset, + ind_offset=self.servo_gripper_home_index_offset, + home_pos=self.servo_gripper_home_pos, + srch_vel=self.servo_gripper_home_search_vel, + srch_acc=self.servo_gripper_home_search_accel, + max_pe=self.servo_gripper_home_default_position_error, + hs_pe=self.servo_gripper_home_hard_stop_position_error, + offset_vel=self.servo_gripper_home_offset_vel, + offset_acc=self.servo_gripper_home_offset_accel, + timeout=self.servo_gripper_home_timeout_msec / 1000, + ) + + await self.servo_gripper_set_default_gripping_force(100) + + async def servo_gripper_set_default_gripping_force(self, max_force_percent: int) -> None: + if max_force_percent < 10: + max_force_percent = 10 + elif max_force_percent > 100: + max_force_percent = 100 + + cont_current = float(self.servo_gripper_continuous_current) * max_force_percent / 100.0 + peak_current = float(self.servo_gripper_peak_current) * max_force_percent / 100.0 + + axis = KX2Axis.SERVO_GRIPPER + + # 1) PL with unscaled peak current + await self.motor_send_command( + axis, "PL", 1, str(self.servo_gripper_peak_current), val_type=ValType.Float + ) + + # 2) CL with scaled continuous current + await self.motor_send_command(axis, "CL", 1, str(cont_current), val_type=ValType.Float) + + # 3) PL with scaled peak current + await self.motor_send_command(axis, "PL", 1, str(peak_current), val_type=ValType.Float) + + self.servo_gripper_force_percent = max_force_percent + + async def get_servo_gripper_max_force(self) -> float: + """Return current gripping force as percentage of max (0-1).""" + cl = await self.motor_send_command( + node_id=KX2Axis.SERVO_GRIPPER, + motor_command="CL", + index=1, + ) + + iq = await self.motor_send_command( + node_id=KX2Axis.SERVO_GRIPPER, + motor_command="IQ", + index=0, + ) + + if cl == 0: + return 0 + + return max(0, min(abs(iq / cl), 1)) + + async def check_plate_gripped(self, num_attempts: int = 5) -> None: + for _ in range(num_attempts): + motor_status = await self.motor_send_command( + node_id=KX2Axis.SERVO_GRIPPER, + motor_command="MS", + index=1, + ) + print(f"Servo Gripper Motor Status: {motor_status}") + + if motor_status in {0, 1}: + max_force_percentage = await self.get_servo_gripper_max_force() + if max_force_percentage > 90: + return + await asyncio.sleep(0.5) + max_force_percentage = await self.get_servo_gripper_max_force() + if max_force_percentage > 90: + return + + current_position = await self.motor_get_current_position(KX2Axis.SERVO_GRIPPER) + closed_position = 1 + if abs(current_position - closed_position) < 2.0 / 625: + raise RuntimeError( + "Servo Gripper was able to move all the way to the closed position, which indicates the absence of an object in the gripper. The closed position value may need to be decreased." + ) + + return + + elif motor_status == 2: + motor_fault = self.can.motor_get_fault(KX2Axis.SERVO_GRIPPER) + if motor_fault is None: + raise RuntimeError("Error querying whether plate is gripped. Error querying motor fault.") + raise RuntimeError( + f"Servo Gripper may not have gripped the plate correctly. Motor fault: '{motor_fault}'" + ) + + asyncio.sleep(0.05) + + raise RuntimeError( + f"Servo Gripper was unable to confirm that the plate is gripped after {num_attempts} attempts." + ) + + async def servo_gripper_close(self, closed_position: int = 0, check_plate_gripped=True) -> None: + await self.motors_move_joint( + {KX2Axis.SERVO_GRIPPER: closed_position}, + cmd_vel_pct=100, + cmd_accel_pct=100, + ) + + if check_plate_gripped: + await self.check_plate_gripped() + + async def servo_gripper_open(self, open_position: float) -> None: + await self.motors_move_joint( + {KX2Axis.SERVO_GRIPPER: open_position}, + cmd_vel_pct=100, + cmd_accel_pct=100, + ) + + async def drive_set_move_count_parameters( + self, + move_count: int, + travel: List[float], + last_maintenance_performed: float, + maintenance_required: bool, + last_maintenance_performed_date: int, + last_maintenance_performed_rail: float, + maintenance_required_rail: bool, + last_maintenance_performed_date_rail: int, + ) -> None: + # MoveCount -> Z axis, UI index 22 + await self.motor_send_command( + node_id=KX2Axis.Z, + motor_command="UI", + index=22, + value=str(int(move_count)), + val_type=ValType.Int, + low_priority=False, + ) + + # Travel[] -> each node, UF index 5 + # The source looked 1-based for Travel and 0-based for NodeIDList; handle both cleanly. + if len(travel) == len(self.node_id_list) + 1: + pairs = zip(self.node_id_list, travel[1:]) + else: + pairs = zip(self.node_id_list, travel) + + for node_id, dist in pairs: + await self.motor_send_command( + node_id=int(node_id), + motor_command="UF", + index=5, + value=str(float(dist)), + val_type=ValType.Float, + low_priority=True, + ) + + # LastMaintenancePerformed -> Z axis, UF index 6 + await self.motor_send_command( + node_id=KX2Axis.Z, + motor_command="UF", + index=6, + value=str(float(last_maintenance_performed)), + val_type=ValType.Float, + low_priority=True, + ) + + # MaintenanceRequired -> Z axis, UI index 23 + await self.motor_send_command( + node_id=KX2Axis.Z, + motor_command="UI", + index=23, + value="1" if maintenance_required else "0", + val_type=ValType.Int, + low_priority=False, + ) + + # LastMaintenancePerformedDate -> Z axis, UI index 21 + await self.motor_send_command( + node_id=KX2Axis.Z, + motor_command="UI", + index=21, + value=str(int(last_maintenance_performed_date)), + val_type=ValType.Int, + low_priority=False, + ) + + # Rail (if present) + if self.robot_on_rail: + await self.motor_send_command( + node_id=KX2Axis.RAIL, + motor_command="UF", + index=6, + value=str(float(last_maintenance_performed_rail)), + val_type=ValType.Float, + low_priority=True, + ) + + await self.motor_send_command( + node_id=KX2Axis.RAIL, + motor_command="UI", + index=23, + value="1" if maintenance_required_rail else "0", + val_type=ValType.Int, + low_priority=False, + ) + + await self.motor_send_command( + node_id=KX2Axis.RAIL, + motor_command="UI", + index=21, + value=str(int(last_maintenance_performed_date_rail)), + val_type=ValType.Int, + low_priority=False, + ) + + async def drive_get_parameters(self, node_ids) -> None: # TODO: list[KX2Axis] + self.robot_on_rail = False + self.has_servo_gripper = False + + nodes = ( + [int(b) for b in node_ids] + if isinstance(node_ids, (bytes, bytearray)) + else [int(x) for x in node_ids] + ) + + def set2d(store: dict, axis: int, ch: int, value: str) -> None: + store.setdefault(axis, {})[ch] = value + + # Pass 1: identify axes by UI[4] + uis = { + node + for node in nodes + if node == await self.motor_send_command(node, "UI", 4, val_type=ValType.Int) + for node in nodes + } + for required_axis in KX2Backend.MOTION_AXES: + if required_axis.value not in uis: + raise CanError(f"Missing required axis with UI[4]={required_axis}") + if 5 in uis: + self.robot_on_rail = True + warnings.warn("Rails has not been tested for KX2 robots.") + if 6 in uis: + self.has_servo_gripper = True + + # Pass 2: per-axis parameters + for axis in nodes: + print() + print("axis", axis) + + # UI[5..10] digital inputs + for ui_idx in range(5, 11): + ret = await self.motor_send_command(axis, "UI", ui_idx, val_type=ValType.Int) + ch = (ui_idx - 5) + 1 + if ret == 101: + set2d(self.digital_input_assignment, axis, ch, "GripperSensor") + self.GripperSensorAxis = axis + self.GripperSensorInput = ch + elif ret == 102: + set2d(self.digital_input_assignment, axis, ch, "TeachButton") + self.TeachButtonAxis = axis + self.TeachButtonInput = ch + else: + set2d( + self.digital_input_assignment, + axis, + ch, + "" if (not _is_number(ret) or _to_float(ret) <= 0.0) else f"AuxPin{ret}", + ) + + # UI[11..12] analog inputs + for ui_idx in range(11, 13): + ret = await self.motor_send_command(axis, "UI", ui_idx, val_type=ValType.Int) + ch = (ui_idx - 11) + 1 + set2d( + self.AnalogInputAssignment, + axis, + ch, + "" if (not _is_number(ret) or _to_float(ret) <= 0.0) else f"AuxPin{ret}", + ) + + # UI[13..16] outputs + for ui_idx in range(13, 17): + ret = await self.motor_send_command(axis, "UI", ui_idx, val_type=ValType.Int) + ch = (ui_idx - 13) + 1 + + if ret == 101: + set2d(self.output_assignment, axis, ch, "IndicatorLightRed") + self.IndicatorLightRedAxis = axis + self.IndicatorLightRedOutput = ch + elif ret == 102: + set2d(self.output_assignment, axis, ch, "IndicatorLightGreen") + self.IndicatorLightGreenAxis = axis + self.IndicatorLightGreenOutput = ch + elif ret == 103: + set2d(self.output_assignment, axis, ch, "IndicatorLightBlue") + self.IndicatorLightBlueAxis = axis + self.IndicatorLightBlueOutput = ch + elif ret == 104: + set2d(self.output_assignment, axis, ch, "IndicatorLight") + self.IndicatorLightAxis = axis + self.IndicatorLightOutput = ch + elif ret == 105: + set2d(self.output_assignment, axis, ch, "Buzzer") + self.BuzzerAxis = axis + self.BuzzerOutput = ch + else: + set2d( + self.output_assignment, + axis, + ch, + "" if (not _is_number(ret) or _to_float(ret) <= 0.0) else f"AuxPin{ret}", + ) + + # UI[24] drive serial number + ret = await self.motor_send_command(axis, "UI", 24, val_type=ValType.Int) + if _is_number(ret): + # self.drive_serial_number[axis] = int(ret) + pass + + # UF[1], UF[2] conversion factor + uf1 = await self.motor_send_command(axis, "UF", 1, val_type=ValType.Float) + uf2 = await self.motor_send_command(axis, "UF", 2, val_type=ValType.Float) + if ( + not (_is_number(uf1) and _is_number(uf2)) or _to_float(uf1) == 0.0 or _to_float(uf2) == 0.0 + ): + raise CanError(f"Invalid Motor Conversion Factor for axis {axis}. UF[1]={uf1}, UF[2]={uf2}") + self.motor_conversion_factor_ax[axis] = _to_float(uf1) / _to_float(uf2) + + # XM / travel + xm1 = await self.motor_send_command(axis, "XM", 1, val_type=ValType.Int) + xm2 = await self.motor_send_command(axis, "XM", 2, val_type=ValType.Int) + uf3 = await self.motor_send_command(axis, "UF", 3, val_type=ValType.Float) + uf4 = await self.motor_send_command(axis, "UF", 4, val_type=ValType.Float) + vh3 = await self.motor_send_command(axis, "VH", 3, val_type=ValType.Int) + vl3 = await self.motor_send_command(axis, "VL", 3, val_type=ValType.Int) + + self.max_travel_ax[axis] = _to_float(uf3) + self.min_travel_ax[axis] = _to_float(uf4) + + if not all(_is_number(x) for x in (xm1, xm2, vh3, vl3)): + raise CanError( + f"Invalid travel limits or modulo settings for axis {axis}. " + f"VH[3]={vh3}, VL[3]={vl3}, XM[1]={xm1}, XM[2]={xm2}" + ) + + xm1v, xm2v, vh3v, vl3v = map(_to_float, (xm1, xm2, vh3, vl3)) + if ((xm1v == 0.0) and (xm2v == 0.0)) or ((xm1v <= vl3v) and (xm2v >= vh3v)): + self.unlimited_travel_ax[axis] = False + elif (xm1v > vl3v) and (xm2v < vh3v): + self.unlimited_travel_ax[axis] = True + else: + raise CanError( + f"Invalid travel limits or modulo settings for axis {axis}. " + f"VH[3]={vh3}, VL[3]={vl3}, XM[1]={xm1}, XM[2]={xm2}" + ) + + # Encoder socket/type + ca45 = await self.motor_send_command(axis, "CA", 45, val_type=ValType.Int) + ca45v = _to_float(ca45, 0.0) + if (not _is_number(ca45)) or not (0.0 < ca45v <= 4.0): + raise CanError(f"Invalid encoder socket number received from axis {axis}. CA[45]={ca45}") + + enc_type = await self.motor_send_command( + axis, "CA", int(round(40.0 + ca45v)), val_type=ValType.Int + ) + if enc_type in (1, 2): + self.absolute_encoder_ax[axis] = False + elif enc_type == 24: + self.absolute_encoder_ax[axis] = True + else: + raise CanError( + f"Unsupported encoder type specified for axis {axis}. CA[4{ca45}]={enc_type}" + ) + + ca46 = await self.motor_send_command(axis, "CA", 46, val_type=ValType.Int) + if ca45 == ca46: + num3 = 1.0 + else: + ff3 = await self.motor_send_command(axis, "FF", 3, val_type=ValType.Float) + num3 = _to_float(ff3, 1.0) + + denom = self.motor_conversion_factor_ax[axis] * num3 # or 1.0 + + sp2 = await self.motor_send_command(axis, "SP", 2, val_type=ValType.Int) + if sp2 == 100000: + vh2 = await self.motor_send_command(axis, "VH", 2, val_type=ValType.Int) + self.max_vel_ax[axis] = _to_float(vh2) / 1.01 / denom + else: + self.max_vel_ax[axis] = _to_float(sp2) / denom + + sd0 = await self.motor_send_command(axis, "SD", 0, val_type=ValType.Int) + self.max_accel_ax[axis] = _to_float(sd0) / 1.01 / denom + + # Robot-level params from shoulder_ax + shoulder = KX2Axis.SHOULDER + + self.base_to_gripper_clearance_z = _to_float( + await self.motor_send_command(shoulder, "UF", 6, val_type=ValType.Float) + ) + self.base_to_gripper_clearance_arm = _to_float( + await self.motor_send_command(shoulder, "UF", 7, val_type=ValType.Float) + ) + self.wrist_offset = _to_float( + await self.motor_send_command(shoulder, "UF", 8, val_type=ValType.Float) + ) + self.elbow_offset = _to_float( + await self.motor_send_command(shoulder, "UF", 9, val_type=ValType.Float) + ) + self.elbow_zero_offset = _to_float( + await self.motor_send_command(shoulder, "UF", 10, val_type=ValType.Float) + ) + self.MaxLinearVelMMPerSec = _to_float( + await self.motor_send_command(shoulder, "UF", 11, val_type=ValType.Float) + ) + self.MaxLinearAccelMMPerSec2 = _to_float( + await self.motor_send_command(shoulder, "UF", 12, val_type=ValType.Float) + ) + self.MaxLinearJerkMMPerSec3 = _to_float( + await self.motor_send_command(shoulder, "UF", 13, val_type=ValType.Float) + ) + self.MaxRotaryVelDegPerSec = _to_float( + await self.motor_send_command(shoulder, "UF", 14, val_type=ValType.Float) + ) + self.MaxRotaryAccelDegPerSec2 = _to_float( + await self.motor_send_command(shoulder, "UF", 15, val_type=ValType.Float) + ) + + ui17 = await self.motor_send_command(shoulder, "UI", 17, val_type=ValType.Int) + self.pvt_time_interval_msec = ( + 25 + if (not _is_number(ui17) or _to_float(ui17) <= 0.0 or _to_float(ui17) > 255.0) + else int(_to_float(ui17)) + ) + + # Servo gripper params (only if present) + sg = KX2Axis.SERVO_GRIPPER + self.servo_gripper_home_pos = int( + await self.motor_send_command(sg, "UF", 6, val_type=ValType.Float) + ) + self.servo_gripper_home_search_vel = int( + await self.motor_send_command(sg, "UF", 7, val_type=ValType.Float) + ) + self.servo_gripper_home_search_accel = int( + await self.motor_send_command(sg, "UF", 8, val_type=ValType.Float) + ) + self.servo_gripper_home_default_position_error = int( + await self.motor_send_command(sg, "UF", 9, val_type=ValType.Float) + ) + self.servo_gripper_home_hard_stop_position_error = int( + await self.motor_send_command(sg, "UF", 10, val_type=ValType.Float) + ) + self.servo_gripper_home_hard_stop_offset = int( + await self.motor_send_command(sg, "UF", 11, val_type=ValType.Float) + ) + self.servo_gripper_home_index_offset = int( + await self.motor_send_command(sg, "UF", 12, val_type=ValType.Float) + ) + self.servo_gripper_home_offset_vel = int( + await self.motor_send_command(sg, "UF", 13, val_type=ValType.Float) + ) + self.servo_gripper_home_offset_accel = int( + await self.motor_send_command(sg, "UF", 14, val_type=ValType.Float) + ) + self.servo_gripper_home_timeout_msec = int( + await self.motor_send_command(sg, "UF", 15, val_type=ValType.Float) + ) + self.servo_gripper_continuous_current = _to_float( + await self.motor_send_command(sg, "UF", 16, val_type=ValType.Float) + ) + self.servo_gripper_peak_current = _to_float( + await self.motor_send_command(sg, "UF", 17, val_type=ValType.Float) + ) + + async def get_estop_state(self) -> bool: + """Return True if in estop, False otherwise.""" + r = await self.motor_send_command( + node_id=KX2Axis.SHOULDER, + motor_command="SR", + index=1, + value="", + ) + r = int(r) + num2 = not (r & 0x4000 == 0x4000) + num3 = not (r & 0x8000 == 0x8000) + if not r == 8438016: + print("!!! not the same") + return num2 == False and num3 == False + + async def motor_send_command( + self, + node_id: int, + motor_command: str, + index: int, + value: str = "", + val_type: ValType = ValType.Int, + *, + low_priority: bool = False, + ) -> str: + if isinstance(node_id, KX2Axis): + print("node_id not int:", node_id, type(node_id)) + node_id = int(node_id) + print( + "motor send command", + node_id, + motor_command, + index, + value, + val_type == ValType.Float, + f"({val_type})", + ) + + cmd_u = motor_command.upper() + OS_CMDS = {"VR", "CD", "LS", "DL", "DF", "BH"} + + has_xc = "XC##" in cmd_u + has_xq = "XQ##" in cmd_u + use_os = (cmd_u in OS_CMDS) or has_xc or has_xq + + returned_data = "" + + cmd_u = motor_command.upper() + + NO_QUERY_CMDS = { + "BG", + "CP", + "EI", + "EO", + "HP", + "HX", + "KL", + "KR", + "LD", + "MI", + "PB", + "RS", + "SV", + "XC##", + } + + if value == "": + if (cmd_u in NO_QUERY_CMDS) or ("XQ##" in cmd_u): + cmd_type = CmdType.Execute + else: + cmd_type = CmdType.ValQuery + else: + cmd_type = CmdType.ValSet + + if use_os: + query_flag = not (has_xc or has_xq) + + # OSInterpreter writes into `str` and can also write a long; you can ignore the long if you don't use it. + s = await self.can.os_interpreter( + node_id=(node_id), + cmd=motor_command, + query=query_flag, + ) + + if cmd_type == CmdType.ValQuery: + returned_data = s if s is not None else "" + else: + s = await self.can.binary_interpreter( + node_id=(node_id), + cmd=motor_command, + cmd_index=int(index), + cmd_type=cmd_type, + value=value, + val_type=val_type, + low_priority=low_priority, + ) + + if cmd_type == CmdType.ValQuery: + returned_data = s + + return returned_data + + def convert_elbow_position_to_angle(self, elbow_pos: float) -> float: + max_travel = self.max_travel_ax[KX2Axis.ELBOW] + denom = max_travel + self.elbow_zero_offset + + if elbow_pos > max_travel: + x = (2.0 * max_travel - elbow_pos + self.elbow_zero_offset) / denom + angle = math.asin(x) * (180.0 / math.pi) + elbow_angle = 90.0 + angle + else: + x = (elbow_pos + self.elbow_zero_offset) / denom + angle = math.asin(x) * (180.0 / math.pi) + elbow_angle = angle + + return elbow_angle + + def convert_elbow_angle_to_position(self, elbow_angle_deg: float) -> float: + elbow_pos = (self.max_travel_ax[KX2Axis.ELBOW] + self.elbow_zero_offset) * math.sin( + elbow_angle_deg * (math.pi / 180.0) + ) - self.elbow_zero_offset + + if elbow_angle_deg > 90.0: + elbow_pos = 2.0 * self.max_travel_ax[KX2Axis.ELBOW] - elbow_pos + + return elbow_pos + + async def motor_get_current_position(self, axis: KX2Axis) -> float: + raw = await self.can.motor_get_current_position(node_id=axis, pu=self.unlimited_travel_ax[axis]) + c = self.motor_conversion_factor_ax[axis] + if axis == KX2Axis.ELBOW: + return self.convert_elbow_angle_to_position(elbow_angle_deg=raw / c) + else: + if c == 0: + print("node", axis, "has conversion factor of 0") + return 0 + else: + return raw / c + + async def read_input(self, axis: int, input_num: int) -> bool: + return await self.can.read_input(node_id=axis, input_num=0x10 + input_num) + + @staticmethod + def _wrap_to_range(x: float, lo: float, hi: float) -> float: + span = hi - lo + if span == 0: + return lo + k = math.trunc(x / span) + x = x - k * span + if x < lo: + x += span + if x == hi: + x -= span + return x + + @staticmethod + def _profile(dist: float, v: float, a: float) -> tuple[float, float, float]: + """ + Returns (v, a, t_total) after applying triangular fallback if needed. + If the distance is short, you cannot reach v before you must decelerate. + """ + if dist <= 0: + return v, a, 0.0 + if a <= 0: + # degenerate; avoid crash + return max(v, 1e-9), 1e-9, dist / max(v, 1e-9) + + t_acc = v / a + d_acc = 0.5 * a * t_acc * t_acc + + # triangular? + if 2.0 * d_acc > dist: + d_acc = dist / 2.0 + t_acc = math.sqrt(2.0 * d_acc / a) + v = a * t_acc + t_total = 2.0 * t_acc + return v, a, t_total + + d_cruise = dist - 2.0 * d_acc + t_cruise = d_cruise / max(v, 1e-9) + t_total = t_cruise + 2.0 * t_acc + return v, a, t_total + + async def calculate_move_abs_all_axes( + self, + cmd_pos: Dict["KX2Axis", float], + cmd_vel_pct: float, + cmd_accel_pct: float, + ) -> Optional[MotorsMovePlan]: + target = cmd_pos.copy() + axes = list(target.keys()) + + enc_pos: Dict[KX2Axis, float] = {} + enc_vel: Dict[KX2Axis, float] = {} + enc_accel: Dict[KX2Axis, float] = {} + # enc_move_dist: Dict[KX2Axis, float] = {} + skip_ax: Dict[KX2Axis, bool] = {} + + # input validation / travel limits / done-wait logic + if cmd_vel_pct <= 0.0 or cmd_vel_pct > 100.0: + raise ValueError("CmdVel out of range") + if cmd_accel_pct <= 0.0 or cmd_accel_pct > 100.0: + raise ValueError("CmdAccel out of range") + + # Convert elbow cmd from position->angle for planning math + if KX2Axis.ELBOW in axes: + target[KX2Axis.ELBOW] = self.convert_elbow_position_to_angle(target[KX2Axis.ELBOW]) + + # Ensure per-axis ready and clamp travel limits like + for ax in axes: + if self.unlimited_travel_ax[ax]: + continue + high = self.max_travel_ax[ax] + low = self.min_travel_ax[ax] + if target[ax] > high: + if (target[ax] - high) < 0.1: + target[ax] = high + else: + raise ValueError(f"Axis {ax!r} above max travel") + if target[ax] < low: + if (low - target[ax]) < 0.1: + target[ax] = low + else: + raise ValueError(f"Axis {ax!r} below min travel") + + # Clearance check + if KX2Axis.Z in axes: + if ( + target[KX2Axis.Z] < self.min_travel_ax[KX2Axis.Z] + self.base_to_gripper_clearance_z + and target[KX2Axis.ELBOW] < self.base_to_gripper_clearance_arm + ): + raise ValueError("Base-to-gripper clearance violated") + + # Determine current/start positions + curr = await self.get_joint_position() + + # Elbow: convert both target and start to angle for distance math + if KX2Axis.ELBOW in curr: + curr[KX2Axis.ELBOW] = self.convert_elbow_position_to_angle(curr[KX2Axis.ELBOW]) + + # Handle unlimited travel normalization when direction != NORMAL + for ax in axes: + if ( + self.unlimited_travel_ax[ax] + and self.g_joint_move_direction[ax] != JointMoveDirection.Normal + ): + target[ax] = self._wrap_to_range(target[ax], self.min_travel_ax[ax], self.max_travel_ax[ax]) + + # Distances, skip flags, initial v/a per axis + dist: Dict[KX2Axis, float] = {} + v: Dict[KX2Axis, float] = {} + a: Dict[KX2Axis, float] = {} + accel_time: Dict[KX2Axis, float] = {} + total_time: Dict[KX2Axis, float] = {} + + for ax in axes: + if self.unlimited_travel_ax[ax]: + d = target[ax] - curr[ax] + span = self.max_travel_ax[ax] - self.min_travel_ax[ax] + dir_ = self.g_joint_move_direction[ax] + + if dir_ == JointMoveDirection.Clockwise and d > 0.01: + d -= span + elif dir_ == JointMoveDirection.Counterclockwise and d < -0.01: + d += span + elif dir_ == JointMoveDirection.ShortestWay: + if d > 180.0: + d -= span + elif d < -180.0: + d += span + + dist[ax] = abs(d) + else: + dist[ax] = abs(target[ax] - curr[ax]) + + skip_ax[ax] = abs(dist[ax]) < 0.01 + + v[ax] = (cmd_vel_pct / 100.0) * self.max_vel_ax[ax] + a[ax] = (cmd_accel_pct / 100.0) * self.max_accel_ax[ax] + + if not skip_ax[ax] and a[ax] > 0: + accel_time[ax] = v[ax] / a[ax] + v[ax], a[ax], total_time[ax] = self._profile(dist[ax], v[ax], a[ax]) + accel_time[ax] = v[ax] / max(a[ax], 1e-9) + else: + total_time[ax] = 0.0 + accel_time[ax] = 0.0 + + if all(skip_ax[ax] for ax in axes): + return None # nothing to do + + # Pick axis with max accel_time among non-skipped; match others to that accel_time + lead_acc_ax = max( + (ax for ax in axes if not skip_ax[ax]), + key=lambda ax: accel_time[ax], + ) + lead_acc_t = accel_time[lead_acc_ax] + + for ax in axes: + if ax == lead_acc_ax or skip_ax[ax]: + continue + if accel_time[ax] > lead_acc_t: + v[ax] = lead_acc_t * a[ax] + elif accel_time[ax] < lead_acc_t: + a[ax] = v[ax] / max(lead_acc_t, 1e-9) + + # Recompute times after accel sync + for ax in axes: + if skip_ax[ax]: + total_time[ax] = 0.0 + continue + v[ax], a[ax], total_time[ax] = self._profile(dist[ax], v[ax], a[ax]) + + # Pick axis with max total_time; scale others to match its total_time + lead_time_ax = max(axes, key=lambda ax: total_time[ax]) + lead_T = total_time[lead_time_ax] + + for ax in axes: + if ax == lead_time_ax or skip_ax[ax]: + continue + denom = v[ax] * (lead_T - (v[ax] / max(a[ax], 1e-9))) + if abs(denom) < 1e-12: + continue + k = dist[ax] / denom + v[ax] *= k + a[ax] *= k + + # Final recompute and final move time + for ax in axes: + if skip_ax[ax]: + total_time[ax] = 0.0 + continue + v[ax], a[ax], total_time[ax] = self._profile(dist[ax], v[ax], a[ax]) + + move_time = max(total_time[ax] for ax in axes) + + # Convert back to encoder units (and elbow back to "position" space) + # keep target in angle-space for elbow during math, then later use conversion factor. + for ax in axes: + conv = self.motor_conversion_factor_ax[ax] + enc_pos[ax] = target[ax] * conv + # enc_move_dist[ax] = dist[ax] * conv + + if skip_ax[ax]: + enc_vel[ax] = 1000.0 + enc_accel[ax] = 1000.0 + else: + enc_vel[ax] = max(v[ax] * abs(conv), 1.0) + enc_accel[ax] = max(a[ax] * abs(conv), 1.0) + + return MotorsMovePlan( + moves=[ + MotorMoveParam( + axis=ax, + position=int(round(enc_pos[ax])), + velocity=int(round(enc_vel[ax])), + acceleration=int(round(enc_accel[ax])), + direction=self.g_joint_move_direction[ax], + ) + for ax in axes + ], + move_time=move_time, + ) + + async def motors_move_joint( + self, + cmd_pos: Dict["KX2Axis", float], + cmd_vel_pct: float, + cmd_accel_pct: float, + ) -> None: + plan = await self.calculate_move_abs_all_axes( + cmd_pos=cmd_pos, + cmd_vel_pct=cmd_vel_pct, + cmd_accel_pct=cmd_accel_pct, + ) + + if plan is None: # if every axis is skipped, exit + return + + await self.can.motors_move_absolute_execute(plan) + + def convert_cartesian_to_joint_position(self, pose: GripperPose) -> Dict["KX2Axis", float]: + if pose.rotation.x != 0 or pose.rotation.y != 0: + raise ValueError("Only Z rotation is supported for KX2") + + joint_position: Dict[KX2Axis, float] = {} + + x, y = (pose.location.x), (pose.location.y) + + # Shoulder axis + shoulder = -math.degrees(math.atan2(x, y)) + if abs(shoulder + 180.0) < 1e-12: + shoulder = 180.0 + + joint_position[KX2Axis.SHOULDER] = shoulder + + # Z axis + joint_position[KX2Axis.Z] = pose.location.z + + # Elbow axis + elbow = ( + math.sqrt(x * x + y * y) - self.wrist_offset - self.elbow_offset - self.elbow_zero_offset + ) + joint_position[KX2Axis.ELBOW] = elbow + + # Wrist axis + wrist = (pose.rotation.z) - joint_position[KX2Axis.SHOULDER] + joint_position[KX2Axis.WRIST] = wrist + + # Wrap wrist into travel range if possible by +/- 360 + w = joint_position[KX2Axis.WRIST] + wmin = self.min_travel_ax[KX2Axis.WRIST] + wmax = self.max_travel_ax[KX2Axis.WRIST] + if (w < wmin - 0.001) and (w + 360.0 <= wmax): + w += 360.0 + elif (w > wmax + 0.001) and (w - 360.0 >= wmin): + w -= 360.0 + joint_position[KX2Axis.WRIST] = w + + return joint_position + + def convert_joint_position_to_cartesian( + self, joint_position: Dict["KX2Axis", float] + ) -> GripperPose: + r = ( + self.wrist_offset + self.elbow_offset + self.elbow_zero_offset + joint_position[KX2Axis.ELBOW] + ) + sh_deg = joint_position[KX2Axis.SHOULDER] + sh = math.radians(sh_deg) + + location = Coordinate( + x=(-(r) * math.sin(sh)), + y=((r) * math.cos(sh)), + z=(joint_position[KX2Axis.Z]), + ) + + rotation_z = joint_position[KX2Axis.WRIST] + sh_deg + + # wrap to [-180, 180] + if rotation_z > 180.0: + rotation_z -= 360.0 + if rotation_z < -180.0: + rotation_z += 360.0 + + return GripperPose( + location=location, + rotation=Rotation(z=rotation_z), + ) + + def convert_joint_position_to_tool_coordinate( + self, + joint_position: Dict[int, float], + ref_frame_rotate: float, + tool_offset: float, + ) -> List[float]: + coordinate = self.convert_joint_position_to_cartesian(joint_position) + tool_coord = [0.0] * (len(joint_position) - 1) + + if tool_offset != 0.0: + ang = math.radians(coordinate[3] + ref_frame_rotate) + dx = -tool_offset * math.sin(ang) + dy = tool_offset * math.cos(ang) + else: + dx = 0.0 + dy = 0.0 + + tool_coord[0] = coordinate[0] + dx + tool_coord[1] = coordinate[1] + dy + tool_coord[2] = coordinate[2] + tool_coord[3] = coordinate[3] + ref_frame_rotate + + if len(coordinate) > 4: + tool_coord[4] = coordinate[4] + + return tool_coord + + def convert_tool_coordinate_to_joint_position( + self, + tool_coordinate: Sequence[float], + ref_frame_rotate: float, + tool_offset: float, + ) -> Dict[int, float]: + coordinate = [0.0] * (len(tool_coordinate) + 1) + + if tool_offset != 0.0: + ang = math.radians(float(tool_coordinate[3]) - ref_frame_rotate) + dx = -tool_offset * math.sin(ang) + dy = tool_offset * math.cos(ang) + else: + dx = 0.0 + dy = 0.0 + + coordinate[0] = float(tool_coordinate[0]) - dx + coordinate[1] = float(tool_coordinate[1]) - dy + coordinate[2] = float(tool_coordinate[2]) + coordinate[3] = float(tool_coordinate[3]) - ref_frame_rotate + + if len(tool_coordinate) >= 5: + coordinate[4] = float(tool_coordinate[4]) + if len(tool_coordinate) >= 6: + coordinate[5] = float(tool_coordinate[5]) + + return self.convert_cartesian_to_joint_position(coordinate) + + async def get_joint_position(self) -> Dict["KX2Axis", float]: + return { + KX2Axis.SHOULDER: await self.motor_get_current_position(KX2Axis.SHOULDER), + KX2Axis.Z: await self.motor_get_current_position(KX2Axis.Z), + KX2Axis.ELBOW: await self.motor_get_current_position(KX2Axis.ELBOW), + KX2Axis.WRIST: await self.motor_get_current_position(KX2Axis.WRIST), + KX2Axis.SERVO_GRIPPER: await self.motor_get_current_position(KX2Axis.SERVO_GRIPPER), + } + + async def get_cartesian_position(self) -> GripperPose: + current_joint_pos = await self.get_joint_position() + cartesian = self.convert_joint_position_to_cartesian(current_joint_pos) + return cartesian + + async def move_to_cartesian_position( + self, + pose: GripperPose, + vel_pct: float = 100, + accel_pct: float = 100, + ) -> None: + joint_pos = self.convert_cartesian_to_joint_position(pose) + await self.motors_move_joint( + cmd_pos=joint_pos, + cmd_vel_pct=vel_pct, + cmd_accel_pct=accel_pct, + ) + + async def activate_free_mode(self) -> None: + for axis in KX2Backend.MOTION_AXES: + await self.can.motor_enable(axis=axis, state=False) + + async def deactivate_free_mode(self) -> None: + for axis in KX2Backend.MOTION_AXES: + await self.can.motor_enable(axis=axis, state=True) diff --git a/pylabrobot/arms/precise_flex/coords.py b/pylabrobot/arms/precise_flex/coords.py index 6d039c0d31a..01f2f0a0ed0 100644 --- a/pylabrobot/arms/precise_flex/coords.py +++ b/pylabrobot/arms/precise_flex/coords.py @@ -2,7 +2,7 @@ from enum import Enum from typing import Optional -from pylabrobot.arms.standard import CartesianCoords +from pylabrobot.arms.standard import GripperPose class ElbowOrientation(Enum): @@ -11,5 +11,5 @@ class ElbowOrientation(Enum): @dataclass -class PreciseFlexCartesianCoords(CartesianCoords): +class PreciseFlexCartesianCoords(GripperPose): orientation: Optional[ElbowOrientation] = None diff --git a/pylabrobot/arms/scara.py b/pylabrobot/arms/scara.py index 8256e34a75b..c7ce92aae17 100644 --- a/pylabrobot/arms/scara.py +++ b/pylabrobot/arms/scara.py @@ -3,7 +3,7 @@ from pylabrobot.arms.backend import AccessPattern, SCARABackend from pylabrobot.arms.precise_flex.coords import PreciseFlexCartesianCoords -from pylabrobot.arms.standard import JointCoords +from pylabrobot.arms.standard import GripperPose, JointCoords from pylabrobot.machines.machine import Machine @@ -28,7 +28,7 @@ async def get_joint_position(self, **backend_kwargs) -> JointCoords: """Get the current position of the arm in joint space.""" return await self.backend.get_joint_position(**backend_kwargs) - async def get_cartesian_position(self, **backend_kwargs) -> PreciseFlexCartesianCoords: + async def get_cartesian_position(self, **backend_kwargs) -> GripperPose: """Get the current position of the arm in 3D space.""" return await self.backend.get_cartesian_position(**backend_kwargs) diff --git a/pylabrobot/arms/standard.py b/pylabrobot/arms/standard.py index a8d702deead..0d13b856e1e 100644 --- a/pylabrobot/arms/standard.py +++ b/pylabrobot/arms/standard.py @@ -7,6 +7,6 @@ @dataclass -class CartesianCoords: +class GripperPose: location: Coordinate rotation: Rotation diff --git a/setup.py b/setup.py index 8fcf9c22876..b977fae1a1d 100644 --- a/setup.py +++ b/setup.py @@ -30,6 +30,8 @@ extras_agrow = ["pymodbus==3.6.8"] +extras_kx2 = ["python-can"] + extras_dev = ( extras_fw + extras_http @@ -40,6 +42,7 @@ + extras_server + extras_inheco + extras_agrow + + extras_kx2 + [ "pydata-sphinx-theme==0.16.1", "myst_nb==1.3.0", @@ -56,7 +59,7 @@ ) # Some extras are not available on all platforms. `dev` should be available everywhere -extras_all = extras_dev +extras_all = extras_dev + extras_kx2 setup( name="PyLabRobot", @@ -78,6 +81,7 @@ "opentrons": extras_opentrons, "server": extras_server, "agrow": extras_agrow, + "kx2": extras_kx2, "dev": extras_dev, "all": extras_all, }, From 18272cc296055bc3ca3c05f5d09a0882f3619bec Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Sat, 7 Feb 2026 22:12:45 -0500 Subject: [PATCH 02/93] tutorial update, connection update --- .../storage/inheco/scila.ipynb | 115 +++++++++++++++--- pylabrobot/arms/kx2/kx2_backend.py | 32 ++++- 2 files changed, 128 insertions(+), 19 deletions(-) diff --git a/docs/user_guide/01_material-handling/storage/inheco/scila.ipynb b/docs/user_guide/01_material-handling/storage/inheco/scila.ipynb index 32707bf4a48..3c5e390b105 100644 --- a/docs/user_guide/01_material-handling/storage/inheco/scila.ipynb +++ b/docs/user_guide/01_material-handling/storage/inheco/scila.ipynb @@ -94,7 +94,7 @@ "data": { "text/plain": [ "{'Drawer1': 'Closed',\n", - " 'Drawer2': 'Opened',\n", + " 'Drawer2': 'Closed',\n", " 'Drawer3': 'Closed',\n", " 'Drawer4': 'Closed'}" ] @@ -220,7 +220,7 @@ { "data": { "text/plain": [ - "24.28" + "19.06" ] }, "execution_count": 11, @@ -234,17 +234,92 @@ }, { "cell_type": "code", - "execution_count": 12, + "execution_count": 15, + "id": "a1958744-006b-4302-a2f4-af03c5764a2d", + "metadata": { + "scrolled": true + }, + { + "data": { + "text/plain": [ + "['DrawerPosition',\n", + " '__abstractmethods__',\n", + " '__annotations__',\n", + " '__class__',\n", + " '__delattr__',\n", + " '__dict__',\n", + " '__dir__',\n", + " '__doc__',\n", + " '__eq__',\n", + " '__format__',\n", + " '__ge__',\n", + " '__getattribute__',\n", + " '__getstate__',\n", + " '__gt__',\n", + " '__hash__',\n", + " '__init__',\n", + " '__init_subclass__',\n", + " '__le__',\n", + " '__lt__',\n", + " '__module__',\n", + " '__ne__',\n", + " '__new__',\n", + " '__reduce__',\n", + " '__reduce_ex__',\n", + " '__repr__',\n", + " '__setattr__',\n", + " '__sizeof__',\n", + " '__slots__',\n", + " '__str__',\n", + " '__subclasshook__',\n", + " '__weakref__',\n", + " '_abc_impl',\n", + " '_instances',\n", + " '_reset_and_initialize',\n", + " '_sila_interface',\n", + " 'close_drawer',\n", + " 'deactivate_temperature_control',\n", + " 'deserialize',\n", + " 'get_all_instances',\n", + " 'get_co2_flow_status',\n", + " 'get_current_temperature',\n", + " 'get_drawer_position',\n", + " 'get_drawer_positions',\n", + " 'get_liquid_level',\n", + " 'get_status',\n", + " 'get_target_temperature',\n", + " 'get_temperature_control_enabled',\n", + " 'get_temperature_information',\n", + " 'get_valve_status',\n", + " 'open_drawer',\n", + " 'serialize',\n", + " 'set_tempeature',\n", + " 'setup',\n", + " 'stop']" + ] + }, + "execution_count": 15, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "dir(scila)" + ] + }, + { + "cell_type": "code", + "execution_count": 17, "id": "cc2f6063", "metadata": {}, "outputs": [], "source": [ - "await scila.set_temperature(37.0)" + "await scila.set_tempeature(37.0)" ] }, { "cell_type": "code", - "execution_count": 13, + "execution_count": 18, "id": "5f04d0ef", "metadata": {}, "outputs": [ @@ -254,7 +329,7 @@ "37.0" ] }, - "execution_count": 13, + "execution_count": 18, "metadata": {}, "output_type": "execute_result" } @@ -265,17 +340,17 @@ }, { "cell_type": "code", - "execution_count": 14, + "execution_count": 19, "id": "02a60f32", "metadata": {}, "outputs": [ { "data": { "text/plain": [ - "24.3" + "23.04" ] }, - "execution_count": 14, + "execution_count": 19, "metadata": {}, "output_type": "execute_result" } @@ -286,7 +361,7 @@ }, { "cell_type": "code", - "execution_count": 15, + "execution_count": 20, "id": "470241e1", "metadata": {}, "outputs": [ @@ -296,7 +371,7 @@ "True" ] }, - "execution_count": 15, + "execution_count": 20, "metadata": {}, "output_type": "execute_result" } @@ -307,7 +382,7 @@ }, { "cell_type": "code", - "execution_count": 16, + "execution_count": 21, "id": "5881c2ce", "metadata": {}, "outputs": [], @@ -317,7 +392,7 @@ }, { "cell_type": "code", - "execution_count": 17, + "execution_count": 22, "id": "ac8ad797", "metadata": {}, "outputs": [ @@ -327,7 +402,7 @@ "False" ] }, - "execution_count": 17, + "execution_count": 22, "metadata": {}, "output_type": "execute_result" } @@ -335,11 +410,19 @@ "source": [ "await scila.get_temperature_control_enabled()" ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "67b2946d-4d5c-4d13-a74c-59eb5e9c9791", + "metadata": {}, + "outputs": [], + "source": [] } ], "metadata": { "kernelspec": { - "display_name": "env", + "display_name": "Python 3 (ipykernel)", "language": "python", "name": "python3" }, @@ -353,7 +436,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.9.24" + "version": "3.12.11" } }, "nbformat": 4, diff --git a/pylabrobot/arms/kx2/kx2_backend.py b/pylabrobot/arms/kx2/kx2_backend.py index c904fff8aa2..a35f513a204 100644 --- a/pylabrobot/arms/kx2/kx2_backend.py +++ b/pylabrobot/arms/kx2/kx2_backend.py @@ -443,6 +443,10 @@ def __init__(self, has_rail: bool = False, has_servo_gripper: bool = True) -> No # Threading flags (for asyncio tasks) self._can_write_task_running = False self._can_read_task_running = True + + # Store task references to prevent garbage collection + self._read_task: Optional[asyncio.Task] = None + self._write_task: Optional[asyncio.Task] = None self.b_pvt_thread_started = False # Can message queues @@ -1071,9 +1075,29 @@ async def connect( self, baud_rate: int = 500000, ) -> None: + # Clean up previous connection if re-connecting + if self._read_task is not None and not self._read_task.done(): + self._can_read_task_running = False + self._read_task.cancel() + try: + await self._read_task + except (asyncio.CancelledError, Exception): + pass + if self._write_task is not None and not self._write_task.done(): + self._can_write_task_running = False + self._write_task.cancel() + try: + await self._write_task + except (asyncio.CancelledError, Exception): + pass + if self._can_device is not None: + self._can_device.shutdown() + self._can_device = None + self.sending_sv_command = False self.move_error_code = 0 self.pvt_time_interval_msec = 0 + self.err_ctrl = [ErrCtrl() for _ in range(8)] # Determine max_node_id for array sizing if dynamic behavior is needed max_node_id = 6 @@ -1095,9 +1119,11 @@ async def connect( is_extended_id=False, ) - # Start asyncio tasks - asyncio.create_task(self._can_read_task()) - asyncio.create_task(self._can_write_task()) + # Start asyncio tasks (store references to prevent GC) + self._can_read_task_running = True + self._can_write_task_running = False + self._read_task = asyncio.create_task(self._can_read_task()) + self._write_task = asyncio.create_task(self._can_write_task()) await asyncio.sleep(0.01) From f3fadad998149564c2c1afd5bec3037507339963 Mon Sep 17 00:00:00 2001 From: Camillo Moschner Date: Sun, 8 Feb 2026 17:49:59 -0500 Subject: [PATCH 03/93] change self.calculate_move_abs_all_axes --- pylabrobot/arms/kx2/kx2_backend.py | 32 ++++++++++++++++-------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/pylabrobot/arms/kx2/kx2_backend.py b/pylabrobot/arms/kx2/kx2_backend.py index a35f513a204..504022c6828 100644 --- a/pylabrobot/arms/kx2/kx2_backend.py +++ b/pylabrobot/arms/kx2/kx2_backend.py @@ -3730,21 +3730,22 @@ async def calculate_move_abs_all_axes( target[KX2Axis.ELBOW] = self.convert_elbow_position_to_angle(target[KX2Axis.ELBOW]) # Ensure per-axis ready and clamp travel limits like - for ax in axes: - if self.unlimited_travel_ax[ax]: - continue - high = self.max_travel_ax[ax] - low = self.min_travel_ax[ax] - if target[ax] > high: - if (target[ax] - high) < 0.1: - target[ax] = high - else: - raise ValueError(f"Axis {ax!r} above max travel") - if target[ax] < low: - if (low - target[ax]) < 0.1: - target[ax] = low - else: - raise ValueError(f"Axis {ax!r} below min travel") + # for ax in axes: + # if self.unlimited_travel_ax[ax]: + # continue + # high = self.max_travel_ax[ax] + # low = self.min_travel_ax[ax] + # print("ax", ax, "target", target[ax], "low", low, "high", high) + # if target[ax] > high: + # if (target[ax] - high) < 0.1: + # target[ax] = high + # else: + # raise ValueError(f"Axis {ax!r} above max travel") + # if target[ax] < low: + # if (low - target[ax]) < 0.1: + # target[ax] = low + # else: + # raise ValueError(f"Axis {ax!r} below min travel") # Clearance check if KX2Axis.Z in axes: @@ -3891,6 +3892,7 @@ async def motors_move_joint( cmd_vel_pct: float, cmd_accel_pct: float, ) -> None: + print("cmd", cmd_pos) plan = await self.calculate_move_abs_all_axes( cmd_pos=cmd_pos, cmd_vel_pct=cmd_vel_pct, From 32d039006c860910230a4fb878f4b7b3043ea384 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 8 Feb 2026 17:59:32 -0500 Subject: [PATCH 04/93] try process message; check if future already done --- pylabrobot/arms/kx2/kx2_backend.py | 55 +++++++++++++++--------------- 1 file changed, 28 insertions(+), 27 deletions(-) diff --git a/pylabrobot/arms/kx2/kx2_backend.py b/pylabrobot/arms/kx2/kx2_backend.py index 504022c6828..2086994fea4 100644 --- a/pylabrobot/arms/kx2/kx2_backend.py +++ b/pylabrobot/arms/kx2/kx2_backend.py @@ -732,7 +732,9 @@ async def _process_tpdo_message(self, node_id: int, message: can.Message, respon if mapped == TPDOMappedObject.StatusWord: if KX2Axis(node_id) in self._waiting_moves: - self._waiting_moves[KX2Axis(node_id)].set_result(None) + fut = self._waiting_moves[KX2Axis(node_id)] + if not fut.done(): + fut.set_result(None) event_data = EventData( event_type=EventType.MotionStatusReceived, @@ -792,7 +794,9 @@ async def _process_tpdo_message(self, node_id: int, message: can.Message, respon ) if edge and logic_enabled and new_bit == 1: - self._waiting_moves[KX2Axis(node_id)].set_result(None) + fut = self._waiting_moves[KX2Axis(node_id)] + if not fut.done(): + fut.set_result(None) print(f"Digital input {index4} enabled motor move done for node {node_id}") self.input_state[node_idx] = num6 @@ -990,31 +994,28 @@ async def _can_read_task( if message is None: # timeout continue - response_type = message.arbitration_id >> 7 - response_type_c = round(message.arbitration_id / 128) - assert ( - response_type == response_type_c - ), f"Response type calculation mismatch: {response_type} != {response_type_c}" - node_id = message.arbitration_id & 0x7F - node_id_c = message.arbitration_id - (response_type * 128) - assert node_id == node_id_c, f"Node index calculation mismatch: {node_id} != {node_id_c}" - - if response_type == 0: - print("NMT message received, ignoring") - elif response_type == 1: - await self._process_emcy_message(node_id=node_id, message=message) - elif response_type in {3, 7, 9}: - await self._process_tpdo_message( - node_id=node_id, message=message, response_type=response_type - ) - elif response_type == 5: - await self._process_binary_interpreter_response(node_id=node_id, message=message) - elif response_type == 11: - await self._process_sdo_response(node_id=node_id, message=message) - elif response_type == 14: - await self._process_motor_drive_restarted(node_id=node_id, message=message) - else: - print(f"Unknown CAN message type received: {response_type}") + try: + response_type = message.arbitration_id >> 7 + node_id = message.arbitration_id & 0x7F + + if response_type == 0: + print("NMT message received, ignoring") + elif response_type == 1: + await self._process_emcy_message(node_id=node_id, message=message) + elif response_type in {3, 7, 9}: + await self._process_tpdo_message( + node_id=node_id, message=message, response_type=response_type + ) + elif response_type == 5: + await self._process_binary_interpreter_response(node_id=node_id, message=message) + elif response_type == 11: + await self._process_sdo_response(node_id=node_id, message=message) + elif response_type == 14: + await self._process_motor_drive_restarted(node_id=node_id, message=message) + else: + print(f"Unknown CAN message type received: {response_type}") + except Exception as e: + print(f"Error processing CAN message (arb_id={message.arbitration_id:#x}): {e}") async def can_write( self, From 290ad1efa5d4a744dcc775b90fcc1501e7affa84 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 14:26:42 -0700 Subject: [PATCH 05/93] remove scila.ipynb accidentally modified in kx2 PR --- .../storage/inheco/scila.ipynb | 444 ------------------ 1 file changed, 444 deletions(-) delete mode 100644 docs/user_guide/01_material-handling/storage/inheco/scila.ipynb diff --git a/docs/user_guide/01_material-handling/storage/inheco/scila.ipynb b/docs/user_guide/01_material-handling/storage/inheco/scila.ipynb deleted file mode 100644 index 3c5e390b105..00000000000 --- a/docs/user_guide/01_material-handling/storage/inheco/scila.ipynb +++ /dev/null @@ -1,444 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "id": "fc3ebf5f", - "metadata": {}, - "source": [ - "# Inheco SCILA storage unit\n", - "\n", - "4 plate incubator" - ] - }, - { - "cell_type": "code", - "execution_count": 1, - "id": "201cd7c5", - "metadata": {}, - "outputs": [], - "source": [ - "%load_ext autoreload\n", - "%autoreload 2" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "id": "474289aa", - "metadata": {}, - "outputs": [], - "source": [ - "from pylabrobot.storage.inheco.scila.soap import soap_encode, soap_decode, XSI\n", - "from pylabrobot.storage.inheco.scila.scila import SCILABackend\n", - "scila = SCILABackend(scila_ip=\"169.254.1.117\")\n", - "await scila.setup()" - ] - }, - { - "cell_type": "markdown", - "id": "da156d46", - "metadata": {}, - "source": [ - "## Status requests" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "id": "d5dc6eda", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "'idle'" - ] - }, - "execution_count": 3, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "await scila.get_status()" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "id": "faaef501", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "'Empty'" - ] - }, - "execution_count": 4, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "await scila.get_liquid_level()" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "id": "5bc58e44", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "{'Drawer1': 'Closed',\n", - " 'Drawer2': 'Closed',\n", - " 'Drawer3': 'Closed',\n", - " 'Drawer4': 'Closed'}" - ] - }, - "execution_count": 5, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "await scila.get_drawer_positions()" - ] - }, - { - "cell_type": "code", - "execution_count": 6, - "id": "6e7b7e2a", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "{'H2O': 'Opened', 'CO2 Normal': 'Opened', 'CO2 Boost': 'Closed'}" - ] - }, - "execution_count": 6, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "await scila.get_valve_status()" - ] - }, - { - "cell_type": "code", - "execution_count": 7, - "id": "0b4dd0ce", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "'NOK'" - ] - }, - "execution_count": 7, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "await scila.get_co2_flow_status()" - ] - }, - { - "cell_type": "code", - "execution_count": 8, - "id": "d30745a8", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "'Closed'" - ] - }, - "execution_count": 8, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "await scila.get_drawer_position(3)" - ] - }, - { - "cell_type": "markdown", - "id": "e5c47abe", - "metadata": {}, - "source": [ - "## Movement\n", - "\n", - "Only one drawer can be open at a time. When a drawer is opened and you try to open another drawer, an error is raised." - ] - }, - { - "cell_type": "code", - "execution_count": 9, - "id": "63ea94b0", - "metadata": {}, - "outputs": [], - "source": [ - "await scila.open_drawer(2)" - ] - }, - { - "cell_type": "code", - "execution_count": 10, - "id": "3d1bca31", - "metadata": {}, - "outputs": [], - "source": [ - "await scila.close_drawer(2)" - ] - }, - { - "cell_type": "markdown", - "id": "f6d1452a", - "metadata": {}, - "source": [ - "## Temperature control\n", - "\n", - "There is one temperature for all drawers." - ] - }, - { - "cell_type": "code", - "execution_count": 11, - "id": "f88c9c83", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "19.06" - ] - }, - "execution_count": 11, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "await scila.get_current_temperature()" - ] - }, - { - "cell_type": "code", - "execution_count": 15, - "id": "a1958744-006b-4302-a2f4-af03c5764a2d", - "metadata": { - "scrolled": true - }, - { - "data": { - "text/plain": [ - "['DrawerPosition',\n", - " '__abstractmethods__',\n", - " '__annotations__',\n", - " '__class__',\n", - " '__delattr__',\n", - " '__dict__',\n", - " '__dir__',\n", - " '__doc__',\n", - " '__eq__',\n", - " '__format__',\n", - " '__ge__',\n", - " '__getattribute__',\n", - " '__getstate__',\n", - " '__gt__',\n", - " '__hash__',\n", - " '__init__',\n", - " '__init_subclass__',\n", - " '__le__',\n", - " '__lt__',\n", - " '__module__',\n", - " '__ne__',\n", - " '__new__',\n", - " '__reduce__',\n", - " '__reduce_ex__',\n", - " '__repr__',\n", - " '__setattr__',\n", - " '__sizeof__',\n", - " '__slots__',\n", - " '__str__',\n", - " '__subclasshook__',\n", - " '__weakref__',\n", - " '_abc_impl',\n", - " '_instances',\n", - " '_reset_and_initialize',\n", - " '_sila_interface',\n", - " 'close_drawer',\n", - " 'deactivate_temperature_control',\n", - " 'deserialize',\n", - " 'get_all_instances',\n", - " 'get_co2_flow_status',\n", - " 'get_current_temperature',\n", - " 'get_drawer_position',\n", - " 'get_drawer_positions',\n", - " 'get_liquid_level',\n", - " 'get_status',\n", - " 'get_target_temperature',\n", - " 'get_temperature_control_enabled',\n", - " 'get_temperature_information',\n", - " 'get_valve_status',\n", - " 'open_drawer',\n", - " 'serialize',\n", - " 'set_tempeature',\n", - " 'setup',\n", - " 'stop']" - ] - }, - "execution_count": 15, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "dir(scila)" - ] - }, - { - "cell_type": "code", - "execution_count": 17, - "id": "cc2f6063", - "metadata": {}, - "outputs": [], - "source": [ - "await scila.set_tempeature(37.0)" - ] - }, - { - "cell_type": "code", - "execution_count": 18, - "id": "5f04d0ef", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "37.0" - ] - }, - "execution_count": 18, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "await scila.get_target_temperature()" - ] - }, - { - "cell_type": "code", - "execution_count": 19, - "id": "02a60f32", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "23.04" - ] - }, - "execution_count": 19, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "await scila.get_current_temperature()" - ] - }, - { - "cell_type": "code", - "execution_count": 20, - "id": "470241e1", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "True" - ] - }, - "execution_count": 20, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "await scila.get_temperature_control_enabled()" - ] - }, - { - "cell_type": "code", - "execution_count": 21, - "id": "5881c2ce", - "metadata": {}, - "outputs": [], - "source": [ - "await scila.deactivate_temperature_control()" - ] - }, - { - "cell_type": "code", - "execution_count": 22, - "id": "ac8ad797", - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "False" - ] - }, - "execution_count": 22, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "await scila.get_temperature_control_enabled()" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "67b2946d-4d5c-4d13-a74c-59eb5e9c9791", - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3 (ipykernel)", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.12.11" - } - }, - "nbformat": 4, - "nbformat_minor": 5 -} From 046af461b075af2329141d33617739f74a7a70ae Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 15:08:16 -0700 Subject: [PATCH 06/93] port KX2 to new arm architecture under paa/ MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- docs/user_guide/paa/kx2/hello-world.ipynb | 750 ++++++++++++++++++++ pylabrobot/paa/__init__.py | 0 pylabrobot/paa/kx2/__init__.py | 2 + pylabrobot/paa/kx2/kx2.py | 17 + pylabrobot/{arms => paa}/kx2/kx2_backend.py | 250 +++++-- 5 files changed, 965 insertions(+), 54 deletions(-) create mode 100644 docs/user_guide/paa/kx2/hello-world.ipynb create mode 100644 pylabrobot/paa/__init__.py create mode 100644 pylabrobot/paa/kx2/__init__.py create mode 100644 pylabrobot/paa/kx2/kx2.py rename pylabrobot/{arms => paa}/kx2/kx2_backend.py (94%) diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb new file mode 100644 index 00000000000..456a4314d09 --- /dev/null +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -0,0 +1,750 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "kx2-intro", + "metadata": {}, + "source": [ + "# PAA KX2\n", + "\n", + "The [PAA KX2](https://www.peakanalysisandautomation.com/products/plate-handlers/kx2/) (also sold as KiNEDx KX2) is a 5-axis SCARA plate-handling robot from Peak Analysis and Automation. It supports:\n", + "\n", + "- [Arms](../../capabilities/arms) (pick/place, Cartesian and joint movement, freedrive teaching)\n", + "\n", + "The device communicates over CAN bus using the CANopen protocol (CiA-301 + CiA-402 drive profile). A PCAN/SocketCAN-compatible CAN interface is required (e.g. PEAK PCAN-USB).\n", + "\n", + "| Model | PLR Name | Gripper | Notes |\n", + "|---|---|---|---|\n", + "| KX2 | `KX2` | servo | 4 motion axes (shoulder, Z, elbow, wrist) + servo gripper |" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-setup-header", + "metadata": {}, + "source": [ + "## Setup\n", + "\n", + "`setup()` opens the CAN bus, discovers nodes, reads drive parameters, enables the motion axes, and homes + closes the servo gripper." + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "id": "kx2-setup-code", + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "uptime library not available, timestamps are relative to boot time and not to Epoch UTC\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "motor send command 1 UI 4 False (1)\n", + "motor send command 2 UI 4 False (1)\n", + "motor send command 3 UI 4 False (1)\n", + "motor send command 4 UI 4 False (1)\n", + "motor send command 6 UI 4 False (1)\n", + "\n", + "axis 1\n", + "motor send command 1 UI 5 False (1)\n", + "motor send command 1 UI 6 False (1)\n", + "motor send command 1 UI 7 False (1)\n", + "motor send command 1 UI 8 False (1)\n", + "motor send command 1 UI 9 False (1)\n", + "motor send command 1 UI 10 False (1)\n", + "motor send command 1 UI 11 False (1)\n", + "motor send command 1 UI 12 False (1)\n", + "motor send command 1 UI 13 False (1)\n", + "motor send command 1 UI 14 False (1)\n", + "motor send command 1 UI 15 False (1)\n", + "motor send command 1 UI 16 False (1)\n", + "motor send command 1 UI 24 False (1)\n", + "motor send command 1 UF 1 True (2)\n", + "motor send command 1 UF 2 True (2)\n", + "motor send command 1 XM 1 False (1)\n", + "motor send command 1 XM 2 False (1)\n", + "motor send command 1 UF 3 True (2)\n", + "motor send command 1 UF 4 True (2)\n", + "motor send command 1 VH 3 False (1)\n", + "motor send command 1 VL 3 False (1)\n", + "motor send command 1 CA 45 False (1)\n", + "motor send command 1 CA 41 False (1)\n", + "motor send command 1 CA 46 False (1)\n", + "motor send command 1 SP 2 False (1)\n", + "motor send command 1 VH 2 False (1)\n", + "motor send command 1 SD 0 False (1)\n", + "\n", + "axis 2\n", + "motor send command 2 UI 5 False (1)\n", + "motor send command 2 UI 6 False (1)\n", + "motor send command 2 UI 7 False (1)\n", + "motor send command 2 UI 8 False (1)\n", + "motor send command 2 UI 9 False (1)\n", + "motor send command 2 UI 10 False (1)\n", + "motor send command 2 UI 11 False (1)\n", + "motor send command 2 UI 12 False (1)\n", + "motor send command 2 UI 13 False (1)\n", + "motor send command 2 UI 14 False (1)\n", + "motor send command 2 UI 15 False (1)\n", + "motor send command 2 UI 16 False (1)\n", + "motor send command 2 UI 24 False (1)\n", + "motor send command 2 UF 1 True (2)\n", + "motor send command 2 UF 2 True (2)\n", + "motor send command 2 XM 1 False (1)\n", + "motor send command 2 XM 2 False (1)\n", + "motor send command 2 UF 3 True (2)\n", + "motor send command 2 UF 4 True (2)\n", + "motor send command 2 VH 3 False (1)\n", + "motor send command 2 VL 3 False (1)\n", + "motor send command 2 CA 45 False (1)\n", + "motor send command 2 CA 41 False (1)\n", + "motor send command 2 CA 46 False (1)\n", + "motor send command 2 SP 2 False (1)\n", + "motor send command 2 VH 2 False (1)\n", + "motor send command 2 SD 0 False (1)\n", + "\n", + "axis 3\n", + "motor send command 3 UI 5 False (1)\n", + "motor send command 3 UI 6 False (1)\n", + "motor send command 3 UI 7 False (1)\n", + "motor send command 3 UI 8 False (1)\n", + "motor send command 3 UI 9 False (1)\n", + "motor send command 3 UI 10 False (1)\n", + "motor send command 3 UI 11 False (1)\n", + "motor send command 3 UI 12 False (1)\n", + "motor send command 3 UI 13 False (1)\n", + "motor send command 3 UI 14 False (1)\n", + "motor send command 3 UI 15 False (1)\n", + "motor send command 3 UI 16 False (1)\n", + "motor send command 3 UI 24 False (1)\n", + "motor send command 3 UF 1 True (2)\n", + "motor send command 3 UF 2 True (2)\n", + "motor send command 3 XM 1 False (1)\n", + "motor send command 3 XM 2 False (1)\n", + "motor send command 3 UF 3 True (2)\n", + "motor send command 3 UF 4 True (2)\n", + "motor send command 3 VH 3 False (1)\n", + "motor send command 3 VL 3 False (1)\n", + "motor send command 3 CA 45 False (1)\n", + "motor send command 3 CA 41 False (1)\n", + "motor send command 3 CA 46 False (1)\n", + "motor send command 3 SP 2 False (1)\n", + "motor send command 3 VH 2 False (1)\n", + "motor send command 3 SD 0 False (1)\n", + "\n", + "axis 4\n", + "motor send command 4 UI 5 False (1)\n", + "motor send command 4 UI 6 False (1)\n", + "motor send command 4 UI 7 False (1)\n", + "motor send command 4 UI 8 False (1)\n", + "motor send command 4 UI 9 False (1)\n", + "motor send command 4 UI 10 False (1)\n", + "motor send command 4 UI 11 False (1)\n", + "motor send command 4 UI 12 False (1)\n", + "motor send command 4 UI 13 False (1)\n", + "motor send command 4 UI 14 False (1)\n", + "motor send command 4 UI 15 False (1)\n", + "motor send command 4 UI 16 False (1)\n", + "motor send command 4 UI 24 False (1)\n", + "motor send command 4 UF 1 True (2)\n", + "motor send command 4 UF 2 True (2)\n", + "motor send command 4 XM 1 False (1)\n", + "motor send command 4 XM 2 False (1)\n", + "motor send command 4 UF 3 True (2)\n", + "motor send command 4 UF 4 True (2)\n", + "motor send command 4 VH 3 False (1)\n", + "motor send command 4 VL 3 False (1)\n", + "motor send command 4 CA 45 False (1)\n", + "motor send command 4 CA 42 False (1)\n", + "motor send command 4 CA 46 False (1)\n", + "motor send command 4 FF 3 True (2)\n", + "motor send command 4 SP 2 False (1)\n", + "motor send command 4 VH 2 False (1)\n", + "motor send command 4 SD 0 False (1)\n", + "\n", + "axis 6\n", + "motor send command 6 UI 5 False (1)\n", + "motor send command 6 UI 6 False (1)\n", + "motor send command 6 UI 7 False (1)\n", + "motor send command 6 UI 8 False (1)\n", + "motor send command 6 UI 9 False (1)\n", + "motor send command 6 UI 10 False (1)\n", + "motor send command 6 UI 11 False (1)\n", + "motor send command 6 UI 12 False (1)\n", + "motor send command 6 UI 13 False (1)\n", + "motor send command 6 UI 14 False (1)\n", + "motor send command 6 UI 15 False (1)\n", + "motor send command 6 UI 16 False (1)\n", + "motor send command 6 UI 24 False (1)\n", + "motor send command 6 UF 1 True (2)\n", + "motor send command 6 UF 2 True (2)\n", + "motor send command 6 XM 1 False (1)\n", + "motor send command 6 XM 2 False (1)\n", + "motor send command 6 UF 3 True (2)\n", + "motor send command 6 UF 4 True (2)\n", + "motor send command 6 VH 3 False (1)\n", + "motor send command 6 VL 3 False (1)\n", + "motor send command 6 CA 45 False (1)\n", + "motor send command 6 CA 41 False (1)\n", + "motor send command 6 CA 46 False (1)\n", + "motor send command 6 SP 2 False (1)\n", + "motor send command 6 VH 2 False (1)\n", + "motor send command 6 SD 0 False (1)\n", + "node_id not int: KX2Axis.SHOULDER \n", + "motor send command 1 UF 6 True (2)\n", + "node_id not int: KX2Axis.SHOULDER \n", + "motor send command 1 UF 7 True (2)\n", + "node_id not int: KX2Axis.SHOULDER \n", + "motor send command 1 UF 8 True (2)\n", + "node_id not int: KX2Axis.SHOULDER \n", + "motor send command 1 UF 9 True (2)\n", + "node_id not int: KX2Axis.SHOULDER \n", + "motor send command 1 UF 10 True (2)\n", + "node_id not int: KX2Axis.SHOULDER \n", + "motor send command 1 UF 11 True (2)\n", + "node_id not int: KX2Axis.SHOULDER \n", + "motor send command 1 UF 12 True (2)\n", + "node_id not int: KX2Axis.SHOULDER \n", + "motor send command 1 UF 13 True (2)\n", + "node_id not int: KX2Axis.SHOULDER \n", + "motor send command 1 UF 14 True (2)\n", + "node_id not int: KX2Axis.SHOULDER \n", + "motor send command 1 UF 15 True (2)\n", + "node_id not int: KX2Axis.SHOULDER \n", + "motor send command 1 UI 17 False (1)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 UF 6 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 UF 7 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 UF 8 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 UF 9 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 UF 10 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 UF 11 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 UF 12 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 UF 13 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 UF 14 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 UF 15 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 UF 16 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 UF 17 True (2)\n", + "Raising event: EventData(event_type=, pending=False, node_id=1, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=1591, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=2, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=1591, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=3, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=1719, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=4, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=1591, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "motor send command 6 PL 1 0.9200000166893005 True (2)\n", + "motor send command 6 CL 1 0.9100000262260437 True (2)\n", + "['100', '5000']\n", + "(100,5000)\n", + "XQ##Home(100,5000)\n", + "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 PL 1 0.9200000166893005 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 CL 1 0.9100000262260437 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 PL 1 0.9200000166893005 True (2)\n", + "cmd {: 0}\n", + "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 MS 1 False (1)\n", + "Servo Gripper Motor Status: 0\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 CL 1 False (1)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 IQ 0 False (1)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 CL 1 False (1)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 IQ 0 False (1)\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=2, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=1, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=4, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=3, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=2, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=4, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=3, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=1, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=4, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=3, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=1, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=2, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=2, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=4, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=3, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=1, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=4, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=3, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=2, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "Raising event: EventData(event_type=, pending=False, node_id=1, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", + "CAN Read Error: The value of a handle (PCAN-Channel, PCAN-Hardware, PCAN-Net, PCAN-Client) is invalid\n" + ] + } + ], + "source": [ + "from pylabrobot.paa.kx2 import KX2\n", + "\n", + "kx2 = KX2()\n", + "await kx2.setup()" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-arm-header", + "metadata": {}, + "source": [ + "## Arm capabilities\n", + "\n", + "The KX2 exposes an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm` on `kx2.arm`, backed by {class}`~pylabrobot.paa.kx2.KX2ArmBackend`. Gripper yaw is controlled via a single `direction` float (degrees; 0° = front). For the full arm API, see [Arms](../../capabilities/arms).\n", + "\n", + "The raw driver (`kx2.driver`, a `KX2Driver`) stays available for low-level access — motor commands, drive diagnostics, binary interpreter, etc." + ] + }, + { + "cell_type": "markdown", + "id": "kx2-gripper-header", + "metadata": {}, + "source": [ + "### Gripper control\n", + "\n", + "The servo gripper is force-aware. `close_gripper` moves to the target width and then verifies the plate is gripped; pass `check_plate_gripped=False` via `GripParams` to skip the verification move. Units are KX2 gripper encoder units (mm-equivalent once calibrated)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "kx2-gripper-open", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "cmd {: 30}\n" + ] + } + ], + "source": [ + "await kx2.arm.open_gripper(gripper_width=30)" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "kx2-gripper-close", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "cmd {: 0}\n" + ] + } + ], + "source": [ + "from pylabrobot.paa.kx2 import KX2ArmBackend\n", + "\n", + "await kx2.arm.close_gripper(\n", + " gripper_width=0,\n", + " backend_params=KX2ArmBackend.GripParams(check_plate_gripped=False),\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "kx2-gripper-status", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "True" + ] + }, + "execution_count": 4, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "await kx2.arm.is_gripper_closed()" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-gripper-force-md", + "metadata": {}, + "source": [ + "Adjust the default gripping force (as a percentage of the motor's peak current) before closing on fragile labware:" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "kx2-gripper-force", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 PL 1 0.9200000166893005 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 CL 1 0.2730000078678131 True (2)\n", + "node_id not int: KX2Axis.SERVO_GRIPPER \n", + "motor send command 6 PL 1 0.27600000500679017 True (2)\n" + ] + } + ], + "source": [ + "await kx2.arm.backend.servo_gripper_set_default_gripping_force(max_force_percent=30)" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-cartesian-header", + "metadata": {}, + "source": [ + "### Cartesian movement\n", + "\n", + "Move the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only — the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity and acceleration." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "11b06c86", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "GripperLocation(location=Coordinate(x=349.124, y=170.552, z=280.0441), rotation=Rotation(x=0, y=0, z=198.93898581596522))" + ] + }, + "execution_count": 6, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "await kx2.arm.request_gripper_location()" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "id": "kx2-cartesian-code", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "cmd {: -59.474537393375996, : 290.0441, : 243.19263109523325, : 257.474537393376}\n" + ] + } + ], + "source": [ + "from pylabrobot.resources import Coordinate\n", + "\n", + "await kx2.arm.move_to_location(\n", + " location=Coordinate(x=340.124, y=200.552, z=290.0441),\n", + " direction=198,\n", + " backend_params=KX2ArmBackend.CartesianMoveParams(vel_pct=25, accel_pct=25),\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-joint-header", + "metadata": {}, + "source": [ + "### Joint movement\n", + "\n", + "Move in joint space using the {class}`~pylabrobot.paa.kx2.KX2Axis` enum. Rotary axes in degrees; Z (and gripper) in mm-equivalent encoder units.\n", + "\n", + "```{warning}\n", + "Moving to arbitrary joint positions can cause the arm to collide with its base, gripper, or nearby equipment. Verify coordinates in a safe workspace first, and start with low velocity.\n", + "```" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-joint-code", + "metadata": {}, + "outputs": [], + "source": [ + "from pylabrobot.paa.kx2 import KX2Axis\n", + "\n", + "position = {\n", + " KX2Axis.SHOULDER: 0.0,\n", + " KX2Axis.Z: 150.0,\n", + " KX2Axis.ELBOW: 90.0,\n", + " KX2Axis.WRIST: 0.0,\n", + "}\n", + "await kx2.arm.backend.move_to_joint_position(\n", + " position,\n", + " backend_params=KX2ArmBackend.JointMoveParams(vel_pct=25, accel_pct=25),\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-query-header", + "metadata": {}, + "source": [ + "### Querying position\n", + "\n", + "Read the current joint angles or Cartesian end-effector pose:" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "id": "kx2-query-joints", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{: 300.52548395979784,\n", + " : 290.0440412368887,\n", + " : 243.19265215891733,\n", + " : 257.4755859375,\n", + " : 0.0}" + ] + }, + "execution_count": 12, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "await kx2.arm.backend.request_joint_position()" + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "id": "kx2-query-cart", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "GripperLocation(location=Coordinate(x=340.1239, y=200.5521, z=290.044), rotation=Rotation(x=0, y=0, z=198.00106989729784))" + ] + }, + "execution_count": 13, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "await kx2.arm.request_gripper_location()" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-pick-place-header", + "metadata": {}, + "source": [ + "### Pick and place\n", + "\n", + "`pick_up_at_location` moves to the target pose and closes the gripper to `resource_width`. `drop_at_location` moves and opens the gripper. Approach and retract motions are the caller's responsibility — bracket these calls with your own pre- and post-moves." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-pick-place-code", + "metadata": {}, + "outputs": [], + "source": [ + "pick = Coordinate(x=450.0, y=0.0, z=80.0)\n", + "place = Coordinate(x=450.0, y=-200.0, z=80.0)\n", + "above = Coordinate(x=0, y=0, z=60.0)\n", + "yaw = 0.0\n", + "\n", + "await kx2.arm.move_to_location(pick + above, direction=yaw)\n", + "await kx2.arm.pick_up_at_location(location=pick, direction=yaw, resource_width=0)\n", + "await kx2.arm.move_to_location(pick + above, direction=yaw)\n", + "\n", + "await kx2.arm.move_to_location(place + above, direction=yaw)\n", + "await kx2.arm.drop_at_location(location=place, direction=yaw, resource_width=30)\n", + "await kx2.arm.move_to_location(place + above, direction=yaw)" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-freedrive-header", + "metadata": {}, + "source": [ + "### Freedrive (teaching mode)\n", + "\n", + "Freedrive disables torque on the motion axes so you can push the arm by hand; the servo gripper stays powered. The KX2 frees all motion axes at once — the `free_axes` argument is accepted for API parity and ignored." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-free-on", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.arm.backend.start_freedrive_mode(free_axes=[0])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-free-read", + "metadata": {}, + "outputs": [], + "source": [ + "# Manually guide the arm to the desired pose, then read it:\n", + "taught = await kx2.arm.request_gripper_location()\n", + "print(taught)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-free-off", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.arm.backend.stop_freedrive_mode()" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-misc-header", + "metadata": {}, + "source": [ + "### Halt and fault diagnostics\n", + "\n", + "`halt` issues an emergency stop on every motion axis. Driver-level methods give you estop-line state and per-axis fault codes." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-halt", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.arm.backend.halt()" + ] + }, + { + "cell_type": "code", + "execution_count": 14, + "id": "kx2-estop", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "node_id not int: KX2Axis.SHOULDER \n", + "motor send command 1 SR 1 False (1)\n", + "!!! not the same\n" + ] + }, + { + "data": { + "text/plain": [ + "True" + ] + }, + "execution_count": 14, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "await kx2.arm.backend.get_estop_state()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-fault", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.driver.motor_get_fault(KX2Axis.SHOULDER)" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-teardown-header", + "metadata": {}, + "source": [ + "## Teardown" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-teardown-code", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.stop()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "env", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.15" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/pylabrobot/paa/__init__.py b/pylabrobot/paa/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/pylabrobot/paa/kx2/__init__.py b/pylabrobot/paa/kx2/__init__.py new file mode 100644 index 00000000000..4f16a1c70e2 --- /dev/null +++ b/pylabrobot/paa/kx2/__init__.py @@ -0,0 +1,2 @@ +from pylabrobot.paa.kx2.kx2 import KX2 +from pylabrobot.paa.kx2.kx2_backend import KX2ArmBackend, KX2Axis, KX2Driver diff --git a/pylabrobot/paa/kx2/kx2.py b/pylabrobot/paa/kx2/kx2.py new file mode 100644 index 00000000000..64e458cda8f --- /dev/null +++ b/pylabrobot/paa/kx2/kx2.py @@ -0,0 +1,17 @@ +from pylabrobot.capabilities.arms.orientable_arm import OrientableArm +from pylabrobot.device import Device +from pylabrobot.paa.kx2.kx2_backend import KX2ArmBackend, KX2Driver +from pylabrobot.resources.resource import Resource + + +class KX2(Device): + """PAA KX2 robotic plate handler.""" + + def __init__(self) -> None: + driver = KX2Driver() + super().__init__(driver=driver) + self.driver: KX2Driver = driver + backend = KX2ArmBackend(driver=driver) + self.reference = Resource(name="KX2", size_x=200, size_y=200, size_z=200) + self.arm = OrientableArm(backend=backend, reference_resource=self.reference) + self._capabilities = [self.arm] diff --git a/pylabrobot/arms/kx2/kx2_backend.py b/pylabrobot/paa/kx2/kx2_backend.py similarity index 94% rename from pylabrobot/arms/kx2/kx2_backend.py rename to pylabrobot/paa/kx2/kx2_backend.py index 2086994fea4..aae537d8363 100644 --- a/pylabrobot/arms/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/kx2_backend.py @@ -9,7 +9,15 @@ import can -from pylabrobot.arms.standard import GripperPose +from pylabrobot.capabilities.arms.backend import ( + CanFreedrive, + HasJoints, + OrientableGripperArmBackend, +) +from pylabrobot.capabilities.arms.standard import GripperLocation +from pylabrobot.capabilities.arms.standard import GripperLocation as GripperPose +from pylabrobot.capabilities.capability import BackendParams +from pylabrobot.device import Driver from pylabrobot.resources import Coordinate, Rotation @@ -22,6 +30,9 @@ class KX2Axis(IntEnum): SERVO_GRIPPER = 6 +MOTION_AXES = (KX2Axis.SHOULDER, KX2Axis.Z, KX2Axis.ELBOW, KX2Axis.WRIST) + + def _is_number(s: str) -> bool: try: float(str(s).strip()) @@ -422,8 +433,11 @@ class MotorsMovePlan: move_time: float = 10.0 -class KX2Can: +class KX2Driver(Driver): + """Low-level CAN transport + CANopen/DS402 drive primitives for the PAA KX2.""" + def __init__(self, has_rail: bool = False, has_servo_gripper: bool = True) -> None: + super().__init__() self.connecting: bool = False self.grp_id: int = 0 @@ -1166,7 +1180,7 @@ async def connect_part_two(self): # Configure Elmo objects if group_node_id is set # This section involves can_sdo_download_ElmoObject which needs to be translated first. - for axis in KX2Backend.MOTION_AXES: + for axis in MOTION_AXES: await self.can_sdo_download_elmo_object( node_id=int(axis), elmo_object_int=24768, @@ -1215,7 +1229,7 @@ async def connect_part_two(self): data_type=ElmoObjectDataType.INTEGER16, ) - for axis in KX2Backend.MOTION_AXES: + for axis in MOTION_AXES: await self.can_rpdo1_map(int(axis)) await self.can_rpdo3_map(int(axis)) @@ -1231,6 +1245,12 @@ async def disconnect(self) -> None: self._can_device.shutdown() self._can_device = None + async def setup(self, backend_params: Optional[BackendParams] = None) -> None: + await self.connect() + + async def stop(self) -> None: + await self.disconnect() + async def _raise_an_event(self, event_data: EventData): print(f"Raising event: {event_data}") @@ -2123,7 +2143,7 @@ async def pvt_select_mode(self, enable: bool) -> None: if enable: if not self._pvt_mode: - for axis in KX2Backend.MOTION_AXES: + for axis in MOTION_AXES: await self.can_sdo_download( node_id=int(axis), object_byte0=0x60, @@ -2140,7 +2160,7 @@ async def pvt_select_mode(self, enable: bool) -> None: ) self._pvt_mode = True else: - for axis in KX2Backend.MOTION_AXES: + for axis in MOTION_AXES: await self.can_sdo_download( node_id=int(axis), object_byte0=0x60, @@ -2163,7 +2183,7 @@ async def pvt_select_mode(self, enable: bool) -> None: data_byte=[7], ) elif self._pvt_mode: - for axis in KX2Backend.MOTION_AXES: + for axis in MOTION_AXES: await self.can_sdo_download( node_id=int(axis), object_byte0=0x60, @@ -2327,7 +2347,7 @@ def _float_matches(expected_str: str, actual_str: str) -> bool: raise CanError("Internal error in binary_interpreter (single-node)") # Group path (NodeID == 10) - grp_ids = [int(axis) for axis in KX2Backend.MOTION_AXES] + grp_ids = [int(axis) for axis in MOTION_AXES] for attempt in range(1, max_attempts + 1): if value == "": @@ -2490,7 +2510,7 @@ async def motor_enable(self, axis: KX2Axis, state: bool) -> None: if not isinstance(axis, KX2Axis): raise - flag = not (axis in KX2Backend.MOTION_AXES or int(axis) == self.grp_id) + flag = not (axis in MOTION_AXES or int(axis) == self.grp_id) if state: self.EmcyMoveErrorReceived = False @@ -2977,11 +2997,17 @@ async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: await self._wait_for_moves_done(timeout=plan.move_time + 2) -class KX2Backend: - MOTION_AXES = (KX2Axis.SHOULDER, KX2Axis.Z, KX2Axis.ELBOW, KX2Axis.WRIST) +class KX2ArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive): + """Arm-capability backend for the PAA KX2. + + Owns a :class:`KX2Driver` (low-level CAN transport) and implements the + capability-based arm interface (``OrientableGripperArmBackend`` + + ``HasJoints`` + ``CanFreedrive``) directly on top of the drive primitives. + """ - def __init__(self): - self.can = KX2Can() + def __init__(self, driver: "KX2Driver") -> None: + super().__init__() + self.driver = driver self.digital_input_assignment = {} # TODO: just cache? self.AnalogInputAssignment = {} @@ -3004,20 +3030,22 @@ def __init__(self): self.node_id_list = [1, 2, 3, 4, 6] - async def initialize(self): - await self.can.connect() # just to get the node IDs - await self.drive_get_parameters(self.can.node_id_list) - await self.can.connect_part_two() + async def _on_setup(self, backend_params: Optional[BackendParams] = None): + # Driver has already brought CAN up (connect + node discovery) via + # Device.setup(). Now read per-drive parameters, finish CANopen mapping, + # enable motion axes, and initialize the servo gripper. + await self.drive_get_parameters(self.driver.node_id_list) + await self.driver.connect_part_two() await asyncio.sleep(2) - for axis in KX2Backend.MOTION_AXES: + for axis in MOTION_AXES: if self.unlimited_travel_ax[axis]: self.g_joint_move_direction[axis] = JointMoveDirection.ShortestWay - for axis in KX2Backend.MOTION_AXES: + for axis in MOTION_AXES: try: - await self.can.motor_enable(axis=axis, state=True) + await self.driver.motor_enable(axis=axis, state=True) except Exception as e: print(f"Error enabling motor on axis {axis}: {e}") @@ -3025,7 +3053,7 @@ async def initialize(self): async def servo_gripper_initialize(self): try: - await self.can.motor_enable(axis=KX2Axis.SERVO_GRIPPER, state=True) + await self.driver.motor_enable(axis=KX2Axis.SERVO_GRIPPER, state=True) except Exception as e: print(f"Error enabling servo gripper motor on node {KX2Axis.SERVO_GRIPPER}: {e}") @@ -3050,7 +3078,7 @@ async def servo_gripper_home(self) -> None: val_type=ValType.Float, ) - await self.can.home_motor( + await self.driver.home_motor( axis=KX2Axis.SERVO_GRIPPER, hs_offset=self.servo_gripper_home_hard_stop_offset, ind_offset=self.servo_gripper_home_index_offset, @@ -3137,7 +3165,7 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: return elif motor_status == 2: - motor_fault = self.can.motor_get_fault(KX2Axis.SERVO_GRIPPER) + motor_fault = self.driver.motor_get_fault(KX2Axis.SERVO_GRIPPER) if motor_fault is None: raise RuntimeError("Error querying whether plate is gripped. Error querying motor fault.") raise RuntimeError( @@ -3284,7 +3312,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: if node == await self.motor_send_command(node, "UI", 4, val_type=ValType.Int) for node in nodes } - for required_axis in KX2Backend.MOTION_AXES: + for required_axis in MOTION_AXES: if required_axis.value not in uis: raise CanError(f"Missing required axis with UI[4]={required_axis}") if 5 in uis: @@ -3600,7 +3628,7 @@ async def motor_send_command( query_flag = not (has_xc or has_xq) # OSInterpreter writes into `str` and can also write a long; you can ignore the long if you don't use it. - s = await self.can.os_interpreter( + s = await self.driver.os_interpreter( node_id=(node_id), cmd=motor_command, query=query_flag, @@ -3609,7 +3637,7 @@ async def motor_send_command( if cmd_type == CmdType.ValQuery: returned_data = s if s is not None else "" else: - s = await self.can.binary_interpreter( + s = await self.driver.binary_interpreter( node_id=(node_id), cmd=motor_command, cmd_index=int(index), @@ -3650,7 +3678,7 @@ def convert_elbow_angle_to_position(self, elbow_angle_deg: float) -> float: return elbow_pos async def motor_get_current_position(self, axis: KX2Axis) -> float: - raw = await self.can.motor_get_current_position(node_id=axis, pu=self.unlimited_travel_ax[axis]) + raw = await self.driver.motor_get_current_position(node_id=axis, pu=self.unlimited_travel_ax[axis]) c = self.motor_conversion_factor_ax[axis] if axis == KX2Axis.ELBOW: return self.convert_elbow_angle_to_position(elbow_angle_deg=raw / c) @@ -3662,7 +3690,7 @@ async def motor_get_current_position(self, axis: KX2Axis) -> float: return raw / c async def read_input(self, axis: int, input_num: int) -> bool: - return await self.can.read_input(node_id=axis, input_num=0x10 + input_num) + return await self.driver.read_input(node_id=axis, input_num=0x10 + input_num) @staticmethod def _wrap_to_range(x: float, lo: float, hi: float) -> float: @@ -3757,7 +3785,7 @@ async def calculate_move_abs_all_axes( raise ValueError("Base-to-gripper clearance violated") # Determine current/start positions - curr = await self.get_joint_position() + curr = await self.request_joint_position() # Elbow: convert both target and start to angle for distance math if KX2Axis.ELBOW in curr: @@ -3903,7 +3931,7 @@ async def motors_move_joint( if plan is None: # if every axis is skipped, exit return - await self.can.motors_move_absolute_execute(plan) + await self.driver.motors_move_absolute_execute(plan) def convert_cartesian_to_joint_position(self, pose: GripperPose) -> Dict["KX2Axis", float]: if pose.rotation.x != 0 or pose.rotation.y != 0: @@ -4028,37 +4056,151 @@ def convert_tool_coordinate_to_joint_position( return self.convert_cartesian_to_joint_position(coordinate) - async def get_joint_position(self) -> Dict["KX2Axis", float]: - return { - KX2Axis.SHOULDER: await self.motor_get_current_position(KX2Axis.SHOULDER), - KX2Axis.Z: await self.motor_get_current_position(KX2Axis.Z), - KX2Axis.ELBOW: await self.motor_get_current_position(KX2Axis.ELBOW), - KX2Axis.WRIST: await self.motor_get_current_position(KX2Axis.WRIST), - KX2Axis.SERVO_GRIPPER: await self.motor_get_current_position(KX2Axis.SERVO_GRIPPER), - } + # -- capability interface (OrientableGripperArmBackend + HasJoints + CanFreedrive) -- + + @dataclass + class CartesianMoveParams(BackendParams): + vel_pct: float = 100.0 + accel_pct: float = 100.0 + + @dataclass + class JointMoveParams(BackendParams): + vel_pct: float = 100.0 + accel_pct: float = 100.0 + + @dataclass + class GripParams(BackendParams): + check_plate_gripped: bool = True + + async def halt(self, backend_params: Optional[BackendParams] = None) -> None: + for axis in MOTION_AXES: + await self.driver.motor_emergency_stop(node_id=int(axis)) + + async def park(self, backend_params: Optional[BackendParams] = None) -> None: + raise NotImplementedError( + "KX2 does not define a default park pose. Use move_to_joint_position with a " + "site-specific safe configuration." + ) + + async def request_gripper_location( + self, backend_params: Optional[BackendParams] = None + ) -> GripperLocation: + current_joint_pos = await self.request_joint_position() + return self.convert_joint_position_to_cartesian(current_joint_pos) + + async def open_gripper( + self, gripper_width: float, backend_params: Optional[BackendParams] = None + ) -> None: + await self.motors_move_joint( + {KX2Axis.SERVO_GRIPPER: gripper_width}, + cmd_vel_pct=100, + cmd_accel_pct=100, + ) + + async def close_gripper( + self, gripper_width: float, backend_params: Optional[BackendParams] = None + ) -> None: + if not isinstance(backend_params, KX2ArmBackend.GripParams): + backend_params = KX2ArmBackend.GripParams() + await self.motors_move_joint( + {KX2Axis.SERVO_GRIPPER: gripper_width}, + cmd_vel_pct=100, + cmd_accel_pct=100, + ) + if backend_params.check_plate_gripped: + await self.check_plate_gripped() - async def get_cartesian_position(self) -> GripperPose: - current_joint_pos = await self.get_joint_position() - cartesian = self.convert_joint_position_to_cartesian(current_joint_pos) - return cartesian + async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool: + pos = await self.motor_get_current_position(KX2Axis.SERVO_GRIPPER) + return abs(pos) < 1.0 - async def move_to_cartesian_position( + async def move_to_location( self, - pose: GripperPose, - vel_pct: float = 100, - accel_pct: float = 100, + location: Coordinate, + direction: float, + backend_params: Optional[BackendParams] = None, ) -> None: + if not isinstance(backend_params, KX2ArmBackend.CartesianMoveParams): + backend_params = KX2ArmBackend.CartesianMoveParams() + pose = GripperLocation(location=location, rotation=Rotation(z=direction)) joint_pos = self.convert_cartesian_to_joint_position(pose) await self.motors_move_joint( cmd_pos=joint_pos, - cmd_vel_pct=vel_pct, - cmd_accel_pct=accel_pct, + cmd_vel_pct=backend_params.vel_pct, + cmd_accel_pct=backend_params.accel_pct, + ) + + async def pick_up_at_location( + self, + location: Coordinate, + direction: float, + resource_width: float, + backend_params: Optional[BackendParams] = None, + ) -> None: + await self.move_to_location(location, direction, backend_params=backend_params) + await self.close_gripper(resource_width, backend_params=backend_params) + + async def drop_at_location( + self, + location: Coordinate, + direction: float, + resource_width: float, + backend_params: Optional[BackendParams] = None, + ) -> None: + await self.move_to_location(location, direction, backend_params=backend_params) + await self.open_gripper(resource_width, backend_params=backend_params) + + async def move_to_joint_position( + self, + position: Dict[int, float], + backend_params: Optional[BackendParams] = None, + ) -> None: + if not isinstance(backend_params, KX2ArmBackend.JointMoveParams): + backend_params = KX2ArmBackend.JointMoveParams() + cmd_pos = {KX2Axis(int(k)): float(v) for k, v in position.items()} + await self.motors_move_joint( + cmd_pos=cmd_pos, + cmd_vel_pct=backend_params.vel_pct, + cmd_accel_pct=backend_params.accel_pct, ) - async def activate_free_mode(self) -> None: - for axis in KX2Backend.MOTION_AXES: - await self.can.motor_enable(axis=axis, state=False) + async def pick_up_at_joint_position( + self, + position: Dict[int, float], + resource_width: float, + backend_params: Optional[BackendParams] = None, + ) -> None: + await self.move_to_joint_position(position, backend_params=backend_params) + await self.close_gripper(resource_width, backend_params=backend_params) + + async def drop_at_joint_position( + self, + position: Dict[int, float], + resource_width: float, + backend_params: Optional[BackendParams] = None, + ) -> None: + await self.move_to_joint_position(position, backend_params=backend_params) + await self.open_gripper(resource_width, backend_params=backend_params) - async def deactivate_free_mode(self) -> None: - for axis in KX2Backend.MOTION_AXES: - await self.can.motor_enable(axis=axis, state=True) + async def request_joint_position( + self, backend_params: Optional[BackendParams] = None + ) -> Dict[int, float]: + return { + KX2Axis.SHOULDER: await self.motor_get_current_position(KX2Axis.SHOULDER), + KX2Axis.Z: await self.motor_get_current_position(KX2Axis.Z), + KX2Axis.ELBOW: await self.motor_get_current_position(KX2Axis.ELBOW), + KX2Axis.WRIST: await self.motor_get_current_position(KX2Axis.WRIST), + KX2Axis.SERVO_GRIPPER: await self.motor_get_current_position(KX2Axis.SERVO_GRIPPER), + } + + async def start_freedrive_mode( + self, free_axes: List[int], backend_params: Optional[BackendParams] = None + ) -> None: + # KX2 frees all motion axes at once; free_axes is accepted for API parity. + del free_axes + for axis in MOTION_AXES: + await self.driver.motor_enable(axis=axis, state=False) + + async def stop_freedrive_mode(self, backend_params: Optional[BackendParams] = None) -> None: + for axis in MOTION_AXES: + await self.driver.motor_enable(axis=axis, state=True) From 788d4c856501fa78211e67704deaac5c4e18795e Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 15:12:54 -0700 Subject: [PATCH 07/93] replace prints with module logger in KX2 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) --- pylabrobot/paa/kx2/kx2_backend.py | 73 +++++++++++++++---------------- 1 file changed, 35 insertions(+), 38 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_backend.py b/pylabrobot/paa/kx2/kx2_backend.py index aae537d8363..a7b3b0a8ebc 100644 --- a/pylabrobot/paa/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/kx2_backend.py @@ -1,4 +1,5 @@ import asyncio +import logging import math import struct import time @@ -20,6 +21,8 @@ from pylabrobot.device import Driver from pylabrobot.resources import Coordinate, Rotation +logger = logging.getLogger(__name__) + class KX2Axis(IntEnum): SHOULDER = 1 @@ -539,16 +542,14 @@ async def _can_write_task(self): message.fut.set_result(None) except Exception as e: error_msg = f"CAN Write Error: {e}" - print(f"CAN Write Error: {e}") + logger.error(error_msg) if message.fut and not message.fut.done(): message.fut.set_exception(CanError(error_msg)) async def _process_emcy_message(self, node_id: int, message: can.Message) -> None: - print("EMCY received!!") - data = message.data if not data or len(data) < 8: - print(f"EMCY malformed: expected 8 bytes, got {0 if not data else len(data)}") + logger.warning("EMCY malformed: expected 8 bytes, got %d", 0 if not data else len(data)) return def u16_le(i: int) -> int: @@ -702,12 +703,13 @@ def u16_le(i: int) -> int: event.cemr_description = desc event.cemr_emcy_msg = emcy event.cemr_disable_motors = disable_motors - print(event) + logger.debug("EMCY event: %s", event) - print(emcy) - print( - f"EMCY node={node_id} desc='{desc}' disable_motors={disable_motors} suppress_event={suppress_event}" + logger.info( + "EMCY node=%d desc=%r disable_motors=%s suppress_event=%s", + node_id, desc, disable_motors, suppress_event, ) + logger.debug("EMCY payload: %s", emcy) async def _process_tpdo_message(self, node_id: int, message: can.Message, response_type: int): tpdo_index = {0: TPDO.TPDO1, 4: TPDO.TPDO3, 6: TPDO.TPDO4}[response_type - 3] @@ -739,8 +741,6 @@ async def _process_tpdo_message(self, node_id: int, message: can.Message, respon ) num2 += 4 if num4_ is None: - # print("Failed to read TPDO mapped object value, probably fine....") - # raise CanError("Failed to read TPDO mapped object value.") continue num4 = num4_ @@ -811,7 +811,9 @@ async def _process_tpdo_message(self, node_id: int, message: can.Message, respon fut = self._waiting_moves[KX2Axis(node_id)] if not fut.done(): fut.set_result(None) - print(f"Digital input {index4} enabled motor move done for node {node_id}") + logger.debug( + "Digital input %d enabled motor move done for node %d", index4, node_id + ) self.input_state[node_idx] = num6 @@ -1002,7 +1004,7 @@ async def _can_read_task( try: message = await asyncio.to_thread(self.can_device.recv, timeout=1.0) except Exception as e: - print(f"CAN Read Error: {e}") + logger.error("CAN Read Error: %s", e) raise CanError(f"CAN Read Error: {e}") if message is None: # timeout @@ -1013,7 +1015,7 @@ async def _can_read_task( node_id = message.arbitration_id & 0x7F if response_type == 0: - print("NMT message received, ignoring") + logger.debug("NMT message received, ignoring") elif response_type == 1: await self._process_emcy_message(node_id=node_id, message=message) elif response_type in {3, 7, 9}: @@ -1027,9 +1029,11 @@ async def _can_read_task( elif response_type == 14: await self._process_motor_drive_restarted(node_id=node_id, message=message) else: - print(f"Unknown CAN message type received: {response_type}") - except Exception as e: - print(f"Error processing CAN message (arb_id={message.arbitration_id:#x}): {e}") + logger.warning("Unknown CAN message type received: %s", response_type) + except Exception: + logger.exception( + "Error processing CAN message (arb_id=%#x)", message.arbitration_id + ) async def can_write( self, @@ -1252,7 +1256,7 @@ async def stop(self) -> None: await self.disconnect() async def _raise_an_event(self, event_data: EventData): - print(f"Raising event: {event_data}") + logger.info("Raising event: %s", event_data) # TODO: on move error / emergency, we should set the indicator light and disable motors @@ -2619,10 +2623,8 @@ async def user_program_run( arg_str = "" if params: parts = [str(p) for p in params] - print(parts) if parts: arg_str = f"({','.join(parts)})" - print(arg_str) # Arm UI[1]=1 then execute XQ await self.binary_interpreter( @@ -2635,7 +2637,7 @@ async def user_program_run( ) cmd = f"XQ##{user_function}{arg_str}" - print(cmd) + logger.debug("user_program_run: %s", cmd) await self.os_interpreter(node_id, cmd, query=False) last_line_completed = 0 @@ -3047,7 +3049,7 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): try: await self.driver.motor_enable(axis=axis, state=True) except Exception as e: - print(f"Error enabling motor on axis {axis}: {e}") + logger.warning("Error enabling motor on axis %s: %s", axis, e) await self.servo_gripper_initialize() @@ -3055,7 +3057,9 @@ async def servo_gripper_initialize(self): try: await self.driver.motor_enable(axis=KX2Axis.SERVO_GRIPPER, state=True) except Exception as e: - print(f"Error enabling servo gripper motor on node {KX2Axis.SERVO_GRIPPER}: {e}") + logger.warning( + "Error enabling servo gripper motor on node %s: %s", KX2Axis.SERVO_GRIPPER, e + ) await self.servo_gripper_home() @@ -3144,7 +3148,7 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: motor_command="MS", index=1, ) - print(f"Servo Gripper Motor Status: {motor_status}") + logger.debug("Servo gripper motor status: %s", motor_status) if motor_status in {0, 1}: max_force_percentage = await self.get_servo_gripper_max_force() @@ -3323,8 +3327,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # Pass 2: per-axis parameters for axis in nodes: - print() - print("axis", axis) + logger.debug("Reading parameters for axis %s", axis) # UI[5..10] digital inputs for ui_idx in range(5, 11): @@ -3561,8 +3564,8 @@ async def get_estop_state(self) -> bool: r = int(r) num2 = not (r & 0x4000 == 0x4000) num3 = not (r & 0x8000 == 0x8000) - if not r == 8438016: - print("!!! not the same") + if r != 8438016: + logger.warning("get_estop_state: SR register unexpected value %d (expected 8438016)", r) return num2 == False and num3 == False async def motor_send_command( @@ -3576,16 +3579,10 @@ async def motor_send_command( low_priority: bool = False, ) -> str: if isinstance(node_id, KX2Axis): - print("node_id not int:", node_id, type(node_id)) node_id = int(node_id) - print( - "motor send command", - node_id, - motor_command, - index, - value, - val_type == ValType.Float, - f"({val_type})", + logger.debug( + "motor_send_command node=%d cmd=%s[%d] value=%r val_type=%s", + node_id, motor_command, index, value, val_type, ) cmd_u = motor_command.upper() @@ -3684,7 +3681,7 @@ async def motor_get_current_position(self, axis: KX2Axis) -> float: return self.convert_elbow_angle_to_position(elbow_angle_deg=raw / c) else: if c == 0: - print("node", axis, "has conversion factor of 0") + logger.warning("Axis %s has conversion factor of 0", axis) return 0 else: return raw / c @@ -3921,7 +3918,7 @@ async def motors_move_joint( cmd_vel_pct: float, cmd_accel_pct: float, ) -> None: - print("cmd", cmd_pos) + logger.debug("motors_move_joint cmd_pos=%s", cmd_pos) plan = await self.calculate_move_abs_all_axes( cmd_pos=cmd_pos, cmd_vel_pct=cmd_vel_pct, From 3029e9f8af645144da84abd2834a4a3439f20e49 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 15:18:02 -0700 Subject: [PATCH 08/93] scaffold KX2CanopenDriver (parallel class, not yet wired up) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/kx2_canopen_driver.py | 212 +++++++++++++++++++++++ 1 file changed, 212 insertions(+) create mode 100644 pylabrobot/paa/kx2/kx2_canopen_driver.py diff --git a/pylabrobot/paa/kx2/kx2_canopen_driver.py b/pylabrobot/paa/kx2/kx2_canopen_driver.py new file mode 100644 index 00000000000..b83d6b36fac --- /dev/null +++ b/pylabrobot/paa/kx2/kx2_canopen_driver.py @@ -0,0 +1,212 @@ +"""canopen-library-backed KX2 driver. + +Parallel implementation of :class:`KX2Driver` (the hand-rolled CAN transport in +``kx2_backend.py``). Built side-by-side so the legacy driver stays working +during development. When this class passes the hello-world notebook end-to-end +on real hardware, `kx2.py` will be switched over and the legacy driver deleted. + +Public method surface intentionally mirrors ``KX2Driver`` so ``KX2ArmBackend`` +can be pointed at either without any other code changes. +""" + +from __future__ import annotations + +import asyncio +import logging +from typing import Dict, List, Optional, Union + +import canopen + +from pylabrobot.capabilities.capability import BackendParams +from pylabrobot.device import Driver +from pylabrobot.paa.kx2.kx2_backend import ( + CanError, + CmdType, + ElmoObjectDataType, + KX2Axis, + MotorsMovePlan, + ValType, +) + +logger = logging.getLogger(__name__) + + +class KX2CanopenDriver(Driver): + """KX2 driver built on the `canopen` library. + + Uses `canopen.Network` for bus ownership + NMT, `node.sdo` for SDO traffic, + `node.tpdo`/`node.rpdo` for PDO mapping, and `network.send_message` / + `network.subscribe` for the vendor-specific Elmo binary interpreter + (non-standard, on TPDO2/RPDO2). + """ + + def __init__( + self, + has_rail: bool = False, + has_servo_gripper: bool = True, + interface: str = "pcan", + channel: Optional[str] = None, + bitrate: int = 500000, + ) -> None: + super().__init__() + self._interface = interface + self._channel = channel + self._bitrate = bitrate + + self.has_rail = has_rail + self.has_servo_gripper = has_servo_gripper + + self.node_id_list: List[int] = [1, 2, 3, 4] + if has_rail: + self.node_id_list.append(5) + if has_servo_gripper: + self.node_id_list.append(6) + + self._network: Optional[canopen.Network] = None + self._nodes: Dict[int, canopen.RemoteNode] = {} + self._loop: Optional[asyncio.AbstractEventLoop] = None + + # --- lifecycle ----------------------------------------------------------- + + async def setup(self, backend_params: Optional[BackendParams] = None) -> None: + """Bring up the CAN bus, reset & start all nodes, discover them.""" + if self._network is not None: + await self.stop() + + self._loop = asyncio.get_running_loop() + + network = canopen.Network() + network.connect(interface=self._interface, channel=self._channel, bitrate=self._bitrate) + self._network = network + + # Reset all nodes, then start scanner so bootup messages populate it, + # then start all nodes. + network.nmt.send_command(0x82) + await asyncio.sleep(0.5) + network.scanner.search() + network.nmt.send_command(0x01) + await asyncio.sleep(0.5) + + discovered = sorted(network.scanner.nodes) + if discovered != self.node_id_list: + raise CanError( + f"Node IDs on CAN bus do not match expected list: " + f"{discovered} != {self.node_id_list}" + ) + + for nid in self.node_id_list: + self._nodes[nid] = network.add_node(nid, canopen.ObjectDictionary()) + + logger.info("canopen: connected, nodes=%s", discovered) + + async def stop(self) -> None: + if self._network is not None: + self._network.disconnect() + self._network = None + self._nodes = {} + + # --- drive init (called by KX2ArmBackend._on_setup after setup()) -------- + + async def connect_part_two(self) -> None: + raise NotImplementedError + + # --- SDO ----------------------------------------------------------------- + + async def can_sdo_upload( + self, + node_id: int, + object_byte0: int, + object_byte1: int, + sub_index: int, + ) -> bytes: + raise NotImplementedError + + async def can_sdo_download( + self, + node_id: int, + object_byte0: int, + object_byte1: int, + sub_index: int, + data_byte: List[int], + ) -> None: + raise NotImplementedError + + async def can_sdo_upload_elmo_object( + self, + node_id: int, + elmo_object_int: int, + sub_index: int, + data_type: ElmoObjectDataType, + ) -> Union[str, int, float]: + raise NotImplementedError + + async def can_sdo_download_elmo_object( + self, + node_id: int, + elmo_object_int: int, + sub_index: int, + data: str, + data_type: ElmoObjectDataType, + ) -> None: + raise NotImplementedError + + # --- Elmo binary interpreter (vendor protocol on TPDO2/RPDO2) ------------ + + async def binary_interpreter( + self, + node_id: int, + cmd: str, + cmd_index: int, + cmd_type: CmdType, + value: str = "0", + val_type: ValType = ValType.Int, + low_priority: bool = False, + ) -> Union[str, float]: + raise NotImplementedError + + async def os_interpreter( + self, + node_id: int, + cmd: str, + *, + query: bool = False, + ) -> str: + raise NotImplementedError + + # --- DS402 / motor control ---------------------------------------------- + + async def motor_enable(self, axis: KX2Axis, state: bool) -> None: + raise NotImplementedError + + async def motor_emergency_stop(self, node_id: int) -> None: + raise NotImplementedError + + async def motor_get_current_position(self, node_id: int, pu: bool = False) -> int: + raise NotImplementedError + + async def motor_get_fault(self, axis: KX2Axis) -> Optional[str]: + raise NotImplementedError + + async def home_motor( + self, + axis: KX2Axis, + hs_offset: int, + ind_offset: int, + home_pos: int, + srch_vel: int, + srch_acc: int, + max_pe: int, + hs_pe: int, + offset_vel: int, + offset_acc: int, + timeout: float, + ) -> None: + raise NotImplementedError + + async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: + raise NotImplementedError + + # --- I/O ----------------------------------------------------------------- + + async def read_input(self, node_id: int, input_num: int) -> bool: + raise NotImplementedError From f93dc6c464271c50044cde4b0fae96898165273b Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 15:22:21 -0700 Subject: [PATCH 09/93] implement SDO on KX2CanopenDriver MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit `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) --- pylabrobot/paa/kx2/kx2_canopen_driver.py | 61 ++++++++++++++++++++++-- 1 file changed, 56 insertions(+), 5 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_canopen_driver.py b/pylabrobot/paa/kx2/kx2_canopen_driver.py index b83d6b36fac..fe44b6131d5 100644 --- a/pylabrobot/paa/kx2/kx2_canopen_driver.py +++ b/pylabrobot/paa/kx2/kx2_canopen_driver.py @@ -119,7 +119,11 @@ async def can_sdo_upload( object_byte1: int, sub_index: int, ) -> bytes: - raise NotImplementedError + index = (object_byte0 << 8) | object_byte1 + node = self._nodes[node_id] + # node.sdo.upload is blocking I/O (library handles expedited + segmented + # transfers + abort codes); run off the event loop. + return await asyncio.to_thread(node.sdo.upload, index, sub_index) async def can_sdo_download( self, @@ -129,7 +133,9 @@ async def can_sdo_download( sub_index: int, data_byte: List[int], ) -> None: - raise NotImplementedError + index = (object_byte0 << 8) | object_byte1 + node = self._nodes[node_id] + await asyncio.to_thread(node.sdo.download, index, sub_index, bytes(data_byte)) async def can_sdo_upload_elmo_object( self, @@ -137,8 +143,30 @@ async def can_sdo_upload_elmo_object( elmo_object_int: int, sub_index: int, data_type: ElmoObjectDataType, - ) -> Union[str, int, float]: - raise NotImplementedError + ) -> str: + obj_byte0 = elmo_object_int >> 8 + obj_byte1 = elmo_object_int & 0xFF + data_bytes = await self.can_sdo_upload(node_id, obj_byte0, obj_byte1, sub_index) + + if len(data_bytes) == 0: + return "" + if data_type == ElmoObjectDataType.UNSIGNED8: + return str(int.from_bytes(data_bytes[:1], "little", signed=False)) + if data_type == ElmoObjectDataType.UNSIGNED16: + return str(int.from_bytes(data_bytes[:2], "little", signed=False)) + if data_type == ElmoObjectDataType.UNSIGNED32: + return str(int.from_bytes(data_bytes[:4], "little", signed=False)) + if data_type == ElmoObjectDataType.UNSIGNED64: + return str(int.from_bytes(data_bytes[:8], "little", signed=False)) + if data_type == ElmoObjectDataType.INTEGER16: + return str(int.from_bytes(data_bytes[:2], "little", signed=True)) + if data_type == ElmoObjectDataType.INTEGER32: + return str(int.from_bytes(data_bytes[:4], "little", signed=True)) + if data_type == ElmoObjectDataType.INTEGER64: + return str(int.from_bytes(data_bytes[:8], "little", signed=True)) + if data_type == ElmoObjectDataType.STR: + return "".join(chr(b) for b in data_bytes) + raise CanError(f"Unsupported data type for SDO Read conversion: {data_type.name}") async def can_sdo_download_elmo_object( self, @@ -148,7 +176,30 @@ async def can_sdo_download_elmo_object( data: str, data_type: ElmoObjectDataType, ) -> None: - raise NotImplementedError + if data_type == ElmoObjectDataType.UNSIGNED8: + data_bytes = list(int(data).to_bytes(1, "little")) + elif data_type == ElmoObjectDataType.UNSIGNED16: + data_bytes = list(int(data).to_bytes(2, "little")) + elif data_type == ElmoObjectDataType.UNSIGNED32: + data_bytes = list(int(float(data)).to_bytes(4, "little")) + elif data_type == ElmoObjectDataType.UNSIGNED64: + data_bytes = list(int(data).to_bytes(8, "little")) + elif data_type == ElmoObjectDataType.INTEGER8: + data_bytes = list(int(data).to_bytes(1, "little", signed=True)) + elif data_type == ElmoObjectDataType.INTEGER16: + data_bytes = list(int(data).to_bytes(2, "little", signed=True)) + elif data_type == ElmoObjectDataType.INTEGER32: + data_bytes = list(int(float(data)).to_bytes(4, "little", signed=True)) + elif data_type == ElmoObjectDataType.INTEGER64: + data_bytes = list(int(data).to_bytes(8, "little", signed=True)) + elif data_type == ElmoObjectDataType.STR: + data_bytes = [ord(c) for c in data] + else: + raise CanError(f"Unsupported data type for SDO Write: {data_type.name}") + + obj_byte0 = elmo_object_int >> 8 + obj_byte1 = elmo_object_int & 0xFF + await self.can_sdo_download(node_id, obj_byte0, obj_byte1, sub_index, data_bytes) # --- Elmo binary interpreter (vendor protocol on TPDO2/RPDO2) ------------ From 95fb03e206a85797241dea1517ace04c7fbefa01 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 15:25:30 -0700 Subject: [PATCH 10/93] implement binary_interpreter on KX2CanopenDriver MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/kx2_canopen_driver.py | 141 ++++++++++++++++++++++- 1 file changed, 139 insertions(+), 2 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_canopen_driver.py b/pylabrobot/paa/kx2/kx2_canopen_driver.py index fe44b6131d5..f5186499f99 100644 --- a/pylabrobot/paa/kx2/kx2_canopen_driver.py +++ b/pylabrobot/paa/kx2/kx2_canopen_driver.py @@ -13,13 +13,15 @@ import asyncio import logging -from typing import Dict, List, Optional, Union +import struct +from typing import Dict, List, Optional, Tuple, Union import canopen from pylabrobot.capabilities.capability import BackendParams from pylabrobot.device import Driver from pylabrobot.paa.kx2.kx2_backend import ( + MOTION_AXES, CanError, CmdType, ElmoObjectDataType, @@ -28,6 +30,13 @@ ValType, ) +# Vendor-specific Elmo binary interpreter rides on PDO2 COB-IDs (non-standard). +# Request: RPDO2 = (6 << 7) | node_id = 0x300 + node_id +# Response: TPDO2 = (5 << 7) | node_id = 0x280 + node_id +_BI_REQUEST_COB_BASE = 0x300 +_BI_RESPONSE_COB_BASE = 0x280 +_GROUP_NODE_ID = 10 + logger = logging.getLogger(__name__) @@ -66,6 +75,12 @@ def __init__( self._nodes: Dict[int, canopen.RemoteNode] = {} self._loop: Optional[asyncio.AbstractEventLoop] = None + # Pending binary-interpreter response futures keyed by + # (node_id, msg_type, msg_index). Set from the canopen listener thread + # via loop.call_soon_threadsafe; only the event-loop thread touches + # this dict directly. + self._pending_bi: Dict[Tuple[int, str, int], asyncio.Future] = {} + # --- lifecycle ----------------------------------------------------------- async def setup(self, backend_params: Optional[BackendParams] = None) -> None: @@ -96,6 +111,8 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: for nid in self.node_id_list: self._nodes[nid] = network.add_node(nid, canopen.ObjectDictionary()) + # Elmo binary-interpreter response subscription. + network.subscribe(_BI_RESPONSE_COB_BASE + nid, self._make_bi_callback(nid)) logger.info("canopen: connected, nodes=%s", discovered) @@ -203,6 +220,35 @@ async def can_sdo_download_elmo_object( # --- Elmo binary interpreter (vendor protocol on TPDO2/RPDO2) ------------ + def _make_bi_callback(self, node_id: int): + """Return a `canopen.Network.subscribe` callback bound to a specific node.""" + + def _cb(cob_id: int, data: bytes, timestamp: float) -> None: + # Fires on canopen's listener thread. Marshal decoding into the loop. + if self._loop is None: + return + self._loop.call_soon_threadsafe(self._dispatch_bi_response, node_id, bytes(data)) + + return _cb + + def _dispatch_bi_response(self, node_id: int, data: bytes) -> None: + if len(data) < 8: + logger.warning("Binary interpreter response too short from node %d: %s", node_id, data.hex()) + return + msg_type = chr(data[0]) + chr(data[1]) + msg_index = ((data[3] & 0x3F) << 8) | data[2] + is_int = (data[3] & 0x80) == 0 + if is_int: + (val,) = struct.unpack(" Union[str, float]: - raise NotImplementedError + del low_priority # request priority is not meaningful over canopen.Network.send_message + if self._network is None: + raise CanError("binary_interpreter called before setup()") + if value == "": + value = "0" + + timeout = 10.0 if cmd.upper() == "SV" else 1.0 + is_float = val_type == ValType.Float + is_query = cmd_type == CmdType.ValQuery + is_execute = cmd_type == CmdType.Execute + + # -- build the 8-byte request -- + byte0 = ord(cmd[0]) & 0xFF + byte1 = ord(cmd[-1]) & 0xFF + byte2 = cmd_index & 0xFF + byte3 = (cmd_index >> 8) & 0x3F + if is_query: + byte3 |= 0x40 + if is_float: + byte3 |= 0x80 + if is_float: + val_bytes = struct.pack(" bool: + try: + expected = float(expected_str) + actual = float(actual_str) + except ValueError: + return False + if actual == 0.0: + return expected == 0.0 + ratio = expected / actual + return expected == actual or (0.99 < ratio < 1.01) + + # -- dispatch: single node vs. group -- + target_nodes = ( + [int(a) for a in MOTION_AXES] if node_id == _GROUP_NODE_ID else [node_id] + ) + + futures: List[asyncio.Future] = [] + for nid in target_nodes: + key = (nid, cmd, cmd_index) + # If a stale pending future exists, drop it. + old = self._pending_bi.pop(key, None) + if old is not None and not old.done(): + old.cancel() + fut = self._loop.create_future() if self._loop else asyncio.get_event_loop().create_future() + self._pending_bi[key] = fut + futures.append(fut) + + self._network.send_message(_BI_REQUEST_COB_BASE + node_id, data_to_send) + + try: + resps = await asyncio.wait_for(asyncio.gather(*futures), timeout=timeout) + except asyncio.TimeoutError: + for nid in target_nodes: + self._pending_bi.pop((nid, cmd, cmd_index), None) + raise CanError( + f"Timeout waiting for response to {cmd}[{cmd_index}] from node {node_id}" + ) + + # -- interpret responses -- + if is_query: + if node_id == _GROUP_NODE_ID: + value = ",".join(str(r) for r in resps) + else: + value = str(resps[0]) + return float(value) if is_float else int(float(value)) + + if is_execute: + if any(r == "" for r in resps): + missing = [nid for nid, r in zip(target_nodes, resps) if r == ""] + raise CanError(f"No execute response from nodes {missing} for {cmd}[{cmd_index}]") + return float(value) if is_float else int(float(value)) + + # Write: verify each echoed value matches the one we sent. + for nid, resp in zip(target_nodes, resps): + if is_float: + ok = _float_matches(value, str(resp)) + else: + ok = int(float(resp)) == int(float(value)) + if not ok: + raise CanError( + f"Unexpected CAN response: sent {cmd}[{cmd_index}]={value}, " + f"got {resp} from node {nid}" + ) + return float(value) if is_float else int(float(value)) async def os_interpreter( self, From 9344217b696d844d4e7956f60d855e779804424c Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 15:28:32 -0700 Subject: [PATCH 11/93] implement motor primitives, DS402 controlword, PVT mode on KX2CanopenDriver MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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, )` 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) --- pylabrobot/paa/kx2/kx2_canopen_driver.py | 271 ++++++++++++++++++++++- 1 file changed, 260 insertions(+), 11 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_canopen_driver.py b/pylabrobot/paa/kx2/kx2_canopen_driver.py index f5186499f99..adc6f3b764f 100644 --- a/pylabrobot/paa/kx2/kx2_canopen_driver.py +++ b/pylabrobot/paa/kx2/kx2_canopen_driver.py @@ -25,7 +25,10 @@ CanError, CmdType, ElmoObjectDataType, + HomeStatus, + JointMoveDirection, KX2Axis, + MotorMoveParam, MotorsMovePlan, ValType, ) @@ -81,6 +84,14 @@ def __init__( # this dict directly. self._pending_bi: Dict[Tuple[int, str, int], asyncio.Future] = {} + self._pvt_mode: bool = False + self.EmcyMoveErrorReceived: bool = False + self.grp_id: int = 0 + # Move-done futures per axis; resolved by TPDO4 digital-input callbacks + # once PDO mapping lands. Until then, `_wait_for_moves_done` falls back + # to polling `motor_check_if_move_done`. + self._waiting_moves: Dict[KX2Axis, asyncio.Future] = {} + # --- lifecycle ----------------------------------------------------------- async def setup(self, backend_params: Optional[BackendParams] = None) -> None: @@ -361,19 +372,249 @@ async def os_interpreter( ) -> str: raise NotImplementedError - # --- DS402 / motor control ---------------------------------------------- + # --- raw CANopen sends (SYNC + RPDO1 controlword) ----------------------- - async def motor_enable(self, axis: KX2Axis, state: bool) -> None: - raise NotImplementedError + async def can_sync(self) -> None: + if self._network is None: + raise CanError("can_sync called before setup()") + # SYNC object (0x080), no data. + self._network.send_message(0x80, b"") + + async def control_word_set(self, node_id: int, value: int, sync: bool = True) -> None: + if self._network is None: + raise CanError("control_word_set called before setup()") + val_bytes = value.to_bytes(2, byteorder="little") + # RPDO1 COB-ID = (4 << 7) | node_id = 0x200 + node_id + self._network.send_message(0x200 + node_id, val_bytes) + if sync: + await self.can_sync() + + # --- DS402 / motor control ---------------------------------------------- async def motor_emergency_stop(self, node_id: int) -> None: - raise NotImplementedError + await self.binary_interpreter(node_id, "MO", 0, CmdType.ValSet, "0") async def motor_get_current_position(self, node_id: int, pu: bool = False) -> int: - raise NotImplementedError + cmd = "PU" if pu else "PX" + val_str = await self.binary_interpreter(int(node_id), cmd, 0, CmdType.ValQuery) + return int(round(float(val_str))) + + async def motor_get_motion_status(self, node_id: int) -> int: + val = await self.binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) + return int(round(float(val))) + + async def motor_set_move_direction( + self, node_id: int, direction: JointMoveDirection + ) -> None: + val_str = "1" + if direction == JointMoveDirection.Clockwise: + val_str = "65" + elif direction == JointMoveDirection.Counterclockwise: + val_str = "129" + elif direction == JointMoveDirection.ShortestWay: + val_str = "193" + await self.can_sdo_download_elmo_object( + node_id, 24818, 0, val_str, ElmoObjectDataType.UNSIGNED16 + ) + + async def motor_set_homed_status(self, axis: KX2Axis, status: HomeStatus) -> None: + val = "0" + if status == HomeStatus.Homed: + val = "1" + elif status == HomeStatus.InitializedWithoutHoming: + val = "2" + await self.binary_interpreter(int(axis), "UI", 3, CmdType.ValSet, val) + + async def motor_get_homed_status(self, node_id: int) -> HomeStatus: + left = await self.binary_interpreter(node_id, "UI", 3, CmdType.ValQuery) + if left == 1: + return HomeStatus.Homed + if left == 2: + return HomeStatus.InitializedWithoutHoming + return HomeStatus.NotHomed + + async def motor_reset_encoder_position(self, axis: KX2Axis, position: float) -> None: + await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, str(position)) + await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") + + async def motor_check_if_move_done(self, node_id: int) -> bool: + ms_val = await self.binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) + if ms_val == 0: + return True + if ms_val == 1: + mo_val = await self.binary_interpreter(node_id, "MO", 0, CmdType.ValQuery) + if mo_val == 1: + return True + fault = await self.motor_get_fault(node_id) + if fault is not None: + raise RuntimeError(f"Motor Fault: {fault}") + raise RuntimeError("Motor Fault (Unknown)") + if ms_val == 2: + return False + return False async def motor_get_fault(self, axis: KX2Axis) -> Optional[str]: - raise NotImplementedError + val = await self.binary_interpreter(int(axis), "MF", 0, CmdType.ValQuery) + if val == 0: + return None + assert isinstance(val, int) + + faults: list[str] = [] + bit_msgs = { + 0x0001: "Motor Hall sensor feedback angle not found yet.", + 0x0004: "Feedback loss: no match between encoder and Hall location.", + 0x0008: "The peak current has been exceeded.", + 0x0010: "Inhibit.", + 0x0040: "Two digital Hall sensors were changed at the same time.", + 0x0080: "Speed tracking error.", + 0x0100: "Position tracking error.", + 0x0200: "Inconsistent drive database.", + 0x0400: "Too large a difference in ECAM table.", + 0x0800: "CAN heartbeat failure.", + 0x1000: "Servo drive fault.", + 0x010000: "Failed to find the electrical zero of the motor during startup.", + 0x020000: "Speed limit exceeded.", + 0x040000: "Drive CPU stack overflow.", + 0x080000: "Drive CPU exception.", + 0x200000: "Motor stuck.", + 0x400000: "Position limit exceeded.", + 0x20000000: "Cannot start motor.", + } + for bit, msg in bit_msgs.items(): + if val & bit: + faults.append(msg) + + b13 = bool(val & 0x2000) + b14 = bool(val & 0x4000) + b15 = bool(val & 0x8000) + if (not b15) and (not b14) and b13: + faults.append("Power supply under voltage.") + if (not b15) and b14 and (not b13): + faults.append("Power supply over voltage.") + if b15 and (not b14) and b13: + faults.append("Motor lead short circuit or faulty drive.") + if b15 and b14 and (not b13): + faults.append("Drive overheated.") + + if not faults: + return f"Unknown fault code: {val} (0x{val:08X})" + return " ".join(faults) + + async def motor_enable(self, axis: KX2Axis, state: bool) -> None: + if not isinstance(axis, KX2Axis): + raise TypeError(f"axis must be KX2Axis, got {type(axis).__name__}") + + # Motion axes use the DS402 controlword sequence over RPDO1; the gripper + # (and any non-motion axis) uses the binary-interpreter MO command. + use_bi = not (axis in MOTION_AXES or int(axis) == self.grp_id) + + if state: + self.EmcyMoveErrorReceived = False + if use_bi: + await self.binary_interpreter(axis, "MO", 0, CmdType.ValSet, "1") + else: + for cw in (0, 128, 6, 7, 15): + await self.control_word_set(node_id=int(axis), value=cw) + await asyncio.sleep(0.1) + left = await self.binary_interpreter( + node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery + ) + if left != 1: + raise CanError(f"Motor failed to enable (axis = {axis})") + else: + if use_bi: + try: + await self.binary_interpreter( + node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValSet, value="0" + ) + except Exception: + pass + else: + await self.control_word_set(node_id=int(axis), value=7) + await self.control_word_set(node_id=int(axis), value=6) + await asyncio.sleep(0.1) + left = await self.binary_interpreter( + node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery + ) + if left != 0: + raise RuntimeError(f"Motor failed to disable (axis = {axis})") + + # --- motion primitives -------------------------------------------------- + + async def pvt_select_mode(self, enable: bool) -> None: + """Enable/disable PVT mode on all motion axes via standard SDO writes.""" + if enable: + if not self._pvt_mode: + for axis in MOTION_AXES: + # 0x60C4 sub 6 = 0 (disable interpolation buffer) + await self.can_sdo_download(int(axis), 0x60, 0xC4, 0x06, [0]) + # 0x6060 = 7 (interpolated position mode) + await self.can_sdo_download(int(axis), 0x60, 0x60, 0x00, [7]) + self._pvt_mode = True + else: + for axis in MOTION_AXES: + await self.can_sdo_download(int(axis), 0x60, 0x60, 0x00, [1]) + else: + if self._pvt_mode: + for axis in MOTION_AXES: + # 0x6060 = 1 (profile position mode) + await self.can_sdo_download(int(axis), 0x60, 0x60, 0x00, [1]) + self._pvt_mode = False + + async def _wait_for_moves_done(self, timeout: float) -> None: + async def _one(axis: KX2Axis) -> None: + try: + await asyncio.wait_for(self._waiting_moves[axis], timeout=timeout) + except asyncio.TimeoutError: + pass + # Fallback query in case the digital-input edge was missed (or TPDO + # mapping hasn't landed yet on this driver). + await self.motor_check_if_move_done(int(axis)) + + await asyncio.gather(*(_one(axis) for axis in self._waiting_moves.keys())) + + async def _motors_move_start( + self, axes: List[KX2Axis], *, relative: bool = False + ) -> None: + assert self._loop is not None + self._waiting_moves = {ax: self._loop.create_future() for ax in axes} + relative_bit = 0x40 if relative else 0 + for i, nid in enumerate(axes): + last = i == (len(axes) - 1) + await self.control_word_set(int(nid), 47 + relative_bit, sync=last) + for i, nid in enumerate(axes): + last = i == (len(axes) - 1) + await self.control_word_set(int(nid), 47 + 0x10 + relative_bit, sync=last) + + async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: + await self.pvt_select_mode(False) + + for move in plan.moves: + await self.motor_set_move_direction(move.axis.value, move.direction) + # 0x607A = Target Position (24698 decimal) + await self.can_sdo_download_elmo_object( + move.axis.value, 24698, 0, str(int(move.position)), ElmoObjectDataType.INTEGER32, + ) + # 0x6081 = Profile Velocity (24705 decimal) + await self.can_sdo_download_elmo_object( + move.axis.value, 24705, 0, str(int(move.velocity)), ElmoObjectDataType.UNSIGNED32, + ) + acc = max(int(move.acceleration), 100) + # 0x6083 = Profile Acceleration (24707 decimal) + await self.can_sdo_download_elmo_object( + move.axis.value, 24707, 0, str(acc), ElmoObjectDataType.UNSIGNED32, + ) + # 0x6084 = Profile Deceleration (24708 decimal) + await self.can_sdo_download_elmo_object( + move.axis.value, 24708, 0, str(acc), ElmoObjectDataType.UNSIGNED32, + ) + + await self._motors_move_start([move.axis for move in plan.moves]) + await self._wait_for_moves_done(timeout=plan.move_time + 2) async def home_motor( self, @@ -389,12 +630,20 @@ async def home_motor( offset_acc: int, timeout: float, ) -> None: - raise NotImplementedError - - async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: - raise NotImplementedError + raise NotImplementedError("home_motor: pending os_interpreter + hard-stop/index search port") # --- I/O ----------------------------------------------------------------- async def read_input(self, node_id: int, input_num: int) -> bool: - raise NotImplementedError + left = await self.binary_interpreter(node_id, "IB", input_num, CmdType.ValQuery) + return left == 1 + + async def read_output(self, node_id: int, output_num: int) -> bool: + expression = await self.binary_interpreter(node_id, "OP", 0, CmdType.ValQuery) + val = int(expression) + mask = 1 << (output_num - 1) + return (val & mask) == mask + + async def set_output(self, node_id: int, output_num: int, state: bool) -> Union[str, float]: + val = "1" if state else "0" + return await self.binary_interpreter(node_id, "OB", output_num, CmdType.ValSet, val) From 0845a026e36210e5af1e784bdf084b47d6c3510a Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 15:30:48 -0700 Subject: [PATCH 12/93] implement connect_part_two + PDO mapping on KX2CanopenDriver MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/kx2_canopen_driver.py | 172 ++++++++++++++++++++++- 1 file changed, 171 insertions(+), 1 deletion(-) diff --git a/pylabrobot/paa/kx2/kx2_canopen_driver.py b/pylabrobot/paa/kx2/kx2_canopen_driver.py index adc6f3b764f..50f9cb51648 100644 --- a/pylabrobot/paa/kx2/kx2_canopen_driver.py +++ b/pylabrobot/paa/kx2/kx2_canopen_driver.py @@ -22,6 +22,7 @@ from pylabrobot.device import Driver from pylabrobot.paa.kx2.kx2_backend import ( MOTION_AXES, + COBType, CanError, CmdType, ElmoObjectDataType, @@ -30,9 +31,19 @@ KX2Axis, MotorMoveParam, MotorsMovePlan, + PDOTransmissionType, + RPDO, + RPDOMappedObject, + TPDO, + TPDOMappedObject, + TPDOTrigger, ValType, ) + +def _u32_le(value: int) -> List[int]: + return list((value & 0xFFFFFFFF).to_bytes(4, byteorder="little", signed=False)) + # Vendor-specific Elmo binary interpreter rides on PDO2 COB-IDs (non-standard). # Request: RPDO2 = (6 << 7) | node_id = 0x300 + node_id # Response: TPDO2 = (5 << 7) | node_id = 0x280 + node_id @@ -136,7 +147,166 @@ async def stop(self) -> None: # --- drive init (called by KX2ArmBackend._on_setup after setup()) -------- async def connect_part_two(self) -> None: - raise NotImplementedError + """Configure PDO mapping + Elmo DS402 parameters after the CAN bus is up. + + Mirrors the legacy driver: unmap TPDO1, map TPDO3 (StatusWord, triggered + on MotionComplete) and TPDO4 (DigitalInputs, triggered on edge). Then + program Elmo vendor objects that set interpolation config, and finally + map RPDO1 (ControlWord) and RPDO3 (interpolated target position+velocity) + per motion axis. Subscribe to each node's TPDO3 cob_id so move-done + completes `_waiting_moves` futures. + """ + assert self._network is not None + + for node_id in self.node_id_list: + await self.can_tpdo_unmap(TPDO.TPDO1, node_id) + await self._tpdo_map( + TPDO.TPDO3, node_id, [TPDOMappedObject.StatusWord], TPDOTrigger.MotionComplete + ) + await self._tpdo_map( + TPDO.TPDO4, node_id, [TPDOMappedObject.DigitalInputs], TPDOTrigger.DigitalInputEvent + ) + + for axis in MOTION_AXES: + await self.can_sdo_download_elmo_object( + int(axis), 24768, 0, "-1", ElmoObjectDataType.INTEGER16 + ) + await self.can_sdo_download_elmo_object( + int(axis), 24772, 2, "16", ElmoObjectDataType.UNSIGNED32 + ) + await self.can_sdo_download_elmo_object( + int(axis), 24772, 3, "0", ElmoObjectDataType.UNSIGNED8 + ) + await self.can_sdo_download_elmo_object( + int(axis), 24772, 5, "8", ElmoObjectDataType.UNSIGNED8 + ) + await self.can_sdo_download_elmo_object( + int(axis), 24770, 2, "-3", ElmoObjectDataType.INTEGER8 + ) + await self.can_sdo_download_elmo_object( + int(axis), 24669, 0, "1", ElmoObjectDataType.INTEGER16 + ) + + for axis in MOTION_AXES: + await self._rpdo_map( + RPDO.RPDO1, int(axis), [RPDOMappedObject.ControlWord], + PDOTransmissionType.SynchronousCyclic, + ) + await self._rpdo_map( + RPDO.RPDO3, int(axis), + [RPDOMappedObject.TargetPositionIP, RPDOMappedObject.TargetVelocityIP], + PDOTransmissionType.EventDrivenDev, + ) + + # TPDO3 subscription: StatusWord frames arrive on MotionComplete trigger, + # so any TPDO3 on an axis with a pending waiting_move completes it. + for nid in self.node_id_list: + tpdo3_cob = ((int(COBType.TPDO3) & 0x0F) << 7) | (nid & 0x7F) + self._network.subscribe(tpdo3_cob, self._make_tpdo3_callback(nid)) + + self._pvt_mode = True + await self.pvt_select_mode(False) + + def _make_tpdo3_callback(self, node_id: int): + def _cb(cob_id: int, data: bytes, timestamp: float) -> None: + if self._loop is None: + return + self._loop.call_soon_threadsafe(self._dispatch_tpdo3, node_id) + return _cb + + def _dispatch_tpdo3(self, node_id: int) -> None: + axis = KX2Axis(node_id) if node_id in {a.value for a in KX2Axis} else None + if axis is None: + return + fut = self._waiting_moves.get(axis) + if fut is not None and not fut.done(): + fut.set_result(None) + + # --- PDO configuration (pure SDO writes; no library-PDO machinery) ------ + + async def can_tpdo_unmap(self, tpdo: TPDO, node_id: int) -> None: + cob_type_int = { + TPDO.TPDO1: COBType.TPDO1.value, + TPDO.TPDO3: COBType.TPDO3.value, + TPDO.TPDO4: COBType.TPDO4.value, + }[tpdo] + node_id &= 0x7F + num1 = ((cob_type_int & 0x01) << 7) | node_id + num2 = (cob_type_int >> 1) & 0x07 + await self.can_sdo_download(node_id, 0x18, tpdo.value - 1, 1, [num1, num2, 0, 0xC0]) + await self.can_sdo_download(node_id, 0x1A, tpdo.value - 1, 0, [0, 0, 0, 0]) + + async def _rpdo_map( + self, + rpdo: RPDO, + node_id: int, + mapped_objects: List[RPDOMappedObject], + transmission_type: PDOTransmissionType, + ) -> None: + rpdo_idx = (int(rpdo) - 1) & 0xFF + cob_type = { + RPDO.RPDO1: COBType.RPDO1, RPDO.RPDO3: COBType.RPDO3, RPDO.RPDO4: COBType.RPDO4, + }[rpdo] + cob_id_11 = ((int(cob_type) & 0x0F) << 7) | (node_id & 0x7F) + + # Disable PDO (bit 31 set) + await self.can_sdo_download(node_id, 0x14, rpdo_idx, 1, _u32_le(0x80000000 | cob_id_11)) + # Clear mapping count + await self.can_sdo_download(node_id, 0x16, rpdo_idx, 0, [0, 0, 0, 0]) + # Transmission type + await self.can_sdo_download( + node_id, 0x14, rpdo_idx, 2, [int(transmission_type) & 0xFF, 0, 0, 0] + ) + # Mapped objects + for i, mo in enumerate(mapped_objects): + await self.can_sdo_download(node_id, 0x16, rpdo_idx, i + 1, _u32_le(int(mo))) + # Mapping count + await self.can_sdo_download( + node_id, 0x16, rpdo_idx, 0, [len(mapped_objects) & 0xFF, 0, 0, 0] + ) + # Re-enable (clear bit 31) + await self.can_sdo_download(node_id, 0x14, rpdo_idx, 1, _u32_le(cob_id_11)) + + async def _tpdo_map( + self, + tpdo: TPDO, + node_id: int, + mapped_objects: List[TPDOMappedObject], + event_trigger: TPDOTrigger, + event_timer_ms: int = 0, + delay_100_us: int = 0, + transmission_type: PDOTransmissionType = PDOTransmissionType.EventDrivenDev, + ) -> None: + tpdo_idx = (int(tpdo) - 1) & 0xFF + cob_type = { + TPDO.TPDO1: COBType.TPDO1, TPDO.TPDO3: COBType.TPDO3, TPDO.TPDO4: COBType.TPDO4, + }[tpdo] + cob_id_11 = ((int(cob_type) & 0x0F) << 7) | (node_id & 0x7F) + event_mask = 1 << int(event_trigger) + + # Disable TPDO (bit 30 + 31) + await self.can_sdo_download(node_id, 0x18, tpdo_idx, 1, _u32_le(0xC0000000 | cob_id_11)) + # Clear mapping count + await self.can_sdo_download(node_id, 0x1A, tpdo_idx, 0, [0, 0, 0, 0]) + # Transmission type + await self.can_sdo_download( + node_id, 0x18, tpdo_idx, 2, [int(transmission_type) & 0xFF, 0, 0, 0] + ) + # Inhibit / delay 100us + await self.can_sdo_download(node_id, 0x18, tpdo_idx, 3, [delay_100_us & 0xFF, 0, 0, 0]) + # Event timer (ms) + await self.can_sdo_download(node_id, 0x18, tpdo_idx, 5, [event_timer_ms & 0xFF, 0, 0, 0]) + # Vendor event mask at 0x2F20: + await self.can_sdo_download(node_id, 0x2F, 0x20, int(tpdo) & 0xFF, _u32_le(event_mask)) + # Mapped objects + for i, mo in enumerate(mapped_objects): + await self.can_sdo_download(node_id, 0x1A, tpdo_idx, i + 1, _u32_le(int(mo))) + # Mapping count + await self.can_sdo_download( + node_id, 0x1A, tpdo_idx, 0, [len(mapped_objects) & 0xFF, 0, 0, 0] + ) + # Re-enable (clear bits 30 + 31) + await self.can_sdo_download(node_id, 0x18, tpdo_idx, 1, _u32_le(cob_id_11)) # --- SDO ----------------------------------------------------------------- From 6095dec87e716771cc45e97d6575c23a8601a2ba Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 15:34:06 -0700 Subject: [PATCH 13/93] implement os_interpreter + homing on KX2CanopenDriver MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit `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) --- pylabrobot/paa/kx2/kx2_canopen_driver.py | 225 ++++++++++++++++++++++- 1 file changed, 223 insertions(+), 2 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_canopen_driver.py b/pylabrobot/paa/kx2/kx2_canopen_driver.py index 50f9cb51648..c50db7639f8 100644 --- a/pylabrobot/paa/kx2/kx2_canopen_driver.py +++ b/pylabrobot/paa/kx2/kx2_canopen_driver.py @@ -14,6 +14,7 @@ import asyncio import logging import struct +import time from typing import Dict, List, Optional, Tuple, Union import canopen @@ -540,7 +541,38 @@ async def os_interpreter( *, query: bool = False, ) -> str: - raise NotImplementedError + """Run an OS interpreter command via standard CiA-301 OS Command objects. + + Uses 0x1024 (OS Command Mode) + 0x1023 (OSCommand record) — the library + handles the expedited vs. segmented SDO choice and toggle-bit dance + automatically, replacing ~260 lines of hand-rolled segmented SDO in the + legacy driver. + """ + if node_id not in self._nodes: + raise CanError(f"os_interpreter: unknown node {node_id}") + node = self._nodes[node_id] + + # 0x1024:0 = OS Command Mode. Elmo/legacy code sets this to 0 ("evaluate + # immediately") before each command. + await asyncio.to_thread(node.sdo.download, 0x1024, 0, bytes([0])) + + # 0x1023:1 = OSCommand.Command. ASCII-encoded; library segments if >4 bytes. + await asyncio.to_thread(node.sdo.download, 0x1023, 1, cmd.encode("ascii")) + + # 0x1023:2 = OSCommand.Status (U8). Nonzero indicates an error. + status_bytes = await asyncio.to_thread(node.sdo.upload, 0x1023, 2) + status = int.from_bytes(status_bytes[:1], "little") + if status != 0: + raise CanError( + f"OS Interpreter command '{cmd}' returned status {status} from node {node_id}" + ) + + if not query: + return "" + + # 0x1023:3 = OSCommand.Reply (DOMAIN / string). Library handles segmented. + reply = await asyncio.to_thread(node.sdo.upload, 0x1023, 3) + return reply.decode("ascii", errors="replace").rstrip("\x00").rstrip() # --- raw CANopen sends (SYNC + RPDO1 controlword) ----------------------- @@ -786,6 +818,148 @@ async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: await self._motors_move_start([move.axis for move in plan.moves]) await self._wait_for_moves_done(timeout=plan.move_time + 2) + async def user_program_run( + self, + axis: KX2Axis, + user_function: str, + params=None, + timeout_sec: int = 0, + wait_until_done: bool = False, + ) -> int: + if not isinstance(axis, int): + raise ValueError("axis must be int") + if axis < 0 or axis > 255: + raise ValueError("axis must be in [0, 255]") + node_id = int(axis) + + ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + if ps == -2: + raise CanError(f"Axis {axis}: controller reported PS=-2 (not ready / unavailable)") + + if ps != -1: + await self.binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="0") + t0 = time.monotonic() + while (time.monotonic() - t0) < 3.0: + ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + if ps == -1: + break + await asyncio.sleep(0.01) + else: + raise CanError(f"Axis {axis}: did not reach idle state (PS=-1) within 3s (last PS={ps})") + + arg_str = "" + if params: + parts = [str(p) for p in params] + if parts: + arg_str = f"({','.join(parts)})" + + await self.binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="1") + + cmd = f"XQ##{user_function}{arg_str}" + logger.debug("user_program_run: %s", cmd) + await self.os_interpreter(node_id, cmd, query=False) + + last_line_completed = 0 + if wait_until_done: + t0 = time.monotonic() + ps = 1 + ui1 = 1 + while ps == 1 and ui1 == 1 and (time.monotonic() - t0) < float(timeout_sec): + ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + ui1 = int(await self.binary_interpreter(node_id, "UI", 1, CmdType.ValQuery)) + await asyncio.sleep(0.01) + + expr_raw = await self.binary_interpreter(node_id, "UI", 2, CmdType.ValQuery) + try: + last_line_completed = int(str(expr_raw).strip()) + except Exception: + last_line_completed = 0 + + if ui1 != 0: + raise CanError( + f"Axis {axis}: user program ended with UI[1]={ui1} (expected 0), " + f"last_line={last_line_completed}" + ) + if ps == 1 and ui1 == 1: + raise CanError( + f"Axis {axis}: timeout waiting for '{user_function}' after {timeout_sec}s, " + f"last_line={last_line_completed}" + ) + + return 0 + + async def motor_hard_stop_search( + self, + axis: KX2Axis, + srch_vel: int, + srch_acc: int, + max_pe: int, + hs_pe: int, + timeout: float, + ) -> None: + await self.binary_interpreter(int(axis), "ER", 3, CmdType.ValSet, str(max_pe * 10)) + await self.binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) + await self.binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) + for i in [3, 4, 5, 2]: + await self.binary_interpreter(int(axis), "HM", i, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "JV", 0, CmdType.ValSet, str(srch_vel)) + + try: + params = [str(int(hs_pe)), str(int(timeout * 1000))] + last_line = await self.user_program_run(axis, "Home", params, int(timeout), True) + if last_line in [1, 2, 3]: + raise RuntimeError(f"Homing Script Error {34 + last_line}") + + curr_pos = await self.motor_get_current_position(int(axis)) + await self.binary_interpreter(int(axis), "PA", 0, CmdType.ValSet, str(curr_pos)) + await self.binary_interpreter(int(axis), "SP", 0, CmdType.ValSet, str(srch_vel)) + await self.binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) + await self.binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) + finally: + await asyncio.sleep(0.3) + await self.binary_interpreter(int(axis), "BG", 0, CmdType.Execute, value="0") + await asyncio.sleep(0.3) + await self.binary_interpreter(int(axis), "ER", 3, CmdType.ValSet, str(int(max_pe))) + + async def motor_index_search( + self, + axis: KX2Axis, + srch_vel: int, + srch_acc: int, + positive_direction: bool, + timeout: float, + ) -> Tuple[int, int]: + assert self._loop is not None + await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") + + rev = await self.binary_interpreter(int(axis), "CA", 18, CmdType.ValQuery) + one_revolution = int(float(rev)) + if not positive_direction: + one_revolution *= -1 + + await self.binary_interpreter(int(axis), "PR", 1, CmdType.ValSet, str(one_revolution)) + await self.binary_interpreter(int(axis), "SP", 0, CmdType.ValSet, str(srch_vel)) + await self.binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) + await self.binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) + + await self.binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "3") # index only + await self.binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, "0") + await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") # arm + + self._waiting_moves = {axis: self._loop.create_future()} + await self.binary_interpreter(int(axis), "BG", 0, CmdType.Execute) + await self._wait_for_moves_done(timeout) + + left = await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValQuery) + if left != 0: + raise RuntimeError("Homing Failure: Failed to finish index pulse search.") + + cap = await self.binary_interpreter(int(axis), "HM", 7, CmdType.ValQuery) + captured_position = int(float(cap)) + return one_revolution, captured_position + async def home_motor( self, axis: KX2Axis, @@ -800,7 +974,54 @@ async def home_motor( offset_acc: int, timeout: float, ) -> None: - raise NotImplementedError("home_motor: pending os_interpreter + hard-stop/index search port") + left = await self.binary_interpreter(int(axis), "CA", 41, CmdType.ValQuery) + if left == 24: + raise RuntimeError("Error 43") + + try: + await self.motor_hard_stop_search(axis, srch_vel, srch_acc, max_pe, hs_pe, timeout) + except Exception as e: + fault = await self.motor_get_fault(axis) + if fault is not None: + raise RuntimeError(fault) + raise e + + await self.motor_enable(axis=axis, state=True) + + await self.motors_move_absolute_execute( + plan=MotorsMovePlan( + moves=[ + MotorMoveParam( + axis=KX2Axis(axis), + position=hs_offset, + velocity=offset_vel, + acceleration=offset_acc, + relative=False, + direction=JointMoveDirection.ShortestWay, + ) + ], + ) + ) + + is_positive = hs_offset > 0 + await self.motor_index_search(axis, abs(srch_vel), srch_acc, is_positive, timeout) + + await self.motors_move_absolute_execute( + plan=MotorsMovePlan( + moves=[ + MotorMoveParam( + axis=KX2Axis(axis), + position=ind_offset, + velocity=offset_vel, + acceleration=offset_acc, + relative=False, + direction=JointMoveDirection.ShortestWay, + ) + ] + ) + ) + await self.motor_reset_encoder_position(axis, home_pos) + await self.motor_set_homed_status(axis, HomeStatus.Homed) # --- I/O ----------------------------------------------------------------- From 2e1084a6c15a4146687bfa68e464bd69f0c330e5 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 15:34:48 -0700 Subject: [PATCH 14/93] expose KX2Canopen device alongside legacy KX2 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit `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) --- pylabrobot/paa/kx2/__init__.py | 3 ++- pylabrobot/paa/kx2/kx2.py | 23 ++++++++++++++++++++++- 2 files changed, 24 insertions(+), 2 deletions(-) diff --git a/pylabrobot/paa/kx2/__init__.py b/pylabrobot/paa/kx2/__init__.py index 4f16a1c70e2..511b59a3013 100644 --- a/pylabrobot/paa/kx2/__init__.py +++ b/pylabrobot/paa/kx2/__init__.py @@ -1,2 +1,3 @@ -from pylabrobot.paa.kx2.kx2 import KX2 +from pylabrobot.paa.kx2.kx2 import KX2, KX2Canopen from pylabrobot.paa.kx2.kx2_backend import KX2ArmBackend, KX2Axis, KX2Driver +from pylabrobot.paa.kx2.kx2_canopen_driver import KX2CanopenDriver diff --git a/pylabrobot/paa/kx2/kx2.py b/pylabrobot/paa/kx2/kx2.py index 64e458cda8f..ec0741be7f5 100644 --- a/pylabrobot/paa/kx2/kx2.py +++ b/pylabrobot/paa/kx2/kx2.py @@ -1,11 +1,12 @@ from pylabrobot.capabilities.arms.orientable_arm import OrientableArm from pylabrobot.device import Device from pylabrobot.paa.kx2.kx2_backend import KX2ArmBackend, KX2Driver +from pylabrobot.paa.kx2.kx2_canopen_driver import KX2CanopenDriver from pylabrobot.resources.resource import Resource class KX2(Device): - """PAA KX2 robotic plate handler.""" + """PAA KX2 robotic plate handler (legacy hand-rolled CAN driver).""" def __init__(self) -> None: driver = KX2Driver() @@ -15,3 +16,23 @@ def __init__(self) -> None: self.reference = Resource(name="KX2", size_x=200, size_y=200, size_z=200) self.arm = OrientableArm(backend=backend, reference_resource=self.reference) self._capabilities = [self.arm] + + +class KX2Canopen(Device): + """PAA KX2 robotic plate handler (canopen-library driver). + + Drop-in replacement for :class:`KX2` using :class:`KX2CanopenDriver` + underneath. Public API is identical — both wrap the same + `KX2ArmBackend` capability backend. Prefer this once the hello-world + notebook runs clean against it on hardware; the legacy class will be + removed afterwards. + """ + + def __init__(self) -> None: + driver = KX2CanopenDriver() + super().__init__(driver=driver) + self.driver: KX2CanopenDriver = driver + backend = KX2ArmBackend(driver=driver) + self.reference = Resource(name="KX2", size_x=200, size_y=200, size_z=200) + self.arm = OrientableArm(backend=backend, reference_resource=self.reference) + self._capabilities = [self.arm] From ec4459686b722857a36a679128c9ab22591e2c53 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 15:38:17 -0700 Subject: [PATCH 15/93] raise canopen SDO timeout to 1s 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) --- pylabrobot/paa/kx2/kx2_canopen_driver.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/pylabrobot/paa/kx2/kx2_canopen_driver.py b/pylabrobot/paa/kx2/kx2_canopen_driver.py index c50db7639f8..e1e4a3461eb 100644 --- a/pylabrobot/paa/kx2/kx2_canopen_driver.py +++ b/pylabrobot/paa/kx2/kx2_canopen_driver.py @@ -133,7 +133,12 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: ) for nid in self.node_id_list: - self._nodes[nid] = network.add_node(nid, canopen.ObjectDictionary()) + node = network.add_node(nid, canopen.ObjectDictionary()) + # canopen's default SDO response timeout is 0.3s, which is tight for + # drives that queue vendor objects (Elmo 0x20xx/0x30xx). Match the 1s + # the legacy driver used for its own futures. + node.sdo.RESPONSE_TIMEOUT = 1.0 + self._nodes[nid] = node # Elmo binary-interpreter response subscription. network.subscribe(_BI_RESPONSE_COB_BASE + nid, self._make_bi_callback(nid)) From c6da02bdaf9946d0cc18386c5398fd3692bc0d27 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 15:56:15 -0700 Subject: [PATCH 16/93] flip hello-world to KX2Canopen for hardware testing MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- docs/user_guide/paa/kx2/hello-world.ipynb | 291 +--------------------- 1 file changed, 5 insertions(+), 286 deletions(-) diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 456a4314d09..fb6ee06ebec 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -22,296 +22,15 @@ "cell_type": "markdown", "id": "kx2-setup-header", "metadata": {}, - "source": [ - "## Setup\n", - "\n", - "`setup()` opens the CAN bus, discovers nodes, reads drive parameters, enables the motion axes, and homes + closes the servo gripper." - ] + "source": "## Setup\n\n`setup()` opens the CAN bus, discovers nodes, reads drive parameters, enables the motion axes, and homes + closes the servo gripper.\n\n```{note}\nThis notebook uses `KX2Canopen`, the [`canopen`](https://pypi.org/project/canopen/)-library-backed driver. The legacy hand-rolled transport is still available as `KX2`; both expose the same arm-capability frontend (`kx2.arm`), so switching between them is a one-line import change.\n```" }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "id": "kx2-setup-code", "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "uptime library not available, timestamps are relative to boot time and not to Epoch UTC\n" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ - "motor send command 1 UI 4 False (1)\n", - "motor send command 2 UI 4 False (1)\n", - "motor send command 3 UI 4 False (1)\n", - "motor send command 4 UI 4 False (1)\n", - "motor send command 6 UI 4 False (1)\n", - "\n", - "axis 1\n", - "motor send command 1 UI 5 False (1)\n", - "motor send command 1 UI 6 False (1)\n", - "motor send command 1 UI 7 False (1)\n", - "motor send command 1 UI 8 False (1)\n", - "motor send command 1 UI 9 False (1)\n", - "motor send command 1 UI 10 False (1)\n", - "motor send command 1 UI 11 False (1)\n", - "motor send command 1 UI 12 False (1)\n", - "motor send command 1 UI 13 False (1)\n", - "motor send command 1 UI 14 False (1)\n", - "motor send command 1 UI 15 False (1)\n", - "motor send command 1 UI 16 False (1)\n", - "motor send command 1 UI 24 False (1)\n", - "motor send command 1 UF 1 True (2)\n", - "motor send command 1 UF 2 True (2)\n", - "motor send command 1 XM 1 False (1)\n", - "motor send command 1 XM 2 False (1)\n", - "motor send command 1 UF 3 True (2)\n", - "motor send command 1 UF 4 True (2)\n", - "motor send command 1 VH 3 False (1)\n", - "motor send command 1 VL 3 False (1)\n", - "motor send command 1 CA 45 False (1)\n", - "motor send command 1 CA 41 False (1)\n", - "motor send command 1 CA 46 False (1)\n", - "motor send command 1 SP 2 False (1)\n", - "motor send command 1 VH 2 False (1)\n", - "motor send command 1 SD 0 False (1)\n", - "\n", - "axis 2\n", - "motor send command 2 UI 5 False (1)\n", - "motor send command 2 UI 6 False (1)\n", - "motor send command 2 UI 7 False (1)\n", - "motor send command 2 UI 8 False (1)\n", - "motor send command 2 UI 9 False (1)\n", - "motor send command 2 UI 10 False (1)\n", - "motor send command 2 UI 11 False (1)\n", - "motor send command 2 UI 12 False (1)\n", - "motor send command 2 UI 13 False (1)\n", - "motor send command 2 UI 14 False (1)\n", - "motor send command 2 UI 15 False (1)\n", - "motor send command 2 UI 16 False (1)\n", - "motor send command 2 UI 24 False (1)\n", - "motor send command 2 UF 1 True (2)\n", - "motor send command 2 UF 2 True (2)\n", - "motor send command 2 XM 1 False (1)\n", - "motor send command 2 XM 2 False (1)\n", - "motor send command 2 UF 3 True (2)\n", - "motor send command 2 UF 4 True (2)\n", - "motor send command 2 VH 3 False (1)\n", - "motor send command 2 VL 3 False (1)\n", - "motor send command 2 CA 45 False (1)\n", - "motor send command 2 CA 41 False (1)\n", - "motor send command 2 CA 46 False (1)\n", - "motor send command 2 SP 2 False (1)\n", - "motor send command 2 VH 2 False (1)\n", - "motor send command 2 SD 0 False (1)\n", - "\n", - "axis 3\n", - "motor send command 3 UI 5 False (1)\n", - "motor send command 3 UI 6 False (1)\n", - "motor send command 3 UI 7 False (1)\n", - "motor send command 3 UI 8 False (1)\n", - "motor send command 3 UI 9 False (1)\n", - "motor send command 3 UI 10 False (1)\n", - "motor send command 3 UI 11 False (1)\n", - "motor send command 3 UI 12 False (1)\n", - "motor send command 3 UI 13 False (1)\n", - "motor send command 3 UI 14 False (1)\n", - "motor send command 3 UI 15 False (1)\n", - "motor send command 3 UI 16 False (1)\n", - "motor send command 3 UI 24 False (1)\n", - "motor send command 3 UF 1 True (2)\n", - "motor send command 3 UF 2 True (2)\n", - "motor send command 3 XM 1 False (1)\n", - "motor send command 3 XM 2 False (1)\n", - "motor send command 3 UF 3 True (2)\n", - "motor send command 3 UF 4 True (2)\n", - "motor send command 3 VH 3 False (1)\n", - "motor send command 3 VL 3 False (1)\n", - "motor send command 3 CA 45 False (1)\n", - "motor send command 3 CA 41 False (1)\n", - "motor send command 3 CA 46 False (1)\n", - "motor send command 3 SP 2 False (1)\n", - "motor send command 3 VH 2 False (1)\n", - "motor send command 3 SD 0 False (1)\n", - "\n", - "axis 4\n", - "motor send command 4 UI 5 False (1)\n", - "motor send command 4 UI 6 False (1)\n", - "motor send command 4 UI 7 False (1)\n", - "motor send command 4 UI 8 False (1)\n", - "motor send command 4 UI 9 False (1)\n", - "motor send command 4 UI 10 False (1)\n", - "motor send command 4 UI 11 False (1)\n", - "motor send command 4 UI 12 False (1)\n", - "motor send command 4 UI 13 False (1)\n", - "motor send command 4 UI 14 False (1)\n", - "motor send command 4 UI 15 False (1)\n", - "motor send command 4 UI 16 False (1)\n", - "motor send command 4 UI 24 False (1)\n", - "motor send command 4 UF 1 True (2)\n", - "motor send command 4 UF 2 True (2)\n", - "motor send command 4 XM 1 False (1)\n", - "motor send command 4 XM 2 False (1)\n", - "motor send command 4 UF 3 True (2)\n", - "motor send command 4 UF 4 True (2)\n", - "motor send command 4 VH 3 False (1)\n", - "motor send command 4 VL 3 False (1)\n", - "motor send command 4 CA 45 False (1)\n", - "motor send command 4 CA 42 False (1)\n", - "motor send command 4 CA 46 False (1)\n", - "motor send command 4 FF 3 True (2)\n", - "motor send command 4 SP 2 False (1)\n", - "motor send command 4 VH 2 False (1)\n", - "motor send command 4 SD 0 False (1)\n", - "\n", - "axis 6\n", - "motor send command 6 UI 5 False (1)\n", - "motor send command 6 UI 6 False (1)\n", - "motor send command 6 UI 7 False (1)\n", - "motor send command 6 UI 8 False (1)\n", - "motor send command 6 UI 9 False (1)\n", - "motor send command 6 UI 10 False (1)\n", - "motor send command 6 UI 11 False (1)\n", - "motor send command 6 UI 12 False (1)\n", - "motor send command 6 UI 13 False (1)\n", - "motor send command 6 UI 14 False (1)\n", - "motor send command 6 UI 15 False (1)\n", - "motor send command 6 UI 16 False (1)\n", - "motor send command 6 UI 24 False (1)\n", - "motor send command 6 UF 1 True (2)\n", - "motor send command 6 UF 2 True (2)\n", - "motor send command 6 XM 1 False (1)\n", - "motor send command 6 XM 2 False (1)\n", - "motor send command 6 UF 3 True (2)\n", - "motor send command 6 UF 4 True (2)\n", - "motor send command 6 VH 3 False (1)\n", - "motor send command 6 VL 3 False (1)\n", - "motor send command 6 CA 45 False (1)\n", - "motor send command 6 CA 41 False (1)\n", - "motor send command 6 CA 46 False (1)\n", - "motor send command 6 SP 2 False (1)\n", - "motor send command 6 VH 2 False (1)\n", - "motor send command 6 SD 0 False (1)\n", - "node_id not int: KX2Axis.SHOULDER \n", - "motor send command 1 UF 6 True (2)\n", - "node_id not int: KX2Axis.SHOULDER \n", - "motor send command 1 UF 7 True (2)\n", - "node_id not int: KX2Axis.SHOULDER \n", - "motor send command 1 UF 8 True (2)\n", - "node_id not int: KX2Axis.SHOULDER \n", - "motor send command 1 UF 9 True (2)\n", - "node_id not int: KX2Axis.SHOULDER \n", - "motor send command 1 UF 10 True (2)\n", - "node_id not int: KX2Axis.SHOULDER \n", - "motor send command 1 UF 11 True (2)\n", - "node_id not int: KX2Axis.SHOULDER \n", - "motor send command 1 UF 12 True (2)\n", - "node_id not int: KX2Axis.SHOULDER \n", - "motor send command 1 UF 13 True (2)\n", - "node_id not int: KX2Axis.SHOULDER \n", - "motor send command 1 UF 14 True (2)\n", - "node_id not int: KX2Axis.SHOULDER \n", - "motor send command 1 UF 15 True (2)\n", - "node_id not int: KX2Axis.SHOULDER \n", - "motor send command 1 UI 17 False (1)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 UF 6 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 UF 7 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 UF 8 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 UF 9 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 UF 10 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 UF 11 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 UF 12 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 UF 13 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 UF 14 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 UF 15 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 UF 16 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 UF 17 True (2)\n", - "Raising event: EventData(event_type=, pending=False, node_id=1, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=1591, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=2, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=1591, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=3, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=1719, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=4, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=1591, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "motor send command 6 PL 1 0.9200000166893005 True (2)\n", - "motor send command 6 CL 1 0.9100000262260437 True (2)\n", - "['100', '5000']\n", - "(100,5000)\n", - "XQ##Home(100,5000)\n", - "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 PL 1 0.9200000166893005 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 CL 1 0.9100000262260437 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 PL 1 0.9200000166893005 True (2)\n", - "cmd {: 0}\n", - "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 MS 1 False (1)\n", - "Servo Gripper Motor Status: 0\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 CL 1 False (1)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 IQ 0 False (1)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 CL 1 False (1)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 IQ 0 False (1)\n" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Raising event: EventData(event_type=, pending=False, node_id=6, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=2, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=1, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=4, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=3, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=2, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=4, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=3, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=1, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=4, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=3, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=1, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=2, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=2, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=4, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=3, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=1, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=4, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=3, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=2, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5815, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "Raising event: EventData(event_type=, pending=False, node_id=1, cmr_msg_type='', cmr_data_type='', cmr_data='', mpc_position=None, msr_status=True, msr_status_word=5687, mesr_status=0, dics_state=None, cemr_emcy_msg=None, cemr_description='', cemr_disable_motors=False, me_error_code=0, me_index=0, mmd_all_moves_done=False)\n", - "CAN Read Error: The value of a handle (PCAN-Channel, PCAN-Hardware, PCAN-Net, PCAN-Client) is invalid\n" - ] - } - ], - "source": [ - "from pylabrobot.paa.kx2 import KX2\n", - "\n", - "kx2 = KX2()\n", - "await kx2.setup()" - ] + "outputs": [], + "source": "from pylabrobot.paa.kx2 import KX2Canopen\n\nkx2 = KX2Canopen()\nawait kx2.setup()" }, { "cell_type": "markdown", @@ -747,4 +466,4 @@ }, "nbformat": 4, "nbformat_minor": 5 -} +} \ No newline at end of file From 1a0c2ef897b0b52f05cff22c5f110d377794f8fe Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 15:57:37 -0700 Subject: [PATCH 17/93] stop treating OS command status=1 as an error in os_interpreter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/kx2_canopen_driver.py | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_canopen_driver.py b/pylabrobot/paa/kx2/kx2_canopen_driver.py index e1e4a3461eb..d672b9cd0ad 100644 --- a/pylabrobot/paa/kx2/kx2_canopen_driver.py +++ b/pylabrobot/paa/kx2/kx2_canopen_driver.py @@ -564,13 +564,20 @@ async def os_interpreter( # 0x1023:1 = OSCommand.Command. ASCII-encoded; library segments if >4 bytes. await asyncio.to_thread(node.sdo.download, 0x1023, 1, cmd.encode("ascii")) - # 0x1023:2 = OSCommand.Status (U8). Nonzero indicates an error. + # 0x1023:2 = OSCommand.Status (U8). This is the CiA-301 OS-command lifecycle + # byte, not an error flag: + # 0x00 no reply yet / no error 0x01 command is being executed + # 0x02 completed, no reply 0x03 completed with reply + # 0xFF no command + # For async `XQ##` dispatches the drive returns 0x01 immediately, which is + # expected — the caller (e.g. `user_program_run`) polls PS/UI afterward for + # completion. SDO abort codes surface as `SdoAbortedError` from the upload + # itself; we don't need to inspect the byte. Log at debug for diagnostics. status_bytes = await asyncio.to_thread(node.sdo.upload, 0x1023, 2) - status = int.from_bytes(status_bytes[:1], "little") - if status != 0: - raise CanError( - f"OS Interpreter command '{cmd}' returned status {status} from node {node_id}" - ) + logger.debug( + "os_interpreter node=%d cmd=%r status=0x%02X", + node_id, cmd, int.from_bytes(status_bytes[:1], "little"), + ) if not query: return "" From 200065bb2aa60564aa52e7ecb3dbd46407254855 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 16:20:06 -0700 Subject: [PATCH 18/93] switch KX2 over to canopen driver, delete legacy transport MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- docs/user_guide/paa/kx2/hello-world.ipynb | 55 +- pylabrobot/paa/kx2/__init__.py | 6 +- pylabrobot/paa/kx2/kx2.py | 26 +- pylabrobot/paa/kx2/kx2_backend.py | 2821 +---------------- .../{kx2_canopen_driver.py => kx2_driver.py} | 22 +- 5 files changed, 29 insertions(+), 2901 deletions(-) rename pylabrobot/paa/kx2/{kx2_canopen_driver.py => kx2_driver.py} (98%) diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index fb6ee06ebec..1ceffb9a31f 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -22,7 +22,7 @@ "cell_type": "markdown", "id": "kx2-setup-header", "metadata": {}, - "source": "## Setup\n\n`setup()` opens the CAN bus, discovers nodes, reads drive parameters, enables the motion axes, and homes + closes the servo gripper.\n\n```{note}\nThis notebook uses `KX2Canopen`, the [`canopen`](https://pypi.org/project/canopen/)-library-backed driver. The legacy hand-rolled transport is still available as `KX2`; both expose the same arm-capability frontend (`kx2.arm`), so switching between them is a one-line import change.\n```" + "source": "## Setup\n\n`setup()` opens the CAN bus, discovers nodes, reads drive parameters, enables the motion axes, and homes + closes the servo gripper." }, { "cell_type": "code", @@ -30,7 +30,7 @@ "id": "kx2-setup-code", "metadata": {}, "outputs": [], - "source": "from pylabrobot.paa.kx2 import KX2Canopen\n\nkx2 = KX2Canopen()\nawait kx2.setup()" + "source": "from pylabrobot.paa.kx2 import KX2\n\nkx2 = KX2()\nawait kx2.setup()" }, { "cell_type": "markdown", @@ -59,15 +59,7 @@ "execution_count": 2, "id": "kx2-gripper-open", "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "cmd {: 30}\n" - ] - } - ], + "outputs": [], "source": [ "await kx2.arm.open_gripper(gripper_width=30)" ] @@ -77,15 +69,7 @@ "execution_count": 3, "id": "kx2-gripper-close", "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "cmd {: 0}\n" - ] - } - ], + "outputs": [], "source": [ "from pylabrobot.paa.kx2 import KX2ArmBackend\n", "\n", @@ -129,20 +113,7 @@ "execution_count": 5, "id": "kx2-gripper-force", "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 PL 1 0.9200000166893005 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 CL 1 0.2730000078678131 True (2)\n", - "node_id not int: KX2Axis.SERVO_GRIPPER \n", - "motor send command 6 PL 1 0.27600000500679017 True (2)\n" - ] - } - ], + "outputs": [], "source": [ "await kx2.arm.backend.servo_gripper_set_default_gripping_force(max_force_percent=30)" ] @@ -166,7 +137,7 @@ { "data": { "text/plain": [ - "GripperLocation(location=Coordinate(x=349.124, y=170.552, z=280.0441), rotation=Rotation(x=0, y=0, z=198.93898581596522))" + "GripperLocation(location=Coordinate(x=340.0977, y=200.5003, z=290.0518), rotation=Rotation(x=0, y=0, z=197.99652087189315))" ] }, "execution_count": 6, @@ -180,23 +151,15 @@ }, { "cell_type": "code", - "execution_count": 11, + "execution_count": 8, "id": "kx2-cartesian-code", "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "cmd {: -59.474537393375996, : 290.0441, : 243.19263109523325, : 257.474537393376}\n" - ] - } - ], + "outputs": [], "source": [ "from pylabrobot.resources import Coordinate\n", "\n", "await kx2.arm.move_to_location(\n", - " location=Coordinate(x=340.124, y=200.552, z=290.0441),\n", + " location=Coordinate(x=330.124, y=210.552, z=320.0441),\n", " direction=198,\n", " backend_params=KX2ArmBackend.CartesianMoveParams(vel_pct=25, accel_pct=25),\n", ")" diff --git a/pylabrobot/paa/kx2/__init__.py b/pylabrobot/paa/kx2/__init__.py index 511b59a3013..7ae2b2c5b80 100644 --- a/pylabrobot/paa/kx2/__init__.py +++ b/pylabrobot/paa/kx2/__init__.py @@ -1,3 +1,3 @@ -from pylabrobot.paa.kx2.kx2 import KX2, KX2Canopen -from pylabrobot.paa.kx2.kx2_backend import KX2ArmBackend, KX2Axis, KX2Driver -from pylabrobot.paa.kx2.kx2_canopen_driver import KX2CanopenDriver +from pylabrobot.paa.kx2.kx2 import KX2 +from pylabrobot.paa.kx2.kx2_backend import KX2ArmBackend, KX2Axis +from pylabrobot.paa.kx2.kx2_driver import KX2Driver diff --git a/pylabrobot/paa/kx2/kx2.py b/pylabrobot/paa/kx2/kx2.py index ec0741be7f5..804051ac549 100644 --- a/pylabrobot/paa/kx2/kx2.py +++ b/pylabrobot/paa/kx2/kx2.py @@ -1,12 +1,12 @@ from pylabrobot.capabilities.arms.orientable_arm import OrientableArm from pylabrobot.device import Device -from pylabrobot.paa.kx2.kx2_backend import KX2ArmBackend, KX2Driver -from pylabrobot.paa.kx2.kx2_canopen_driver import KX2CanopenDriver +from pylabrobot.paa.kx2.kx2_backend import KX2ArmBackend +from pylabrobot.paa.kx2.kx2_driver import KX2Driver from pylabrobot.resources.resource import Resource class KX2(Device): - """PAA KX2 robotic plate handler (legacy hand-rolled CAN driver).""" + """PAA KX2 robotic plate handler.""" def __init__(self) -> None: driver = KX2Driver() @@ -16,23 +16,3 @@ def __init__(self) -> None: self.reference = Resource(name="KX2", size_x=200, size_y=200, size_z=200) self.arm = OrientableArm(backend=backend, reference_resource=self.reference) self._capabilities = [self.arm] - - -class KX2Canopen(Device): - """PAA KX2 robotic plate handler (canopen-library driver). - - Drop-in replacement for :class:`KX2` using :class:`KX2CanopenDriver` - underneath. Public API is identical — both wrap the same - `KX2ArmBackend` capability backend. Prefer this once the hello-world - notebook runs clean against it on hardware; the legacy class will be - removed afterwards. - """ - - def __init__(self) -> None: - driver = KX2CanopenDriver() - super().__init__(driver=driver) - self.driver: KX2CanopenDriver = driver - backend = KX2ArmBackend(driver=driver) - self.reference = Resource(name="KX2", size_x=200, size_y=200, size_z=200) - self.arm = OrientableArm(backend=backend, reference_resource=self.reference) - self._capabilities = [self.arm] diff --git a/pylabrobot/paa/kx2/kx2_backend.py b/pylabrobot/paa/kx2/kx2_backend.py index a7b3b0a8ebc..d2e159581bd 100644 --- a/pylabrobot/paa/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/kx2_backend.py @@ -1,14 +1,10 @@ import asyncio import logging import math -import struct -import time import warnings -from dataclasses import dataclass, field +from dataclasses import dataclass from enum import IntEnum -from typing import Any, Dict, List, Optional, Sequence, Tuple, Union - -import can +from typing import TYPE_CHECKING, Dict, List, Optional, Sequence from pylabrobot.capabilities.arms.backend import ( CanFreedrive, @@ -18,9 +14,11 @@ from pylabrobot.capabilities.arms.standard import GripperLocation from pylabrobot.capabilities.arms.standard import GripperLocation as GripperPose from pylabrobot.capabilities.capability import BackendParams -from pylabrobot.device import Driver from pylabrobot.resources import Coordinate, Rotation +if TYPE_CHECKING: + from pylabrobot.paa.kx2.kx2_driver import KX2Driver + logger = logging.getLogger(__name__) @@ -52,10 +50,6 @@ def _to_float(s: str, default: float = 0.0) -> float: return default -def _u32_le(value: int) -> List[int]: - return list((value & 0xFFFFFFFF).to_bytes(4, byteorder="little", signed=False)) - - class JointMoveDirection(IntEnum): Normal = 0 Clockwise = 1 @@ -69,30 +63,6 @@ class HomeStatus(IntEnum): InitializedWithoutHoming = 2 -class InputLogic(IntEnum): - GeneralPurpose = 0 - EnableForwardOnly = 1 - EnableReverseOnly = 2 - - -class EventType(IntEnum): - MotorPositionChanged = 1 - MotionStatusReceived = 2 - MotorEnabledStatusReceived = 3 - DigitalInputChangedState = 4 - CANEmergencyMessageReceived = 5 - MoveError = 6 - MotorMoveDone = 7 - MotorsMoveDone = 8 - MotorsMovePathDone = 9 - - -class MoveType(IntEnum): - MotorMove = 0 - MotorsMoveAbsolute = 1 - MotorsMovePath = 2 - - class CmdType(IntEnum): ValQuery = 1 ValSet = 2 @@ -185,131 +155,6 @@ class TPDOMappedObject(IntEnum): DigitalInputs = 0x60FD0020 -class ElmoObject(IntEnum): - DeviceType = 0x1000 - ErrorRegister = 0x1001 - ManfStatusRegister = 0x1002 - ErrorField = 0x1003 - CommCyclePeriod = 0x1006 - ManfDeviceName = 0x1008 - ManfHWVersion = 0x1009 - ManfSWVersion = 0x100A - NodeID = 0x100B - StoreParameters = 0x1010 - RestoreParameters = 0x1011 - CustomerHeartbeatTime = 0x1016 - ProducerHeartbeatTime = 0x1017 - IdentityObject = 0x1018 - OSCommand = 0x1023 - OSCommandMode = 0x1024 - ErrorBehavior = 0x1029 - RPDO1CommParam = 0x1400 - RPDO2CommParam = 0x1401 - RPDO3CommParam = 0x1402 - RPDO4CommParam = 0x1403 - RPDO1Mapping = 0x1600 - RPDO2Mapping = 0x1601 - RPDO3Mapping = 0x1602 - RPDO4Mapping = 0x1603 - TPDO1CommParam = 0x1800 - TPDO2CommParam = 0x1801 - TPDO3CommParam = 0x1802 - TPDO4CommParam = 0x1803 - TPDO1Mapping = 0x1A00 - TPDO2Mapping = 0x1A01 - TPDO3Mapping = 0x1A02 - TPDO4Mapping = 0x1A03 - FastReference = 0x2005 - BinaryInterpreterInput = 0x2012 - BinaryInterpreterOutput = 0x2013 - FilteredRMSCurrent = 0x201B - HomeOnBlockLimitParams = 0x2020 - RecorderData = 0x2030 - TimeStamp = 0x2040 - DriveParamsChecksum = 0x2060 - AdditionalPositionRangeLimit = 0x207B - ExtendedErrorCode = 0x2081 - CANControllerStatus = 0x2082 - SerialEncoderStatus = 0x2084 - ExtraStatusRegister = 0x2085 - STOStatusRegister = 0x2086 - PALVersion = 0x2087 - AuxPositionActualValue = 0x20A0 - SocketAdditionalFunction = 0x20B0 - AbsoluteSensorFunctions = 0x20FC - DigitalInputs = 0x20FD - DigitalInputLowByte = 0x2201 - ExtendedInputs = 0x2202 - Application = 0x2203 - AnalogInputs = 0x2205 - Supply5VDC = 0x2206 - DigitalOutputs = 0x22A0 - ExtendedOutputs = 0x22A1 - DriveTemperature = 0x22A2 - DriveTemperature2 = 0x22A3 - MotorTemperature = 0x22A4 - GainSchedulingIndex = 0x2E00 - TorqueWindow = 0x2E06 - TorqueWindowTime = 0x2E07 - HomeOnTouchProbe = 0x2E10 - GantryYawOffset = 0x2E15 - UserInteger = 0x2F00 - UserFloat = 0x2F01 - GetCtrlBoardType = 0x2F05 - TPDOAsyncEvents = 0x2F20 - EmergencyEvents = 0x2F21 - DS402Config = 0x2F41 - ThresholdParam = 0x2F45 - CANEncoderRange = 0x2F70 - ExtrapolationCyclesTimeout = 0x2F75 - ElmoParamBG = 0x3020 - ElmoParamCA = 0x3034 - ElmoParamCL = 0x303F - ElmoParamDC = 0x3050 - ElmoParamEC = 0x306A - ElmoParamER = 0x3079 - ElmoParamHL = 0x30C1 - ElmoParamHP = 0x30C5 - ElmoParamHT = 0x30C9 - ElmoParamIB = 0x30D1 - ElmoParamID = 0x30D3 - ElmoParamIF = 0x30D5 - ElmoParamIL = 0x30DB - ElmoParamIP = 0x30DF - ElmoParamIQ = 0x30E0 - ElmoParamJV = 0x30FF - ElmoParamKI = 0x310C - ElmoParamKL = 0x310F - ElmoParamKP = 0x3113 - ElmoParamKV = 0x3119 - ElmoParamLL = 0x3129 - ElmoParamMC = 0x313A - ElmoParamMO = 0x3146 - ElmoParamMS = 0x314A - ElmoParamOB = 0x316D - ElmoParamOL = 0x3177 - ElmoParamOP = 0x317B - ElmoParamPA = 0x3186 - ElmoParamPE = 0x318A - ElmoParamPL = 0x3191 - ElmoParamPP = 0x3195 - ElmoParamPR = 0x3197 - ElmoParamPS = 0x3198 - ElmoParamPX = 0x319D - ElmoParamSD = 0x31D7 - ElmoParamSF = 0x31D9 - ElmoParamSR = 0x31E5 - ElmoParamSV = 0x31E9 - ElmoParamSW = 0x31EA - ElmoParamTC = 0x31F0 - ElmoParamTM = 0x31FA - ElmoParamUC = 0x320D - ElmoParamUI = 0x3210 - ElmoParamVE = 0x3226 - ElmoParamVH = 0x3229 - ElmoParamVL = 0x322D - - class ElmoObjectDataType(IntEnum): UNSIGNED8 = 0 UNSIGNED16 = 1 @@ -322,99 +167,6 @@ class ElmoObjectDataType(IntEnum): STR = 8 -@dataclass -class EventData: - event_type: EventType = EventType.MotorPositionChanged - pending: bool = False - node_id: int = 0 - cmr_msg_type: str = "" - cmr_data_type: str = "" - cmr_data: str = "" - mpc_position: Optional[List[int]] = None # Array - msr_status: bool = False - msr_status_word: int = 0 - mesr_status: int = 0 - dics_state: Optional[List[bool]] = None # Array - cemr_emcy_msg: Any = None # sEmcy - cemr_description: str = "" - cemr_disable_motors: bool = False - me_error_code: int = 0 - me_index: int = 0 - mmd_all_moves_done: bool = False - - -@dataclass -class ErrCtrl: - data_byte: Optional[List[int]] = None # Array - - -@dataclass -class PVT_EMCY_QueueLow: - state: bool = False - write_pointer: int = 0 - read_pointer: int = 0 - - -@dataclass -class PVT_EMCY_QueueFull: - state: bool = False - failed_write_pointer: int = 0 - - -@dataclass -class PVT_EMCY: - queue_low: PVT_EMCY_QueueLow = field(default_factory=PVT_EMCY_QueueLow) - queue_full: PVT_EMCY_QueueFull = field(default_factory=PVT_EMCY_QueueFull) - bad_head_pointer: bool = False - bad_mode_init_data: bool = False - motion_terminated: bool = False - out_of_modulo: bool = False - - -@dataclass -class Emcy: - err_code: int = 0 - err_reg: int = 0 - elmo_err_code: int = 0 - err_code_data1: int = 0 - err_code_data2: int = 0 - - -@dataclass -class Query: - node_id: int = 0 - object_byte0: int = 0 - object_byte1: int = 0 - sub_index: int = 0 - msg_type: str = "" # "SDODI" "CHR" "STAT" "SDOUA" "SDOSU" - msg_index: int = 0 - - -@dataclass -class CAN_Msg: - cob: COBType = COBType.NMT - node_id: int = 0 - byte0: int = 0 - byte1: int = 0 - byte2: int = 0 - byte3: int = 0 - byte4: int = 0 - byte5: int = 0 - byte6: int = 0 - byte7: int = 0 - execute: bool = False - data_length: int = 8 - error_msg: str = "" - pending: bool = False - time_stamp: int = 0 - fut: Optional[asyncio.Future] = None - - -@dataclass -class NodeInputConfig: - logic: InputLogic = InputLogic.GeneralPurpose - logic_high: bool = True - class CanError(Exception): """Custom exception for CAN motor errors.""" @@ -436,2569 +188,6 @@ class MotorsMovePlan: move_time: float = 10.0 -class KX2Driver(Driver): - """Low-level CAN transport + CANopen/DS402 drive primitives for the PAA KX2.""" - - def __init__(self, has_rail: bool = False, has_servo_gripper: bool = True) -> None: - super().__init__() - self.connecting: bool = False - self.grp_id: int = 0 - - # Error control - self.err_ctrl: List[ErrCtrl] = [ErrCtrl() for _ in range(8)] - - self.move_error_code: int = 0 - - self.pvt_time_interval_msec: int = 0 - self.pvt_stop: bool = False - - self.pvt_emcy: List[PVT_EMCY] = [PVT_EMCY() for _ in range(128)] - self._pvt_mode: bool = False - - self._can_device: Optional[can.BusABC] = None - - # Threading flags (for asyncio tasks) - self._can_write_task_running = False - self._can_read_task_running = True - - # Store task references to prevent garbage collection - self._read_task: Optional[asyncio.Task] = None - self._write_task: Optional[asyncio.Task] = None - self.b_pvt_thread_started = False - - # Can message queues - self._can_msg_queue_hp: asyncio.Queue[CAN_Msg] = asyncio.Queue() - self._can_msg_queue_lp: asyncio.Queue[CAN_Msg] = asyncio.Queue() - - self._waiting_moves: Dict[KX2Axis, asyncio.Future] = {} - - self.node_id_list = [1, 2, 3, 4] - if has_rail: - self.node_id_list.append(5) - if has_servo_gripper: - self.node_id_list.append(6) - - self.input_state: Dict[int, int] = {i: 0 for i in range(len(self.node_id_list))} - - # initialize based on maximum possible node_id (assume 127 + 1 + 1, so 129), 5, 1 - self.tpdo_mapped_object: List[List[List[TPDOMappedObject]]] = [ - [[TPDOMappedObject.NotMapped for _ in range(1)] for _ in range(5)] for _ in range(129) - ] - self.node_input_config: List[List[NodeInputConfig]] = [ - [NodeInputConfig() for _ in range(7)] for _ in range(129) - ] - - # Wait buffers - self._os_query_wait_buffer: List[Tuple[Query, asyncio.Future]] = [] - self._os_query_wait_buffer_lock = asyncio.Lock() - - self._query_wait_buffer: List[Tuple[Query, asyncio.Future]] = [] - self._query_wait_buffer_lock = asyncio.Lock() - - @property - def can_device(self) -> can.BusABC: - if self._can_device is None: - raise CanError("CAN device is not initialized.") - return self._can_device - - async def _can_write_task(self): - self._can_write_task_running = True - - while self._can_write_task_running: - message: Optional[CAN_Msg] = None - try: - message = self._can_msg_queue_hp.get_nowait() - except asyncio.QueueEmpty: - try: - message = self._can_msg_queue_lp.get_nowait() - except asyncio.QueueEmpty: # No messages to send - await asyncio.sleep(0.001) - continue - - if message.fut is not None and message.fut.done(): - continue # Skip if future is already done - - msg_id = (message.cob.value << 7) + message.node_id - data_bytes = [ - message.byte0, - message.byte1, - message.byte2, - message.byte3, - message.byte4, - message.byte5, - message.byte6, - message.byte7, - ][: message.data_length] - - try: - can_msg = can.Message( - arbitration_id=msg_id, - data=data_bytes, - is_extended_id=False, # Assuming standard IDs for CANopen - ) - - await asyncio.to_thread(self.can_device.send, can_msg) - if message.fut and not message.fut.done(): - message.fut.set_result(None) - except Exception as e: - error_msg = f"CAN Write Error: {e}" - logger.error(error_msg) - if message.fut and not message.fut.done(): - message.fut.set_exception(CanError(error_msg)) - - async def _process_emcy_message(self, node_id: int, message: can.Message) -> None: - data = message.data - if not data or len(data) < 8: - logger.warning("EMCY malformed: expected 8 bytes, got %d", 0 if not data else len(data)) - return - - def u16_le(i: int) -> int: - return int(data[i]) | (int(data[i + 1]) << 8) - - emcy = Emcy() - emcy.err_code = u16_le(0) - emcy.err_reg = int(data[2]) - emcy.elmo_err_code = int(data[3]) - emcy.err_code_data1 = u16_le(4) - emcy.err_code_data2 = u16_le(6) - - self.last_emcy = emcy - - desc = "" - disable_motors = False - suppress_event = False - - err = emcy.err_code - elmo = emcy.elmo_err_code - - # Simple (err_code -> (description, disable_motors)) - ERR_MAP: dict[int, tuple[str, bool]] = { - 0x8110: ("CAN message lost (corrupted or overrun)", False), - 0x8200: ("Protocol error (unrecognized NMT request)", False), - 0x8210: ("Attempt to access an unconfigured RPDO", False), - 0x8130: ("Heartbeat event", False), - 0x6180: ("Fatal CPU error: stack overflow", False), - 0x6181: ("CPU exception: fatal exception", False), - 0x6200: ("User program aborted by an error", False), - 0xFF01: ("Request by user program 'emit' function", False), - 0x6300: ( - "Object mapped to an RPDO returned an error during interpretation or a referenced motion failed to be performed", - False, - ), - 0x7300: ("Resolver or Analog Encoder feedback failed", True), - 0x7380: ("Feedback loss: no match between encoder and Hall locations.", True), - 0x8311: ( - "Peak current has been exceeded due to drive malfunction or badly-tuned current controller", - True, - ), - 0x5441: ("E-stop button was pressed", True), - 0x5280: ("ECAM table problem", False), - 0x7381: ( - "Two digital Hall sensors changed at once; only one sensor can be changed at a time", - True, - ), - 0x8480: ("Speed tracking error", True), - 0x8611: ("Position tracking error", True), - 0x6320: ("Cannot start due to inconsistent database", False), - 0x8380: ("Cannot find electrical zero of motor when attempting to start motor", False), - 0x8481: ("Speed limit exceeded", True), - 0x5281: ("Timing error", False), - 0x7121: ("Motor stuck: motor powered but not moving", True), - 0x8680: ("Position limit exceeded", True), - 0x8381: ("Cannot tune current offsets", False), - 0xFF10: ("Cannot start motor", False), - 0x5400: ("Cannot start motor", False), - 0x3120: ( - "Under-voltage: power supply is shut down or it has too high an output impedance", - True, - ), - 0x3310: ( - "Over-voltage: power supply voltage is too high or servo driver could not absorb kinetic energy while braking a load", - True, - ), - 0x2340: ("Short circuit: motor or its wiring may be defective, or drive is faulty", True), - 0x4310: ("Temperature: drive overheating", True), - 0xFF20: ("Safety switch is sensed - Drive in safety state", True), - } - - if err == 0xFF00: - if elmo == 0x56: - st = self.pvt_emcy[node_id] - st.queue_low.state = True - st.queue_low.write_pointer = emcy.err_code_data1 - st.queue_low.read_pointer = emcy.err_code_data2 - desc = "Queue Low" - elif elmo == 0x5B: - self.pvt_emcy[node_id].bad_head_pointer = True - desc = "Bad Head Pointer" - elif elmo == 0x34: - st = self.pvt_emcy[node_id] - st.queue_full.state = True - st.queue_full.failed_write_pointer = emcy.err_code_data1 - desc = "Queue Full" - elif elmo == 0x07: - self.pvt_emcy[node_id].bad_mode_init_data = True - desc = "Bad Mode Init Data" - elif elmo == 0x08: - self.pvt_emcy[node_id].motion_terminated = True - desc = "Motion Terminated" - elif elmo == 0xA6: - self.pvt_emcy[node_id].out_of_modulo = True - desc = "Out Of Modulo" - else: - desc = f"DS402 IP Error {elmo}" - - elif err == 0xFF02: - if elmo == 0x07: - self.pvt_emcy[node_id].bad_mode_init_data = True - desc = "Bad Mode Init Data" - elif elmo == 0x08: - self.pvt_emcy[node_id].motion_terminated = True - desc = "Motion Terminated" - elif elmo == 0x34: - st = self.pvt_emcy[node_id] - st.queue_full.state = True - st.queue_full.failed_write_pointer = emcy.err_code_data1 - desc = "Queue Full" - elif elmo == 0x56: - st = self.pvt_emcy[node_id] - st.queue_low.state = True - st.queue_low.write_pointer = emcy.err_code_data1 - st.queue_low.read_pointer = emcy.err_code_data2 - desc = "Queue Low" - elif elmo == 0x5B: - self.pvt_emcy[node_id].bad_head_pointer = True - desc = "Bad Head Pointer" - elif elmo == 0x8A: - self.pvt_emcy[node_id].queue_low.state = True - desc = "Position Interpolation buffer underflow" - suppress_event = True - elif elmo == 0xA6: - self.pvt_emcy[node_id].out_of_modulo = True - desc = "Out Of Modulo" - elif elmo == 0xBA: - self.pvt_emcy[node_id].queue_full.state = True - desc = "Interpolation queue is full" - elif elmo == 0xBB: - desc = "Incorrect interpolation sub-mode" - else: - desc = f"DS402 IP Error {elmo}" - - else: - mapped = ERR_MAP.get(err) - if mapped: - desc, disable_motors = mapped - else: - desc = f"Unknown EMCY err={err} elmo={elmo}" - - if disable_motors: - for fut in self._waiting_moves.values(): - if not fut.done(): - fut.set_exception(CanError(f"Motor move error on node {node_id}: {desc}")) - - if not suppress_event: - event = EventData() - event.event_type = EventType.CANEmergencyMessageReceived - event.node_id = node_id - event.cemr_description = desc - event.cemr_emcy_msg = emcy - event.cemr_disable_motors = disable_motors - logger.debug("EMCY event: %s", event) - - logger.info( - "EMCY node=%d desc=%r disable_motors=%s suppress_event=%s", - node_id, desc, disable_motors, suppress_event, - ) - logger.debug("EMCY payload: %s", emcy) - - async def _process_tpdo_message(self, node_id: int, message: can.Message, response_type: int): - tpdo_index = {0: TPDO.TPDO1, 4: TPDO.TPDO3, 6: TPDO.TPDO4}[response_type - 3] - - if self.node_id_list is None or node_id not in self.node_id_list: - return - - if self.tpdo_mapped_object is None: - return - - num2 = 0 - - node_idx = self.node_id_list.index(node_id) - - for i in range(len(self.tpdo_mapped_object[node_id][tpdo_index])): - mapped = self.tpdo_mapped_object[node_id][tpdo_index][i] - - # read 16-bit or 32-bit value depending on flags - num4_: Optional[int] = None - if mapped.value & 0x10 == 0x10: # 16 bit - num4_ = (message.data[num2 + 1] << 8) | message.data[num2] - num2 += 2 - if mapped.value & 0x20 == 0x20: # 32 bit - num4_ = ( - (message.data[num2 + 3] << 24) - | (message.data[num2 + 2] << 16) - | (message.data[num2 + 1] << 8) - | message.data[num2] - ) - num2 += 4 - if num4_ is None: - continue - num4 = num4_ - - if mapped == TPDOMappedObject.StatusWord: - if KX2Axis(node_id) in self._waiting_moves: - fut = self._waiting_moves[KX2Axis(node_id)] - if not fut.done(): - fut.set_result(None) - - event_data = EventData( - event_type=EventType.MotionStatusReceived, - node_id=node_id, - msr_status=True, - msr_status_word=int(num4 & 0xFFFF), # short - ) - await self._raise_an_event(event_data) - elif mapped == TPDOMappedObject.PositionActualValue: - pass # we don't need to mirror the encoder position on the client side - elif mapped == TPDOMappedObject.DigitalInputs: - # temp array for digital inputs (indices 16..21 used) - num_array = [0] * 22 # 0..21 - - # clear bits 16..21?? - for i in range(16, 21 + 1): - num_array[i] = 0 - - # set bits from num4 - if num4 & (1 << 16): - num_array[16] = 1 - if num4 & (1 << 17): - num_array[17] = 1 - if num4 & (1 << 18): - num_array[18] = 1 - if num4 & (1 << 19): - num_array[19] = 1 - if num4 & (1 << 20): - num_array[20] = 1 - if num4 & (1 << 21): - num_array[21] = 1 - - # num6: packed 6-bit value - num6 = ( - num_array[16] - + num_array[17] * 2 - + num_array[18] * 4 - + num_array[19] * 8 - + num_array[20] * 16 - + num_array[21] * 32 - ) - - if self.input_state[node_idx] != num6: - # check for transitions that imply move done - for index4 in range(1, 7): # 1..6 - # previous bit for this input (0/1) - prev_bit = 1 if (self.input_state[node_idx] & (1 << (index4 - 1))) else 0 - new_bit = num_array[15 + index4] - - cfg = self.node_input_config[node_idx][index4] - logic = cfg.logic - - edge = prev_bit != new_bit - logic_enabled = logic in ( - InputLogic.EnableForwardOnly, - InputLogic.EnableReverseOnly, - ) - - if edge and logic_enabled and new_bit == 1: - fut = self._waiting_moves[KX2Axis(node_id)] - if not fut.done(): - fut.set_result(None) - logger.debug( - "Digital input %d enabled motor move done for node %d", index4, node_id - ) - - self.input_state[node_idx] = num6 - - # index 0 unused, 1..6 valid - dics_state = [False] * 7 - dics_state[1] = num_array[16] > 0 - dics_state[2] = num_array[17] > 0 - dics_state[3] = num_array[18] > 0 - dics_state[4] = num_array[19] > 0 - dics_state[5] = num_array[20] > 0 - dics_state[6] = num_array[21] > 0 - - # raise digital input change event - event_data = EventData( - event_type=EventType.DigitalInputChangedState, - node_id=node_id, - dics_state=dics_state, - ) - - await self._raise_an_event(event_data) - - async def _process_binary_interpreter_response(self, node_id: int, message: can.Message) -> None: - data = message.data - if not data or len(data) < 8: - raise CanError("Invalid binary interpreter response data.") - - msg_type = chr(data[0]) + chr(data[1]) - - # Build index: lower 8 bits from DATA[2], upper 6 bits from DATA[3] - msg_index = ((data[3] & 0b0011_1111) << 8) | data[2] - - is_int = (data[3] & 0x80) == 0 - - if is_int: - raw = bytes(data[4:8]) - value_int = struct.unpack(" None: - data = message.data - if not data: - return - - cmd = data[0] - length = len(data) - - async with self._os_query_wait_buffer_lock: - # ---- 1) SDO abort (0x80) ---- - if cmd == 0x80 and length >= 8: - abort_code = data[4] | (data[5] << 8) | (data[6] << 16) | (data[7] << 24) - - msg = self.get_sdo_abort_message(abort_code) - - for query, fut in self._get_os_queries(node_id): - if ( - query.object_byte0 == data[2] - and query.object_byte1 == data[1] - and query.sub_index == data[3] - ): - fut.set_exception(CanError(msg)) - - # ---- 2) SDO download initiate response (0x60) -> "SDODI" ---- - elif cmd == 0x60 and length >= 4: - for query, fut in self._get_os_queries(node_id): - if ( - query.object_byte0 == data[2] - and query.object_byte1 == data[1] - and query.sub_index == data[3] - and query.msg_type == "SDODI" - ): - fut.set_result("SDODI") - - # ---- 3) "CHR" response: 0x20/0x30 and index bytes zero ---- - elif cmd in (0x20, 0x30) and length >= 4 and data[1] == 0 and data[2] == 0 and data[3] == 0: - for query, fut in self._get_os_queries(node_id): - if query.msg_type == "CHR": - fut.set_result("CHR") - - # ---- 4) "STAT" response ---- - elif length >= 5 and cmd == 0x4F and data[1] == 0x23 and data[2] == 0x10 and data[3] == 2: - stat_value = data[4] - for query, fut in self._get_os_queries(node_id): - if query.msg_type == "STAT": - fut.set_result(f"STAT{stat_value}") - - # ---- 5) "VAL" response ---- - elif length >= 4 and cmd == 0x43 and data[1] == 0x23 and data[2] == 0x10 and data[3] == 3: - if length > 4: - tail = ",".join(str(b) for b in data[4:length]) - val_response = f"VAL,{tail}" - else: - val_response = "VAL" - - for query, fut in self._get_os_queries(node_id): - if query.msg_type == "VAL": - fut.set_result(val_response) - - # ---- 6) SDO upload (expedited) "SDOUA" ---- - elif (cmd & 0x40) == 0x40 and length >= 4: - # Decode bits according to CANopen-style layout: - # n = number of unused bytes in 4-byte data field - # e = (usually) expedited bit - n = (cmd >> 2) & 0x03 - e = (cmd >> 1) & 0x01 - # s = cmd & 0x01 # not needed here - - used = max(0, min(4 - n, length - 4)) - raw_bytes = data[4 : 4 + used] - - for query, fut in self._get_os_queries(node_id): - if ( - query.msg_type == "SDOUA" - and query.object_byte0 == data[2] - and query.object_byte1 == data[1] - and query.sub_index == data[3] - ): - if e == 0: - # Numeric value: sum(data[i] * 256^(i-1)) - value = 0 - for i, b in enumerate(raw_bytes): - value |= int(b) << (8 * i) - fut.set_result(f"SDOUAN{value}") - else: - # String value: concatenate chars from data bytes - text = "".join(chr(b) for b in raw_bytes) - fut.set_result(f"SDOUAE{text}") - - # ---- 7) SDO segmented upload "SDOSU" ---- - elif (cmd & 0x20) == 0 and (cmd & 0x40) == 0 and (cmd & 0x80) == 0 and length >= 1: - # Again, follow CANopen-ish semantics: - # toggle bit, n = number of unused bytes (among 7), last-segment bit. - toggle = (cmd >> 4) & 0x01 - n = (cmd >> 1) & 0x07 - last = cmd & 0x01 - - used = max(0, min(7 - n, length - 1)) - payload = data[1 : 1 + used] - payload_str = "".join(chr(b) for b in payload) - - for query, fut in self._get_os_queries(node_id): - if query.msg_type == "SDOSU": - prefix = "SDOSUC" if last == 0 else "SDOSUD" - fut.set_result(f"{prefix}{toggle}{payload_str}") - - # ---- Cleanup: drop finished futures from buffer ---- - self._os_query_wait_buffer = [ - (query, fut) - for query, fut in self._os_query_wait_buffer - if query.node_id != node_id or not fut.done() - ] - - async def _process_motor_drive_restarted(self, node_id: int, message: can.Message): - self.err_ctrl[node_id].data_byte = list(message.data) - if not self.connecting: - event = EventData( - event_type=EventType.CANEmergencyMessageReceived, - node_id=node_id, - cemr_description="Node Guarding error. Motor drive restarted spontaneously.", - cemr_disable_motors=True, - ) - await self._raise_an_event(event) - - await self.can_write(cob=COBType.NMT, node_id=0, byte0=0x01, byte1=node_id) - await self.can_tpdo_unmap(tpdo=TPDO.TPDO1, node_id=node_id) - await self.can_tpdo3_map(node_id=node_id) - await self.can_tpdo4_map(node_id=node_id) - - async def _can_read_task( - self, - ): - while self._can_read_task_running: - try: - message = await asyncio.to_thread(self.can_device.recv, timeout=1.0) - except Exception as e: - logger.error("CAN Read Error: %s", e) - raise CanError(f"CAN Read Error: {e}") - - if message is None: # timeout - continue - - try: - response_type = message.arbitration_id >> 7 - node_id = message.arbitration_id & 0x7F - - if response_type == 0: - logger.debug("NMT message received, ignoring") - elif response_type == 1: - await self._process_emcy_message(node_id=node_id, message=message) - elif response_type in {3, 7, 9}: - await self._process_tpdo_message( - node_id=node_id, message=message, response_type=response_type - ) - elif response_type == 5: - await self._process_binary_interpreter_response(node_id=node_id, message=message) - elif response_type == 11: - await self._process_sdo_response(node_id=node_id, message=message) - elif response_type == 14: - await self._process_motor_drive_restarted(node_id=node_id, message=message) - else: - logger.warning("Unknown CAN message type received: %s", response_type) - except Exception: - logger.exception( - "Error processing CAN message (arb_id=%#x)", message.arbitration_id - ) - - async def can_write( - self, - cob: COBType, - node_id: int, - byte0: int = 0, - byte1: int = 0, - byte2: int = 0, - byte3: int = 0, - byte4: int = 0, - byte5: int = 0, - byte6: int = 0, - byte7: int = 0, - execute: bool = False, - data_length: int = 8, - low_priority: bool = False, - time_stamp: int = 0, - ) -> None: - """Queues a CAN message for transmission.""" - - fut = asyncio.get_event_loop().create_future() - - msg_entry = CAN_Msg( - cob=cob, - node_id=node_id, - byte0=byte0, - byte1=byte1, - byte2=byte2, - byte3=byte3, - byte4=byte4, - byte5=byte5, - byte6=byte6, - byte7=byte7, - execute=execute, - data_length=data_length, - error_msg="", - pending=True, # Still use pending for internal state if needed elsewhere, but fut is primary - time_stamp=time_stamp, - fut=fut, - ) - - if low_priority: - await self._can_msg_queue_lp.put(msg_entry) - else: - await self._can_msg_queue_hp.put(msg_entry) - - timeout_sec = 5.0 # 5000ms - try: - await asyncio.wait_for(fut, timeout=timeout_sec) - except asyncio.TimeoutError: - raise CanError( - f"Failed to send CAN message {cob.name},{node_id},... Low Priority = {low_priority} (timeout)" - ) - except Exception as e: - raise CanError(str(e)) - - async def connect( - self, - baud_rate: int = 500000, - ) -> None: - # Clean up previous connection if re-connecting - if self._read_task is not None and not self._read_task.done(): - self._can_read_task_running = False - self._read_task.cancel() - try: - await self._read_task - except (asyncio.CancelledError, Exception): - pass - if self._write_task is not None and not self._write_task.done(): - self._can_write_task_running = False - self._write_task.cancel() - try: - await self._write_task - except (asyncio.CancelledError, Exception): - pass - if self._can_device is not None: - self._can_device.shutdown() - self._can_device = None - - self.sending_sv_command = False - self.move_error_code = 0 - self.pvt_time_interval_msec = 0 - self.err_ctrl = [ErrCtrl() for _ in range(8)] - - # Determine max_node_id for array sizing if dynamic behavior is needed - max_node_id = 6 - - # Resize buffers based on max_node_id - buffer_size = max_node_id + 1 if max_node_id > 0 else 8 - - self.node_input_config = [[NodeInputConfig() for _ in range(7)] for _ in range(buffer_size + 1)] - for i in range(buffer_size + 1): - for j in range(1, 7): - self.node_input_config[i][j].logic = InputLogic.GeneralPurpose - self.node_input_config[i][j].logic_high = True - - # Initialize python-can bus - self._can_device = can.Bus( - interface="pcan", # Or 'usbcan', 'kvaser', etc. based on setup - channel=None, # e.g., 'PCAN_USBBUS1' or int 0 for default - bitrate=baud_rate, - is_extended_id=False, - ) - - # Start asyncio tasks (store references to prevent GC) - self._can_read_task_running = True - self._can_write_task_running = False - self._read_task = asyncio.create_task(self._can_read_task()) - self._write_task = asyncio.create_task(self._can_write_task()) - - await asyncio.sleep(0.01) - - # --- CANopen Initialization Sequence --- - self.connecting = True - # NMT Reset Nodes (0x80) - await self.can_write(cob=COBType.NMT, node_id=0, byte0=0x82) - - await asyncio.sleep(0.5) - - # NMT Start Nodes (0x01) - await self.can_write(cob=COBType.NMT, node_id=0, byte0=0x01) - await asyncio.sleep(0.1) - self.connecting = False - - discovered_nodes = [ - i for i in range(len(self.err_ctrl)) if self.err_ctrl[i].data_byte is not None - ] - if discovered_nodes != self.node_id_list: - raise CanError( - f"Node IDs on CAN bus do not match expected list: {discovered_nodes} != {self.node_id_list}" - ) - - async def connect_part_two(self): - """After setting up the threads and can connection, and receiving node IDs, map the things. this is flag=True""" - - max_node_id = max(self.node_id_list) - - self.tpdo_mapped_object = [ - [[TPDOMappedObject.NotMapped for _ in range(1)] for _ in range(5)] - for _ in range(max_node_id + 2) - ] - - for node_id in self.node_id_list: - await self.can_tpdo_unmap(TPDO.TPDO1, node_id) - - await self.can_tpdo3_map(node_id) - await self.can_tpdo4_map(node_id) - - # Configure Elmo objects if group_node_id is set - # This section involves can_sdo_download_ElmoObject which needs to be translated first. - for axis in MOTION_AXES: - await self.can_sdo_download_elmo_object( - node_id=int(axis), - elmo_object_int=24768, - sub_index=0, - data="-1", - data_type=ElmoObjectDataType.INTEGER16, - ) - - await self.can_sdo_download_elmo_object( - node_id=int(axis), - elmo_object_int=24772, - sub_index=2, - data="16", - data_type=ElmoObjectDataType.UNSIGNED32, - ) - - await self.can_sdo_download_elmo_object( - node_id=int(axis), - elmo_object_int=24772, - sub_index=3, - data="0", - data_type=ElmoObjectDataType.UNSIGNED8, - ) - - await self.can_sdo_download_elmo_object( - node_id=int(axis), - elmo_object_int=24772, - sub_index=5, - data="8", - data_type=ElmoObjectDataType.UNSIGNED8, - ) - - await self.can_sdo_download_elmo_object( - node_id=int(axis), - elmo_object_int=24770, - sub_index=2, - data="-3", - data_type=ElmoObjectDataType.INTEGER8, - ) - - await self.can_sdo_download_elmo_object( - node_id=int(axis), - elmo_object_int=24669, - sub_index=0, - data="1", - data_type=ElmoObjectDataType.INTEGER16, - ) - - for axis in MOTION_AXES: - await self.can_rpdo1_map(int(axis)) - await self.can_rpdo3_map(int(axis)) - - # PVT Mode setup - self._pvt_mode = True - await self.pvt_select_mode(False) - - async def disconnect(self) -> None: - self._can_read_task_running = False - self._can_write_task_running = False - - if self._can_device is not None: - self._can_device.shutdown() - self._can_device = None - - async def setup(self, backend_params: Optional[BackendParams] = None) -> None: - await self.connect() - - async def stop(self) -> None: - await self.disconnect() - - async def _raise_an_event(self, event_data: EventData): - logger.info("Raising event: %s", event_data) - - # TODO: on move error / emergency, we should set the indicator light and disable motors - - # --- helper methods and core functionalities --- - - def get_sdo_abort_message(self, code: int) -> str: - sdo_default_abort_message = f"Unknown error {code:#010x}." - return { - 0x05030000: "Toggle bit not alternated.", - 0x05040001: "Invalid or unknown client/server command specifier.", - 0x05040002: "Invalid block size.", - 0x05040003: "Invalid sequence number in SDO block upload.", - 0x05040005: "Out of memory.", - 0x06010000: "Unsupported access to an object.", - 0x06010001: "Attempt to read a write-only object.", - 0x06010002: "Attempt to write a read-only object.", - 0x06020000: "Object does not exist in object dictionary.", - 0x06040041: "Object cannot be mapped to PDO.", - 0x06040042: "Number and length of objects to be mapped exceeds PDO length.", - 0x06040043: "General parameter incompatibility.", - 0x06060000: "Access failed due to hardware error.", - 0x06070012: "Data type does not match. Service parameter too long.", - 0x06090011: "Sub-index does not exist.", - 0x06090030: "Value range of parameter exceeded (only for write access).", - 0x06090031: "Value of parameter written too high.", - 0x06090032: "Value of parameter written too low.", - 0x06090036: "Maximum value is less than minimum value.", - 0x08000000: "General error. Use the EC command to retrieve the actual error.", - 0x08000020: "Data cannot be transferred to or stored in application.", - 0x08000022: "Data cannot be transferred to or stored in application due to present device state.", - 0x08000024: "There is no data available to transmit.", - }.get(code, sdo_default_abort_message) - - async def _add_os_query_wait_buffer(self, query: Query): - """Adds a new query to the OS query wait buffer and returns a Future for awaiting the response. - The Future will be set by the reading thread when the response is received. - """ - fut = asyncio.get_event_loop().create_future() - async with self._os_query_wait_buffer_lock: - self._os_query_wait_buffer.append((query, fut)) - return fut - - def _get_os_queries(self, node_id: int) -> List[Tuple[Query, asyncio.Future]]: - if not self._os_query_wait_buffer_lock.locked(): - raise RuntimeError("Lock must be held to access OS queries.") - return [ - (query, fut) - for query, fut in self._os_query_wait_buffer - if not fut.done() and query.node_id == node_id - ] - - async def _add_query_wait_buffer(self, query): - """Adds a new query to the query wait buffer and returns a Future for awaiting the response. - The Future will be set by the reading thread when the response is received. - """ - fut = asyncio.get_event_loop().create_future() - async with self._query_wait_buffer_lock: - self._query_wait_buffer.append((query, fut)) - return fut - - def _get_queries(self, node_id: int) -> List[Tuple[Query, asyncio.Future]]: - if not self._query_wait_buffer_lock.locked(): - raise RuntimeError("Lock must be held to access queries.") - return [ - (query, fut) - for query, fut in self._query_wait_buffer - if not fut.done() and query.node_id == node_id - ] - - async def can_sdo_upload( - self, - node_id: int, - object_byte0: int, - object_byte1: int, - sub_index: int, - ) -> bytes: - """can_sdo_upload (read). Sends an SDO Upload request and waits for the response.""" - - query = Query( - node_id=node_id, - object_byte0=object_byte0, - object_byte1=object_byte1, - sub_index=sub_index, - msg_type="SDOUA", # SDO Upload Acknowledge - ) - fut = await self._add_os_query_wait_buffer(query) - - # Command for SDO_UPLOAD_INITIATE (0x40) - await self.can_write( - cob=COBType.RSDO, - node_id=node_id, - byte0=0x40, - byte1=object_byte1, - byte2=object_byte0, - byte3=sub_index, - ) - - # Wait for response - resp = await asyncio.wait_for(fut, timeout=1.0) - - if "SDOUA" not in resp: - raise CanError( - f"Failed to receive Initiate SDO Upload acknowledgement from motor drive {node_id}" - ) - - if "ABORT" in resp: - abort_code_start = resp.index("ABORT") - abort_msg = resp[abort_code_start + 4 :] - raise CanError( - f"SDO command {hex(object_byte0 << 8 | object_byte1)} sub-index {sub_index}. Abort code received from motor drive {node_id}. {abort_msg}" - ) - - # At this point, `resp` should contain SDOUANxxx or SDOUAExxx or actual data - # This is where segmented transfer or direct values are extracted. - # This part is highly complex and depends on `SDOUA` command. - # TODO: more work here. - - # Simplified conversion from query.response to data_byte_ref - if "SDOUAN" in resp: - val_str = resp[len("SDOUAN") :] - try: - return int(val_str).to_bytes(8, "little")[:4] # Max 4 bytes for expedited - except ValueError: - raise CanError( - f"Failed to receive numeric response to {hex(object_byte0 << 8 | object_byte1)} sub-index {sub_index} from motor drive {node_id}" - ) - if "SDOUAE" in resp: - data_str = resp[len("SDOUAE") :] - return [ord(c) for c in data_str] - raise CanError(f"Failed to interpret SDO Upload response: {resp}") - - async def can_sdo_download( - self, - node_id: int, - object_byte0: int, - object_byte1: int, - sub_index: int, - data_byte: List[int], - ) -> None: - """ - can_sdo_download (write). - Sends an SDO message and waits for a response. - """ - if len(data_byte) <= 4: # Expedited SDO Download - query = Query( - node_id=node_id, - object_byte0=object_byte0, - object_byte1=object_byte1, - sub_index=sub_index, - msg_type="SDODI", # SDO Download Initiate Acknowledge - ) - fut = await self._add_os_query_wait_buffer(query) - - # 0x21: initiate download, data length indicated by n = 4 - len(data) in bytes - # If len 1 => 0x2F, len 2 => 0x2B, len 3 => 0x27, len 4 => 0x23 - cmd_byte = 0x23 | ((4 - len(data_byte)) << 2) | 0x01 # 0x01 is probably a fixed bit - filled_data = list(data_byte) + [0] * (4 - len(data_byte)) # Pad with zeros to 4 bytes - - await self.can_write( - COBType.RSDO, - node_id, - byte0=cmd_byte, - byte1=object_byte1, - byte2=object_byte0, - byte3=sub_index, - byte4=filled_data[0], - byte5=filled_data[1], - byte6=filled_data[2], - byte7=filled_data[3], - ) - - resp = await asyncio.wait_for(fut, timeout=1.0) - - if "SDODI" not in resp: - raise CanError( - f"Failed to receive Expedited SDO Download acknowledgement from motor drive {node_id}" - ) - - if "ABORT" in resp: - abort_code_start = resp.index("ABORT") - abort_msg = resp[abort_code_start + 5 :] - raise CanError( - f"SDO command {hex(object_byte0 << 8 | object_byte1)} sub-index {sub_index} abort code received from motor drive {node_id}. {abort_msg}" - ) - - else: # Segmented SDO Download (more than 4 bytes) - # This is much more involved, involving multiple CAN messages for data transfer. - query = Query( - node_id=node_id, - object_byte0=object_byte0, - object_byte1=object_byte1, - sub_index=sub_index, - msg_type="SDODI", # SDO Download Initiate Acknowledge - ) - fut = await self._add_os_query_wait_buffer(query) - - # Initiate Segmented SDO Download (0x21): Object, Subindex, Data size (total len in bytes) - # Data payload for this initiate message: object bytes, subindex, and total data len. - # `CAN_Write(eCOBType.RSDO, NodeID, (byte) 33, ObjectByte1, ObjectByte0, SubIndex)` - # Byte0=0x21 (initiate segment download), then object/subindex for the SDO. - # The data length is sent with this message if known. - # For `segmented` method, the total size is determined. - - # Byte0 = (byte)33 -- (CSID = 001, E=0, S=0) -- initiate Segmented SDO Download - # Followed by Object and Subindex - await self.can_write( - COBType.RSDO, node_id, byte0=0x21, byte1=object_byte1, byte2=object_byte0, byte3=sub_index - ) # Missing length - - # Wait for response on initiated segmented download. - resp = await asyncio.wait_for(fut, timeout=1.0) # 1000ms timeout - - if "SDODI" not in resp: - raise CanError( - f"Failed to receive Segmented SDO Download acknowledgement from motor drive {node_id}" - ) - - if "ABORT" in resp: - abort_code_start = resp.index("ABORT") - abort_msg = resp[abort_code_start + 5 :] - raise CanError( - f"SDO command {hex(object_byte0 << 8 | object_byte1)} sub-index {sub_index} abort code received from motor drive {node_id}. {abort_msg}" - ) - - # Token byte toggling and sending segments. This is a loops over data_byte_ref. - toggle_bit = 0 - bytes_sent = 0 - while bytes_sent < len(data_byte): - query_frag = Query( - node_id=node_id, - object_byte0=object_byte0, - object_byte1=object_byte1, - sub_index=sub_index, - msg_type="CHR", - ) - fut_frag = await self._add_os_query_wait_buffer(query_frag) - - remaining_data = len(data_byte) - bytes_sent - segment_len = min(remaining_data, 7) # Max 7 bytes payload for SDO segment - - # `num1` is toggle bit, `num10` is len_indicator. Last bit set if last fragment. - # Base is 0x00, toggle bit is 0x10. N=num_bytes_not_used. - # If last segment: add 0x01. - cmd_sdo_seg = (toggle_bit << 4) | ((7 - segment_len) << 1) - if bytes_sent + segment_len >= len(data_byte): # This is the last segment - cmd_sdo_seg |= 0x01 # Set last segment bit (C=1) - - segment_data = data_byte[bytes_sent : bytes_sent + segment_len] - - # Pad segment_data to 7 bytes for can_write - padded_segment = segment_data + [0] * (7 - len(segment_data)) - - await self.can_write( - COBType.RSDO, - node_id, - byte0=cmd_sdo_seg, - byte1=padded_segment[0], - byte2=padded_segment[1], - byte3=padded_segment[2], - byte4=padded_segment[3], - byte5=padded_segment[4], - byte6=padded_segment[5], - byte7=padded_segment[6], - data_length=segment_len + 1, - ) # cmd_sdo_seg itself + data - - resp = await asyncio.wait_for(fut_frag, timeout=1.0) # 1000ms timeout - - if "CHR" not in resp: - raise CanError( - f"Failed to receive OS Character Transfer acknowledgement for segment from drive {node_id}" - ) - - bytes_sent += segment_len - toggle_bit = 1 - toggle_bit # Toggle - - async def os_interpreter( - self, - node_id: int, - cmd: str, - *, - query: bool = False, - ) -> str: - def _abort_detail(resp_txt: str) -> Optional[str]: - i = resp_txt.upper().find("ABORT") - if i < 0: - return None - return resp_txt[i + 5 :].strip() - - async def send_and_wait( - *, - object_byte0: int, - object_byte1: int, - sub_index: int, - msg_type: str, - write_kwargs: dict, - timeout_s: float = 1.0, - ) -> str: - q = Query( - node_id=node_id, - object_byte0=object_byte0, - object_byte1=object_byte1, - sub_index=sub_index, - msg_type=msg_type, - ) - fut = await self._add_os_query_wait_buffer(query=q) - await self.can_write(**write_kwargs) - return await asyncio.wait_for(fut, timeout=timeout_s) - - # --- 1) "Evaluate Immediately mode" acknowledge (SDODI), write: 35,36,0x10 - try: - r = await send_and_wait( - object_byte0=0x10, - object_byte1=0x24, # 36 - sub_index=0, - msg_type="SDODI", - write_kwargs=dict( - cob=COBType.RSDO, - node_id=node_id, - byte0=0x23, # 35 - byte1=0x24, # 36 - byte2=0x10, - ), - ) - except Exception as e: - raise CanError( - f"Failed to send OS evaluate-immediately command to motor drive {node_id}: {e}" - ) - - if "SDODI" not in r: - raise CanError( - f"Failed to receive OS Evaluate Immediately mode acknowledgement from motor drive {node_id}" - ) - - ab = _abort_detail(r) - if ab is not None: - raise CanError( - f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}" - ) - - # --- 2) Initiate segmented SDO download, sub=1, write: 33,35,0x10,1 - r = await send_and_wait( - object_byte0=0x10, - object_byte1=0x23, # 35 - sub_index=1, - msg_type="SDODI", - write_kwargs=dict( - cob=COBType.RSDO, - node_id=node_id, - byte0=0x21, # 33 - byte1=0x23, # 35 - byte2=0x10, - byte3=0x01, - ), - ) - - if "SDODI" not in r: - raise CanError( - f"Failed to receive Segmented SDO Download acknowledgement from motor drive {node_id}" - ) - - ab = _abort_detail(r) - if ab is not None: - raise CanError( - f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}" - ) - - # --- 3) Send command as 7-byte segments with CANopen toggle/unused/last bits. - cmd_bytes = cmd.encode("ascii", errors="strict") - chunks: List[bytes] = [cmd_bytes[i : i + 7] for i in range(0, len(cmd_bytes), 7)] - if not chunks: - chunks = [b""] - - toggle = 1 - - for idx, chunk in enumerate(chunks): - toggle = 0 if toggle != 0 else 1 # flip each segment - unused = 7 - len(chunk) - last = idx == len(chunks) - 1 - - # Byte0: [toggle in bit4] + [unused in bits1-3] + [last in bit0] - byte0 = (toggle << 4) | ((unused & 0x07) << 1) | (1 if last else 0) - - padded = chunk + (b"0" * unused) - b1, b2, b3, b4, b5, b6, b7 = (padded[i] for i in range(7)) - - r = await send_and_wait( - object_byte0=0, - object_byte1=0, - sub_index=0, - msg_type="CHR", - write_kwargs=dict( - cob=COBType.RSDO, - node_id=node_id, - byte0=byte0, - byte1=b1, - byte2=b2, - byte3=b3, - byte4=b4, - byte5=b5, - byte6=b6, - byte7=b7, - ), - ) - - if "CHR" not in r: - raise CanError( - f"Failed to receive OS Character Transfer acknowledgement from motor drive {node_id}" - ) - - ab = _abort_detail(r) - if ab is not None: - raise CanError( - f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}" - ) - - # --- 4) Read OS command status (STAT), sub=2, write: 0x40,35,0x10,2 - r = await send_and_wait( - object_byte0=0x10, - object_byte1=0x23, # 35 - sub_index=2, - msg_type="STAT", - write_kwargs=dict( - cob=COBType.RSDO, - node_id=node_id, - byte0=0x40, - byte1=0x23, - byte2=0x10, - byte3=0x02, - ), - ) - - if "STAT" not in r: - raise CanError(f"Failed to receive OS command status from motor drive {node_id}") - - ab = _abort_detail(r) - if ab is not None: - raise CanError( - f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}" - ) - - # If not querying a response, done. - if not query: - return "" - - # --- 5) Initiate SDO upload (SDOUA), sub=3, write: 0x40,35,0x10,3 - r = await send_and_wait( - object_byte0=0x10, - object_byte1=0x23, - sub_index=3, - msg_type="SDOUA", - write_kwargs=dict( - cob=COBType.RSDO, - node_id=node_id, - byte0=0x40, - byte1=0x23, - byte2=0x10, - byte3=0x03, - ), - ) - - if "SDOUA" not in r: - raise CanError( - f"Failed to receive Initiate SDO Upload acknowledgement from motor drive {node_id}" - ) - - ab = _abort_detail(r) - if ab is not None: - raise CanError( - f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}" - ) - - # expects r like: "SDOUA" + ("N" or "E") + payload - response_out = "" - - if len(r) >= 6 and r.startswith("SDOUA"): - kind = r[5:6] # 6th char (1-based) - payload = r[6:] # everything after that char - - if kind == "E": - response_out = payload - return response_out - - if kind == "N": - # if non-numeric => error - if not payload.strip().lstrip("+-").isdigit(): - raise CanError(f"Failed to receive a response to '{cmd}' from motor drive {node_id}") - # else fall through to segmented upload reading below - - # --- 6) Segmented SDO upload (SDOSU) - seg_idx = 0 - response_out = "" - - while True: - # client request byte0 toggles between 0x60 and 0x70 - req0 = 0x60 if (seg_idx % 2) == 0 else 0x70 - - r = await send_and_wait( - object_byte0=0, - object_byte1=0, - sub_index=0, - msg_type="SDOSU", - write_kwargs=dict( - cob=COBType.RSDO, - node_id=node_id, - byte0=req0, - ), - ) - - if "SDOSU" not in r: - raise CanError(f"Failed to receive OS response value from motor drive {node_id}") - - ab = _abort_detail(r) - if ab is not None: - raise CanError( - f"OS Interpreter command {cmd} abort code received from motor drive {node_id}. {ab}" - ) - - if len(r) <= 7: - raise CanError(f"Failed to extract data response for '{cmd}' from motor drive {node_id}") - - # r: "SDOSU" + (pos6: 'C'/'D') + (pos7: toggle bit '0'/'1') + data... - cd_flag = r[5:6] - toggle_bit = r[6:7] - data_part = r[7:] - - response_out += data_part - - if (req0 == 0x60 and toggle_bit == "1") or (req0 == 0x70 and toggle_bit == "0"): - raise CanError(f"Toggle bit mismatch in response for '{cmd}' from motor drive {node_id}") - - if cd_flag == "C": - seg_idx += 1 - continue - - if cd_flag == "D": - return response_out - - raise CanError(f"Failed to receive data response for '{cmd}' from motor drive {node_id}") - - async def can_sync(self) -> None: - await self.can_write(cob=COBType.SYNC, node_id=0) - - async def can_sdo_download_elmo_object( - self, - node_id: int, - elmo_object_int: int, - sub_index: int, - data: str, - data_type: ElmoObjectDataType, - ) -> None: - """Wrapper around can_sdo_download for Elmo specific objects, handling data type conversions.""" - data_bytes: List[int] = [] - - if data_type == ElmoObjectDataType.UNSIGNED8: - data_bytes = list(int(data).to_bytes(1, "little")) - elif data_type == ElmoObjectDataType.UNSIGNED16: - data_bytes = list(int(data).to_bytes(2, "little")) - elif data_type == ElmoObjectDataType.UNSIGNED32: - data_bytes = list(int(float(data)).to_bytes(4, "little")) - elif data_type == ElmoObjectDataType.UNSIGNED64: - data_bytes = list(int(data).to_bytes(8, "little")) - elif data_type == ElmoObjectDataType.INTEGER8: - data_bytes = list(int(data).to_bytes(1, "little", signed=True)) - elif data_type == ElmoObjectDataType.INTEGER16: - data_bytes = list(int(data).to_bytes(2, "little", signed=True)) - elif data_type == ElmoObjectDataType.INTEGER32: - data_bytes = list(int(float(data)).to_bytes(4, "little", signed=True)) - elif data_type == ElmoObjectDataType.INTEGER64: - data_bytes = list(int(data).to_bytes(8, "little", signed=True)) - elif data_type == ElmoObjectDataType.STR: - data_bytes = [ord(c) for c in data] - else: - raise CanError(f"Unsupported data type for SDO Write: {data_type.name}") - - obj_byte0 = elmo_object_int >> 8 - obj_byte1 = elmo_object_int & 0xFF - await self.can_sdo_download(node_id, obj_byte0, obj_byte1, sub_index, data_bytes) - - async def can_sdo_upload_elmo_object( - self, - node_id: int, - elmo_object_int: int, - sub_index: int, - data_type: ElmoObjectDataType, - ) -> str: - """Wrapper around can_sdo_upload for Elmo specific objects, handling data type conversions.""" - - obj_byte0 = elmo_object_int >> 8 - obj_byte1 = elmo_object_int & 0xFF - data_bytes = await self.can_sdo_upload(node_id, obj_byte0, obj_byte1, sub_index) - - if len(data_bytes) == 0: - return "" - if data_type == ElmoObjectDataType.UNSIGNED8: - return str(int.from_bytes(data_bytes[:1], "little", signed=False)) - if data_type == ElmoObjectDataType.UNSIGNED16: - return str(int.from_bytes(data_bytes[:2], "little", signed=False)) - if data_type == ElmoObjectDataType.UNSIGNED32: - return str(int.from_bytes(data_bytes[:4], "little", signed=False)) - if data_type == ElmoObjectDataType.UNSIGNED64: - return str(int.from_bytes(data_bytes[:8], "little", signed=False)) - if data_type == ElmoObjectDataType.INTEGER16: - return str(int.from_bytes(data_bytes[:2], "little", signed=True)) - if data_type == ElmoObjectDataType.INTEGER32: - return str(int.from_bytes(data_bytes[:4], "little", signed=True)) - if data_type == ElmoObjectDataType.INTEGER64: - return str(int.from_bytes(data_bytes[:8], "little", signed=True)) - if data_type == ElmoObjectDataType.STR: - return "".join([chr(b) for b in data_bytes]) - raise CanError(f"Unsupported data type for SDO Read conversion: {data_type.name}") - - # --- PDO Mapping methods --- - - async def can_tpdo_unmap(self, tpdo: TPDO, node_id: int): - cob_type_int = { - TPDO.TPDO1: COBType.TPDO1.value, - TPDO.TPDO3: COBType.TPDO3.value, - TPDO.TPDO4: COBType.TPDO4.value, - }[tpdo] - - if not (0 <= node_id <= 0x7F): - raise ValueError(f"node_id must be 0..127, got {node_id}") - - node_id = node_id & 0x7F - num1 = ((cob_type_int & 0x01) << 7) | node_id - num2 = (cob_type_int >> 1) & 0x07 - - await self.can_sdo_download( - node_id=node_id, - object_byte0=24, - object_byte1=tpdo.value - 1, - sub_index=1, - data_byte=[ - num1, - num2, - 0, - 0xC0, - ], - ) - - await self.can_sdo_download( - node_id=node_id, - object_byte0=26, - object_byte1=tpdo.value - 1, - sub_index=0, - data_byte=[0, 0, 0, 0], - ) - - for index in range(len(self.tpdo_mapped_object[node_id][tpdo])): - self.tpdo_mapped_object[node_id][tpdo][index] = TPDOMappedObject.NotMapped - - async def can_tpdo3_map(self, node_id: int) -> None: - mapped_objects = [TPDOMappedObject.StatusWord] - # EventTimerMs=0, Delay100us=0, TransmissionType=EventDrivenDev - await self.can_tpdo_map( - tpdo=TPDO.TPDO3, - node_id=node_id, - mapped_objects=mapped_objects, - event_trigger=TPDOTrigger.MotionComplete, - ) - - async def can_tpdo4_map(self, node_id: int) -> None: - mapped_objects = [TPDOMappedObject.DigitalInputs] - await self.can_tpdo_map( - tpdo=TPDO.TPDO4, - node_id=node_id, - mapped_objects=mapped_objects, - event_trigger=TPDOTrigger.DigitalInputEvent, - ) - - async def can_rpdo1_map(self, node_id: int) -> None: - mapped_objects = [RPDOMappedObject.ControlWord] - await self.can_rpdo_map( - rpdo=RPDO.RPDO1, - node_id=node_id, - mapped_objects=mapped_objects, - transmission_type=PDOTransmissionType.SynchronousCyclic, - ) - - async def can_rpdo3_map(self, node_id: int) -> None: - mapped_objects = [RPDOMappedObject.TargetPositionIP, RPDOMappedObject.TargetVelocityIP] - await self.can_rpdo_map( - rpdo=RPDO.RPDO3, - node_id=node_id, - mapped_objects=mapped_objects, - transmission_type=PDOTransmissionType.EventDrivenDev, - ) - - async def can_rpdo_map( - self, - rpdo: RPDO, - node_id: int, - mapped_objects: List[RPDOMappedObject], - transmission_type: PDOTransmissionType, - ) -> None: - """Maps RPDOs for incoming messages.""" - - rpdo_num = int(rpdo) # expects 1..4 - rpdo_idx = (rpdo_num - 1) & 0xFF # original passes (byte)(RPDO - 1) - - # Map RPDO -> COB function code (4-bit) used to build the 11-bit COB-ID - # (original decompilation only showed 1,3,4; RPDO2 is included here) - if rpdo == RPDO.RPDO1: - cob_type = COBType.RPDO1 - elif rpdo == RPDO.RPDO3: - cob_type = COBType.RPDO3 - elif rpdo == RPDO.RPDO4: - cob_type = COBType.RPDO4 - else: - raise ValueError(f"Unsupported RPDO: {rpdo!r}") - - # CANopen 11-bit COB-ID: (function_code << 7) | node_id(7 bits) - cob_id_11 = ((int(cob_type) & 0x0F) << 7) | (node_id & 0x7F) - - # 1) Disable PDO (set bit 31) while configuring: 0x80000000 | cob_id - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x14, - object_byte1=rpdo_idx, - sub_index=0x01, - data_byte=_u32_le(0x80000000 | cob_id_11), - ) - - # 2) Clear mapping count (sub 0) at 0x1600 + rpdo_idx - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x16, - object_byte1=rpdo_idx, - sub_index=0x00, - data_byte=[0, 0, 0, 0], - ) - - # 3) Set transmission type (sub 2) at 0x1400 + rpdo_idx - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x14, - object_byte1=rpdo_idx, - sub_index=0x02, - data_byte=[int(transmission_type) & 0xFF, 0, 0, 0], - ) - - # 4) Write each mapped object (sub 1..n) at 0x1600 + rpdo_idx - for i, mo in enumerate(mapped_objects): - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x16, - object_byte1=rpdo_idx, - sub_index=(i + 1) & 0xFF, - data_byte=_u32_le(int(mo)), - ) - - # 5) Set mapping count (sub 0) - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x16, - object_byte1=rpdo_idx, - sub_index=0x00, - data_byte=[len(mapped_objects) & 0xFF, 0, 0, 0], - ) - - # 6) Enable PDO (clear bit 31): write cob_id only - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x14, - object_byte1=rpdo_idx, - sub_index=0x01, - data_byte=_u32_le(cob_id_11), - ) - - async def can_tpdo_map( - self, - tpdo: TPDO, - node_id: int, - mapped_objects: List[TPDOMappedObject], - event_trigger: TPDOTrigger, - event_timer_ms: int = 0, - delay_100_us: int = 0, - transmission_type: PDOTransmissionType = PDOTransmissionType.EventDrivenDev, - ) -> None: - """Maps TPDOs for outgoing messages.""" - - tpdo_num = int(tpdo) # expects 1..4 - tpdo_idx = (tpdo_num - 1) & 0xFF # (byte)(TPDO - 1) - - if tpdo == TPDO.TPDO1: - cob_type = COBType.TPDO1 - elif tpdo == TPDO.TPDO3: - cob_type = COBType.TPDO3 - elif tpdo == TPDO.TPDO4: - cob_type = COBType.TPDO4 - else: - raise ValueError(f"Unsupported TPDO: {tpdo!r}") - - # CANopen 11-bit COB-ID - cob_id_11 = ((int(cob_type) & 0x0F) << 7) | (node_id & 0x7F) - - # Event trigger mask: 2**EventTrigger, stored as u32 - event_mask = 1 << int(event_trigger) - - # 1) Disable TPDO while configuring: 0xC0000000 | cob_id - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x18, - object_byte1=tpdo_idx, - sub_index=0x01, - data_byte=_u32_le(0xC0000000 | cob_id_11), - ) - - # 2) Clear mapping count: 0x1A00 + tpdo_idx, sub 0 - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x1A, - object_byte1=tpdo_idx, - sub_index=0x00, - data_byte=[0, 0, 0, 0], - ) - - # 3) Transmission type: 0x1800 + tpdo_idx, sub 2 - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x18, - object_byte1=tpdo_idx, - sub_index=0x02, - data_byte=[int(transmission_type) & 0xFF, 0, 0, 0], - ) - - # 4) Inhibit time / delay (100us units): sub 3 - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x18, - object_byte1=tpdo_idx, - sub_index=0x03, - data_byte=[delay_100_us & 0xFF, 0, 0, 0], - ) - - # 5) Event timer (ms): sub 5 - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x18, - object_byte1=tpdo_idx, - sub_index=0x05, - data_byte=[event_timer_ms & 0xFF, 0, 0, 0], - ) - - # 6) Write event trigger mask to 0x2F20 sub = TPDO (matches can_sdo_download(NodeID, 0x2F, 0x20, TPDO, ...)) - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x2F, - object_byte1=0x20, - sub_index=tpdo_num & 0xFF, - data_byte=_u32_le(event_mask), - ) - - # 7) Write mapped objects into 0x1A00 + tpdo_idx, sub 1..n - for i, mo in enumerate(mapped_objects): - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x1A, - object_byte1=tpdo_idx, - sub_index=(i + 1) & 0xFF, - data_byte=_u32_le(int(mo)), - ) - - # 8) Set mapping count (sub 0) - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x1A, - object_byte1=tpdo_idx, - sub_index=0x00, - data_byte=[len(mapped_objects) & 0xFF, 0, 0, 0], - ) - - # 9) Re-enable TPDO: write 0x40000000 | cob_id - await self.can_sdo_download( - node_id=node_id, - object_byte0=0x18, - object_byte1=tpdo_idx, - sub_index=0x01, - data_byte=_u32_le(0x40000000 | cob_id_11), - ) - - # 10) Mirror the original side-effect: store mapping in self.tpdo_mapped_object - self.tpdo_mapped_object[node_id][tpdo] = mapped_objects - - async def pvt_select_mode(self, enable: bool) -> None: - """Enables or disables PVT mode on all motors in the group.""" - - if enable: - if not self._pvt_mode: - for axis in MOTION_AXES: - await self.can_sdo_download( - node_id=int(axis), - object_byte0=0x60, - object_byte1=0xC4, - sub_index=0x06, - data_byte=[0], - ) - await self.can_sdo_download( - node_id=int(axis), - object_byte0=0x60, - object_byte1=0x60, - sub_index=0x00, - data_byte=[7], - ) - self._pvt_mode = True - else: - for axis in MOTION_AXES: - await self.can_sdo_download( - node_id=int(axis), - object_byte0=0x60, - object_byte1=0x60, - sub_index=0x00, - data_byte=[1], - ) - await self.can_sdo_download( - node_id=int(axis), - object_byte0=0x60, - object_byte1=0xC4, - sub_index=0x06, - data_byte=[0], - ) - await self.can_sdo_download( - node_id=int(axis), - object_byte0=0x60, - object_byte1=0x60, - sub_index=0x00, - data_byte=[7], - ) - elif self._pvt_mode: - for axis in MOTION_AXES: - await self.can_sdo_download( - node_id=int(axis), - object_byte0=0x60, - object_byte1=0x60, - sub_index=0x00, - data_byte=[1], - ) - self._pvt_mode = False - - async def binary_interpreter( - self, - node_id: int, - cmd: str, - cmd_index: int, - cmd_type: CmdType, - value: str = "0", - val_type: ValType = ValType.Int, - low_priority: bool = False, - ) -> Union[str, float]: - timeout = 10.0 if cmd.upper() == "SV" else 1.0 - - is_float = val_type == ValType.Float - is_query = cmd_type == CmdType.ValQuery - is_execute = cmd_type == CmdType.Execute - - # Helper: build command bytes - def _build_bytes() -> tuple[int, int, int, int, int, int, int, int]: - # byte0, byte1: ASCII of first and last char of cmd - byte0 = ord(cmd[0]) - byte1 = ord(cmd[-1]) - - # Encode cmd_index into 14 bits: - # - low 8 bits -> byte2 - # - high 6 bits -> lower bits of byte3 - byte2 = cmd_index & 0xFF - byte3 = (cmd_index >> 8) & 0x3F # keep only 6 bits - - # Set flags in bits 6 and 7 of byte3 - if is_query: - byte3 |= 0x40 # bit 6 - if is_float: - byte3 |= 0x80 # bit 7 - - # Encode value - if is_float: - byte4, byte5, byte6, byte7 = struct.pack(" bool: - try: - expected = float(expected_str) - actual = float(actual_str) - except ValueError: - return False - if actual == 0.0: - return expected == 0.0 - ratio = expected / actual - return expected == actual or (0.99 < ratio < 1.01) - - max_attempts = 1 - - # Single-node path (NodeID != 10) - if node_id != 10: - for attempt in range(1, max_attempts + 1): - if value == "": - value = "0" - - ( - byte0, - byte1, - byte2, - byte3, - byte4, - byte5, - byte6, - byte7, - ) = _build_bytes() - - # Build a minimal query object compatible with your buffer/reader - # We don't know your exact type, so SimpleNamespace gives us attributes. - query = Query( - node_id=node_id, - msg_index=cmd_index, - msg_type=cmd, - ) - - fut = await self._add_query_wait_buffer(query) - - await self.can_write( - COBType.RPDO2, - node_id, - byte0, - byte1, - byte2, - byte3, - byte4, - byte5, - byte6, - byte7, - execute=is_execute, - low_priority=low_priority, - data_length=4 if is_execute else 8, - ) - - try: - resp = await asyncio.wait_for(fut, timeout=timeout) - except asyncio.TimeoutError: - if attempt == max_attempts: - raise CanError( - f"Timeout waiting for response to {cmd}[{cmd_index}] from node {node_id}" - ) - # retry - continue - - # Query: just return the response - if is_query: - value = str(resp) - return float(value) if is_float else int(float(value)) - - # Execute: only care that we got *some* response - if is_execute: - if resp == "" and attempt == max_attempts: - raise CanError( - f"No response for execute command {cmd}[{cmd_index}] from node {node_id}" - ) - if resp != "": - return float(value) if is_float else int(float(value)) - # else retry - continue - - # Write: verify echoed value - if is_float: - ok = _float_matches(value, str(resp)) - else: - ok = (int(float(resp))) == (int(float(value))) - - if ok: - return float(value) if is_float else int(float(value)) - - if attempt == max_attempts: - raise CanError( - f"Unexpected CAN response: attempted to send {cmd}[{cmd_index}]={value}, " - f"but received response={resp} from node {node_id}" - ) - # else retry - - # Should never get here - raise CanError("Internal error in binary_interpreter (single-node)") - - # Group path (NodeID == 10) - grp_ids = [int(axis) for axis in MOTION_AXES] - - for attempt in range(1, max_attempts + 1): - if value == "": - value = "0" - - ( - byte0, - byte1, - byte2, - byte3, - byte4, - byte5, - byte6, - byte7, - ) = _build_bytes() - - # One query per node in group - queries = [] - futures = [] - for gid in grp_ids: - q = Query( - node_id=gid, - msg_index=cmd_index, - msg_type=cmd, - ) - queries.append(q) - fut = await self._add_query_wait_buffer(q) - futures.append(fut) - - await self.can_write( - COBType.RPDO2, - node_id, # broadcast/group node (10) - byte0, - byte1, - byte2, - byte3, - byte4, - byte5, - byte6, - byte7, - execute=is_execute, - low_priority=low_priority, - data_length=4 if is_execute else 8, - ) - - try: - # Wait for *all* group responses - resps = await asyncio.wait_for( - asyncio.gather(*futures, return_exceptions=False), - timeout=timeout, - ) - except asyncio.TimeoutError: - if attempt == max_attempts: - raise CanError( - f"Timeout waiting for group response to {cmd}[{cmd_index}] " f"from nodes {grp_ids}" - ) - # retry - continue - - # Query: concatenate responses with commas - if is_query: - if any(r == "" for r in resps): - if attempt == max_attempts: - raise CanError( - f"Incomplete group query response for {cmd}[{cmd_index}] " f"from nodes {grp_ids}" - ) - # retry - continue - - value = ",".join(str(r) for r in resps) - return float(value) if is_float else int(float(value)) - - # Execute: just require all responses non-empty - if is_execute: - if all(r != "" for r in resps): - return float(value) if is_float else int(float(value)) - if attempt == max_attempts: - missing_nodes = [gid for gid, r in zip(grp_ids, resps) if r == ""] - raise CanError( - f"No execute response from nodes {missing_nodes} " f"for {cmd}[{cmd_index}]" - ) - # retry - continue - - # Write: verify each node's echoed value - mismatch_node = None - mismatch_resp = None - - for gid, resp in zip(grp_ids, resps): - if is_float: - ok = _float_matches(value, str(resp)) - else: - ok = str(resp) == str(value) - - if not ok: - mismatch_node = gid - mismatch_resp = resp - break - - if mismatch_node is None: - # everyone matched - return float(value) if is_float else int(float(value)) - - if attempt == max_attempts: - raise CanError( - f"Unexpected CAN response: attempted to send {cmd}[{cmd_index}]={value}, " - f"but received response={mismatch_resp} from node {mismatch_node}" - ) - # else retry - - # Should never get here - raise CanError("Internal error in binary_interpreter (group)") - - # --- Functions --- - - async def configure_input_logic( - self, - node_id: int, - input_num: int, - logic_high: bool, - logic_type: InputLogic, - ) -> None: - val_to_set = logic_type.value - if logic_high: - val_to_set += 1 - - right = await self.binary_interpreter(node_id, "IL", input_num, CmdType.ValQuery) - - if val_to_set != right: - await self.binary_interpreter(node_id, "IL", input_num, CmdType.ValSet, str(val_to_set)) - await asyncio.sleep(0.25) - - async def read_input(self, node_id: int, input_num: int) -> bool: - """Returns the State (bool).""" - left = await self.binary_interpreter(node_id, "IB", input_num, CmdType.ValQuery) - return left == 1 - - async def read_output(self, node_id: int, output_num: int) -> bool: - """Returns the State.""" - expression = await self.binary_interpreter(node_id, "OP", 0, CmdType.ValQuery) - val = int(expression) - - mask = int(round(math.pow(2, output_num - 1))) - return (val & mask) == mask - - async def set_output(self, node_id: int, output_num: int, state: bool) -> str: - val = "1" if state else "0" - return await self.binary_interpreter(node_id, "OB", output_num, CmdType.ValSet, val) - - async def motor_get_current_position(self, node_id: int, pu: bool = False) -> int: - cmd = "PU" if pu else "PX" - val_str = await self.binary_interpreter(int(node_id), cmd, 0, CmdType.ValQuery) - return int(round(float(val_str))) - - async def motor_get_motion_status(self, node_id: int) -> int: - val = await self.binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) - return int(round(float(val))) - - async def motor_enable(self, axis: KX2Axis, state: bool) -> None: - if not isinstance(axis, KX2Axis): - raise - - flag = not (axis in MOTION_AXES or int(axis) == self.grp_id) - - if state: - self.EmcyMoveErrorReceived = False - if flag: - await self.binary_interpreter(axis, "MO", 0, CmdType.ValSet, "1") - else: - # Standard DS402 Enable Sequence - await self.control_word_set(node_id=int(axis), value=0) - await self.control_word_set(node_id=int(axis), value=128) - await self.control_word_set(node_id=int(axis), value=6) - await self.control_word_set(node_id=int(axis), value=7) - await self.control_word_set(node_id=int(axis), value=15) - - await asyncio.sleep(0.1) - - left = await self.binary_interpreter( - node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery, val_type=ValType.Int - ) - if left != 1: - raise CanError(f"Motor failed to enable (axis = {axis})") - else: - if flag: - try: - await self.binary_interpreter( - node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValSet, value="0" - ) - except Exception as e: - pass - else: - await self.control_word_set(node_id=int(axis), value=7) - await self.control_word_set(node_id=int(axis), value=6) - - await asyncio.sleep(0.1) - left = await self.binary_interpreter( - node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery - ) - if left != 0: - raise RuntimeError(f"Motor failed to disable (axis = {axis}") - - async def motor_set_move_direction(self, node_id: int, direction: JointMoveDirection) -> None: - val_str = "1" - if direction == JointMoveDirection.Clockwise: - val_str = "65" - elif direction == JointMoveDirection.Counterclockwise: - val_str = "129" - elif direction == JointMoveDirection.ShortestWay: - val_str = "193" - - await self.can_sdo_download_elmo_object( - node_id, 24818, 0, val_str, ElmoObjectDataType.UNSIGNED16 - ) - - async def motor_emergency_stop(self, node_id: int) -> None: - await self.binary_interpreter(node_id, "MO", 0, CmdType.ValSet, "0") - - async def user_program_run( - self, - axis: KX2Axis, - user_function: str, - params=None, - timeout_sec: int = 0, - wait_until_done: bool = False, - ) -> int: - """ - Runs a user program on `axis` and optionally waits for completion. - - Returns: - last_line_completed (0 if unknown / not provided by controller) - Raises: - CanError on any controller/protocol/runtime failure or timeout. - """ - if not isinstance(axis, int): - raise ValueError("axis must be int") - if axis < 0 or axis > 255: - raise ValueError("axis must be in [0, 255]") - - node_id = int(axis) - - # PS query - ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) - - if ps == -2: - raise CanError(f"Axis {axis}: controller reported PS=-2 (not ready / unavailable)") - - # If not idle (-1), request idle by setting UI[1]=0 and wait up to 3s for PS=-1 - if ps != -1: - await self.binary_interpreter( - node_id, - "UI", - 1, - CmdType.ValSet, - value=0, # don't stringify bytes; pass normal ints - val_type=ValType.Int, - ) - - t0 = time.monotonic() - while (time.monotonic() - t0) < 3.0: - ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) - if ps == -1: - break - await asyncio.sleep(0.01) - else: - raise CanError(f"Axis {axis}: did not reach idle state (PS=-1) within 3s (last PS={ps})") - - # Build "(a,b,c)" argument list - arg_str = "" - if params: - parts = [str(p) for p in params] - if parts: - arg_str = f"({','.join(parts)})" - - # Arm UI[1]=1 then execute XQ - await self.binary_interpreter( - node_id, - "UI", - 1, - CmdType.ValSet, - value="1", - val_type=ValType.Int, - ) - - cmd = f"XQ##{user_function}{arg_str}" - logger.debug("user_program_run: %s", cmd) - await self.os_interpreter(node_id, cmd, query=False) - - last_line_completed = 0 - - if wait_until_done: - # Wait while PS==1 and UI[1]==1, or until timeout - t0 = time.monotonic() - ps = 1 - ui1 = 1 - while ps == 1 and ui1 == 1 and (time.monotonic() - t0) < float(timeout_sec): - ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) - ui1 = int(await self.binary_interpreter(node_id, "UI", 1, CmdType.ValQuery)) - await asyncio.sleep(0.01) - - # Grab UI[2] (last line completed) after wait loop (matches original behavior) - expr_raw = await self.binary_interpreter(node_id, "UI", 2, CmdType.ValQuery) - try: - last_line_completed = int(str(expr_raw).strip()) - except Exception: - last_line_completed = 0 - - # UI[1] should be "0" on successful completion - if ui1 != 0: - raise CanError( - f"Axis {axis}: user program ended with UI[1]={ui1} (expected 0), last_line={last_line_completed}" - ) - - # Timeout condition: still stuck in running state - if ps == 1 and ui1 == 1: - raise CanError( - f"Axis {axis}: timeout waiting for '{user_function}' after {timeout_sec}s, last_line={last_line_completed}" - ) - - return 0 - - async def home_motor( - self, - axis: "KX2Axis", - hs_offset: int, - ind_offset: int, - home_pos: int, - srch_vel: int, - srch_acc: int, - max_pe: int, - hs_pe: int, - offset_vel: int, - offset_acc: int, - timeout: float, - ) -> None: - left = await self.binary_interpreter(int(axis), "CA", 41, CmdType.ValQuery) - if left == 24: - raise RuntimeError("Error 43") - - try: - await self.motor_hard_stop_search(axis, srch_vel, srch_acc, max_pe, hs_pe, timeout) - except Exception as e: - # Check fault - fault = await self.motor_get_fault(axis) - if fault is not None: - raise RuntimeError(fault) - raise e - - await self.motor_enable(axis=axis, state=True) - - await self.motors_move_absolute_execute( - plan=MotorsMovePlan( - moves=[ - MotorMoveParam( - axis=KX2Axis(axis), - position=hs_offset, - velocity=offset_vel, - acceleration=offset_acc, - relative=False, - direction=JointMoveDirection.ShortestWay, - ) - ], - ) - ) - - is_positive = hs_offset > 0 - await self.motor_index_search(axis, abs(srch_vel), srch_acc, is_positive, timeout) - - await self.motors_move_absolute_execute( - plan=MotorsMovePlan( - moves=[ - MotorMoveParam( - axis=KX2Axis(axis), - position=ind_offset, - velocity=offset_vel, - acceleration=offset_acc, - relative=False, - direction=JointMoveDirection.ShortestWay, - ) - ] - ) - ) - await self.motor_reset_encoder_position(axis, home_pos) - await self.motor_set_homed_status(axis, HomeStatus.Homed) - - async def motor_hard_stop_search( - self, axis: KX2Axis, srch_vel: int, srch_acc: int, max_pe: int, hs_pe: int, timeout: float - ) -> None: - await self.binary_interpreter(int(axis), "ER", 3, CmdType.ValSet, str(max_pe * 10)) - await self.binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) - await self.binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) - # Clear homing params - for i in [3, 4, 5, 2]: - await self.binary_interpreter(int(axis), "HM", i, CmdType.ValSet, "0") - - await self.binary_interpreter( - node_id=int(axis), cmd="JV", cmd_index=0, cmd_type=CmdType.ValSet, value=str(srch_vel) - ) - - try: - params = [str(int(hs_pe)), str(int(timeout * 1000))] - try: - last_line = await self.user_program_run(axis, "Home", params, int(timeout), True) - if last_line in [1, 2, 3]: - raise RuntimeError(f"Homing Script Error {34 + last_line}") - except Exception as e: - # Re-raise unless specific handling needed - raise e - - curr_pos = await self.motor_get_current_position(axis) - - await self.binary_interpreter( - node_id=int(axis), cmd="PA", cmd_index=0, cmd_type=CmdType.ValSet, value=str(curr_pos) - ) - await self.binary_interpreter( - node_id=int(axis), cmd="SP", cmd_index=0, cmd_type=CmdType.ValSet, value=str(srch_vel) - ) - await self.binary_interpreter( - node_id=int(axis), cmd="AC", cmd_index=0, cmd_type=CmdType.ValSet, value=str(srch_acc) - ) - await self.binary_interpreter( - node_id=int(axis), cmd="DC", cmd_index=0, cmd_type=CmdType.ValSet, value=str(srch_acc) - ) - finally: - # Stop any lingering motion/scripts - await asyncio.sleep(0.3) - await self.binary_interpreter(int(axis), "BG", 0, CmdType.Execute, value="0") - await asyncio.sleep(0.3) - - # Restore Error Range to normal safety limits - await self.binary_interpreter(int(axis), "ER", 3, CmdType.ValSet, str(int(max_pe))) - - # Force Motor Off to kill any zombie scripts holding the state machine - # if self.move_error_code != 0: # Only if we had an error - # await self.binary_interpreter(int(axis), "MO", 0, eCmdType.ValSet, "0") - - async def motor_index_search( - self, axis: KX2Axis, srch_vel: int, srch_acc: int, positive_direction: bool, timeout: float - ) -> Tuple[int, int]: - """Returns (OneRevolution, CapturedPosition).""" - await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") - - rev = await self.binary_interpreter(int(axis), "CA", 18, CmdType.ValQuery) - one_revolution = int(float(rev)) - if not positive_direction: - one_revolution *= -1 - - await self.binary_interpreter(int(axis), "PR", 1, CmdType.ValSet, str(one_revolution)) - await self.binary_interpreter(int(axis), "SP", 0, CmdType.ValSet, str(srch_vel)) - await self.binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) - await self.binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) - - await self.binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "3") # Index only - await self.binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") # Arm - - self._waiting_moves = {axis: asyncio.get_event_loop().create_future()} - await self.binary_interpreter(int(axis), "BG", 0, CmdType.Execute) - await self._wait_for_moves_done(timeout) - - left = await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValQuery) - if left != 0: - raise RuntimeError("Homing Failure: Failed to finish index pulse search.") - - cap = await self.binary_interpreter(int(axis), "HM", 7, CmdType.ValQuery) - captured_position = int(float(cap)) - - return one_revolution, captured_position - - async def motor_reset_encoder_position(self, axis: KX2Axis, position: float) -> None: - await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, str(position)) - await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") - - async def motor_set_homed_status(self, axis: KX2Axis, status: HomeStatus) -> None: - val = "0" - if status == HomeStatus.Homed: - val = "1" - elif status == HomeStatus.InitializedWithoutHoming: - val = "2" - await self.binary_interpreter(int(axis), "UI", 3, CmdType.ValSet, val) - - async def motor_get_homed_status(self, node_id: int) -> HomeStatus: - left = await self.binary_interpreter(node_id, "UI", 3, CmdType.ValQuery) - if left == 1: - return HomeStatus.Homed - if left == 2: - return HomeStatus.InitializedWithoutHoming - return HomeStatus.NotHomed - - async def motor_check_if_move_done(self, node_id: int) -> bool: - """Returns Done status. Raises error on fault.""" - ms_val = await self.binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) - - if ms_val == 0: - return True - if ms_val == 1: - mo_val = await self.binary_interpreter(node_id, "MO", 0, CmdType.ValQuery) - if mo_val == 1: - return True - fault = await self.motor_get_fault(node_id) - if fault is not None: - raise RuntimeError(f"Motor Fault: {fault}") - raise RuntimeError("Motor Fault (Unknown)") - if ms_val == 2: - return False - - return False - - async def motor_get_fault(self, axis: KX2Axis) -> Optional[str]: - val = await self.binary_interpreter(int(axis), "MF", 0, CmdType.ValQuery) - if val == 0: - return None - assert isinstance(val, int) - - faults: list[str] = [] - - # Simple bit flags - bit_msgs = { - 0x0001: "Motor Hall sensor feedback angle not found yet.", - 0x0004: "Feedback loss: no match between encoder and Hall location.", - 0x0008: "The peak current has been exceeded.", - 0x0010: "Inhibit.", - 0x0040: "Two digital Hall sensors were changed at the same time.", - 0x0080: "Speed tracking error.", - 0x0100: "Position tracking error.", - 0x0200: "Inconsistent drive database.", - 0x0400: "Too large a difference in ECAM table.", - 0x0800: "CAN heartbeat failure.", - 0x1000: "Servo drive fault.", - 0x010000: "Failed to find the electrical zero of the motor during startup.", - 0x020000: "Speed limit exceeded.", - 0x040000: "Drive CPU stack overflow.", - 0x080000: "Drive CPU exception.", - 0x200000: "Motor stuck.", - 0x400000: "Position limit exceeded.", - 0x20000000: "Cannot start motor.", - } - - for bit, msg in bit_msgs.items(): - if val & bit: - faults.append(msg) - - # 0x2000/0x4000/0x8000 triad - b13 = bool(val & 0x2000) - b14 = bool(val & 0x4000) - b15 = bool(val & 0x8000) - - if (not b15) and (not b14) and b13: - faults.append("Power supply under voltage.") - if (not b15) and b14 and (not b13): - faults.append("Power supply over voltage.") - if b15 and (not b14) and b13: - faults.append("Motor lead short circuit or faulty drive.") - if b15 and b14 and (not b13): - faults.append("Drive overheated.") - - if len(faults) == 0: - return f"Unknown fault code: {val} (0x{val:08X})" - return " ".join(faults) - - async def control_word_set(self, node_id: int, value: int, sync: bool = True) -> None: - val_bytes = value.to_bytes(2, byteorder="little") - await self.can_write(COBType.RPDO1, node_id, val_bytes[0], val_bytes[1], data_length=2) - if sync: - await self.can_sync() - - async def _wait_for_moves_done(self, timeout: float) -> None: - async def wait_for_move_done(axis: KX2Axis) -> None: - try: - await asyncio.wait_for(self._waiting_moves[axis], timeout=timeout) - except asyncio.TimeoutError: - pass - - # if not set on time, make a query - await self.motor_check_if_move_done(axis) - - await asyncio.gather(*(wait_for_move_done(axis) for axis in self._waiting_moves.keys())) - - async def _motors_move_start( - self, - axes: List[KX2Axis], - *, - relative: bool = False, - ) -> None: - # Create futures to wait on - self._waiting_moves = {ax: asyncio.get_event_loop().create_future() for ax in axes} - - # send control word 47 to group; SYNC only on last - relative_bit = 0x40 if relative else 0 - for i, nid in enumerate(axes): - last = i == (len(axes) - 1) - await self.control_word_set(nid, 47 + relative_bit, sync=last) - - # send control word 63 to group; SYNC only on last - for i, nid in enumerate(axes): - last = i == (len(axes) - 1) - await self.control_word_set(nid, 47 + 0x10 + relative_bit, sync=last) - - async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: - await self.pvt_select_mode(False) - - # Send per-axis parameters - for move in plan.moves: - await self.motor_set_move_direction(move.axis.value, move.direction) - - await self.can_sdo_download_elmo_object( - node_id=move.axis.value, - elmo_object_int=24698, - sub_index=0, - data=str(int(move.position)), - data_type=ElmoObjectDataType.INTEGER32, - ) - - await self.can_sdo_download_elmo_object( - node_id=move.axis.value, - elmo_object_int=24705, - sub_index=0, - data=str(int(move.velocity)), - data_type=ElmoObjectDataType.UNSIGNED32, - ) - - acc = max(int(move.acceleration), 100) - await self.can_sdo_download_elmo_object( - node_id=move.axis.value, - elmo_object_int=24707, - sub_index=0, - data=str(acc), - data_type=ElmoObjectDataType.UNSIGNED32, - ) - await self.can_sdo_download_elmo_object( - node_id=move.axis.value, - elmo_object_int=24708, - sub_index=0, - data=str(acc), - data_type=ElmoObjectDataType.UNSIGNED32, - ) - - await self._motors_move_start([move.axis for move in plan.moves]) - await self._wait_for_moves_done(timeout=plan.move_time + 2) - - class KX2ArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive): """Arm-capability backend for the PAA KX2. diff --git a/pylabrobot/paa/kx2/kx2_canopen_driver.py b/pylabrobot/paa/kx2/kx2_driver.py similarity index 98% rename from pylabrobot/paa/kx2/kx2_canopen_driver.py rename to pylabrobot/paa/kx2/kx2_driver.py index d672b9cd0ad..a6a7f7792db 100644 --- a/pylabrobot/paa/kx2/kx2_canopen_driver.py +++ b/pylabrobot/paa/kx2/kx2_driver.py @@ -1,12 +1,8 @@ -"""canopen-library-backed KX2 driver. +"""Low-level CAN transport + CANopen/DS402 drive primitives for the PAA KX2. -Parallel implementation of :class:`KX2Driver` (the hand-rolled CAN transport in -``kx2_backend.py``). Built side-by-side so the legacy driver stays working -during development. When this class passes the hello-world notebook end-to-end -on real hardware, `kx2.py` will be switched over and the legacy driver deleted. - -Public method surface intentionally mirrors ``KX2Driver`` so ``KX2ArmBackend`` -can be pointed at either without any other code changes. +Uses the `canopen` library (python-can bus + CANopen SDO/PDO/NMT/EMCY). +Paired with :class:`KX2ArmBackend` in ``kx2_backend.py`` via the standard +``Device`` + ``Driver`` + capability-backend split. """ from __future__ import annotations @@ -55,13 +51,13 @@ def _u32_le(value: int) -> List[int]: logger = logging.getLogger(__name__) -class KX2CanopenDriver(Driver): - """KX2 driver built on the `canopen` library. +class KX2Driver(Driver): + """CANopen-library-backed KX2 drive transport. Uses `canopen.Network` for bus ownership + NMT, `node.sdo` for SDO traffic, - `node.tpdo`/`node.rpdo` for PDO mapping, and `network.send_message` / - `network.subscribe` for the vendor-specific Elmo binary interpreter - (non-standard, on TPDO2/RPDO2). + raw SDO writes to 0x14xx/0x16xx/0x18xx/0x1Axx for PDO mapping, and + `network.send_message` / `network.subscribe` for the vendor-specific Elmo + binary interpreter (non-standard, rides on TPDO2/RPDO2 COB-IDs). """ def __init__( From 6290d1e99223e9df52eeb0e35874612bee37393c Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 16:39:54 -0700 Subject: [PATCH 19/93] fix freedrive re-enable + notebook blockers MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - \`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) --- docs/user_guide/paa/kx2/hello-world.ipynb | 152 ++++++++++++++-------- pylabrobot/paa/kx2/kx2_driver.py | 8 ++ 2 files changed, 109 insertions(+), 51 deletions(-) diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 1ceffb9a31f..e51ed166988 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -22,15 +22,33 @@ "cell_type": "markdown", "id": "kx2-setup-header", "metadata": {}, - "source": "## Setup\n\n`setup()` opens the CAN bus, discovers nodes, reads drive parameters, enables the motion axes, and homes + closes the servo gripper." + "source": [ + "## Setup\n", + "\n", + "`setup()` opens the CAN bus, discovers nodes, reads drive parameters, enables the motion axes, and homes + closes the servo gripper." + ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "id": "kx2-setup-code", "metadata": {}, - "outputs": [], - "source": "from pylabrobot.paa.kx2 import KX2\n\nkx2 = KX2()\nawait kx2.setup()" + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "uptime library not available, timestamps are relative to boot time and not to Epoch UTC\n", + "2026-04-14 16:37:07,444 - pylabrobot.paa.kx2.kx2_driver - INFO - canopen: connected, nodes=[1, 2, 3, 4, 6]\n" + ] + } + ], + "source": [ + "from pylabrobot.paa.kx2 import KX2\n", + "\n", + "kx2 = KX2()\n", + "await kx2.setup()" + ] }, { "cell_type": "markdown", @@ -137,7 +155,7 @@ { "data": { "text/plain": [ - "GripperLocation(location=Coordinate(x=340.0977, y=200.5003, z=290.0518), rotation=Rotation(x=0, y=0, z=197.99652087189315))" + "GripperLocation(location=Coordinate(x=0.0025, y=241.699, z=150.0018), rotation=Rotation(x=0, y=0, z=255.4754734040739))" ] }, "execution_count": 6, @@ -151,7 +169,7 @@ }, { "cell_type": "code", - "execution_count": 8, + "execution_count": 7, "id": "kx2-cartesian-code", "metadata": {}, "outputs": [], @@ -181,7 +199,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 8, "id": "kx2-joint-code", "metadata": {}, "outputs": [], @@ -200,6 +218,40 @@ ")" ] }, + { + "cell_type": "markdown", + "id": "9a6d0ed1", + "metadata": {}, + "source": [ + "### Joint-space pick and place\n", + "\n", + "`pick_up_at_joint_position` / `drop_at_joint_position` do the same thing as their Cartesian counterparts (move, then close/open the gripper), but the target pose is specified in joint space. Useful when you've taught a position via freedrive and want to return to it without going through inverse kinematics." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "id": "0a19ceeb", + "metadata": {}, + "outputs": [], + "source": [ + "pick_joints = {\n", + " KX2Axis.SHOULDER: 0.0,\n", + " KX2Axis.Z: 150.0,\n", + " KX2Axis.ELBOW: 90.0,\n", + " KX2Axis.WRIST: 0.0,\n", + "}\n", + "place_joints = {\n", + " KX2Axis.SHOULDER: 30.0,\n", + " KX2Axis.Z: 150.0,\n", + " KX2Axis.ELBOW: 90.0,\n", + " KX2Axis.WRIST: 0.0,\n", + "}\n", + "\n", + "await kx2.arm.backend.pick_up_at_joint_position(pick_joints, resource_width=0)\n", + "await kx2.arm.backend.drop_at_joint_position(place_joints, resource_width=30)" + ] + }, { "cell_type": "markdown", "id": "kx2-query-header", @@ -212,21 +264,21 @@ }, { "cell_type": "code", - "execution_count": 12, + "execution_count": 10, "id": "kx2-query-joints", "metadata": {}, "outputs": [ { "data": { "text/plain": [ - "{: 300.52548395979784,\n", - " : 290.0440412368887,\n", - " : 243.19265215891733,\n", - " : 257.4755859375,\n", - " : 0.0}" + "{: 30.0002217292257,\n", + " : 150.00008664814723,\n", + " : 90.0000385241022,\n", + " : 0.0,\n", + " : 0.22776546532767158}" ] }, - "execution_count": 12, + "execution_count": 10, "metadata": {}, "output_type": "execute_result" } @@ -237,17 +289,17 @@ }, { "cell_type": "code", - "execution_count": 13, + "execution_count": 11, "id": "kx2-query-cart", "metadata": {}, "outputs": [ { "data": { "text/plain": [ - "GripperLocation(location=Coordinate(x=340.1239, y=200.5521, z=290.044), rotation=Rotation(x=0, y=0, z=198.00106989729784))" + "GripperLocation(location=Coordinate(x=-120.829, y=209.2797, z=150.0001), rotation=Rotation(x=0, y=0, z=30.000264644559707))" ] }, - "execution_count": 13, + "execution_count": 11, "metadata": {}, "output_type": "execute_result" } @@ -272,20 +324,7 @@ "id": "kx2-pick-place-code", "metadata": {}, "outputs": [], - "source": [ - "pick = Coordinate(x=450.0, y=0.0, z=80.0)\n", - "place = Coordinate(x=450.0, y=-200.0, z=80.0)\n", - "above = Coordinate(x=0, y=0, z=60.0)\n", - "yaw = 0.0\n", - "\n", - "await kx2.arm.move_to_location(pick + above, direction=yaw)\n", - "await kx2.arm.pick_up_at_location(location=pick, direction=yaw, resource_width=0)\n", - "await kx2.arm.move_to_location(pick + above, direction=yaw)\n", - "\n", - "await kx2.arm.move_to_location(place + above, direction=yaw)\n", - "await kx2.arm.drop_at_location(location=place, direction=yaw, resource_width=30)\n", - "await kx2.arm.move_to_location(place + above, direction=yaw)" - ] + "source": "pick = Coordinate(x=450.0, y=0.0, z=80.0)\nplace = Coordinate(x=450.0, y=-200.0, z=80.0)\nabove = Coordinate(x=0, y=0, z=60.0)\nyaw = 0.0\n\n# pick_up_at_location stores resource_width on the frontend; drop_at_location\n# reuses it, so you don't pass it again.\nawait kx2.arm.move_to_location(pick + above, direction=yaw)\nawait kx2.arm.pick_up_at_location(location=pick, direction=yaw, resource_width=0)\nawait kx2.arm.move_to_location(pick + above, direction=yaw)\n\nawait kx2.arm.move_to_location(place + above, direction=yaw)\nawait kx2.arm.drop_at_location(location=place, direction=yaw)\nawait kx2.arm.move_to_location(place + above, direction=yaw)" }, { "cell_type": "markdown", @@ -299,7 +338,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, "id": "kx2-free-on", "metadata": {}, "outputs": [], @@ -309,10 +348,18 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 3, "id": "kx2-free-read", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "GripperLocation(location=Coordinate(x=353.1052, y=233.0403, z=140.0082), rotation=Rotation(x=0, y=0, z=121.77582182317468))\n" + ] + } + ], "source": [ "# Manually guide the arm to the desired pose, then read it:\n", "taught = await kx2.arm.request_gripper_location()\n", @@ -321,10 +368,24 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 4, "id": "kx2-free-off", "metadata": {}, - "outputs": [], + "outputs": [ + { + "ename": "CanError", + "evalue": "Motor failed to enable (axis = 1)", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mCanError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[4], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m kx2\u001b[38;5;241m.\u001b[39marm\u001b[38;5;241m.\u001b[39mbackend\u001b[38;5;241m.\u001b[39mstop_freedrive_mode()\n", + "File \u001b[0;32m~/real/pylabrobot/pylabrobot/paa/kx2/kx2_backend.py:1392\u001b[0m, in \u001b[0;36mKX2ArmBackend.stop_freedrive_mode\u001b[0;34m(self, backend_params)\u001b[0m\n\u001b[1;32m 1390\u001b[0m \u001b[38;5;28;01masync\u001b[39;00m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21mstop_freedrive_mode\u001b[39m(\u001b[38;5;28mself\u001b[39m, backend_params: Optional[BackendParams] \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m:\n\u001b[1;32m 1391\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m axis \u001b[38;5;129;01min\u001b[39;00m MOTION_AXES:\n\u001b[0;32m-> 1392\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mdriver\u001b[38;5;241m.\u001b[39mmotor_enable(axis\u001b[38;5;241m=\u001b[39maxis, state\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mTrue\u001b[39;00m)\n", + "File \u001b[0;32m~/real/pylabrobot/pylabrobot/paa/kx2/kx2_driver.py:737\u001b[0m, in \u001b[0;36mKX2Driver.motor_enable\u001b[0;34m(self, axis, state)\u001b[0m\n\u001b[1;32m 733\u001b[0m left \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mbinary_interpreter(\n\u001b[1;32m 734\u001b[0m node_id\u001b[38;5;241m=\u001b[39m\u001b[38;5;28mint\u001b[39m(axis), cmd\u001b[38;5;241m=\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mMO\u001b[39m\u001b[38;5;124m\"\u001b[39m, cmd_index\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m0\u001b[39m, cmd_type\u001b[38;5;241m=\u001b[39mCmdType\u001b[38;5;241m.\u001b[39mValQuery\n\u001b[1;32m 735\u001b[0m )\n\u001b[1;32m 736\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m left \u001b[38;5;241m!=\u001b[39m \u001b[38;5;241m1\u001b[39m:\n\u001b[0;32m--> 737\u001b[0m \u001b[38;5;28;01mraise\u001b[39;00m CanError(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mMotor failed to enable (axis = \u001b[39m\u001b[38;5;132;01m{\u001b[39;00maxis\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m)\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[1;32m 738\u001b[0m \u001b[38;5;28;01melse\u001b[39;00m:\n\u001b[1;32m 739\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m use_bi:\n", + "\u001b[0;31mCanError\u001b[0m: Motor failed to enable (axis = 1)" + ] + } + ], "source": [ "await kx2.arm.backend.stop_freedrive_mode()" ] @@ -341,7 +402,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 5, "id": "kx2-halt", "metadata": {}, "outputs": [], @@ -351,26 +412,17 @@ }, { "cell_type": "code", - "execution_count": 14, + "execution_count": 6, "id": "kx2-estop", "metadata": {}, "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "node_id not int: KX2Axis.SHOULDER \n", - "motor send command 1 SR 1 False (1)\n", - "!!! not the same\n" - ] - }, { "data": { "text/plain": [ "True" ] }, - "execution_count": 14, + "execution_count": 6, "metadata": {}, "output_type": "execute_result" } @@ -385,9 +437,7 @@ "id": "kx2-fault", "metadata": {}, "outputs": [], - "source": [ - "await kx2.driver.motor_get_fault(KX2Axis.SHOULDER)" - ] + "source": "from pylabrobot.paa.kx2 import KX2Axis\n\nawait kx2.driver.motor_get_fault(KX2Axis.SHOULDER)" }, { "cell_type": "markdown", diff --git a/pylabrobot/paa/kx2/kx2_driver.py b/pylabrobot/paa/kx2/kx2_driver.py index a6a7f7792db..b2acd3f2948 100644 --- a/pylabrobot/paa/kx2/kx2_driver.py +++ b/pylabrobot/paa/kx2/kx2_driver.py @@ -727,8 +727,14 @@ async def motor_enable(self, axis: KX2Axis, state: bool) -> None: if use_bi: await self.binary_interpreter(axis, "MO", 0, CmdType.ValSet, "1") else: + # DS402 enable sequence: Fault -> Shutdown -> Switched On -> Op Enabled. + # The drive needs ≥1 CANopen cycle between transitions to update its + # state machine; without a gap the legacy driver's queue serialization + # provided one, but `network.send_message` is synchronous so we insert + # our own small delay. for cw in (0, 128, 6, 7, 15): await self.control_word_set(node_id=int(axis), value=cw) + await asyncio.sleep(0.01) await asyncio.sleep(0.1) left = await self.binary_interpreter( node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery @@ -744,7 +750,9 @@ async def motor_enable(self, axis: KX2Axis, state: bool) -> None: except Exception: pass else: + # DS402 disable: Op Enabled -> Switched On -> Ready to Switch On. await self.control_word_set(node_id=int(axis), value=7) + await asyncio.sleep(0.01) await self.control_word_set(node_id=int(axis), value=6) await asyncio.sleep(0.1) left = await self.binary_interpreter( From c67bf61f23274a048a92ca6f863471b33a3e8a46 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 16:41:01 -0700 Subject: [PATCH 20/93] privatize driver methods that aren't part of the user API MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/kx2_backend.py | 14 +-- pylabrobot/paa/kx2/kx2_driver.py | 146 +++++++++++++++--------------- 2 files changed, 80 insertions(+), 80 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_backend.py b/pylabrobot/paa/kx2/kx2_backend.py index d2e159581bd..89213f50908 100644 --- a/pylabrobot/paa/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/kx2_backend.py @@ -226,7 +226,7 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): # Device.setup(). Now read per-drive parameters, finish CANopen mapping, # enable motion axes, and initialize the servo gripper. await self.drive_get_parameters(self.driver.node_id_list) - await self.driver.connect_part_two() + await self.driver._connect_part_two() await asyncio.sleep(2) @@ -236,7 +236,7 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): for axis in MOTION_AXES: try: - await self.driver.motor_enable(axis=axis, state=True) + await self.driver._motor_enable(axis=axis, state=True) except Exception as e: logger.warning("Error enabling motor on axis %s: %s", axis, e) @@ -244,7 +244,7 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): async def servo_gripper_initialize(self): try: - await self.driver.motor_enable(axis=KX2Axis.SERVO_GRIPPER, state=True) + await self.driver._motor_enable(axis=KX2Axis.SERVO_GRIPPER, state=True) except Exception as e: logger.warning( "Error enabling servo gripper motor on node %s: %s", KX2Axis.SERVO_GRIPPER, e @@ -271,7 +271,7 @@ async def servo_gripper_home(self) -> None: val_type=ValType.Float, ) - await self.driver.home_motor( + await self.driver._home_motor( axis=KX2Axis.SERVO_GRIPPER, hs_offset=self.servo_gripper_home_hard_stop_offset, ind_offset=self.servo_gripper_home_index_offset, @@ -1117,7 +1117,7 @@ async def motors_move_joint( if plan is None: # if every axis is skipped, exit return - await self.driver.motors_move_absolute_execute(plan) + await self.driver._motors_move_absolute_execute(plan) def convert_cartesian_to_joint_position(self, pose: GripperPose) -> Dict["KX2Axis", float]: if pose.rotation.x != 0 or pose.rotation.y != 0: @@ -1385,8 +1385,8 @@ async def start_freedrive_mode( # KX2 frees all motion axes at once; free_axes is accepted for API parity. del free_axes for axis in MOTION_AXES: - await self.driver.motor_enable(axis=axis, state=False) + await self.driver._motor_enable(axis=axis, state=False) async def stop_freedrive_mode(self, backend_params: Optional[BackendParams] = None) -> None: for axis in MOTION_AXES: - await self.driver.motor_enable(axis=axis, state=True) + await self.driver._motor_enable(axis=axis, state=True) diff --git a/pylabrobot/paa/kx2/kx2_driver.py b/pylabrobot/paa/kx2/kx2_driver.py index b2acd3f2948..dfab2926641 100644 --- a/pylabrobot/paa/kx2/kx2_driver.py +++ b/pylabrobot/paa/kx2/kx2_driver.py @@ -148,7 +148,7 @@ async def stop(self) -> None: # --- drive init (called by KX2ArmBackend._on_setup after setup()) -------- - async def connect_part_two(self) -> None: + async def _connect_part_two(self) -> None: """Configure PDO mapping + Elmo DS402 parameters after the CAN bus is up. Mirrors the legacy driver: unmap TPDO1, map TPDO3 (StatusWord, triggered @@ -161,7 +161,7 @@ async def connect_part_two(self) -> None: assert self._network is not None for node_id in self.node_id_list: - await self.can_tpdo_unmap(TPDO.TPDO1, node_id) + await self._can_tpdo_unmap(TPDO.TPDO1, node_id) await self._tpdo_map( TPDO.TPDO3, node_id, [TPDOMappedObject.StatusWord], TPDOTrigger.MotionComplete ) @@ -170,22 +170,22 @@ async def connect_part_two(self) -> None: ) for axis in MOTION_AXES: - await self.can_sdo_download_elmo_object( + await self._can_sdo_download_elmo_object( int(axis), 24768, 0, "-1", ElmoObjectDataType.INTEGER16 ) - await self.can_sdo_download_elmo_object( + await self._can_sdo_download_elmo_object( int(axis), 24772, 2, "16", ElmoObjectDataType.UNSIGNED32 ) - await self.can_sdo_download_elmo_object( + await self._can_sdo_download_elmo_object( int(axis), 24772, 3, "0", ElmoObjectDataType.UNSIGNED8 ) - await self.can_sdo_download_elmo_object( + await self._can_sdo_download_elmo_object( int(axis), 24772, 5, "8", ElmoObjectDataType.UNSIGNED8 ) - await self.can_sdo_download_elmo_object( + await self._can_sdo_download_elmo_object( int(axis), 24770, 2, "-3", ElmoObjectDataType.INTEGER8 ) - await self.can_sdo_download_elmo_object( + await self._can_sdo_download_elmo_object( int(axis), 24669, 0, "1", ElmoObjectDataType.INTEGER16 ) @@ -207,7 +207,7 @@ async def connect_part_two(self) -> None: self._network.subscribe(tpdo3_cob, self._make_tpdo3_callback(nid)) self._pvt_mode = True - await self.pvt_select_mode(False) + await self._pvt_select_mode(False) def _make_tpdo3_callback(self, node_id: int): def _cb(cob_id: int, data: bytes, timestamp: float) -> None: @@ -226,7 +226,7 @@ def _dispatch_tpdo3(self, node_id: int) -> None: # --- PDO configuration (pure SDO writes; no library-PDO machinery) ------ - async def can_tpdo_unmap(self, tpdo: TPDO, node_id: int) -> None: + async def _can_tpdo_unmap(self, tpdo: TPDO, node_id: int) -> None: cob_type_int = { TPDO.TPDO1: COBType.TPDO1.value, TPDO.TPDO3: COBType.TPDO3.value, @@ -235,8 +235,8 @@ async def can_tpdo_unmap(self, tpdo: TPDO, node_id: int) -> None: node_id &= 0x7F num1 = ((cob_type_int & 0x01) << 7) | node_id num2 = (cob_type_int >> 1) & 0x07 - await self.can_sdo_download(node_id, 0x18, tpdo.value - 1, 1, [num1, num2, 0, 0xC0]) - await self.can_sdo_download(node_id, 0x1A, tpdo.value - 1, 0, [0, 0, 0, 0]) + await self._can_sdo_download(node_id, 0x18, tpdo.value - 1, 1, [num1, num2, 0, 0xC0]) + await self._can_sdo_download(node_id, 0x1A, tpdo.value - 1, 0, [0, 0, 0, 0]) async def _rpdo_map( self, @@ -252,22 +252,22 @@ async def _rpdo_map( cob_id_11 = ((int(cob_type) & 0x0F) << 7) | (node_id & 0x7F) # Disable PDO (bit 31 set) - await self.can_sdo_download(node_id, 0x14, rpdo_idx, 1, _u32_le(0x80000000 | cob_id_11)) + await self._can_sdo_download(node_id, 0x14, rpdo_idx, 1, _u32_le(0x80000000 | cob_id_11)) # Clear mapping count - await self.can_sdo_download(node_id, 0x16, rpdo_idx, 0, [0, 0, 0, 0]) + await self._can_sdo_download(node_id, 0x16, rpdo_idx, 0, [0, 0, 0, 0]) # Transmission type - await self.can_sdo_download( + await self._can_sdo_download( node_id, 0x14, rpdo_idx, 2, [int(transmission_type) & 0xFF, 0, 0, 0] ) # Mapped objects for i, mo in enumerate(mapped_objects): - await self.can_sdo_download(node_id, 0x16, rpdo_idx, i + 1, _u32_le(int(mo))) + await self._can_sdo_download(node_id, 0x16, rpdo_idx, i + 1, _u32_le(int(mo))) # Mapping count - await self.can_sdo_download( + await self._can_sdo_download( node_id, 0x16, rpdo_idx, 0, [len(mapped_objects) & 0xFF, 0, 0, 0] ) # Re-enable (clear bit 31) - await self.can_sdo_download(node_id, 0x14, rpdo_idx, 1, _u32_le(cob_id_11)) + await self._can_sdo_download(node_id, 0x14, rpdo_idx, 1, _u32_le(cob_id_11)) async def _tpdo_map( self, @@ -287,32 +287,32 @@ async def _tpdo_map( event_mask = 1 << int(event_trigger) # Disable TPDO (bit 30 + 31) - await self.can_sdo_download(node_id, 0x18, tpdo_idx, 1, _u32_le(0xC0000000 | cob_id_11)) + await self._can_sdo_download(node_id, 0x18, tpdo_idx, 1, _u32_le(0xC0000000 | cob_id_11)) # Clear mapping count - await self.can_sdo_download(node_id, 0x1A, tpdo_idx, 0, [0, 0, 0, 0]) + await self._can_sdo_download(node_id, 0x1A, tpdo_idx, 0, [0, 0, 0, 0]) # Transmission type - await self.can_sdo_download( + await self._can_sdo_download( node_id, 0x18, tpdo_idx, 2, [int(transmission_type) & 0xFF, 0, 0, 0] ) # Inhibit / delay 100us - await self.can_sdo_download(node_id, 0x18, tpdo_idx, 3, [delay_100_us & 0xFF, 0, 0, 0]) + await self._can_sdo_download(node_id, 0x18, tpdo_idx, 3, [delay_100_us & 0xFF, 0, 0, 0]) # Event timer (ms) - await self.can_sdo_download(node_id, 0x18, tpdo_idx, 5, [event_timer_ms & 0xFF, 0, 0, 0]) + await self._can_sdo_download(node_id, 0x18, tpdo_idx, 5, [event_timer_ms & 0xFF, 0, 0, 0]) # Vendor event mask at 0x2F20: - await self.can_sdo_download(node_id, 0x2F, 0x20, int(tpdo) & 0xFF, _u32_le(event_mask)) + await self._can_sdo_download(node_id, 0x2F, 0x20, int(tpdo) & 0xFF, _u32_le(event_mask)) # Mapped objects for i, mo in enumerate(mapped_objects): - await self.can_sdo_download(node_id, 0x1A, tpdo_idx, i + 1, _u32_le(int(mo))) + await self._can_sdo_download(node_id, 0x1A, tpdo_idx, i + 1, _u32_le(int(mo))) # Mapping count - await self.can_sdo_download( + await self._can_sdo_download( node_id, 0x1A, tpdo_idx, 0, [len(mapped_objects) & 0xFF, 0, 0, 0] ) # Re-enable (clear bits 30 + 31) - await self.can_sdo_download(node_id, 0x18, tpdo_idx, 1, _u32_le(cob_id_11)) + await self._can_sdo_download(node_id, 0x18, tpdo_idx, 1, _u32_le(cob_id_11)) # --- SDO ----------------------------------------------------------------- - async def can_sdo_upload( + async def _can_sdo_upload( self, node_id: int, object_byte0: int, @@ -325,7 +325,7 @@ async def can_sdo_upload( # transfers + abort codes); run off the event loop. return await asyncio.to_thread(node.sdo.upload, index, sub_index) - async def can_sdo_download( + async def _can_sdo_download( self, node_id: int, object_byte0: int, @@ -337,7 +337,7 @@ async def can_sdo_download( node = self._nodes[node_id] await asyncio.to_thread(node.sdo.download, index, sub_index, bytes(data_byte)) - async def can_sdo_upload_elmo_object( + async def _can_sdo_upload_elmo_object( self, node_id: int, elmo_object_int: int, @@ -346,7 +346,7 @@ async def can_sdo_upload_elmo_object( ) -> str: obj_byte0 = elmo_object_int >> 8 obj_byte1 = elmo_object_int & 0xFF - data_bytes = await self.can_sdo_upload(node_id, obj_byte0, obj_byte1, sub_index) + data_bytes = await self._can_sdo_upload(node_id, obj_byte0, obj_byte1, sub_index) if len(data_bytes) == 0: return "" @@ -368,7 +368,7 @@ async def can_sdo_upload_elmo_object( return "".join(chr(b) for b in data_bytes) raise CanError(f"Unsupported data type for SDO Read conversion: {data_type.name}") - async def can_sdo_download_elmo_object( + async def _can_sdo_download_elmo_object( self, node_id: int, elmo_object_int: int, @@ -399,7 +399,7 @@ async def can_sdo_download_elmo_object( obj_byte0 = elmo_object_int >> 8 obj_byte1 = elmo_object_int & 0xFF - await self.can_sdo_download(node_id, obj_byte0, obj_byte1, sub_index, data_bytes) + await self._can_sdo_download(node_id, obj_byte0, obj_byte1, sub_index, data_bytes) # --- Elmo binary interpreter (vendor protocol on TPDO2/RPDO2) ------------ @@ -584,20 +584,20 @@ async def os_interpreter( # --- raw CANopen sends (SYNC + RPDO1 controlword) ----------------------- - async def can_sync(self) -> None: + async def _can_sync(self) -> None: if self._network is None: - raise CanError("can_sync called before setup()") + raise CanError("_can_sync called before setup()") # SYNC object (0x080), no data. self._network.send_message(0x80, b"") - async def control_word_set(self, node_id: int, value: int, sync: bool = True) -> None: + async def _control_word_set(self, node_id: int, value: int, sync: bool = True) -> None: if self._network is None: - raise CanError("control_word_set called before setup()") + raise CanError("_control_word_set called before setup()") val_bytes = value.to_bytes(2, byteorder="little") # RPDO1 COB-ID = (4 << 7) | node_id = 0x200 + node_id self._network.send_message(0x200 + node_id, val_bytes) if sync: - await self.can_sync() + await self._can_sync() # --- DS402 / motor control ---------------------------------------------- @@ -613,7 +613,7 @@ async def motor_get_motion_status(self, node_id: int) -> int: val = await self.binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) return int(round(float(val))) - async def motor_set_move_direction( + async def _motor_set_move_direction( self, node_id: int, direction: JointMoveDirection ) -> None: val_str = "1" @@ -623,11 +623,11 @@ async def motor_set_move_direction( val_str = "129" elif direction == JointMoveDirection.ShortestWay: val_str = "193" - await self.can_sdo_download_elmo_object( + await self._can_sdo_download_elmo_object( node_id, 24818, 0, val_str, ElmoObjectDataType.UNSIGNED16 ) - async def motor_set_homed_status(self, axis: KX2Axis, status: HomeStatus) -> None: + async def _motor_set_homed_status(self, axis: KX2Axis, status: HomeStatus) -> None: val = "0" if status == HomeStatus.Homed: val = "1" @@ -643,7 +643,7 @@ async def motor_get_homed_status(self, node_id: int) -> HomeStatus: return HomeStatus.InitializedWithoutHoming return HomeStatus.NotHomed - async def motor_reset_encoder_position(self, axis: KX2Axis, position: float) -> None: + async def _motor_reset_encoder_position(self, axis: KX2Axis, position: float) -> None: await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") await self.binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "0") await self.binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") @@ -714,7 +714,7 @@ async def motor_get_fault(self, axis: KX2Axis) -> Optional[str]: return f"Unknown fault code: {val} (0x{val:08X})" return " ".join(faults) - async def motor_enable(self, axis: KX2Axis, state: bool) -> None: + async def _motor_enable(self, axis: KX2Axis, state: bool) -> None: if not isinstance(axis, KX2Axis): raise TypeError(f"axis must be KX2Axis, got {type(axis).__name__}") @@ -733,7 +733,7 @@ async def motor_enable(self, axis: KX2Axis, state: bool) -> None: # provided one, but `network.send_message` is synchronous so we insert # our own small delay. for cw in (0, 128, 6, 7, 15): - await self.control_word_set(node_id=int(axis), value=cw) + await self._control_word_set(node_id=int(axis), value=cw) await asyncio.sleep(0.01) await asyncio.sleep(0.1) left = await self.binary_interpreter( @@ -751,9 +751,9 @@ async def motor_enable(self, axis: KX2Axis, state: bool) -> None: pass else: # DS402 disable: Op Enabled -> Switched On -> Ready to Switch On. - await self.control_word_set(node_id=int(axis), value=7) + await self._control_word_set(node_id=int(axis), value=7) await asyncio.sleep(0.01) - await self.control_word_set(node_id=int(axis), value=6) + await self._control_word_set(node_id=int(axis), value=6) await asyncio.sleep(0.1) left = await self.binary_interpreter( node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery @@ -763,24 +763,24 @@ async def motor_enable(self, axis: KX2Axis, state: bool) -> None: # --- motion primitives -------------------------------------------------- - async def pvt_select_mode(self, enable: bool) -> None: + async def _pvt_select_mode(self, enable: bool) -> None: """Enable/disable PVT mode on all motion axes via standard SDO writes.""" if enable: if not self._pvt_mode: for axis in MOTION_AXES: # 0x60C4 sub 6 = 0 (disable interpolation buffer) - await self.can_sdo_download(int(axis), 0x60, 0xC4, 0x06, [0]) + await self._can_sdo_download(int(axis), 0x60, 0xC4, 0x06, [0]) # 0x6060 = 7 (interpolated position mode) - await self.can_sdo_download(int(axis), 0x60, 0x60, 0x00, [7]) + await self._can_sdo_download(int(axis), 0x60, 0x60, 0x00, [7]) self._pvt_mode = True else: for axis in MOTION_AXES: - await self.can_sdo_download(int(axis), 0x60, 0x60, 0x00, [1]) + await self._can_sdo_download(int(axis), 0x60, 0x60, 0x00, [1]) else: if self._pvt_mode: for axis in MOTION_AXES: # 0x6060 = 1 (profile position mode) - await self.can_sdo_download(int(axis), 0x60, 0x60, 0x00, [1]) + await self._can_sdo_download(int(axis), 0x60, 0x60, 0x00, [1]) self._pvt_mode = False async def _wait_for_moves_done(self, timeout: float) -> None: @@ -803,38 +803,38 @@ async def _motors_move_start( relative_bit = 0x40 if relative else 0 for i, nid in enumerate(axes): last = i == (len(axes) - 1) - await self.control_word_set(int(nid), 47 + relative_bit, sync=last) + await self._control_word_set(int(nid), 47 + relative_bit, sync=last) for i, nid in enumerate(axes): last = i == (len(axes) - 1) - await self.control_word_set(int(nid), 47 + 0x10 + relative_bit, sync=last) + await self._control_word_set(int(nid), 47 + 0x10 + relative_bit, sync=last) - async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: - await self.pvt_select_mode(False) + async def _motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: + await self._pvt_select_mode(False) for move in plan.moves: - await self.motor_set_move_direction(move.axis.value, move.direction) + await self._motor_set_move_direction(move.axis.value, move.direction) # 0x607A = Target Position (24698 decimal) - await self.can_sdo_download_elmo_object( + await self._can_sdo_download_elmo_object( move.axis.value, 24698, 0, str(int(move.position)), ElmoObjectDataType.INTEGER32, ) # 0x6081 = Profile Velocity (24705 decimal) - await self.can_sdo_download_elmo_object( + await self._can_sdo_download_elmo_object( move.axis.value, 24705, 0, str(int(move.velocity)), ElmoObjectDataType.UNSIGNED32, ) acc = max(int(move.acceleration), 100) # 0x6083 = Profile Acceleration (24707 decimal) - await self.can_sdo_download_elmo_object( + await self._can_sdo_download_elmo_object( move.axis.value, 24707, 0, str(acc), ElmoObjectDataType.UNSIGNED32, ) # 0x6084 = Profile Deceleration (24708 decimal) - await self.can_sdo_download_elmo_object( + await self._can_sdo_download_elmo_object( move.axis.value, 24708, 0, str(acc), ElmoObjectDataType.UNSIGNED32, ) await self._motors_move_start([move.axis for move in plan.moves]) await self._wait_for_moves_done(timeout=plan.move_time + 2) - async def user_program_run( + async def _user_program_run( self, axis: KX2Axis, user_function: str, @@ -872,7 +872,7 @@ async def user_program_run( await self.binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="1") cmd = f"XQ##{user_function}{arg_str}" - logger.debug("user_program_run: %s", cmd) + logger.debug("_user_program_run: %s", cmd) await self.os_interpreter(node_id, cmd, query=False) last_line_completed = 0 @@ -904,7 +904,7 @@ async def user_program_run( return 0 - async def motor_hard_stop_search( + async def _motor_hard_stop_search( self, axis: KX2Axis, srch_vel: int, @@ -922,7 +922,7 @@ async def motor_hard_stop_search( try: params = [str(int(hs_pe)), str(int(timeout * 1000))] - last_line = await self.user_program_run(axis, "Home", params, int(timeout), True) + last_line = await self._user_program_run(axis, "Home", params, int(timeout), True) if last_line in [1, 2, 3]: raise RuntimeError(f"Homing Script Error {34 + last_line}") @@ -937,7 +937,7 @@ async def motor_hard_stop_search( await asyncio.sleep(0.3) await self.binary_interpreter(int(axis), "ER", 3, CmdType.ValSet, str(int(max_pe))) - async def motor_index_search( + async def _motor_index_search( self, axis: KX2Axis, srch_vel: int, @@ -976,7 +976,7 @@ async def motor_index_search( captured_position = int(float(cap)) return one_revolution, captured_position - async def home_motor( + async def _home_motor( self, axis: KX2Axis, hs_offset: int, @@ -995,16 +995,16 @@ async def home_motor( raise RuntimeError("Error 43") try: - await self.motor_hard_stop_search(axis, srch_vel, srch_acc, max_pe, hs_pe, timeout) + await self._motor_hard_stop_search(axis, srch_vel, srch_acc, max_pe, hs_pe, timeout) except Exception as e: fault = await self.motor_get_fault(axis) if fault is not None: raise RuntimeError(fault) raise e - await self.motor_enable(axis=axis, state=True) + await self._motor_enable(axis=axis, state=True) - await self.motors_move_absolute_execute( + await self._motors_move_absolute_execute( plan=MotorsMovePlan( moves=[ MotorMoveParam( @@ -1020,9 +1020,9 @@ async def home_motor( ) is_positive = hs_offset > 0 - await self.motor_index_search(axis, abs(srch_vel), srch_acc, is_positive, timeout) + await self._motor_index_search(axis, abs(srch_vel), srch_acc, is_positive, timeout) - await self.motors_move_absolute_execute( + await self._motors_move_absolute_execute( plan=MotorsMovePlan( moves=[ MotorMoveParam( @@ -1036,8 +1036,8 @@ async def home_motor( ] ) ) - await self.motor_reset_encoder_position(axis, home_pos) - await self.motor_set_homed_status(axis, HomeStatus.Homed) + await self._motor_reset_encoder_position(axis, home_pos) + await self._motor_set_homed_status(axis, HomeStatus.Homed) # --- I/O ----------------------------------------------------------------- From 18dd60509dbc11e761f3413f11133fe7697c8a62 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 16:46:47 -0700 Subject: [PATCH 21/93] privatize interpreters + I/O on driver - \`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) --- pylabrobot/paa/kx2/kx2_driver.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_driver.py b/pylabrobot/paa/kx2/kx2_driver.py index dfab2926641..725323bad62 100644 --- a/pylabrobot/paa/kx2/kx2_driver.py +++ b/pylabrobot/paa/kx2/kx2_driver.py @@ -432,7 +432,7 @@ def _dispatch_bi_response(self, node_id: int, data: bytes) -> None: if fut is not None and not fut.done(): fut.set_result(value_str) - async def binary_interpreter( + async def _binary_interpreter( self, node_id: int, cmd: str, @@ -535,7 +535,7 @@ def _float_matches(expected_str: str, actual_str: str) -> bool: ) return float(value) if is_float else int(float(value)) - async def os_interpreter( + async def _os_interpreter( self, node_id: int, cmd: str, @@ -1041,16 +1041,16 @@ async def _home_motor( # --- I/O ----------------------------------------------------------------- - async def read_input(self, node_id: int, input_num: int) -> bool: + async def _read_input(self, node_id: int, input_num: int) -> bool: left = await self.binary_interpreter(node_id, "IB", input_num, CmdType.ValQuery) return left == 1 - async def read_output(self, node_id: int, output_num: int) -> bool: + async def _read_output(self, node_id: int, output_num: int) -> bool: expression = await self.binary_interpreter(node_id, "OP", 0, CmdType.ValQuery) val = int(expression) mask = 1 << (output_num - 1) return (val & mask) == mask - async def set_output(self, node_id: int, output_num: int, state: bool) -> Union[str, float]: + async def _set_output(self, node_id: int, output_num: int, state: bool) -> Union[str, float]: val = "1" if state else "0" return await self.binary_interpreter(node_id, "OB", output_num, CmdType.ValSet, val) From 8ee428c5db1c9561e6ca93d07fc1db0046526eff Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 16:54:53 -0700 Subject: [PATCH 22/93] move motor_send_command + get_estop_state to KX2Driver MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- docs/user_guide/paa/kx2/hello-world.ipynb | 19 +- pylabrobot/paa/kx2/kx2_backend.py | 216 ++++++---------------- pylabrobot/paa/kx2/kx2_driver.py | 190 +++++++++++++------ 3 files changed, 198 insertions(+), 227 deletions(-) diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index e51ed166988..8b59a712b51 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -412,24 +412,11 @@ }, { "cell_type": "code", - "execution_count": 6, + "execution_count": null, "id": "kx2-estop", "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "True" - ] - }, - "execution_count": 6, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "await kx2.arm.backend.get_estop_state()" - ] + "outputs": [], + "source": "await kx2.driver.get_estop_state()" }, { "cell_type": "code", diff --git a/pylabrobot/paa/kx2/kx2_backend.py b/pylabrobot/paa/kx2/kx2_backend.py index 89213f50908..883a335b4e6 100644 --- a/pylabrobot/paa/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/kx2_backend.py @@ -255,7 +255,7 @@ async def servo_gripper_initialize(self): await self.servo_gripper_close() async def servo_gripper_home(self) -> None: - await self.motor_send_command( + await self.driver.motor_send_command( node_id=int(KX2Axis.SERVO_GRIPPER), motor_command="PL", index=1, @@ -263,7 +263,7 @@ async def servo_gripper_home(self) -> None: val_type=ValType.Float, ) - await self.motor_send_command( + await self.driver.motor_send_command( node_id=int(KX2Axis.SERVO_GRIPPER), motor_command="CL", index=1, @@ -299,27 +299,27 @@ async def servo_gripper_set_default_gripping_force(self, max_force_percent: int) axis = KX2Axis.SERVO_GRIPPER # 1) PL with unscaled peak current - await self.motor_send_command( + await self.driver.motor_send_command( axis, "PL", 1, str(self.servo_gripper_peak_current), val_type=ValType.Float ) # 2) CL with scaled continuous current - await self.motor_send_command(axis, "CL", 1, str(cont_current), val_type=ValType.Float) + await self.driver.motor_send_command(axis, "CL", 1, str(cont_current), val_type=ValType.Float) # 3) PL with scaled peak current - await self.motor_send_command(axis, "PL", 1, str(peak_current), val_type=ValType.Float) + await self.driver.motor_send_command(axis, "PL", 1, str(peak_current), val_type=ValType.Float) self.servo_gripper_force_percent = max_force_percent async def get_servo_gripper_max_force(self) -> float: """Return current gripping force as percentage of max (0-1).""" - cl = await self.motor_send_command( + cl = await self.driver.motor_send_command( node_id=KX2Axis.SERVO_GRIPPER, motor_command="CL", index=1, ) - iq = await self.motor_send_command( + iq = await self.driver.motor_send_command( node_id=KX2Axis.SERVO_GRIPPER, motor_command="IQ", index=0, @@ -332,7 +332,7 @@ async def get_servo_gripper_max_force(self) -> float: async def check_plate_gripped(self, num_attempts: int = 5) -> None: for _ in range(num_attempts): - motor_status = await self.motor_send_command( + motor_status = await self.driver.motor_send_command( node_id=KX2Axis.SERVO_GRIPPER, motor_command="MS", index=1, @@ -400,7 +400,7 @@ async def drive_set_move_count_parameters( last_maintenance_performed_date_rail: int, ) -> None: # MoveCount -> Z axis, UI index 22 - await self.motor_send_command( + await self.driver.motor_send_command( node_id=KX2Axis.Z, motor_command="UI", index=22, @@ -417,7 +417,7 @@ async def drive_set_move_count_parameters( pairs = zip(self.node_id_list, travel) for node_id, dist in pairs: - await self.motor_send_command( + await self.driver.motor_send_command( node_id=int(node_id), motor_command="UF", index=5, @@ -427,7 +427,7 @@ async def drive_set_move_count_parameters( ) # LastMaintenancePerformed -> Z axis, UF index 6 - await self.motor_send_command( + await self.driver.motor_send_command( node_id=KX2Axis.Z, motor_command="UF", index=6, @@ -437,7 +437,7 @@ async def drive_set_move_count_parameters( ) # MaintenanceRequired -> Z axis, UI index 23 - await self.motor_send_command( + await self.driver.motor_send_command( node_id=KX2Axis.Z, motor_command="UI", index=23, @@ -447,7 +447,7 @@ async def drive_set_move_count_parameters( ) # LastMaintenancePerformedDate -> Z axis, UI index 21 - await self.motor_send_command( + await self.driver.motor_send_command( node_id=KX2Axis.Z, motor_command="UI", index=21, @@ -458,7 +458,7 @@ async def drive_set_move_count_parameters( # Rail (if present) if self.robot_on_rail: - await self.motor_send_command( + await self.driver.motor_send_command( node_id=KX2Axis.RAIL, motor_command="UF", index=6, @@ -467,7 +467,7 @@ async def drive_set_move_count_parameters( low_priority=True, ) - await self.motor_send_command( + await self.driver.motor_send_command( node_id=KX2Axis.RAIL, motor_command="UI", index=23, @@ -476,7 +476,7 @@ async def drive_set_move_count_parameters( low_priority=False, ) - await self.motor_send_command( + await self.driver.motor_send_command( node_id=KX2Axis.RAIL, motor_command="UI", index=21, @@ -502,7 +502,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: uis = { node for node in nodes - if node == await self.motor_send_command(node, "UI", 4, val_type=ValType.Int) + if node == await self.driver.motor_send_command(node, "UI", 4, val_type=ValType.Int) for node in nodes } for required_axis in MOTION_AXES: @@ -520,7 +520,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # UI[5..10] digital inputs for ui_idx in range(5, 11): - ret = await self.motor_send_command(axis, "UI", ui_idx, val_type=ValType.Int) + ret = await self.driver.motor_send_command(axis, "UI", ui_idx, val_type=ValType.Int) ch = (ui_idx - 5) + 1 if ret == 101: set2d(self.digital_input_assignment, axis, ch, "GripperSensor") @@ -540,7 +540,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # UI[11..12] analog inputs for ui_idx in range(11, 13): - ret = await self.motor_send_command(axis, "UI", ui_idx, val_type=ValType.Int) + ret = await self.driver.motor_send_command(axis, "UI", ui_idx, val_type=ValType.Int) ch = (ui_idx - 11) + 1 set2d( self.AnalogInputAssignment, @@ -551,7 +551,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # UI[13..16] outputs for ui_idx in range(13, 17): - ret = await self.motor_send_command(axis, "UI", ui_idx, val_type=ValType.Int) + ret = await self.driver.motor_send_command(axis, "UI", ui_idx, val_type=ValType.Int) ch = (ui_idx - 13) + 1 if ret == 101: @@ -583,14 +583,14 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: ) # UI[24] drive serial number - ret = await self.motor_send_command(axis, "UI", 24, val_type=ValType.Int) + ret = await self.driver.motor_send_command(axis, "UI", 24, val_type=ValType.Int) if _is_number(ret): # self.drive_serial_number[axis] = int(ret) pass # UF[1], UF[2] conversion factor - uf1 = await self.motor_send_command(axis, "UF", 1, val_type=ValType.Float) - uf2 = await self.motor_send_command(axis, "UF", 2, val_type=ValType.Float) + uf1 = await self.driver.motor_send_command(axis, "UF", 1, val_type=ValType.Float) + uf2 = await self.driver.motor_send_command(axis, "UF", 2, val_type=ValType.Float) if ( not (_is_number(uf1) and _is_number(uf2)) or _to_float(uf1) == 0.0 or _to_float(uf2) == 0.0 ): @@ -598,12 +598,12 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: self.motor_conversion_factor_ax[axis] = _to_float(uf1) / _to_float(uf2) # XM / travel - xm1 = await self.motor_send_command(axis, "XM", 1, val_type=ValType.Int) - xm2 = await self.motor_send_command(axis, "XM", 2, val_type=ValType.Int) - uf3 = await self.motor_send_command(axis, "UF", 3, val_type=ValType.Float) - uf4 = await self.motor_send_command(axis, "UF", 4, val_type=ValType.Float) - vh3 = await self.motor_send_command(axis, "VH", 3, val_type=ValType.Int) - vl3 = await self.motor_send_command(axis, "VL", 3, val_type=ValType.Int) + xm1 = await self.driver.motor_send_command(axis, "XM", 1, val_type=ValType.Int) + xm2 = await self.driver.motor_send_command(axis, "XM", 2, val_type=ValType.Int) + uf3 = await self.driver.motor_send_command(axis, "UF", 3, val_type=ValType.Float) + uf4 = await self.driver.motor_send_command(axis, "UF", 4, val_type=ValType.Float) + vh3 = await self.driver.motor_send_command(axis, "VH", 3, val_type=ValType.Int) + vl3 = await self.driver.motor_send_command(axis, "VL", 3, val_type=ValType.Int) self.max_travel_ax[axis] = _to_float(uf3) self.min_travel_ax[axis] = _to_float(uf4) @@ -626,12 +626,12 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: ) # Encoder socket/type - ca45 = await self.motor_send_command(axis, "CA", 45, val_type=ValType.Int) + ca45 = await self.driver.motor_send_command(axis, "CA", 45, val_type=ValType.Int) ca45v = _to_float(ca45, 0.0) if (not _is_number(ca45)) or not (0.0 < ca45v <= 4.0): raise CanError(f"Invalid encoder socket number received from axis {axis}. CA[45]={ca45}") - enc_type = await self.motor_send_command( + enc_type = await self.driver.motor_send_command( axis, "CA", int(round(40.0 + ca45v)), val_type=ValType.Int ) if enc_type in (1, 2): @@ -643,60 +643,60 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: f"Unsupported encoder type specified for axis {axis}. CA[4{ca45}]={enc_type}" ) - ca46 = await self.motor_send_command(axis, "CA", 46, val_type=ValType.Int) + ca46 = await self.driver.motor_send_command(axis, "CA", 46, val_type=ValType.Int) if ca45 == ca46: num3 = 1.0 else: - ff3 = await self.motor_send_command(axis, "FF", 3, val_type=ValType.Float) + ff3 = await self.driver.motor_send_command(axis, "FF", 3, val_type=ValType.Float) num3 = _to_float(ff3, 1.0) denom = self.motor_conversion_factor_ax[axis] * num3 # or 1.0 - sp2 = await self.motor_send_command(axis, "SP", 2, val_type=ValType.Int) + sp2 = await self.driver.motor_send_command(axis, "SP", 2, val_type=ValType.Int) if sp2 == 100000: - vh2 = await self.motor_send_command(axis, "VH", 2, val_type=ValType.Int) + vh2 = await self.driver.motor_send_command(axis, "VH", 2, val_type=ValType.Int) self.max_vel_ax[axis] = _to_float(vh2) / 1.01 / denom else: self.max_vel_ax[axis] = _to_float(sp2) / denom - sd0 = await self.motor_send_command(axis, "SD", 0, val_type=ValType.Int) + sd0 = await self.driver.motor_send_command(axis, "SD", 0, val_type=ValType.Int) self.max_accel_ax[axis] = _to_float(sd0) / 1.01 / denom # Robot-level params from shoulder_ax shoulder = KX2Axis.SHOULDER self.base_to_gripper_clearance_z = _to_float( - await self.motor_send_command(shoulder, "UF", 6, val_type=ValType.Float) + await self.driver.motor_send_command(shoulder, "UF", 6, val_type=ValType.Float) ) self.base_to_gripper_clearance_arm = _to_float( - await self.motor_send_command(shoulder, "UF", 7, val_type=ValType.Float) + await self.driver.motor_send_command(shoulder, "UF", 7, val_type=ValType.Float) ) self.wrist_offset = _to_float( - await self.motor_send_command(shoulder, "UF", 8, val_type=ValType.Float) + await self.driver.motor_send_command(shoulder, "UF", 8, val_type=ValType.Float) ) self.elbow_offset = _to_float( - await self.motor_send_command(shoulder, "UF", 9, val_type=ValType.Float) + await self.driver.motor_send_command(shoulder, "UF", 9, val_type=ValType.Float) ) self.elbow_zero_offset = _to_float( - await self.motor_send_command(shoulder, "UF", 10, val_type=ValType.Float) + await self.driver.motor_send_command(shoulder, "UF", 10, val_type=ValType.Float) ) self.MaxLinearVelMMPerSec = _to_float( - await self.motor_send_command(shoulder, "UF", 11, val_type=ValType.Float) + await self.driver.motor_send_command(shoulder, "UF", 11, val_type=ValType.Float) ) self.MaxLinearAccelMMPerSec2 = _to_float( - await self.motor_send_command(shoulder, "UF", 12, val_type=ValType.Float) + await self.driver.motor_send_command(shoulder, "UF", 12, val_type=ValType.Float) ) self.MaxLinearJerkMMPerSec3 = _to_float( - await self.motor_send_command(shoulder, "UF", 13, val_type=ValType.Float) + await self.driver.motor_send_command(shoulder, "UF", 13, val_type=ValType.Float) ) self.MaxRotaryVelDegPerSec = _to_float( - await self.motor_send_command(shoulder, "UF", 14, val_type=ValType.Float) + await self.driver.motor_send_command(shoulder, "UF", 14, val_type=ValType.Float) ) self.MaxRotaryAccelDegPerSec2 = _to_float( - await self.motor_send_command(shoulder, "UF", 15, val_type=ValType.Float) + await self.driver.motor_send_command(shoulder, "UF", 15, val_type=ValType.Float) ) - ui17 = await self.motor_send_command(shoulder, "UI", 17, val_type=ValType.Int) + ui17 = await self.driver.motor_send_command(shoulder, "UI", 17, val_type=ValType.Int) self.pvt_time_interval_msec = ( 25 if (not _is_number(ui17) or _to_float(ui17) <= 0.0 or _to_float(ui17) > 255.0) @@ -706,138 +706,42 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # Servo gripper params (only if present) sg = KX2Axis.SERVO_GRIPPER self.servo_gripper_home_pos = int( - await self.motor_send_command(sg, "UF", 6, val_type=ValType.Float) + await self.driver.motor_send_command(sg, "UF", 6, val_type=ValType.Float) ) self.servo_gripper_home_search_vel = int( - await self.motor_send_command(sg, "UF", 7, val_type=ValType.Float) + await self.driver.motor_send_command(sg, "UF", 7, val_type=ValType.Float) ) self.servo_gripper_home_search_accel = int( - await self.motor_send_command(sg, "UF", 8, val_type=ValType.Float) + await self.driver.motor_send_command(sg, "UF", 8, val_type=ValType.Float) ) self.servo_gripper_home_default_position_error = int( - await self.motor_send_command(sg, "UF", 9, val_type=ValType.Float) + await self.driver.motor_send_command(sg, "UF", 9, val_type=ValType.Float) ) self.servo_gripper_home_hard_stop_position_error = int( - await self.motor_send_command(sg, "UF", 10, val_type=ValType.Float) + await self.driver.motor_send_command(sg, "UF", 10, val_type=ValType.Float) ) self.servo_gripper_home_hard_stop_offset = int( - await self.motor_send_command(sg, "UF", 11, val_type=ValType.Float) + await self.driver.motor_send_command(sg, "UF", 11, val_type=ValType.Float) ) self.servo_gripper_home_index_offset = int( - await self.motor_send_command(sg, "UF", 12, val_type=ValType.Float) + await self.driver.motor_send_command(sg, "UF", 12, val_type=ValType.Float) ) self.servo_gripper_home_offset_vel = int( - await self.motor_send_command(sg, "UF", 13, val_type=ValType.Float) + await self.driver.motor_send_command(sg, "UF", 13, val_type=ValType.Float) ) self.servo_gripper_home_offset_accel = int( - await self.motor_send_command(sg, "UF", 14, val_type=ValType.Float) + await self.driver.motor_send_command(sg, "UF", 14, val_type=ValType.Float) ) self.servo_gripper_home_timeout_msec = int( - await self.motor_send_command(sg, "UF", 15, val_type=ValType.Float) + await self.driver.motor_send_command(sg, "UF", 15, val_type=ValType.Float) ) self.servo_gripper_continuous_current = _to_float( - await self.motor_send_command(sg, "UF", 16, val_type=ValType.Float) + await self.driver.motor_send_command(sg, "UF", 16, val_type=ValType.Float) ) self.servo_gripper_peak_current = _to_float( - await self.motor_send_command(sg, "UF", 17, val_type=ValType.Float) + await self.driver.motor_send_command(sg, "UF", 17, val_type=ValType.Float) ) - async def get_estop_state(self) -> bool: - """Return True if in estop, False otherwise.""" - r = await self.motor_send_command( - node_id=KX2Axis.SHOULDER, - motor_command="SR", - index=1, - value="", - ) - r = int(r) - num2 = not (r & 0x4000 == 0x4000) - num3 = not (r & 0x8000 == 0x8000) - if r != 8438016: - logger.warning("get_estop_state: SR register unexpected value %d (expected 8438016)", r) - return num2 == False and num3 == False - - async def motor_send_command( - self, - node_id: int, - motor_command: str, - index: int, - value: str = "", - val_type: ValType = ValType.Int, - *, - low_priority: bool = False, - ) -> str: - if isinstance(node_id, KX2Axis): - node_id = int(node_id) - logger.debug( - "motor_send_command node=%d cmd=%s[%d] value=%r val_type=%s", - node_id, motor_command, index, value, val_type, - ) - - cmd_u = motor_command.upper() - OS_CMDS = {"VR", "CD", "LS", "DL", "DF", "BH"} - - has_xc = "XC##" in cmd_u - has_xq = "XQ##" in cmd_u - use_os = (cmd_u in OS_CMDS) or has_xc or has_xq - - returned_data = "" - - cmd_u = motor_command.upper() - - NO_QUERY_CMDS = { - "BG", - "CP", - "EI", - "EO", - "HP", - "HX", - "KL", - "KR", - "LD", - "MI", - "PB", - "RS", - "SV", - "XC##", - } - - if value == "": - if (cmd_u in NO_QUERY_CMDS) or ("XQ##" in cmd_u): - cmd_type = CmdType.Execute - else: - cmd_type = CmdType.ValQuery - else: - cmd_type = CmdType.ValSet - - if use_os: - query_flag = not (has_xc or has_xq) - - # OSInterpreter writes into `str` and can also write a long; you can ignore the long if you don't use it. - s = await self.driver.os_interpreter( - node_id=(node_id), - cmd=motor_command, - query=query_flag, - ) - - if cmd_type == CmdType.ValQuery: - returned_data = s if s is not None else "" - else: - s = await self.driver.binary_interpreter( - node_id=(node_id), - cmd=motor_command, - cmd_index=int(index), - cmd_type=cmd_type, - value=value, - val_type=val_type, - low_priority=low_priority, - ) - - if cmd_type == CmdType.ValQuery: - returned_data = s - - return returned_data - def convert_elbow_position_to_angle(self, elbow_pos: float) -> float: max_travel = self.max_travel_ax[KX2Axis.ELBOW] denom = max_travel + self.elbow_zero_offset @@ -876,7 +780,7 @@ async def motor_get_current_position(self, axis: KX2Axis) -> float: return raw / c async def read_input(self, axis: int, input_num: int) -> bool: - return await self.driver.read_input(node_id=axis, input_num=0x10 + input_num) + return await self.driver._read_input(node_id=axis, input_num=0x10 + input_num) @staticmethod def _wrap_to_range(x: float, lo: float, hi: float) -> float: diff --git a/pylabrobot/paa/kx2/kx2_driver.py b/pylabrobot/paa/kx2/kx2_driver.py index 725323bad62..122df35992b 100644 --- a/pylabrobot/paa/kx2/kx2_driver.py +++ b/pylabrobot/paa/kx2/kx2_driver.py @@ -599,18 +599,98 @@ async def _control_word_set(self, node_id: int, value: int, sync: bool = True) - if sync: await self._can_sync() + # --- high-level motor command dispatch ---------------------------------- + + _OS_INTERPRETER_CMDS = {"VR", "CD", "LS", "DL", "DF", "BH"} + _NO_QUERY_CMDS = { + "BG", "CP", "EI", "EO", "HP", "HX", "KL", "KR", + "LD", "MI", "PB", "RS", "SV", "XC##", + } + + async def motor_send_command( + self, + node_id: int, + motor_command: str, + index: int, + value: str = "", + val_type: ValType = ValType.Int, + *, + low_priority: bool = False, + ) -> str: + """Send an Elmo motor command, auto-routing between the OS and binary interpreters. + + Classifies by command name to decide which interpreter to use, and by + whether a value was given + whether it's a no-reply command to pick + `ValQuery` / `ValSet` / `Execute`. Returns the drive's response string + for queries; empty string otherwise. + """ + if isinstance(node_id, KX2Axis): + node_id = int(node_id) + logger.debug( + "motor_send_command node=%d cmd=%s[%d] value=%r val_type=%s", + node_id, motor_command, index, value, val_type, + ) + + cmd_u = motor_command.upper() + has_xc = "XC##" in cmd_u + has_xq = "XQ##" in cmd_u + use_os = (cmd_u in self._OS_INTERPRETER_CMDS) or has_xc or has_xq + + if value == "": + if (cmd_u in self._NO_QUERY_CMDS) or has_xq: + cmd_type = CmdType.Execute + else: + cmd_type = CmdType.ValQuery + else: + cmd_type = CmdType.ValSet + + if use_os: + reply = await self._os_interpreter( + node_id=node_id, cmd=motor_command, query=not (has_xc or has_xq) + ) + return reply if (cmd_type == CmdType.ValQuery and reply is not None) else "" + + reply = await self._binary_interpreter( + node_id=node_id, + cmd=motor_command, + cmd_index=int(index), + cmd_type=cmd_type, + value=value, + val_type=val_type, + low_priority=low_priority, + ) + return str(reply) if cmd_type == CmdType.ValQuery else "" + + async def get_estop_state(self) -> bool: + """Return True if the arm is in estop, False otherwise. + + Reads the shoulder drive's SR (status register) via the binary + interpreter. Bits 14/15 encode the stop/safety state. + """ + r = int(await self.motor_send_command( + node_id=int(KX2Axis.SHOULDER), + motor_command="SR", + index=1, + value="", + )) + if r != 8438016: + logger.warning("get_estop_state: SR register unexpected value %d (expected 8438016)", r) + b14 = (r & 0x4000) == 0x4000 + b15 = (r & 0x8000) == 0x8000 + return (not b14) and (not b15) + # --- DS402 / motor control ---------------------------------------------- async def motor_emergency_stop(self, node_id: int) -> None: - await self.binary_interpreter(node_id, "MO", 0, CmdType.ValSet, "0") + await self._binary_interpreter(node_id, "MO", 0, CmdType.ValSet, "0") async def motor_get_current_position(self, node_id: int, pu: bool = False) -> int: cmd = "PU" if pu else "PX" - val_str = await self.binary_interpreter(int(node_id), cmd, 0, CmdType.ValQuery) + val_str = await self._binary_interpreter(int(node_id), cmd, 0, CmdType.ValQuery) return int(round(float(val_str))) async def motor_get_motion_status(self, node_id: int) -> int: - val = await self.binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) + val = await self._binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) return int(round(float(val))) async def _motor_set_move_direction( @@ -633,10 +713,10 @@ async def _motor_set_homed_status(self, axis: KX2Axis, status: HomeStatus) -> No val = "1" elif status == HomeStatus.InitializedWithoutHoming: val = "2" - await self.binary_interpreter(int(axis), "UI", 3, CmdType.ValSet, val) + await self._binary_interpreter(int(axis), "UI", 3, CmdType.ValSet, val) async def motor_get_homed_status(self, node_id: int) -> HomeStatus: - left = await self.binary_interpreter(node_id, "UI", 3, CmdType.ValQuery) + left = await self._binary_interpreter(node_id, "UI", 3, CmdType.ValQuery) if left == 1: return HomeStatus.Homed if left == 2: @@ -644,19 +724,19 @@ async def motor_get_homed_status(self, node_id: int) -> HomeStatus: return HomeStatus.NotHomed async def _motor_reset_encoder_position(self, axis: KX2Axis, position: float) -> None: - await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, str(position)) - await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") + await self._binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") + await self._binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "0") + await self._binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") + await self._binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") + await self._binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, str(position)) + await self._binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") async def motor_check_if_move_done(self, node_id: int) -> bool: - ms_val = await self.binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) + ms_val = await self._binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) if ms_val == 0: return True if ms_val == 1: - mo_val = await self.binary_interpreter(node_id, "MO", 0, CmdType.ValQuery) + mo_val = await self._binary_interpreter(node_id, "MO", 0, CmdType.ValQuery) if mo_val == 1: return True fault = await self.motor_get_fault(node_id) @@ -668,7 +748,7 @@ async def motor_check_if_move_done(self, node_id: int) -> bool: return False async def motor_get_fault(self, axis: KX2Axis) -> Optional[str]: - val = await self.binary_interpreter(int(axis), "MF", 0, CmdType.ValQuery) + val = await self._binary_interpreter(int(axis), "MF", 0, CmdType.ValQuery) if val == 0: return None assert isinstance(val, int) @@ -725,7 +805,7 @@ async def _motor_enable(self, axis: KX2Axis, state: bool) -> None: if state: self.EmcyMoveErrorReceived = False if use_bi: - await self.binary_interpreter(axis, "MO", 0, CmdType.ValSet, "1") + await self._binary_interpreter(axis, "MO", 0, CmdType.ValSet, "1") else: # DS402 enable sequence: Fault -> Shutdown -> Switched On -> Op Enabled. # The drive needs ≥1 CANopen cycle between transitions to update its @@ -736,7 +816,7 @@ async def _motor_enable(self, axis: KX2Axis, state: bool) -> None: await self._control_word_set(node_id=int(axis), value=cw) await asyncio.sleep(0.01) await asyncio.sleep(0.1) - left = await self.binary_interpreter( + left = await self._binary_interpreter( node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery ) if left != 1: @@ -744,7 +824,7 @@ async def _motor_enable(self, axis: KX2Axis, state: bool) -> None: else: if use_bi: try: - await self.binary_interpreter( + await self._binary_interpreter( node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValSet, value="0" ) except Exception: @@ -755,7 +835,7 @@ async def _motor_enable(self, axis: KX2Axis, state: bool) -> None: await asyncio.sleep(0.01) await self._control_word_set(node_id=int(axis), value=6) await asyncio.sleep(0.1) - left = await self.binary_interpreter( + left = await self._binary_interpreter( node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery ) if left != 0: @@ -848,15 +928,15 @@ async def _user_program_run( raise ValueError("axis must be in [0, 255]") node_id = int(axis) - ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + ps = int(await self._binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) if ps == -2: raise CanError(f"Axis {axis}: controller reported PS=-2 (not ready / unavailable)") if ps != -1: - await self.binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="0") + await self._binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="0") t0 = time.monotonic() while (time.monotonic() - t0) < 3.0: - ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + ps = int(await self._binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) if ps == -1: break await asyncio.sleep(0.01) @@ -869,11 +949,11 @@ async def _user_program_run( if parts: arg_str = f"({','.join(parts)})" - await self.binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="1") + await self._binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="1") cmd = f"XQ##{user_function}{arg_str}" logger.debug("_user_program_run: %s", cmd) - await self.os_interpreter(node_id, cmd, query=False) + await self._os_interpreter(node_id, cmd, query=False) last_line_completed = 0 if wait_until_done: @@ -881,11 +961,11 @@ async def _user_program_run( ps = 1 ui1 = 1 while ps == 1 and ui1 == 1 and (time.monotonic() - t0) < float(timeout_sec): - ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) - ui1 = int(await self.binary_interpreter(node_id, "UI", 1, CmdType.ValQuery)) + ps = int(await self._binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + ui1 = int(await self._binary_interpreter(node_id, "UI", 1, CmdType.ValQuery)) await asyncio.sleep(0.01) - expr_raw = await self.binary_interpreter(node_id, "UI", 2, CmdType.ValQuery) + expr_raw = await self._binary_interpreter(node_id, "UI", 2, CmdType.ValQuery) try: last_line_completed = int(str(expr_raw).strip()) except Exception: @@ -913,12 +993,12 @@ async def _motor_hard_stop_search( hs_pe: int, timeout: float, ) -> None: - await self.binary_interpreter(int(axis), "ER", 3, CmdType.ValSet, str(max_pe * 10)) - await self.binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) - await self.binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) + await self._binary_interpreter(int(axis), "ER", 3, CmdType.ValSet, str(max_pe * 10)) + await self._binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) + await self._binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) for i in [3, 4, 5, 2]: - await self.binary_interpreter(int(axis), "HM", i, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "JV", 0, CmdType.ValSet, str(srch_vel)) + await self._binary_interpreter(int(axis), "HM", i, CmdType.ValSet, "0") + await self._binary_interpreter(int(axis), "JV", 0, CmdType.ValSet, str(srch_vel)) try: params = [str(int(hs_pe)), str(int(timeout * 1000))] @@ -927,15 +1007,15 @@ async def _motor_hard_stop_search( raise RuntimeError(f"Homing Script Error {34 + last_line}") curr_pos = await self.motor_get_current_position(int(axis)) - await self.binary_interpreter(int(axis), "PA", 0, CmdType.ValSet, str(curr_pos)) - await self.binary_interpreter(int(axis), "SP", 0, CmdType.ValSet, str(srch_vel)) - await self.binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) - await self.binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) + await self._binary_interpreter(int(axis), "PA", 0, CmdType.ValSet, str(curr_pos)) + await self._binary_interpreter(int(axis), "SP", 0, CmdType.ValSet, str(srch_vel)) + await self._binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) + await self._binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) finally: await asyncio.sleep(0.3) - await self.binary_interpreter(int(axis), "BG", 0, CmdType.Execute, value="0") + await self._binary_interpreter(int(axis), "BG", 0, CmdType.Execute, value="0") await asyncio.sleep(0.3) - await self.binary_interpreter(int(axis), "ER", 3, CmdType.ValSet, str(int(max_pe))) + await self._binary_interpreter(int(axis), "ER", 3, CmdType.ValSet, str(int(max_pe))) async def _motor_index_search( self, @@ -946,33 +1026,33 @@ async def _motor_index_search( timeout: float, ) -> Tuple[int, int]: assert self._loop is not None - await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") + await self._binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") - rev = await self.binary_interpreter(int(axis), "CA", 18, CmdType.ValQuery) + rev = await self._binary_interpreter(int(axis), "CA", 18, CmdType.ValQuery) one_revolution = int(float(rev)) if not positive_direction: one_revolution *= -1 - await self.binary_interpreter(int(axis), "PR", 1, CmdType.ValSet, str(one_revolution)) - await self.binary_interpreter(int(axis), "SP", 0, CmdType.ValSet, str(srch_vel)) - await self.binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) - await self.binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) + await self._binary_interpreter(int(axis), "PR", 1, CmdType.ValSet, str(one_revolution)) + await self._binary_interpreter(int(axis), "SP", 0, CmdType.ValSet, str(srch_vel)) + await self._binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) + await self._binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) - await self.binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "3") # index only - await self.binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, "0") - await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") # arm + await self._binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "3") # index only + await self._binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") + await self._binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") + await self._binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, "0") + await self._binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") # arm self._waiting_moves = {axis: self._loop.create_future()} - await self.binary_interpreter(int(axis), "BG", 0, CmdType.Execute) + await self._binary_interpreter(int(axis), "BG", 0, CmdType.Execute) await self._wait_for_moves_done(timeout) - left = await self.binary_interpreter(int(axis), "HM", 1, CmdType.ValQuery) + left = await self._binary_interpreter(int(axis), "HM", 1, CmdType.ValQuery) if left != 0: raise RuntimeError("Homing Failure: Failed to finish index pulse search.") - cap = await self.binary_interpreter(int(axis), "HM", 7, CmdType.ValQuery) + cap = await self._binary_interpreter(int(axis), "HM", 7, CmdType.ValQuery) captured_position = int(float(cap)) return one_revolution, captured_position @@ -990,7 +1070,7 @@ async def _home_motor( offset_acc: int, timeout: float, ) -> None: - left = await self.binary_interpreter(int(axis), "CA", 41, CmdType.ValQuery) + left = await self._binary_interpreter(int(axis), "CA", 41, CmdType.ValQuery) if left == 24: raise RuntimeError("Error 43") @@ -1042,15 +1122,15 @@ async def _home_motor( # --- I/O ----------------------------------------------------------------- async def _read_input(self, node_id: int, input_num: int) -> bool: - left = await self.binary_interpreter(node_id, "IB", input_num, CmdType.ValQuery) + left = await self._binary_interpreter(node_id, "IB", input_num, CmdType.ValQuery) return left == 1 async def _read_output(self, node_id: int, output_num: int) -> bool: - expression = await self.binary_interpreter(node_id, "OP", 0, CmdType.ValQuery) + expression = await self._binary_interpreter(node_id, "OP", 0, CmdType.ValQuery) val = int(expression) mask = 1 << (output_num - 1) return (val & mask) == mask async def _set_output(self, node_id: int, output_num: int, state: bool) -> Union[str, float]: val = "1" if state else "0" - return await self.binary_interpreter(node_id, "OB", output_num, CmdType.ValSet, val) + return await self._binary_interpreter(node_id, "OB", output_num, CmdType.ValSet, val) From 62f4f5f43dd450bc4c5b69764d4eb3f1d5165b12 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 16:56:52 -0700 Subject: [PATCH 23/93] un-privatize driver methods consumed by KX2ArmBackend MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/kx2_backend.py | 16 ++++++++-------- pylabrobot/paa/kx2/kx2_driver.py | 16 ++++++++-------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_backend.py b/pylabrobot/paa/kx2/kx2_backend.py index 883a335b4e6..3768401bd5c 100644 --- a/pylabrobot/paa/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/kx2_backend.py @@ -226,7 +226,7 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): # Device.setup(). Now read per-drive parameters, finish CANopen mapping, # enable motion axes, and initialize the servo gripper. await self.drive_get_parameters(self.driver.node_id_list) - await self.driver._connect_part_two() + await self.driver.connect_part_two() await asyncio.sleep(2) @@ -236,7 +236,7 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): for axis in MOTION_AXES: try: - await self.driver._motor_enable(axis=axis, state=True) + await self.driver.motor_enable(axis=axis, state=True) except Exception as e: logger.warning("Error enabling motor on axis %s: %s", axis, e) @@ -244,7 +244,7 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): async def servo_gripper_initialize(self): try: - await self.driver._motor_enable(axis=KX2Axis.SERVO_GRIPPER, state=True) + await self.driver.motor_enable(axis=KX2Axis.SERVO_GRIPPER, state=True) except Exception as e: logger.warning( "Error enabling servo gripper motor on node %s: %s", KX2Axis.SERVO_GRIPPER, e @@ -271,7 +271,7 @@ async def servo_gripper_home(self) -> None: val_type=ValType.Float, ) - await self.driver._home_motor( + await self.driver.home_motor( axis=KX2Axis.SERVO_GRIPPER, hs_offset=self.servo_gripper_home_hard_stop_offset, ind_offset=self.servo_gripper_home_index_offset, @@ -780,7 +780,7 @@ async def motor_get_current_position(self, axis: KX2Axis) -> float: return raw / c async def read_input(self, axis: int, input_num: int) -> bool: - return await self.driver._read_input(node_id=axis, input_num=0x10 + input_num) + return await self.driver.read_input(node_id=axis, input_num=0x10 + input_num) @staticmethod def _wrap_to_range(x: float, lo: float, hi: float) -> float: @@ -1021,7 +1021,7 @@ async def motors_move_joint( if plan is None: # if every axis is skipped, exit return - await self.driver._motors_move_absolute_execute(plan) + await self.driver.motors_move_absolute_execute(plan) def convert_cartesian_to_joint_position(self, pose: GripperPose) -> Dict["KX2Axis", float]: if pose.rotation.x != 0 or pose.rotation.y != 0: @@ -1289,8 +1289,8 @@ async def start_freedrive_mode( # KX2 frees all motion axes at once; free_axes is accepted for API parity. del free_axes for axis in MOTION_AXES: - await self.driver._motor_enable(axis=axis, state=False) + await self.driver.motor_enable(axis=axis, state=False) async def stop_freedrive_mode(self, backend_params: Optional[BackendParams] = None) -> None: for axis in MOTION_AXES: - await self.driver._motor_enable(axis=axis, state=True) + await self.driver.motor_enable(axis=axis, state=True) diff --git a/pylabrobot/paa/kx2/kx2_driver.py b/pylabrobot/paa/kx2/kx2_driver.py index 122df35992b..b11ac5509df 100644 --- a/pylabrobot/paa/kx2/kx2_driver.py +++ b/pylabrobot/paa/kx2/kx2_driver.py @@ -148,7 +148,7 @@ async def stop(self) -> None: # --- drive init (called by KX2ArmBackend._on_setup after setup()) -------- - async def _connect_part_two(self) -> None: + async def connect_part_two(self) -> None: """Configure PDO mapping + Elmo DS402 parameters after the CAN bus is up. Mirrors the legacy driver: unmap TPDO1, map TPDO3 (StatusWord, triggered @@ -794,7 +794,7 @@ async def motor_get_fault(self, axis: KX2Axis) -> Optional[str]: return f"Unknown fault code: {val} (0x{val:08X})" return " ".join(faults) - async def _motor_enable(self, axis: KX2Axis, state: bool) -> None: + async def motor_enable(self, axis: KX2Axis, state: bool) -> None: if not isinstance(axis, KX2Axis): raise TypeError(f"axis must be KX2Axis, got {type(axis).__name__}") @@ -888,7 +888,7 @@ async def _motors_move_start( last = i == (len(axes) - 1) await self._control_word_set(int(nid), 47 + 0x10 + relative_bit, sync=last) - async def _motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: + async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: await self._pvt_select_mode(False) for move in plan.moves: @@ -1056,7 +1056,7 @@ async def _motor_index_search( captured_position = int(float(cap)) return one_revolution, captured_position - async def _home_motor( + async def home_motor( self, axis: KX2Axis, hs_offset: int, @@ -1082,9 +1082,9 @@ async def _home_motor( raise RuntimeError(fault) raise e - await self._motor_enable(axis=axis, state=True) + await self.motor_enable(axis=axis, state=True) - await self._motors_move_absolute_execute( + await self.motors_move_absolute_execute( plan=MotorsMovePlan( moves=[ MotorMoveParam( @@ -1102,7 +1102,7 @@ async def _home_motor( is_positive = hs_offset > 0 await self._motor_index_search(axis, abs(srch_vel), srch_acc, is_positive, timeout) - await self._motors_move_absolute_execute( + await self.motors_move_absolute_execute( plan=MotorsMovePlan( moves=[ MotorMoveParam( @@ -1121,7 +1121,7 @@ async def _home_motor( # --- I/O ----------------------------------------------------------------- - async def _read_input(self, node_id: int, input_num: int) -> bool: + async def read_input(self, node_id: int, input_num: int) -> bool: left = await self._binary_interpreter(node_id, "IB", input_num, CmdType.ValQuery) return left == 1 From 08e5e7be31667a5538ea17faf885dafe3e63f883 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 14 Apr 2026 17:03:45 -0700 Subject: [PATCH 24/93] add KX2BarcodeReader for the onboard Microscan-style serial reader MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 \`\\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) --- docs/user_guide/paa/kx2/hello-world.ipynb | 14 ++ pylabrobot/paa/kx2/__init__.py | 5 + pylabrobot/paa/kx2/kx2_barcode_reader.py | 191 ++++++++++++++++++++++ 3 files changed, 210 insertions(+) create mode 100644 pylabrobot/paa/kx2/kx2_barcode_reader.py diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 8b59a712b51..c9193f1b679 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -426,6 +426,20 @@ "outputs": [], "source": "from pylabrobot.paa.kx2 import KX2Axis\n\nawait kx2.driver.motor_get_fault(KX2Axis.SHOULDER)" }, + { + "cell_type": "markdown", + "id": "24a4fe6e", + "source": "## Barcode reader\n\nThe KX2's onboard barcode reader is a separate RS-232 device, not on the CAN bus — so it's exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`. It can be used independently of the motor stack (even with the arm e-stopped).\n\nFind the port with `python -m serial.tools.list_ports -v`; defaults are 9600 8N1, overridable via the `baudrate` kwarg.", + "metadata": {} + }, + { + "cell_type": "code", + "id": "de492820", + "source": "from pylabrobot.paa.kx2 import KX2BarcodeReader\n\nbcr = KX2BarcodeReader(port=\"/dev/ttyUSB1\") # adjust to your actual port\nawait bcr.setup()\n\nbarcode = await bcr.barcode_scanning.scan()\nprint(barcode)\n\nawait bcr.stop()", + "metadata": {}, + "execution_count": null, + "outputs": [] + }, { "cell_type": "markdown", "id": "kx2-teardown-header", diff --git a/pylabrobot/paa/kx2/__init__.py b/pylabrobot/paa/kx2/__init__.py index 7ae2b2c5b80..85fc41eaa7f 100644 --- a/pylabrobot/paa/kx2/__init__.py +++ b/pylabrobot/paa/kx2/__init__.py @@ -1,3 +1,8 @@ from pylabrobot.paa.kx2.kx2 import KX2 from pylabrobot.paa.kx2.kx2_backend import KX2ArmBackend, KX2Axis +from pylabrobot.paa.kx2.kx2_barcode_reader import ( + KX2BarcodeReader, + KX2BarcodeReaderBackend, + KX2BarcodeReaderDriver, +) from pylabrobot.paa.kx2.kx2_driver import KX2Driver diff --git a/pylabrobot/paa/kx2/kx2_barcode_reader.py b/pylabrobot/paa/kx2/kx2_barcode_reader.py new file mode 100644 index 00000000000..a884afc6498 --- /dev/null +++ b/pylabrobot/paa/kx2/kx2_barcode_reader.py @@ -0,0 +1,191 @@ +"""PAA KX2 onboard barcode reader. + +The KX2's onboard barcode reader is a plain RS-232 device (Microscan/Omron-style +ESC-prefixed, CR-terminated commands) wired to the controller PC — entirely +independent of the CAN bus that drives the motors. As such it lives in its own +`Device` class alongside :class:`KX2`, mirroring the Keyence scanner pattern. + +Protocol reference: `KX2RobotControl.cs:15573–15848` in the vendor SDK. +""" + +from __future__ import annotations + +import asyncio +import logging +import time +from typing import Optional + +try: + import serial as pyserial + + _HAS_SERIAL = True +except ImportError as _e: + _HAS_SERIAL = False + _SERIAL_IMPORT_ERROR = _e + +from pylabrobot.capabilities.barcode_scanning import BarcodeScanner +from pylabrobot.capabilities.barcode_scanning.backend import ( + BarcodeScannerBackend, + BarcodeScannerError, +) +from pylabrobot.capabilities.capability import BackendParams +from pylabrobot.device import Device, Driver +from pylabrobot.io.serial import Serial +from pylabrobot.resources.barcode import Barcode + +logger = logging.getLogger(__name__) + +_ESC = b"\x1b" +_CR = b"\r" + + +class KX2BarcodeReaderDriver(Driver): + """Serial driver for the KX2's onboard Microscan-style barcode reader. + + Factory defaults (per `KX2RobotControl.cs:1712–1741`): 9600 8N1, no flow + control. All commands are `ESC + + CR`; all responses are + ` + CR`. + """ + + default_baudrate = 9600 + + def __init__(self, port: str, baudrate: int = default_baudrate): + if not _HAS_SERIAL: + raise RuntimeError( + "pyserial is not installed. Install with: pip install pylabrobot[serial]. " + f"Import error: {_SERIAL_IMPORT_ERROR}" + ) + super().__init__() + self.io = Serial( + human_readable_device_name="KX2 Barcode Reader", + port=port, + baudrate=baudrate, + bytesize=pyserial.EIGHTBITS, + parity=pyserial.PARITY_NONE, + stopbits=pyserial.STOPBITS_ONE, + write_timeout=1, + timeout=0.1, # short per-byte timeout; send_command handles the real deadline + rtscts=False, + ) + + async def setup(self, backend_params: Optional[BackendParams] = None) -> None: + await self.io.setup() + logger.info("[KX2 BCR %s] connected", self.io.port) + + async def stop(self) -> None: + # Match the C# teardown: turn trigger off + restore default read time + # (KX2RobotControl.cs:15623–15624) so the reader is left in a known state. + try: + await self.send_command("Y", timeout=1.0) + except BarcodeScannerError: + pass + try: + await self.send_command("Y2", timeout=1.0) # read time = 2s + except BarcodeScannerError: + pass + await self.io.stop() + logger.info("[KX2 BCR %s] disconnected", self.io.port) + + async def _read_until_cr(self, timeout: float) -> str: + """Read from the port until we see a CR, returning the decoded line. + + Raises `BarcodeScannerError` on timeout. + """ + deadline = time.monotonic() + timeout + buf = bytearray() + while time.monotonic() < deadline: + chunk = await self.io.read(32) + if chunk: + buf.extend(chunk) + if _CR in buf: + line, _, _ = buf.partition(_CR) + decoded = line.decode("ascii", errors="replace").lstrip("\x1b").rstrip() + return decoded + else: + await asyncio.sleep(0.01) + raise BarcodeScannerError( + f"KX2 barcode reader: timeout waiting for CR after {timeout}s (buffered={bytes(buf)!r})" + ) + + async def send_command(self, cmd: str, timeout: float = 5.0) -> str: + """Send `ESC + cmd + CR` and return the response up to the terminating CR.""" + payload = _ESC + cmd.encode("ascii") + _CR + await self.io.write(payload) + decoded = await self._read_until_cr(timeout) + logger.debug("[KX2 BCR %s] %s -> %r", self.io.port, cmd, decoded) + return decoded + + async def read_decoded_barcode(self, timeout: float) -> str: + """Listen for an asynchronously-delivered decoded barcode line. + + Used after firing `trigger(True)` — the reader emits the decoded data + followed by CR whenever it makes a successful read. + """ + return await self._read_until_cr(timeout) + + # --- typed command helpers (names mirror the C# API) -------------------- + + async def trigger(self, on: bool) -> None: + await self.send_command("Z" if on else "Y") + + async def set_read_mode(self, mode: str) -> None: + """mode: 'single' | 'multiple' | 'continuous' (maps to S0/S1/S2).""" + code = {"single": "S0", "multiple": "S1", "continuous": "S2"}[mode] + await self.send_command(code) + + async def set_read_time(self, seconds: int) -> None: + """seconds: 1..9, or 0 for indefinite (mapped to YM).""" + if seconds == 0: + await self.send_command("YM") + elif 1 <= seconds <= 9: + await self.send_command(f"Y{seconds}") + else: + raise ValueError("read_time must be 0 (indefinite) or 1..9 seconds") + + async def set_auto_trigger(self, on: bool) -> None: + await self.send_command("+I" if on else "+F") + + async def get_software_version(self) -> str: + return await self.send_command("Z1") + + +class KX2BarcodeReaderBackend(BarcodeScannerBackend): + """Adapts :class:`KX2BarcodeReaderDriver` to the BarcodeScanner capability.""" + + def __init__(self, driver: KX2BarcodeReaderDriver, read_time: int = 2): + super().__init__() + self.driver = driver + self._read_time = read_time + + async def _on_setup(self, backend_params: Optional[BackendParams] = None) -> None: + # Handshake: version query (mirrors KX2RobotControl.cs:15617). + version = await self.driver.get_software_version() + if not version: + raise BarcodeScannerError( + "KX2 barcode reader: empty software-version response during handshake. " + "Verify port, baud rate, and that the reader is powered on." + ) + logger.info("[KX2 BCR %s] software version: %s", self.driver.io.port, version) + await self.driver.set_read_mode("single") + await self.driver.set_read_time(self._read_time) + + async def scan_barcode(self) -> Barcode: + # Fire the trigger, then listen for the decoded line. The reader delivers + # data asynchronously (not as a command-response), so we read-until-CR + # rather than sending another command. + await self.driver.trigger(True) + data = await self.driver.read_decoded_barcode(timeout=float(self._read_time + 1)) + if not data: + raise BarcodeScannerError("KX2 barcode reader: no read within read-time window") + return Barcode(data=data, symbology="ANY 1D", position_on_resource="front") + + +class KX2BarcodeReader(Device): + """PAA KX2 onboard barcode reader (Microscan-style serial device).""" + + def __init__(self, port: str, baudrate: int = KX2BarcodeReaderDriver.default_baudrate): + driver = KX2BarcodeReaderDriver(port=port, baudrate=baudrate) + super().__init__(driver=driver) + self.driver: KX2BarcodeReaderDriver = driver + self.barcode_scanning = BarcodeScanner(backend=KX2BarcodeReaderBackend(driver)) + self._capabilities = [self.barcode_scanning] From 73bd30bb8e3977d8a9016ac9dc3ebef4cf1e865a Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 21 Apr 2026 12:14:49 -0700 Subject: [PATCH 25/93] send CR+LF (not just CR) to KX2 barcode reader + expand notebook section MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- docs/user_guide/paa/kx2/hello-world.ipynb | 177 +++++++++++++++++++++- pylabrobot/paa/kx2/kx2_barcode_reader.py | 9 +- 2 files changed, 176 insertions(+), 10 deletions(-) diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index c9193f1b679..1b8cb6c0aea 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -324,7 +324,22 @@ "id": "kx2-pick-place-code", "metadata": {}, "outputs": [], - "source": "pick = Coordinate(x=450.0, y=0.0, z=80.0)\nplace = Coordinate(x=450.0, y=-200.0, z=80.0)\nabove = Coordinate(x=0, y=0, z=60.0)\nyaw = 0.0\n\n# pick_up_at_location stores resource_width on the frontend; drop_at_location\n# reuses it, so you don't pass it again.\nawait kx2.arm.move_to_location(pick + above, direction=yaw)\nawait kx2.arm.pick_up_at_location(location=pick, direction=yaw, resource_width=0)\nawait kx2.arm.move_to_location(pick + above, direction=yaw)\n\nawait kx2.arm.move_to_location(place + above, direction=yaw)\nawait kx2.arm.drop_at_location(location=place, direction=yaw)\nawait kx2.arm.move_to_location(place + above, direction=yaw)" + "source": [ + "pick = Coordinate(x=450.0, y=0.0, z=80.0)\n", + "place = Coordinate(x=450.0, y=-200.0, z=80.0)\n", + "above = Coordinate(x=0, y=0, z=60.0)\n", + "yaw = 0.0\n", + "\n", + "# pick_up_at_location stores resource_width on the frontend; drop_at_location\n", + "# reuses it, so you don't pass it again.\n", + "await kx2.arm.move_to_location(pick + above, direction=yaw)\n", + "await kx2.arm.pick_up_at_location(location=pick, direction=yaw, resource_width=0)\n", + "await kx2.arm.move_to_location(pick + above, direction=yaw)\n", + "\n", + "await kx2.arm.move_to_location(place + above, direction=yaw)\n", + "await kx2.arm.drop_at_location(location=place, direction=yaw)\n", + "await kx2.arm.move_to_location(place + above, direction=yaw)" + ] }, { "cell_type": "markdown", @@ -416,7 +431,9 @@ "id": "kx2-estop", "metadata": {}, "outputs": [], - "source": "await kx2.driver.get_estop_state()" + "source": [ + "await kx2.driver.get_estop_state()" + ] }, { "cell_type": "code", @@ -424,21 +441,165 @@ "id": "kx2-fault", "metadata": {}, "outputs": [], - "source": "from pylabrobot.paa.kx2 import KX2Axis\n\nawait kx2.driver.motor_get_fault(KX2Axis.SHOULDER)" + "source": [ + "from pylabrobot.paa.kx2 import KX2Axis\n", + "\n", + "await kx2.driver.motor_get_fault(KX2Axis.SHOULDER)" + ] }, { "cell_type": "markdown", "id": "24a4fe6e", - "source": "## Barcode reader\n\nThe KX2's onboard barcode reader is a separate RS-232 device, not on the CAN bus — so it's exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`. It can be used independently of the motor stack (even with the arm e-stopped).\n\nFind the port with `python -m serial.tools.list_ports -v`; defaults are 9600 8N1, overridable via the `baudrate` kwarg.", - "metadata": {} + "metadata": {}, + "source": [ + "## Barcode reader\n", + "\n", + "The KX2's onboard barcode reader is a Microscan/Omron-style serial device wired to the controller PC (HD-15 host cable out of the reader, typically broken out to a DB9 or USB-serial adapter). It's fully independent of the CAN motor stack, so it works even with the arm e-stopped — exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`.\n", + "\n", + "Factory defaults: **9600 8N1**, no flow control. Find your port with `python -m serial.tools.list_ports -v`.\n", + "\n", + "### Supported symbologies\n", + "\n", + "The Microscan scanners PAA ships with KX2 systems (MS-3 / MS-4 / QX-830) decode the standard 1D set out of the box:\n", + "\n", + "- Code 128 (all subsets)\n", + "- Code 39\n", + "- Codabar\n", + "- Interleaved 2 of 5\n", + "- Code 93\n", + "- UPC-A / UPC-E\n", + "- EAN-8 / EAN-13\n", + "\n", + "2D models (QX-870 and up) add PDF417, DataMatrix, and QR Code. Which symbologies are *enabled* is a per-unit configuration; use the vendor's ESP software if you need to change it.\n", + "\n", + "{class}`~pylabrobot.resources.barcode.Barcode` returned from `scan()` has `symbology=\"ANY 1D\"` by default — the reader doesn't emit a symbology ID unless AIM-ID prefixing is turned on in ESP (`Preamble` / `Postamble` settings). If you need the symbology per-read, enable that and parse the leading `]xN` bytes from `barcode.data`." + ] }, { "cell_type": "code", + "execution_count": 1, "id": "de492820", - "source": "from pylabrobot.paa.kx2 import KX2BarcodeReader\n\nbcr = KX2BarcodeReader(port=\"/dev/ttyUSB1\") # adjust to your actual port\nawait bcr.setup()\n\nbarcode = await bcr.barcode_scanning.scan()\nprint(barcode)\n\nawait bcr.stop()", "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "2026-04-21 12:09:43,639 - pylabrobot.io.serial - INFO - Using explicitly provided port: /dev/tty.usbserial-FTE1YWTI (for VID=None, PID=None)\n", + "2026-04-21 12:09:43,670 - pylabrobot.paa.kx2.kx2_barcode_reader - INFO - [KX2 BCR /dev/tty.usbserial-FTE1YWTI] connected\n" + ] + }, + { + "ename": "BarcodeScannerError", + "evalue": "KX2 barcode reader: timeout waiting for CR after 5.0s (buffered=b'')", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mBarcodeScannerError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[1], line 4\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mpylabrobot\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mpaa\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mkx2\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m KX2BarcodeReader\n\u001b[1;32m 3\u001b[0m bcr \u001b[38;5;241m=\u001b[39m KX2BarcodeReader(port\u001b[38;5;241m=\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m/dev/tty.usbserial-FTE1YWTI\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m----> 4\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m bcr\u001b[38;5;241m.\u001b[39msetup()\n", + "File \u001b[0;32m~/real/pylabrobot/pylabrobot/device.py:110\u001b[0m, in \u001b[0;36mDevice.setup\u001b[0;34m(self, backend_params)\u001b[0m\n\u001b[1;32m 108\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mdriver\u001b[38;5;241m.\u001b[39msetup(backend_params\u001b[38;5;241m=\u001b[39mbackend_params)\n\u001b[1;32m 109\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m cap \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39m_capabilities:\n\u001b[0;32m--> 110\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m cap\u001b[38;5;241m.\u001b[39m_on_setup()\n\u001b[1;32m 111\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39m_setup_finished \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mTrue\u001b[39;00m\n", + "File \u001b[0;32m~/real/pylabrobot/pylabrobot/capabilities/capability.py:90\u001b[0m, in \u001b[0;36mCapability._on_setup\u001b[0;34m(self, backend_params)\u001b[0m\n\u001b[1;32m 88\u001b[0m \u001b[38;5;28;01masync\u001b[39;00m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21m_on_setup\u001b[39m(\u001b[38;5;28mself\u001b[39m, backend_params: Optional[BackendParams] \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m):\n\u001b[1;32m 89\u001b[0m \u001b[38;5;250m \u001b[39m\u001b[38;5;124;03m\"\"\"Called by the parent Device after driver.setup() completes.\"\"\"\u001b[39;00m\n\u001b[0;32m---> 90\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mbackend\u001b[38;5;241m.\u001b[39m_on_setup(backend_params\u001b[38;5;241m=\u001b[39mbackend_params)\n\u001b[1;32m 91\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39m_setup_finished \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mTrue\u001b[39;00m\n", + "File \u001b[0;32m~/real/pylabrobot/pylabrobot/paa/kx2/kx2_barcode_reader.py:162\u001b[0m, in \u001b[0;36mKX2BarcodeReaderBackend._on_setup\u001b[0;34m(self, backend_params)\u001b[0m\n\u001b[1;32m 160\u001b[0m \u001b[38;5;28;01masync\u001b[39;00m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21m_on_setup\u001b[39m(\u001b[38;5;28mself\u001b[39m, backend_params: Optional[BackendParams] \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m:\n\u001b[1;32m 161\u001b[0m \u001b[38;5;66;03m# Handshake: version query (mirrors KX2RobotControl.cs:15617).\u001b[39;00m\n\u001b[0;32m--> 162\u001b[0m version \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mdriver\u001b[38;5;241m.\u001b[39mget_software_version()\n\u001b[1;32m 163\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;129;01mnot\u001b[39;00m version:\n\u001b[1;32m 164\u001b[0m \u001b[38;5;28;01mraise\u001b[39;00m BarcodeScannerError(\n\u001b[1;32m 165\u001b[0m \u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mKX2 barcode reader: empty software-version response during handshake. \u001b[39m\u001b[38;5;124m\"\u001b[39m\n\u001b[1;32m 166\u001b[0m \u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mVerify port, baud rate, and that the reader is powered on.\u001b[39m\u001b[38;5;124m\"\u001b[39m\n\u001b[1;32m 167\u001b[0m )\n", + "File \u001b[0;32m~/real/pylabrobot/pylabrobot/paa/kx2/kx2_barcode_reader.py:149\u001b[0m, in \u001b[0;36mKX2BarcodeReaderDriver.get_software_version\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m 148\u001b[0m \u001b[38;5;28;01masync\u001b[39;00m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21mget_software_version\u001b[39m(\u001b[38;5;28mself\u001b[39m) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m \u001b[38;5;28mstr\u001b[39m:\n\u001b[0;32m--> 149\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39msend_command(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mZ1\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n", + "File \u001b[0;32m~/real/pylabrobot/pylabrobot/paa/kx2/kx2_barcode_reader.py:114\u001b[0m, in \u001b[0;36mKX2BarcodeReaderDriver.send_command\u001b[0;34m(self, cmd, timeout)\u001b[0m\n\u001b[1;32m 112\u001b[0m payload \u001b[38;5;241m=\u001b[39m _ESC \u001b[38;5;241m+\u001b[39m cmd\u001b[38;5;241m.\u001b[39mencode(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mascii\u001b[39m\u001b[38;5;124m\"\u001b[39m) \u001b[38;5;241m+\u001b[39m _CR\n\u001b[1;32m 113\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mio\u001b[38;5;241m.\u001b[39mwrite(payload)\n\u001b[0;32m--> 114\u001b[0m decoded \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39m_read_until_cr(timeout)\n\u001b[1;32m 115\u001b[0m logger\u001b[38;5;241m.\u001b[39mdebug(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m[KX2 BCR \u001b[39m\u001b[38;5;132;01m%s\u001b[39;00m\u001b[38;5;124m] \u001b[39m\u001b[38;5;132;01m%s\u001b[39;00m\u001b[38;5;124m -> \u001b[39m\u001b[38;5;132;01m%r\u001b[39;00m\u001b[38;5;124m\"\u001b[39m, \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mio\u001b[38;5;241m.\u001b[39mport, cmd, decoded)\n\u001b[1;32m 116\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m decoded\n", + "File \u001b[0;32m~/real/pylabrobot/pylabrobot/paa/kx2/kx2_barcode_reader.py:106\u001b[0m, in \u001b[0;36mKX2BarcodeReaderDriver._read_until_cr\u001b[0;34m(self, timeout)\u001b[0m\n\u001b[1;32m 104\u001b[0m \u001b[38;5;28;01melse\u001b[39;00m:\n\u001b[1;32m 105\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m asyncio\u001b[38;5;241m.\u001b[39msleep(\u001b[38;5;241m0.01\u001b[39m)\n\u001b[0;32m--> 106\u001b[0m \u001b[38;5;28;01mraise\u001b[39;00m BarcodeScannerError(\n\u001b[1;32m 107\u001b[0m \u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mKX2 barcode reader: timeout waiting for CR after \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mtimeout\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124ms (buffered=\u001b[39m\u001b[38;5;132;01m{\u001b[39;00m\u001b[38;5;28mbytes\u001b[39m(buf)\u001b[38;5;132;01m!r}\u001b[39;00m\u001b[38;5;124m)\u001b[39m\u001b[38;5;124m\"\u001b[39m\n\u001b[1;32m 108\u001b[0m )\n", + "\u001b[0;31mBarcodeScannerError\u001b[0m: KX2 barcode reader: timeout waiting for CR after 5.0s (buffered=b'')" + ] + } + ], + "source": [ + "from pylabrobot.paa.kx2 import KX2BarcodeReader\n", + "\n", + "bcr = KX2BarcodeReader(port=\"/dev/tty.usbserial-FTE1YWTI\")\n", + "await bcr.setup()" + ] + }, + { + "cell_type": "markdown", + "id": "c240108f", + "metadata": {}, + "source": [ + "`setup()` opens the serial port, does a `Z1` (software-version) handshake, switches the reader to single-trigger mode, and sets a default 2-second read window.\n", + "\n", + "### Scan\n", + "\n", + "Fires the trigger (`Z`), waits up to `read_time + 1s` for a decode, returns a {class}`~pylabrobot.resources.barcode.Barcode`." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "6ca16712", + "metadata": {}, + "outputs": [ + { + "ename": "RuntimeError", + "evalue": "The capability has not been set up. Call setup() on the parent device.", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mRuntimeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[2], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m barcode \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mawait\u001b[39;00m bcr\u001b[38;5;241m.\u001b[39mbarcode_scanning\u001b[38;5;241m.\u001b[39mscan()\n\u001b[1;32m 2\u001b[0m \u001b[38;5;28mprint\u001b[39m(barcode)\n", + "File \u001b[0;32m~/real/pylabrobot/pylabrobot/capabilities/capability.py:45\u001b[0m, in \u001b[0;36mneed_capability_ready..wrapper\u001b[0;34m(*args, **kwargs)\u001b[0m\n\u001b[1;32m 42\u001b[0m \u001b[38;5;28mself\u001b[39m \u001b[38;5;241m=\u001b[39m args[\u001b[38;5;241m0\u001b[39m]\n\u001b[1;32m 44\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;129;01mnot\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39msetup_finished:\n\u001b[0;32m---> 45\u001b[0m \u001b[38;5;28;01mraise\u001b[39;00m \u001b[38;5;167;01mRuntimeError\u001b[39;00m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mThe capability has not been set up. Call setup() on the parent device.\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[1;32m 46\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28;01mawait\u001b[39;00m func(\u001b[38;5;241m*\u001b[39margs, \u001b[38;5;241m*\u001b[39m\u001b[38;5;241m*\u001b[39mkwargs)\n", + "\u001b[0;31mRuntimeError\u001b[0m: The capability has not been set up. Call setup() on the parent device." + ] + } + ], + "source": [ + "barcode = await bcr.barcode_scanning.scan()\n", + "print(barcode)" + ] + }, + { + "cell_type": "markdown", + "id": "cdc9257f", + "metadata": {}, + "source": [ + "### Driver-level controls\n", + "\n", + "For raw access — longer read windows, continuous mode, auto-trigger, or direct vendor commands — go through `bcr.driver`:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "fb52ca64", + "metadata": {}, + "outputs": [], + "source": [ + "print(await bcr.driver.get_software_version())\n", + "\n", + "# Switch to multi-read (two reads without requiring a new trigger) or continuous:\n", + "# await bcr.driver.set_read_mode(\"multiple\")\n", + "# await bcr.driver.set_read_mode(\"continuous\")\n", + "\n", + "# Read-time window (1..9 seconds, or 0 for indefinite / up to 60s):\n", + "await bcr.driver.set_read_time(5)\n", + "\n", + "# Auto-trigger — reader fires on its own when it sees motion in frame:\n", + "# await bcr.driver.set_auto_trigger(True)\n", + "# await bcr.driver.set_auto_trigger(False)" + ] + }, + { + "cell_type": "markdown", + "id": "e26f0d79", + "metadata": {}, + "source": [ + "### Teardown\n", + "\n", + "`stop()` sends `Y` (trigger off) + `Y2` (reset to 2-second read time), then closes the serial port — leaving the reader in a known state for the next session." + ] + }, + { + "cell_type": "code", "execution_count": null, - "outputs": [] + "id": "df9ef753", + "metadata": {}, + "outputs": [], + "source": [ + "await bcr.stop()" + ] }, { "cell_type": "markdown", @@ -480,4 +641,4 @@ }, "nbformat": 4, "nbformat_minor": 5 -} \ No newline at end of file +} diff --git a/pylabrobot/paa/kx2/kx2_barcode_reader.py b/pylabrobot/paa/kx2/kx2_barcode_reader.py index a884afc6498..a5eeb33ec95 100644 --- a/pylabrobot/paa/kx2/kx2_barcode_reader.py +++ b/pylabrobot/paa/kx2/kx2_barcode_reader.py @@ -37,6 +37,11 @@ _ESC = b"\x1b" _CR = b"\r" +# Vendor C# uses `SerialPort.WriteLine(ESC + cmd + "\r")`, which appends the +# default .NET `NewLine` of `"\n"` — so the actual frame on the wire is +# ESC + cmd + CR + LF. Responses are split on CR alone (see +# SerialPortBCR_DataReceived in KX2RobotControl.cs). +_CMD_TERM = b"\r\n" class KX2BarcodeReaderDriver(Driver): @@ -108,8 +113,8 @@ async def _read_until_cr(self, timeout: float) -> str: ) async def send_command(self, cmd: str, timeout: float = 5.0) -> str: - """Send `ESC + cmd + CR` and return the response up to the terminating CR.""" - payload = _ESC + cmd.encode("ascii") + _CR + """Send `ESC + cmd + CR + LF` and return the response up to the terminating CR.""" + payload = _ESC + cmd.encode("ascii") + _CMD_TERM await self.io.write(payload) decoded = await self._read_until_cr(timeout) logger.debug("[KX2 BCR %s] %s -> %r", self.io.port, cmd, decoded) From 38718a3e3c7b27771f936c1bf2501a31f0b713ca Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Wed, 22 Apr 2026 17:01:57 -0700 Subject: [PATCH 26/93] harden KX2 motor_enable + fix axis-identify comprehension MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - retry motor_enable up to 5x before raising (drives occasionally miss the first DS402 transition); drop per-CW sleeps to match the C# reference (clscanmotor.cs:4495-4509) — back-to-back CW writes, single 100ms settle, then MO query - mirror the same back-to-back pattern on motor_disable (clscanmotor.cs:4540-4543) - stop str()-casting the reply in motor_send_command ValQuery path so callers can get typed values back - rewrite the broken nested-`for` set comprehension in identify_axes as a plain loop — the double `for node in nodes` was syntactically valid but semantically wrong Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/kx2_backend.py | 10 +++----- pylabrobot/paa/kx2/kx2_driver.py | 41 +++++++++++++++++-------------- 2 files changed, 27 insertions(+), 24 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_backend.py b/pylabrobot/paa/kx2/kx2_backend.py index 3768401bd5c..21c2000886f 100644 --- a/pylabrobot/paa/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/kx2_backend.py @@ -499,12 +499,10 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: store.setdefault(axis, {})[ch] = value # Pass 1: identify axes by UI[4] - uis = { - node - for node in nodes - if node == await self.driver.motor_send_command(node, "UI", 4, val_type=ValType.Int) - for node in nodes - } + uis = set() + for node in nodes: + if node == await self.driver.motor_send_command(node, "UI", 4, val_type=ValType.Int): + uis.add(node) for required_axis in MOTION_AXES: if required_axis.value not in uis: raise CanError(f"Missing required axis with UI[4]={required_axis}") diff --git a/pylabrobot/paa/kx2/kx2_driver.py b/pylabrobot/paa/kx2/kx2_driver.py index b11ac5509df..8ddcd39e95e 100644 --- a/pylabrobot/paa/kx2/kx2_driver.py +++ b/pylabrobot/paa/kx2/kx2_driver.py @@ -659,7 +659,7 @@ async def motor_send_command( val_type=val_type, low_priority=low_priority, ) - return str(reply) if cmd_type == CmdType.ValQuery else "" + return reply if cmd_type == CmdType.ValQuery else "" async def get_estop_state(self) -> bool: """Return True if the arm is in estop, False otherwise. @@ -804,23 +804,28 @@ async def motor_enable(self, axis: KX2Axis, state: bool) -> None: if state: self.EmcyMoveErrorReceived = False - if use_bi: - await self._binary_interpreter(axis, "MO", 0, CmdType.ValSet, "1") + max_attempts = 5 + for attempt in range(1, max_attempts + 1): + if use_bi: + await self._binary_interpreter(axis, "MO", 0, CmdType.ValSet, "1") + else: + # DS402 enable sequence: Fault -> Shutdown -> Switched On -> Op Enabled. + # Matches the C# reference (clscanmotor.cs:4495-4509): back-to-back + # CW writes, a single 100 ms settle at the end, then MO query. + for cw in (0, 128, 6, 7, 15): + await self._control_word_set(node_id=int(axis), value=cw) + await asyncio.sleep(0.1) + left = await self._binary_interpreter( + node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery + ) + if left == 1: + break + logger.warning( + "motor_enable attempt %d/%d failed for axis %s (MO=%s); retrying", + attempt, max_attempts, axis, left, + ) else: - # DS402 enable sequence: Fault -> Shutdown -> Switched On -> Op Enabled. - # The drive needs ≥1 CANopen cycle between transitions to update its - # state machine; without a gap the legacy driver's queue serialization - # provided one, but `network.send_message` is synchronous so we insert - # our own small delay. - for cw in (0, 128, 6, 7, 15): - await self._control_word_set(node_id=int(axis), value=cw) - await asyncio.sleep(0.01) - await asyncio.sleep(0.1) - left = await self._binary_interpreter( - node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery - ) - if left != 1: - raise CanError(f"Motor failed to enable (axis = {axis})") + raise CanError(f"Motor failed to enable (axis = {axis}) after {max_attempts} attempts") else: if use_bi: try: @@ -831,8 +836,8 @@ async def motor_enable(self, axis: KX2Axis, state: bool) -> None: pass else: # DS402 disable: Op Enabled -> Switched On -> Ready to Switch On. + # Matches C# (clscanmotor.cs:4540-4543) — back-to-back, no inter-CW sleep. await self._control_word_set(node_id=int(axis), value=7) - await asyncio.sleep(0.01) await self._control_word_set(node_id=int(axis), value=6) await asyncio.sleep(0.1) left = await self._binary_interpreter( From b7a5688f843cc81078d67a467924ee2b4992ca77 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Wed, 22 Apr 2026 17:21:19 -0700 Subject: [PATCH 27/93] poll MS for move completion instead of TPDO3 MotionComplete MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Elmo's MotionComplete event is unreliable on short / zero-distance moves: the future-based wait in `_wait_for_moves_done` would sit on the missing event for the full `move_time + 2` timeout before the one-shot fallback confirmed the motor was already idle. Replace the TPDO3-future path with a 30ms MS poll after a 50ms warm-up. The warm-up avoids reading MS=0 in the window between CW=63 and motion actually starting; 30ms cadence keeps worst-case extra latency on the order of a poll interval rather than 2 seconds. TPDO3 stays mapped on the drive to match the C# reference config, but we no longer subscribe or keep per-axis futures — `_waiting_moves`, `_dispatch_tpdo3`, and `_make_tpdo3_callback` are gone. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/kx2_driver.py | 79 +++++++++++++------------------- 1 file changed, 32 insertions(+), 47 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_driver.py b/pylabrobot/paa/kx2/kx2_driver.py index 8ddcd39e95e..6b64fb6bedc 100644 --- a/pylabrobot/paa/kx2/kx2_driver.py +++ b/pylabrobot/paa/kx2/kx2_driver.py @@ -95,10 +95,6 @@ def __init__( self._pvt_mode: bool = False self.EmcyMoveErrorReceived: bool = False self.grp_id: int = 0 - # Move-done futures per axis; resolved by TPDO4 digital-input callbacks - # once PDO mapping lands. Until then, `_wait_for_moves_done` falls back - # to polling `motor_check_if_move_done`. - self._waiting_moves: Dict[KX2Axis, asyncio.Future] = {} # --- lifecycle ----------------------------------------------------------- @@ -151,12 +147,13 @@ async def stop(self) -> None: async def connect_part_two(self) -> None: """Configure PDO mapping + Elmo DS402 parameters after the CAN bus is up. - Mirrors the legacy driver: unmap TPDO1, map TPDO3 (StatusWord, triggered - on MotionComplete) and TPDO4 (DigitalInputs, triggered on edge). Then - program Elmo vendor objects that set interpolation config, and finally - map RPDO1 (ControlWord) and RPDO3 (interpolated target position+velocity) - per motion axis. Subscribe to each node's TPDO3 cob_id so move-done - completes `_waiting_moves` futures. + Unmap TPDO1, map TPDO3 (StatusWord, triggered on MotionComplete) and + TPDO4 (DigitalInputs, triggered on edge). TPDO3 is kept mapped to match + the C# reference config, but move completion is detected by polling MS + (the MotionComplete event is unreliable on short moves). Then program + Elmo vendor objects that set interpolation config, and finally map + RPDO1 (ControlWord) and RPDO3 (interpolated target position+velocity) + per motion axis. """ assert self._network is not None @@ -200,30 +197,9 @@ async def connect_part_two(self) -> None: PDOTransmissionType.EventDrivenDev, ) - # TPDO3 subscription: StatusWord frames arrive on MotionComplete trigger, - # so any TPDO3 on an axis with a pending waiting_move completes it. - for nid in self.node_id_list: - tpdo3_cob = ((int(COBType.TPDO3) & 0x0F) << 7) | (nid & 0x7F) - self._network.subscribe(tpdo3_cob, self._make_tpdo3_callback(nid)) - self._pvt_mode = True await self._pvt_select_mode(False) - def _make_tpdo3_callback(self, node_id: int): - def _cb(cob_id: int, data: bytes, timestamp: float) -> None: - if self._loop is None: - return - self._loop.call_soon_threadsafe(self._dispatch_tpdo3, node_id) - return _cb - - def _dispatch_tpdo3(self, node_id: int) -> None: - axis = KX2Axis(node_id) if node_id in {a.value for a in KX2Axis} else None - if axis is None: - return - fut = self._waiting_moves.get(axis) - if fut is not None and not fut.done(): - fut.set_result(None) - # --- PDO configuration (pure SDO writes; no library-PDO machinery) ------ async def _can_tpdo_unmap(self, tpdo: TPDO, node_id: int) -> None: @@ -868,23 +844,32 @@ async def _pvt_select_mode(self, enable: bool) -> None: await self._can_sdo_download(int(axis), 0x60, 0x60, 0x00, [1]) self._pvt_mode = False - async def _wait_for_moves_done(self, timeout: float) -> None: - async def _one(axis: KX2Axis) -> None: - try: - await asyncio.wait_for(self._waiting_moves[axis], timeout=timeout) - except asyncio.TimeoutError: - pass - # Fallback query in case the digital-input edge was missed (or TPDO - # mapping hasn't landed yet on this driver). - await self.motor_check_if_move_done(int(axis)) + async def _wait_for_moves_done( + self, axes: List[KX2Axis], timeout: float + ) -> None: + # Poll MS every 30ms after a 50ms warm-up. The warm-up avoids reading + # MS=0 in the window between CW=63 and motion actually starting. + assert self._loop is not None - await asyncio.gather(*(_one(axis) for axis in self._waiting_moves.keys())) + async def _poll_axis(axis: KX2Axis) -> None: + deadline = self._loop.time() + timeout + await asyncio.sleep(0.05) + while self._loop.time() < deadline: + try: + if await self.motor_check_if_move_done(int(axis)): + return + except CanError: + pass + await asyncio.sleep(0.03) + # Final authoritative check; propagates CanError / motor-fault. + if not await self.motor_check_if_move_done(int(axis)): + raise CanError(f"Axis {axis} move did not complete within {timeout}s") + + await asyncio.gather(*(_poll_axis(a) for a in axes)) async def _motors_move_start( self, axes: List[KX2Axis], *, relative: bool = False ) -> None: - assert self._loop is not None - self._waiting_moves = {ax: self._loop.create_future() for ax in axes} relative_bit = 0x40 if relative else 0 for i, nid in enumerate(axes): last = i == (len(axes) - 1) @@ -916,8 +901,9 @@ async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: move.axis.value, 24708, 0, str(acc), ElmoObjectDataType.UNSIGNED32, ) - await self._motors_move_start([move.axis for move in plan.moves]) - await self._wait_for_moves_done(timeout=plan.move_time + 2) + axes = [move.axis for move in plan.moves] + await self._motors_move_start(axes) + await self._wait_for_moves_done(axes, timeout=plan.move_time + 2) async def _user_program_run( self, @@ -1049,9 +1035,8 @@ async def _motor_index_search( await self._binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, "0") await self._binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") # arm - self._waiting_moves = {axis: self._loop.create_future()} await self._binary_interpreter(int(axis), "BG", 0, CmdType.Execute) - await self._wait_for_moves_done(timeout) + await self._wait_for_moves_done([axis], timeout) left = await self._binary_interpreter(int(axis), "HM", 1, CmdType.ValQuery) if left != 0: From 81cff3de6aa7f0e5ec8f9b2a5aaea076504fd1a0 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 23 Apr 2026 17:34:40 -0700 Subject: [PATCH 28/93] split KX2 layers: driver is pure CAN transport, backend owns robot topology MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Driver-layer cleanup, rolled together: - Drop `motor_send_command` dispatcher — callers now pass `CmdType` explicitly to `_binary_interpreter`. OS-routing through it was dead (no Python caller used VR/CD/LS/DL/DF/BH or XC##/XQ## through the dispatcher; `_user_program_run` calls `_os_interpreter` directly). - Add `request_drive_version(node_id)` — thin `_os_interpreter("VR")` wrapper, replacing the one ergonomic thing the dispatcher offered. - Move all robot-topology knowledge to the backend: - `KX2Axis` → nested `KX2ArmBackend.Axis` (no module-level alias — clean break; external callers use `KX2ArmBackend.Axis`). - `MOTION_AXES`, `HomeStatus` → backend module level. - Methods moved driver → backend: `home_motor`, `_motor_hard_stop_search`, `_motor_index_search`, `_motor_reset_encoder_position`, `_motor_set_homed_status`, `motor_get_homed_status`, `get_estop_state`. - `motor_enable` now takes `use_ds402: bool` kwarg — caller (backend) picks the DS402 vs BI `MO` path instead of driver inferring from a hardcoded `MOTION_AXES` membership + dead `self.grp_id`. - `MotorMoveParam.axis` → `MotorMoveParam.node_id` (driver stays axis-agnostic). - Driver now has `self.motion_node_ids = [1, 2, 3, 4]` for its own internal loops (`connect_part_two`, `_pvt_select_mode`). - Zero `TYPE_CHECKING` imports; zero runtime imports inside function bodies. One-way dep: backend → driver. Notebook `hello-world.ipynb` updated for `KX2ArmBackend.Axis`; the untracked scratch copy is left alone. Co-Authored-By: Claude Opus 4.7 (1M context) --- docs/user_guide/paa/kx2/hello-world.ipynb | 55 +- pylabrobot/paa/kx2/__init__.py | 2 +- pylabrobot/paa/kx2/kx2_backend.py | 767 ++++++++++++---------- pylabrobot/paa/kx2/kx2_driver.py | 571 +++++++--------- 4 files changed, 678 insertions(+), 717 deletions(-) diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 1b8cb6c0aea..4e1dffd97ea 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -187,36 +187,15 @@ "cell_type": "markdown", "id": "kx2-joint-header", "metadata": {}, - "source": [ - "### Joint movement\n", - "\n", - "Move in joint space using the {class}`~pylabrobot.paa.kx2.KX2Axis` enum. Rotary axes in degrees; Z (and gripper) in mm-equivalent encoder units.\n", - "\n", - "```{warning}\n", - "Moving to arbitrary joint positions can cause the arm to collide with its base, gripper, or nearby equipment. Verify coordinates in a safe workspace first, and start with low velocity.\n", - "```" - ] + "source": "### Joint movement\n\nMove in joint space using the {class}`~pylabrobot.paa.kx2.KX2ArmBackend.Axis` enum. Rotary axes in degrees; Z (and gripper) in mm-equivalent encoder units.\n\n```{warning}\nMoving to arbitrary joint positions can cause the arm to collide with its base, gripper, or nearby equipment. Verify coordinates in a safe workspace first, and start with low velocity.\n```" }, { "cell_type": "code", - "execution_count": 8, + "execution_count": null, "id": "kx2-joint-code", "metadata": {}, "outputs": [], - "source": [ - "from pylabrobot.paa.kx2 import KX2Axis\n", - "\n", - "position = {\n", - " KX2Axis.SHOULDER: 0.0,\n", - " KX2Axis.Z: 150.0,\n", - " KX2Axis.ELBOW: 90.0,\n", - " KX2Axis.WRIST: 0.0,\n", - "}\n", - "await kx2.arm.backend.move_to_joint_position(\n", - " position,\n", - " backend_params=KX2ArmBackend.JointMoveParams(vel_pct=25, accel_pct=25),\n", - ")" - ] + "source": "from pylabrobot.paa.kx2 import KX2ArmBackend\n\nposition = {\n KX2ArmBackend.Axis.SHOULDER: 0.0,\n KX2ArmBackend.Axis.Z: 150.0,\n KX2ArmBackend.Axis.ELBOW: 90.0,\n KX2ArmBackend.Axis.WRIST: 0.0,\n}\nawait kx2.arm.backend.move_to_joint_position(\n position,\n backend_params=KX2ArmBackend.JointMoveParams(vel_pct=25, accel_pct=25),\n)" }, { "cell_type": "markdown", @@ -230,27 +209,11 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": null, "id": "0a19ceeb", "metadata": {}, "outputs": [], - "source": [ - "pick_joints = {\n", - " KX2Axis.SHOULDER: 0.0,\n", - " KX2Axis.Z: 150.0,\n", - " KX2Axis.ELBOW: 90.0,\n", - " KX2Axis.WRIST: 0.0,\n", - "}\n", - "place_joints = {\n", - " KX2Axis.SHOULDER: 30.0,\n", - " KX2Axis.Z: 150.0,\n", - " KX2Axis.ELBOW: 90.0,\n", - " KX2Axis.WRIST: 0.0,\n", - "}\n", - "\n", - "await kx2.arm.backend.pick_up_at_joint_position(pick_joints, resource_width=0)\n", - "await kx2.arm.backend.drop_at_joint_position(place_joints, resource_width=30)" - ] + "source": "pick_joints = {\n KX2ArmBackend.Axis.SHOULDER: 0.0,\n KX2ArmBackend.Axis.Z: 150.0,\n KX2ArmBackend.Axis.ELBOW: 90.0,\n KX2ArmBackend.Axis.WRIST: 0.0,\n}\nplace_joints = {\n KX2ArmBackend.Axis.SHOULDER: 30.0,\n KX2ArmBackend.Axis.Z: 150.0,\n KX2ArmBackend.Axis.ELBOW: 90.0,\n KX2ArmBackend.Axis.WRIST: 0.0,\n}\n\nawait kx2.arm.backend.pick_up_at_joint_position(pick_joints, resource_width=0)\nawait kx2.arm.backend.drop_at_joint_position(place_joints, resource_width=30)" }, { "cell_type": "markdown", @@ -441,11 +404,7 @@ "id": "kx2-fault", "metadata": {}, "outputs": [], - "source": [ - "from pylabrobot.paa.kx2 import KX2Axis\n", - "\n", - "await kx2.driver.motor_get_fault(KX2Axis.SHOULDER)" - ] + "source": "from pylabrobot.paa.kx2 import KX2ArmBackend\n\nawait kx2.driver.motor_get_fault(KX2ArmBackend.Axis.SHOULDER)" }, { "cell_type": "markdown", @@ -641,4 +600,4 @@ }, "nbformat": 4, "nbformat_minor": 5 -} +} \ No newline at end of file diff --git a/pylabrobot/paa/kx2/__init__.py b/pylabrobot/paa/kx2/__init__.py index 85fc41eaa7f..c92a2b82c91 100644 --- a/pylabrobot/paa/kx2/__init__.py +++ b/pylabrobot/paa/kx2/__init__.py @@ -1,5 +1,5 @@ from pylabrobot.paa.kx2.kx2 import KX2 -from pylabrobot.paa.kx2.kx2_backend import KX2ArmBackend, KX2Axis +from pylabrobot.paa.kx2.kx2_backend import KX2ArmBackend from pylabrobot.paa.kx2.kx2_barcode_reader import ( KX2BarcodeReader, KX2BarcodeReaderBackend, diff --git a/pylabrobot/paa/kx2/kx2_backend.py b/pylabrobot/paa/kx2/kx2_backend.py index 21c2000886f..bb787d19130 100644 --- a/pylabrobot/paa/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/kx2_backend.py @@ -4,7 +4,7 @@ import warnings from dataclasses import dataclass from enum import IntEnum -from typing import TYPE_CHECKING, Dict, List, Optional, Sequence +from typing import Dict, List, Optional, Sequence from pylabrobot.capabilities.arms.backend import ( CanFreedrive, @@ -14,24 +14,24 @@ from pylabrobot.capabilities.arms.standard import GripperLocation from pylabrobot.capabilities.arms.standard import GripperLocation as GripperPose from pylabrobot.capabilities.capability import BackendParams +from pylabrobot.paa.kx2.kx2_driver import ( + CanError, + CmdType, + JointMoveDirection, + KX2Driver, + MotorMoveParam, + MotorsMovePlan, + ValType, +) from pylabrobot.resources import Coordinate, Rotation -if TYPE_CHECKING: - from pylabrobot.paa.kx2.kx2_driver import KX2Driver - logger = logging.getLogger(__name__) -class KX2Axis(IntEnum): - SHOULDER = 1 - Z = 2 - ELBOW = 3 - WRIST = 4 - RAIL = 5 - SERVO_GRIPPER = 6 - - -MOTION_AXES = (KX2Axis.SHOULDER, KX2Axis.Z, KX2Axis.ELBOW, KX2Axis.WRIST) +class HomeStatus(IntEnum): + NotHomed = 0 + Homed = 1 + InitializedWithoutHoming = 2 def _is_number(s: str) -> bool: @@ -50,153 +50,34 @@ def _to_float(s: str, default: float = 0.0) -> float: return default -class JointMoveDirection(IntEnum): - Normal = 0 - Clockwise = 1 - Counterclockwise = 2 - ShortestWay = 3 - - -class HomeStatus(IntEnum): - NotHomed = 0 - Homed = 1 - InitializedWithoutHoming = 2 - - -class CmdType(IntEnum): - ValQuery = 1 - ValSet = 2 - Execute = 3 - - -class ValType(IntEnum): - Int = 1 - Float = 2 - - -class COBType(IntEnum): - NMT = 0 - EMCY = 1 - SYNC = 1 - TIMESTAMP = 2 - TPDO1 = 3 - RPDO1 = 4 - TPDO2 = 5 - RPDO2 = 6 - TPDO3 = 7 - RPDO3 = 8 - TPDO4 = 9 - RPDO4 = 10 - TSDO = 11 - RSDO = 12 - ERRCTRL = 14 - HEARTBEAT = 14 - - -class RPDO(IntEnum): - RPDO1 = 1 - RPDO3 = 3 - RPDO4 = 4 - - -class PDOTransmissionType(IntEnum): - SynchronousAcyclic = 0 - SynchronousCyclic = 1 - EventDrivenManf = 254 # 0xFE - EventDrivenDev = 255 # 0xFF - - -class RPDOMappedObject(IntEnum): - NotMapped = 0 - ControlWord = 0x60400010 - TargetTorque = 0x60710010 - MaxTorque = 0x60720010 - TargetPosition = 0x607A0020 - VelocityOffset = 0x60B10020 - TargetPositionIP = 0x60C10120 - TargetVelocityIP = 0x60C10220 - DigitalOutputs = 0x60FE0020 - TargetVelocity = 0x60FF0020 - - -class TPDO(IntEnum): - TPDO1 = 1 - TPDO3 = 3 - TPDO4 = 4 - - -class TPDOTrigger(IntEnum): - MotionComplete = 0 - MainHomingComplete = 1 - AuxiliaryHomingComplete = 2 - MotorShutDownByException = 3 - MotorStarted = 4 - UserProgramEmitCommand = 5 - OSInterpreterExecutionComplete = 6 - MotionStartedEvent = 8 - PDODataChanged = 24 - DigitalInputEvent = 26 - StatusWordEvent = 27 - BinaryInterpreterCommandComplete = 31 - - -class TPDOMappedObject(IntEnum): - NotMapped = 0 - Timestamp = 0x20410020 - PVTHeadPointer = 0x2F110010 - PVTTailPointer = 0x2F120010 - StatusWord = 0x60410010 - PositionActualValue = 0x60640020 - VelocityDemandValue = 0x606B0020 - VelocityActualValue = 0x606C0020 - TorqueDemandValue = 0x60740010 - TorqueActualValue = 0x60770010 - IPBufferPosition = 0x60C40410 - DigitalInputs = 0x60FD0020 - - -class ElmoObjectDataType(IntEnum): - UNSIGNED8 = 0 - UNSIGNED16 = 1 - UNSIGNED32 = 2 - UNSIGNED64 = 3 - INTEGER8 = 4 - INTEGER16 = 5 - INTEGER32 = 6 - INTEGER64 = 7 - STR = 8 - - - -class CanError(Exception): - """Custom exception for CAN motor errors.""" - - -@dataclass -class MotorMoveParam: - axis: "KX2Axis" - position: int - velocity: int - acceleration: int - relative: bool = False - direction: JointMoveDirection = JointMoveDirection.ShortestWay - - -@dataclass -class MotorsMovePlan: - moves: List[MotorMoveParam] - move_time: float = 10.0 - - class KX2ArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive): """Arm-capability backend for the PAA KX2. Owns a :class:`KX2Driver` (low-level CAN transport) and implements the capability-based arm interface (``OrientableGripperArmBackend`` + ``HasJoints`` + ``CanFreedrive``) directly on top of the drive primitives. + + This layer owns all robot-specific procedural logic: the axis -> node-ID + map, the motion/rail/gripper split for `motor_enable`, homing sequences, + estop polling, etc. The driver underneath is a pure CAN transport. """ - def __init__(self, driver: "KX2Driver") -> None: + class Axis(IntEnum): + """KX2 axis -> CANopen node-ID mapping. + + Lives here (not in the driver) because the driver is axis-agnostic and + deals only with node IDs. External code should reference + ``KX2ArmBackend.Axis``. + """ + + SHOULDER = 1 + Z = 2 + ELBOW = 3 + WRIST = 4 + RAIL = 5 + SERVO_GRIPPER = 6 + + def __init__(self, driver: KX2Driver) -> None: super().__init__() self.driver = driver @@ -221,6 +102,18 @@ def __init__(self, driver: "KX2Driver") -> None: self.node_id_list = [1, 2, 3, 4, 6] + # -- motion/rail/gripper decision helper -------------------------------- + + def _uses_ds402(self, axis: int) -> bool: + """Return True if this axis uses the DS402 controlword-based enable path. + + The four motion joints (shoulder/Z/elbow/wrist) are driven via the DS402 + state machine over RPDO1; the rail and the servo gripper use the Elmo + binary-interpreter ``MO`` command. This is the single piece of robot- + topology knowledge that selects between the driver's two enable paths. + """ + return int(axis) in (int(a) for a in MOTION_AXES) + async def _on_setup(self, backend_params: Optional[BackendParams] = None): # Driver has already brought CAN up (connect + node discovery) via # Device.setup(). Now read per-drive parameters, finish CANopen mapping, @@ -236,18 +129,207 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): for axis in MOTION_AXES: try: - await self.driver.motor_enable(axis=axis, state=True) + await self.driver.motor_enable(node_id=int(axis), state=True, use_ds402=True) except Exception as e: logger.warning("Error enabling motor on axis %s: %s", axis, e) await self.servo_gripper_initialize() + # -- robot-level homing / estop (moved from driver) --------------------- + + async def get_estop_state(self) -> bool: + """Return True if the arm is in estop, False otherwise. + + Reads the shoulder drive's SR (status register) via the binary + interpreter. Bits 14/15 encode the stop/safety state. + """ + r = int(await self.driver._binary_interpreter( + node_id=int(self.Axis.SHOULDER), + cmd="SR", + cmd_index=1, + cmd_type=CmdType.ValQuery, + )) + if r != 8438016: + logger.warning("get_estop_state: SR register unexpected value %d (expected 8438016)", r) + b14 = (r & 0x4000) == 0x4000 + b15 = (r & 0x8000) == 0x8000 + return (not b14) and (not b15) + + async def _motor_set_homed_status(self, axis: int, status: HomeStatus) -> None: + status_int = int(status) + if status_int == 1: + val = "1" + elif status_int == 2: + val = "2" + else: + val = "0" + await self.driver._binary_interpreter(int(axis), "UI", 3, CmdType.ValSet, val) + + async def motor_get_homed_status(self, node_id: int) -> HomeStatus: + left = await self.driver._binary_interpreter(int(node_id), "UI", 3, CmdType.ValQuery) + if left == 1: + return HomeStatus.Homed + if left == 2: + return HomeStatus.InitializedWithoutHoming + return HomeStatus.NotHomed + + async def _motor_reset_encoder_position(self, axis: int, position: float) -> None: + await self.driver._binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") + await self.driver._binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "0") + await self.driver._binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") + await self.driver._binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") + await self.driver._binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, str(position)) + await self.driver._binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") + + async def _motor_hard_stop_search( + self, + axis: int, + srch_vel: int, + srch_acc: int, + max_pe: int, + hs_pe: int, + timeout: float, + ) -> None: + nid = int(axis) + await self.driver._binary_interpreter(nid, "ER", 3, CmdType.ValSet, str(max_pe * 10)) + await self.driver._binary_interpreter(nid, "AC", 0, CmdType.ValSet, str(srch_acc)) + await self.driver._binary_interpreter(nid, "DC", 0, CmdType.ValSet, str(srch_acc)) + for i in [3, 4, 5, 2]: + await self.driver._binary_interpreter(nid, "HM", i, CmdType.ValSet, "0") + await self.driver._binary_interpreter(nid, "JV", 0, CmdType.ValSet, str(srch_vel)) + + try: + params = [str(int(hs_pe)), str(int(timeout * 1000))] + last_line = await self.driver._user_program_run( + nid, "Home", params, int(timeout), True + ) + if last_line in [1, 2, 3]: + raise RuntimeError(f"Homing Script Error {34 + last_line}") + + curr_pos = await self.driver.motor_get_current_position(nid) + await self.driver._binary_interpreter(nid, "PA", 0, CmdType.ValSet, str(curr_pos)) + await self.driver._binary_interpreter(nid, "SP", 0, CmdType.ValSet, str(srch_vel)) + await self.driver._binary_interpreter(nid, "AC", 0, CmdType.ValSet, str(srch_acc)) + await self.driver._binary_interpreter(nid, "DC", 0, CmdType.ValSet, str(srch_acc)) + finally: + await asyncio.sleep(0.3) + await self.driver._binary_interpreter(nid, "BG", 0, CmdType.Execute, value="0") + await asyncio.sleep(0.3) + await self.driver._binary_interpreter(nid, "ER", 3, CmdType.ValSet, str(int(max_pe))) + + async def _motor_index_search( + self, + axis: int, + srch_vel: int, + srch_acc: int, + positive_direction: bool, + timeout: float, + ) -> tuple: + nid = int(axis) + await self.driver._binary_interpreter(nid, "HM", 1, CmdType.ValSet, "0") + + rev = await self.driver._binary_interpreter(nid, "CA", 18, CmdType.ValQuery) + one_revolution = int(float(rev)) + if not positive_direction: + one_revolution *= -1 + + await self.driver._binary_interpreter(nid, "PR", 1, CmdType.ValSet, str(one_revolution)) + await self.driver._binary_interpreter(nid, "SP", 0, CmdType.ValSet, str(srch_vel)) + await self.driver._binary_interpreter(nid, "AC", 0, CmdType.ValSet, str(srch_acc)) + await self.driver._binary_interpreter(nid, "DC", 0, CmdType.ValSet, str(srch_acc)) + + await self.driver._binary_interpreter(nid, "HM", 3, CmdType.ValSet, "3") # index only + await self.driver._binary_interpreter(nid, "HM", 4, CmdType.ValSet, "0") + await self.driver._binary_interpreter(nid, "HM", 5, CmdType.ValSet, "0") + await self.driver._binary_interpreter(nid, "HM", 2, CmdType.ValSet, "0") + await self.driver._binary_interpreter(nid, "HM", 1, CmdType.ValSet, "1") # arm + + await self.driver._binary_interpreter(nid, "BG", 0, CmdType.Execute) + await self.driver._wait_for_moves_done([nid], timeout) + + left = await self.driver._binary_interpreter(nid, "HM", 1, CmdType.ValQuery) + if left != 0: + raise RuntimeError("Homing Failure: Failed to finish index pulse search.") + + cap = await self.driver._binary_interpreter(nid, "HM", 7, CmdType.ValQuery) + captured_position = int(float(cap)) + return one_revolution, captured_position + + async def home_motor( + self, + axis: int, + hs_offset: int, + ind_offset: int, + home_pos: int, + srch_vel: int, + srch_acc: int, + max_pe: int, + hs_pe: int, + offset_vel: int, + offset_acc: int, + timeout: float, + ) -> None: + nid = int(axis) + + left = await self.driver._binary_interpreter(nid, "CA", 41, CmdType.ValQuery) + if left == 24: + raise RuntimeError("Error 43") + + try: + await self._motor_hard_stop_search(nid, srch_vel, srch_acc, max_pe, hs_pe, timeout) + except Exception as e: + fault = await self.driver.motor_get_fault(nid) + if fault is not None: + raise RuntimeError(fault) + raise e + + await self.driver.motor_enable(node_id=nid, state=True, use_ds402=self._uses_ds402(nid)) + + await self.driver.motors_move_absolute_execute( + plan=MotorsMovePlan( + moves=[ + MotorMoveParam( + node_id=nid, + position=hs_offset, + velocity=offset_vel, + acceleration=offset_acc, + relative=False, + direction=JointMoveDirection.ShortestWay, + ) + ], + ) + ) + + is_positive = hs_offset > 0 + await self._motor_index_search(nid, abs(srch_vel), srch_acc, is_positive, timeout) + + await self.driver.motors_move_absolute_execute( + plan=MotorsMovePlan( + moves=[ + MotorMoveParam( + node_id=nid, + position=ind_offset, + velocity=offset_vel, + acceleration=offset_acc, + relative=False, + direction=JointMoveDirection.ShortestWay, + ) + ] + ) + ) + await self._motor_reset_encoder_position(nid, home_pos) + await self._motor_set_homed_status(nid, HomeStatus.Homed) + + # -- servo gripper ------------------------------------------------------ + async def servo_gripper_initialize(self): try: - await self.driver.motor_enable(axis=KX2Axis.SERVO_GRIPPER, state=True) + await self.driver.motor_enable( + node_id=int(self.Axis.SERVO_GRIPPER), state=True, use_ds402=False + ) except Exception as e: logger.warning( - "Error enabling servo gripper motor on node %s: %s", KX2Axis.SERVO_GRIPPER, e + "Error enabling servo gripper motor on node %s: %s", self.Axis.SERVO_GRIPPER, e ) await self.servo_gripper_home() @@ -255,24 +337,26 @@ async def servo_gripper_initialize(self): await self.servo_gripper_close() async def servo_gripper_home(self) -> None: - await self.driver.motor_send_command( - node_id=int(KX2Axis.SERVO_GRIPPER), - motor_command="PL", - index=1, + await self.driver._binary_interpreter( + node_id=int(self.Axis.SERVO_GRIPPER), + cmd="PL", + cmd_index=1, + cmd_type=CmdType.ValSet, value=str(self.servo_gripper_peak_current), val_type=ValType.Float, ) - await self.driver.motor_send_command( - node_id=int(KX2Axis.SERVO_GRIPPER), - motor_command="CL", - index=1, + await self.driver._binary_interpreter( + node_id=int(self.Axis.SERVO_GRIPPER), + cmd="CL", + cmd_index=1, + cmd_type=CmdType.ValSet, value=str(self.servo_gripper_continuous_current), val_type=ValType.Float, ) - await self.driver.home_motor( - axis=KX2Axis.SERVO_GRIPPER, + await self.home_motor( + axis=self.Axis.SERVO_GRIPPER, hs_offset=self.servo_gripper_home_hard_stop_offset, ind_offset=self.servo_gripper_home_index_offset, home_pos=self.servo_gripper_home_pos, @@ -296,33 +380,39 @@ async def servo_gripper_set_default_gripping_force(self, max_force_percent: int) cont_current = float(self.servo_gripper_continuous_current) * max_force_percent / 100.0 peak_current = float(self.servo_gripper_peak_current) * max_force_percent / 100.0 - axis = KX2Axis.SERVO_GRIPPER + axis = self.Axis.SERVO_GRIPPER # 1) PL with unscaled peak current - await self.driver.motor_send_command( - axis, "PL", 1, str(self.servo_gripper_peak_current), val_type=ValType.Float + await self.driver._binary_interpreter( + int(axis), "PL", 1, CmdType.ValSet, str(self.servo_gripper_peak_current), val_type=ValType.Float ) # 2) CL with scaled continuous current - await self.driver.motor_send_command(axis, "CL", 1, str(cont_current), val_type=ValType.Float) + await self.driver._binary_interpreter( + int(axis), "CL", 1, CmdType.ValSet, str(cont_current), val_type=ValType.Float + ) # 3) PL with scaled peak current - await self.driver.motor_send_command(axis, "PL", 1, str(peak_current), val_type=ValType.Float) + await self.driver._binary_interpreter( + int(axis), "PL", 1, CmdType.ValSet, str(peak_current), val_type=ValType.Float + ) self.servo_gripper_force_percent = max_force_percent async def get_servo_gripper_max_force(self) -> float: """Return current gripping force as percentage of max (0-1).""" - cl = await self.driver.motor_send_command( - node_id=KX2Axis.SERVO_GRIPPER, - motor_command="CL", - index=1, + cl = await self.driver._binary_interpreter( + node_id=int(self.Axis.SERVO_GRIPPER), + cmd="CL", + cmd_index=1, + cmd_type=CmdType.ValQuery, ) - iq = await self.driver.motor_send_command( - node_id=KX2Axis.SERVO_GRIPPER, - motor_command="IQ", - index=0, + iq = await self.driver._binary_interpreter( + node_id=int(self.Axis.SERVO_GRIPPER), + cmd="IQ", + cmd_index=0, + cmd_type=CmdType.ValQuery, ) if cl == 0: @@ -332,10 +422,11 @@ async def get_servo_gripper_max_force(self) -> float: async def check_plate_gripped(self, num_attempts: int = 5) -> None: for _ in range(num_attempts): - motor_status = await self.driver.motor_send_command( - node_id=KX2Axis.SERVO_GRIPPER, - motor_command="MS", - index=1, + motor_status = await self.driver._binary_interpreter( + node_id=int(self.Axis.SERVO_GRIPPER), + cmd="MS", + cmd_index=1, + cmd_type=CmdType.ValQuery, ) logger.debug("Servo gripper motor status: %s", motor_status) @@ -348,7 +439,7 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: if max_force_percentage > 90: return - current_position = await self.motor_get_current_position(KX2Axis.SERVO_GRIPPER) + current_position = await self.motor_get_current_position(self.Axis.SERVO_GRIPPER) closed_position = 1 if abs(current_position - closed_position) < 2.0 / 625: raise RuntimeError( @@ -358,7 +449,7 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: return elif motor_status == 2: - motor_fault = self.driver.motor_get_fault(KX2Axis.SERVO_GRIPPER) + motor_fault = self.driver.motor_get_fault(self.Axis.SERVO_GRIPPER) if motor_fault is None: raise RuntimeError("Error querying whether plate is gripped. Error querying motor fault.") raise RuntimeError( @@ -373,7 +464,7 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: async def servo_gripper_close(self, closed_position: int = 0, check_plate_gripped=True) -> None: await self.motors_move_joint( - {KX2Axis.SERVO_GRIPPER: closed_position}, + {self.Axis.SERVO_GRIPPER: closed_position}, cmd_vel_pct=100, cmd_accel_pct=100, ) @@ -383,7 +474,7 @@ async def servo_gripper_close(self, closed_position: int = 0, check_plate_grippe async def servo_gripper_open(self, open_position: float) -> None: await self.motors_move_joint( - {KX2Axis.SERVO_GRIPPER: open_position}, + {self.Axis.SERVO_GRIPPER: open_position}, cmd_vel_pct=100, cmd_accel_pct=100, ) @@ -400,10 +491,11 @@ async def drive_set_move_count_parameters( last_maintenance_performed_date_rail: int, ) -> None: # MoveCount -> Z axis, UI index 22 - await self.driver.motor_send_command( - node_id=KX2Axis.Z, - motor_command="UI", - index=22, + await self.driver._binary_interpreter( + node_id=int(self.Axis.Z), + cmd="UI", + cmd_index=22, + cmd_type=CmdType.ValSet, value=str(int(move_count)), val_type=ValType.Int, low_priority=False, @@ -417,40 +509,44 @@ async def drive_set_move_count_parameters( pairs = zip(self.node_id_list, travel) for node_id, dist in pairs: - await self.driver.motor_send_command( + await self.driver._binary_interpreter( node_id=int(node_id), - motor_command="UF", - index=5, + cmd="UF", + cmd_index=5, + cmd_type=CmdType.ValSet, value=str(float(dist)), val_type=ValType.Float, low_priority=True, ) # LastMaintenancePerformed -> Z axis, UF index 6 - await self.driver.motor_send_command( - node_id=KX2Axis.Z, - motor_command="UF", - index=6, + await self.driver._binary_interpreter( + node_id=int(self.Axis.Z), + cmd="UF", + cmd_index=6, + cmd_type=CmdType.ValSet, value=str(float(last_maintenance_performed)), val_type=ValType.Float, low_priority=True, ) # MaintenanceRequired -> Z axis, UI index 23 - await self.driver.motor_send_command( - node_id=KX2Axis.Z, - motor_command="UI", - index=23, + await self.driver._binary_interpreter( + node_id=int(self.Axis.Z), + cmd="UI", + cmd_index=23, + cmd_type=CmdType.ValSet, value="1" if maintenance_required else "0", val_type=ValType.Int, low_priority=False, ) # LastMaintenancePerformedDate -> Z axis, UI index 21 - await self.driver.motor_send_command( - node_id=KX2Axis.Z, - motor_command="UI", - index=21, + await self.driver._binary_interpreter( + node_id=int(self.Axis.Z), + cmd="UI", + cmd_index=21, + cmd_type=CmdType.ValSet, value=str(int(last_maintenance_performed_date)), val_type=ValType.Int, low_priority=False, @@ -458,34 +554,37 @@ async def drive_set_move_count_parameters( # Rail (if present) if self.robot_on_rail: - await self.driver.motor_send_command( - node_id=KX2Axis.RAIL, - motor_command="UF", - index=6, + await self.driver._binary_interpreter( + node_id=int(self.Axis.RAIL), + cmd="UF", + cmd_index=6, + cmd_type=CmdType.ValSet, value=str(float(last_maintenance_performed_rail)), val_type=ValType.Float, low_priority=True, ) - await self.driver.motor_send_command( - node_id=KX2Axis.RAIL, - motor_command="UI", - index=23, + await self.driver._binary_interpreter( + node_id=int(self.Axis.RAIL), + cmd="UI", + cmd_index=23, + cmd_type=CmdType.ValSet, value="1" if maintenance_required_rail else "0", val_type=ValType.Int, low_priority=False, ) - await self.driver.motor_send_command( - node_id=KX2Axis.RAIL, - motor_command="UI", - index=21, + await self.driver._binary_interpreter( + node_id=int(self.Axis.RAIL), + cmd="UI", + cmd_index=21, + cmd_type=CmdType.ValSet, value=str(int(last_maintenance_performed_date_rail)), val_type=ValType.Int, low_priority=False, ) - async def drive_get_parameters(self, node_ids) -> None: # TODO: list[KX2Axis] + async def drive_get_parameters(self, node_ids) -> None: self.robot_on_rail = False self.has_servo_gripper = False @@ -501,7 +600,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # Pass 1: identify axes by UI[4] uis = set() for node in nodes: - if node == await self.driver.motor_send_command(node, "UI", 4, val_type=ValType.Int): + if node == await self.driver._binary_interpreter(int(node), "UI", 4, CmdType.ValQuery, val_type=ValType.Int): uis.add(node) for required_axis in MOTION_AXES: if required_axis.value not in uis: @@ -518,7 +617,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # UI[5..10] digital inputs for ui_idx in range(5, 11): - ret = await self.driver.motor_send_command(axis, "UI", ui_idx, val_type=ValType.Int) + ret = await self.driver._binary_interpreter(int(axis), "UI", ui_idx, CmdType.ValQuery, val_type=ValType.Int) ch = (ui_idx - 5) + 1 if ret == 101: set2d(self.digital_input_assignment, axis, ch, "GripperSensor") @@ -538,7 +637,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # UI[11..12] analog inputs for ui_idx in range(11, 13): - ret = await self.driver.motor_send_command(axis, "UI", ui_idx, val_type=ValType.Int) + ret = await self.driver._binary_interpreter(int(axis), "UI", ui_idx, CmdType.ValQuery, val_type=ValType.Int) ch = (ui_idx - 11) + 1 set2d( self.AnalogInputAssignment, @@ -549,7 +648,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # UI[13..16] outputs for ui_idx in range(13, 17): - ret = await self.driver.motor_send_command(axis, "UI", ui_idx, val_type=ValType.Int) + ret = await self.driver._binary_interpreter(int(axis), "UI", ui_idx, CmdType.ValQuery, val_type=ValType.Int) ch = (ui_idx - 13) + 1 if ret == 101: @@ -581,14 +680,14 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: ) # UI[24] drive serial number - ret = await self.driver.motor_send_command(axis, "UI", 24, val_type=ValType.Int) + ret = await self.driver._binary_interpreter(int(axis), "UI", 24, CmdType.ValQuery, val_type=ValType.Int) if _is_number(ret): # self.drive_serial_number[axis] = int(ret) pass # UF[1], UF[2] conversion factor - uf1 = await self.driver.motor_send_command(axis, "UF", 1, val_type=ValType.Float) - uf2 = await self.driver.motor_send_command(axis, "UF", 2, val_type=ValType.Float) + uf1 = await self.driver._binary_interpreter(int(axis), "UF", 1, CmdType.ValQuery, val_type=ValType.Float) + uf2 = await self.driver._binary_interpreter(int(axis), "UF", 2, CmdType.ValQuery, val_type=ValType.Float) if ( not (_is_number(uf1) and _is_number(uf2)) or _to_float(uf1) == 0.0 or _to_float(uf2) == 0.0 ): @@ -596,12 +695,12 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: self.motor_conversion_factor_ax[axis] = _to_float(uf1) / _to_float(uf2) # XM / travel - xm1 = await self.driver.motor_send_command(axis, "XM", 1, val_type=ValType.Int) - xm2 = await self.driver.motor_send_command(axis, "XM", 2, val_type=ValType.Int) - uf3 = await self.driver.motor_send_command(axis, "UF", 3, val_type=ValType.Float) - uf4 = await self.driver.motor_send_command(axis, "UF", 4, val_type=ValType.Float) - vh3 = await self.driver.motor_send_command(axis, "VH", 3, val_type=ValType.Int) - vl3 = await self.driver.motor_send_command(axis, "VL", 3, val_type=ValType.Int) + xm1 = await self.driver._binary_interpreter(int(axis), "XM", 1, CmdType.ValQuery, val_type=ValType.Int) + xm2 = await self.driver._binary_interpreter(int(axis), "XM", 2, CmdType.ValQuery, val_type=ValType.Int) + uf3 = await self.driver._binary_interpreter(int(axis), "UF", 3, CmdType.ValQuery, val_type=ValType.Float) + uf4 = await self.driver._binary_interpreter(int(axis), "UF", 4, CmdType.ValQuery, val_type=ValType.Float) + vh3 = await self.driver._binary_interpreter(int(axis), "VH", 3, CmdType.ValQuery, val_type=ValType.Int) + vl3 = await self.driver._binary_interpreter(int(axis), "VL", 3, CmdType.ValQuery, val_type=ValType.Int) self.max_travel_ax[axis] = _to_float(uf3) self.min_travel_ax[axis] = _to_float(uf4) @@ -624,13 +723,13 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: ) # Encoder socket/type - ca45 = await self.driver.motor_send_command(axis, "CA", 45, val_type=ValType.Int) + ca45 = await self.driver._binary_interpreter(int(axis), "CA", 45, CmdType.ValQuery, val_type=ValType.Int) ca45v = _to_float(ca45, 0.0) if (not _is_number(ca45)) or not (0.0 < ca45v <= 4.0): raise CanError(f"Invalid encoder socket number received from axis {axis}. CA[45]={ca45}") - enc_type = await self.driver.motor_send_command( - axis, "CA", int(round(40.0 + ca45v)), val_type=ValType.Int + enc_type = await self.driver._binary_interpreter( + int(axis), "CA", int(round(40.0 + ca45v)), CmdType.ValQuery, val_type=ValType.Int ) if enc_type in (1, 2): self.absolute_encoder_ax[axis] = False @@ -641,60 +740,60 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: f"Unsupported encoder type specified for axis {axis}. CA[4{ca45}]={enc_type}" ) - ca46 = await self.driver.motor_send_command(axis, "CA", 46, val_type=ValType.Int) + ca46 = await self.driver._binary_interpreter(int(axis), "CA", 46, CmdType.ValQuery, val_type=ValType.Int) if ca45 == ca46: num3 = 1.0 else: - ff3 = await self.driver.motor_send_command(axis, "FF", 3, val_type=ValType.Float) + ff3 = await self.driver._binary_interpreter(int(axis), "FF", 3, CmdType.ValQuery, val_type=ValType.Float) num3 = _to_float(ff3, 1.0) denom = self.motor_conversion_factor_ax[axis] * num3 # or 1.0 - sp2 = await self.driver.motor_send_command(axis, "SP", 2, val_type=ValType.Int) + sp2 = await self.driver._binary_interpreter(int(axis), "SP", 2, CmdType.ValQuery, val_type=ValType.Int) if sp2 == 100000: - vh2 = await self.driver.motor_send_command(axis, "VH", 2, val_type=ValType.Int) + vh2 = await self.driver._binary_interpreter(int(axis), "VH", 2, CmdType.ValQuery, val_type=ValType.Int) self.max_vel_ax[axis] = _to_float(vh2) / 1.01 / denom else: self.max_vel_ax[axis] = _to_float(sp2) / denom - sd0 = await self.driver.motor_send_command(axis, "SD", 0, val_type=ValType.Int) + sd0 = await self.driver._binary_interpreter(int(axis), "SD", 0, CmdType.ValQuery, val_type=ValType.Int) self.max_accel_ax[axis] = _to_float(sd0) / 1.01 / denom # Robot-level params from shoulder_ax - shoulder = KX2Axis.SHOULDER + shoulder = self.Axis.SHOULDER self.base_to_gripper_clearance_z = _to_float( - await self.driver.motor_send_command(shoulder, "UF", 6, val_type=ValType.Float) + await self.driver._binary_interpreter(int(shoulder), "UF", 6, CmdType.ValQuery, val_type=ValType.Float) ) self.base_to_gripper_clearance_arm = _to_float( - await self.driver.motor_send_command(shoulder, "UF", 7, val_type=ValType.Float) + await self.driver._binary_interpreter(int(shoulder), "UF", 7, CmdType.ValQuery, val_type=ValType.Float) ) self.wrist_offset = _to_float( - await self.driver.motor_send_command(shoulder, "UF", 8, val_type=ValType.Float) + await self.driver._binary_interpreter(int(shoulder), "UF", 8, CmdType.ValQuery, val_type=ValType.Float) ) self.elbow_offset = _to_float( - await self.driver.motor_send_command(shoulder, "UF", 9, val_type=ValType.Float) + await self.driver._binary_interpreter(int(shoulder), "UF", 9, CmdType.ValQuery, val_type=ValType.Float) ) self.elbow_zero_offset = _to_float( - await self.driver.motor_send_command(shoulder, "UF", 10, val_type=ValType.Float) + await self.driver._binary_interpreter(int(shoulder), "UF", 10, CmdType.ValQuery, val_type=ValType.Float) ) self.MaxLinearVelMMPerSec = _to_float( - await self.driver.motor_send_command(shoulder, "UF", 11, val_type=ValType.Float) + await self.driver._binary_interpreter(int(shoulder), "UF", 11, CmdType.ValQuery, val_type=ValType.Float) ) self.MaxLinearAccelMMPerSec2 = _to_float( - await self.driver.motor_send_command(shoulder, "UF", 12, val_type=ValType.Float) + await self.driver._binary_interpreter(int(shoulder), "UF", 12, CmdType.ValQuery, val_type=ValType.Float) ) self.MaxLinearJerkMMPerSec3 = _to_float( - await self.driver.motor_send_command(shoulder, "UF", 13, val_type=ValType.Float) + await self.driver._binary_interpreter(int(shoulder), "UF", 13, CmdType.ValQuery, val_type=ValType.Float) ) self.MaxRotaryVelDegPerSec = _to_float( - await self.driver.motor_send_command(shoulder, "UF", 14, val_type=ValType.Float) + await self.driver._binary_interpreter(int(shoulder), "UF", 14, CmdType.ValQuery, val_type=ValType.Float) ) self.MaxRotaryAccelDegPerSec2 = _to_float( - await self.driver.motor_send_command(shoulder, "UF", 15, val_type=ValType.Float) + await self.driver._binary_interpreter(int(shoulder), "UF", 15, CmdType.ValQuery, val_type=ValType.Float) ) - ui17 = await self.driver.motor_send_command(shoulder, "UI", 17, val_type=ValType.Int) + ui17 = await self.driver._binary_interpreter(int(shoulder), "UI", 17, CmdType.ValQuery, val_type=ValType.Int) self.pvt_time_interval_msec = ( 25 if (not _is_number(ui17) or _to_float(ui17) <= 0.0 or _to_float(ui17) > 255.0) @@ -702,46 +801,46 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: ) # Servo gripper params (only if present) - sg = KX2Axis.SERVO_GRIPPER + sg = self.Axis.SERVO_GRIPPER self.servo_gripper_home_pos = int( - await self.driver.motor_send_command(sg, "UF", 6, val_type=ValType.Float) + await self.driver._binary_interpreter(int(sg), "UF", 6, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_search_vel = int( - await self.driver.motor_send_command(sg, "UF", 7, val_type=ValType.Float) + await self.driver._binary_interpreter(int(sg), "UF", 7, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_search_accel = int( - await self.driver.motor_send_command(sg, "UF", 8, val_type=ValType.Float) + await self.driver._binary_interpreter(int(sg), "UF", 8, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_default_position_error = int( - await self.driver.motor_send_command(sg, "UF", 9, val_type=ValType.Float) + await self.driver._binary_interpreter(int(sg), "UF", 9, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_hard_stop_position_error = int( - await self.driver.motor_send_command(sg, "UF", 10, val_type=ValType.Float) + await self.driver._binary_interpreter(int(sg), "UF", 10, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_hard_stop_offset = int( - await self.driver.motor_send_command(sg, "UF", 11, val_type=ValType.Float) + await self.driver._binary_interpreter(int(sg), "UF", 11, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_index_offset = int( - await self.driver.motor_send_command(sg, "UF", 12, val_type=ValType.Float) + await self.driver._binary_interpreter(int(sg), "UF", 12, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_offset_vel = int( - await self.driver.motor_send_command(sg, "UF", 13, val_type=ValType.Float) + await self.driver._binary_interpreter(int(sg), "UF", 13, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_offset_accel = int( - await self.driver.motor_send_command(sg, "UF", 14, val_type=ValType.Float) + await self.driver._binary_interpreter(int(sg), "UF", 14, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_timeout_msec = int( - await self.driver.motor_send_command(sg, "UF", 15, val_type=ValType.Float) + await self.driver._binary_interpreter(int(sg), "UF", 15, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_continuous_current = _to_float( - await self.driver.motor_send_command(sg, "UF", 16, val_type=ValType.Float) + await self.driver._binary_interpreter(int(sg), "UF", 16, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_peak_current = _to_float( - await self.driver.motor_send_command(sg, "UF", 17, val_type=ValType.Float) + await self.driver._binary_interpreter(int(sg), "UF", 17, CmdType.ValQuery, val_type=ValType.Float) ) def convert_elbow_position_to_angle(self, elbow_pos: float) -> float: - max_travel = self.max_travel_ax[KX2Axis.ELBOW] + max_travel = self.max_travel_ax[self.Axis.ELBOW] denom = max_travel + self.elbow_zero_offset if elbow_pos > max_travel: @@ -756,19 +855,19 @@ def convert_elbow_position_to_angle(self, elbow_pos: float) -> float: return elbow_angle def convert_elbow_angle_to_position(self, elbow_angle_deg: float) -> float: - elbow_pos = (self.max_travel_ax[KX2Axis.ELBOW] + self.elbow_zero_offset) * math.sin( + elbow_pos = (self.max_travel_ax[self.Axis.ELBOW] + self.elbow_zero_offset) * math.sin( elbow_angle_deg * (math.pi / 180.0) ) - self.elbow_zero_offset if elbow_angle_deg > 90.0: - elbow_pos = 2.0 * self.max_travel_ax[KX2Axis.ELBOW] - elbow_pos + elbow_pos = 2.0 * self.max_travel_ax[self.Axis.ELBOW] - elbow_pos return elbow_pos - async def motor_get_current_position(self, axis: KX2Axis) -> float: - raw = await self.driver.motor_get_current_position(node_id=axis, pu=self.unlimited_travel_ax[axis]) + async def motor_get_current_position(self, axis: "KX2ArmBackend.Axis") -> float: + raw = await self.driver.motor_get_current_position(node_id=int(axis), pu=self.unlimited_travel_ax[axis]) c = self.motor_conversion_factor_ax[axis] - if axis == KX2Axis.ELBOW: + if axis == self.Axis.ELBOW: return self.convert_elbow_angle_to_position(elbow_angle_deg=raw / c) else: if c == 0: @@ -794,7 +893,7 @@ def _wrap_to_range(x: float, lo: float, hi: float) -> float: return x @staticmethod - def _profile(dist: float, v: float, a: float) -> tuple[float, float, float]: + def _profile(dist: float, v: float, a: float) -> tuple: """ Returns (v, a, t_total) after applying triangular fallback if needed. If the distance is short, you cannot reach v before you must decelerate. @@ -823,18 +922,17 @@ def _profile(dist: float, v: float, a: float) -> tuple[float, float, float]: async def calculate_move_abs_all_axes( self, - cmd_pos: Dict["KX2Axis", float], + cmd_pos: Dict["KX2ArmBackend.Axis", float], cmd_vel_pct: float, cmd_accel_pct: float, ) -> Optional[MotorsMovePlan]: target = cmd_pos.copy() axes = list(target.keys()) - enc_pos: Dict[KX2Axis, float] = {} - enc_vel: Dict[KX2Axis, float] = {} - enc_accel: Dict[KX2Axis, float] = {} - # enc_move_dist: Dict[KX2Axis, float] = {} - skip_ax: Dict[KX2Axis, bool] = {} + enc_pos: Dict[KX2ArmBackend.Axis, float] = {} + enc_vel: Dict[KX2ArmBackend.Axis, float] = {} + enc_accel: Dict[KX2ArmBackend.Axis, float] = {} + skip_ax: Dict[KX2ArmBackend.Axis, bool] = {} # input validation / travel limits / done-wait logic if cmd_vel_pct <= 0.0 or cmd_vel_pct > 100.0: @@ -843,32 +941,14 @@ async def calculate_move_abs_all_axes( raise ValueError("CmdAccel out of range") # Convert elbow cmd from position->angle for planning math - if KX2Axis.ELBOW in axes: - target[KX2Axis.ELBOW] = self.convert_elbow_position_to_angle(target[KX2Axis.ELBOW]) - - # Ensure per-axis ready and clamp travel limits like - # for ax in axes: - # if self.unlimited_travel_ax[ax]: - # continue - # high = self.max_travel_ax[ax] - # low = self.min_travel_ax[ax] - # print("ax", ax, "target", target[ax], "low", low, "high", high) - # if target[ax] > high: - # if (target[ax] - high) < 0.1: - # target[ax] = high - # else: - # raise ValueError(f"Axis {ax!r} above max travel") - # if target[ax] < low: - # if (low - target[ax]) < 0.1: - # target[ax] = low - # else: - # raise ValueError(f"Axis {ax!r} below min travel") + if self.Axis.ELBOW in axes: + target[self.Axis.ELBOW] = self.convert_elbow_position_to_angle(target[self.Axis.ELBOW]) # Clearance check - if KX2Axis.Z in axes: + if self.Axis.Z in axes: if ( - target[KX2Axis.Z] < self.min_travel_ax[KX2Axis.Z] + self.base_to_gripper_clearance_z - and target[KX2Axis.ELBOW] < self.base_to_gripper_clearance_arm + target[self.Axis.Z] < self.min_travel_ax[self.Axis.Z] + self.base_to_gripper_clearance_z + and target[self.Axis.ELBOW] < self.base_to_gripper_clearance_arm ): raise ValueError("Base-to-gripper clearance violated") @@ -876,8 +956,8 @@ async def calculate_move_abs_all_axes( curr = await self.request_joint_position() # Elbow: convert both target and start to angle for distance math - if KX2Axis.ELBOW in curr: - curr[KX2Axis.ELBOW] = self.convert_elbow_position_to_angle(curr[KX2Axis.ELBOW]) + if self.Axis.ELBOW in curr: + curr[self.Axis.ELBOW] = self.convert_elbow_position_to_angle(curr[self.Axis.ELBOW]) # Handle unlimited travel normalization when direction != NORMAL for ax in axes: @@ -888,11 +968,11 @@ async def calculate_move_abs_all_axes( target[ax] = self._wrap_to_range(target[ax], self.min_travel_ax[ax], self.max_travel_ax[ax]) # Distances, skip flags, initial v/a per axis - dist: Dict[KX2Axis, float] = {} - v: Dict[KX2Axis, float] = {} - a: Dict[KX2Axis, float] = {} - accel_time: Dict[KX2Axis, float] = {} - total_time: Dict[KX2Axis, float] = {} + dist: Dict[KX2ArmBackend.Axis, float] = {} + v: Dict[KX2ArmBackend.Axis, float] = {} + a: Dict[KX2ArmBackend.Axis, float] = {} + accel_time: Dict[KX2ArmBackend.Axis, float] = {} + total_time: Dict[KX2ArmBackend.Axis, float] = {} for ax in axes: if self.unlimited_travel_ax[ax]: @@ -976,11 +1056,9 @@ async def calculate_move_abs_all_axes( move_time = max(total_time[ax] for ax in axes) # Convert back to encoder units (and elbow back to "position" space) - # keep target in angle-space for elbow during math, then later use conversion factor. for ax in axes: conv = self.motor_conversion_factor_ax[ax] enc_pos[ax] = target[ax] * conv - # enc_move_dist[ax] = dist[ax] * conv if skip_ax[ax]: enc_vel[ax] = 1000.0 @@ -992,7 +1070,7 @@ async def calculate_move_abs_all_axes( return MotorsMovePlan( moves=[ MotorMoveParam( - axis=ax, + node_id=int(ax), position=int(round(enc_pos[ax])), velocity=int(round(enc_vel[ax])), acceleration=int(round(enc_accel[ax])), @@ -1005,7 +1083,7 @@ async def calculate_move_abs_all_axes( async def motors_move_joint( self, - cmd_pos: Dict["KX2Axis", float], + cmd_pos: Dict["KX2ArmBackend.Axis", float], cmd_vel_pct: float, cmd_accel_pct: float, ) -> None: @@ -1021,11 +1099,11 @@ async def motors_move_joint( await self.driver.motors_move_absolute_execute(plan) - def convert_cartesian_to_joint_position(self, pose: GripperPose) -> Dict["KX2Axis", float]: + def convert_cartesian_to_joint_position(self, pose: GripperPose) -> Dict["KX2ArmBackend.Axis", float]: if pose.rotation.x != 0 or pose.rotation.y != 0: raise ValueError("Only Z rotation is supported for KX2") - joint_position: Dict[KX2Axis, float] = {} + joint_position: Dict[KX2ArmBackend.Axis, float] = {} x, y = (pose.location.x), (pose.location.y) @@ -1034,49 +1112,49 @@ def convert_cartesian_to_joint_position(self, pose: GripperPose) -> Dict["KX2Axi if abs(shoulder + 180.0) < 1e-12: shoulder = 180.0 - joint_position[KX2Axis.SHOULDER] = shoulder + joint_position[self.Axis.SHOULDER] = shoulder # Z axis - joint_position[KX2Axis.Z] = pose.location.z + joint_position[self.Axis.Z] = pose.location.z # Elbow axis elbow = ( math.sqrt(x * x + y * y) - self.wrist_offset - self.elbow_offset - self.elbow_zero_offset ) - joint_position[KX2Axis.ELBOW] = elbow + joint_position[self.Axis.ELBOW] = elbow # Wrist axis - wrist = (pose.rotation.z) - joint_position[KX2Axis.SHOULDER] - joint_position[KX2Axis.WRIST] = wrist + wrist = (pose.rotation.z) - joint_position[self.Axis.SHOULDER] + joint_position[self.Axis.WRIST] = wrist # Wrap wrist into travel range if possible by +/- 360 - w = joint_position[KX2Axis.WRIST] - wmin = self.min_travel_ax[KX2Axis.WRIST] - wmax = self.max_travel_ax[KX2Axis.WRIST] + w = joint_position[self.Axis.WRIST] + wmin = self.min_travel_ax[self.Axis.WRIST] + wmax = self.max_travel_ax[self.Axis.WRIST] if (w < wmin - 0.001) and (w + 360.0 <= wmax): w += 360.0 elif (w > wmax + 0.001) and (w - 360.0 >= wmin): w -= 360.0 - joint_position[KX2Axis.WRIST] = w + joint_position[self.Axis.WRIST] = w return joint_position def convert_joint_position_to_cartesian( - self, joint_position: Dict["KX2Axis", float] + self, joint_position: Dict["KX2ArmBackend.Axis", float] ) -> GripperPose: r = ( - self.wrist_offset + self.elbow_offset + self.elbow_zero_offset + joint_position[KX2Axis.ELBOW] + self.wrist_offset + self.elbow_offset + self.elbow_zero_offset + joint_position[self.Axis.ELBOW] ) - sh_deg = joint_position[KX2Axis.SHOULDER] + sh_deg = joint_position[self.Axis.SHOULDER] sh = math.radians(sh_deg) location = Coordinate( x=(-(r) * math.sin(sh)), y=((r) * math.cos(sh)), - z=(joint_position[KX2Axis.Z]), + z=(joint_position[self.Axis.Z]), ) - rotation_z = joint_position[KX2Axis.WRIST] + sh_deg + rotation_z = joint_position[self.Axis.WRIST] + sh_deg # wrap to [-180, 180] if rotation_z > 180.0: @@ -1180,7 +1258,7 @@ async def open_gripper( self, gripper_width: float, backend_params: Optional[BackendParams] = None ) -> None: await self.motors_move_joint( - {KX2Axis.SERVO_GRIPPER: gripper_width}, + {self.Axis.SERVO_GRIPPER: gripper_width}, cmd_vel_pct=100, cmd_accel_pct=100, ) @@ -1191,7 +1269,7 @@ async def close_gripper( if not isinstance(backend_params, KX2ArmBackend.GripParams): backend_params = KX2ArmBackend.GripParams() await self.motors_move_joint( - {KX2Axis.SERVO_GRIPPER: gripper_width}, + {self.Axis.SERVO_GRIPPER: gripper_width}, cmd_vel_pct=100, cmd_accel_pct=100, ) @@ -1199,7 +1277,7 @@ async def close_gripper( await self.check_plate_gripped() async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool: - pos = await self.motor_get_current_position(KX2Axis.SERVO_GRIPPER) + pos = await self.motor_get_current_position(self.Axis.SERVO_GRIPPER) return abs(pos) < 1.0 async def move_to_location( @@ -1245,7 +1323,7 @@ async def move_to_joint_position( ) -> None: if not isinstance(backend_params, KX2ArmBackend.JointMoveParams): backend_params = KX2ArmBackend.JointMoveParams() - cmd_pos = {KX2Axis(int(k)): float(v) for k, v in position.items()} + cmd_pos = {self.Axis(int(k)): float(v) for k, v in position.items()} await self.motors_move_joint( cmd_pos=cmd_pos, cmd_vel_pct=backend_params.vel_pct, @@ -1274,11 +1352,11 @@ async def request_joint_position( self, backend_params: Optional[BackendParams] = None ) -> Dict[int, float]: return { - KX2Axis.SHOULDER: await self.motor_get_current_position(KX2Axis.SHOULDER), - KX2Axis.Z: await self.motor_get_current_position(KX2Axis.Z), - KX2Axis.ELBOW: await self.motor_get_current_position(KX2Axis.ELBOW), - KX2Axis.WRIST: await self.motor_get_current_position(KX2Axis.WRIST), - KX2Axis.SERVO_GRIPPER: await self.motor_get_current_position(KX2Axis.SERVO_GRIPPER), + self.Axis.SHOULDER: await self.motor_get_current_position(self.Axis.SHOULDER), + self.Axis.Z: await self.motor_get_current_position(self.Axis.Z), + self.Axis.ELBOW: await self.motor_get_current_position(self.Axis.ELBOW), + self.Axis.WRIST: await self.motor_get_current_position(self.Axis.WRIST), + self.Axis.SERVO_GRIPPER: await self.motor_get_current_position(self.Axis.SERVO_GRIPPER), } async def start_freedrive_mode( @@ -1287,8 +1365,19 @@ async def start_freedrive_mode( # KX2 frees all motion axes at once; free_axes is accepted for API parity. del free_axes for axis in MOTION_AXES: - await self.driver.motor_enable(axis=axis, state=False) + await self.driver.motor_enable(node_id=int(axis), state=False, use_ds402=True) async def stop_freedrive_mode(self, backend_params: Optional[BackendParams] = None) -> None: for axis in MOTION_AXES: - await self.driver.motor_enable(axis=axis, state=True) + await self.driver.motor_enable(node_id=int(axis), state=True, use_ds402=True) + + +# Motion axes = the four coordinated joints. Defined at module scope (after the +# class body so `KX2ArmBackend.Axis` exists) and imported by callers that need +# to iterate "all motion axes". +MOTION_AXES = ( + KX2ArmBackend.Axis.SHOULDER, + KX2ArmBackend.Axis.Z, + KX2ArmBackend.Axis.ELBOW, + KX2ArmBackend.Axis.WRIST, +) diff --git a/pylabrobot/paa/kx2/kx2_driver.py b/pylabrobot/paa/kx2/kx2_driver.py index 6b64fb6bedc..d179c969a4f 100644 --- a/pylabrobot/paa/kx2/kx2_driver.py +++ b/pylabrobot/paa/kx2/kx2_driver.py @@ -3,6 +3,11 @@ Uses the `canopen` library (python-can bus + CANopen SDO/PDO/NMT/EMCY). Paired with :class:`KX2ArmBackend` in ``kx2_backend.py`` via the standard ``Device`` + ``Driver`` + capability-backend split. + +This module is purely a CAN transport + Elmo interpreter layer. It knows only +CANopen node IDs (ints). All axis-level / robot-topology concepts (axis names, +motion-axis tuples, home status, move plans, joint-move direction, homing +sequences) live in ``kx2_backend``. """ from __future__ import annotations @@ -11,36 +16,162 @@ import logging import struct import time +from dataclasses import dataclass, field +from enum import IntEnum from typing import Dict, List, Optional, Tuple, Union import canopen from pylabrobot.capabilities.capability import BackendParams from pylabrobot.device import Driver -from pylabrobot.paa.kx2.kx2_backend import ( - MOTION_AXES, - COBType, - CanError, - CmdType, - ElmoObjectDataType, - HomeStatus, - JointMoveDirection, - KX2Axis, - MotorMoveParam, - MotorsMovePlan, - PDOTransmissionType, - RPDO, - RPDOMappedObject, - TPDO, - TPDOMappedObject, - TPDOTrigger, - ValType, -) def _u32_le(value: int) -> List[int]: return list((value & 0xFFFFFFFF).to_bytes(4, byteorder="little", signed=False)) + +class CmdType(IntEnum): + ValQuery = 1 + ValSet = 2 + Execute = 3 + + +class ValType(IntEnum): + Int = 1 + Float = 2 + + +class COBType(IntEnum): + NMT = 0 + EMCY = 1 + SYNC = 1 + TIMESTAMP = 2 + TPDO1 = 3 + RPDO1 = 4 + TPDO2 = 5 + RPDO2 = 6 + TPDO3 = 7 + RPDO3 = 8 + TPDO4 = 9 + RPDO4 = 10 + TSDO = 11 + RSDO = 12 + ERRCTRL = 14 + HEARTBEAT = 14 + + +class RPDO(IntEnum): + RPDO1 = 1 + RPDO3 = 3 + RPDO4 = 4 + + +class PDOTransmissionType(IntEnum): + SynchronousAcyclic = 0 + SynchronousCyclic = 1 + EventDrivenManf = 254 # 0xFE + EventDrivenDev = 255 # 0xFF + + +class RPDOMappedObject(IntEnum): + NotMapped = 0 + ControlWord = 0x60400010 + TargetTorque = 0x60710010 + MaxTorque = 0x60720010 + TargetPosition = 0x607A0020 + VelocityOffset = 0x60B10020 + TargetPositionIP = 0x60C10120 + TargetVelocityIP = 0x60C10220 + DigitalOutputs = 0x60FE0020 + TargetVelocity = 0x60FF0020 + + +class TPDO(IntEnum): + TPDO1 = 1 + TPDO3 = 3 + TPDO4 = 4 + + +class TPDOTrigger(IntEnum): + MotionComplete = 0 + MainHomingComplete = 1 + AuxiliaryHomingComplete = 2 + MotorShutDownByException = 3 + MotorStarted = 4 + UserProgramEmitCommand = 5 + OSInterpreterExecutionComplete = 6 + MotionStartedEvent = 8 + PDODataChanged = 24 + DigitalInputEvent = 26 + StatusWordEvent = 27 + BinaryInterpreterCommandComplete = 31 + + +class TPDOMappedObject(IntEnum): + NotMapped = 0 + Timestamp = 0x20410020 + PVTHeadPointer = 0x2F110010 + PVTTailPointer = 0x2F120010 + StatusWord = 0x60410010 + PositionActualValue = 0x60640020 + VelocityDemandValue = 0x606B0020 + VelocityActualValue = 0x606C0020 + TorqueDemandValue = 0x60740010 + TorqueActualValue = 0x60770010 + IPBufferPosition = 0x60C40410 + DigitalInputs = 0x60FD0020 + + +class ElmoObjectDataType(IntEnum): + UNSIGNED8 = 0 + UNSIGNED16 = 1 + UNSIGNED32 = 2 + UNSIGNED64 = 3 + INTEGER8 = 4 + INTEGER16 = 5 + INTEGER32 = 6 + INTEGER64 = 7 + STR = 8 + + +class CanError(Exception): + """Custom exception for CAN motor errors.""" + + +class JointMoveDirection(IntEnum): + """Move-direction hint used by the driver's move primitives. + + Lives here (not in the backend) because the driver's + `_motor_set_move_direction` primitive consumes it to program Elmo's modulo + mode register. Backend-side planning also uses it, but the canonical + definition is the driver's. + """ + + Normal = 0 + Clockwise = 1 + Counterclockwise = 2 + ShortestWay = 3 + + +@dataclass +class MotorMoveParam: + """One axis of a coordinated move, expressed purely in node-ID terms.""" + + # CANopen node ID for this axis. Backend passes `int(self.Axis.X)`. + node_id: int + position: int + velocity: int + acceleration: int + relative: bool = False + direction: JointMoveDirection = JointMoveDirection.ShortestWay + + +@dataclass +class MotorsMovePlan: + moves: List[MotorMoveParam] = field(default_factory=list) + move_time: float = 10.0 + + # Vendor-specific Elmo binary interpreter rides on PDO2 COB-IDs (non-standard). # Request: RPDO2 = (6 << 7) | node_id = 0x300 + node_id # Response: TPDO2 = (5 << 7) | node_id = 0x280 + node_id @@ -58,6 +189,10 @@ class KX2Driver(Driver): raw SDO writes to 0x14xx/0x16xx/0x18xx/0x1Axx for PDO mapping, and `network.send_message` / `network.subscribe` for the vendor-specific Elmo binary interpreter (non-standard, rides on TPDO2/RPDO2 COB-IDs). + + Pure CAN transport + Elmo interpreter primitives — takes node IDs (ints), + knows nothing about robot topology. Axis-level concepts live in + :class:`KX2ArmBackend`. """ def __init__( @@ -82,6 +217,10 @@ def __init__( if has_servo_gripper: self.node_id_list.append(6) + # Motion axes = shoulder/Z/elbow/wrist. Driver only knows node IDs; + # axis-level names live in the backend's KX2ArmBackend.Axis enum. + self.motion_node_ids: List[int] = [1, 2, 3, 4] + self._network: Optional[canopen.Network] = None self._nodes: Dict[int, canopen.RemoteNode] = {} self._loop: Optional[asyncio.AbstractEventLoop] = None @@ -94,7 +233,6 @@ def __init__( self._pvt_mode: bool = False self.EmcyMoveErrorReceived: bool = False - self.grp_id: int = 0 # --- lifecycle ----------------------------------------------------------- @@ -166,33 +304,33 @@ async def connect_part_two(self) -> None: TPDO.TPDO4, node_id, [TPDOMappedObject.DigitalInputs], TPDOTrigger.DigitalInputEvent ) - for axis in MOTION_AXES: + for nid in self.motion_node_ids: await self._can_sdo_download_elmo_object( - int(axis), 24768, 0, "-1", ElmoObjectDataType.INTEGER16 + nid, 24768, 0, "-1", ElmoObjectDataType.INTEGER16 ) await self._can_sdo_download_elmo_object( - int(axis), 24772, 2, "16", ElmoObjectDataType.UNSIGNED32 + nid, 24772, 2, "16", ElmoObjectDataType.UNSIGNED32 ) await self._can_sdo_download_elmo_object( - int(axis), 24772, 3, "0", ElmoObjectDataType.UNSIGNED8 + nid, 24772, 3, "0", ElmoObjectDataType.UNSIGNED8 ) await self._can_sdo_download_elmo_object( - int(axis), 24772, 5, "8", ElmoObjectDataType.UNSIGNED8 + nid, 24772, 5, "8", ElmoObjectDataType.UNSIGNED8 ) await self._can_sdo_download_elmo_object( - int(axis), 24770, 2, "-3", ElmoObjectDataType.INTEGER8 + nid, 24770, 2, "-3", ElmoObjectDataType.INTEGER8 ) await self._can_sdo_download_elmo_object( - int(axis), 24669, 0, "1", ElmoObjectDataType.INTEGER16 + nid, 24669, 0, "1", ElmoObjectDataType.INTEGER16 ) - for axis in MOTION_AXES: + for nid in self.motion_node_ids: await self._rpdo_map( - RPDO.RPDO1, int(axis), [RPDOMappedObject.ControlWord], + RPDO.RPDO1, nid, [RPDOMappedObject.ControlWord], PDOTransmissionType.SynchronousCyclic, ) await self._rpdo_map( - RPDO.RPDO3, int(axis), + RPDO.RPDO3, nid, [RPDOMappedObject.TargetPositionIP, RPDOMappedObject.TargetVelocityIP], PDOTransmissionType.EventDrivenDev, ) @@ -459,7 +597,7 @@ def _float_matches(expected_str: str, actual_str: str) -> bool: # -- dispatch: single node vs. group -- target_nodes = ( - [int(a) for a in MOTION_AXES] if node_id == _GROUP_NODE_ID else [node_id] + list(self.motion_node_ids) if node_id == _GROUP_NODE_ID else [node_id] ) futures: List[asyncio.Future] = [] @@ -575,85 +713,9 @@ async def _control_word_set(self, node_id: int, value: int, sync: bool = True) - if sync: await self._can_sync() - # --- high-level motor command dispatch ---------------------------------- - - _OS_INTERPRETER_CMDS = {"VR", "CD", "LS", "DL", "DF", "BH"} - _NO_QUERY_CMDS = { - "BG", "CP", "EI", "EO", "HP", "HX", "KL", "KR", - "LD", "MI", "PB", "RS", "SV", "XC##", - } - - async def motor_send_command( - self, - node_id: int, - motor_command: str, - index: int, - value: str = "", - val_type: ValType = ValType.Int, - *, - low_priority: bool = False, - ) -> str: - """Send an Elmo motor command, auto-routing between the OS and binary interpreters. - - Classifies by command name to decide which interpreter to use, and by - whether a value was given + whether it's a no-reply command to pick - `ValQuery` / `ValSet` / `Execute`. Returns the drive's response string - for queries; empty string otherwise. - """ - if isinstance(node_id, KX2Axis): - node_id = int(node_id) - logger.debug( - "motor_send_command node=%d cmd=%s[%d] value=%r val_type=%s", - node_id, motor_command, index, value, val_type, - ) - - cmd_u = motor_command.upper() - has_xc = "XC##" in cmd_u - has_xq = "XQ##" in cmd_u - use_os = (cmd_u in self._OS_INTERPRETER_CMDS) or has_xc or has_xq - - if value == "": - if (cmd_u in self._NO_QUERY_CMDS) or has_xq: - cmd_type = CmdType.Execute - else: - cmd_type = CmdType.ValQuery - else: - cmd_type = CmdType.ValSet - - if use_os: - reply = await self._os_interpreter( - node_id=node_id, cmd=motor_command, query=not (has_xc or has_xq) - ) - return reply if (cmd_type == CmdType.ValQuery and reply is not None) else "" - - reply = await self._binary_interpreter( - node_id=node_id, - cmd=motor_command, - cmd_index=int(index), - cmd_type=cmd_type, - value=value, - val_type=val_type, - low_priority=low_priority, - ) - return reply if cmd_type == CmdType.ValQuery else "" - - async def get_estop_state(self) -> bool: - """Return True if the arm is in estop, False otherwise. - - Reads the shoulder drive's SR (status register) via the binary - interpreter. Bits 14/15 encode the stop/safety state. - """ - r = int(await self.motor_send_command( - node_id=int(KX2Axis.SHOULDER), - motor_command="SR", - index=1, - value="", - )) - if r != 8438016: - logger.warning("get_estop_state: SR register unexpected value %d (expected 8438016)", r) - b14 = (r & 0x4000) == 0x4000 - b15 = (r & 0x8000) == 0x8000 - return (not b14) and (not b15) + async def request_drive_version(self, node_id: int) -> str: + """Query Elmo drive firmware version (VR) via the OS interpreter.""" + return await self._os_interpreter(int(node_id), "VR", query=True) # --- DS402 / motor control ---------------------------------------------- @@ -672,41 +734,20 @@ async def motor_get_motion_status(self, node_id: int) -> int: async def _motor_set_move_direction( self, node_id: int, direction: JointMoveDirection ) -> None: - val_str = "1" - if direction == JointMoveDirection.Clockwise: + # 0=Normal, 1=Clockwise, 2=Counterclockwise, 3=ShortestWay. + dir_int = int(direction) + if dir_int == 1: val_str = "65" - elif direction == JointMoveDirection.Counterclockwise: + elif dir_int == 2: val_str = "129" - elif direction == JointMoveDirection.ShortestWay: + elif dir_int == 3: val_str = "193" + else: + val_str = "1" await self._can_sdo_download_elmo_object( node_id, 24818, 0, val_str, ElmoObjectDataType.UNSIGNED16 ) - async def _motor_set_homed_status(self, axis: KX2Axis, status: HomeStatus) -> None: - val = "0" - if status == HomeStatus.Homed: - val = "1" - elif status == HomeStatus.InitializedWithoutHoming: - val = "2" - await self._binary_interpreter(int(axis), "UI", 3, CmdType.ValSet, val) - - async def motor_get_homed_status(self, node_id: int) -> HomeStatus: - left = await self._binary_interpreter(node_id, "UI", 3, CmdType.ValQuery) - if left == 1: - return HomeStatus.Homed - if left == 2: - return HomeStatus.InitializedWithoutHoming - return HomeStatus.NotHomed - - async def _motor_reset_encoder_position(self, axis: KX2Axis, position: float) -> None: - await self._binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") - await self._binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "0") - await self._binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") - await self._binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") - await self._binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, str(position)) - await self._binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") - async def motor_check_if_move_done(self, node_id: int) -> bool: ms_val = await self._binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) if ms_val == 0: @@ -723,8 +764,8 @@ async def motor_check_if_move_done(self, node_id: int) -> bool: return False return False - async def motor_get_fault(self, axis: KX2Axis) -> Optional[str]: - val = await self._binary_interpreter(int(axis), "MF", 0, CmdType.ValQuery) + async def motor_get_fault(self, node_id: int) -> Optional[str]: + val = await self._binary_interpreter(int(node_id), "MF", 0, CmdType.ValQuery) if val == 0: return None assert isinstance(val, int) @@ -770,57 +811,62 @@ async def motor_get_fault(self, axis: KX2Axis) -> Optional[str]: return f"Unknown fault code: {val} (0x{val:08X})" return " ".join(faults) - async def motor_enable(self, axis: KX2Axis, state: bool) -> None: - if not isinstance(axis, KX2Axis): - raise TypeError(f"axis must be KX2Axis, got {type(axis).__name__}") + async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> None: + """Enable or disable a single drive. - # Motion axes use the DS402 controlword sequence over RPDO1; the gripper - # (and any non-motion axis) uses the binary-interpreter MO command. - use_bi = not (axis in MOTION_AXES or int(axis) == self.grp_id) + - ``use_ds402=True``: DS402 controlword sequence over RPDO1 (Fault -> + Shutdown -> Switched On -> Op Enabled on enable; reverse on disable). + Used for the four motion axes (shoulder/Z/elbow/wrist). + - ``use_ds402=False``: vendor binary-interpreter ``MO=1/0``. Used for the + rail and the servo gripper. + + Caller picks the path; the driver does not know about robot topology. + """ + node_id = int(node_id) if state: self.EmcyMoveErrorReceived = False max_attempts = 5 for attempt in range(1, max_attempts + 1): - if use_bi: - await self._binary_interpreter(axis, "MO", 0, CmdType.ValSet, "1") + if not use_ds402: + await self._binary_interpreter(node_id, "MO", 0, CmdType.ValSet, "1") else: # DS402 enable sequence: Fault -> Shutdown -> Switched On -> Op Enabled. # Matches the C# reference (clscanmotor.cs:4495-4509): back-to-back # CW writes, a single 100 ms settle at the end, then MO query. for cw in (0, 128, 6, 7, 15): - await self._control_word_set(node_id=int(axis), value=cw) + await self._control_word_set(node_id=node_id, value=cw) await asyncio.sleep(0.1) left = await self._binary_interpreter( - node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery + node_id=node_id, cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery ) if left == 1: break logger.warning( - "motor_enable attempt %d/%d failed for axis %s (MO=%s); retrying", - attempt, max_attempts, axis, left, + "motor_enable attempt %d/%d failed for node %d (MO=%s); retrying", + attempt, max_attempts, node_id, left, ) else: - raise CanError(f"Motor failed to enable (axis = {axis}) after {max_attempts} attempts") + raise CanError(f"Motor failed to enable (node_id = {node_id}) after {max_attempts} attempts") else: - if use_bi: + if not use_ds402: try: await self._binary_interpreter( - node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValSet, value="0" + node_id=node_id, cmd="MO", cmd_index=0, cmd_type=CmdType.ValSet, value="0" ) except Exception: pass else: # DS402 disable: Op Enabled -> Switched On -> Ready to Switch On. # Matches C# (clscanmotor.cs:4540-4543) — back-to-back, no inter-CW sleep. - await self._control_word_set(node_id=int(axis), value=7) - await self._control_word_set(node_id=int(axis), value=6) + await self._control_word_set(node_id=node_id, value=7) + await self._control_word_set(node_id=node_id, value=6) await asyncio.sleep(0.1) left = await self._binary_interpreter( - node_id=int(axis), cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery + node_id=node_id, cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery ) if left != 0: - raise RuntimeError(f"Motor failed to disable (axis = {axis})") + raise RuntimeError(f"Motor failed to disable (node_id = {node_id})") # --- motion primitives -------------------------------------------------- @@ -828,100 +874,101 @@ async def _pvt_select_mode(self, enable: bool) -> None: """Enable/disable PVT mode on all motion axes via standard SDO writes.""" if enable: if not self._pvt_mode: - for axis in MOTION_AXES: + for nid in self.motion_node_ids: # 0x60C4 sub 6 = 0 (disable interpolation buffer) - await self._can_sdo_download(int(axis), 0x60, 0xC4, 0x06, [0]) + await self._can_sdo_download(nid, 0x60, 0xC4, 0x06, [0]) # 0x6060 = 7 (interpolated position mode) - await self._can_sdo_download(int(axis), 0x60, 0x60, 0x00, [7]) + await self._can_sdo_download(nid, 0x60, 0x60, 0x00, [7]) self._pvt_mode = True else: - for axis in MOTION_AXES: - await self._can_sdo_download(int(axis), 0x60, 0x60, 0x00, [1]) + for nid in self.motion_node_ids: + await self._can_sdo_download(nid, 0x60, 0x60, 0x00, [1]) else: if self._pvt_mode: - for axis in MOTION_AXES: + for nid in self.motion_node_ids: # 0x6060 = 1 (profile position mode) - await self._can_sdo_download(int(axis), 0x60, 0x60, 0x00, [1]) + await self._can_sdo_download(nid, 0x60, 0x60, 0x00, [1]) self._pvt_mode = False async def _wait_for_moves_done( - self, axes: List[KX2Axis], timeout: float + self, node_ids: List[int], timeout: float ) -> None: # Poll MS every 30ms after a 50ms warm-up. The warm-up avoids reading # MS=0 in the window between CW=63 and motion actually starting. assert self._loop is not None - async def _poll_axis(axis: KX2Axis) -> None: + async def _poll_axis(nid: int) -> None: deadline = self._loop.time() + timeout await asyncio.sleep(0.05) while self._loop.time() < deadline: try: - if await self.motor_check_if_move_done(int(axis)): + if await self.motor_check_if_move_done(int(nid)): return except CanError: pass await asyncio.sleep(0.03) # Final authoritative check; propagates CanError / motor-fault. - if not await self.motor_check_if_move_done(int(axis)): - raise CanError(f"Axis {axis} move did not complete within {timeout}s") + if not await self.motor_check_if_move_done(int(nid)): + raise CanError(f"Node {nid} move did not complete within {timeout}s") - await asyncio.gather(*(_poll_axis(a) for a in axes)) + await asyncio.gather(*(_poll_axis(n) for n in node_ids)) async def _motors_move_start( - self, axes: List[KX2Axis], *, relative: bool = False + self, node_ids: List[int], *, relative: bool = False ) -> None: relative_bit = 0x40 if relative else 0 - for i, nid in enumerate(axes): - last = i == (len(axes) - 1) + for i, nid in enumerate(node_ids): + last = i == (len(node_ids) - 1) await self._control_word_set(int(nid), 47 + relative_bit, sync=last) - for i, nid in enumerate(axes): - last = i == (len(axes) - 1) + for i, nid in enumerate(node_ids): + last = i == (len(node_ids) - 1) await self._control_word_set(int(nid), 47 + 0x10 + relative_bit, sync=last) async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: await self._pvt_select_mode(False) for move in plan.moves: - await self._motor_set_move_direction(move.axis.value, move.direction) + nid = int(move.node_id) + await self._motor_set_move_direction(nid, move.direction) # 0x607A = Target Position (24698 decimal) await self._can_sdo_download_elmo_object( - move.axis.value, 24698, 0, str(int(move.position)), ElmoObjectDataType.INTEGER32, + nid, 24698, 0, str(int(move.position)), ElmoObjectDataType.INTEGER32, ) # 0x6081 = Profile Velocity (24705 decimal) await self._can_sdo_download_elmo_object( - move.axis.value, 24705, 0, str(int(move.velocity)), ElmoObjectDataType.UNSIGNED32, + nid, 24705, 0, str(int(move.velocity)), ElmoObjectDataType.UNSIGNED32, ) acc = max(int(move.acceleration), 100) # 0x6083 = Profile Acceleration (24707 decimal) await self._can_sdo_download_elmo_object( - move.axis.value, 24707, 0, str(acc), ElmoObjectDataType.UNSIGNED32, + nid, 24707, 0, str(acc), ElmoObjectDataType.UNSIGNED32, ) # 0x6084 = Profile Deceleration (24708 decimal) await self._can_sdo_download_elmo_object( - move.axis.value, 24708, 0, str(acc), ElmoObjectDataType.UNSIGNED32, + nid, 24708, 0, str(acc), ElmoObjectDataType.UNSIGNED32, ) - axes = [move.axis for move in plan.moves] - await self._motors_move_start(axes) - await self._wait_for_moves_done(axes, timeout=plan.move_time + 2) + node_ids = [int(move.node_id) for move in plan.moves] + await self._motors_move_start(node_ids) + await self._wait_for_moves_done(node_ids, timeout=plan.move_time + 2) async def _user_program_run( self, - axis: KX2Axis, + node_id: int, user_function: str, params=None, timeout_sec: int = 0, wait_until_done: bool = False, ) -> int: - if not isinstance(axis, int): - raise ValueError("axis must be int") - if axis < 0 or axis > 255: - raise ValueError("axis must be in [0, 255]") - node_id = int(axis) + if not isinstance(node_id, int): + raise ValueError("node_id must be int") + if node_id < 0 or node_id > 255: + raise ValueError("node_id must be in [0, 255]") + node_id = int(node_id) ps = int(await self._binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) if ps == -2: - raise CanError(f"Axis {axis}: controller reported PS=-2 (not ready / unavailable)") + raise CanError(f"Node {node_id}: controller reported PS=-2 (not ready / unavailable)") if ps != -1: await self._binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="0") @@ -932,7 +979,7 @@ async def _user_program_run( break await asyncio.sleep(0.01) else: - raise CanError(f"Axis {axis}: did not reach idle state (PS=-1) within 3s (last PS={ps})") + raise CanError(f"Node {node_id}: did not reach idle state (PS=-1) within 3s (last PS={ps})") arg_str = "" if params: @@ -964,151 +1011,17 @@ async def _user_program_run( if ui1 != 0: raise CanError( - f"Axis {axis}: user program ended with UI[1]={ui1} (expected 0), " + f"Node {node_id}: user program ended with UI[1]={ui1} (expected 0), " f"last_line={last_line_completed}" ) if ps == 1 and ui1 == 1: raise CanError( - f"Axis {axis}: timeout waiting for '{user_function}' after {timeout_sec}s, " + f"Node {node_id}: timeout waiting for '{user_function}' after {timeout_sec}s, " f"last_line={last_line_completed}" ) return 0 - async def _motor_hard_stop_search( - self, - axis: KX2Axis, - srch_vel: int, - srch_acc: int, - max_pe: int, - hs_pe: int, - timeout: float, - ) -> None: - await self._binary_interpreter(int(axis), "ER", 3, CmdType.ValSet, str(max_pe * 10)) - await self._binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) - await self._binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) - for i in [3, 4, 5, 2]: - await self._binary_interpreter(int(axis), "HM", i, CmdType.ValSet, "0") - await self._binary_interpreter(int(axis), "JV", 0, CmdType.ValSet, str(srch_vel)) - - try: - params = [str(int(hs_pe)), str(int(timeout * 1000))] - last_line = await self._user_program_run(axis, "Home", params, int(timeout), True) - if last_line in [1, 2, 3]: - raise RuntimeError(f"Homing Script Error {34 + last_line}") - - curr_pos = await self.motor_get_current_position(int(axis)) - await self._binary_interpreter(int(axis), "PA", 0, CmdType.ValSet, str(curr_pos)) - await self._binary_interpreter(int(axis), "SP", 0, CmdType.ValSet, str(srch_vel)) - await self._binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) - await self._binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) - finally: - await asyncio.sleep(0.3) - await self._binary_interpreter(int(axis), "BG", 0, CmdType.Execute, value="0") - await asyncio.sleep(0.3) - await self._binary_interpreter(int(axis), "ER", 3, CmdType.ValSet, str(int(max_pe))) - - async def _motor_index_search( - self, - axis: KX2Axis, - srch_vel: int, - srch_acc: int, - positive_direction: bool, - timeout: float, - ) -> Tuple[int, int]: - assert self._loop is not None - await self._binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") - - rev = await self._binary_interpreter(int(axis), "CA", 18, CmdType.ValQuery) - one_revolution = int(float(rev)) - if not positive_direction: - one_revolution *= -1 - - await self._binary_interpreter(int(axis), "PR", 1, CmdType.ValSet, str(one_revolution)) - await self._binary_interpreter(int(axis), "SP", 0, CmdType.ValSet, str(srch_vel)) - await self._binary_interpreter(int(axis), "AC", 0, CmdType.ValSet, str(srch_acc)) - await self._binary_interpreter(int(axis), "DC", 0, CmdType.ValSet, str(srch_acc)) - - await self._binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "3") # index only - await self._binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") - await self._binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") - await self._binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, "0") - await self._binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") # arm - - await self._binary_interpreter(int(axis), "BG", 0, CmdType.Execute) - await self._wait_for_moves_done([axis], timeout) - - left = await self._binary_interpreter(int(axis), "HM", 1, CmdType.ValQuery) - if left != 0: - raise RuntimeError("Homing Failure: Failed to finish index pulse search.") - - cap = await self._binary_interpreter(int(axis), "HM", 7, CmdType.ValQuery) - captured_position = int(float(cap)) - return one_revolution, captured_position - - async def home_motor( - self, - axis: KX2Axis, - hs_offset: int, - ind_offset: int, - home_pos: int, - srch_vel: int, - srch_acc: int, - max_pe: int, - hs_pe: int, - offset_vel: int, - offset_acc: int, - timeout: float, - ) -> None: - left = await self._binary_interpreter(int(axis), "CA", 41, CmdType.ValQuery) - if left == 24: - raise RuntimeError("Error 43") - - try: - await self._motor_hard_stop_search(axis, srch_vel, srch_acc, max_pe, hs_pe, timeout) - except Exception as e: - fault = await self.motor_get_fault(axis) - if fault is not None: - raise RuntimeError(fault) - raise e - - await self.motor_enable(axis=axis, state=True) - - await self.motors_move_absolute_execute( - plan=MotorsMovePlan( - moves=[ - MotorMoveParam( - axis=KX2Axis(axis), - position=hs_offset, - velocity=offset_vel, - acceleration=offset_acc, - relative=False, - direction=JointMoveDirection.ShortestWay, - ) - ], - ) - ) - - is_positive = hs_offset > 0 - await self._motor_index_search(axis, abs(srch_vel), srch_acc, is_positive, timeout) - - await self.motors_move_absolute_execute( - plan=MotorsMovePlan( - moves=[ - MotorMoveParam( - axis=KX2Axis(axis), - position=ind_offset, - velocity=offset_vel, - acceleration=offset_acc, - relative=False, - direction=JointMoveDirection.ShortestWay, - ) - ] - ) - ) - await self._motor_reset_encoder_position(axis, home_pos) - await self._motor_set_homed_status(axis, HomeStatus.Homed) - # --- I/O ----------------------------------------------------------------- async def read_input(self, node_id: int, input_num: int) -> bool: From 52820c3b4b109470b2f9b16bdb6972644f275284 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 23 Apr 2026 17:47:21 -0700 Subject: [PATCH 29/93] fold gripper geometry into KX2 IK/FK KX2 cartesian API now takes gripper_length (radial) and gripper_z_offset (vertical) kwargs on the KX2 device and forwards them into KX2ArmBackend. convert_cartesian_to_joint_position translates the incoming gripper pose to the wrist before the shoulder/Z/elbow math; convert_joint_position_to_cartesian applies the inverse so callers observe the gripper clamp point, symmetric with what they pass in. Defaults are 0.0, preserving today's behaviour for callers who haven't set the geometry. Also drops the broken, unused convert_joint_position_to_tool_coordinate / convert_tool_coordinate_to_joint_position pair (indexed GripperLocation like a tuple, no callers). Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/kx2.py | 12 +++- pylabrobot/paa/kx2/kx2_backend.py | 97 ++++++++++--------------------- 2 files changed, 41 insertions(+), 68 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2.py b/pylabrobot/paa/kx2/kx2.py index 804051ac549..6c2ac1fdb15 100644 --- a/pylabrobot/paa/kx2/kx2.py +++ b/pylabrobot/paa/kx2/kx2.py @@ -8,11 +8,19 @@ class KX2(Device): """PAA KX2 robotic plate handler.""" - def __init__(self) -> None: + def __init__( + self, + gripper_length: float = 0.0, + gripper_z_offset: float = 0.0, + ) -> None: driver = KX2Driver() super().__init__(driver=driver) self.driver: KX2Driver = driver - backend = KX2ArmBackend(driver=driver) + backend = KX2ArmBackend( + driver=driver, + gripper_length=gripper_length, + gripper_z_offset=gripper_z_offset, + ) self.reference = Resource(name="KX2", size_x=200, size_y=200, size_z=200) self.arm = OrientableArm(backend=backend, reference_resource=self.reference) self._capabilities = [self.arm] diff --git a/pylabrobot/paa/kx2/kx2_backend.py b/pylabrobot/paa/kx2/kx2_backend.py index bb787d19130..e3e905343ad 100644 --- a/pylabrobot/paa/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/kx2_backend.py @@ -4,7 +4,7 @@ import warnings from dataclasses import dataclass from enum import IntEnum -from typing import Dict, List, Optional, Sequence +from typing import Dict, List, Optional from pylabrobot.capabilities.arms.backend import ( CanFreedrive, @@ -77,9 +77,16 @@ class Axis(IntEnum): RAIL = 5 SERVO_GRIPPER = 6 - def __init__(self, driver: KX2Driver) -> None: + def __init__( + self, + driver: KX2Driver, + gripper_length: float = 0.0, + gripper_z_offset: float = 0.0, + ) -> None: super().__init__() self.driver = driver + self.gripper_length = float(gripper_length) + self.gripper_z_offset = float(gripper_z_offset) self.digital_input_assignment = {} # TODO: just cache? self.AnalogInputAssignment = {} @@ -1103,9 +1110,16 @@ def convert_cartesian_to_joint_position(self, pose: GripperPose) -> Dict["KX2Arm if pose.rotation.x != 0 or pose.rotation.y != 0: raise ValueError("Only Z rotation is supported for KX2") - joint_position: Dict[KX2ArmBackend.Axis, float] = {} + # Gripper -> wrist: the incoming pose describes the gripper clamp point; + # the joint-space math operates on the wrist axis. Rigid offset with the + # gripper length on the radial axis (governed by world rotation z) and the + # gripper z offset downward. + ang = math.radians(pose.rotation.z) + x = pose.location.x - self.gripper_length * math.sin(ang) + y = pose.location.y + self.gripper_length * math.cos(ang) + wrist_z = pose.location.z + self.gripper_z_offset - x, y = (pose.location.x), (pose.location.y) + joint_position: Dict[KX2ArmBackend.Axis, float] = {} # Shoulder axis shoulder = -math.degrees(math.atan2(x, y)) @@ -1115,7 +1129,7 @@ def convert_cartesian_to_joint_position(self, pose: GripperPose) -> Dict["KX2Arm joint_position[self.Axis.SHOULDER] = shoulder # Z axis - joint_position[self.Axis.Z] = pose.location.z + joint_position[self.Axis.Z] = wrist_z # Elbow axis elbow = ( @@ -1148,11 +1162,9 @@ def convert_joint_position_to_cartesian( sh_deg = joint_position[self.Axis.SHOULDER] sh = math.radians(sh_deg) - location = Coordinate( - x=(-(r) * math.sin(sh)), - y=((r) * math.cos(sh)), - z=(joint_position[self.Axis.Z]), - ) + wrist_x = -(r) * math.sin(sh) + wrist_y = (r) * math.cos(sh) + wrist_z = joint_position[self.Axis.Z] rotation_z = joint_position[self.Axis.WRIST] + sh_deg @@ -1162,66 +1174,19 @@ def convert_joint_position_to_cartesian( if rotation_z < -180.0: rotation_z += 360.0 + # Wrist -> gripper: inverse of the gripper -> wrist translation in + # convert_cartesian_to_joint_position so callers observe the gripper clamp + # point, symmetric with what they pass in. + ang = math.radians(rotation_z) + gripper_x = wrist_x + self.gripper_length * math.sin(ang) + gripper_y = wrist_y - self.gripper_length * math.cos(ang) + gripper_z = wrist_z - self.gripper_z_offset + return GripperPose( - location=location, + location=Coordinate(x=gripper_x, y=gripper_y, z=gripper_z), rotation=Rotation(z=rotation_z), ) - def convert_joint_position_to_tool_coordinate( - self, - joint_position: Dict[int, float], - ref_frame_rotate: float, - tool_offset: float, - ) -> List[float]: - coordinate = self.convert_joint_position_to_cartesian(joint_position) - tool_coord = [0.0] * (len(joint_position) - 1) - - if tool_offset != 0.0: - ang = math.radians(coordinate[3] + ref_frame_rotate) - dx = -tool_offset * math.sin(ang) - dy = tool_offset * math.cos(ang) - else: - dx = 0.0 - dy = 0.0 - - tool_coord[0] = coordinate[0] + dx - tool_coord[1] = coordinate[1] + dy - tool_coord[2] = coordinate[2] - tool_coord[3] = coordinate[3] + ref_frame_rotate - - if len(coordinate) > 4: - tool_coord[4] = coordinate[4] - - return tool_coord - - def convert_tool_coordinate_to_joint_position( - self, - tool_coordinate: Sequence[float], - ref_frame_rotate: float, - tool_offset: float, - ) -> Dict[int, float]: - coordinate = [0.0] * (len(tool_coordinate) + 1) - - if tool_offset != 0.0: - ang = math.radians(float(tool_coordinate[3]) - ref_frame_rotate) - dx = -tool_offset * math.sin(ang) - dy = tool_offset * math.cos(ang) - else: - dx = 0.0 - dy = 0.0 - - coordinate[0] = float(tool_coordinate[0]) - dx - coordinate[1] = float(tool_coordinate[1]) - dy - coordinate[2] = float(tool_coordinate[2]) - coordinate[3] = float(tool_coordinate[3]) - ref_frame_rotate - - if len(tool_coordinate) >= 5: - coordinate[4] = float(tool_coordinate[4]) - if len(tool_coordinate) >= 6: - coordinate[5] = float(tool_coordinate[5]) - - return self.convert_cartesian_to_joint_position(coordinate) - # -- capability interface (OrientableGripperArmBackend + HasJoints + CanFreedrive) -- @dataclass From 7fa2d2df83d18f2fc33144fac19e1acf028c64f4 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 23 Apr 2026 21:47:38 -0700 Subject: [PATCH 30/93] drop leading underscore on KX2Driver methods used by backend binary_interpreter, user_program_run, and wait_for_moves_done are called from KX2ArmBackend across a few dozen sites, so they're part of the driver's public surface despite the underscore. Rename to match. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/kx2_backend.py | 192 +++++++++++++++--------------- pylabrobot/paa/kx2/kx2_driver.py | 50 ++++---- 2 files changed, 121 insertions(+), 121 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_backend.py b/pylabrobot/paa/kx2/kx2_backend.py index e3e905343ad..474c4382a1f 100644 --- a/pylabrobot/paa/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/kx2_backend.py @@ -150,7 +150,7 @@ async def get_estop_state(self) -> bool: Reads the shoulder drive's SR (status register) via the binary interpreter. Bits 14/15 encode the stop/safety state. """ - r = int(await self.driver._binary_interpreter( + r = int(await self.driver.binary_interpreter( node_id=int(self.Axis.SHOULDER), cmd="SR", cmd_index=1, @@ -170,10 +170,10 @@ async def _motor_set_homed_status(self, axis: int, status: HomeStatus) -> None: val = "2" else: val = "0" - await self.driver._binary_interpreter(int(axis), "UI", 3, CmdType.ValSet, val) + await self.driver.binary_interpreter(int(axis), "UI", 3, CmdType.ValSet, val) async def motor_get_homed_status(self, node_id: int) -> HomeStatus: - left = await self.driver._binary_interpreter(int(node_id), "UI", 3, CmdType.ValQuery) + left = await self.driver.binary_interpreter(int(node_id), "UI", 3, CmdType.ValQuery) if left == 1: return HomeStatus.Homed if left == 2: @@ -181,12 +181,12 @@ async def motor_get_homed_status(self, node_id: int) -> HomeStatus: return HomeStatus.NotHomed async def _motor_reset_encoder_position(self, axis: int, position: float) -> None: - await self.driver._binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") - await self.driver._binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "0") - await self.driver._binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") - await self.driver._binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") - await self.driver._binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, str(position)) - await self.driver._binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") + await self.driver.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") + await self.driver.binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "0") + await self.driver.binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") + await self.driver.binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") + await self.driver.binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, str(position)) + await self.driver.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") async def _motor_hard_stop_search( self, @@ -198,31 +198,31 @@ async def _motor_hard_stop_search( timeout: float, ) -> None: nid = int(axis) - await self.driver._binary_interpreter(nid, "ER", 3, CmdType.ValSet, str(max_pe * 10)) - await self.driver._binary_interpreter(nid, "AC", 0, CmdType.ValSet, str(srch_acc)) - await self.driver._binary_interpreter(nid, "DC", 0, CmdType.ValSet, str(srch_acc)) + await self.driver.binary_interpreter(nid, "ER", 3, CmdType.ValSet, str(max_pe * 10)) + await self.driver.binary_interpreter(nid, "AC", 0, CmdType.ValSet, str(srch_acc)) + await self.driver.binary_interpreter(nid, "DC", 0, CmdType.ValSet, str(srch_acc)) for i in [3, 4, 5, 2]: - await self.driver._binary_interpreter(nid, "HM", i, CmdType.ValSet, "0") - await self.driver._binary_interpreter(nid, "JV", 0, CmdType.ValSet, str(srch_vel)) + await self.driver.binary_interpreter(nid, "HM", i, CmdType.ValSet, "0") + await self.driver.binary_interpreter(nid, "JV", 0, CmdType.ValSet, str(srch_vel)) try: params = [str(int(hs_pe)), str(int(timeout * 1000))] - last_line = await self.driver._user_program_run( + last_line = await self.driver.user_program_run( nid, "Home", params, int(timeout), True ) if last_line in [1, 2, 3]: raise RuntimeError(f"Homing Script Error {34 + last_line}") curr_pos = await self.driver.motor_get_current_position(nid) - await self.driver._binary_interpreter(nid, "PA", 0, CmdType.ValSet, str(curr_pos)) - await self.driver._binary_interpreter(nid, "SP", 0, CmdType.ValSet, str(srch_vel)) - await self.driver._binary_interpreter(nid, "AC", 0, CmdType.ValSet, str(srch_acc)) - await self.driver._binary_interpreter(nid, "DC", 0, CmdType.ValSet, str(srch_acc)) + await self.driver.binary_interpreter(nid, "PA", 0, CmdType.ValSet, str(curr_pos)) + await self.driver.binary_interpreter(nid, "SP", 0, CmdType.ValSet, str(srch_vel)) + await self.driver.binary_interpreter(nid, "AC", 0, CmdType.ValSet, str(srch_acc)) + await self.driver.binary_interpreter(nid, "DC", 0, CmdType.ValSet, str(srch_acc)) finally: await asyncio.sleep(0.3) - await self.driver._binary_interpreter(nid, "BG", 0, CmdType.Execute, value="0") + await self.driver.binary_interpreter(nid, "BG", 0, CmdType.Execute, value="0") await asyncio.sleep(0.3) - await self.driver._binary_interpreter(nid, "ER", 3, CmdType.ValSet, str(int(max_pe))) + await self.driver.binary_interpreter(nid, "ER", 3, CmdType.ValSet, str(int(max_pe))) async def _motor_index_search( self, @@ -233,32 +233,32 @@ async def _motor_index_search( timeout: float, ) -> tuple: nid = int(axis) - await self.driver._binary_interpreter(nid, "HM", 1, CmdType.ValSet, "0") + await self.driver.binary_interpreter(nid, "HM", 1, CmdType.ValSet, "0") - rev = await self.driver._binary_interpreter(nid, "CA", 18, CmdType.ValQuery) + rev = await self.driver.binary_interpreter(nid, "CA", 18, CmdType.ValQuery) one_revolution = int(float(rev)) if not positive_direction: one_revolution *= -1 - await self.driver._binary_interpreter(nid, "PR", 1, CmdType.ValSet, str(one_revolution)) - await self.driver._binary_interpreter(nid, "SP", 0, CmdType.ValSet, str(srch_vel)) - await self.driver._binary_interpreter(nid, "AC", 0, CmdType.ValSet, str(srch_acc)) - await self.driver._binary_interpreter(nid, "DC", 0, CmdType.ValSet, str(srch_acc)) + await self.driver.binary_interpreter(nid, "PR", 1, CmdType.ValSet, str(one_revolution)) + await self.driver.binary_interpreter(nid, "SP", 0, CmdType.ValSet, str(srch_vel)) + await self.driver.binary_interpreter(nid, "AC", 0, CmdType.ValSet, str(srch_acc)) + await self.driver.binary_interpreter(nid, "DC", 0, CmdType.ValSet, str(srch_acc)) - await self.driver._binary_interpreter(nid, "HM", 3, CmdType.ValSet, "3") # index only - await self.driver._binary_interpreter(nid, "HM", 4, CmdType.ValSet, "0") - await self.driver._binary_interpreter(nid, "HM", 5, CmdType.ValSet, "0") - await self.driver._binary_interpreter(nid, "HM", 2, CmdType.ValSet, "0") - await self.driver._binary_interpreter(nid, "HM", 1, CmdType.ValSet, "1") # arm + await self.driver.binary_interpreter(nid, "HM", 3, CmdType.ValSet, "3") # index only + await self.driver.binary_interpreter(nid, "HM", 4, CmdType.ValSet, "0") + await self.driver.binary_interpreter(nid, "HM", 5, CmdType.ValSet, "0") + await self.driver.binary_interpreter(nid, "HM", 2, CmdType.ValSet, "0") + await self.driver.binary_interpreter(nid, "HM", 1, CmdType.ValSet, "1") # arm - await self.driver._binary_interpreter(nid, "BG", 0, CmdType.Execute) - await self.driver._wait_for_moves_done([nid], timeout) + await self.driver.binary_interpreter(nid, "BG", 0, CmdType.Execute) + await self.driver.wait_for_moves_done([nid], timeout) - left = await self.driver._binary_interpreter(nid, "HM", 1, CmdType.ValQuery) + left = await self.driver.binary_interpreter(nid, "HM", 1, CmdType.ValQuery) if left != 0: raise RuntimeError("Homing Failure: Failed to finish index pulse search.") - cap = await self.driver._binary_interpreter(nid, "HM", 7, CmdType.ValQuery) + cap = await self.driver.binary_interpreter(nid, "HM", 7, CmdType.ValQuery) captured_position = int(float(cap)) return one_revolution, captured_position @@ -278,7 +278,7 @@ async def home_motor( ) -> None: nid = int(axis) - left = await self.driver._binary_interpreter(nid, "CA", 41, CmdType.ValQuery) + left = await self.driver.binary_interpreter(nid, "CA", 41, CmdType.ValQuery) if left == 24: raise RuntimeError("Error 43") @@ -344,7 +344,7 @@ async def servo_gripper_initialize(self): await self.servo_gripper_close() async def servo_gripper_home(self) -> None: - await self.driver._binary_interpreter( + await self.driver.binary_interpreter( node_id=int(self.Axis.SERVO_GRIPPER), cmd="PL", cmd_index=1, @@ -353,7 +353,7 @@ async def servo_gripper_home(self) -> None: val_type=ValType.Float, ) - await self.driver._binary_interpreter( + await self.driver.binary_interpreter( node_id=int(self.Axis.SERVO_GRIPPER), cmd="CL", cmd_index=1, @@ -390,17 +390,17 @@ async def servo_gripper_set_default_gripping_force(self, max_force_percent: int) axis = self.Axis.SERVO_GRIPPER # 1) PL with unscaled peak current - await self.driver._binary_interpreter( + await self.driver.binary_interpreter( int(axis), "PL", 1, CmdType.ValSet, str(self.servo_gripper_peak_current), val_type=ValType.Float ) # 2) CL with scaled continuous current - await self.driver._binary_interpreter( + await self.driver.binary_interpreter( int(axis), "CL", 1, CmdType.ValSet, str(cont_current), val_type=ValType.Float ) # 3) PL with scaled peak current - await self.driver._binary_interpreter( + await self.driver.binary_interpreter( int(axis), "PL", 1, CmdType.ValSet, str(peak_current), val_type=ValType.Float ) @@ -408,14 +408,14 @@ async def servo_gripper_set_default_gripping_force(self, max_force_percent: int) async def get_servo_gripper_max_force(self) -> float: """Return current gripping force as percentage of max (0-1).""" - cl = await self.driver._binary_interpreter( + cl = await self.driver.binary_interpreter( node_id=int(self.Axis.SERVO_GRIPPER), cmd="CL", cmd_index=1, cmd_type=CmdType.ValQuery, ) - iq = await self.driver._binary_interpreter( + iq = await self.driver.binary_interpreter( node_id=int(self.Axis.SERVO_GRIPPER), cmd="IQ", cmd_index=0, @@ -429,7 +429,7 @@ async def get_servo_gripper_max_force(self) -> float: async def check_plate_gripped(self, num_attempts: int = 5) -> None: for _ in range(num_attempts): - motor_status = await self.driver._binary_interpreter( + motor_status = await self.driver.binary_interpreter( node_id=int(self.Axis.SERVO_GRIPPER), cmd="MS", cmd_index=1, @@ -498,7 +498,7 @@ async def drive_set_move_count_parameters( last_maintenance_performed_date_rail: int, ) -> None: # MoveCount -> Z axis, UI index 22 - await self.driver._binary_interpreter( + await self.driver.binary_interpreter( node_id=int(self.Axis.Z), cmd="UI", cmd_index=22, @@ -516,7 +516,7 @@ async def drive_set_move_count_parameters( pairs = zip(self.node_id_list, travel) for node_id, dist in pairs: - await self.driver._binary_interpreter( + await self.driver.binary_interpreter( node_id=int(node_id), cmd="UF", cmd_index=5, @@ -527,7 +527,7 @@ async def drive_set_move_count_parameters( ) # LastMaintenancePerformed -> Z axis, UF index 6 - await self.driver._binary_interpreter( + await self.driver.binary_interpreter( node_id=int(self.Axis.Z), cmd="UF", cmd_index=6, @@ -538,7 +538,7 @@ async def drive_set_move_count_parameters( ) # MaintenanceRequired -> Z axis, UI index 23 - await self.driver._binary_interpreter( + await self.driver.binary_interpreter( node_id=int(self.Axis.Z), cmd="UI", cmd_index=23, @@ -549,7 +549,7 @@ async def drive_set_move_count_parameters( ) # LastMaintenancePerformedDate -> Z axis, UI index 21 - await self.driver._binary_interpreter( + await self.driver.binary_interpreter( node_id=int(self.Axis.Z), cmd="UI", cmd_index=21, @@ -561,7 +561,7 @@ async def drive_set_move_count_parameters( # Rail (if present) if self.robot_on_rail: - await self.driver._binary_interpreter( + await self.driver.binary_interpreter( node_id=int(self.Axis.RAIL), cmd="UF", cmd_index=6, @@ -571,7 +571,7 @@ async def drive_set_move_count_parameters( low_priority=True, ) - await self.driver._binary_interpreter( + await self.driver.binary_interpreter( node_id=int(self.Axis.RAIL), cmd="UI", cmd_index=23, @@ -581,7 +581,7 @@ async def drive_set_move_count_parameters( low_priority=False, ) - await self.driver._binary_interpreter( + await self.driver.binary_interpreter( node_id=int(self.Axis.RAIL), cmd="UI", cmd_index=21, @@ -607,7 +607,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # Pass 1: identify axes by UI[4] uis = set() for node in nodes: - if node == await self.driver._binary_interpreter(int(node), "UI", 4, CmdType.ValQuery, val_type=ValType.Int): + if node == await self.driver.binary_interpreter(int(node), "UI", 4, CmdType.ValQuery, val_type=ValType.Int): uis.add(node) for required_axis in MOTION_AXES: if required_axis.value not in uis: @@ -624,7 +624,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # UI[5..10] digital inputs for ui_idx in range(5, 11): - ret = await self.driver._binary_interpreter(int(axis), "UI", ui_idx, CmdType.ValQuery, val_type=ValType.Int) + ret = await self.driver.binary_interpreter(int(axis), "UI", ui_idx, CmdType.ValQuery, val_type=ValType.Int) ch = (ui_idx - 5) + 1 if ret == 101: set2d(self.digital_input_assignment, axis, ch, "GripperSensor") @@ -644,7 +644,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # UI[11..12] analog inputs for ui_idx in range(11, 13): - ret = await self.driver._binary_interpreter(int(axis), "UI", ui_idx, CmdType.ValQuery, val_type=ValType.Int) + ret = await self.driver.binary_interpreter(int(axis), "UI", ui_idx, CmdType.ValQuery, val_type=ValType.Int) ch = (ui_idx - 11) + 1 set2d( self.AnalogInputAssignment, @@ -655,7 +655,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # UI[13..16] outputs for ui_idx in range(13, 17): - ret = await self.driver._binary_interpreter(int(axis), "UI", ui_idx, CmdType.ValQuery, val_type=ValType.Int) + ret = await self.driver.binary_interpreter(int(axis), "UI", ui_idx, CmdType.ValQuery, val_type=ValType.Int) ch = (ui_idx - 13) + 1 if ret == 101: @@ -687,14 +687,14 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: ) # UI[24] drive serial number - ret = await self.driver._binary_interpreter(int(axis), "UI", 24, CmdType.ValQuery, val_type=ValType.Int) + ret = await self.driver.binary_interpreter(int(axis), "UI", 24, CmdType.ValQuery, val_type=ValType.Int) if _is_number(ret): # self.drive_serial_number[axis] = int(ret) pass # UF[1], UF[2] conversion factor - uf1 = await self.driver._binary_interpreter(int(axis), "UF", 1, CmdType.ValQuery, val_type=ValType.Float) - uf2 = await self.driver._binary_interpreter(int(axis), "UF", 2, CmdType.ValQuery, val_type=ValType.Float) + uf1 = await self.driver.binary_interpreter(int(axis), "UF", 1, CmdType.ValQuery, val_type=ValType.Float) + uf2 = await self.driver.binary_interpreter(int(axis), "UF", 2, CmdType.ValQuery, val_type=ValType.Float) if ( not (_is_number(uf1) and _is_number(uf2)) or _to_float(uf1) == 0.0 or _to_float(uf2) == 0.0 ): @@ -702,12 +702,12 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: self.motor_conversion_factor_ax[axis] = _to_float(uf1) / _to_float(uf2) # XM / travel - xm1 = await self.driver._binary_interpreter(int(axis), "XM", 1, CmdType.ValQuery, val_type=ValType.Int) - xm2 = await self.driver._binary_interpreter(int(axis), "XM", 2, CmdType.ValQuery, val_type=ValType.Int) - uf3 = await self.driver._binary_interpreter(int(axis), "UF", 3, CmdType.ValQuery, val_type=ValType.Float) - uf4 = await self.driver._binary_interpreter(int(axis), "UF", 4, CmdType.ValQuery, val_type=ValType.Float) - vh3 = await self.driver._binary_interpreter(int(axis), "VH", 3, CmdType.ValQuery, val_type=ValType.Int) - vl3 = await self.driver._binary_interpreter(int(axis), "VL", 3, CmdType.ValQuery, val_type=ValType.Int) + xm1 = await self.driver.binary_interpreter(int(axis), "XM", 1, CmdType.ValQuery, val_type=ValType.Int) + xm2 = await self.driver.binary_interpreter(int(axis), "XM", 2, CmdType.ValQuery, val_type=ValType.Int) + uf3 = await self.driver.binary_interpreter(int(axis), "UF", 3, CmdType.ValQuery, val_type=ValType.Float) + uf4 = await self.driver.binary_interpreter(int(axis), "UF", 4, CmdType.ValQuery, val_type=ValType.Float) + vh3 = await self.driver.binary_interpreter(int(axis), "VH", 3, CmdType.ValQuery, val_type=ValType.Int) + vl3 = await self.driver.binary_interpreter(int(axis), "VL", 3, CmdType.ValQuery, val_type=ValType.Int) self.max_travel_ax[axis] = _to_float(uf3) self.min_travel_ax[axis] = _to_float(uf4) @@ -730,12 +730,12 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: ) # Encoder socket/type - ca45 = await self.driver._binary_interpreter(int(axis), "CA", 45, CmdType.ValQuery, val_type=ValType.Int) + ca45 = await self.driver.binary_interpreter(int(axis), "CA", 45, CmdType.ValQuery, val_type=ValType.Int) ca45v = _to_float(ca45, 0.0) if (not _is_number(ca45)) or not (0.0 < ca45v <= 4.0): raise CanError(f"Invalid encoder socket number received from axis {axis}. CA[45]={ca45}") - enc_type = await self.driver._binary_interpreter( + enc_type = await self.driver.binary_interpreter( int(axis), "CA", int(round(40.0 + ca45v)), CmdType.ValQuery, val_type=ValType.Int ) if enc_type in (1, 2): @@ -747,60 +747,60 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: f"Unsupported encoder type specified for axis {axis}. CA[4{ca45}]={enc_type}" ) - ca46 = await self.driver._binary_interpreter(int(axis), "CA", 46, CmdType.ValQuery, val_type=ValType.Int) + ca46 = await self.driver.binary_interpreter(int(axis), "CA", 46, CmdType.ValQuery, val_type=ValType.Int) if ca45 == ca46: num3 = 1.0 else: - ff3 = await self.driver._binary_interpreter(int(axis), "FF", 3, CmdType.ValQuery, val_type=ValType.Float) + ff3 = await self.driver.binary_interpreter(int(axis), "FF", 3, CmdType.ValQuery, val_type=ValType.Float) num3 = _to_float(ff3, 1.0) denom = self.motor_conversion_factor_ax[axis] * num3 # or 1.0 - sp2 = await self.driver._binary_interpreter(int(axis), "SP", 2, CmdType.ValQuery, val_type=ValType.Int) + sp2 = await self.driver.binary_interpreter(int(axis), "SP", 2, CmdType.ValQuery, val_type=ValType.Int) if sp2 == 100000: - vh2 = await self.driver._binary_interpreter(int(axis), "VH", 2, CmdType.ValQuery, val_type=ValType.Int) + vh2 = await self.driver.binary_interpreter(int(axis), "VH", 2, CmdType.ValQuery, val_type=ValType.Int) self.max_vel_ax[axis] = _to_float(vh2) / 1.01 / denom else: self.max_vel_ax[axis] = _to_float(sp2) / denom - sd0 = await self.driver._binary_interpreter(int(axis), "SD", 0, CmdType.ValQuery, val_type=ValType.Int) + sd0 = await self.driver.binary_interpreter(int(axis), "SD", 0, CmdType.ValQuery, val_type=ValType.Int) self.max_accel_ax[axis] = _to_float(sd0) / 1.01 / denom # Robot-level params from shoulder_ax shoulder = self.Axis.SHOULDER self.base_to_gripper_clearance_z = _to_float( - await self.driver._binary_interpreter(int(shoulder), "UF", 6, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(shoulder), "UF", 6, CmdType.ValQuery, val_type=ValType.Float) ) self.base_to_gripper_clearance_arm = _to_float( - await self.driver._binary_interpreter(int(shoulder), "UF", 7, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(shoulder), "UF", 7, CmdType.ValQuery, val_type=ValType.Float) ) self.wrist_offset = _to_float( - await self.driver._binary_interpreter(int(shoulder), "UF", 8, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(shoulder), "UF", 8, CmdType.ValQuery, val_type=ValType.Float) ) self.elbow_offset = _to_float( - await self.driver._binary_interpreter(int(shoulder), "UF", 9, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(shoulder), "UF", 9, CmdType.ValQuery, val_type=ValType.Float) ) self.elbow_zero_offset = _to_float( - await self.driver._binary_interpreter(int(shoulder), "UF", 10, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(shoulder), "UF", 10, CmdType.ValQuery, val_type=ValType.Float) ) self.MaxLinearVelMMPerSec = _to_float( - await self.driver._binary_interpreter(int(shoulder), "UF", 11, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(shoulder), "UF", 11, CmdType.ValQuery, val_type=ValType.Float) ) self.MaxLinearAccelMMPerSec2 = _to_float( - await self.driver._binary_interpreter(int(shoulder), "UF", 12, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(shoulder), "UF", 12, CmdType.ValQuery, val_type=ValType.Float) ) self.MaxLinearJerkMMPerSec3 = _to_float( - await self.driver._binary_interpreter(int(shoulder), "UF", 13, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(shoulder), "UF", 13, CmdType.ValQuery, val_type=ValType.Float) ) self.MaxRotaryVelDegPerSec = _to_float( - await self.driver._binary_interpreter(int(shoulder), "UF", 14, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(shoulder), "UF", 14, CmdType.ValQuery, val_type=ValType.Float) ) self.MaxRotaryAccelDegPerSec2 = _to_float( - await self.driver._binary_interpreter(int(shoulder), "UF", 15, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(shoulder), "UF", 15, CmdType.ValQuery, val_type=ValType.Float) ) - ui17 = await self.driver._binary_interpreter(int(shoulder), "UI", 17, CmdType.ValQuery, val_type=ValType.Int) + ui17 = await self.driver.binary_interpreter(int(shoulder), "UI", 17, CmdType.ValQuery, val_type=ValType.Int) self.pvt_time_interval_msec = ( 25 if (not _is_number(ui17) or _to_float(ui17) <= 0.0 or _to_float(ui17) > 255.0) @@ -810,40 +810,40 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # Servo gripper params (only if present) sg = self.Axis.SERVO_GRIPPER self.servo_gripper_home_pos = int( - await self.driver._binary_interpreter(int(sg), "UF", 6, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(sg), "UF", 6, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_search_vel = int( - await self.driver._binary_interpreter(int(sg), "UF", 7, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(sg), "UF", 7, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_search_accel = int( - await self.driver._binary_interpreter(int(sg), "UF", 8, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(sg), "UF", 8, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_default_position_error = int( - await self.driver._binary_interpreter(int(sg), "UF", 9, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(sg), "UF", 9, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_hard_stop_position_error = int( - await self.driver._binary_interpreter(int(sg), "UF", 10, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(sg), "UF", 10, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_hard_stop_offset = int( - await self.driver._binary_interpreter(int(sg), "UF", 11, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(sg), "UF", 11, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_index_offset = int( - await self.driver._binary_interpreter(int(sg), "UF", 12, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(sg), "UF", 12, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_offset_vel = int( - await self.driver._binary_interpreter(int(sg), "UF", 13, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(sg), "UF", 13, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_offset_accel = int( - await self.driver._binary_interpreter(int(sg), "UF", 14, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(sg), "UF", 14, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_home_timeout_msec = int( - await self.driver._binary_interpreter(int(sg), "UF", 15, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(sg), "UF", 15, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_continuous_current = _to_float( - await self.driver._binary_interpreter(int(sg), "UF", 16, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(sg), "UF", 16, CmdType.ValQuery, val_type=ValType.Float) ) self.servo_gripper_peak_current = _to_float( - await self.driver._binary_interpreter(int(sg), "UF", 17, CmdType.ValQuery, val_type=ValType.Float) + await self.driver.binary_interpreter(int(sg), "UF", 17, CmdType.ValQuery, val_type=ValType.Float) ) def convert_elbow_position_to_angle(self, elbow_pos: float) -> float: diff --git a/pylabrobot/paa/kx2/kx2_driver.py b/pylabrobot/paa/kx2/kx2_driver.py index d179c969a4f..9d0c0fde9ba 100644 --- a/pylabrobot/paa/kx2/kx2_driver.py +++ b/pylabrobot/paa/kx2/kx2_driver.py @@ -546,7 +546,7 @@ def _dispatch_bi_response(self, node_id: int, data: bytes) -> None: if fut is not None and not fut.done(): fut.set_result(value_str) - async def _binary_interpreter( + async def binary_interpreter( self, node_id: int, cmd: str, @@ -720,15 +720,15 @@ async def request_drive_version(self, node_id: int) -> str: # --- DS402 / motor control ---------------------------------------------- async def motor_emergency_stop(self, node_id: int) -> None: - await self._binary_interpreter(node_id, "MO", 0, CmdType.ValSet, "0") + await self.binary_interpreter(node_id, "MO", 0, CmdType.ValSet, "0") async def motor_get_current_position(self, node_id: int, pu: bool = False) -> int: cmd = "PU" if pu else "PX" - val_str = await self._binary_interpreter(int(node_id), cmd, 0, CmdType.ValQuery) + val_str = await self.binary_interpreter(int(node_id), cmd, 0, CmdType.ValQuery) return int(round(float(val_str))) async def motor_get_motion_status(self, node_id: int) -> int: - val = await self._binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) + val = await self.binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) return int(round(float(val))) async def _motor_set_move_direction( @@ -749,11 +749,11 @@ async def _motor_set_move_direction( ) async def motor_check_if_move_done(self, node_id: int) -> bool: - ms_val = await self._binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) + ms_val = await self.binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) if ms_val == 0: return True if ms_val == 1: - mo_val = await self._binary_interpreter(node_id, "MO", 0, CmdType.ValQuery) + mo_val = await self.binary_interpreter(node_id, "MO", 0, CmdType.ValQuery) if mo_val == 1: return True fault = await self.motor_get_fault(node_id) @@ -765,7 +765,7 @@ async def motor_check_if_move_done(self, node_id: int) -> bool: return False async def motor_get_fault(self, node_id: int) -> Optional[str]: - val = await self._binary_interpreter(int(node_id), "MF", 0, CmdType.ValQuery) + val = await self.binary_interpreter(int(node_id), "MF", 0, CmdType.ValQuery) if val == 0: return None assert isinstance(val, int) @@ -829,7 +829,7 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N max_attempts = 5 for attempt in range(1, max_attempts + 1): if not use_ds402: - await self._binary_interpreter(node_id, "MO", 0, CmdType.ValSet, "1") + await self.binary_interpreter(node_id, "MO", 0, CmdType.ValSet, "1") else: # DS402 enable sequence: Fault -> Shutdown -> Switched On -> Op Enabled. # Matches the C# reference (clscanmotor.cs:4495-4509): back-to-back @@ -837,7 +837,7 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N for cw in (0, 128, 6, 7, 15): await self._control_word_set(node_id=node_id, value=cw) await asyncio.sleep(0.1) - left = await self._binary_interpreter( + left = await self.binary_interpreter( node_id=node_id, cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery ) if left == 1: @@ -851,7 +851,7 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N else: if not use_ds402: try: - await self._binary_interpreter( + await self.binary_interpreter( node_id=node_id, cmd="MO", cmd_index=0, cmd_type=CmdType.ValSet, value="0" ) except Exception: @@ -862,7 +862,7 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N await self._control_word_set(node_id=node_id, value=7) await self._control_word_set(node_id=node_id, value=6) await asyncio.sleep(0.1) - left = await self._binary_interpreter( + left = await self.binary_interpreter( node_id=node_id, cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery ) if left != 0: @@ -890,7 +890,7 @@ async def _pvt_select_mode(self, enable: bool) -> None: await self._can_sdo_download(nid, 0x60, 0x60, 0x00, [1]) self._pvt_mode = False - async def _wait_for_moves_done( + async def wait_for_moves_done( self, node_ids: List[int], timeout: float ) -> None: # Poll MS every 30ms after a 50ms warm-up. The warm-up avoids reading @@ -950,9 +950,9 @@ async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: node_ids = [int(move.node_id) for move in plan.moves] await self._motors_move_start(node_ids) - await self._wait_for_moves_done(node_ids, timeout=plan.move_time + 2) + await self.wait_for_moves_done(node_ids, timeout=plan.move_time + 2) - async def _user_program_run( + async def user_program_run( self, node_id: int, user_function: str, @@ -966,15 +966,15 @@ async def _user_program_run( raise ValueError("node_id must be in [0, 255]") node_id = int(node_id) - ps = int(await self._binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) if ps == -2: raise CanError(f"Node {node_id}: controller reported PS=-2 (not ready / unavailable)") if ps != -1: - await self._binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="0") + await self.binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="0") t0 = time.monotonic() while (time.monotonic() - t0) < 3.0: - ps = int(await self._binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) if ps == -1: break await asyncio.sleep(0.01) @@ -987,10 +987,10 @@ async def _user_program_run( if parts: arg_str = f"({','.join(parts)})" - await self._binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="1") + await self.binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="1") cmd = f"XQ##{user_function}{arg_str}" - logger.debug("_user_program_run: %s", cmd) + logger.debug("user_program_run: %s", cmd) await self._os_interpreter(node_id, cmd, query=False) last_line_completed = 0 @@ -999,11 +999,11 @@ async def _user_program_run( ps = 1 ui1 = 1 while ps == 1 and ui1 == 1 and (time.monotonic() - t0) < float(timeout_sec): - ps = int(await self._binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) - ui1 = int(await self._binary_interpreter(node_id, "UI", 1, CmdType.ValQuery)) + ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + ui1 = int(await self.binary_interpreter(node_id, "UI", 1, CmdType.ValQuery)) await asyncio.sleep(0.01) - expr_raw = await self._binary_interpreter(node_id, "UI", 2, CmdType.ValQuery) + expr_raw = await self.binary_interpreter(node_id, "UI", 2, CmdType.ValQuery) try: last_line_completed = int(str(expr_raw).strip()) except Exception: @@ -1025,15 +1025,15 @@ async def _user_program_run( # --- I/O ----------------------------------------------------------------- async def read_input(self, node_id: int, input_num: int) -> bool: - left = await self._binary_interpreter(node_id, "IB", input_num, CmdType.ValQuery) + left = await self.binary_interpreter(node_id, "IB", input_num, CmdType.ValQuery) return left == 1 async def _read_output(self, node_id: int, output_num: int) -> bool: - expression = await self._binary_interpreter(node_id, "OP", 0, CmdType.ValQuery) + expression = await self.binary_interpreter(node_id, "OP", 0, CmdType.ValQuery) val = int(expression) mask = 1 << (output_num - 1) return (val & mask) == mask async def _set_output(self, node_id: int, output_num: int, state: bool) -> Union[str, float]: val = "1" if state else "0" - return await self._binary_interpreter(node_id, "OB", output_num, CmdType.ValSet, val) + return await self.binary_interpreter(node_id, "OB", output_num, CmdType.ValSet, val) From 663ce42d5d457c0201df0f0f04d7d2c2d93ea13a Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 23 Apr 2026 22:00:30 -0700 Subject: [PATCH 31/93] fix KX2 dead-error paths and untruncate grip force MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - user_program_run now returns last_line_completed so _motor_hard_stop_search can actually map script-level failures (last_line in 1/2/3) to homing error codes 35/36/37. Previously returned 0 unconditionally, so that branch was dead. - reorder the timeout and abnormal-exit checks in user_program_run so timeout surfaces with the specific "timeout waiting for ..." message; the prior order let the generic ui1!=0 branch swallow it. - await motor_get_fault and asyncio.sleep in check_plate_gripped — the missing awaits made the MS==2 branch report "" as the fault and skipped the 50ms inter-attempt backoff. - query CL/IQ as Float in get_servo_gripper_max_force; default Int was truncating the float response and corrupting the grip-force gate. - collapse _motor_set_move_direction if-chain into arithmetic. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/kx2_backend.py | 6 ++++-- pylabrobot/paa/kx2/kx2_driver.py | 26 ++++++++++---------------- 2 files changed, 14 insertions(+), 18 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_backend.py b/pylabrobot/paa/kx2/kx2_backend.py index 474c4382a1f..7848c995fd5 100644 --- a/pylabrobot/paa/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/kx2_backend.py @@ -413,6 +413,7 @@ async def get_servo_gripper_max_force(self) -> float: cmd="CL", cmd_index=1, cmd_type=CmdType.ValQuery, + val_type=ValType.Float, ) iq = await self.driver.binary_interpreter( @@ -420,6 +421,7 @@ async def get_servo_gripper_max_force(self) -> float: cmd="IQ", cmd_index=0, cmd_type=CmdType.ValQuery, + val_type=ValType.Float, ) if cl == 0: @@ -456,14 +458,14 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: return elif motor_status == 2: - motor_fault = self.driver.motor_get_fault(self.Axis.SERVO_GRIPPER) + motor_fault = await self.driver.motor_get_fault(int(self.Axis.SERVO_GRIPPER)) if motor_fault is None: raise RuntimeError("Error querying whether plate is gripped. Error querying motor fault.") raise RuntimeError( f"Servo Gripper may not have gripped the plate correctly. Motor fault: '{motor_fault}'" ) - asyncio.sleep(0.05) + await asyncio.sleep(0.05) raise RuntimeError( f"Servo Gripper was unable to confirm that the plate is gripped after {num_attempts} attempts." diff --git a/pylabrobot/paa/kx2/kx2_driver.py b/pylabrobot/paa/kx2/kx2_driver.py index 9d0c0fde9ba..97d89b36d83 100644 --- a/pylabrobot/paa/kx2/kx2_driver.py +++ b/pylabrobot/paa/kx2/kx2_driver.py @@ -734,18 +734,12 @@ async def motor_get_motion_status(self, node_id: int) -> int: async def _motor_set_move_direction( self, node_id: int, direction: JointMoveDirection ) -> None: - # 0=Normal, 1=Clockwise, 2=Counterclockwise, 3=ShortestWay. - dir_int = int(direction) - if dir_int == 1: - val_str = "65" - elif dir_int == 2: - val_str = "129" - elif dir_int == 3: - val_str = "193" - else: - val_str = "1" + # Elmo modulo mode register: bit0 enables modulo; bits6..7 encode the + # direction (0=Normal, 1=CW, 2=CCW, 3=Shortest). Packs to 1 + 64*direction + # = 1/65/129/193. + val = 1 + 64 * int(direction) await self._can_sdo_download_elmo_object( - node_id, 24818, 0, val_str, ElmoObjectDataType.UNSIGNED16 + node_id, 24818, 0, str(val), ElmoObjectDataType.UNSIGNED16 ) async def motor_check_if_move_done(self, node_id: int) -> bool: @@ -1009,18 +1003,18 @@ async def user_program_run( except Exception: last_line_completed = 0 - if ui1 != 0: + if ps == 1 and ui1 == 1: raise CanError( - f"Node {node_id}: user program ended with UI[1]={ui1} (expected 0), " + f"Node {node_id}: timeout waiting for '{user_function}' after {timeout_sec}s, " f"last_line={last_line_completed}" ) - if ps == 1 and ui1 == 1: + if ui1 != 0: raise CanError( - f"Node {node_id}: timeout waiting for '{user_function}' after {timeout_sec}s, " + f"Node {node_id}: user program ended with UI[1]={ui1} (expected 0), " f"last_line={last_line_completed}" ) - return 0 + return last_line_completed # --- I/O ----------------------------------------------------------------- From 3a2e586ebab099220f8171067054b327139a10aa Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 23 Apr 2026 22:38:06 -0700 Subject: [PATCH 32/93] =?UTF-8?q?native=20int/float=20through=20KX2=20driv?= =?UTF-8?q?er=20=E2=80=94=20no=20more=20stringified=20numbers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Callers used to build strings just to have the driver parse them back. Replace the single stringly-typed binary_interpreter with four shape- specific methods whose arguments and return types are plain int/float: query_int(node_id, cmd, cmd_index) -> int query_float(node_id, cmd, cmd_index) -> float write(node_id, cmd, cmd_index, value) # int vs float framing via # isinstance(value, float) execute(node_id, cmd, cmd_index) _dispatch_bi_response now stores the decoded int/float on the future directly (was str(val)), so callers receive the native type Elmo sent without a stringify/reparse round-trip. CmdType and ValType enums go away; the method identity carries the command kind and the value's type carries the wire-framing choice. Also clean up three adjacent pockets of the same str↔int pattern: - Delete _is_number/_to_float helpers in the backend; they only existed to reparse strings that are now native numbers. ~16 call sites in drive_get_parameters collapse to plain comparisons. - _can_sdo_download_elmo_object takes data: Union[int, float] with a table-driven byte-width/signedness map; unused _can_sdo_upload_elmo_object deleted. 11 call sites no longer build strings. - user_program_run takes params: Optional[List[Union[int, float]]]; the caller no longer pre-stringifies. Other small tidies: drop redundant float() wrappers on values that are already float (e.g. attributes set from query_float), drop float(timeout_sec) comparison cast, fix two `return 0` → `return 0.0` to match `-> float` annotations. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/kx2_backend.py | 470 +++++++++--------------------- pylabrobot/paa/kx2/kx2_driver.py | 321 ++++++++------------ 2 files changed, 253 insertions(+), 538 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_backend.py b/pylabrobot/paa/kx2/kx2_backend.py index 7848c995fd5..91b20a13d10 100644 --- a/pylabrobot/paa/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/kx2_backend.py @@ -16,12 +16,10 @@ from pylabrobot.capabilities.capability import BackendParams from pylabrobot.paa.kx2.kx2_driver import ( CanError, - CmdType, JointMoveDirection, KX2Driver, MotorMoveParam, MotorsMovePlan, - ValType, ) from pylabrobot.resources import Coordinate, Rotation @@ -34,22 +32,6 @@ class HomeStatus(IntEnum): InitializedWithoutHoming = 2 -def _is_number(s: str) -> bool: - try: - float(str(s).strip()) - return True - except Exception: - return False - - -def _to_float(s: str, default: float = 0.0) -> float: - try: - return float(str(s).strip()) - except Exception: - warnings.warn(f"Error converting '{s}' to float, returning default") - return default - - class KX2ArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive): """Arm-capability backend for the PAA KX2. @@ -109,18 +91,6 @@ def __init__( self.node_id_list = [1, 2, 3, 4, 6] - # -- motion/rail/gripper decision helper -------------------------------- - - def _uses_ds402(self, axis: int) -> bool: - """Return True if this axis uses the DS402 controlword-based enable path. - - The four motion joints (shoulder/Z/elbow/wrist) are driven via the DS402 - state machine over RPDO1; the rail and the servo gripper use the Elmo - binary-interpreter ``MO`` command. This is the single piece of robot- - topology knowledge that selects between the driver's two enable paths. - """ - return int(axis) in (int(a) for a in MOTION_AXES) - async def _on_setup(self, backend_params: Optional[BackendParams] = None): # Driver has already brought CAN up (connect + node discovery) via # Device.setup(). Now read per-drive parameters, finish CANopen mapping, @@ -150,12 +120,7 @@ async def get_estop_state(self) -> bool: Reads the shoulder drive's SR (status register) via the binary interpreter. Bits 14/15 encode the stop/safety state. """ - r = int(await self.driver.binary_interpreter( - node_id=int(self.Axis.SHOULDER), - cmd="SR", - cmd_index=1, - cmd_type=CmdType.ValQuery, - )) + r = await self.driver.query_int(int(self.Axis.SHOULDER), "SR", 1) if r != 8438016: logger.warning("get_estop_state: SR register unexpected value %d (expected 8438016)", r) b14 = (r & 0x4000) == 0x4000 @@ -163,30 +128,20 @@ async def get_estop_state(self) -> bool: return (not b14) and (not b15) async def _motor_set_homed_status(self, axis: int, status: HomeStatus) -> None: - status_int = int(status) - if status_int == 1: - val = "1" - elif status_int == 2: - val = "2" - else: - val = "0" - await self.driver.binary_interpreter(int(axis), "UI", 3, CmdType.ValSet, val) + await self.driver.write(int(axis), "UI", 3, int(status)) async def motor_get_homed_status(self, node_id: int) -> HomeStatus: - left = await self.driver.binary_interpreter(int(node_id), "UI", 3, CmdType.ValQuery) - if left == 1: - return HomeStatus.Homed - if left == 2: - return HomeStatus.InitializedWithoutHoming - return HomeStatus.NotHomed + return HomeStatus(await self.driver.query_int(int(node_id), "UI", 3)) async def _motor_reset_encoder_position(self, axis: int, position: float) -> None: - await self.driver.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "0") - await self.driver.binary_interpreter(int(axis), "HM", 3, CmdType.ValSet, "0") - await self.driver.binary_interpreter(int(axis), "HM", 4, CmdType.ValSet, "0") - await self.driver.binary_interpreter(int(axis), "HM", 5, CmdType.ValSet, "0") - await self.driver.binary_interpreter(int(axis), "HM", 2, CmdType.ValSet, str(position)) - await self.driver.binary_interpreter(int(axis), "HM", 1, CmdType.ValSet, "1") + await self.driver.write(int(axis), "HM", 1, 0) + await self.driver.write(int(axis), "HM", 3, 0) + await self.driver.write(int(axis), "HM", 4, 0) + await self.driver.write(int(axis), "HM", 5, 0) + # Old code packed `position` as int32 via `int(round(float(str(position))))`; + # preserve that rounding semantic for callers that pass fractional values. + await self.driver.write(int(axis), "HM", 2, int(round(position))) + await self.driver.write(int(axis), "HM", 1, 1) async def _motor_hard_stop_search( self, @@ -198,15 +153,15 @@ async def _motor_hard_stop_search( timeout: float, ) -> None: nid = int(axis) - await self.driver.binary_interpreter(nid, "ER", 3, CmdType.ValSet, str(max_pe * 10)) - await self.driver.binary_interpreter(nid, "AC", 0, CmdType.ValSet, str(srch_acc)) - await self.driver.binary_interpreter(nid, "DC", 0, CmdType.ValSet, str(srch_acc)) + await self.driver.write(nid, "ER", 3, max_pe * 10) + await self.driver.write(nid, "AC", 0, srch_acc) + await self.driver.write(nid, "DC", 0, srch_acc) for i in [3, 4, 5, 2]: - await self.driver.binary_interpreter(nid, "HM", i, CmdType.ValSet, "0") - await self.driver.binary_interpreter(nid, "JV", 0, CmdType.ValSet, str(srch_vel)) + await self.driver.write(nid, "HM", i, 0) + await self.driver.write(nid, "JV", 0, srch_vel) try: - params = [str(int(hs_pe)), str(int(timeout * 1000))] + params = [int(hs_pe), int(timeout * 1000)] last_line = await self.driver.user_program_run( nid, "Home", params, int(timeout), True ) @@ -214,15 +169,15 @@ async def _motor_hard_stop_search( raise RuntimeError(f"Homing Script Error {34 + last_line}") curr_pos = await self.driver.motor_get_current_position(nid) - await self.driver.binary_interpreter(nid, "PA", 0, CmdType.ValSet, str(curr_pos)) - await self.driver.binary_interpreter(nid, "SP", 0, CmdType.ValSet, str(srch_vel)) - await self.driver.binary_interpreter(nid, "AC", 0, CmdType.ValSet, str(srch_acc)) - await self.driver.binary_interpreter(nid, "DC", 0, CmdType.ValSet, str(srch_acc)) + await self.driver.write(nid, "PA", 0, curr_pos) + await self.driver.write(nid, "SP", 0, srch_vel) + await self.driver.write(nid, "AC", 0, srch_acc) + await self.driver.write(nid, "DC", 0, srch_acc) finally: await asyncio.sleep(0.3) - await self.driver.binary_interpreter(nid, "BG", 0, CmdType.Execute, value="0") + await self.driver.execute(nid, "BG", 0) await asyncio.sleep(0.3) - await self.driver.binary_interpreter(nid, "ER", 3, CmdType.ValSet, str(int(max_pe))) + await self.driver.write(nid, "ER", 3, int(max_pe)) async def _motor_index_search( self, @@ -233,33 +188,31 @@ async def _motor_index_search( timeout: float, ) -> tuple: nid = int(axis) - await self.driver.binary_interpreter(nid, "HM", 1, CmdType.ValSet, "0") + await self.driver.write(nid, "HM", 1, 0) - rev = await self.driver.binary_interpreter(nid, "CA", 18, CmdType.ValQuery) - one_revolution = int(float(rev)) + one_revolution = await self.driver.query_int(nid, "CA", 18) if not positive_direction: one_revolution *= -1 - await self.driver.binary_interpreter(nid, "PR", 1, CmdType.ValSet, str(one_revolution)) - await self.driver.binary_interpreter(nid, "SP", 0, CmdType.ValSet, str(srch_vel)) - await self.driver.binary_interpreter(nid, "AC", 0, CmdType.ValSet, str(srch_acc)) - await self.driver.binary_interpreter(nid, "DC", 0, CmdType.ValSet, str(srch_acc)) + await self.driver.write(nid, "PR", 1, one_revolution) + await self.driver.write(nid, "SP", 0, srch_vel) + await self.driver.write(nid, "AC", 0, srch_acc) + await self.driver.write(nid, "DC", 0, srch_acc) - await self.driver.binary_interpreter(nid, "HM", 3, CmdType.ValSet, "3") # index only - await self.driver.binary_interpreter(nid, "HM", 4, CmdType.ValSet, "0") - await self.driver.binary_interpreter(nid, "HM", 5, CmdType.ValSet, "0") - await self.driver.binary_interpreter(nid, "HM", 2, CmdType.ValSet, "0") - await self.driver.binary_interpreter(nid, "HM", 1, CmdType.ValSet, "1") # arm + await self.driver.write(nid, "HM", 3, 3) # index only + await self.driver.write(nid, "HM", 4, 0) + await self.driver.write(nid, "HM", 5, 0) + await self.driver.write(nid, "HM", 2, 0) + await self.driver.write(nid, "HM", 1, 1) # arm - await self.driver.binary_interpreter(nid, "BG", 0, CmdType.Execute) + await self.driver.execute(nid, "BG", 0) await self.driver.wait_for_moves_done([nid], timeout) - left = await self.driver.binary_interpreter(nid, "HM", 1, CmdType.ValQuery) + left = await self.driver.query_int(nid, "HM", 1) if left != 0: raise RuntimeError("Homing Failure: Failed to finish index pulse search.") - cap = await self.driver.binary_interpreter(nid, "HM", 7, CmdType.ValQuery) - captured_position = int(float(cap)) + captured_position = await self.driver.query_int(nid, "HM", 7) return one_revolution, captured_position async def home_motor( @@ -278,7 +231,7 @@ async def home_motor( ) -> None: nid = int(axis) - left = await self.driver.binary_interpreter(nid, "CA", 41, CmdType.ValQuery) + left = await self.driver.query_int(nid, "CA", 41) if left == 24: raise RuntimeError("Error 43") @@ -290,7 +243,7 @@ async def home_motor( raise RuntimeError(fault) raise e - await self.driver.motor_enable(node_id=nid, state=True, use_ds402=self._uses_ds402(nid)) + await self.driver.motor_enable(node_id=nid, state=True, use_ds402=nid in MOTION_AXES) await self.driver.motors_move_absolute_execute( plan=MotorsMovePlan( @@ -344,23 +297,9 @@ async def servo_gripper_initialize(self): await self.servo_gripper_close() async def servo_gripper_home(self) -> None: - await self.driver.binary_interpreter( - node_id=int(self.Axis.SERVO_GRIPPER), - cmd="PL", - cmd_index=1, - cmd_type=CmdType.ValSet, - value=str(self.servo_gripper_peak_current), - val_type=ValType.Float, - ) - - await self.driver.binary_interpreter( - node_id=int(self.Axis.SERVO_GRIPPER), - cmd="CL", - cmd_index=1, - cmd_type=CmdType.ValSet, - value=str(self.servo_gripper_continuous_current), - val_type=ValType.Float, - ) + sg = int(self.Axis.SERVO_GRIPPER) + await self.driver.write(sg, "PL", 1, self.servo_gripper_peak_current) + await self.driver.write(sg, "CL", 1, self.servo_gripper_continuous_current) await self.home_motor( axis=self.Axis.SERVO_GRIPPER, @@ -379,63 +318,39 @@ async def servo_gripper_home(self) -> None: await self.servo_gripper_set_default_gripping_force(100) async def servo_gripper_set_default_gripping_force(self, max_force_percent: int) -> None: - if max_force_percent < 10: - max_force_percent = 10 - elif max_force_percent > 100: - max_force_percent = 100 + max_force_percent = max(10, min(max_force_percent, 100)) - cont_current = float(self.servo_gripper_continuous_current) * max_force_percent / 100.0 - peak_current = float(self.servo_gripper_peak_current) * max_force_percent / 100.0 + cont_current = self.servo_gripper_continuous_current * max_force_percent / 100.0 + peak_current = self.servo_gripper_peak_current * max_force_percent / 100.0 - axis = self.Axis.SERVO_GRIPPER + sg = int(self.Axis.SERVO_GRIPPER) # 1) PL with unscaled peak current - await self.driver.binary_interpreter( - int(axis), "PL", 1, CmdType.ValSet, str(self.servo_gripper_peak_current), val_type=ValType.Float - ) + await self.driver.write(sg, "PL", 1, self.servo_gripper_peak_current) # 2) CL with scaled continuous current - await self.driver.binary_interpreter( - int(axis), "CL", 1, CmdType.ValSet, str(cont_current), val_type=ValType.Float - ) + await self.driver.write(sg, "CL", 1, cont_current) # 3) PL with scaled peak current - await self.driver.binary_interpreter( - int(axis), "PL", 1, CmdType.ValSet, str(peak_current), val_type=ValType.Float - ) + await self.driver.write(sg, "PL", 1, peak_current) self.servo_gripper_force_percent = max_force_percent async def get_servo_gripper_max_force(self) -> float: """Return current gripping force as percentage of max (0-1).""" - cl = await self.driver.binary_interpreter( - node_id=int(self.Axis.SERVO_GRIPPER), - cmd="CL", - cmd_index=1, - cmd_type=CmdType.ValQuery, - val_type=ValType.Float, - ) - - iq = await self.driver.binary_interpreter( - node_id=int(self.Axis.SERVO_GRIPPER), - cmd="IQ", - cmd_index=0, - cmd_type=CmdType.ValQuery, - val_type=ValType.Float, - ) + sg = int(self.Axis.SERVO_GRIPPER) + cl = await self.driver.query_float(sg, "CL", 1) + iq = await self.driver.query_float(sg, "IQ", 0) if cl == 0: - return 0 + return 0.0 - return max(0, min(abs(iq / cl), 1)) + return max(0.0, min(abs(iq / cl), 1.0)) async def check_plate_gripped(self, num_attempts: int = 5) -> None: for _ in range(num_attempts): - motor_status = await self.driver.binary_interpreter( - node_id=int(self.Axis.SERVO_GRIPPER), - cmd="MS", - cmd_index=1, - cmd_type=CmdType.ValQuery, + motor_status = await self.driver.query_int( + int(self.Axis.SERVO_GRIPPER), "MS", 1 ) logger.debug("Servo gripper motor status: %s", motor_status) @@ -499,16 +414,10 @@ async def drive_set_move_count_parameters( maintenance_required_rail: bool, last_maintenance_performed_date_rail: int, ) -> None: + z = int(self.Axis.Z) + # MoveCount -> Z axis, UI index 22 - await self.driver.binary_interpreter( - node_id=int(self.Axis.Z), - cmd="UI", - cmd_index=22, - cmd_type=CmdType.ValSet, - value=str(int(move_count)), - val_type=ValType.Int, - low_priority=False, - ) + await self.driver.write(z, "UI", 22, int(move_count)) # Travel[] -> each node, UF index 5 # The source looked 1-based for Travel and 0-based for NodeIDList; handle both cleanly. @@ -518,80 +427,23 @@ async def drive_set_move_count_parameters( pairs = zip(self.node_id_list, travel) for node_id, dist in pairs: - await self.driver.binary_interpreter( - node_id=int(node_id), - cmd="UF", - cmd_index=5, - cmd_type=CmdType.ValSet, - value=str(float(dist)), - val_type=ValType.Float, - low_priority=True, - ) + await self.driver.write(int(node_id), "UF", 5, float(dist)) # LastMaintenancePerformed -> Z axis, UF index 6 - await self.driver.binary_interpreter( - node_id=int(self.Axis.Z), - cmd="UF", - cmd_index=6, - cmd_type=CmdType.ValSet, - value=str(float(last_maintenance_performed)), - val_type=ValType.Float, - low_priority=True, - ) + await self.driver.write(z, "UF", 6, float(last_maintenance_performed)) # MaintenanceRequired -> Z axis, UI index 23 - await self.driver.binary_interpreter( - node_id=int(self.Axis.Z), - cmd="UI", - cmd_index=23, - cmd_type=CmdType.ValSet, - value="1" if maintenance_required else "0", - val_type=ValType.Int, - low_priority=False, - ) + await self.driver.write(z, "UI", 23, 1 if maintenance_required else 0) # LastMaintenancePerformedDate -> Z axis, UI index 21 - await self.driver.binary_interpreter( - node_id=int(self.Axis.Z), - cmd="UI", - cmd_index=21, - cmd_type=CmdType.ValSet, - value=str(int(last_maintenance_performed_date)), - val_type=ValType.Int, - low_priority=False, - ) + await self.driver.write(z, "UI", 21, int(last_maintenance_performed_date)) # Rail (if present) if self.robot_on_rail: - await self.driver.binary_interpreter( - node_id=int(self.Axis.RAIL), - cmd="UF", - cmd_index=6, - cmd_type=CmdType.ValSet, - value=str(float(last_maintenance_performed_rail)), - val_type=ValType.Float, - low_priority=True, - ) - - await self.driver.binary_interpreter( - node_id=int(self.Axis.RAIL), - cmd="UI", - cmd_index=23, - cmd_type=CmdType.ValSet, - value="1" if maintenance_required_rail else "0", - val_type=ValType.Int, - low_priority=False, - ) - - await self.driver.binary_interpreter( - node_id=int(self.Axis.RAIL), - cmd="UI", - cmd_index=21, - cmd_type=CmdType.ValSet, - value=str(int(last_maintenance_performed_date_rail)), - val_type=ValType.Int, - low_priority=False, - ) + rail = int(self.Axis.RAIL) + await self.driver.write(rail, "UF", 6, float(last_maintenance_performed_rail)) + await self.driver.write(rail, "UI", 23, 1 if maintenance_required_rail else 0) + await self.driver.write(rail, "UI", 21, int(last_maintenance_performed_date_rail)) async def drive_get_parameters(self, node_ids) -> None: self.robot_on_rail = False @@ -609,7 +461,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # Pass 1: identify axes by UI[4] uis = set() for node in nodes: - if node == await self.driver.binary_interpreter(int(node), "UI", 4, CmdType.ValQuery, val_type=ValType.Int): + if node == await self.driver.query_int(int(node), "UI", 4): uis.add(node) for required_axis in MOTION_AXES: if required_axis.value not in uis: @@ -626,7 +478,7 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: # UI[5..10] digital inputs for ui_idx in range(5, 11): - ret = await self.driver.binary_interpreter(int(axis), "UI", ui_idx, CmdType.ValQuery, val_type=ValType.Int) + ret = await self.driver.query_int(int(axis), "UI", ui_idx) ch = (ui_idx - 5) + 1 if ret == 101: set2d(self.digital_input_assignment, axis, ch, "GripperSensor") @@ -641,23 +493,23 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: self.digital_input_assignment, axis, ch, - "" if (not _is_number(ret) or _to_float(ret) <= 0.0) else f"AuxPin{ret}", + "" if ret <= 0 else f"AuxPin{ret}", ) # UI[11..12] analog inputs for ui_idx in range(11, 13): - ret = await self.driver.binary_interpreter(int(axis), "UI", ui_idx, CmdType.ValQuery, val_type=ValType.Int) + ret = await self.driver.query_int(int(axis), "UI", ui_idx) ch = (ui_idx - 11) + 1 set2d( self.AnalogInputAssignment, axis, ch, - "" if (not _is_number(ret) or _to_float(ret) <= 0.0) else f"AuxPin{ret}", + "" if ret <= 0 else f"AuxPin{ret}", ) # UI[13..16] outputs for ui_idx in range(13, 17): - ret = await self.driver.binary_interpreter(int(axis), "UI", ui_idx, CmdType.ValQuery, val_type=ValType.Int) + ret = await self.driver.query_int(int(axis), "UI", ui_idx) ch = (ui_idx - 13) + 1 if ret == 101: @@ -685,45 +537,33 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: self.output_assignment, axis, ch, - "" if (not _is_number(ret) or _to_float(ret) <= 0.0) else f"AuxPin{ret}", + "" if ret <= 0 else f"AuxPin{ret}", ) - # UI[24] drive serial number - ret = await self.driver.binary_interpreter(int(axis), "UI", 24, CmdType.ValQuery, val_type=ValType.Int) - if _is_number(ret): - # self.drive_serial_number[axis] = int(ret) - pass + # UI[24] drive serial number (queried but not currently stored) + await self.driver.query_int(int(axis), "UI", 24) # UF[1], UF[2] conversion factor - uf1 = await self.driver.binary_interpreter(int(axis), "UF", 1, CmdType.ValQuery, val_type=ValType.Float) - uf2 = await self.driver.binary_interpreter(int(axis), "UF", 2, CmdType.ValQuery, val_type=ValType.Float) - if ( - not (_is_number(uf1) and _is_number(uf2)) or _to_float(uf1) == 0.0 or _to_float(uf2) == 0.0 - ): + uf1 = await self.driver.query_float(int(axis), "UF", 1) + uf2 = await self.driver.query_float(int(axis), "UF", 2) + if uf1 == 0.0 or uf2 == 0.0: raise CanError(f"Invalid Motor Conversion Factor for axis {axis}. UF[1]={uf1}, UF[2]={uf2}") - self.motor_conversion_factor_ax[axis] = _to_float(uf1) / _to_float(uf2) + self.motor_conversion_factor_ax[axis] = uf1 / uf2 # XM / travel - xm1 = await self.driver.binary_interpreter(int(axis), "XM", 1, CmdType.ValQuery, val_type=ValType.Int) - xm2 = await self.driver.binary_interpreter(int(axis), "XM", 2, CmdType.ValQuery, val_type=ValType.Int) - uf3 = await self.driver.binary_interpreter(int(axis), "UF", 3, CmdType.ValQuery, val_type=ValType.Float) - uf4 = await self.driver.binary_interpreter(int(axis), "UF", 4, CmdType.ValQuery, val_type=ValType.Float) - vh3 = await self.driver.binary_interpreter(int(axis), "VH", 3, CmdType.ValQuery, val_type=ValType.Int) - vl3 = await self.driver.binary_interpreter(int(axis), "VL", 3, CmdType.ValQuery, val_type=ValType.Int) + xm1 = await self.driver.query_int(int(axis), "XM", 1) + xm2 = await self.driver.query_int(int(axis), "XM", 2) + uf3 = await self.driver.query_float(int(axis), "UF", 3) + uf4 = await self.driver.query_float(int(axis), "UF", 4) + vh3 = await self.driver.query_int(int(axis), "VH", 3) + vl3 = await self.driver.query_int(int(axis), "VL", 3) - self.max_travel_ax[axis] = _to_float(uf3) - self.min_travel_ax[axis] = _to_float(uf4) + self.max_travel_ax[axis] = uf3 + self.min_travel_ax[axis] = uf4 - if not all(_is_number(x) for x in (xm1, xm2, vh3, vl3)): - raise CanError( - f"Invalid travel limits or modulo settings for axis {axis}. " - f"VH[3]={vh3}, VL[3]={vl3}, XM[1]={xm1}, XM[2]={xm2}" - ) - - xm1v, xm2v, vh3v, vl3v = map(_to_float, (xm1, xm2, vh3, vl3)) - if ((xm1v == 0.0) and (xm2v == 0.0)) or ((xm1v <= vl3v) and (xm2v >= vh3v)): + if ((xm1 == 0) and (xm2 == 0)) or ((xm1 <= vl3) and (xm2 >= vh3)): self.unlimited_travel_ax[axis] = False - elif (xm1v > vl3v) and (xm2v < vh3v): + elif (xm1 > vl3) and (xm2 < vh3): self.unlimited_travel_ax[axis] = True else: raise CanError( @@ -732,14 +572,11 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: ) # Encoder socket/type - ca45 = await self.driver.binary_interpreter(int(axis), "CA", 45, CmdType.ValQuery, val_type=ValType.Int) - ca45v = _to_float(ca45, 0.0) - if (not _is_number(ca45)) or not (0.0 < ca45v <= 4.0): + ca45 = await self.driver.query_int(int(axis), "CA", 45) + if not (0 < ca45 <= 4): raise CanError(f"Invalid encoder socket number received from axis {axis}. CA[45]={ca45}") - enc_type = await self.driver.binary_interpreter( - int(axis), "CA", int(round(40.0 + ca45v)), CmdType.ValQuery, val_type=ValType.Int - ) + enc_type = await self.driver.query_int(int(axis), "CA", 40 + ca45) if enc_type in (1, 2): self.absolute_encoder_ax[axis] = False elif enc_type == 24: @@ -749,104 +586,57 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: f"Unsupported encoder type specified for axis {axis}. CA[4{ca45}]={enc_type}" ) - ca46 = await self.driver.binary_interpreter(int(axis), "CA", 46, CmdType.ValQuery, val_type=ValType.Int) + ca46 = await self.driver.query_int(int(axis), "CA", 46) if ca45 == ca46: num3 = 1.0 else: - ff3 = await self.driver.binary_interpreter(int(axis), "FF", 3, CmdType.ValQuery, val_type=ValType.Float) - num3 = _to_float(ff3, 1.0) + num3 = await self.driver.query_float(int(axis), "FF", 3) denom = self.motor_conversion_factor_ax[axis] * num3 # or 1.0 - sp2 = await self.driver.binary_interpreter(int(axis), "SP", 2, CmdType.ValQuery, val_type=ValType.Int) + sp2 = await self.driver.query_int(int(axis), "SP", 2) if sp2 == 100000: - vh2 = await self.driver.binary_interpreter(int(axis), "VH", 2, CmdType.ValQuery, val_type=ValType.Int) - self.max_vel_ax[axis] = _to_float(vh2) / 1.01 / denom + vh2 = await self.driver.query_int(int(axis), "VH", 2) + self.max_vel_ax[axis] = vh2 / 1.01 / denom else: - self.max_vel_ax[axis] = _to_float(sp2) / denom + self.max_vel_ax[axis] = sp2 / denom - sd0 = await self.driver.binary_interpreter(int(axis), "SD", 0, CmdType.ValQuery, val_type=ValType.Int) - self.max_accel_ax[axis] = _to_float(sd0) / 1.01 / denom + sd0 = await self.driver.query_int(int(axis), "SD", 0) + self.max_accel_ax[axis] = sd0 / 1.01 / denom # Robot-level params from shoulder_ax - shoulder = self.Axis.SHOULDER - - self.base_to_gripper_clearance_z = _to_float( - await self.driver.binary_interpreter(int(shoulder), "UF", 6, CmdType.ValQuery, val_type=ValType.Float) - ) - self.base_to_gripper_clearance_arm = _to_float( - await self.driver.binary_interpreter(int(shoulder), "UF", 7, CmdType.ValQuery, val_type=ValType.Float) - ) - self.wrist_offset = _to_float( - await self.driver.binary_interpreter(int(shoulder), "UF", 8, CmdType.ValQuery, val_type=ValType.Float) - ) - self.elbow_offset = _to_float( - await self.driver.binary_interpreter(int(shoulder), "UF", 9, CmdType.ValQuery, val_type=ValType.Float) - ) - self.elbow_zero_offset = _to_float( - await self.driver.binary_interpreter(int(shoulder), "UF", 10, CmdType.ValQuery, val_type=ValType.Float) - ) - self.MaxLinearVelMMPerSec = _to_float( - await self.driver.binary_interpreter(int(shoulder), "UF", 11, CmdType.ValQuery, val_type=ValType.Float) - ) - self.MaxLinearAccelMMPerSec2 = _to_float( - await self.driver.binary_interpreter(int(shoulder), "UF", 12, CmdType.ValQuery, val_type=ValType.Float) - ) - self.MaxLinearJerkMMPerSec3 = _to_float( - await self.driver.binary_interpreter(int(shoulder), "UF", 13, CmdType.ValQuery, val_type=ValType.Float) - ) - self.MaxRotaryVelDegPerSec = _to_float( - await self.driver.binary_interpreter(int(shoulder), "UF", 14, CmdType.ValQuery, val_type=ValType.Float) - ) - self.MaxRotaryAccelDegPerSec2 = _to_float( - await self.driver.binary_interpreter(int(shoulder), "UF", 15, CmdType.ValQuery, val_type=ValType.Float) - ) - - ui17 = await self.driver.binary_interpreter(int(shoulder), "UI", 17, CmdType.ValQuery, val_type=ValType.Int) + shoulder = int(self.Axis.SHOULDER) + + self.base_to_gripper_clearance_z = await self.driver.query_float(shoulder, "UF", 6) + self.base_to_gripper_clearance_arm = await self.driver.query_float(shoulder, "UF", 7) + self.wrist_offset = await self.driver.query_float(shoulder, "UF", 8) + self.elbow_offset = await self.driver.query_float(shoulder, "UF", 9) + self.elbow_zero_offset = await self.driver.query_float(shoulder, "UF", 10) + self.MaxLinearVelMMPerSec = await self.driver.query_float(shoulder, "UF", 11) + self.MaxLinearAccelMMPerSec2 = await self.driver.query_float(shoulder, "UF", 12) + self.MaxLinearJerkMMPerSec3 = await self.driver.query_float(shoulder, "UF", 13) + self.MaxRotaryVelDegPerSec = await self.driver.query_float(shoulder, "UF", 14) + self.MaxRotaryAccelDegPerSec2 = await self.driver.query_float(shoulder, "UF", 15) + + ui17 = await self.driver.query_int(shoulder, "UI", 17) self.pvt_time_interval_msec = ( - 25 - if (not _is_number(ui17) or _to_float(ui17) <= 0.0 or _to_float(ui17) > 255.0) - else int(_to_float(ui17)) + 25 if (ui17 <= 0 or ui17 > 255) else ui17 ) # Servo gripper params (only if present) - sg = self.Axis.SERVO_GRIPPER - self.servo_gripper_home_pos = int( - await self.driver.binary_interpreter(int(sg), "UF", 6, CmdType.ValQuery, val_type=ValType.Float) - ) - self.servo_gripper_home_search_vel = int( - await self.driver.binary_interpreter(int(sg), "UF", 7, CmdType.ValQuery, val_type=ValType.Float) - ) - self.servo_gripper_home_search_accel = int( - await self.driver.binary_interpreter(int(sg), "UF", 8, CmdType.ValQuery, val_type=ValType.Float) - ) - self.servo_gripper_home_default_position_error = int( - await self.driver.binary_interpreter(int(sg), "UF", 9, CmdType.ValQuery, val_type=ValType.Float) - ) - self.servo_gripper_home_hard_stop_position_error = int( - await self.driver.binary_interpreter(int(sg), "UF", 10, CmdType.ValQuery, val_type=ValType.Float) - ) - self.servo_gripper_home_hard_stop_offset = int( - await self.driver.binary_interpreter(int(sg), "UF", 11, CmdType.ValQuery, val_type=ValType.Float) - ) - self.servo_gripper_home_index_offset = int( - await self.driver.binary_interpreter(int(sg), "UF", 12, CmdType.ValQuery, val_type=ValType.Float) - ) - self.servo_gripper_home_offset_vel = int( - await self.driver.binary_interpreter(int(sg), "UF", 13, CmdType.ValQuery, val_type=ValType.Float) - ) - self.servo_gripper_home_offset_accel = int( - await self.driver.binary_interpreter(int(sg), "UF", 14, CmdType.ValQuery, val_type=ValType.Float) - ) - self.servo_gripper_home_timeout_msec = int( - await self.driver.binary_interpreter(int(sg), "UF", 15, CmdType.ValQuery, val_type=ValType.Float) - ) - self.servo_gripper_continuous_current = _to_float( - await self.driver.binary_interpreter(int(sg), "UF", 16, CmdType.ValQuery, val_type=ValType.Float) - ) - self.servo_gripper_peak_current = _to_float( - await self.driver.binary_interpreter(int(sg), "UF", 17, CmdType.ValQuery, val_type=ValType.Float) - ) + sg = int(self.Axis.SERVO_GRIPPER) + self.servo_gripper_home_pos = int(await self.driver.query_float(sg, "UF", 6)) + self.servo_gripper_home_search_vel = int(await self.driver.query_float(sg, "UF", 7)) + self.servo_gripper_home_search_accel = int(await self.driver.query_float(sg, "UF", 8)) + self.servo_gripper_home_default_position_error = int(await self.driver.query_float(sg, "UF", 9)) + self.servo_gripper_home_hard_stop_position_error = int(await self.driver.query_float(sg, "UF", 10)) + self.servo_gripper_home_hard_stop_offset = int(await self.driver.query_float(sg, "UF", 11)) + self.servo_gripper_home_index_offset = int(await self.driver.query_float(sg, "UF", 12)) + self.servo_gripper_home_offset_vel = int(await self.driver.query_float(sg, "UF", 13)) + self.servo_gripper_home_offset_accel = int(await self.driver.query_float(sg, "UF", 14)) + self.servo_gripper_home_timeout_msec = int(await self.driver.query_float(sg, "UF", 15)) + self.servo_gripper_continuous_current = await self.driver.query_float(sg, "UF", 16) + self.servo_gripper_peak_current = await self.driver.query_float(sg, "UF", 17) def convert_elbow_position_to_angle(self, elbow_pos: float) -> float: max_travel = self.max_travel_ax[self.Axis.ELBOW] @@ -881,7 +671,7 @@ async def motor_get_current_position(self, axis: "KX2ArmBackend.Axis") -> float: else: if c == 0: logger.warning("Axis %s has conversion factor of 0", axis) - return 0 + return 0.0 else: return raw / c diff --git a/pylabrobot/paa/kx2/kx2_driver.py b/pylabrobot/paa/kx2/kx2_driver.py index 97d89b36d83..8aba7a6873a 100644 --- a/pylabrobot/paa/kx2/kx2_driver.py +++ b/pylabrobot/paa/kx2/kx2_driver.py @@ -30,17 +30,6 @@ def _u32_le(value: int) -> List[int]: return list((value & 0xFFFFFFFF).to_bytes(4, byteorder="little", signed=False)) -class CmdType(IntEnum): - ValQuery = 1 - ValSet = 2 - Execute = 3 - - -class ValType(IntEnum): - Int = 1 - Float = 2 - - class COBType(IntEnum): NMT = 0 EMCY = 1 @@ -305,24 +294,12 @@ async def connect_part_two(self) -> None: ) for nid in self.motion_node_ids: - await self._can_sdo_download_elmo_object( - nid, 24768, 0, "-1", ElmoObjectDataType.INTEGER16 - ) - await self._can_sdo_download_elmo_object( - nid, 24772, 2, "16", ElmoObjectDataType.UNSIGNED32 - ) - await self._can_sdo_download_elmo_object( - nid, 24772, 3, "0", ElmoObjectDataType.UNSIGNED8 - ) - await self._can_sdo_download_elmo_object( - nid, 24772, 5, "8", ElmoObjectDataType.UNSIGNED8 - ) - await self._can_sdo_download_elmo_object( - nid, 24770, 2, "-3", ElmoObjectDataType.INTEGER8 - ) - await self._can_sdo_download_elmo_object( - nid, 24669, 0, "1", ElmoObjectDataType.INTEGER16 - ) + await self._can_sdo_download_elmo_object(nid, 24768, 0, -1, ElmoObjectDataType.INTEGER16) + await self._can_sdo_download_elmo_object(nid, 24772, 2, 16, ElmoObjectDataType.UNSIGNED32) + await self._can_sdo_download_elmo_object(nid, 24772, 3, 0, ElmoObjectDataType.UNSIGNED8) + await self._can_sdo_download_elmo_object(nid, 24772, 5, 8, ElmoObjectDataType.UNSIGNED8) + await self._can_sdo_download_elmo_object(nid, 24770, 2, -3, ElmoObjectDataType.INTEGER8) + await self._can_sdo_download_elmo_object(nid, 24669, 0, 1, ElmoObjectDataType.INTEGER16) for nid in self.motion_node_ids: await self._rpdo_map( @@ -451,65 +428,30 @@ async def _can_sdo_download( node = self._nodes[node_id] await asyncio.to_thread(node.sdo.download, index, sub_index, bytes(data_byte)) - async def _can_sdo_upload_elmo_object( - self, - node_id: int, - elmo_object_int: int, - sub_index: int, - data_type: ElmoObjectDataType, - ) -> str: - obj_byte0 = elmo_object_int >> 8 - obj_byte1 = elmo_object_int & 0xFF - data_bytes = await self._can_sdo_upload(node_id, obj_byte0, obj_byte1, sub_index) - - if len(data_bytes) == 0: - return "" - if data_type == ElmoObjectDataType.UNSIGNED8: - return str(int.from_bytes(data_bytes[:1], "little", signed=False)) - if data_type == ElmoObjectDataType.UNSIGNED16: - return str(int.from_bytes(data_bytes[:2], "little", signed=False)) - if data_type == ElmoObjectDataType.UNSIGNED32: - return str(int.from_bytes(data_bytes[:4], "little", signed=False)) - if data_type == ElmoObjectDataType.UNSIGNED64: - return str(int.from_bytes(data_bytes[:8], "little", signed=False)) - if data_type == ElmoObjectDataType.INTEGER16: - return str(int.from_bytes(data_bytes[:2], "little", signed=True)) - if data_type == ElmoObjectDataType.INTEGER32: - return str(int.from_bytes(data_bytes[:4], "little", signed=True)) - if data_type == ElmoObjectDataType.INTEGER64: - return str(int.from_bytes(data_bytes[:8], "little", signed=True)) - if data_type == ElmoObjectDataType.STR: - return "".join(chr(b) for b in data_bytes) - raise CanError(f"Unsupported data type for SDO Read conversion: {data_type.name}") - async def _can_sdo_download_elmo_object( self, node_id: int, elmo_object_int: int, sub_index: int, - data: str, + data: Union[int, float], data_type: ElmoObjectDataType, ) -> None: - if data_type == ElmoObjectDataType.UNSIGNED8: - data_bytes = list(int(data).to_bytes(1, "little")) - elif data_type == ElmoObjectDataType.UNSIGNED16: - data_bytes = list(int(data).to_bytes(2, "little")) - elif data_type == ElmoObjectDataType.UNSIGNED32: - data_bytes = list(int(float(data)).to_bytes(4, "little")) - elif data_type == ElmoObjectDataType.UNSIGNED64: - data_bytes = list(int(data).to_bytes(8, "little")) - elif data_type == ElmoObjectDataType.INTEGER8: - data_bytes = list(int(data).to_bytes(1, "little", signed=True)) - elif data_type == ElmoObjectDataType.INTEGER16: - data_bytes = list(int(data).to_bytes(2, "little", signed=True)) - elif data_type == ElmoObjectDataType.INTEGER32: - data_bytes = list(int(float(data)).to_bytes(4, "little", signed=True)) - elif data_type == ElmoObjectDataType.INTEGER64: - data_bytes = list(int(data).to_bytes(8, "little", signed=True)) - elif data_type == ElmoObjectDataType.STR: - data_bytes = [ord(c) for c in data] - else: + # Byte width + signedness derived from data_type; float inputs truncate to int. + _SDO_ELMO_PACK = { + ElmoObjectDataType.UNSIGNED8: (1, False), + ElmoObjectDataType.UNSIGNED16: (2, False), + ElmoObjectDataType.UNSIGNED32: (4, False), + ElmoObjectDataType.UNSIGNED64: (8, False), + ElmoObjectDataType.INTEGER8: (1, True), + ElmoObjectDataType.INTEGER16: (2, True), + ElmoObjectDataType.INTEGER32: (4, True), + ElmoObjectDataType.INTEGER64: (8, True), + } + spec = _SDO_ELMO_PACK.get(data_type) + if spec is None: raise CanError(f"Unsupported data type for SDO Write: {data_type.name}") + width, signed = spec + data_bytes = list(int(data).to_bytes(width, "little", signed=signed)) obj_byte0 = elmo_object_int >> 8 obj_byte1 = elmo_object_int & 0xFF @@ -535,39 +477,33 @@ def _dispatch_bi_response(self, node_id: int, data: bytes) -> None: msg_type = chr(data[0]) + chr(data[1]) msg_index = ((data[3] & 0x3F) << 8) | data[2] is_int = (data[3] & 0x80) == 0 - if is_int: - (val,) = struct.unpack(" Union[str, float]: - del low_priority # request priority is not meaningful over canopen.Network.send_message + *, + is_query: bool, + is_execute: bool, + is_float: bool, + value: Union[int, float] = 0, + ) -> List[Union[int, float]]: + """Frame + send an 8-byte binary-interpreter request; await one response + per target node. Each response is decoded to its native type (int or + float) by :meth:`_dispatch_bi_response`. + """ if self._network is None: - raise CanError("binary_interpreter called before setup()") - if value == "": - value = "0" + raise CanError("binary interpreter called before setup()") timeout = 10.0 if cmd.upper() == "SV" else 1.0 - is_float = val_type == ValType.Float - is_query = cmd_type == CmdType.ValQuery - is_execute = cmd_type == CmdType.Execute - # -- build the 8-byte request -- byte0 = ord(cmd[0]) & 0xFF byte1 = ord(cmd[-1]) & 0xFF byte2 = cmd_index & 0xFF @@ -576,34 +512,22 @@ async def binary_interpreter( byte3 |= 0x40 if is_float: byte3 |= 0x80 - if is_float: - val_bytes = struct.pack(" bool: - try: - expected = float(expected_str) - actual = float(actual_str) - except ValueError: - return False - if actual == 0.0: - return expected == 0.0 - ratio = expected / actual - return expected == actual or (0.99 < ratio < 1.01) - - # -- dispatch: single node vs. group -- - target_nodes = ( + data_to_send = payload[:4] if is_execute else payload + + targets = ( list(self.motion_node_ids) if node_id == _GROUP_NODE_ID else [node_id] ) futures: List[asyncio.Future] = [] - for nid in target_nodes: + for nid in targets: key = (nid, cmd, cmd_index) - # If a stale pending future exists, drop it. + # If a stale pending future exists for the same (node, cmd, index), drop it. old = self._pending_bi.pop(key, None) if old is not None and not old.done(): old.cancel() @@ -614,40 +538,67 @@ def _float_matches(expected_str: str, actual_str: str) -> bool: self._network.send_message(_BI_REQUEST_COB_BASE + node_id, data_to_send) try: - resps = await asyncio.wait_for(asyncio.gather(*futures), timeout=timeout) + return await asyncio.wait_for(asyncio.gather(*futures), timeout=timeout) except asyncio.TimeoutError: - for nid in target_nodes: + for nid in targets: self._pending_bi.pop((nid, cmd, cmd_index), None) raise CanError( f"Timeout waiting for response to {cmd}[{cmd_index}] from node {node_id}" ) - # -- interpret responses -- - if is_query: - if node_id == _GROUP_NODE_ID: - value = ",".join(str(r) for r in resps) - else: - value = str(resps[0]) - return float(value) if is_float else int(float(value)) - - if is_execute: - if any(r == "" for r in resps): - missing = [nid for nid, r in zip(target_nodes, resps) if r == ""] - raise CanError(f"No execute response from nodes {missing} for {cmd}[{cmd_index}]") - return float(value) if is_float else int(float(value)) + async def query_int(self, node_id: int, cmd: str, cmd_index: int) -> int: + """Query an int-typed Elmo parameter. Returns the drive's current value.""" + if node_id == _GROUP_NODE_ID: + raise CanError("Group queries are not supported") + resps = await self._send_bi( + node_id, cmd, cmd_index, is_query=True, is_execute=False, is_float=False, + ) + return int(resps[0]) + + async def query_float(self, node_id: int, cmd: str, cmd_index: int) -> float: + """Query a float-typed Elmo parameter. Returns the drive's current value.""" + if node_id == _GROUP_NODE_ID: + raise CanError("Group queries are not supported") + resps = await self._send_bi( + node_id, cmd, cmd_index, is_query=True, is_execute=False, is_float=True, + ) + return float(resps[0]) - # Write: verify each echoed value matches the one we sent. - for nid, resp in zip(target_nodes, resps): + async def write( + self, node_id: int, cmd: str, cmd_index: int, value: Union[int, float], + ) -> None: + """Write an Elmo parameter. The type of ``value`` selects int vs float + framing on the wire. The drive echoes the accepted value back, which we + verify — a mismatch raises :class:`CanError`. + """ + is_float = isinstance(value, float) + resps = await self._send_bi( + node_id, cmd, cmd_index, + is_query=False, is_execute=False, is_float=is_float, value=value, + ) + targets = ( + list(self.motion_node_ids) if node_id == _GROUP_NODE_ID else [node_id] + ) + for nid, resp in zip(targets, resps): if is_float: - ok = _float_matches(value, str(resp)) + # Elmo stores floats as float32; the echo may drift slightly relative + # to our float64 input — accept within ~1% ratio. + exp, act = float(value), float(resp) + ok = exp == act or (act != 0.0 and 0.99 < exp / act < 1.01) else: - ok = int(float(resp)) == int(float(value)) + ok = int(resp) == int(value) if not ok: raise CanError( f"Unexpected CAN response: sent {cmd}[{cmd_index}]={value}, " f"got {resp} from node {nid}" ) - return float(value) if is_float else int(float(value)) + + async def execute(self, node_id: int, cmd: str, cmd_index: int = 0) -> None: + """Fire-and-forget execute (e.g. ``BG``). Awaits the drive's response so + the caller sees the command completed on the wire, but no echo-check.""" + await self._send_bi( + node_id, cmd, cmd_index, is_query=False, is_execute=True, is_float=False, + ) async def _os_interpreter( self, @@ -720,16 +671,14 @@ async def request_drive_version(self, node_id: int) -> str: # --- DS402 / motor control ---------------------------------------------- async def motor_emergency_stop(self, node_id: int) -> None: - await self.binary_interpreter(node_id, "MO", 0, CmdType.ValSet, "0") + await self.write(node_id, "MO", 0, 0) async def motor_get_current_position(self, node_id: int, pu: bool = False) -> int: cmd = "PU" if pu else "PX" - val_str = await self.binary_interpreter(int(node_id), cmd, 0, CmdType.ValQuery) - return int(round(float(val_str))) + return await self.query_int(int(node_id), cmd, 0) async def motor_get_motion_status(self, node_id: int) -> int: - val = await self.binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) - return int(round(float(val))) + return await self.query_int(int(node_id), "MS", 0) async def _motor_set_move_direction( self, node_id: int, direction: JointMoveDirection @@ -738,16 +687,14 @@ async def _motor_set_move_direction( # direction (0=Normal, 1=CW, 2=CCW, 3=Shortest). Packs to 1 + 64*direction # = 1/65/129/193. val = 1 + 64 * int(direction) - await self._can_sdo_download_elmo_object( - node_id, 24818, 0, str(val), ElmoObjectDataType.UNSIGNED16 - ) + await self._can_sdo_download_elmo_object(node_id, 24818, 0, val, ElmoObjectDataType.UNSIGNED16) async def motor_check_if_move_done(self, node_id: int) -> bool: - ms_val = await self.binary_interpreter(node_id, "MS", 0, CmdType.ValQuery) + ms_val = await self.query_int(node_id, "MS", 0) if ms_val == 0: return True if ms_val == 1: - mo_val = await self.binary_interpreter(node_id, "MO", 0, CmdType.ValQuery) + mo_val = await self.query_int(node_id, "MO", 0) if mo_val == 1: return True fault = await self.motor_get_fault(node_id) @@ -759,10 +706,9 @@ async def motor_check_if_move_done(self, node_id: int) -> bool: return False async def motor_get_fault(self, node_id: int) -> Optional[str]: - val = await self.binary_interpreter(int(node_id), "MF", 0, CmdType.ValQuery) + val = await self.query_int(int(node_id), "MF", 0) if val == 0: return None - assert isinstance(val, int) faults: list[str] = [] bit_msgs = { @@ -823,7 +769,7 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N max_attempts = 5 for attempt in range(1, max_attempts + 1): if not use_ds402: - await self.binary_interpreter(node_id, "MO", 0, CmdType.ValSet, "1") + await self.write(node_id, "MO", 0, 1) else: # DS402 enable sequence: Fault -> Shutdown -> Switched On -> Op Enabled. # Matches the C# reference (clscanmotor.cs:4495-4509): back-to-back @@ -831,9 +777,7 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N for cw in (0, 128, 6, 7, 15): await self._control_word_set(node_id=node_id, value=cw) await asyncio.sleep(0.1) - left = await self.binary_interpreter( - node_id=node_id, cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery - ) + left = await self.query_int(node_id, "MO", 0) if left == 1: break logger.warning( @@ -845,9 +789,7 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N else: if not use_ds402: try: - await self.binary_interpreter( - node_id=node_id, cmd="MO", cmd_index=0, cmd_type=CmdType.ValSet, value="0" - ) + await self.write(node_id, "MO", 0, 0) except Exception: pass else: @@ -856,9 +798,7 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N await self._control_word_set(node_id=node_id, value=7) await self._control_word_set(node_id=node_id, value=6) await asyncio.sleep(0.1) - left = await self.binary_interpreter( - node_id=node_id, cmd="MO", cmd_index=0, cmd_type=CmdType.ValQuery - ) + left = await self.query_int(node_id, "MO", 0) if left != 0: raise RuntimeError(f"Motor failed to disable (node_id = {node_id})") @@ -926,21 +866,17 @@ async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: await self._motor_set_move_direction(nid, move.direction) # 0x607A = Target Position (24698 decimal) await self._can_sdo_download_elmo_object( - nid, 24698, 0, str(int(move.position)), ElmoObjectDataType.INTEGER32, + nid, 24698, 0, int(move.position), ElmoObjectDataType.INTEGER32, ) # 0x6081 = Profile Velocity (24705 decimal) await self._can_sdo_download_elmo_object( - nid, 24705, 0, str(int(move.velocity)), ElmoObjectDataType.UNSIGNED32, + nid, 24705, 0, int(move.velocity), ElmoObjectDataType.UNSIGNED32, ) acc = max(int(move.acceleration), 100) # 0x6083 = Profile Acceleration (24707 decimal) - await self._can_sdo_download_elmo_object( - nid, 24707, 0, str(acc), ElmoObjectDataType.UNSIGNED32, - ) + await self._can_sdo_download_elmo_object(nid, 24707, 0, acc, ElmoObjectDataType.UNSIGNED32) # 0x6084 = Profile Deceleration (24708 decimal) - await self._can_sdo_download_elmo_object( - nid, 24708, 0, str(acc), ElmoObjectDataType.UNSIGNED32, - ) + await self._can_sdo_download_elmo_object(nid, 24708, 0, acc, ElmoObjectDataType.UNSIGNED32) node_ids = [int(move.node_id) for move in plan.moves] await self._motors_move_start(node_ids) @@ -950,7 +886,7 @@ async def user_program_run( self, node_id: int, user_function: str, - params=None, + params: Optional[List[Union[int, float]]] = None, timeout_sec: int = 0, wait_until_done: bool = False, ) -> int: @@ -960,28 +896,24 @@ async def user_program_run( raise ValueError("node_id must be in [0, 255]") node_id = int(node_id) - ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + ps = await self.query_int(node_id, "PS", 0) if ps == -2: raise CanError(f"Node {node_id}: controller reported PS=-2 (not ready / unavailable)") if ps != -1: - await self.binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="0") + await self.write(node_id, "UI", 1, 0) t0 = time.monotonic() while (time.monotonic() - t0) < 3.0: - ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) + ps = await self.query_int(node_id, "PS", 0) if ps == -1: break await asyncio.sleep(0.01) else: raise CanError(f"Node {node_id}: did not reach idle state (PS=-1) within 3s (last PS={ps})") - arg_str = "" - if params: - parts = [str(p) for p in params] - if parts: - arg_str = f"({','.join(parts)})" + arg_str = f"({','.join(str(p) for p in params)})" if params else "" - await self.binary_interpreter(node_id, "UI", 1, CmdType.ValSet, value="1") + await self.write(node_id, "UI", 1, 1) cmd = f"XQ##{user_function}{arg_str}" logger.debug("user_program_run: %s", cmd) @@ -992,16 +924,12 @@ async def user_program_run( t0 = time.monotonic() ps = 1 ui1 = 1 - while ps == 1 and ui1 == 1 and (time.monotonic() - t0) < float(timeout_sec): - ps = int(await self.binary_interpreter(node_id, "PS", 0, CmdType.ValQuery)) - ui1 = int(await self.binary_interpreter(node_id, "UI", 1, CmdType.ValQuery)) + while ps == 1 and ui1 == 1 and (time.monotonic() - t0) < timeout_sec: + ps = await self.query_int(node_id, "PS", 0) + ui1 = await self.query_int(node_id, "UI", 1) await asyncio.sleep(0.01) - expr_raw = await self.binary_interpreter(node_id, "UI", 2, CmdType.ValQuery) - try: - last_line_completed = int(str(expr_raw).strip()) - except Exception: - last_line_completed = 0 + last_line_completed = await self.query_int(node_id, "UI", 2) if ps == 1 and ui1 == 1: raise CanError( @@ -1019,15 +947,12 @@ async def user_program_run( # --- I/O ----------------------------------------------------------------- async def read_input(self, node_id: int, input_num: int) -> bool: - left = await self.binary_interpreter(node_id, "IB", input_num, CmdType.ValQuery) - return left == 1 + return await self.query_int(node_id, "IB", input_num) == 1 async def _read_output(self, node_id: int, output_num: int) -> bool: - expression = await self.binary_interpreter(node_id, "OP", 0, CmdType.ValQuery) - val = int(expression) + val = await self.query_int(node_id, "OP", 0) mask = 1 << (output_num - 1) return (val & mask) == mask - async def _set_output(self, node_id: int, output_num: int, state: bool) -> Union[str, float]: - val = "1" if state else "0" - return await self.binary_interpreter(node_id, "OB", output_num, CmdType.ValSet, val) + async def _set_output(self, node_id: int, output_num: int, state: bool) -> None: + await self.write(node_id, "OB", output_num, 1 if state else 0) From 73e262f3b227959e0e6f299950793c449f9c759a Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Fri, 24 Apr 2026 15:10:12 -0700 Subject: [PATCH 33/93] revert KX2 user_program_run to return 0 on success Returning UI[2] from a successful run breaks _motor_hard_stop_search: the Home script leaves UI[2]=3 after a clean homing (each line is a checkpoint, not a failure marker), which made the `if last_line in [1, 2, 3]: raise RuntimeError("Homing Script Error 37")` check fire on every init. Go back to returning 0 on success; the existing CanError path already carries `last_line=X` in its message string for diagnostics on the failure path. Also folds connect_part_two into setup and moves the unlimited-travel direction assignment into drive_get_parameters where the flag is set. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/kx2_backend.py | 13 +++++------ pylabrobot/paa/kx2/kx2_driver.py | 37 ++++++++++++------------------- 2 files changed, 19 insertions(+), 31 deletions(-) diff --git a/pylabrobot/paa/kx2/kx2_backend.py b/pylabrobot/paa/kx2/kx2_backend.py index 91b20a13d10..6e25c8be570 100644 --- a/pylabrobot/paa/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/kx2_backend.py @@ -92,18 +92,13 @@ def __init__( self.node_id_list = [1, 2, 3, 4, 6] async def _on_setup(self, backend_params: Optional[BackendParams] = None): - # Driver has already brought CAN up (connect + node discovery) via - # Device.setup(). Now read per-drive parameters, finish CANopen mapping, - # enable motion axes, and initialize the servo gripper. + # Driver has already brought CAN up (connect + node discovery + PDO + # mapping) via Device.setup(). Now read per-drive parameters, enable + # motion axes, and initialize the servo gripper. await self.drive_get_parameters(self.driver.node_id_list) - await self.driver.connect_part_two() await asyncio.sleep(2) - for axis in MOTION_AXES: - if self.unlimited_travel_ax[axis]: - self.g_joint_move_direction[axis] = JointMoveDirection.ShortestWay - for axis in MOTION_AXES: try: await self.driver.motor_enable(node_id=int(axis), state=True, use_ds402=True) @@ -565,6 +560,8 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: self.unlimited_travel_ax[axis] = False elif (xm1 > vl3) and (xm2 < vh3): self.unlimited_travel_ax[axis] = True + if axis in MOTION_AXES: + self.g_joint_move_direction[axis] = JointMoveDirection.ShortestWay else: raise CanError( f"Invalid travel limits or modulo settings for axis {axis}. " diff --git a/pylabrobot/paa/kx2/kx2_driver.py b/pylabrobot/paa/kx2/kx2_driver.py index 8aba7a6873a..f1bb3097ab8 100644 --- a/pylabrobot/paa/kx2/kx2_driver.py +++ b/pylabrobot/paa/kx2/kx2_driver.py @@ -226,7 +226,7 @@ def __init__( # --- lifecycle ----------------------------------------------------------- async def setup(self, backend_params: Optional[BackendParams] = None) -> None: - """Bring up the CAN bus, reset & start all nodes, discover them.""" + """Bring up the CAN bus, reset/start nodes, and configure PDO mapping.""" if self._network is not None: await self.stop() @@ -263,27 +263,10 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: logger.info("canopen: connected, nodes=%s", discovered) - async def stop(self) -> None: - if self._network is not None: - self._network.disconnect() - self._network = None - self._nodes = {} - - # --- drive init (called by KX2ArmBackend._on_setup after setup()) -------- - - async def connect_part_two(self) -> None: - """Configure PDO mapping + Elmo DS402 parameters after the CAN bus is up. - - Unmap TPDO1, map TPDO3 (StatusWord, triggered on MotionComplete) and - TPDO4 (DigitalInputs, triggered on edge). TPDO3 is kept mapped to match - the C# reference config, but move completion is detected by polling MS - (the MotionComplete event is unreliable on short moves). Then program - Elmo vendor objects that set interpolation config, and finally map - RPDO1 (ControlWord) and RPDO3 (interpolated target position+velocity) - per motion axis. - """ - assert self._network is not None - + # Unmap TPDO1, map TPDO3 (StatusWord, triggered on MotionComplete) and + # TPDO4 (DigitalInputs, triggered on edge). TPDO3 is kept mapped to match + # the C# reference config, but move completion is detected by polling MS + # (the MotionComplete event is unreliable on short moves). for node_id in self.node_id_list: await self._can_tpdo_unmap(TPDO.TPDO1, node_id) await self._tpdo_map( @@ -293,6 +276,7 @@ async def connect_part_two(self) -> None: TPDO.TPDO4, node_id, [TPDOMappedObject.DigitalInputs], TPDOTrigger.DigitalInputEvent ) + # Elmo vendor objects: interpolation config for PVT mode. for nid in self.motion_node_ids: await self._can_sdo_download_elmo_object(nid, 24768, 0, -1, ElmoObjectDataType.INTEGER16) await self._can_sdo_download_elmo_object(nid, 24772, 2, 16, ElmoObjectDataType.UNSIGNED32) @@ -301,6 +285,7 @@ async def connect_part_two(self) -> None: await self._can_sdo_download_elmo_object(nid, 24770, 2, -3, ElmoObjectDataType.INTEGER8) await self._can_sdo_download_elmo_object(nid, 24669, 0, 1, ElmoObjectDataType.INTEGER16) + # RPDO1 = ControlWord (for DS402 enable), RPDO3 = interpolated target. for nid in self.motion_node_ids: await self._rpdo_map( RPDO.RPDO1, nid, [RPDOMappedObject.ControlWord], @@ -315,6 +300,12 @@ async def connect_part_two(self) -> None: self._pvt_mode = True await self._pvt_select_mode(False) + async def stop(self) -> None: + if self._network is not None: + self._network.disconnect() + self._network = None + self._nodes = {} + # --- PDO configuration (pure SDO writes; no library-PDO machinery) ------ async def _can_tpdo_unmap(self, tpdo: TPDO, node_id: int) -> None: @@ -942,7 +933,7 @@ async def user_program_run( f"last_line={last_line_completed}" ) - return last_line_completed + return 0 # --- I/O ----------------------------------------------------------------- From edefa191be66ee86b3041af7d8f718d014b7d7ca Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sat, 25 Apr 2026 14:41:52 -0700 Subject: [PATCH 34/93] restructure KX2 backend: split kinematics + config; native types end-to-end MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The kx2 module had grown into one ~1100-line backend file mixing CAN transport, axis-level procedural logic, IK/FK math, drive parameter storage, servo gripper choreography, and homing. Split it along its natural seams: - `arm_backend.py` (renamed from `kx2_backend.py`): only the capability-layer stuff — axis-to-node-id mapping, motion/rail/gripper `motor_enable` split, homing sequences, estop polling, motion planning. Owns no transport-layer or kinematics state. - `kinematics.py` (new): pure module — `ik`, `fk`, `snap_to_current`, `KX2GripperLocation` (the gripper-pose dataclass with optional cw/ccw wrist constraint). Closed-form math, no async, no driver awareness. Backend wraps it with closest-mode (fill `wrist=None` from current joint sign, then snap toward current). - `config.py` (new): `Axis` IntEnum (out of the nested `KX2ArmBackend.Axis` class), `AxisConfig`, `ServoGripperConfig`, `KX2Config`. Single source-of-truth dataclass populated once at setup; the backend stores it as `Optional[KX2Config]` and gates all reads through a `_cfg` property that raises pre-setup. Replaces ~20 loose `self._ax` dicts and a flat list of `self.` attributes that used to be scattered throughout `__init__`. - `driver.py` (renamed from `kx2_driver.py`): now a pure CAN/SDO/ DS402 transport. Drops the redundant `int()` casts on already-int parameters; methods used by the backend lose their leading underscore. - `kinematics_test.py` (new): 19 unit tests covering ik/fk round-trip, cw/ccw sign enforcement, the wrist-near-zero edge case, error paths (None / invalid wrist, x/y rotation), the FK wrist-field derivation, and `snap_to_current` (shifts toward current, doesn't touch prismatic axes, optional wrist-sign re- enforce, closest-mode = no re-enforce). Also adds `gripper_finger_side` (`"barcode_reader" | "proximity_sensor"`) on `KX2`/`KX2ArmBackend`/`KX2Config`. The fingers are physically symmetric, so flipping side just negates the gripper-length offset in the wrist frame — used by ik/fk to keep the world frame consistent regardless of which finger the user calls "front". External public surface (re-exported from `pylabrobot.paa.kx2`): `KX2`, `KX2ArmBackend`, `KX2Driver`, `KX2Config`, `Axis`, `KX2GripperLocation`. The previous nested `KX2ArmBackend.Axis` is gone in favor of the module-level enum; callers updated accordingly. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/__init__.py | 11 +- .../kx2/{kx2_backend.py => arm_backend.py} | 636 ++++++++---------- pylabrobot/paa/kx2/config.py | 105 +++ .../paa/kx2/{kx2_driver.py => driver.py} | 37 +- pylabrobot/paa/kx2/kinematics.py | 168 +++++ pylabrobot/paa/kx2/kinematics_test.py | 309 +++++++++ pylabrobot/paa/kx2/kx2.py | 7 +- 7 files changed, 886 insertions(+), 387 deletions(-) rename pylabrobot/paa/kx2/{kx2_backend.py => arm_backend.py} (58%) create mode 100644 pylabrobot/paa/kx2/config.py rename pylabrobot/paa/kx2/{kx2_driver.py => driver.py} (97%) create mode 100644 pylabrobot/paa/kx2/kinematics.py create mode 100644 pylabrobot/paa/kx2/kinematics_test.py diff --git a/pylabrobot/paa/kx2/__init__.py b/pylabrobot/paa/kx2/__init__.py index c92a2b82c91..86b1ce61967 100644 --- a/pylabrobot/paa/kx2/__init__.py +++ b/pylabrobot/paa/kx2/__init__.py @@ -1,8 +1,5 @@ +from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend +from pylabrobot.paa.kx2.config import Axis, KX2Config +from pylabrobot.paa.kx2.driver import KX2Driver +from pylabrobot.paa.kx2.kinematics import KX2GripperLocation from pylabrobot.paa.kx2.kx2 import KX2 -from pylabrobot.paa.kx2.kx2_backend import KX2ArmBackend -from pylabrobot.paa.kx2.kx2_barcode_reader import ( - KX2BarcodeReader, - KX2BarcodeReaderBackend, - KX2BarcodeReaderDriver, -) -from pylabrobot.paa.kx2.kx2_driver import KX2Driver diff --git a/pylabrobot/paa/kx2/kx2_backend.py b/pylabrobot/paa/kx2/arm_backend.py similarity index 58% rename from pylabrobot/paa/kx2/kx2_backend.py rename to pylabrobot/paa/kx2/arm_backend.py index 6e25c8be570..3099a8153ab 100644 --- a/pylabrobot/paa/kx2/kx2_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -1,4 +1,5 @@ import asyncio +import dataclasses import logging import math import warnings @@ -12,9 +13,16 @@ OrientableGripperArmBackend, ) from pylabrobot.capabilities.arms.standard import GripperLocation -from pylabrobot.capabilities.arms.standard import GripperLocation as GripperPose from pylabrobot.capabilities.capability import BackendParams -from pylabrobot.paa.kx2.kx2_driver import ( +from pylabrobot.paa.kx2 import kinematics +from pylabrobot.paa.kx2.config import ( + Axis, + AxisConfig, + GripperFingerSide, + KX2Config, + ServoGripperConfig, +) +from pylabrobot.paa.kx2.driver import ( CanError, JointMoveDirection, KX2Driver, @@ -44,52 +52,22 @@ class KX2ArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive): estop polling, etc. The driver underneath is a pure CAN transport. """ - class Axis(IntEnum): - """KX2 axis -> CANopen node-ID mapping. - - Lives here (not in the driver) because the driver is axis-agnostic and - deals only with node IDs. External code should reference - ``KX2ArmBackend.Axis``. - """ - - SHOULDER = 1 - Z = 2 - ELBOW = 3 - WRIST = 4 - RAIL = 5 - SERVO_GRIPPER = 6 def __init__( self, driver: KX2Driver, gripper_length: float = 0.0, gripper_z_offset: float = 0.0, + gripper_finger_side: GripperFingerSide = "barcode_reader", ) -> None: super().__init__() self.driver = driver - self.gripper_length = float(gripper_length) - self.gripper_z_offset = float(gripper_z_offset) - - self.digital_input_assignment = {} # TODO: just cache? - self.AnalogInputAssignment = {} - self.output_assignment = {} - self.motor_conversion_factor_ax = {} - self.max_travel_ax = {} - self.min_travel_ax = {} - self.unlimited_travel_ax = {} - self.absolute_encoder_ax = {} - self.max_vel_ax = {} - self.max_accel_ax = {} - - self.g_joint_move_direction = { - 1: JointMoveDirection.Normal, - 2: JointMoveDirection.Normal, - 3: JointMoveDirection.Normal, - 4: JointMoveDirection.Normal, - 6: JointMoveDirection.Normal, - } - - self.node_id_list = [1, 2, 3, 4, 6] + # Tooling offsets are user-supplied; everything else on the config is + # filled in from the drives during setup. + self._gripper_length = float(gripper_length) + self._gripper_z_offset = float(gripper_z_offset) + self._gripper_finger_side: GripperFingerSide = gripper_finger_side + self._config: Optional[KX2Config] = None async def _on_setup(self, backend_params: Optional[BackendParams] = None): # Driver has already brought CAN up (connect + node discovery + PDO @@ -101,7 +79,7 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): for axis in MOTION_AXES: try: - await self.driver.motor_enable(node_id=int(axis), state=True, use_ds402=True) + await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) except Exception as e: logger.warning("Error enabling motor on axis %s: %s", axis, e) @@ -115,39 +93,39 @@ async def get_estop_state(self) -> bool: Reads the shoulder drive's SR (status register) via the binary interpreter. Bits 14/15 encode the stop/safety state. """ - r = await self.driver.query_int(int(self.Axis.SHOULDER), "SR", 1) + r = await self.driver.query_int(int(Axis.SHOULDER), "SR", 1) if r != 8438016: logger.warning("get_estop_state: SR register unexpected value %d (expected 8438016)", r) b14 = (r & 0x4000) == 0x4000 b15 = (r & 0x8000) == 0x8000 return (not b14) and (not b15) - async def _motor_set_homed_status(self, axis: int, status: HomeStatus) -> None: - await self.driver.write(int(axis), "UI", 3, int(status)) + async def _motor_set_homed_status(self, axis: Axis, status: HomeStatus) -> None: + await self.driver.write(axis, "UI", 3, int(status)) - async def motor_get_homed_status(self, node_id: int) -> HomeStatus: - return HomeStatus(await self.driver.query_int(int(node_id), "UI", 3)) + async def motor_get_homed_status(self, axis: Axis) -> HomeStatus: + return HomeStatus(await self.driver.query_int(axis, "UI", 3)) - async def _motor_reset_encoder_position(self, axis: int, position: float) -> None: - await self.driver.write(int(axis), "HM", 1, 0) - await self.driver.write(int(axis), "HM", 3, 0) - await self.driver.write(int(axis), "HM", 4, 0) - await self.driver.write(int(axis), "HM", 5, 0) + async def _motor_reset_encoder_position(self, axis: Axis, position: float) -> None: + await self.driver.write(axis, "HM", 1, 0) + await self.driver.write(axis, "HM", 3, 0) + await self.driver.write(axis, "HM", 4, 0) + await self.driver.write(axis, "HM", 5, 0) # Old code packed `position` as int32 via `int(round(float(str(position))))`; # preserve that rounding semantic for callers that pass fractional values. - await self.driver.write(int(axis), "HM", 2, int(round(position))) - await self.driver.write(int(axis), "HM", 1, 1) + await self.driver.write(axis, "HM", 2, int(round(position))) + await self.driver.write(axis, "HM", 1, 1) async def _motor_hard_stop_search( self, - axis: int, + axis: Axis, srch_vel: int, srch_acc: int, max_pe: int, hs_pe: int, timeout: float, ) -> None: - nid = int(axis) + nid = axis await self.driver.write(nid, "ER", 3, max_pe * 10) await self.driver.write(nid, "AC", 0, srch_acc) await self.driver.write(nid, "DC", 0, srch_acc) @@ -176,13 +154,13 @@ async def _motor_hard_stop_search( async def _motor_index_search( self, - axis: int, + axis: Axis, srch_vel: int, srch_acc: int, positive_direction: bool, timeout: float, ) -> tuple: - nid = int(axis) + nid = axis await self.driver.write(nid, "HM", 1, 0) one_revolution = await self.driver.query_int(nid, "CA", 18) @@ -212,7 +190,7 @@ async def _motor_index_search( async def home_motor( self, - axis: int, + axis: Axis, hs_offset: int, ind_offset: int, home_pos: int, @@ -224,7 +202,7 @@ async def home_motor( offset_acc: int, timeout: float, ) -> None: - nid = int(axis) + nid = axis left = await self.driver.query_int(nid, "CA", 41) if left == 24: @@ -280,11 +258,11 @@ async def home_motor( async def servo_gripper_initialize(self): try: await self.driver.motor_enable( - node_id=int(self.Axis.SERVO_GRIPPER), state=True, use_ds402=False + node_id=int(Axis.SERVO_GRIPPER), state=True, use_ds402=False ) except Exception as e: logger.warning( - "Error enabling servo gripper motor on node %s: %s", self.Axis.SERVO_GRIPPER, e + "Error enabling servo gripper motor on node %s: %s", Axis.SERVO_GRIPPER, e ) await self.servo_gripper_home() @@ -292,36 +270,42 @@ async def servo_gripper_initialize(self): await self.servo_gripper_close() async def servo_gripper_home(self) -> None: - sg = int(self.Axis.SERVO_GRIPPER) - await self.driver.write(sg, "PL", 1, self.servo_gripper_peak_current) - await self.driver.write(sg, "CL", 1, self.servo_gripper_continuous_current) + sgc = self._cfg.servo_gripper + if sgc is None: + raise RuntimeError("Servo gripper not present") + sg = int(Axis.SERVO_GRIPPER) + await self.driver.write(sg, "PL", 1, sgc.peak_current) + await self.driver.write(sg, "CL", 1, sgc.continuous_current) await self.home_motor( - axis=self.Axis.SERVO_GRIPPER, - hs_offset=self.servo_gripper_home_hard_stop_offset, - ind_offset=self.servo_gripper_home_index_offset, - home_pos=self.servo_gripper_home_pos, - srch_vel=self.servo_gripper_home_search_vel, - srch_acc=self.servo_gripper_home_search_accel, - max_pe=self.servo_gripper_home_default_position_error, - hs_pe=self.servo_gripper_home_hard_stop_position_error, - offset_vel=self.servo_gripper_home_offset_vel, - offset_acc=self.servo_gripper_home_offset_accel, - timeout=self.servo_gripper_home_timeout_msec / 1000, + axis=Axis.SERVO_GRIPPER, + hs_offset=sgc.home_hard_stop_offset, + ind_offset=sgc.home_index_offset, + home_pos=sgc.home_pos, + srch_vel=sgc.home_search_vel, + srch_acc=sgc.home_search_accel, + max_pe=sgc.home_default_position_error, + hs_pe=sgc.home_hard_stop_position_error, + offset_vel=sgc.home_offset_vel, + offset_acc=sgc.home_offset_accel, + timeout=sgc.home_timeout_msec / 1000, ) await self.servo_gripper_set_default_gripping_force(100) async def servo_gripper_set_default_gripping_force(self, max_force_percent: int) -> None: + sgc = self._cfg.servo_gripper + if sgc is None: + raise RuntimeError("Servo gripper not present") max_force_percent = max(10, min(max_force_percent, 100)) - cont_current = self.servo_gripper_continuous_current * max_force_percent / 100.0 - peak_current = self.servo_gripper_peak_current * max_force_percent / 100.0 + cont_current = sgc.continuous_current * max_force_percent / 100.0 + peak_current = sgc.peak_current * max_force_percent / 100.0 - sg = int(self.Axis.SERVO_GRIPPER) + sg = int(Axis.SERVO_GRIPPER) # 1) PL with unscaled peak current - await self.driver.write(sg, "PL", 1, self.servo_gripper_peak_current) + await self.driver.write(sg, "PL", 1, sgc.peak_current) # 2) CL with scaled continuous current await self.driver.write(sg, "CL", 1, cont_current) @@ -329,11 +313,9 @@ async def servo_gripper_set_default_gripping_force(self, max_force_percent: int) # 3) PL with scaled peak current await self.driver.write(sg, "PL", 1, peak_current) - self.servo_gripper_force_percent = max_force_percent - async def get_servo_gripper_max_force(self) -> float: """Return current gripping force as percentage of max (0-1).""" - sg = int(self.Axis.SERVO_GRIPPER) + sg = int(Axis.SERVO_GRIPPER) cl = await self.driver.query_float(sg, "CL", 1) iq = await self.driver.query_float(sg, "IQ", 0) @@ -345,7 +327,7 @@ async def get_servo_gripper_max_force(self) -> float: async def check_plate_gripped(self, num_attempts: int = 5) -> None: for _ in range(num_attempts): motor_status = await self.driver.query_int( - int(self.Axis.SERVO_GRIPPER), "MS", 1 + int(Axis.SERVO_GRIPPER), "MS", 1 ) logger.debug("Servo gripper motor status: %s", motor_status) @@ -358,7 +340,7 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: if max_force_percentage > 90: return - current_position = await self.motor_get_current_position(self.Axis.SERVO_GRIPPER) + current_position = await self.motor_get_current_position(Axis.SERVO_GRIPPER) closed_position = 1 if abs(current_position - closed_position) < 2.0 / 625: raise RuntimeError( @@ -368,7 +350,7 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: return elif motor_status == 2: - motor_fault = await self.driver.motor_get_fault(int(self.Axis.SERVO_GRIPPER)) + motor_fault = await self.driver.motor_get_fault(int(Axis.SERVO_GRIPPER)) if motor_fault is None: raise RuntimeError("Error querying whether plate is gripped. Error querying motor fault.") raise RuntimeError( @@ -383,7 +365,7 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: async def servo_gripper_close(self, closed_position: int = 0, check_plate_gripped=True) -> None: await self.motors_move_joint( - {self.Axis.SERVO_GRIPPER: closed_position}, + {Axis.SERVO_GRIPPER: closed_position}, cmd_vel_pct=100, cmd_accel_pct=100, ) @@ -393,7 +375,7 @@ async def servo_gripper_close(self, closed_position: int = 0, check_plate_grippe async def servo_gripper_open(self, open_position: float) -> None: await self.motors_move_joint( - {self.Axis.SERVO_GRIPPER: open_position}, + {Axis.SERVO_GRIPPER: open_position}, cmd_vel_pct=100, cmd_accel_pct=100, ) @@ -409,20 +391,20 @@ async def drive_set_move_count_parameters( maintenance_required_rail: bool, last_maintenance_performed_date_rail: int, ) -> None: - z = int(self.Axis.Z) + z = int(Axis.Z) # MoveCount -> Z axis, UI index 22 await self.driver.write(z, "UI", 22, int(move_count)) # Travel[] -> each node, UF index 5 # The source looked 1-based for Travel and 0-based for NodeIDList; handle both cleanly. - if len(travel) == len(self.node_id_list) + 1: - pairs = zip(self.node_id_list, travel[1:]) + if len(travel) == len(self._cfg.axes) + 1: + pairs = zip(self._cfg.axes, travel[1:]) else: - pairs = zip(self.node_id_list, travel) + pairs = zip(self._cfg.axes, travel) for node_id, dist in pairs: - await self.driver.write(int(node_id), "UF", 5, float(dist)) + await self.driver.write(node_id, "UF", 5, float(dist)) # LastMaintenancePerformed -> Z axis, UF index 6 await self.driver.write(z, "UF", 6, float(last_maintenance_performed)) @@ -434,25 +416,19 @@ async def drive_set_move_count_parameters( await self.driver.write(z, "UI", 21, int(last_maintenance_performed_date)) # Rail (if present) - if self.robot_on_rail: - rail = int(self.Axis.RAIL) + if self._cfg.robot_on_rail: + rail = int(Axis.RAIL) await self.driver.write(rail, "UF", 6, float(last_maintenance_performed_rail)) await self.driver.write(rail, "UI", 23, 1 if maintenance_required_rail else 0) await self.driver.write(rail, "UI", 21, int(last_maintenance_performed_date_rail)) async def drive_get_parameters(self, node_ids) -> None: - self.robot_on_rail = False - self.has_servo_gripper = False - nodes = ( [int(b) for b in node_ids] if isinstance(node_ids, (bytes, bytearray)) else [int(x) for x in node_ids] ) - def set2d(store: dict, axis: int, ch: int, value: str) -> None: - store.setdefault(axis, {})[ch] = value - # Pass 1: identify axes by UI[4] uis = set() for node in nodes: @@ -461,107 +437,78 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: for required_axis in MOTION_AXES: if required_axis.value not in uis: raise CanError(f"Missing required axis with UI[4]={required_axis}") - if 5 in uis: - self.robot_on_rail = True + robot_on_rail = 5 in uis + has_servo_gripper = 6 in uis + if robot_on_rail: warnings.warn("Rails has not been tested for KX2 robots.") - if 6 in uis: - self.has_servo_gripper = True # Pass 2: per-axis parameters + axes: Dict[int, AxisConfig] = {} for axis in nodes: logger.debug("Reading parameters for axis %s", axis) # UI[5..10] digital inputs + digital_inputs: Dict[int, str] = {} for ui_idx in range(5, 11): - ret = await self.driver.query_int(int(axis), "UI", ui_idx) + ret = await self.driver.query_int(axis, "UI", ui_idx) ch = (ui_idx - 5) + 1 if ret == 101: - set2d(self.digital_input_assignment, axis, ch, "GripperSensor") - self.GripperSensorAxis = axis - self.GripperSensorInput = ch + digital_inputs[ch] = "GripperSensor" elif ret == 102: - set2d(self.digital_input_assignment, axis, ch, "TeachButton") - self.TeachButtonAxis = axis - self.TeachButtonInput = ch + digital_inputs[ch] = "TeachButton" else: - set2d( - self.digital_input_assignment, - axis, - ch, - "" if ret <= 0 else f"AuxPin{ret}", - ) + digital_inputs[ch] = "" if ret <= 0 else f"AuxPin{ret}" # UI[11..12] analog inputs + analog_inputs: Dict[int, str] = {} for ui_idx in range(11, 13): - ret = await self.driver.query_int(int(axis), "UI", ui_idx) + ret = await self.driver.query_int(axis, "UI", ui_idx) ch = (ui_idx - 11) + 1 - set2d( - self.AnalogInputAssignment, - axis, - ch, - "" if ret <= 0 else f"AuxPin{ret}", - ) + analog_inputs[ch] = "" if ret <= 0 else f"AuxPin{ret}" # UI[13..16] outputs + outputs: Dict[int, str] = {} for ui_idx in range(13, 17): - ret = await self.driver.query_int(int(axis), "UI", ui_idx) + ret = await self.driver.query_int(axis, "UI", ui_idx) ch = (ui_idx - 13) + 1 - if ret == 101: - set2d(self.output_assignment, axis, ch, "IndicatorLightRed") - self.IndicatorLightRedAxis = axis - self.IndicatorLightRedOutput = ch + outputs[ch] = "IndicatorLightRed" elif ret == 102: - set2d(self.output_assignment, axis, ch, "IndicatorLightGreen") - self.IndicatorLightGreenAxis = axis - self.IndicatorLightGreenOutput = ch + outputs[ch] = "IndicatorLightGreen" elif ret == 103: - set2d(self.output_assignment, axis, ch, "IndicatorLightBlue") - self.IndicatorLightBlueAxis = axis - self.IndicatorLightBlueOutput = ch + outputs[ch] = "IndicatorLightBlue" elif ret == 104: - set2d(self.output_assignment, axis, ch, "IndicatorLight") - self.IndicatorLightAxis = axis - self.IndicatorLightOutput = ch + outputs[ch] = "IndicatorLight" elif ret == 105: - set2d(self.output_assignment, axis, ch, "Buzzer") - self.BuzzerAxis = axis - self.BuzzerOutput = ch + outputs[ch] = "Buzzer" else: - set2d( - self.output_assignment, - axis, - ch, - "" if ret <= 0 else f"AuxPin{ret}", - ) + outputs[ch] = "" if ret <= 0 else f"AuxPin{ret}" # UI[24] drive serial number (queried but not currently stored) - await self.driver.query_int(int(axis), "UI", 24) + await self.driver.query_int(axis, "UI", 24) # UF[1], UF[2] conversion factor - uf1 = await self.driver.query_float(int(axis), "UF", 1) - uf2 = await self.driver.query_float(int(axis), "UF", 2) + uf1 = await self.driver.query_float(axis, "UF", 1) + uf2 = await self.driver.query_float(axis, "UF", 2) if uf1 == 0.0 or uf2 == 0.0: raise CanError(f"Invalid Motor Conversion Factor for axis {axis}. UF[1]={uf1}, UF[2]={uf2}") - self.motor_conversion_factor_ax[axis] = uf1 / uf2 + motor_conversion_factor = uf1 / uf2 # XM / travel - xm1 = await self.driver.query_int(int(axis), "XM", 1) - xm2 = await self.driver.query_int(int(axis), "XM", 2) - uf3 = await self.driver.query_float(int(axis), "UF", 3) - uf4 = await self.driver.query_float(int(axis), "UF", 4) - vh3 = await self.driver.query_int(int(axis), "VH", 3) - vl3 = await self.driver.query_int(int(axis), "VL", 3) - - self.max_travel_ax[axis] = uf3 - self.min_travel_ax[axis] = uf4 - - if ((xm1 == 0) and (xm2 == 0)) or ((xm1 <= vl3) and (xm2 >= vh3)): - self.unlimited_travel_ax[axis] = False - elif (xm1 > vl3) and (xm2 < vh3): - self.unlimited_travel_ax[axis] = True + xm1 = await self.driver.query_int(axis, "XM", 1) + xm2 = await self.driver.query_int(axis, "XM", 2) + max_travel = await self.driver.query_float(axis, "UF", 3) + min_travel = await self.driver.query_float(axis, "UF", 4) + vh3 = await self.driver.query_int(axis, "VH", 3) + vl3 = await self.driver.query_int(axis, "VL", 3) + + joint_move_direction = JointMoveDirection.Normal + if (xm1 == 0 and xm2 == 0) or (xm1 <= vl3 and xm2 >= vh3): + unlimited_travel = False + elif xm1 > vl3 and xm2 < vh3: + unlimited_travel = True if axis in MOTION_AXES: - self.g_joint_move_direction[axis] = JointMoveDirection.ShortestWay + joint_move_direction = JointMoveDirection.ShortestWay else: raise CanError( f"Invalid travel limits or modulo settings for axis {axis}. " @@ -569,101 +516,126 @@ def set2d(store: dict, axis: int, ch: int, value: str) -> None: ) # Encoder socket/type - ca45 = await self.driver.query_int(int(axis), "CA", 45) + ca45 = await self.driver.query_int(axis, "CA", 45) if not (0 < ca45 <= 4): raise CanError(f"Invalid encoder socket number received from axis {axis}. CA[45]={ca45}") - - enc_type = await self.driver.query_int(int(axis), "CA", 40 + ca45) + enc_type = await self.driver.query_int(axis, "CA", 40 + ca45) if enc_type in (1, 2): - self.absolute_encoder_ax[axis] = False + absolute_encoder = False elif enc_type == 24: - self.absolute_encoder_ax[axis] = True + absolute_encoder = True else: raise CanError( f"Unsupported encoder type specified for axis {axis}. CA[4{ca45}]={enc_type}" ) - ca46 = await self.driver.query_int(int(axis), "CA", 46) + ca46 = await self.driver.query_int(axis, "CA", 46) if ca45 == ca46: num3 = 1.0 else: - num3 = await self.driver.query_float(int(axis), "FF", 3) + num3 = await self.driver.query_float(axis, "FF", 3) - denom = self.motor_conversion_factor_ax[axis] * num3 # or 1.0 + denom = motor_conversion_factor * num3 - sp2 = await self.driver.query_int(int(axis), "SP", 2) + sp2 = await self.driver.query_int(axis, "SP", 2) if sp2 == 100000: - vh2 = await self.driver.query_int(int(axis), "VH", 2) - self.max_vel_ax[axis] = vh2 / 1.01 / denom + vh2 = await self.driver.query_int(axis, "VH", 2) + max_vel = vh2 / 1.01 / denom else: - self.max_vel_ax[axis] = sp2 / denom - - sd0 = await self.driver.query_int(int(axis), "SD", 0) - self.max_accel_ax[axis] = sd0 / 1.01 / denom - - # Robot-level params from shoulder_ax - shoulder = int(self.Axis.SHOULDER) - - self.base_to_gripper_clearance_z = await self.driver.query_float(shoulder, "UF", 6) - self.base_to_gripper_clearance_arm = await self.driver.query_float(shoulder, "UF", 7) - self.wrist_offset = await self.driver.query_float(shoulder, "UF", 8) - self.elbow_offset = await self.driver.query_float(shoulder, "UF", 9) - self.elbow_zero_offset = await self.driver.query_float(shoulder, "UF", 10) - self.MaxLinearVelMMPerSec = await self.driver.query_float(shoulder, "UF", 11) - self.MaxLinearAccelMMPerSec2 = await self.driver.query_float(shoulder, "UF", 12) - self.MaxLinearJerkMMPerSec3 = await self.driver.query_float(shoulder, "UF", 13) - self.MaxRotaryVelDegPerSec = await self.driver.query_float(shoulder, "UF", 14) - self.MaxRotaryAccelDegPerSec2 = await self.driver.query_float(shoulder, "UF", 15) - - ui17 = await self.driver.query_int(shoulder, "UI", 17) - self.pvt_time_interval_msec = ( - 25 if (ui17 <= 0 or ui17 > 255) else ui17 + max_vel = sp2 / denom + + sd0 = await self.driver.query_int(axis, "SD", 0) + max_accel = sd0 / 1.01 / denom + + axes[axis] = AxisConfig( + motor_conversion_factor=motor_conversion_factor, + max_travel=max_travel, + min_travel=min_travel, + unlimited_travel=unlimited_travel, + absolute_encoder=absolute_encoder, + max_vel=max_vel, + max_accel=max_accel, + joint_move_direction=joint_move_direction, + digital_inputs=digital_inputs, + analog_inputs=analog_inputs, + outputs=outputs, + ) + + # Robot-level params from shoulder. + shoulder = int(Axis.SHOULDER) + base_to_gripper_clearance_z = await self.driver.query_float(shoulder, "UF", 6) + base_to_gripper_clearance_arm = await self.driver.query_float(shoulder, "UF", 7) + wrist_offset = await self.driver.query_float(shoulder, "UF", 8) + elbow_offset = await self.driver.query_float(shoulder, "UF", 9) + elbow_zero_offset = await self.driver.query_float(shoulder, "UF", 10) + + servo_gripper: Optional[ServoGripperConfig] = None + if has_servo_gripper: + sg = int(Axis.SERVO_GRIPPER) + servo_gripper = ServoGripperConfig( + home_pos=int(await self.driver.query_float(sg, "UF", 6)), + home_search_vel=int(await self.driver.query_float(sg, "UF", 7)), + home_search_accel=int(await self.driver.query_float(sg, "UF", 8)), + home_default_position_error=int(await self.driver.query_float(sg, "UF", 9)), + home_hard_stop_position_error=int(await self.driver.query_float(sg, "UF", 10)), + home_hard_stop_offset=int(await self.driver.query_float(sg, "UF", 11)), + home_index_offset=int(await self.driver.query_float(sg, "UF", 12)), + home_offset_vel=int(await self.driver.query_float(sg, "UF", 13)), + home_offset_accel=int(await self.driver.query_float(sg, "UF", 14)), + home_timeout_msec=int(await self.driver.query_float(sg, "UF", 15)), + continuous_current=await self.driver.query_float(sg, "UF", 16), + peak_current=await self.driver.query_float(sg, "UF", 17), + ) + + self._config = KX2Config( + wrist_offset=wrist_offset, + elbow_offset=elbow_offset, + elbow_zero_offset=elbow_zero_offset, + gripper_length=self._gripper_length, + gripper_z_offset=self._gripper_z_offset, + gripper_finger_side=self._gripper_finger_side, + axes=axes, + base_to_gripper_clearance_z=base_to_gripper_clearance_z, + base_to_gripper_clearance_arm=base_to_gripper_clearance_arm, + robot_on_rail=robot_on_rail, + servo_gripper=servo_gripper, ) - # Servo gripper params (only if present) - sg = int(self.Axis.SERVO_GRIPPER) - self.servo_gripper_home_pos = int(await self.driver.query_float(sg, "UF", 6)) - self.servo_gripper_home_search_vel = int(await self.driver.query_float(sg, "UF", 7)) - self.servo_gripper_home_search_accel = int(await self.driver.query_float(sg, "UF", 8)) - self.servo_gripper_home_default_position_error = int(await self.driver.query_float(sg, "UF", 9)) - self.servo_gripper_home_hard_stop_position_error = int(await self.driver.query_float(sg, "UF", 10)) - self.servo_gripper_home_hard_stop_offset = int(await self.driver.query_float(sg, "UF", 11)) - self.servo_gripper_home_index_offset = int(await self.driver.query_float(sg, "UF", 12)) - self.servo_gripper_home_offset_vel = int(await self.driver.query_float(sg, "UF", 13)) - self.servo_gripper_home_offset_accel = int(await self.driver.query_float(sg, "UF", 14)) - self.servo_gripper_home_timeout_msec = int(await self.driver.query_float(sg, "UF", 15)) - self.servo_gripper_continuous_current = await self.driver.query_float(sg, "UF", 16) - self.servo_gripper_peak_current = await self.driver.query_float(sg, "UF", 17) + @property + def _cfg(self) -> KX2Config: + if self._config is None: + raise RuntimeError("KX2 not set up — call setup() first") + return self._config def convert_elbow_position_to_angle(self, elbow_pos: float) -> float: - max_travel = self.max_travel_ax[self.Axis.ELBOW] - denom = max_travel + self.elbow_zero_offset + max_travel = self._cfg.axes[Axis.ELBOW].max_travel + denom = max_travel + self._cfg.elbow_zero_offset if elbow_pos > max_travel: - x = (2.0 * max_travel - elbow_pos + self.elbow_zero_offset) / denom + x = (2.0 * max_travel - elbow_pos + self._cfg.elbow_zero_offset) / denom angle = math.asin(x) * (180.0 / math.pi) elbow_angle = 90.0 + angle else: - x = (elbow_pos + self.elbow_zero_offset) / denom + x = (elbow_pos + self._cfg.elbow_zero_offset) / denom angle = math.asin(x) * (180.0 / math.pi) elbow_angle = angle return elbow_angle def convert_elbow_angle_to_position(self, elbow_angle_deg: float) -> float: - elbow_pos = (self.max_travel_ax[self.Axis.ELBOW] + self.elbow_zero_offset) * math.sin( + elbow_pos = (self._cfg.axes[Axis.ELBOW].max_travel + self._cfg.elbow_zero_offset) * math.sin( elbow_angle_deg * (math.pi / 180.0) - ) - self.elbow_zero_offset + ) - self._cfg.elbow_zero_offset if elbow_angle_deg > 90.0: - elbow_pos = 2.0 * self.max_travel_ax[self.Axis.ELBOW] - elbow_pos + elbow_pos = 2.0 * self._cfg.axes[Axis.ELBOW].max_travel - elbow_pos return elbow_pos - async def motor_get_current_position(self, axis: "KX2ArmBackend.Axis") -> float: - raw = await self.driver.motor_get_current_position(node_id=int(axis), pu=self.unlimited_travel_ax[axis]) - c = self.motor_conversion_factor_ax[axis] - if axis == self.Axis.ELBOW: + async def motor_get_current_position(self, axis: Axis) -> float: + raw = await self.driver.motor_get_current_position(node_id=axis, pu=self._cfg.axes[axis].unlimited_travel) + c = self._cfg.axes[axis].motor_conversion_factor + if axis == Axis.ELBOW: return self.convert_elbow_angle_to_position(elbow_angle_deg=raw / c) else: if c == 0: @@ -672,7 +644,7 @@ async def motor_get_current_position(self, axis: "KX2ArmBackend.Axis") -> float: else: return raw / c - async def read_input(self, axis: int, input_num: int) -> bool: + async def read_input(self, axis: Axis, input_num: int) -> bool: return await self.driver.read_input(node_id=axis, input_num=0x10 + input_num) @staticmethod @@ -718,17 +690,17 @@ def _profile(dist: float, v: float, a: float) -> tuple: async def calculate_move_abs_all_axes( self, - cmd_pos: Dict["KX2ArmBackend.Axis", float], + cmd_pos: Dict[Axis, float], cmd_vel_pct: float, cmd_accel_pct: float, ) -> Optional[MotorsMovePlan]: target = cmd_pos.copy() axes = list(target.keys()) - enc_pos: Dict[KX2ArmBackend.Axis, float] = {} - enc_vel: Dict[KX2ArmBackend.Axis, float] = {} - enc_accel: Dict[KX2ArmBackend.Axis, float] = {} - skip_ax: Dict[KX2ArmBackend.Axis, bool] = {} + enc_pos: Dict[Axis, float] = {} + enc_vel: Dict[Axis, float] = {} + enc_accel: Dict[Axis, float] = {} + skip_ax: Dict[Axis, bool] = {} # input validation / travel limits / done-wait logic if cmd_vel_pct <= 0.0 or cmd_vel_pct > 100.0: @@ -737,14 +709,14 @@ async def calculate_move_abs_all_axes( raise ValueError("CmdAccel out of range") # Convert elbow cmd from position->angle for planning math - if self.Axis.ELBOW in axes: - target[self.Axis.ELBOW] = self.convert_elbow_position_to_angle(target[self.Axis.ELBOW]) + if Axis.ELBOW in axes: + target[Axis.ELBOW] = self.convert_elbow_position_to_angle(target[Axis.ELBOW]) # Clearance check - if self.Axis.Z in axes: + if Axis.Z in axes: if ( - target[self.Axis.Z] < self.min_travel_ax[self.Axis.Z] + self.base_to_gripper_clearance_z - and target[self.Axis.ELBOW] < self.base_to_gripper_clearance_arm + target[Axis.Z] < self._cfg.axes[Axis.Z].min_travel + self._cfg.base_to_gripper_clearance_z + and target[Axis.ELBOW] < self._cfg.base_to_gripper_clearance_arm ): raise ValueError("Base-to-gripper clearance violated") @@ -752,29 +724,29 @@ async def calculate_move_abs_all_axes( curr = await self.request_joint_position() # Elbow: convert both target and start to angle for distance math - if self.Axis.ELBOW in curr: - curr[self.Axis.ELBOW] = self.convert_elbow_position_to_angle(curr[self.Axis.ELBOW]) + if Axis.ELBOW in curr: + curr[Axis.ELBOW] = self.convert_elbow_position_to_angle(curr[Axis.ELBOW]) # Handle unlimited travel normalization when direction != NORMAL for ax in axes: if ( - self.unlimited_travel_ax[ax] - and self.g_joint_move_direction[ax] != JointMoveDirection.Normal + self._cfg.axes[ax].unlimited_travel + and self._cfg.axes[ax].joint_move_direction != JointMoveDirection.Normal ): - target[ax] = self._wrap_to_range(target[ax], self.min_travel_ax[ax], self.max_travel_ax[ax]) + target[ax] = self._wrap_to_range(target[ax], self._cfg.axes[ax].min_travel, self._cfg.axes[ax].max_travel) # Distances, skip flags, initial v/a per axis - dist: Dict[KX2ArmBackend.Axis, float] = {} - v: Dict[KX2ArmBackend.Axis, float] = {} - a: Dict[KX2ArmBackend.Axis, float] = {} - accel_time: Dict[KX2ArmBackend.Axis, float] = {} - total_time: Dict[KX2ArmBackend.Axis, float] = {} + dist: Dict[Axis, float] = {} + v: Dict[Axis, float] = {} + a: Dict[Axis, float] = {} + accel_time: Dict[Axis, float] = {} + total_time: Dict[Axis, float] = {} for ax in axes: - if self.unlimited_travel_ax[ax]: + if self._cfg.axes[ax].unlimited_travel: d = target[ax] - curr[ax] - span = self.max_travel_ax[ax] - self.min_travel_ax[ax] - dir_ = self.g_joint_move_direction[ax] + span = self._cfg.axes[ax].max_travel - self._cfg.axes[ax].min_travel + dir_ = self._cfg.axes[ax].joint_move_direction if dir_ == JointMoveDirection.Clockwise and d > 0.01: d -= span @@ -792,8 +764,8 @@ async def calculate_move_abs_all_axes( skip_ax[ax] = abs(dist[ax]) < 0.01 - v[ax] = (cmd_vel_pct / 100.0) * self.max_vel_ax[ax] - a[ax] = (cmd_accel_pct / 100.0) * self.max_accel_ax[ax] + v[ax] = (cmd_vel_pct / 100.0) * self._cfg.axes[ax].max_vel + a[ax] = (cmd_accel_pct / 100.0) * self._cfg.axes[ax].max_accel if not skip_ax[ax] and a[ax] > 0: accel_time[ax] = v[ax] / a[ax] @@ -853,7 +825,7 @@ async def calculate_move_abs_all_axes( # Convert back to encoder units (and elbow back to "position" space) for ax in axes: - conv = self.motor_conversion_factor_ax[ax] + conv = self._cfg.axes[ax].motor_conversion_factor enc_pos[ax] = target[ax] * conv if skip_ax[ax]: @@ -870,7 +842,7 @@ async def calculate_move_abs_all_axes( position=int(round(enc_pos[ax])), velocity=int(round(enc_vel[ax])), acceleration=int(round(enc_accel[ax])), - direction=self.g_joint_move_direction[ax], + direction=self._cfg.axes[ax].joint_move_direction, ) for ax in axes ], @@ -879,7 +851,7 @@ async def calculate_move_abs_all_axes( async def motors_move_joint( self, - cmd_pos: Dict["KX2ArmBackend.Axis", float], + cmd_pos: Dict[Axis, float], cmd_vel_pct: float, cmd_accel_pct: float, ) -> None: @@ -895,86 +867,29 @@ async def motors_move_joint( await self.driver.motors_move_absolute_execute(plan) - def convert_cartesian_to_joint_position(self, pose: GripperPose) -> Dict["KX2ArmBackend.Axis", float]: - if pose.rotation.x != 0 or pose.rotation.y != 0: - raise ValueError("Only Z rotation is supported for KX2") - - # Gripper -> wrist: the incoming pose describes the gripper clamp point; - # the joint-space math operates on the wrist axis. Rigid offset with the - # gripper length on the radial axis (governed by world rotation z) and the - # gripper z offset downward. - ang = math.radians(pose.rotation.z) - x = pose.location.x - self.gripper_length * math.sin(ang) - y = pose.location.y + self.gripper_length * math.cos(ang) - wrist_z = pose.location.z + self.gripper_z_offset - - joint_position: Dict[KX2ArmBackend.Axis, float] = {} + async def _cart_to_joints( + self, pose: kinematics.KX2GripperLocation + ) -> Dict[Axis, float]: + """Cartesian -> joints with closest-solution semantics. - # Shoulder axis - shoulder = -math.degrees(math.atan2(x, y)) - if abs(shoulder + 180.0) < 1e-12: - shoulder = 180.0 - - joint_position[self.Axis.SHOULDER] = shoulder - - # Z axis - joint_position[self.Axis.Z] = wrist_z - - # Elbow axis - elbow = ( - math.sqrt(x * x + y * y) - self.wrist_offset - self.elbow_offset - self.elbow_zero_offset - ) - joint_position[self.Axis.ELBOW] = elbow - - # Wrist axis - wrist = (pose.rotation.z) - joint_position[self.Axis.SHOULDER] - joint_position[self.Axis.WRIST] = wrist - - # Wrap wrist into travel range if possible by +/- 360 - w = joint_position[self.Axis.WRIST] - wmin = self.min_travel_ax[self.Axis.WRIST] - wmax = self.max_travel_ax[self.Axis.WRIST] - if (w < wmin - 0.001) and (w + 360.0 <= wmax): - w += 360.0 - elif (w > wmax + 0.001) and (w - 360.0 >= wmin): - w -= 360.0 - joint_position[self.Axis.WRIST] = w - - return joint_position - - def convert_joint_position_to_cartesian( - self, joint_position: Dict["KX2ArmBackend.Axis", float] - ) -> GripperPose: - r = ( - self.wrist_offset + self.elbow_offset + self.elbow_zero_offset + joint_position[self.Axis.ELBOW] - ) - sh_deg = joint_position[self.Axis.SHOULDER] - sh = math.radians(sh_deg) - - wrist_x = -(r) * math.sin(sh) - wrist_y = (r) * math.cos(sh) - wrist_z = joint_position[self.Axis.Z] - - rotation_z = joint_position[self.Axis.WRIST] + sh_deg - - # wrap to [-180, 180] - if rotation_z > 180.0: - rotation_z -= 360.0 - if rotation_z < -180.0: - rotation_z += 360.0 - - # Wrist -> gripper: inverse of the gripper -> wrist translation in - # convert_cartesian_to_joint_position so callers observe the gripper clamp - # point, symmetric with what they pass in. - ang = math.radians(rotation_z) - gripper_x = wrist_x + self.gripper_length * math.sin(ang) - gripper_y = wrist_y - self.gripper_length * math.cos(ang) - gripper_z = wrist_z - self.gripper_z_offset - - return GripperPose( - location=Coordinate(x=gripper_x, y=gripper_y, z=gripper_z), - rotation=Rotation(z=rotation_z), + If `pose.wrist` is None, fills it with the current joint's wrist sign + so the arm picks whichever IK solution needs the least motion. Then + snaps each rotary axis to the nearest 360° multiple of the current + position, re-enforcing the wrist sign afterward. + """ + current = await self.request_joint_position() + current_int = {int(k): v for k, v in current.items()} + # IK needs an explicit cw/ccw; for closest mode fill from the current + # joint's sign so IK has a valid choice. Snap then runs with the + # *original* pose.wrist — None disables sign re-enforce so the snap + # actually picks the closest J4. + ik_wrist = pose.wrist if pose.wrist is not None else ( + "ccw" if current_int[Axis.WRIST] >= 0 else "cw" ) + resolved = dataclasses.replace(pose, wrist=ik_wrist) + ik_joints = kinematics.ik(resolved, self._cfg) + snapped = kinematics.snap_to_current(ik_joints, current_int, pose.wrist) + return {Axis(k): v for k, v in snapped.items()} # -- capability interface (OrientableGripperArmBackend + HasJoints + CanFreedrive) -- @@ -994,7 +909,7 @@ class GripParams(BackendParams): async def halt(self, backend_params: Optional[BackendParams] = None) -> None: for axis in MOTION_AXES: - await self.driver.motor_emergency_stop(node_id=int(axis)) + await self.driver.motor_emergency_stop(node_id=axis) async def park(self, backend_params: Optional[BackendParams] = None) -> None: raise NotImplementedError( @@ -1005,14 +920,13 @@ async def park(self, backend_params: Optional[BackendParams] = None) -> None: async def request_gripper_location( self, backend_params: Optional[BackendParams] = None ) -> GripperLocation: - current_joint_pos = await self.request_joint_position() - return self.convert_joint_position_to_cartesian(current_joint_pos) + return kinematics.fk(await self.request_joint_position(), self._cfg) async def open_gripper( self, gripper_width: float, backend_params: Optional[BackendParams] = None ) -> None: await self.motors_move_joint( - {self.Axis.SERVO_GRIPPER: gripper_width}, + {Axis.SERVO_GRIPPER: gripper_width}, cmd_vel_pct=100, cmd_accel_pct=100, ) @@ -1023,7 +937,7 @@ async def close_gripper( if not isinstance(backend_params, KX2ArmBackend.GripParams): backend_params = KX2ArmBackend.GripParams() await self.motors_move_joint( - {self.Axis.SERVO_GRIPPER: gripper_width}, + {Axis.SERVO_GRIPPER: gripper_width}, cmd_vel_pct=100, cmd_accel_pct=100, ) @@ -1031,25 +945,39 @@ async def close_gripper( await self.check_plate_gripped() async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool: - pos = await self.motor_get_current_position(self.Axis.SERVO_GRIPPER) + pos = await self.motor_get_current_position(Axis.SERVO_GRIPPER) return abs(pos) < 1.0 - async def move_to_location( + async def move_to_gripper_location( self, - location: Coordinate, - direction: float, + pose: kinematics.KX2GripperLocation, backend_params: Optional[BackendParams] = None, ) -> None: + """Cartesian move with optional explicit wrist sign. + + `pose.wrist`: "cw" or "ccw" picks the wrist solution explicitly; None + falls back to the closest-to-current solution (same as `move_to_location`). + """ if not isinstance(backend_params, KX2ArmBackend.CartesianMoveParams): backend_params = KX2ArmBackend.CartesianMoveParams() - pose = GripperLocation(location=location, rotation=Rotation(z=direction)) - joint_pos = self.convert_cartesian_to_joint_position(pose) + joint_pos = await self._cart_to_joints(pose) await self.motors_move_joint( cmd_pos=joint_pos, cmd_vel_pct=backend_params.vel_pct, cmd_accel_pct=backend_params.accel_pct, ) + async def move_to_location( + self, + location: Coordinate, + direction: float, + backend_params: Optional[BackendParams] = None, + ) -> None: + pose = kinematics.KX2GripperLocation( + location=location, rotation=Rotation(z=direction), wrist=None + ) + await self.move_to_gripper_location(pose, backend_params=backend_params) + async def pick_up_at_location( self, location: Coordinate, @@ -1077,7 +1005,7 @@ async def move_to_joint_position( ) -> None: if not isinstance(backend_params, KX2ArmBackend.JointMoveParams): backend_params = KX2ArmBackend.JointMoveParams() - cmd_pos = {self.Axis(int(k)): float(v) for k, v in position.items()} + cmd_pos = {Axis(int(k)): float(v) for k, v in position.items()} await self.motors_move_joint( cmd_pos=cmd_pos, cmd_vel_pct=backend_params.vel_pct, @@ -1106,11 +1034,11 @@ async def request_joint_position( self, backend_params: Optional[BackendParams] = None ) -> Dict[int, float]: return { - self.Axis.SHOULDER: await self.motor_get_current_position(self.Axis.SHOULDER), - self.Axis.Z: await self.motor_get_current_position(self.Axis.Z), - self.Axis.ELBOW: await self.motor_get_current_position(self.Axis.ELBOW), - self.Axis.WRIST: await self.motor_get_current_position(self.Axis.WRIST), - self.Axis.SERVO_GRIPPER: await self.motor_get_current_position(self.Axis.SERVO_GRIPPER), + Axis.SHOULDER: await self.motor_get_current_position(Axis.SHOULDER), + Axis.Z: await self.motor_get_current_position(Axis.Z), + Axis.ELBOW: await self.motor_get_current_position(Axis.ELBOW), + Axis.WRIST: await self.motor_get_current_position(Axis.WRIST), + Axis.SERVO_GRIPPER: await self.motor_get_current_position(Axis.SERVO_GRIPPER), } async def start_freedrive_mode( @@ -1119,19 +1047,11 @@ async def start_freedrive_mode( # KX2 frees all motion axes at once; free_axes is accepted for API parity. del free_axes for axis in MOTION_AXES: - await self.driver.motor_enable(node_id=int(axis), state=False, use_ds402=True) + await self.driver.motor_enable(node_id=axis, state=False, use_ds402=True) async def stop_freedrive_mode(self, backend_params: Optional[BackendParams] = None) -> None: for axis in MOTION_AXES: - await self.driver.motor_enable(node_id=int(axis), state=True, use_ds402=True) + await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) -# Motion axes = the four coordinated joints. Defined at module scope (after the -# class body so `KX2ArmBackend.Axis` exists) and imported by callers that need -# to iterate "all motion axes". -MOTION_AXES = ( - KX2ArmBackend.Axis.SHOULDER, - KX2ArmBackend.Axis.Z, - KX2ArmBackend.Axis.ELBOW, - KX2ArmBackend.Axis.WRIST, -) +MOTION_AXES = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) diff --git a/pylabrobot/paa/kx2/config.py b/pylabrobot/paa/kx2/config.py new file mode 100644 index 00000000000..89b6be5dbe1 --- /dev/null +++ b/pylabrobot/paa/kx2/config.py @@ -0,0 +1,105 @@ +"""Per-arm configuration loaded from the KX2 drives during setup. + +The arm-specific calibration (link lengths, travel limits, motor conversion +factors) lives on the drives themselves; the backend reads it once into a +single `KX2Config` record at setup. The backend holds it as +`Optional[KX2Config]` (None pre-setup) — but every field on the record +itself is required, because once setup has run the whole record is filled. +""" + +from dataclasses import dataclass +from enum import IntEnum +from typing import Dict, Literal, Optional + +from pylabrobot.paa.kx2.driver import JointMoveDirection + + +GripperFingerSide = Literal["barcode_reader", "proximity_sensor"] + + +class Axis(IntEnum): + """KX2 axis -> CANopen node-ID mapping.""" + + SHOULDER = 1 + Z = 2 + ELBOW = 3 + WRIST = 4 + RAIL = 5 + SERVO_GRIPPER = 6 + + +@dataclass +class AxisConfig: + """Per-axis params loaded from a single drive during setup. + + Lives in `KX2Config.axes[axis]`. Order of fields here doesn't reflect + drive register order — they're grouped by what they describe. + """ + + motor_conversion_factor: float + max_travel: float + min_travel: float + unlimited_travel: bool + absolute_encoder: bool + max_vel: float + max_accel: float + joint_move_direction: JointMoveDirection + + # I/O pin assignments — channel -> human-readable name ("GripperSensor", + # "Buzzer", "AuxPin42", or "" when unassigned). Probed from UI[5..16] but + # not currently consumed; kept for introspection. + digital_inputs: Dict[int, str] + analog_inputs: Dict[int, str] + outputs: Dict[int, str] + + +@dataclass +class ServoGripperConfig: + """Servo gripper params (axis 6 UF6..UF17). Only present when the + gripper is detected on the bus.""" + + home_pos: int + home_search_vel: int + home_search_accel: int + home_default_position_error: int + home_hard_stop_position_error: int + home_hard_stop_offset: int + home_index_offset: int + home_offset_vel: int + home_offset_accel: int + home_timeout_msec: int + continuous_current: float + peak_current: float + + +@dataclass +class KX2Config: + # Geometry (read from the shoulder drive's UF8/UF9/UF10). + wrist_offset: float + elbow_offset: float + elbow_zero_offset: float + + # Tooling — supplied at backend construction, copied here so kinematics + # has a single source of truth. + gripper_length: float + gripper_z_offset: float + + # Which finger is treated as the radial "front" by IK/FK. The fingers are + # physically symmetric, so flipping side just negates the gripper_length + # offset in the wrist frame. Default matches the legacy convention. + gripper_finger_side: GripperFingerSide + + # Per-axis params keyed by drive node-id (= Axis value). Iterating + # `axes` (or `axes.keys()`) gives the axes present on this arm. + axes: Dict[int, AxisConfig] + + # Robot-level clearance limits (shoulder UF6/UF7). + base_to_gripper_clearance_z: float + base_to_gripper_clearance_arm: float + + robot_on_rail: bool + + # None if no servo gripper is present on the bus. + servo_gripper: Optional[ServoGripperConfig] + + eps: float = 1e-6 diff --git a/pylabrobot/paa/kx2/kx2_driver.py b/pylabrobot/paa/kx2/driver.py similarity index 97% rename from pylabrobot/paa/kx2/kx2_driver.py rename to pylabrobot/paa/kx2/driver.py index f1bb3097ab8..afe04692d31 100644 --- a/pylabrobot/paa/kx2/kx2_driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -1,13 +1,13 @@ """Low-level CAN transport + CANopen/DS402 drive primitives for the PAA KX2. Uses the `canopen` library (python-can bus + CANopen SDO/PDO/NMT/EMCY). -Paired with :class:`KX2ArmBackend` in ``kx2_backend.py`` via the standard +Paired with :class:`KX2ArmBackend` in ``arm_backend.py`` via the standard ``Device`` + ``Driver`` + capability-backend split. This module is purely a CAN transport + Elmo interpreter layer. It knows only CANopen node IDs (ints). All axis-level / robot-topology concepts (axis names, motion-axis tuples, home status, move plans, joint-move direction, homing -sequences) live in ``kx2_backend``. +sequences) live in ``arm_backend``. """ from __future__ import annotations @@ -657,7 +657,7 @@ async def _control_word_set(self, node_id: int, value: int, sync: bool = True) - async def request_drive_version(self, node_id: int) -> str: """Query Elmo drive firmware version (VR) via the OS interpreter.""" - return await self._os_interpreter(int(node_id), "VR", query=True) + return await self._os_interpreter(node_id, "VR", query=True) # --- DS402 / motor control ---------------------------------------------- @@ -666,10 +666,10 @@ async def motor_emergency_stop(self, node_id: int) -> None: async def motor_get_current_position(self, node_id: int, pu: bool = False) -> int: cmd = "PU" if pu else "PX" - return await self.query_int(int(node_id), cmd, 0) + return await self.query_int(node_id, cmd, 0) async def motor_get_motion_status(self, node_id: int) -> int: - return await self.query_int(int(node_id), "MS", 0) + return await self.query_int(node_id, "MS", 0) async def _motor_set_move_direction( self, node_id: int, direction: JointMoveDirection @@ -692,12 +692,10 @@ async def motor_check_if_move_done(self, node_id: int) -> bool: if fault is not None: raise RuntimeError(f"Motor Fault: {fault}") raise RuntimeError("Motor Fault (Unknown)") - if ms_val == 2: - return False return False async def motor_get_fault(self, node_id: int) -> Optional[str]: - val = await self.query_int(int(node_id), "MF", 0) + val = await self.query_int(node_id, "MF", 0) if val == 0: return None @@ -753,8 +751,6 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N Caller picks the path; the driver does not know about robot topology. """ - node_id = int(node_id) - if state: self.EmcyMoveErrorReceived = False max_attempts = 5 @@ -779,10 +775,7 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N raise CanError(f"Motor failed to enable (node_id = {node_id}) after {max_attempts} attempts") else: if not use_ds402: - try: - await self.write(node_id, "MO", 0, 0) - except Exception: - pass + await self.write(node_id, "MO", 0, 0) else: # DS402 disable: Op Enabled -> Switched On -> Ready to Switch On. # Matches C# (clscanmotor.cs:4540-4543) — back-to-back, no inter-CW sleep. @@ -852,6 +845,13 @@ async def _motors_move_start( async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: await self._pvt_select_mode(False) + print(f"[MOVE PLAN] move_time={plan.move_time:.3f}s, {len(plan.moves)} axes:") + for move in plan.moves: + print( + f" node={move.node_id} pos={move.position} vel={move.velocity} " + f"acc={move.acceleration} dir={move.direction.name}" + ) + for move in plan.moves: nid = int(move.node_id) await self._motor_set_move_direction(nid, move.direction) @@ -869,7 +869,7 @@ async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: # 0x6084 = Profile Deceleration (24708 decimal) await self._can_sdo_download_elmo_object(nid, 24708, 0, acc, ElmoObjectDataType.UNSIGNED32) - node_ids = [int(move.node_id) for move in plan.moves] + node_ids = [move.node_id for move in plan.moves] await self._motors_move_start(node_ids) await self.wait_for_moves_done(node_ids, timeout=plan.move_time + 2) @@ -881,11 +881,8 @@ async def user_program_run( timeout_sec: int = 0, wait_until_done: bool = False, ) -> int: - if not isinstance(node_id, int): - raise ValueError("node_id must be int") if node_id < 0 or node_id > 255: raise ValueError("node_id must be in [0, 255]") - node_id = int(node_id) ps = await self.query_int(node_id, "PS", 0) if ps == -2: @@ -940,10 +937,10 @@ async def user_program_run( async def read_input(self, node_id: int, input_num: int) -> bool: return await self.query_int(node_id, "IB", input_num) == 1 - async def _read_output(self, node_id: int, output_num: int) -> bool: + async def read_output(self, node_id: int, output_num: int) -> bool: val = await self.query_int(node_id, "OP", 0) mask = 1 << (output_num - 1) return (val & mask) == mask - async def _set_output(self, node_id: int, output_num: int, state: bool) -> None: + async def set_output(self, node_id: int, output_num: int, state: bool) -> None: await self.write(node_id, "OB", output_num, 1 if state else 0) diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py new file mode 100644 index 00000000000..2204f71ea16 --- /dev/null +++ b/pylabrobot/paa/kx2/kinematics.py @@ -0,0 +1,168 @@ +""" +KX2 kinematics: FK and IK. + +The KX2 is *not* a dual-link SCARA. The elbow is a prismatic radial slide +(not a revolute J3), Z is a separate prismatic axis, and the rail is +outside this kinematic model. So the math is closed-form and trivially +cheap — no two-link cosine law, no elbow-up/elbow-down branch, no +unreachable-pose check beyond "rotation must be about +Z". + +Joint dict keys match the drive node-IDs and the `KX2ArmBackend.Axis` enum: + 1: shoulder [deg] + 2: Z [mm] + 3: elbow [mm] (radial extension) + 4: wrist [deg] + +Task pose is a `KX2GripperLocation` (GripperLocation + a wrist-sign +convention). The gripper clamp point is in world coordinates; rotation.z +is yaw in degrees about world +Z, and rotation.x/y must be 0. Sign +convention follows right-hand rule about +Z (CCW positive looking down). + +For "closest" semantics — minimizing arm motion between two poses — call +`snap_to_current` after `ik`. +""" + +from dataclasses import dataclass +from math import atan2, cos, degrees, hypot, radians, sin +from typing import Dict, Literal, Optional + +from pylabrobot.capabilities.arms.standard import GripperLocation +from pylabrobot.paa.kx2.config import Axis, KX2Config +from pylabrobot.resources import Coordinate, Rotation + + +Wrist = Literal["cw", "ccw"] + + +@dataclass +class KX2GripperLocation(GripperLocation): + """Gripper pose with an optional wrist-sign constraint. + + wrist: + - "cw": J4 ≤ 0 (negative), with a small tolerance near 0. + - "ccw": J4 ≥ 0 (positive), with a small tolerance near 0. + - None: unspecified. `ik` rejects None; backend wrappers fill it with + the current joint's wrist sign so the arm picks whichever solution + needs the least motion. + """ + + wrist: Optional[Wrist] = None + + +class IKError(ValueError): + """Target pose is unreachable (for now: non-Z rotation requested).""" + + +def fk(joints: Dict[int, float], c: KX2Config) -> KX2GripperLocation: + """Forward kinematics. + + Args: + joints: {1: shoulder deg, 2: Z mm, 3: elbow mm, 4: wrist deg}. + c: arm configuration. + Returns: + KX2GripperLocation with the gripper clamp point and a wrist sign + derived from the joint configuration (J4 ≥ 0 → "ccw", else "cw"). + """ + r = c.wrist_offset + c.elbow_offset + c.elbow_zero_offset + joints[Axis.ELBOW] + sh_deg = joints[Axis.SHOULDER] + sh = radians(sh_deg) + + wrist_x = -r * sin(sh) + wrist_y = r * cos(sh) + wrist_z = joints[Axis.Z] + + yaw_deg = joints[Axis.WRIST] + sh_deg + if yaw_deg > 180.0: + yaw_deg -= 360.0 + if yaw_deg < -180.0: + yaw_deg += 360.0 + + # Wrist -> gripper: inverse of the gripper -> wrist translation in ik, + # so callers observe the gripper clamp point symmetric with what they + # pass into ik. Sign tracks which finger is the radial "front". + yaw = radians(yaw_deg) + gl = c.gripper_length if c.gripper_finger_side == "barcode_reader" else -c.gripper_length + gripper_x = wrist_x + gl * sin(yaw) + gripper_y = wrist_y - gl * cos(yaw) + gripper_z = wrist_z - c.gripper_z_offset + + return KX2GripperLocation( + location=Coordinate(x=gripper_x, y=gripper_y, z=gripper_z), + rotation=Rotation(z=yaw_deg), + wrist="ccw" if joints[Axis.WRIST] >= 0 else "cw", + ) + + +def ik(pose: KX2GripperLocation, c: KX2Config) -> Dict[int, float]: + """Inverse kinematics. + + Args: + pose: KX2GripperLocation. Requires pose.wrist to be "cw" or "ccw" — + "closest" semantics live in the backend wrapper (fill None with the + current joint's sign, then call `snap_to_current` after). + c: arm configuration. + Returns: + joints dict {1: shoulder deg, 2: Z mm, 3: elbow mm, 4: wrist deg}. + J4 is in (-360°, 0°] when wrist="cw" and [0°, 360°) when wrist="ccw" + (J4 ≈ 0 satisfies both, up to `c.eps`). + + Note: J4 is not constrained to wrist travel limits. The backend's + `_cart_to_joints` calls `snap_to_current` immediately after, which + pulls J4 toward the current (in-range) wrist position by 360° + multiples. Direct callers of `ik` must handle travel range themselves. + Raises: + ValueError if pose.wrist is not "cw" or "ccw". + IKError if the requested rotation has an x or y component. + """ + if pose.wrist not in ("cw", "ccw"): + raise ValueError(f"pose.wrist must be 'cw' or 'ccw', got {pose.wrist!r}") + if pose.rotation.x != 0 or pose.rotation.y != 0: + raise IKError("Only Z rotation is supported for KX2") + + # Gripper -> wrist: the incoming pose describes the gripper clamp point; + # the joint-space math operates on the wrist axis. Rigid offset with the + # gripper length on the radial axis (governed by world rotation z) and + # the gripper z offset downward. Sign tracks which finger is the radial + # "front". + yaw = radians(pose.rotation.z) + gl = c.gripper_length if c.gripper_finger_side == "barcode_reader" else -c.gripper_length + x = pose.location.x - gl * sin(yaw) + y = pose.location.y + gl * cos(yaw) + wrist_z = pose.location.z + c.gripper_z_offset + + shoulder = -degrees(atan2(x, y)) + if abs(shoulder + 180.0) < c.eps: + shoulder = 180.0 + + elbow = hypot(x, y) - c.wrist_offset - c.elbow_offset - c.elbow_zero_offset + + wrist = pose.rotation.z - shoulder + # Enforce requested sign. Tolerance on the check so J4 values within FP + # dust of 0 aren't pushed to ±360; J4 ≈ 0 satisfies both conventions. + if pose.wrist == "cw" and wrist > c.eps: + wrist -= 360.0 + elif pose.wrist == "ccw" and wrist < -c.eps: + wrist += 360.0 + + return {Axis.SHOULDER: shoulder, Axis.Z: wrist_z, Axis.ELBOW: elbow, Axis.WRIST: wrist} + + +def snap_to_current( + joints: Dict[int, float], current: Dict[int, float], wrist: Optional[Wrist] = None +) -> Dict[int, float]: + """Shift rotary joints by 360° multiples toward `current`. Z and elbow are + prismatic and pass through unchanged. + + If `wrist` is "cw" or "ccw", re-enforce that sign half on J4 after the + shift (use this when the caller explicitly chose a sign and wants it + preserved even at the cost of extra motion). If `wrist` is None, the + shift is unconditional — the user wanted "closest", so trust the snap. + """ + out = dict(joints) + for axis in (Axis.SHOULDER, Axis.WRIST): + out[axis] += 360 * round((current[axis] - out[axis]) / 360) + if wrist == "ccw" and out[Axis.WRIST] < 0: + out[Axis.WRIST] += 360 + elif wrist == "cw" and out[Axis.WRIST] > 0: + out[Axis.WRIST] -= 360 + return out diff --git a/pylabrobot/paa/kx2/kinematics_test.py b/pylabrobot/paa/kx2/kinematics_test.py new file mode 100644 index 00000000000..9ecff9f7c01 --- /dev/null +++ b/pylabrobot/paa/kx2/kinematics_test.py @@ -0,0 +1,309 @@ +import math +import unittest + +from pylabrobot.paa.kx2 import kinematics +from pylabrobot.paa.kx2.config import Axis, AxisConfig, KX2Config +from pylabrobot.paa.kx2.driver import JointMoveDirection +from pylabrobot.paa.kx2.kinematics import IKError, KX2GripperLocation +from pylabrobot.resources import Coordinate, Rotation + + +def _axis() -> AxisConfig: + return AxisConfig( + motor_conversion_factor=1.0, + max_travel=180.0, + min_travel=-180.0, + unlimited_travel=False, + absolute_encoder=True, + max_vel=100.0, + max_accel=100.0, + joint_move_direction=JointMoveDirection.Normal, + digital_inputs={}, + analog_inputs={}, + outputs={}, + ) + + +def _config( + wrist_offset: float = 10.0, + elbow_offset: float = 20.0, + elbow_zero_offset: float = 5.0, + gripper_length: float = 15.0, + gripper_z_offset: float = 3.0, + gripper_finger_side: str = "barcode_reader", +) -> KX2Config: + return KX2Config( + wrist_offset=wrist_offset, + elbow_offset=elbow_offset, + elbow_zero_offset=elbow_zero_offset, + gripper_length=gripper_length, + gripper_z_offset=gripper_z_offset, + gripper_finger_side=gripper_finger_side, # type: ignore[arg-type] + axes={a: _axis() for a in (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST)}, + base_to_gripper_clearance_z=0.0, + base_to_gripper_clearance_arm=0.0, + robot_on_rail=False, + servo_gripper=None, + ) + + +def _approx(a: float, b: float, eps: float = 1e-9) -> bool: + return abs(a - b) < eps + + +class FKIKRoundTrip(unittest.TestCase): + def test_roundtrip_ccw(self): + c = _config() + pose = KX2GripperLocation( + location=Coordinate(x=100, y=200, z=50), + rotation=Rotation(z=30), + wrist="ccw", + ) + joints = kinematics.ik(pose, c) + back = kinematics.fk(joints, c) + self.assertAlmostEqual(back.location.x, pose.location.x, places=9) + self.assertAlmostEqual(back.location.y, pose.location.y, places=9) + self.assertAlmostEqual(back.location.z, pose.location.z, places=9) + self.assertAlmostEqual(back.rotation.z, pose.rotation.z, places=9) + + def test_roundtrip_cw_yields_same_world_pose(self): + """cw and ccw produce J4 360° apart but represent the same physical pose.""" + c = _config() + pose = KX2GripperLocation( + location=Coordinate(x=100, y=200, z=50), rotation=Rotation(z=30), wrist="cw" + ) + j_cw = kinematics.ik(pose, c) + j_ccw = kinematics.ik(KX2GripperLocation(**{**pose.__dict__, "wrist": "ccw"}), c) + self.assertAlmostEqual(j_ccw[Axis.WRIST] - j_cw[Axis.WRIST], 360.0, places=9) + # FK on either should land at the original pose. + for j in (j_cw, j_ccw): + back = kinematics.fk(j, c) + self.assertAlmostEqual(back.location.x, pose.location.x, places=9) + self.assertAlmostEqual(back.location.y, pose.location.y, places=9) + self.assertAlmostEqual(back.rotation.z, pose.rotation.z, places=9) + + def test_roundtrip_at_origin_yaw_zero(self): + """Sanity: a pose at (0, R, Z, 0°) lands shoulder=0°.""" + c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0, gripper_length=0, + gripper_z_offset=0) + pose = KX2GripperLocation( + location=Coordinate(x=0, y=300, z=10), rotation=Rotation(z=0), wrist="ccw" + ) + joints = kinematics.ik(pose, c) + self.assertAlmostEqual(joints[Axis.SHOULDER], 0.0, places=9) + self.assertAlmostEqual(joints[Axis.ELBOW], 300.0, places=9) + self.assertAlmostEqual(joints[Axis.Z], 10.0, places=9) + self.assertAlmostEqual(joints[Axis.WRIST], 0.0, places=9) + + +class IKWristSign(unittest.TestCase): + def test_cw_yields_non_positive_wrist(self): + c = _config() + pose = KX2GripperLocation( + location=Coordinate(x=50, y=300, z=20), rotation=Rotation(z=45), wrist="cw" + ) + joints = kinematics.ik(pose, c) + self.assertLessEqual(joints[Axis.WRIST], c.eps) + + def test_ccw_yields_non_negative_wrist(self): + c = _config() + pose = KX2GripperLocation( + location=Coordinate(x=50, y=300, z=20), rotation=Rotation(z=45), wrist="ccw" + ) + joints = kinematics.ik(pose, c) + self.assertGreaterEqual(joints[Axis.WRIST], -c.eps) + + def test_wrist_near_zero_satisfies_both(self): + """When the natural J4 is exactly 0, neither cw nor ccw should shift it.""" + c = _config() + # Construct a pose where wrist - shoulder = 0 naturally: + # rotation.z == shoulder. Pick rotation.z = -atan2(x, y) in degrees. + x, y = 100.0, 100.0 + yaw_deg = -math.degrees(math.atan2(x, y)) # ≈ -45° + pose = KX2GripperLocation( + location=Coordinate(x=x, y=y, z=0), + rotation=Rotation(z=yaw_deg), + wrist="ccw", + ) + # Use zero gripper offsets so location maps directly to wrist axis. + c0 = _config(gripper_length=0, gripper_z_offset=0) + joints = kinematics.ik(pose, c0) + self.assertAlmostEqual(joints[Axis.WRIST], 0.0, places=6) + + pose_cw = KX2GripperLocation(**{**pose.__dict__, "wrist": "cw"}) + joints_cw = kinematics.ik(pose_cw, c0) + self.assertAlmostEqual(joints_cw[Axis.WRIST], 0.0, places=6) + + +class IKErrors(unittest.TestCase): + def test_none_wrist_raises_valueerror(self): + c = _config() + pose = KX2GripperLocation( + location=Coordinate(x=0, y=100, z=0), rotation=Rotation(z=0), wrist=None + ) + with self.assertRaises(ValueError): + kinematics.ik(pose, c) + + def test_invalid_wrist_string_raises_valueerror(self): + c = _config() + pose = KX2GripperLocation( + location=Coordinate(x=0, y=100, z=0), rotation=Rotation(z=0), wrist="up", # type: ignore + ) + with self.assertRaises(ValueError): + kinematics.ik(pose, c) + + def test_x_rotation_raises_ikerror(self): + c = _config() + pose = KX2GripperLocation( + location=Coordinate(x=0, y=100, z=0), + rotation=Rotation(x=10, z=0), + wrist="ccw", + ) + with self.assertRaises(IKError): + kinematics.ik(pose, c) + + def test_y_rotation_raises_ikerror(self): + c = _config() + pose = KX2GripperLocation( + location=Coordinate(x=0, y=100, z=0), + rotation=Rotation(y=10, z=0), + wrist="ccw", + ) + with self.assertRaises(IKError): + kinematics.ik(pose, c) + + +class FKResult(unittest.TestCase): + def test_fk_sets_wrist_field_from_j4_sign(self): + c = _config() + j_pos = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 100.0, Axis.WRIST: 45.0} + self.assertEqual(kinematics.fk(j_pos, c).wrist, "ccw") + j_neg = {**j_pos, Axis.WRIST: -45.0} + self.assertEqual(kinematics.fk(j_neg, c).wrist, "cw") + j_zero = {**j_pos, Axis.WRIST: 0.0} + self.assertEqual(kinematics.fk(j_zero, c).wrist, "ccw") # 0 ≥ 0 -> ccw + + +class SnapToCurrent(unittest.TestCase): + def test_shifts_wrist_toward_current(self): + """J4=20 with current=380 should snap to 380 (closer than 20).""" + j = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 20.0} + cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 380.0} + out = kinematics.snap_to_current(j, cur, "ccw") + self.assertAlmostEqual(out[Axis.WRIST], 380.0) + + def test_shifts_shoulder_toward_current(self): + """Shoulder is rotary too — also snaps.""" + j = {Axis.SHOULDER: -170.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0} + cur = {Axis.SHOULDER: 200.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0} + out = kinematics.snap_to_current(j, cur, "ccw") + self.assertAlmostEqual(out[Axis.SHOULDER], 190.0) # -170 + 360 + + def test_prismatic_axes_pass_through(self): + """Z and elbow are not 360°-modulo; they should be untouched.""" + j = {Axis.SHOULDER: 0.0, Axis.Z: 100.0, Axis.ELBOW: 50.0, Axis.WRIST: 0.0} + cur = {Axis.SHOULDER: 0.0, Axis.Z: 999.0, Axis.ELBOW: 999.0, Axis.WRIST: 0.0} + out = kinematics.snap_to_current(j, cur, "ccw") + self.assertEqual(out[Axis.Z], 100.0) + self.assertEqual(out[Axis.ELBOW], 50.0) + + def test_re_enforces_ccw_after_snap(self): + """Explicit ccw: snap might land J4 negative; ccw re-enforce shifts +360.""" + j = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 350.0} + cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0} + out = kinematics.snap_to_current(j, cur, "ccw") + # snap shifts 350 -> -10 (toward current 0); ccw re-enforce -> +350 + self.assertAlmostEqual(out[Axis.WRIST], 350.0) + + def test_re_enforces_cw_after_snap(self): + """Symmetric: explicit cw re-enforce shifts a positive J4 negative.""" + j = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: -350.0} + cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0} + out = kinematics.snap_to_current(j, cur, "cw") + # snap shifts -350 -> 10 (toward current 0); cw re-enforce -> -350 + self.assertAlmostEqual(out[Axis.WRIST], -350.0) + + def test_closest_mode_does_not_re_enforce(self): + """wrist=None means closest: trust the snap, don't shift back.""" + j = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 350.0} + cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0} + out = kinematics.snap_to_current(j, cur, None) + # Pure shift toward current: 350 - 360 = -10. No re-enforce. + self.assertAlmostEqual(out[Axis.WRIST], -10.0) + + def test_no_shift_when_already_close(self): + j = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 30.0} + cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 25.0} + out = kinematics.snap_to_current(j, cur, "ccw") + self.assertAlmostEqual(out[Axis.WRIST], 30.0) + + +class GripperFingerSide(unittest.TestCase): + def test_proximity_side_negates_gripper_offset(self): + """Same joints, opposite finger side -> clamp point reflected through wrist axis.""" + c_bc = _config(gripper_finger_side="barcode_reader") + c_pr = _config(gripper_finger_side="proximity_sensor") + j = {Axis.SHOULDER: 30.0, Axis.Z: 50.0, Axis.ELBOW: 100.0, Axis.WRIST: 15.0} + p_bc = kinematics.fk(j, c_bc) + p_pr = kinematics.fk(j, c_pr) + + # Wrist position is the midpoint between the two clamp points. + wrist_x = (p_bc.location.x + p_pr.location.x) / 2 + wrist_y = (p_bc.location.y + p_pr.location.y) / 2 + yaw_deg = j[Axis.WRIST] + j[Axis.SHOULDER] + yaw = math.radians(yaw_deg) + self.assertAlmostEqual( + p_bc.location.x - wrist_x, c_bc.gripper_length * math.sin(yaw), delta=1e-5 + ) + self.assertAlmostEqual( + p_bc.location.y - wrist_y, -c_bc.gripper_length * math.cos(yaw), delta=1e-5 + ) + # z and yaw are independent of finger side. + self.assertAlmostEqual(p_bc.location.z, p_pr.location.z, places=9) + self.assertAlmostEqual(p_bc.rotation.z, p_pr.rotation.z, places=9) + + def test_proximity_roundtrip(self): + c = _config(gripper_finger_side="proximity_sensor") + pose = KX2GripperLocation( + location=Coordinate(x=100, y=200, z=50), + rotation=Rotation(z=30), + wrist="ccw", + ) + joints = kinematics.ik(pose, c) + back = kinematics.fk(joints, c) + self.assertAlmostEqual(back.location.x, pose.location.x, places=9) + self.assertAlmostEqual(back.location.y, pose.location.y, places=9) + self.assertAlmostEqual(back.location.z, pose.location.z, places=9) + self.assertAlmostEqual(back.rotation.z, pose.rotation.z, places=9) + + def test_ik_elbow_differs_by_twice_gripper_length(self): + """For a clamp point on the +y axis with yaw=0, both sides give + shoulder=0 but the wrist sits 2*gripper_length further out for + barcode_reader (gripper points +y away from base) than for + proximity_sensor (gripper points -y back toward base).""" + pose = KX2GripperLocation( + location=Coordinate(x=0, y=300, z=0), rotation=Rotation(z=0), wrist="ccw" + ) + c_bc = _config(gripper_finger_side="barcode_reader") + c_pr = _config(gripper_finger_side="proximity_sensor") + j_bc = kinematics.ik(pose, c_bc) + j_pr = kinematics.ik(pose, c_pr) + self.assertAlmostEqual(j_bc[Axis.SHOULDER], 0.0, places=9) + self.assertAlmostEqual(j_pr[Axis.SHOULDER], 0.0, places=9) + self.assertAlmostEqual(j_bc[Axis.ELBOW] - j_pr[Axis.ELBOW], 2 * c_bc.gripper_length, places=9) + + +class ShoulderSnapAt180(unittest.TestCase): + def test_negative_180_snaps_to_positive(self): + """A pose pointing exactly along -y has shoulder = ±180; we snap to +180.""" + c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0, gripper_length=0, + gripper_z_offset=0) + pose = KX2GripperLocation( + location=Coordinate(x=0, y=-100, z=0), rotation=Rotation(z=180), wrist="ccw" + ) + joints = kinematics.ik(pose, c) + self.assertAlmostEqual(joints[Axis.SHOULDER], 180.0, places=9) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/paa/kx2/kx2.py b/pylabrobot/paa/kx2/kx2.py index 6c2ac1fdb15..5b651107240 100644 --- a/pylabrobot/paa/kx2/kx2.py +++ b/pylabrobot/paa/kx2/kx2.py @@ -1,7 +1,8 @@ from pylabrobot.capabilities.arms.orientable_arm import OrientableArm from pylabrobot.device import Device -from pylabrobot.paa.kx2.kx2_backend import KX2ArmBackend -from pylabrobot.paa.kx2.kx2_driver import KX2Driver +from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend +from pylabrobot.paa.kx2.config import GripperFingerSide +from pylabrobot.paa.kx2.driver import KX2Driver from pylabrobot.resources.resource import Resource @@ -12,6 +13,7 @@ def __init__( self, gripper_length: float = 0.0, gripper_z_offset: float = 0.0, + gripper_finger_side: GripperFingerSide = "barcode_reader", ) -> None: driver = KX2Driver() super().__init__(driver=driver) @@ -20,6 +22,7 @@ def __init__( driver=driver, gripper_length=gripper_length, gripper_z_offset=gripper_z_offset, + gripper_finger_side=gripper_finger_side, ) self.reference = Resource(name="KX2", size_x=200, size_y=200, size_z=200) self.arm = OrientableArm(backend=backend, reference_resource=self.reference) From 23936571d6f15f42fd3b995049f6c2c21292f54a Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sat, 25 Apr 2026 14:42:31 -0700 Subject: [PATCH 35/93] KX2 barcode reader: rename + Denso MDI-4050 docs + dump_config + macOS install MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Rename `kx2_barcode_reader.py` → `barcode_reader.py` to match the post-refactor naming convention (no more `kx2_` prefix on files inside the `kx2/` package). The vendor identification is also corrected: the reader that ships with these systems is a Denso MDI-4050, not a Microscan unit (verified by sending `Z4` and reading the model header). The protocol family is similar — ESC-prefixed, CR-terminated — but the manuals and config tools are different (Denso's "MDI Setup" / Denso setting-barcode manual rather than Microscan ESP). Behavior changes: - `KX2BarcodeReader(port, read_time=2, baudrate=9600)` constructor now takes the read window upfront, and `set_read_time` / `set_read_mode` are exposed at the device level so callers don't have to reach through `bcr.driver` for runtime config. `read_mode` is a `Literal["single", "multiple", "continuous"]`. - `send_command(cmd)` and `query(cmd)` split. The reader is silent on most commands (S0/S1/S2, Yn/YM, Z trigger, +I/+F) — the previous "wait for CR after every command" code timed out for everything except `Z1`. Caller now picks fire-and-forget vs response-bearing. - Backend `set_read_time(seconds)` updates both the reader (over the wire) and the scan-completion timeout in the same call, so a longer read window doesn't mismatch the scan deadline. - New `KX2BarcodeReaderDriver.dump_config()` — sends `Z4` and returns the reader's full NVRAM dump (model + firmware + per-symbology enable flags + prefixes/suffixes + length limits). Useful for figuring out what's enabled without paging through a config UI. Module docstring documents: - The macOS Prolific PL2303 install path (`brew install --cask prolific-pl2303`, then approve the system extension in System Settings → Privacy & Security). macOS's bundled `AppleUSBPLCOM.dext` doesn't cover the GC/HXN chip (USB ID `067b:23a3`), so without the vendor dext the device never enumerates as `/dev/tty.PL2303G-USBtoUART`. - The MDI-4050 default symbology set (1D only — UPC-A/E, EAN-13/8, Code 39, Codabar, Code 128, Code 93, GS1 DataBar, etc.) and the three options for changing it (Denso MDI Setup / setting-barcode manual / direct register writes). The hello-world notebook's barcode section is updated to match. The arm sections are untouched. Co-Authored-By: Claude Opus 4.7 (1M context) --- docs/user_guide/paa/kx2/hello-world.ipynb | 156 +++++----- pylabrobot/paa/kx2/__init__.py | 5 + pylabrobot/paa/kx2/barcode_reader.py | 343 ++++++++++++++++++++++ pylabrobot/paa/kx2/kx2_barcode_reader.py | 196 ------------- 4 files changed, 423 insertions(+), 277 deletions(-) create mode 100644 pylabrobot/paa/kx2/barcode_reader.py delete mode 100644 pylabrobot/paa/kx2/kx2_barcode_reader.py diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 4e1dffd97ea..070b174d018 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -408,152 +408,146 @@ }, { "cell_type": "markdown", - "id": "24a4fe6e", "metadata": {}, "source": [ "## Barcode reader\n", "\n", - "The KX2's onboard barcode reader is a Microscan/Omron-style serial device wired to the controller PC (HD-15 host cable out of the reader, typically broken out to a DB9 or USB-serial adapter). It's fully independent of the CAN motor stack, so it works even with the arm e-stopped — exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`.\n", + "The KX2's onboard barcode reader is a serial 1D laser scanner wired to the controller PC. It's fully independent of the CAN motor stack, so it works even with the arm e-stopped — exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`.\n", "\n", - "Factory defaults: **9600 8N1**, no flow control. Find your port with `python -m serial.tools.list_ports -v`.\n", + "The model that ships with KX2 systems we've inspected is a **Denso MDI-4050** (firmware reports as `BD01J09`). Factory defaults: **9600 8N1**, no flow control. Find your port with `python -m serial.tools.list_ports -v`.\n", "\n", - "### Supported symbologies\n", + "### macOS: USB-to-serial driver\n", "\n", - "The Microscan scanners PAA ships with KX2 systems (MS-3 / MS-4 / QX-830) decode the standard 1D set out of the box:\n", + "The reader cable is typically a Prolific PL2303. macOS bundles a driver for older variants but not the newer GC/HXN chip (USB ID `067b:23a3`). If the device doesn't show up at `/dev/tty.PL2303G-USBtoUART` after plugging it in:\n", "\n", - "- Code 128 (all subsets)\n", - "- Code 39\n", - "- Codabar\n", - "- Interleaved 2 of 5\n", - "- Code 93\n", - "- UPC-A / UPC-E\n", - "- EAN-8 / EAN-13\n", + "```bash\n", + "brew install --cask prolific-pl2303\n", + "open -a PL2303Serial # registers the system extension\n", + "```\n", "\n", - "2D models (QX-870 and up) add PDF417, DataMatrix, and QR Code. Which symbologies are *enabled* is a per-unit configuration; use the vendor's ESP software if you need to change it.\n", + "Then **System Settings → Privacy & Security → Allow** on the \"System software from PL2303Serial was blocked\" prompt, restart if asked, and replug the cable. Verify with `systemextensionsctl list`: `com.prolific.cdc.PLCdcFSDriver` should show `[activated enabled]`.\n", "\n", - "{class}`~pylabrobot.resources.barcode.Barcode` returned from `scan()` has `symbology=\"ANY 1D\"` by default — the reader doesn't emit a symbology ID unless AIM-ID prefixing is turned on in ESP (`Preamble` / `Postamble` settings). If you need the symbology per-read, enable that and parse the leading `]xN` bytes from `barcode.data`." + "### Symbologies\n", + "\n", + "The MDI-4050 is a 1D laser, so **PDF417 / DataMatrix / QR Code aren't supported** — you'd need the 2D imager variants (e.g. MDI-4150) for those.\n", + "\n", + "Typical out-of-the-box 1D enables: UPC-A, UPC-E, EAN-13, EAN-8, Code 39, Tri-Optic, Codabar, Industrial 2/5, Interleaved 2/5, IATA, Code 128, Code 93, GS1 DataBar, GS1 DataBar Limited. Add-on (supplemental) codes, postal codes, MSI/Plessey, Telepen, UK/Plessey, and Code 11 are off by default.\n", + "\n", + "You can read your unit's full configuration over the wire — model, firmware, every per-symbology enable flag, prefixes/suffixes, and length limits — via `dump_config()` (which sends `Z4`):\n", + "\n", + "```python\n", + "text = await bcr.driver.dump_config()\n", + "print(text[:200]) # header has MODEL / ROM Ver. / I/F\n", + "```\n", + "\n", + "To **change** which symbologies are enabled, three options in order of practicality:\n", + "\n", + "1. **Denso's MDI Setup utility** (free Windows download from Denso ADC) — checkbox UI, talks to the reader over the same serial port, writes to NVRAM.\n", + "2. **Configuration barcodes** printed in the MDI-4050 setting manual — scan a Start → toggle → End sequence; no host required.\n", + "3. Direct register writes via the ESC protocol — possible but the per-symbology byte layout is documented only in Denso's protocol manual.\n", + "\n", + "{class}`~pylabrobot.resources.barcode.Barcode` returned from `scan()` has `symbology=\"ANY 1D\"` by default — the reader doesn't emit a symbology ID unless prefix/suffix bytes are configured to include one." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Setup\n", + "\n", + "Pass the serial port path and (optionally) a read-window in seconds. `setup()` opens the port, does the `Z1` software-version handshake, and puts the reader into single-trigger mode." ] }, { "cell_type": "code", - "execution_count": 1, - "id": "de492820", + "execution_count": null, "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "2026-04-21 12:09:43,639 - pylabrobot.io.serial - INFO - Using explicitly provided port: /dev/tty.usbserial-FTE1YWTI (for VID=None, PID=None)\n", - "2026-04-21 12:09:43,670 - pylabrobot.paa.kx2.kx2_barcode_reader - INFO - [KX2 BCR /dev/tty.usbserial-FTE1YWTI] connected\n" - ] - }, - { - "ename": "BarcodeScannerError", - "evalue": "KX2 barcode reader: timeout waiting for CR after 5.0s (buffered=b'')", - "output_type": "error", - "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mBarcodeScannerError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[0;32mIn[1], line 4\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mpylabrobot\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mpaa\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mkx2\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m KX2BarcodeReader\n\u001b[1;32m 3\u001b[0m bcr \u001b[38;5;241m=\u001b[39m KX2BarcodeReader(port\u001b[38;5;241m=\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m/dev/tty.usbserial-FTE1YWTI\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m----> 4\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m bcr\u001b[38;5;241m.\u001b[39msetup()\n", - "File \u001b[0;32m~/real/pylabrobot/pylabrobot/device.py:110\u001b[0m, in \u001b[0;36mDevice.setup\u001b[0;34m(self, backend_params)\u001b[0m\n\u001b[1;32m 108\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mdriver\u001b[38;5;241m.\u001b[39msetup(backend_params\u001b[38;5;241m=\u001b[39mbackend_params)\n\u001b[1;32m 109\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m cap \u001b[38;5;129;01min\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39m_capabilities:\n\u001b[0;32m--> 110\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m cap\u001b[38;5;241m.\u001b[39m_on_setup()\n\u001b[1;32m 111\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39m_setup_finished \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mTrue\u001b[39;00m\n", - "File \u001b[0;32m~/real/pylabrobot/pylabrobot/capabilities/capability.py:90\u001b[0m, in \u001b[0;36mCapability._on_setup\u001b[0;34m(self, backend_params)\u001b[0m\n\u001b[1;32m 88\u001b[0m \u001b[38;5;28;01masync\u001b[39;00m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21m_on_setup\u001b[39m(\u001b[38;5;28mself\u001b[39m, backend_params: Optional[BackendParams] \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m):\n\u001b[1;32m 89\u001b[0m \u001b[38;5;250m \u001b[39m\u001b[38;5;124;03m\"\"\"Called by the parent Device after driver.setup() completes.\"\"\"\u001b[39;00m\n\u001b[0;32m---> 90\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mbackend\u001b[38;5;241m.\u001b[39m_on_setup(backend_params\u001b[38;5;241m=\u001b[39mbackend_params)\n\u001b[1;32m 91\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39m_setup_finished \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mTrue\u001b[39;00m\n", - "File \u001b[0;32m~/real/pylabrobot/pylabrobot/paa/kx2/kx2_barcode_reader.py:162\u001b[0m, in \u001b[0;36mKX2BarcodeReaderBackend._on_setup\u001b[0;34m(self, backend_params)\u001b[0m\n\u001b[1;32m 160\u001b[0m \u001b[38;5;28;01masync\u001b[39;00m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21m_on_setup\u001b[39m(\u001b[38;5;28mself\u001b[39m, backend_params: Optional[BackendParams] \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m:\n\u001b[1;32m 161\u001b[0m \u001b[38;5;66;03m# Handshake: version query (mirrors KX2RobotControl.cs:15617).\u001b[39;00m\n\u001b[0;32m--> 162\u001b[0m version \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mdriver\u001b[38;5;241m.\u001b[39mget_software_version()\n\u001b[1;32m 163\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;129;01mnot\u001b[39;00m version:\n\u001b[1;32m 164\u001b[0m \u001b[38;5;28;01mraise\u001b[39;00m BarcodeScannerError(\n\u001b[1;32m 165\u001b[0m \u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mKX2 barcode reader: empty software-version response during handshake. \u001b[39m\u001b[38;5;124m\"\u001b[39m\n\u001b[1;32m 166\u001b[0m \u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mVerify port, baud rate, and that the reader is powered on.\u001b[39m\u001b[38;5;124m\"\u001b[39m\n\u001b[1;32m 167\u001b[0m )\n", - "File \u001b[0;32m~/real/pylabrobot/pylabrobot/paa/kx2/kx2_barcode_reader.py:149\u001b[0m, in \u001b[0;36mKX2BarcodeReaderDriver.get_software_version\u001b[0;34m(self)\u001b[0m\n\u001b[1;32m 148\u001b[0m \u001b[38;5;28;01masync\u001b[39;00m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21mget_software_version\u001b[39m(\u001b[38;5;28mself\u001b[39m) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m \u001b[38;5;28mstr\u001b[39m:\n\u001b[0;32m--> 149\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39msend_command(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mZ1\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n", - "File \u001b[0;32m~/real/pylabrobot/pylabrobot/paa/kx2/kx2_barcode_reader.py:114\u001b[0m, in \u001b[0;36mKX2BarcodeReaderDriver.send_command\u001b[0;34m(self, cmd, timeout)\u001b[0m\n\u001b[1;32m 112\u001b[0m payload \u001b[38;5;241m=\u001b[39m _ESC \u001b[38;5;241m+\u001b[39m cmd\u001b[38;5;241m.\u001b[39mencode(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mascii\u001b[39m\u001b[38;5;124m\"\u001b[39m) \u001b[38;5;241m+\u001b[39m _CR\n\u001b[1;32m 113\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mio\u001b[38;5;241m.\u001b[39mwrite(payload)\n\u001b[0;32m--> 114\u001b[0m decoded \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39m_read_until_cr(timeout)\n\u001b[1;32m 115\u001b[0m logger\u001b[38;5;241m.\u001b[39mdebug(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124m[KX2 BCR \u001b[39m\u001b[38;5;132;01m%s\u001b[39;00m\u001b[38;5;124m] \u001b[39m\u001b[38;5;132;01m%s\u001b[39;00m\u001b[38;5;124m -> \u001b[39m\u001b[38;5;132;01m%r\u001b[39;00m\u001b[38;5;124m\"\u001b[39m, \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mio\u001b[38;5;241m.\u001b[39mport, cmd, decoded)\n\u001b[1;32m 116\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m decoded\n", - "File \u001b[0;32m~/real/pylabrobot/pylabrobot/paa/kx2/kx2_barcode_reader.py:106\u001b[0m, in \u001b[0;36mKX2BarcodeReaderDriver._read_until_cr\u001b[0;34m(self, timeout)\u001b[0m\n\u001b[1;32m 104\u001b[0m \u001b[38;5;28;01melse\u001b[39;00m:\n\u001b[1;32m 105\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m asyncio\u001b[38;5;241m.\u001b[39msleep(\u001b[38;5;241m0.01\u001b[39m)\n\u001b[0;32m--> 106\u001b[0m \u001b[38;5;28;01mraise\u001b[39;00m BarcodeScannerError(\n\u001b[1;32m 107\u001b[0m \u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mKX2 barcode reader: timeout waiting for CR after \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mtimeout\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124ms (buffered=\u001b[39m\u001b[38;5;132;01m{\u001b[39;00m\u001b[38;5;28mbytes\u001b[39m(buf)\u001b[38;5;132;01m!r}\u001b[39;00m\u001b[38;5;124m)\u001b[39m\u001b[38;5;124m\"\u001b[39m\n\u001b[1;32m 108\u001b[0m )\n", - "\u001b[0;31mBarcodeScannerError\u001b[0m: KX2 barcode reader: timeout waiting for CR after 5.0s (buffered=b'')" - ] - } - ], + "outputs": [], "source": [ "from pylabrobot.paa.kx2 import KX2BarcodeReader\n", "\n", - "bcr = KX2BarcodeReader(port=\"/dev/tty.usbserial-FTE1YWTI\")\n", + "bcr = KX2BarcodeReader(port=\"/dev/tty.PL2303G-USBtoUART11240\", read_time=8)\n", "await bcr.setup()" ] }, { "cell_type": "markdown", - "id": "c240108f", "metadata": {}, "source": [ - "`setup()` opens the serial port, does a `Z1` (software-version) handshake, switches the reader to single-trigger mode, and sets a default 2-second read window.\n", - "\n", "### Scan\n", "\n", - "Fires the trigger (`Z`), waits up to `read_time + 1s` for a decode, returns a {class}`~pylabrobot.resources.barcode.Barcode`." + "`scan()` fires the trigger and waits up to `read_time + 1s` for the reader to decode something. Returns a {class}`~pylabrobot.resources.barcode.Barcode`." ] }, { "cell_type": "code", - "execution_count": 2, - "id": "6ca16712", + "execution_count": null, "metadata": {}, - "outputs": [ - { - "ename": "RuntimeError", - "evalue": "The capability has not been set up. Call setup() on the parent device.", - "output_type": "error", - "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mRuntimeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[0;32mIn[2], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m barcode \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mawait\u001b[39;00m bcr\u001b[38;5;241m.\u001b[39mbarcode_scanning\u001b[38;5;241m.\u001b[39mscan()\n\u001b[1;32m 2\u001b[0m \u001b[38;5;28mprint\u001b[39m(barcode)\n", - "File \u001b[0;32m~/real/pylabrobot/pylabrobot/capabilities/capability.py:45\u001b[0m, in \u001b[0;36mneed_capability_ready..wrapper\u001b[0;34m(*args, **kwargs)\u001b[0m\n\u001b[1;32m 42\u001b[0m \u001b[38;5;28mself\u001b[39m \u001b[38;5;241m=\u001b[39m args[\u001b[38;5;241m0\u001b[39m]\n\u001b[1;32m 44\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;129;01mnot\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39msetup_finished:\n\u001b[0;32m---> 45\u001b[0m \u001b[38;5;28;01mraise\u001b[39;00m \u001b[38;5;167;01mRuntimeError\u001b[39;00m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mThe capability has not been set up. Call setup() on the parent device.\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[1;32m 46\u001b[0m \u001b[38;5;28;01mreturn\u001b[39;00m \u001b[38;5;28;01mawait\u001b[39;00m func(\u001b[38;5;241m*\u001b[39margs, \u001b[38;5;241m*\u001b[39m\u001b[38;5;241m*\u001b[39mkwargs)\n", - "\u001b[0;31mRuntimeError\u001b[0m: The capability has not been set up. Call setup() on the parent device." - ] - } - ], + "outputs": [], "source": [ "barcode = await bcr.barcode_scanning.scan()\n", - "print(barcode)" + "print(barcode.data)" ] }, { "cell_type": "markdown", - "id": "cdc9257f", "metadata": {}, "source": [ - "### Driver-level controls\n", + "### Configuration\n", "\n", - "For raw access — longer read windows, continuous mode, auto-trigger, or direct vendor commands — go through `bcr.driver`:" + "Adjust the read window or trigger mode at runtime:" ] }, { "cell_type": "code", "execution_count": null, - "id": "fb52ca64", "metadata": {}, "outputs": [], "source": [ - "print(await bcr.driver.get_software_version())\n", + "# 1..9 seconds, or 0 for indefinite. Updates the reader and the scan timeout together.\n", + "await bcr.set_read_time(5)\n", "\n", - "# Switch to multi-read (two reads without requiring a new trigger) or continuous:\n", - "# await bcr.driver.set_read_mode(\"multiple\")\n", - "# await bcr.driver.set_read_mode(\"continuous\")\n", + "# \"single\" (default), \"multiple\" (two reads per trigger), or \"continuous\".\n", + "# await bcr.set_read_mode(\"continuous\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "### Driver-level access\n", + "\n", + "For raw vendor commands or the auto-trigger mode (reader fires on its own), go through `bcr.driver`:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "print(await bcr.driver.get_software_version())\n", "\n", - "# Read-time window (1..9 seconds, or 0 for indefinite / up to 60s):\n", - "await bcr.driver.set_read_time(5)\n", + "# Full NVRAM dump — model, firmware, all symbology enables, prefixes/suffixes, length limits.\n", + "config = await bcr.driver.dump_config()\n", + "print(config[:200])\n", "\n", - "# Auto-trigger — reader fires on its own when it sees motion in frame:\n", - "# await bcr.driver.set_auto_trigger(True)\n", + "# await bcr.driver.set_auto_trigger(True) # reader scans on its own\n", "# await bcr.driver.set_auto_trigger(False)" ] }, { "cell_type": "markdown", - "id": "e26f0d79", "metadata": {}, "source": [ "### Teardown\n", "\n", - "`stop()` sends `Y` (trigger off) + `Y2` (reset to 2-second read time), then closes the serial port — leaving the reader in a known state for the next session." + "`stop()` sends `Y` (trigger off) and `Y2` (reset to a 2s read window), then closes the serial port — leaving the reader in a known state for the next session." ] }, { "cell_type": "code", "execution_count": null, - "id": "df9ef753", "metadata": {}, "outputs": [], "source": [ @@ -600,4 +594,4 @@ }, "nbformat": 4, "nbformat_minor": 5 -} \ No newline at end of file +} diff --git a/pylabrobot/paa/kx2/__init__.py b/pylabrobot/paa/kx2/__init__.py index 86b1ce61967..b628da773f4 100644 --- a/pylabrobot/paa/kx2/__init__.py +++ b/pylabrobot/paa/kx2/__init__.py @@ -1,4 +1,9 @@ from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend +from pylabrobot.paa.kx2.barcode_reader import ( + KX2BarcodeReader, + KX2BarcodeReaderBackend, + KX2BarcodeReaderDriver, +) from pylabrobot.paa.kx2.config import Axis, KX2Config from pylabrobot.paa.kx2.driver import KX2Driver from pylabrobot.paa.kx2.kinematics import KX2GripperLocation diff --git a/pylabrobot/paa/kx2/barcode_reader.py b/pylabrobot/paa/kx2/barcode_reader.py new file mode 100644 index 00000000000..12c6363e9ca --- /dev/null +++ b/pylabrobot/paa/kx2/barcode_reader.py @@ -0,0 +1,343 @@ +"""PAA KX2 onboard barcode reader. + +The KX2's onboard barcode reader is a plain RS-232 device wired to the +controller PC — entirely independent of the CAN bus that drives the motors. +Lives in its own `Device` class alongside :class:`KX2`. + +The unit shipped with KX2 systems we've inspected is a **Denso MDI-4050** +(1D laser scanner) — confirmed via ``Z4``:: + + MODEL = MDI-4050 + ROM Ver. = BD01J09 + I/F = RS-232C + +Denso's MDI series speaks an ESC-prefixed protocol close to (but not +identical to) Microscan's. The vendor C# at ``KX2RobotControl.cs:15573–15848`` +covers the read-cycle subset. + +Factory defaults: **9600 8N1, no flow control**. Commands are framed as +``ESC + + CR + LF``; responses are `` + CR``. Useful commands: + + ``Z`` — fire trigger (start a single read window) + ``Y`` — stop trigger + ``Z1`` — query firmware version (handshake — single line response) + ``Z4`` — dump the entire NVRAM config (model, version, all symbology + enables, prefixes/suffixes, length limits — large multi-KB reply) + ``S0/S1/S2`` — set read mode: single / multiple / continuous + ``Y1..Y9`` / ``YM`` — read-time window (seconds, or M for indefinite) + ``+I/+F`` — auto-trigger on / off + +Symbology configuration +----------------------- + +The MDI-4050 is a 1D laser, so PDF417 / DataMatrix / QR Code aren't supported. +On a fresh unit, the typical enabled set is: UPC-A, UPC-E, EAN-13, EAN-8, +Code 39, Tri-Optic, Codabar, Industrial 2/5, Interleaved 2/5, IATA, Code 128, +Code 93, GS1 DataBar, GS1 DataBar Limited. Add-on (supplemental) codes, +postal codes, MSI/Plessey, Telepen, UK/Plessey, and Code 11 are off by default. + +To re-enable / disable individual symbologies, use one of: + +1. **Denso's MDI Setup Windows utility** (free from Denso ADC) — checkbox UI + over the same RS-232 port, writes to the reader's NVRAM. +2. **Configuration barcodes** printed in the MDI-4050 setting manual — scan + a Start → toggle → End sequence, no host needed. +3. Direct register writes via the ESC protocol. The ``[NN]`` sections in the + ``Z4`` dump correspond to NVRAM registers but the per-symbology byte + layout is documented only in Denso's protocol manual; reverse-engineering + from a ``Z4`` dump alone is fragile. + +Use :meth:`KX2BarcodeReaderDriver.dump_config` to read the current state. + +============================================================================ +USB-to-serial driver setup (macOS) +============================================================================ + +The reader connects via a Prolific PL2303 USB-to-serial cable. macOS bundles a +`AppleUSBPLCOM.dext` for older PL2303 silicon (TA/EA), but newer chips (GC/HXN, +USB ID ``067b:23a3`` and ``067b:2303``) need Prolific's vendor DriverKit +extension: + + 1. ``brew install --cask prolific-pl2303`` + 2. Launch ``/Applications/PL2303Serial.app`` once so macOS registers the + system extension. + 3. Open **System Settings → Privacy & Security**, scroll to the bottom, and + click **Allow** on the "System software from PL2303Serial was blocked" + prompt. macOS may require a restart. + 4. Replug the USB cable. The device shows up as + ``/dev/tty.PL2303G-USBtoUART`` (or ``/dev/cu.PL2303G-USBtoUART``). + Pass that path to :class:`KX2BarcodeReader`. + +To verify before instantiating, ``systemextensionsctl list`` should show +``com.prolific.cdc.PLCdcFSDriver`` in state ``[activated enabled]``. + +On Linux, the PL2303 chip is supported by the in-tree ``pl2303`` driver and +shows up as ``/dev/ttyUSB`` with no extra setup. +""" + +from __future__ import annotations + +import asyncio +import logging +import time +from typing import Literal, Optional + +ReadMode = Literal["single", "multiple", "continuous"] + +try: + import serial as pyserial + + _HAS_SERIAL = True +except ImportError as _e: + _HAS_SERIAL = False + _SERIAL_IMPORT_ERROR = _e + +from pylabrobot.capabilities.barcode_scanning import BarcodeScanner +from pylabrobot.capabilities.barcode_scanning.backend import ( + BarcodeScannerBackend, + BarcodeScannerError, +) +from pylabrobot.capabilities.capability import BackendParams +from pylabrobot.device import Device, Driver +from pylabrobot.io.serial import Serial +from pylabrobot.resources.barcode import Barcode + +logger = logging.getLogger(__name__) + +_ESC = b"\x1b" +_CR = b"\r" +# Vendor C# uses `SerialPort.WriteLine(ESC + cmd + "\r")`, which appends the +# default .NET `NewLine` of `"\n"` — so the actual frame on the wire is +# ESC + cmd + CR + LF. Responses are split on CR alone (see +# SerialPortBCR_DataReceived in KX2RobotControl.cs). +_CMD_TERM = b"\r\n" + + +class KX2BarcodeReaderDriver(Driver): + """Serial driver for the KX2's onboard Microscan-style barcode reader. + + Factory defaults (per `KX2RobotControl.cs:1712–1741`): 9600 8N1, no flow + control. All commands are `ESC + + CR`; all responses are + ` + CR`. + """ + + default_baudrate = 9600 + + def __init__(self, port: str, baudrate: int = default_baudrate): + if not _HAS_SERIAL: + raise RuntimeError( + "pyserial is not installed. Install with: pip install pylabrobot[serial]. " + f"Import error: {_SERIAL_IMPORT_ERROR}" + ) + super().__init__() + self.io = Serial( + human_readable_device_name="KX2 Barcode Reader", + port=port, + baudrate=baudrate, + bytesize=pyserial.EIGHTBITS, + parity=pyserial.PARITY_NONE, + stopbits=pyserial.STOPBITS_ONE, + write_timeout=1, + timeout=0.1, # short per-byte timeout; send_command handles the real deadline + rtscts=False, + ) + + async def setup(self, backend_params: Optional[BackendParams] = None) -> None: + await self.io.setup() + logger.info("[KX2 BCR %s] connected", self.io.port) + + async def stop(self) -> None: + # Match the C# teardown: turn trigger off + restore default read time + # (KX2RobotControl.cs:15623–15624) so the reader is left in a known state. + await self.send_command("Y") + await self.send_command("Y2") # read time = 2s + await self.io.stop() + logger.info("[KX2 BCR %s] disconnected", self.io.port) + + async def _read_until_cr(self, timeout: float) -> str: + """Read from the port until we see a CR, returning the decoded line. + + Raises `BarcodeScannerError` on timeout. + """ + deadline = time.monotonic() + timeout + buf = bytearray() + while time.monotonic() < deadline: + chunk = await self.io.read(32) + if chunk: + buf.extend(chunk) + if _CR in buf: + line, _, _ = buf.partition(_CR) + decoded = line.decode("ascii", errors="replace").lstrip("\x1b").rstrip() + return decoded + else: + await asyncio.sleep(0.01) + raise BarcodeScannerError( + f"KX2 barcode reader: timeout waiting for CR after {timeout}s (buffered={bytes(buf)!r})" + ) + + async def send_command(self, cmd: str) -> None: + """Fire-and-forget: send `ESC + cmd + CR + LF` and return immediately. + + The reader doesn't echo most commands (S0/S1/S2, Y/Yn/YM, Z trigger, + +I/+F). Use :meth:`query` for commands that return a value (e.g. Z1). + Mirrors C# `BarcodeReaderSendCommand` with ``Response=""`` — see + `KX2RobotControl.cs:15639–15700`. + """ + payload = _ESC + cmd.encode("ascii") + _CMD_TERM + await self.io.write(payload) + logger.debug("[KX2 BCR %s] %s (no wait)", self.io.port, cmd) + + async def query(self, cmd: str, timeout: float = 5.0) -> str: + """Send a command and wait for a CR-terminated response. + + Use only for commands that the reader actually replies to (Z1 + software version). Other "set" commands time out here because the + reader is silent on success. + """ + payload = _ESC + cmd.encode("ascii") + _CMD_TERM + await self.io.write(payload) + decoded = await self._read_until_cr(timeout) + logger.debug("[KX2 BCR %s] %s -> %r", self.io.port, cmd, decoded) + return decoded + + async def read_decoded_barcode(self, timeout: float) -> str: + """Listen for an asynchronously-delivered decoded barcode line. + + Used after firing `trigger(True)` — the reader emits the decoded data + followed by CR whenever it makes a successful read. + """ + return await self._read_until_cr(timeout) + + # --- typed command helpers (names mirror the C# API) -------------------- + + async def trigger(self, on: bool) -> None: + await self.send_command("Z" if on else "Y") + + async def set_read_mode(self, mode: ReadMode) -> None: + """Maps to S0/S1/S2 on the wire.""" + code = {"single": "S0", "multiple": "S1", "continuous": "S2"}[mode] + await self.send_command(code) + + async def set_read_time(self, seconds: int) -> None: + """seconds: 1..9, or 0 for indefinite (mapped to YM).""" + if seconds == 0: + await self.send_command("YM") + elif 1 <= seconds <= 9: + await self.send_command(f"Y{seconds}") + else: + raise ValueError("read_time must be 0 (indefinite) or 1..9 seconds") + + async def set_auto_trigger(self, on: bool) -> None: + await self.send_command("+I" if on else "+F") + + async def get_software_version(self) -> str: + return await self.query("Z1") + + async def dump_config(self, timeout: float = 8.0) -> str: + """Return the reader's full NVRAM dump (response to ``Z4``). + + Includes the model string, firmware version, interface type, and the + complete symbology / prefix-suffix / length-limit table. The reply is + multi-KB and the reader streams it over a few hundred ms, so we read + until a quiet period rather than a single CR. + """ + payload = _ESC + b"Z4" + _CMD_TERM + await self.io.write(payload) + deadline = time.monotonic() + timeout + last_byte = time.monotonic() + buf = bytearray() + while time.monotonic() < deadline: + chunk = await self.io.read(2048) + if chunk: + buf.extend(chunk) + last_byte = time.monotonic() + else: + if buf and time.monotonic() - last_byte > 1.0: + break + await asyncio.sleep(0.05) + return buf.decode("ascii", errors="replace") + + +class KX2BarcodeReaderBackend(BarcodeScannerBackend): + """Adapts :class:`KX2BarcodeReaderDriver` to the BarcodeScanner capability.""" + + def __init__(self, driver: KX2BarcodeReaderDriver, read_time: int = 2): + super().__init__() + self.driver = driver + self._read_time = read_time + + async def set_read_time(self, seconds: int) -> None: + """Update both the reader's read window and our scan timeout.""" + await self.driver.set_read_time(seconds) + self._read_time = seconds + + async def _on_setup(self, backend_params: Optional[BackendParams] = None) -> None: + # Handshake: version query (mirrors KX2RobotControl.cs:15617). + version = await self.driver.get_software_version() + if not version: + raise BarcodeScannerError( + "KX2 barcode reader: empty software-version response during handshake. " + "Verify port, baud rate, and that the reader is powered on." + ) + logger.info("[KX2 BCR %s] software version: %s", self.driver.io.port, version) + await self.driver.set_read_mode("single") + await self.set_read_time(self._read_time) + + async def scan_barcode(self) -> Barcode: + # Fire the trigger, then listen for the decoded line. The reader delivers + # data asynchronously (not as a command-response), so we read-until-CR + # rather than sending another command. + await self.driver.trigger(True) + data = await self.driver.read_decoded_barcode(timeout=float(self._read_time + 1)) + if not data: + raise BarcodeScannerError("KX2 barcode reader: no read within read-time window") + return Barcode(data=data, symbology="ANY 1D", position_on_resource="front") + + +class KX2BarcodeReader(Device): + """PAA KX2 onboard barcode reader (Microscan-style serial device). + + Args: + port: Serial device path. On macOS this is typically + ``/dev/tty.PL2303G-USBtoUART`` (after the Prolific driver is + installed and approved — see module docstring). On Linux it's + usually ``/dev/ttyUSB``. + read_time: Default read window in seconds (1–9, or 0 for indefinite). + The reader returns the decoded barcode if it sees one within this + window, otherwise the scan call raises. + baudrate: Serial baud rate; the reader's factory default is 9600. + + Usage:: + + bcr = KX2BarcodeReader(port="/dev/tty.PL2303G-USBtoUART11240", read_time=8) + await bcr.setup() + barcode = await bcr.barcode_scanning.scan() + print(barcode.data) + await bcr.stop() + """ + + def __init__( + self, + port: str, + read_time: int = 2, + baudrate: int = KX2BarcodeReaderDriver.default_baudrate, + ): + driver = KX2BarcodeReaderDriver(port=port, baudrate=baudrate) + super().__init__(driver=driver) + self.driver: KX2BarcodeReaderDriver = driver + self._backend = KX2BarcodeReaderBackend(driver, read_time=read_time) + self.barcode_scanning = BarcodeScanner(backend=self._backend) + self._capabilities = [self.barcode_scanning] + + async def set_read_time(self, seconds: int) -> None: + """Set the read-window timeout (1–9 seconds, or 0 for indefinite). + + Updates both the reader and the scan-completion timeout in lockstep, + so subsequent ``barcode_scanning.scan()`` calls wait long enough. + """ + await self._backend.set_read_time(seconds) + + async def set_read_mode(self, mode: ReadMode) -> None: + """Set the trigger mode: ``"single"`` (default), ``"multiple"``, or + ``"continuous"``.""" + await self.driver.set_read_mode(mode) diff --git a/pylabrobot/paa/kx2/kx2_barcode_reader.py b/pylabrobot/paa/kx2/kx2_barcode_reader.py deleted file mode 100644 index a5eeb33ec95..00000000000 --- a/pylabrobot/paa/kx2/kx2_barcode_reader.py +++ /dev/null @@ -1,196 +0,0 @@ -"""PAA KX2 onboard barcode reader. - -The KX2's onboard barcode reader is a plain RS-232 device (Microscan/Omron-style -ESC-prefixed, CR-terminated commands) wired to the controller PC — entirely -independent of the CAN bus that drives the motors. As such it lives in its own -`Device` class alongside :class:`KX2`, mirroring the Keyence scanner pattern. - -Protocol reference: `KX2RobotControl.cs:15573–15848` in the vendor SDK. -""" - -from __future__ import annotations - -import asyncio -import logging -import time -from typing import Optional - -try: - import serial as pyserial - - _HAS_SERIAL = True -except ImportError as _e: - _HAS_SERIAL = False - _SERIAL_IMPORT_ERROR = _e - -from pylabrobot.capabilities.barcode_scanning import BarcodeScanner -from pylabrobot.capabilities.barcode_scanning.backend import ( - BarcodeScannerBackend, - BarcodeScannerError, -) -from pylabrobot.capabilities.capability import BackendParams -from pylabrobot.device import Device, Driver -from pylabrobot.io.serial import Serial -from pylabrobot.resources.barcode import Barcode - -logger = logging.getLogger(__name__) - -_ESC = b"\x1b" -_CR = b"\r" -# Vendor C# uses `SerialPort.WriteLine(ESC + cmd + "\r")`, which appends the -# default .NET `NewLine` of `"\n"` — so the actual frame on the wire is -# ESC + cmd + CR + LF. Responses are split on CR alone (see -# SerialPortBCR_DataReceived in KX2RobotControl.cs). -_CMD_TERM = b"\r\n" - - -class KX2BarcodeReaderDriver(Driver): - """Serial driver for the KX2's onboard Microscan-style barcode reader. - - Factory defaults (per `KX2RobotControl.cs:1712–1741`): 9600 8N1, no flow - control. All commands are `ESC + + CR`; all responses are - ` + CR`. - """ - - default_baudrate = 9600 - - def __init__(self, port: str, baudrate: int = default_baudrate): - if not _HAS_SERIAL: - raise RuntimeError( - "pyserial is not installed. Install with: pip install pylabrobot[serial]. " - f"Import error: {_SERIAL_IMPORT_ERROR}" - ) - super().__init__() - self.io = Serial( - human_readable_device_name="KX2 Barcode Reader", - port=port, - baudrate=baudrate, - bytesize=pyserial.EIGHTBITS, - parity=pyserial.PARITY_NONE, - stopbits=pyserial.STOPBITS_ONE, - write_timeout=1, - timeout=0.1, # short per-byte timeout; send_command handles the real deadline - rtscts=False, - ) - - async def setup(self, backend_params: Optional[BackendParams] = None) -> None: - await self.io.setup() - logger.info("[KX2 BCR %s] connected", self.io.port) - - async def stop(self) -> None: - # Match the C# teardown: turn trigger off + restore default read time - # (KX2RobotControl.cs:15623–15624) so the reader is left in a known state. - try: - await self.send_command("Y", timeout=1.0) - except BarcodeScannerError: - pass - try: - await self.send_command("Y2", timeout=1.0) # read time = 2s - except BarcodeScannerError: - pass - await self.io.stop() - logger.info("[KX2 BCR %s] disconnected", self.io.port) - - async def _read_until_cr(self, timeout: float) -> str: - """Read from the port until we see a CR, returning the decoded line. - - Raises `BarcodeScannerError` on timeout. - """ - deadline = time.monotonic() + timeout - buf = bytearray() - while time.monotonic() < deadline: - chunk = await self.io.read(32) - if chunk: - buf.extend(chunk) - if _CR in buf: - line, _, _ = buf.partition(_CR) - decoded = line.decode("ascii", errors="replace").lstrip("\x1b").rstrip() - return decoded - else: - await asyncio.sleep(0.01) - raise BarcodeScannerError( - f"KX2 barcode reader: timeout waiting for CR after {timeout}s (buffered={bytes(buf)!r})" - ) - - async def send_command(self, cmd: str, timeout: float = 5.0) -> str: - """Send `ESC + cmd + CR + LF` and return the response up to the terminating CR.""" - payload = _ESC + cmd.encode("ascii") + _CMD_TERM - await self.io.write(payload) - decoded = await self._read_until_cr(timeout) - logger.debug("[KX2 BCR %s] %s -> %r", self.io.port, cmd, decoded) - return decoded - - async def read_decoded_barcode(self, timeout: float) -> str: - """Listen for an asynchronously-delivered decoded barcode line. - - Used after firing `trigger(True)` — the reader emits the decoded data - followed by CR whenever it makes a successful read. - """ - return await self._read_until_cr(timeout) - - # --- typed command helpers (names mirror the C# API) -------------------- - - async def trigger(self, on: bool) -> None: - await self.send_command("Z" if on else "Y") - - async def set_read_mode(self, mode: str) -> None: - """mode: 'single' | 'multiple' | 'continuous' (maps to S0/S1/S2).""" - code = {"single": "S0", "multiple": "S1", "continuous": "S2"}[mode] - await self.send_command(code) - - async def set_read_time(self, seconds: int) -> None: - """seconds: 1..9, or 0 for indefinite (mapped to YM).""" - if seconds == 0: - await self.send_command("YM") - elif 1 <= seconds <= 9: - await self.send_command(f"Y{seconds}") - else: - raise ValueError("read_time must be 0 (indefinite) or 1..9 seconds") - - async def set_auto_trigger(self, on: bool) -> None: - await self.send_command("+I" if on else "+F") - - async def get_software_version(self) -> str: - return await self.send_command("Z1") - - -class KX2BarcodeReaderBackend(BarcodeScannerBackend): - """Adapts :class:`KX2BarcodeReaderDriver` to the BarcodeScanner capability.""" - - def __init__(self, driver: KX2BarcodeReaderDriver, read_time: int = 2): - super().__init__() - self.driver = driver - self._read_time = read_time - - async def _on_setup(self, backend_params: Optional[BackendParams] = None) -> None: - # Handshake: version query (mirrors KX2RobotControl.cs:15617). - version = await self.driver.get_software_version() - if not version: - raise BarcodeScannerError( - "KX2 barcode reader: empty software-version response during handshake. " - "Verify port, baud rate, and that the reader is powered on." - ) - logger.info("[KX2 BCR %s] software version: %s", self.driver.io.port, version) - await self.driver.set_read_mode("single") - await self.driver.set_read_time(self._read_time) - - async def scan_barcode(self) -> Barcode: - # Fire the trigger, then listen for the decoded line. The reader delivers - # data asynchronously (not as a command-response), so we read-until-CR - # rather than sending another command. - await self.driver.trigger(True) - data = await self.driver.read_decoded_barcode(timeout=float(self._read_time + 1)) - if not data: - raise BarcodeScannerError("KX2 barcode reader: no read within read-time window") - return Barcode(data=data, symbology="ANY 1D", position_on_resource="front") - - -class KX2BarcodeReader(Device): - """PAA KX2 onboard barcode reader (Microscan-style serial device).""" - - def __init__(self, port: str, baudrate: int = KX2BarcodeReaderDriver.default_baudrate): - driver = KX2BarcodeReaderDriver(port=port, baudrate=baudrate) - super().__init__(driver=driver) - self.driver: KX2BarcodeReaderDriver = driver - self.barcode_scanning = BarcodeScanner(backend=KX2BarcodeReaderBackend(driver)) - self._capabilities = [self.barcode_scanning] From ea04caf103d588952dd304b09dd2d9ce7b2c834f Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sat, 25 Apr 2026 17:23:26 -0700 Subject: [PATCH 36/93] KX2 proximity sensor: read + drive-side auto-halt Z search MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Exposes the gripper-fingers IR breakbeam (Z drive's IO pin 4, auto-discovered as "ProximitySensor"). Adds `read_proximity_sensor`, `wait_for_proximity_sensor`, and `find_z_with_proximity_sensor` — the last one descends Z and uses Elmo IL=StopForward so the drive halts the motor itself on the input edge (no software in the loop). Polls the sensor in parallel and tracks trip via a flag (re-reading after halt is unreliable since the arm decelerates a few mm and the beam often un-trips). On exit, mirrors C# `MotorStop`: CW=271 + 0x6060 mode kick to clear the post-IL stuck state so subsequent moves don't hang. `InputLogic` enum + `read_input_logic` / `configure_input_logic` go on the driver alongside the existing `read_input` / `set_output` I/O primitives. Co-Authored-By: Claude Opus 4.7 (1M context) --- docs/user_guide/paa/kx2/hello-world.ipynb | 118 +++++++++++++++++++++- pylabrobot/paa/kx2/arm_backend.py | 88 +++++++++++++++- pylabrobot/paa/kx2/driver.py | 32 ++++++ 3 files changed, 232 insertions(+), 6 deletions(-) diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 070b174d018..64afa17d8fd 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -39,7 +39,19 @@ "output_type": "stream", "text": [ "uptime library not available, timestamps are relative to boot time and not to Epoch UTC\n", - "2026-04-14 16:37:07,444 - pylabrobot.paa.kx2.kx2_driver - INFO - canopen: connected, nodes=[1, 2, 3, 4, 6]\n" + "2026-04-25 17:22:25,930 - pylabrobot.paa.kx2.driver - INFO - canopen: connected, nodes=[1, 2, 3, 4, 6]\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[MOVE PLAN] move_time=10.000s, 1 axes:\n", + " node=6 pos=-280 vel=100000 acc=100000 dir=ShortestWay\n", + "[MOVE PLAN] move_time=10.000s, 1 axes:\n", + " node=6 pos=1 vel=100000 acc=100000 dir=ShortestWay\n", + "[MOVE PLAN] move_time=0.791s, 1 axes:\n", + " node=6 pos=0 vel=149606 acc=636620 dir=Normal\n" ] } ], @@ -136,6 +148,61 @@ "await kx2.arm.backend.servo_gripper_set_default_gripping_force(max_force_percent=30)" ] }, + { + "cell_type": "markdown", + "id": "kx2-proximity-header", + "metadata": {}, + "source": [ + "### Proximity sensor\n", + "\n", + "An IR breakbeam between the gripper fingers, wired to the Z drive's IO header. The sensor is binary (no distance, just present/absent) and is exposed via the auto-discovered `\"ProximitySensor\"` digital input on the Z axis (pin 4).\n", + "\n", + "`read_proximity_sensor()` returns `True` when the beam is interrupted. Useful for confirming an object is in the gripper before closing." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-proximity-read", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.arm.backend.read_proximity_sensor()" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-proximity-search-md", + "metadata": {}, + "source": [ + "`find_z_with_proximity_sensor` descends Z up to `max_descent` and halts the instant the beam trips. It arms the Elmo drive's input-logic register so the drive itself stops the motor on the input edge (sub-ms reaction; no software in the loop). Returns the Z where the drive halted; raises `RuntimeError` if the beam never tripped.\n", + "\n", + "Pass `z_start` to first move Z to a known starting height before searching. Position the arm above an object and open the gripper wider than the object before running." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "kx2-proximity-search", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[MOVE PLAN] move_time=1.550s, 1 axes:\n", + " node=2 pos=1599135 vel=149919 acc=199892 dir=Normal\n", + "[MOVE PLAN] move_time=3.417s, 1 axes:\n", + " node=2 pos=1079426 vel=149919 acc=199892 dir=Normal\n", + "object found at Z = 323.49\n" + ] + } + ], + "source": [ + "z_hit = await kx2.arm.backend.find_z_with_proximity_sensor(max_descent=100.0, z_start=400)\n", + "print(f\"object found at Z = {z_hit:.2f}\")" + ] + }, { "cell_type": "markdown", "id": "kx2-cartesian-header", @@ -187,7 +254,15 @@ "cell_type": "markdown", "id": "kx2-joint-header", "metadata": {}, - "source": "### Joint movement\n\nMove in joint space using the {class}`~pylabrobot.paa.kx2.KX2ArmBackend.Axis` enum. Rotary axes in degrees; Z (and gripper) in mm-equivalent encoder units.\n\n```{warning}\nMoving to arbitrary joint positions can cause the arm to collide with its base, gripper, or nearby equipment. Verify coordinates in a safe workspace first, and start with low velocity.\n```" + "source": [ + "### Joint movement\n", + "\n", + "Move in joint space using the {class}`~pylabrobot.paa.kx2.KX2ArmBackend.Axis` enum. Rotary axes in degrees; Z (and gripper) in mm-equivalent encoder units.\n", + "\n", + "```{warning}\n", + "Moving to arbitrary joint positions can cause the arm to collide with its base, gripper, or nearby equipment. Verify coordinates in a safe workspace first, and start with low velocity.\n", + "```" + ] }, { "cell_type": "code", @@ -195,7 +270,20 @@ "id": "kx2-joint-code", "metadata": {}, "outputs": [], - "source": "from pylabrobot.paa.kx2 import KX2ArmBackend\n\nposition = {\n KX2ArmBackend.Axis.SHOULDER: 0.0,\n KX2ArmBackend.Axis.Z: 150.0,\n KX2ArmBackend.Axis.ELBOW: 90.0,\n KX2ArmBackend.Axis.WRIST: 0.0,\n}\nawait kx2.arm.backend.move_to_joint_position(\n position,\n backend_params=KX2ArmBackend.JointMoveParams(vel_pct=25, accel_pct=25),\n)" + "source": [ + "from pylabrobot.paa.kx2 import KX2ArmBackend\n", + "\n", + "position = {\n", + " KX2ArmBackend.Axis.SHOULDER: 0.0,\n", + " KX2ArmBackend.Axis.Z: 150.0,\n", + " KX2ArmBackend.Axis.ELBOW: 90.0,\n", + " KX2ArmBackend.Axis.WRIST: 0.0,\n", + "}\n", + "await kx2.arm.backend.move_to_joint_position(\n", + " position,\n", + " backend_params=KX2ArmBackend.JointMoveParams(vel_pct=25, accel_pct=25),\n", + ")" + ] }, { "cell_type": "markdown", @@ -213,7 +301,23 @@ "id": "0a19ceeb", "metadata": {}, "outputs": [], - "source": "pick_joints = {\n KX2ArmBackend.Axis.SHOULDER: 0.0,\n KX2ArmBackend.Axis.Z: 150.0,\n KX2ArmBackend.Axis.ELBOW: 90.0,\n KX2ArmBackend.Axis.WRIST: 0.0,\n}\nplace_joints = {\n KX2ArmBackend.Axis.SHOULDER: 30.0,\n KX2ArmBackend.Axis.Z: 150.0,\n KX2ArmBackend.Axis.ELBOW: 90.0,\n KX2ArmBackend.Axis.WRIST: 0.0,\n}\n\nawait kx2.arm.backend.pick_up_at_joint_position(pick_joints, resource_width=0)\nawait kx2.arm.backend.drop_at_joint_position(place_joints, resource_width=30)" + "source": [ + "pick_joints = {\n", + " KX2ArmBackend.Axis.SHOULDER: 0.0,\n", + " KX2ArmBackend.Axis.Z: 150.0,\n", + " KX2ArmBackend.Axis.ELBOW: 90.0,\n", + " KX2ArmBackend.Axis.WRIST: 0.0,\n", + "}\n", + "place_joints = {\n", + " KX2ArmBackend.Axis.SHOULDER: 30.0,\n", + " KX2ArmBackend.Axis.Z: 150.0,\n", + " KX2ArmBackend.Axis.ELBOW: 90.0,\n", + " KX2ArmBackend.Axis.WRIST: 0.0,\n", + "}\n", + "\n", + "await kx2.arm.backend.pick_up_at_joint_position(pick_joints, resource_width=0)\n", + "await kx2.arm.backend.drop_at_joint_position(place_joints, resource_width=30)" + ] }, { "cell_type": "markdown", @@ -404,7 +508,11 @@ "id": "kx2-fault", "metadata": {}, "outputs": [], - "source": "from pylabrobot.paa.kx2 import KX2ArmBackend\n\nawait kx2.driver.motor_get_fault(KX2ArmBackend.Axis.SHOULDER)" + "source": [ + "from pylabrobot.paa.kx2 import KX2ArmBackend\n", + "\n", + "await kx2.driver.motor_get_fault(KX2ArmBackend.Axis.SHOULDER)" + ] }, { "cell_type": "markdown", diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 3099a8153ab..facda78cd1b 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -2,6 +2,7 @@ import dataclasses import logging import math +import time import warnings from dataclasses import dataclass from enum import IntEnum @@ -24,6 +25,7 @@ ) from pylabrobot.paa.kx2.driver import ( CanError, + InputLogic, JointMoveDirection, KX2Driver, MotorMoveParam, @@ -453,7 +455,7 @@ async def drive_get_parameters(self, node_ids) -> None: ret = await self.driver.query_int(axis, "UI", ui_idx) ch = (ui_idx - 5) + 1 if ret == 101: - digital_inputs[ch] = "GripperSensor" + digital_inputs[ch] = "ProximitySensor" elif ret == 102: digital_inputs[ch] = "TeachButton" else: @@ -647,6 +649,90 @@ async def motor_get_current_position(self, axis: Axis) -> float: async def read_input(self, axis: Axis, input_num: int) -> bool: return await self.driver.read_input(node_id=axis, input_num=0x10 + input_num) + # IR breakbeam between the gripper fingers, wired to the Z-drive's IO. + # True = beam interrupted (object present). + _PROXIMITY_SENSOR_AXIS: Axis = Axis.Z + _PROXIMITY_SENSOR_INPUT: int = 4 + + async def read_proximity_sensor(self) -> bool: + return await self.read_input(self._PROXIMITY_SENSOR_AXIS, self._PROXIMITY_SENSOR_INPUT) + + async def wait_for_proximity_sensor( + self, state: bool = True, timeout: float = 5.0, poll: float = 0.01, + ) -> bool: + """Poll until the sensor reads `state`. Returns True on trip, False on timeout.""" + deadline = time.monotonic() + timeout + while True: + if await self.read_proximity_sensor() == state: + return True + if time.monotonic() >= deadline: + return False + await asyncio.sleep(poll) + + async def find_z_with_proximity_sensor( + self, + max_descent: float, + z_start: Optional[float] = None, + vel_pct: float = 5.0, + accel_pct: float = 5.0, + ) -> float: + """Descend Z up to `max_descent`; halt when the IR breakbeam trips. + + If `z_start` is given, first move Z to that height (same vel/accel) and + search from there; otherwise search from the current Z. Arms IL[4]= + StopForward so the Elmo drive halts the motor itself on the input edge + (sub-ms latency, no software in the loop). IL is restored to GeneralPurpose + afterwards even if the move raises. Returns the Z position where the drive + halted; raises RuntimeError if the beam never tripped. + """ + move_params = KX2ArmBackend.JointMoveParams(vel_pct=vel_pct, accel_pct=accel_pct) + if z_start is not None: + await self.move_to_joint_position({Axis.Z: z_start}, backend_params=move_params) + z0 = await self.motor_get_current_position(Axis.Z) + if await self.read_proximity_sensor(): + raise RuntimeError( + f"proximity sensor already tripped at start (Z {z0:.2f}); " + f"clear the gripper or raise z_start before searching" + ) + await self.driver.configure_input_logic( + int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, InputLogic.StopForward, + ) + move_task = asyncio.create_task( + self.move_to_joint_position({Axis.Z: z0 - max_descent}, backend_params=move_params) + ) + tripped = False + try: + # The drive halts itself via IL the moment the beam breaks. We poll the + # sensor in parallel so we can stop waiting for "move done" (which never + # arrives — the drive halted short of target). + while not move_task.done(): + if await self.read_proximity_sensor(): + tripped = True + break + await asyncio.sleep(0.01) + move_task.cancel() + try: + await move_task + except (asyncio.CancelledError, CanError): + pass + finally: + await self.driver.configure_input_logic( + int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, InputLogic.GeneralPurpose, + ) + # Mirror C# MotorStop (clscanmotor.cs:5517): controlled-halt CW + mode- + # of-operation kick to clear stuck post-IL state. Without the mode kick + # the next move sees MS hung and times out the same way. + await self.driver._control_word_set(int(Axis.Z), 271) + await asyncio.sleep(0.1) + await self.driver._can_sdo_download(int(Axis.Z), 0x60, 0x60, 0x00, [7]) + await self.driver._can_sdo_download(int(Axis.Z), 0x60, 0x60, 0x00, [1]) + if not tripped: + z_end = await self.motor_get_current_position(Axis.Z) + raise RuntimeError( + f"proximity sensor never tripped within {max_descent} (Z {z0:.2f} -> {z_end:.2f})" + ) + return await self.motor_get_current_position(Axis.Z) + @staticmethod def _wrap_to_range(x: float, lo: float, hi: float) -> float: span = hi - lo diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index afe04692d31..6156c0e18f2 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -127,6 +127,19 @@ class CanError(Exception): """Custom exception for CAN motor errors.""" +class InputLogic(IntEnum): + """Elmo SimplIQ IL[N] codes. Even = active-low; odd (value+1) = active-high.""" + GeneralPurpose = 0 + StopForward = 2 + StopReverse = 4 + BeginMotion = 6 + SoftStop = 8 + MainHomeEnable = 10 + AuxHomeEnable = 12 + StopUnderControl = 14 + AbortMotion = 16 + + class JointMoveDirection(IntEnum): """Move-direction hint used by the driver's move primitives. @@ -944,3 +957,22 @@ async def read_output(self, node_id: int, output_num: int) -> bool: async def set_output(self, node_id: int, output_num: int, state: bool) -> None: await self.write(node_id, "OB", output_num, 1 if state else 0) + + async def read_input_logic(self, node_id: int, input_num: int) -> int: + return await self.query_int(node_id, "IL", input_num) + + async def configure_input_logic( + self, node_id: int, input_num: int, logic: int, logic_high: bool = False, + ) -> None: + """Set IL[input_num]: drive auto-acts on input edges (e.g. halt motion). + + Pass an `InputLogic` member or raw int for `logic`. With `StopForward` the + drive halts the motor itself the instant the input trips during forward + motion — no software in the loop. Skips the write if value already matches; + settles 250ms after a real change (Elmo IL needs time to apply). + """ + value = int(logic) + (1 if logic_high else 0) + if await self.read_input_logic(node_id, input_num) == value: + return + await self.write(node_id, "IL", input_num, value) + await asyncio.sleep(0.25) From 75fa5f9894c70e945796afe4b08b43272b97b8f6 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 26 Apr 2026 15:58:16 -0700 Subject: [PATCH 37/93] KX2 proximity search: port C# MotorStop, fix post-halt drive recovery MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Cleanup after the IL-induced halt now mirrors the C# search (KX2RobotControl.cs:8650-8658) order: motor_stop FIRST, then restore IL. The reverse order (clear IL, then halt) let the drive surge toward the unreached target in the gap and left Z stuck until a power cycle — that's why "Z barely moved" kept appearing on retries. New driver.motor_stop() is a port of C# MotorStop (clscanmotor.cs:5517): CW=271 + 100ms settle + 0x6060 mode-of-operation kick. Replaces the earlier wait_for_moves_done(2.5s) which always timed out (MS never reaches 0 after a halt) and added 2-3s of dead time after every search. Pre-flight motor_enable on Z recovers a previously-stuck drive without a power cycle, so a single bad run no longer poisons the next. KX2.setup() is now idempotent — re-running on a live device logs and returns instead of crashing partway through homing. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/arm_backend.py | 27 +++++++++++++++++---------- pylabrobot/paa/kx2/driver.py | 18 ++++++++++++++++++ pylabrobot/paa/kx2/kx2.py | 15 +++++++++++++++ 3 files changed, 50 insertions(+), 10 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index facda78cd1b..7a83fee2f5b 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -686,6 +686,10 @@ async def find_z_with_proximity_sensor( halted; raises RuntimeError if the beam never tripped. """ move_params = KX2ArmBackend.JointMoveParams(vel_pct=vel_pct, accel_pct=accel_pct) + # Pre-flight: force the drive back to Op Enabled. A prior failed search + # could have left it in Fault/Quick Stop where new moves silently fail + # (Z barely moves). Idempotent if the drive is already healthy. + await self.driver.motor_enable(node_id=int(Axis.Z), state=True, use_ds402=True) if z_start is not None: await self.move_to_joint_position({Axis.Z: z_start}, backend_params=move_params) z0 = await self.motor_get_current_position(Axis.Z) @@ -716,16 +720,19 @@ async def find_z_with_proximity_sensor( except (asyncio.CancelledError, CanError): pass finally: - await self.driver.configure_input_logic( - int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, InputLogic.GeneralPurpose, - ) - # Mirror C# MotorStop (clscanmotor.cs:5517): controlled-halt CW + mode- - # of-operation kick to clear stuck post-IL state. Without the mode kick - # the next move sees MS hung and times out the same way. - await self.driver._control_word_set(int(Axis.Z), 271) - await asyncio.sleep(0.1) - await self.driver._can_sdo_download(int(Axis.Z), 0x60, 0x60, 0x00, [7]) - await self.driver._can_sdo_download(int(Axis.Z), 0x60, 0x60, 0x00, [1]) + # Match C# search cleanup (KX2RobotControl.cs:8650-8658): halt motor + # FIRST, then restore IL. Reverse order would let the drive surge toward + # the unreached target during the gap. + try: + await self.driver.motor_stop(int(Axis.Z)) + except Exception as e: + logger.warning("find_z: motor_stop failed: %s", e) + try: + await self.driver.configure_input_logic( + int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, InputLogic.GeneralPurpose, + ) + except Exception as e: + logger.warning("find_z: IL restore failed: %s", e) if not tripped: z_end = await self.motor_get_current_position(Axis.Z) raise RuntimeError( diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index 6156c0e18f2..d7d8093b9bb 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -958,6 +958,24 @@ async def read_output(self, node_id: int, output_num: int) -> bool: async def set_output(self, node_id: int, output_num: int, state: bool) -> None: await self.write(node_id, "OB", output_num, 1 if state else 0) + async def motor_stop(self, node_id: int, settle: float = 0.1) -> None: + """Controlled halt of one axis (port of C# MotorStop, clscanmotor.cs:5517). + + Sends CW=271 (Op Enabled + Halt — controlled deceleration, no power drop), + waits `settle` seconds for the drive to come to rest, then writes 0x6060 = 7 + then = 1 to clear the post-halt status-word state. Used after an IL-induced + auto-halt so the next move doesn't see a hung MS register. + + The C# version polls a TPDO-event flag with a 2.5s timeout. We can't reuse + `wait_for_moves_done` here because MS never goes to 0 after a halt — the + poll would just burn the full timeout. Drive deceleration is sub-100ms for + the search velocities used here, so a fixed sleep is fine. + """ + await self._control_word_set(node_id, 271) + await asyncio.sleep(settle) + await self._can_sdo_download(node_id, 0x60, 0x60, 0x00, [7]) + await self._can_sdo_download(node_id, 0x60, 0x60, 0x00, [1]) + async def read_input_logic(self, node_id: int, input_num: int) -> int: return await self.query_int(node_id, "IL", input_num) diff --git a/pylabrobot/paa/kx2/kx2.py b/pylabrobot/paa/kx2/kx2.py index 5b651107240..9e79e7c06f4 100644 --- a/pylabrobot/paa/kx2/kx2.py +++ b/pylabrobot/paa/kx2/kx2.py @@ -1,10 +1,16 @@ +import logging +from typing import Optional + from pylabrobot.capabilities.arms.orientable_arm import OrientableArm +from pylabrobot.capabilities.capability import BackendParams from pylabrobot.device import Device from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend from pylabrobot.paa.kx2.config import GripperFingerSide from pylabrobot.paa.kx2.driver import KX2Driver from pylabrobot.resources.resource import Resource +logger = logging.getLogger(__name__) + class KX2(Device): """PAA KX2 robotic plate handler.""" @@ -27,3 +33,12 @@ def __init__( self.reference = Resource(name="KX2", size_x=200, size_y=200, size_z=200) self.arm = OrientableArm(backend=backend, reference_resource=self.reference) self._capabilities = [self.arm] + + async def setup(self, backend_params: Optional[BackendParams] = None): + # Idempotent: re-running setup on a live KX2 used to crash partway through + # the homing/PDO sequence. If already up, stay up — call stop() first to + # force a fresh init. + if self.setup_finished: + logger.info("KX2.setup: already set up; skipping. Call stop() first to re-init.") + return + await super().setup(backend_params=backend_params) From 1fe13dab5468c7be13c3cdbd529cfa76b0180dca Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 26 Apr 2026 16:18:19 -0700 Subject: [PATCH 38/93] KX2 setup: split drive_get_parameters; raise on motor_enable failure drive_get_parameters was a 165-line monolith: a "Pass 1" UI[4] identity check (redundant with the driver's node discovery, which already verifies node IDs at driver.py:261), a bytes/bytearray normalization for an argument no caller ever passed as bytes, then a giant per-axis loop with inline if/elif chains for digital input + output role naming. Replaced with _read_config + _read_axis_config + _read_io_names + _read_servo_gripper_config. The two role lookups become module-level _DIGITAL_INPUT_NAMES / _OUTPUT_NAMES dicts. _on_setup is now seven lines of orchestration. Also dropped the try/except around motor_enable in the MOTION_AXES loop: the driver already retries 5 times and raises CanError on failure (driver.py:788), and the wrapper was downgrading that to a warning that let setup keep going against a half-enabled arm. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/arm_backend.py | 327 ++++++++++++++---------------- 1 file changed, 149 insertions(+), 178 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 7a83fee2f5b..caf73b84653 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -73,18 +73,12 @@ def __init__( async def _on_setup(self, backend_params: Optional[BackendParams] = None): # Driver has already brought CAN up (connect + node discovery + PDO - # mapping) via Device.setup(). Now read per-drive parameters, enable - # motion axes, and initialize the servo gripper. - await self.drive_get_parameters(self.driver.node_id_list) - - await asyncio.sleep(2) - + # mapping) via Device.setup(). Read per-drive config, then enable motion + # axes and the servo gripper. + self._config = await self._read_config() + await asyncio.sleep(2) # let drives settle before motor enables for axis in MOTION_AXES: - try: - await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) - except Exception as e: - logger.warning("Error enabling motor on axis %s: %s", axis, e) - + await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) await self.servo_gripper_initialize() # -- robot-level homing / estop (moved from driver) --------------------- @@ -424,183 +418,145 @@ async def drive_set_move_count_parameters( await self.driver.write(rail, "UI", 23, 1 if maintenance_required_rail else 0) await self.driver.write(rail, "UI", 21, int(last_maintenance_performed_date_rail)) - async def drive_get_parameters(self, node_ids) -> None: - nodes = ( - [int(b) for b in node_ids] - if isinstance(node_ids, (bytes, bytearray)) - else [int(x) for x in node_ids] - ) + async def _read_config(self) -> KX2Config: + """Read the per-arm configuration from the drives. - # Pass 1: identify axes by UI[4] - uis = set() - for node in nodes: - if node == await self.driver.query_int(int(node), "UI", 4): - uis.add(node) - for required_axis in MOTION_AXES: - if required_axis.value not in uis: - raise CanError(f"Missing required axis with UI[4]={required_axis}") - robot_on_rail = 5 in uis - has_servo_gripper = 6 in uis - if robot_on_rail: + Driver discovery has already populated `node_id_list` with everything + on the bus; here we just verify the required motion axes are present + and read each drive's parameters. + """ + nodes = self.driver.node_id_list + for required in MOTION_AXES: + if required not in nodes: + raise CanError(f"Missing required axis {required}") + has_rail = Axis.RAIL in nodes + has_servo_gripper = Axis.SERVO_GRIPPER in nodes + if has_rail: warnings.warn("Rails has not been tested for KX2 robots.") - # Pass 2: per-axis parameters axes: Dict[int, AxisConfig] = {} - for axis in nodes: - logger.debug("Reading parameters for axis %s", axis) - - # UI[5..10] digital inputs - digital_inputs: Dict[int, str] = {} - for ui_idx in range(5, 11): - ret = await self.driver.query_int(axis, "UI", ui_idx) - ch = (ui_idx - 5) + 1 - if ret == 101: - digital_inputs[ch] = "ProximitySensor" - elif ret == 102: - digital_inputs[ch] = "TeachButton" - else: - digital_inputs[ch] = "" if ret <= 0 else f"AuxPin{ret}" - - # UI[11..12] analog inputs - analog_inputs: Dict[int, str] = {} - for ui_idx in range(11, 13): - ret = await self.driver.query_int(axis, "UI", ui_idx) - ch = (ui_idx - 11) + 1 - analog_inputs[ch] = "" if ret <= 0 else f"AuxPin{ret}" - - # UI[13..16] outputs - outputs: Dict[int, str] = {} - for ui_idx in range(13, 17): - ret = await self.driver.query_int(axis, "UI", ui_idx) - ch = (ui_idx - 13) + 1 - if ret == 101: - outputs[ch] = "IndicatorLightRed" - elif ret == 102: - outputs[ch] = "IndicatorLightGreen" - elif ret == 103: - outputs[ch] = "IndicatorLightBlue" - elif ret == 104: - outputs[ch] = "IndicatorLight" - elif ret == 105: - outputs[ch] = "Buzzer" - else: - outputs[ch] = "" if ret <= 0 else f"AuxPin{ret}" - - # UI[24] drive serial number (queried but not currently stored) - await self.driver.query_int(axis, "UI", 24) - - # UF[1], UF[2] conversion factor - uf1 = await self.driver.query_float(axis, "UF", 1) - uf2 = await self.driver.query_float(axis, "UF", 2) - if uf1 == 0.0 or uf2 == 0.0: - raise CanError(f"Invalid Motor Conversion Factor for axis {axis}. UF[1]={uf1}, UF[2]={uf2}") - motor_conversion_factor = uf1 / uf2 - - # XM / travel - xm1 = await self.driver.query_int(axis, "XM", 1) - xm2 = await self.driver.query_int(axis, "XM", 2) - max_travel = await self.driver.query_float(axis, "UF", 3) - min_travel = await self.driver.query_float(axis, "UF", 4) - vh3 = await self.driver.query_int(axis, "VH", 3) - vl3 = await self.driver.query_int(axis, "VL", 3) - - joint_move_direction = JointMoveDirection.Normal - if (xm1 == 0 and xm2 == 0) or (xm1 <= vl3 and xm2 >= vh3): - unlimited_travel = False - elif xm1 > vl3 and xm2 < vh3: - unlimited_travel = True - if axis in MOTION_AXES: - joint_move_direction = JointMoveDirection.ShortestWay - else: - raise CanError( - f"Invalid travel limits or modulo settings for axis {axis}. " - f"VH[3]={vh3}, VL[3]={vl3}, XM[1]={xm1}, XM[2]={xm2}" - ) + for nid in nodes: + axes[nid] = await self._read_axis_config(nid) + + sh = int(Axis.SHOULDER) + return KX2Config( + wrist_offset=await self.driver.query_float(sh, "UF", 8), + elbow_offset=await self.driver.query_float(sh, "UF", 9), + elbow_zero_offset=await self.driver.query_float(sh, "UF", 10), + gripper_length=self._gripper_length, + gripper_z_offset=self._gripper_z_offset, + gripper_finger_side=self._gripper_finger_side, + axes=axes, + base_to_gripper_clearance_z=await self.driver.query_float(sh, "UF", 6), + base_to_gripper_clearance_arm=await self.driver.query_float(sh, "UF", 7), + robot_on_rail=has_rail, + servo_gripper=await self._read_servo_gripper_config() if has_servo_gripper else None, + ) - # Encoder socket/type - ca45 = await self.driver.query_int(axis, "CA", 45) - if not (0 < ca45 <= 4): - raise CanError(f"Invalid encoder socket number received from axis {axis}. CA[45]={ca45}") - enc_type = await self.driver.query_int(axis, "CA", 40 + ca45) - if enc_type in (1, 2): - absolute_encoder = False - elif enc_type == 24: - absolute_encoder = True - else: - raise CanError( - f"Unsupported encoder type specified for axis {axis}. CA[4{ca45}]={enc_type}" - ) + async def _read_axis_config(self, nid: int) -> AxisConfig: + logger.debug("Reading parameters for axis %s", nid) + + digital_inputs = await self._read_io_names(nid, 5, 11, _DIGITAL_INPUT_NAMES) + analog_inputs = await self._read_io_names(nid, 11, 13, {}) + outputs = await self._read_io_names(nid, 13, 17, _OUTPUT_NAMES) + + await self.driver.query_int(nid, "UI", 24) # serial — read for parity, unused + + uf1 = await self.driver.query_float(nid, "UF", 1) + uf2 = await self.driver.query_float(nid, "UF", 2) + if uf1 == 0.0 or uf2 == 0.0: + raise CanError(f"Invalid motor conversion factor for axis {nid}: UF[1]={uf1}, UF[2]={uf2}") + motor_conversion_factor = uf1 / uf2 + + xm1 = await self.driver.query_int(nid, "XM", 1) + xm2 = await self.driver.query_int(nid, "XM", 2) + max_travel = await self.driver.query_float(nid, "UF", 3) + min_travel = await self.driver.query_float(nid, "UF", 4) + vh3 = await self.driver.query_int(nid, "VH", 3) + vl3 = await self.driver.query_int(nid, "VL", 3) + + joint_move_direction = JointMoveDirection.Normal + if (xm1 == 0 and xm2 == 0) or (xm1 <= vl3 and xm2 >= vh3): + unlimited_travel = False + elif xm1 > vl3 and xm2 < vh3: + unlimited_travel = True + if nid in MOTION_AXES: + joint_move_direction = JointMoveDirection.ShortestWay + else: + raise CanError( + f"Invalid travel limits or modulo settings for axis {nid}: " + f"VH[3]={vh3}, VL[3]={vl3}, XM[1]={xm1}, XM[2]={xm2}" + ) - ca46 = await self.driver.query_int(axis, "CA", 46) - if ca45 == ca46: - num3 = 1.0 - else: - num3 = await self.driver.query_float(axis, "FF", 3) + ca45 = await self.driver.query_int(nid, "CA", 45) + if not (0 < ca45 <= 4): + raise CanError(f"Invalid encoder socket for axis {nid}: CA[45]={ca45}") + enc_type = await self.driver.query_int(nid, "CA", 40 + ca45) + if enc_type in (1, 2): + absolute_encoder = False + elif enc_type == 24: + absolute_encoder = True + else: + raise CanError(f"Unsupported encoder type for axis {nid}: CA[4{ca45}]={enc_type}") - denom = motor_conversion_factor * num3 + ca46 = await self.driver.query_int(nid, "CA", 46) + num3 = 1.0 if ca45 == ca46 else await self.driver.query_float(nid, "FF", 3) + denom = motor_conversion_factor * num3 - sp2 = await self.driver.query_int(axis, "SP", 2) - if sp2 == 100000: - vh2 = await self.driver.query_int(axis, "VH", 2) - max_vel = vh2 / 1.01 / denom - else: - max_vel = sp2 / denom - - sd0 = await self.driver.query_int(axis, "SD", 0) - max_accel = sd0 / 1.01 / denom - - axes[axis] = AxisConfig( - motor_conversion_factor=motor_conversion_factor, - max_travel=max_travel, - min_travel=min_travel, - unlimited_travel=unlimited_travel, - absolute_encoder=absolute_encoder, - max_vel=max_vel, - max_accel=max_accel, - joint_move_direction=joint_move_direction, - digital_inputs=digital_inputs, - analog_inputs=analog_inputs, - outputs=outputs, - ) + sp2 = await self.driver.query_int(nid, "SP", 2) + if sp2 == 100000: + max_vel = await self.driver.query_int(nid, "VH", 2) / 1.01 / denom + else: + max_vel = sp2 / denom + max_accel = await self.driver.query_int(nid, "SD", 0) / 1.01 / denom + + return AxisConfig( + motor_conversion_factor=motor_conversion_factor, + max_travel=max_travel, + min_travel=min_travel, + unlimited_travel=unlimited_travel, + absolute_encoder=absolute_encoder, + max_vel=max_vel, + max_accel=max_accel, + joint_move_direction=joint_move_direction, + digital_inputs=digital_inputs, + analog_inputs=analog_inputs, + outputs=outputs, + ) - # Robot-level params from shoulder. - shoulder = int(Axis.SHOULDER) - base_to_gripper_clearance_z = await self.driver.query_float(shoulder, "UF", 6) - base_to_gripper_clearance_arm = await self.driver.query_float(shoulder, "UF", 7) - wrist_offset = await self.driver.query_float(shoulder, "UF", 8) - elbow_offset = await self.driver.query_float(shoulder, "UF", 9) - elbow_zero_offset = await self.driver.query_float(shoulder, "UF", 10) - - servo_gripper: Optional[ServoGripperConfig] = None - if has_servo_gripper: - sg = int(Axis.SERVO_GRIPPER) - servo_gripper = ServoGripperConfig( - home_pos=int(await self.driver.query_float(sg, "UF", 6)), - home_search_vel=int(await self.driver.query_float(sg, "UF", 7)), - home_search_accel=int(await self.driver.query_float(sg, "UF", 8)), - home_default_position_error=int(await self.driver.query_float(sg, "UF", 9)), - home_hard_stop_position_error=int(await self.driver.query_float(sg, "UF", 10)), - home_hard_stop_offset=int(await self.driver.query_float(sg, "UF", 11)), - home_index_offset=int(await self.driver.query_float(sg, "UF", 12)), - home_offset_vel=int(await self.driver.query_float(sg, "UF", 13)), - home_offset_accel=int(await self.driver.query_float(sg, "UF", 14)), - home_timeout_msec=int(await self.driver.query_float(sg, "UF", 15)), - continuous_current=await self.driver.query_float(sg, "UF", 16), - peak_current=await self.driver.query_float(sg, "UF", 17), - ) + async def _read_io_names( + self, nid: int, start: int, end: int, named: Dict[int, str] + ) -> Dict[int, str]: + """Read UI[start..end-1] as a channel -> human name map. - self._config = KX2Config( - wrist_offset=wrist_offset, - elbow_offset=elbow_offset, - elbow_zero_offset=elbow_zero_offset, - gripper_length=self._gripper_length, - gripper_z_offset=self._gripper_z_offset, - gripper_finger_side=self._gripper_finger_side, - axes=axes, - base_to_gripper_clearance_z=base_to_gripper_clearance_z, - base_to_gripper_clearance_arm=base_to_gripper_clearance_arm, - robot_on_rail=robot_on_rail, - servo_gripper=servo_gripper, + Channel index is 1-based. Codes in `named` map to fixed labels; positive + unknowns become "AuxPinN"; non-positive means unassigned. + """ + out: Dict[int, str] = {} + for ui_idx in range(start, end): + code = await self.driver.query_int(nid, "UI", ui_idx) + ch = ui_idx - start + 1 + if code in named: + out[ch] = named[code] + else: + out[ch] = "" if code <= 0 else f"AuxPin{code}" + return out + + async def _read_servo_gripper_config(self) -> ServoGripperConfig: + sg = int(Axis.SERVO_GRIPPER) + return ServoGripperConfig( + home_pos=int(await self.driver.query_float(sg, "UF", 6)), + home_search_vel=int(await self.driver.query_float(sg, "UF", 7)), + home_search_accel=int(await self.driver.query_float(sg, "UF", 8)), + home_default_position_error=int(await self.driver.query_float(sg, "UF", 9)), + home_hard_stop_position_error=int(await self.driver.query_float(sg, "UF", 10)), + home_hard_stop_offset=int(await self.driver.query_float(sg, "UF", 11)), + home_index_offset=int(await self.driver.query_float(sg, "UF", 12)), + home_offset_vel=int(await self.driver.query_float(sg, "UF", 13)), + home_offset_accel=int(await self.driver.query_float(sg, "UF", 14)), + home_timeout_msec=int(await self.driver.query_float(sg, "UF", 15)), + continuous_current=await self.driver.query_float(sg, "UF", 16), + peak_current=await self.driver.query_float(sg, "UF", 17), ) @property @@ -1148,3 +1104,18 @@ async def stop_freedrive_mode(self, backend_params: Optional[BackendParams] = No MOTION_AXES = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) + +# UI[5..10] code -> digital input role. +_DIGITAL_INPUT_NAMES: Dict[int, str] = { + 101: "ProximitySensor", + 102: "TeachButton", +} + +# UI[13..16] code -> output role. +_OUTPUT_NAMES: Dict[int, str] = { + 101: "IndicatorLightRed", + 102: "IndicatorLightGreen", + 103: "IndicatorLightBlue", + 104: "IndicatorLight", + 105: "Buzzer", +} From 528ecdcf2c00c0a3c2f84576ed8b12445690e03d Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 26 Apr 2026 17:02:29 -0700 Subject: [PATCH 39/93] KX2 motion API: physical units (mm/s, deg/s) and motion_limits() Replace vel_pct/accel_pct with linear_speed/linear_acceleration (mm/s, mm/s^2) for Z/rail/gripper and rotary_speed/rotary_acceleration (deg/s, deg/s^2) for shoulder/elbow/wrist on JointMoveParams and CartesianMoveParams. Defaults stay at axis max so behavior is unchanged without overrides; user values clamp silently to the per-axis cap from the drive. find_z_with_proximity_sensor takes speed/acceleration in mm units (Z is linear). Add motion_limits() returning a dict subclass that prints as an aligned ASCII table while preserving dict access. Co-Authored-By: Claude Opus 4.7 (1M context) --- docs/user_guide/paa/kx2/hello-world.ipynb | 151 ++++++++++----------- pylabrobot/paa/kx2/arm_backend.py | 155 ++++++++++++++++------ pylabrobot/paa/kx2/driver.py | 4 +- 3 files changed, 191 insertions(+), 119 deletions(-) diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 64afa17d8fd..2629addef3c 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -30,31 +30,10 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "id": "kx2-setup-code", "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "uptime library not available, timestamps are relative to boot time and not to Epoch UTC\n", - "2026-04-25 17:22:25,930 - pylabrobot.paa.kx2.driver - INFO - canopen: connected, nodes=[1, 2, 3, 4, 6]\n" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ - "[MOVE PLAN] move_time=10.000s, 1 axes:\n", - " node=6 pos=-280 vel=100000 acc=100000 dir=ShortestWay\n", - "[MOVE PLAN] move_time=10.000s, 1 axes:\n", - " node=6 pos=1 vel=100000 acc=100000 dir=ShortestWay\n", - "[MOVE PLAN] move_time=0.791s, 1 axes:\n", - " node=6 pos=0 vel=149606 acc=636620 dir=Normal\n" - ] - } - ], + "outputs": [], "source": [ "from pylabrobot.paa.kx2 import KX2\n", "\n", @@ -69,9 +48,9 @@ "source": [ "## Arm capabilities\n", "\n", - "The KX2 exposes an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm` on `kx2.arm`, backed by {class}`~pylabrobot.paa.kx2.KX2ArmBackend`. Gripper yaw is controlled via a single `direction` float (degrees; 0° = front). For the full arm API, see [Arms](../../capabilities/arms).\n", + "The KX2 exposes an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm` on `kx2.arm`, backed by {class}`~pylabrobot.paa.kx2.KX2ArmBackend`. Gripper yaw is controlled via a single `direction` float (degrees; 0\u00b0 = front). For the full arm API, see [Arms](../../capabilities/arms).\n", "\n", - "The raw driver (`kx2.driver`, a `KX2Driver`) stays available for low-level access — motor commands, drive diagnostics, binary interpreter, etc." + "The raw driver (`kx2.driver`, a `KX2Driver`) stays available for low-level access \u2014 motor commands, drive diagnostics, binary interpreter, etc." ] }, { @@ -86,7 +65,7 @@ }, { "cell_type": "code", - "execution_count": 2, + "execution_count": null, "id": "kx2-gripper-open", "metadata": {}, "outputs": [], @@ -96,7 +75,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": null, "id": "kx2-gripper-close", "metadata": {}, "outputs": [], @@ -111,7 +90,7 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": 5, "id": "kx2-gripper-status", "metadata": {}, "outputs": [ @@ -121,7 +100,7 @@ "True" ] }, - "execution_count": 4, + "execution_count": 5, "metadata": {}, "output_type": "execute_result" } @@ -140,7 +119,7 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": null, "id": "kx2-gripper-force", "metadata": {}, "outputs": [], @@ -182,25 +161,51 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": null, "id": "kx2-proximity-search", "metadata": {}, + "outputs": [], + "source": [ + "z_hit = await kx2.arm.backend.find_z_with_proximity_sensor(max_descent=100.0, z_start=400)\n", + "print(f\"object found at Z = {z_hit:.2f}\")" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-motion-limits-md", + "metadata": {}, + "source": [ + "### Motion limits\n", + "\n", + "`motion_limits()` reads the per-axis maxima cached at setup. Pass values below these into ", + "`JointMoveParams` / `CartesianMoveParams`; anything above is silently clamped. Linear axes are mm/s and mm/s^2; rotary in deg/s and deg/s^2." + ] + }, + { + "cell_type": "code", + "id": "kx2-motion-limits", + "metadata": {}, + "execution_count": 3, "outputs": [ { - "name": "stdout", - "output_type": "stream", - "text": [ - "[MOVE PLAN] move_time=1.550s, 1 axes:\n", - " node=2 pos=1599135 vel=149919 acc=199892 dir=Normal\n", - "[MOVE PLAN] move_time=3.417s, 1 axes:\n", - " node=2 pos=1079426 vel=149919 acc=199892 dir=Normal\n", - "object found at Z = 323.49\n" - ] + "data": { + "text/plain": [ + "axis max speed max acceleration\n", + "------------- ------------ ----------------\n", + "SHOULDER 145.00 deg/s 300.00 deg/s^2 \n", + "Z 750.00 mm/s 1000.00 mm/s^2 \n", + "ELBOW 80.00 deg/s 180.00 deg/s^2 \n", + "WRIST 500.00 deg/s 1000.04 deg/s^2 \n", + "SERVO_GRIPPER 47.00 mm/s 200.00 mm/s^2 " + ] + }, + "execution_count": 3, + "metadata": {}, + "output_type": "execute_result" } ], "source": [ - "z_hit = await kx2.arm.backend.find_z_with_proximity_sensor(max_descent=100.0, z_start=400)\n", - "print(f\"object found at Z = {z_hit:.2f}\")" + "kx2.arm.backend.motion_limits()" ] }, { @@ -210,22 +215,22 @@ "source": [ "### Cartesian movement\n", "\n", - "Move the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only — the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity and acceleration." + "Move the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only \u2014 the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity and acceleration." ] }, { "cell_type": "code", - "execution_count": 6, + "execution_count": 7, "id": "11b06c86", "metadata": {}, "outputs": [ { "data": { "text/plain": [ - "GripperLocation(location=Coordinate(x=0.0025, y=241.699, z=150.0018), rotation=Rotation(x=0, y=0, z=255.4754734040739))" + "GripperLocation(location=Coordinate(x=1.193, y=200.0123, z=300.0067), rotation=Rotation(x=0, y=0, z=99.65582378905646))" ] }, - "execution_count": 6, + "execution_count": 7, "metadata": {}, "output_type": "execute_result" } @@ -236,7 +241,7 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": null, "id": "kx2-cartesian-code", "metadata": {}, "outputs": [], @@ -246,7 +251,7 @@ "await kx2.arm.move_to_location(\n", " location=Coordinate(x=330.124, y=210.552, z=320.0441),\n", " direction=198,\n", - " backend_params=KX2ArmBackend.CartesianMoveParams(vel_pct=25, accel_pct=25),\n", + " backend_params=KX2ArmBackend.CartesianMoveParams(linear_speed=20, linear_acceleration=50, rotary_speed=15, rotary_acceleration=50),\n", ")" ] }, @@ -281,7 +286,7 @@ "}\n", "await kx2.arm.backend.move_to_joint_position(\n", " position,\n", - " backend_params=KX2ArmBackend.JointMoveParams(vel_pct=25, accel_pct=25),\n", + " backend_params=KX2ArmBackend.JointMoveParams(linear_speed=20, linear_acceleration=50, rotary_speed=15, rotary_acceleration=50),\n", ")" ] }, @@ -382,7 +387,7 @@ "source": [ "### Pick and place\n", "\n", - "`pick_up_at_location` moves to the target pose and closes the gripper to `resource_width`. `drop_at_location` moves and opens the gripper. Approach and retract motions are the caller's responsibility — bracket these calls with your own pre- and post-moves." + "`pick_up_at_location` moves to the target pose and closes the gripper to `resource_width`. `drop_at_location` moves and opens the gripper. Approach and retract motions are the caller's responsibility \u2014 bracket these calls with your own pre- and post-moves." ] }, { @@ -415,12 +420,12 @@ "source": [ "### Freedrive (teaching mode)\n", "\n", - "Freedrive disables torque on the motion axes so you can push the arm by hand; the servo gripper stays powered. The KX2 frees all motion axes at once — the `free_axes` argument is accepted for API parity and ignored." + "Freedrive disables torque on the motion axes so you can push the arm by hand; the servo gripper stays powered. The KX2 frees all motion axes at once \u2014 the `free_axes` argument is accepted for API parity and ignored." ] }, { "cell_type": "code", - "execution_count": 2, + "execution_count": null, "id": "kx2-free-on", "metadata": {}, "outputs": [], @@ -450,24 +455,10 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": null, "id": "kx2-free-off", "metadata": {}, - "outputs": [ - { - "ename": "CanError", - "evalue": "Motor failed to enable (axis = 1)", - "output_type": "error", - "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mCanError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[0;32mIn[4], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m kx2\u001b[38;5;241m.\u001b[39marm\u001b[38;5;241m.\u001b[39mbackend\u001b[38;5;241m.\u001b[39mstop_freedrive_mode()\n", - "File \u001b[0;32m~/real/pylabrobot/pylabrobot/paa/kx2/kx2_backend.py:1392\u001b[0m, in \u001b[0;36mKX2ArmBackend.stop_freedrive_mode\u001b[0;34m(self, backend_params)\u001b[0m\n\u001b[1;32m 1390\u001b[0m \u001b[38;5;28;01masync\u001b[39;00m \u001b[38;5;28;01mdef\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21mstop_freedrive_mode\u001b[39m(\u001b[38;5;28mself\u001b[39m, backend_params: Optional[BackendParams] \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m) \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m>\u001b[39m \u001b[38;5;28;01mNone\u001b[39;00m:\n\u001b[1;32m 1391\u001b[0m \u001b[38;5;28;01mfor\u001b[39;00m axis \u001b[38;5;129;01min\u001b[39;00m MOTION_AXES:\n\u001b[0;32m-> 1392\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mdriver\u001b[38;5;241m.\u001b[39mmotor_enable(axis\u001b[38;5;241m=\u001b[39maxis, state\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mTrue\u001b[39;00m)\n", - "File \u001b[0;32m~/real/pylabrobot/pylabrobot/paa/kx2/kx2_driver.py:737\u001b[0m, in \u001b[0;36mKX2Driver.motor_enable\u001b[0;34m(self, axis, state)\u001b[0m\n\u001b[1;32m 733\u001b[0m left \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mawait\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mbinary_interpreter(\n\u001b[1;32m 734\u001b[0m node_id\u001b[38;5;241m=\u001b[39m\u001b[38;5;28mint\u001b[39m(axis), cmd\u001b[38;5;241m=\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mMO\u001b[39m\u001b[38;5;124m\"\u001b[39m, cmd_index\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m0\u001b[39m, cmd_type\u001b[38;5;241m=\u001b[39mCmdType\u001b[38;5;241m.\u001b[39mValQuery\n\u001b[1;32m 735\u001b[0m )\n\u001b[1;32m 736\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m left \u001b[38;5;241m!=\u001b[39m \u001b[38;5;241m1\u001b[39m:\n\u001b[0;32m--> 737\u001b[0m \u001b[38;5;28;01mraise\u001b[39;00m CanError(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mMotor failed to enable (axis = \u001b[39m\u001b[38;5;132;01m{\u001b[39;00maxis\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m)\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[1;32m 738\u001b[0m \u001b[38;5;28;01melse\u001b[39;00m:\n\u001b[1;32m 739\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m use_bi:\n", - "\u001b[0;31mCanError\u001b[0m: Motor failed to enable (axis = 1)" - ] - } - ], + "outputs": [], "source": [ "await kx2.arm.backend.stop_freedrive_mode()" ] @@ -484,7 +475,7 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": null, "id": "kx2-halt", "metadata": {}, "outputs": [], @@ -520,7 +511,7 @@ "source": [ "## Barcode reader\n", "\n", - "The KX2's onboard barcode reader is a serial 1D laser scanner wired to the controller PC. It's fully independent of the CAN motor stack, so it works even with the arm e-stopped — exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`.\n", + "The KX2's onboard barcode reader is a serial 1D laser scanner wired to the controller PC. It's fully independent of the CAN motor stack, so it works even with the arm e-stopped \u2014 exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`.\n", "\n", "The model that ships with KX2 systems we've inspected is a **Denso MDI-4050** (firmware reports as `BD01J09`). Factory defaults: **9600 8N1**, no flow control. Find your port with `python -m serial.tools.list_ports -v`.\n", "\n", @@ -533,15 +524,15 @@ "open -a PL2303Serial # registers the system extension\n", "```\n", "\n", - "Then **System Settings → Privacy & Security → Allow** on the \"System software from PL2303Serial was blocked\" prompt, restart if asked, and replug the cable. Verify with `systemextensionsctl list`: `com.prolific.cdc.PLCdcFSDriver` should show `[activated enabled]`.\n", + "Then **System Settings \u2192 Privacy & Security \u2192 Allow** on the \"System software from PL2303Serial was blocked\" prompt, restart if asked, and replug the cable. Verify with `systemextensionsctl list`: `com.prolific.cdc.PLCdcFSDriver` should show `[activated enabled]`.\n", "\n", "### Symbologies\n", "\n", - "The MDI-4050 is a 1D laser, so **PDF417 / DataMatrix / QR Code aren't supported** — you'd need the 2D imager variants (e.g. MDI-4150) for those.\n", + "The MDI-4050 is a 1D laser, so **PDF417 / DataMatrix / QR Code aren't supported** \u2014 you'd need the 2D imager variants (e.g. MDI-4150) for those.\n", "\n", "Typical out-of-the-box 1D enables: UPC-A, UPC-E, EAN-13, EAN-8, Code 39, Tri-Optic, Codabar, Industrial 2/5, Interleaved 2/5, IATA, Code 128, Code 93, GS1 DataBar, GS1 DataBar Limited. Add-on (supplemental) codes, postal codes, MSI/Plessey, Telepen, UK/Plessey, and Code 11 are off by default.\n", "\n", - "You can read your unit's full configuration over the wire — model, firmware, every per-symbology enable flag, prefixes/suffixes, and length limits — via `dump_config()` (which sends `Z4`):\n", + "You can read your unit's full configuration over the wire \u2014 model, firmware, every per-symbology enable flag, prefixes/suffixes, and length limits \u2014 via `dump_config()` (which sends `Z4`):\n", "\n", "```python\n", "text = await bcr.driver.dump_config()\n", @@ -550,11 +541,11 @@ "\n", "To **change** which symbologies are enabled, three options in order of practicality:\n", "\n", - "1. **Denso's MDI Setup utility** (free Windows download from Denso ADC) — checkbox UI, talks to the reader over the same serial port, writes to NVRAM.\n", - "2. **Configuration barcodes** printed in the MDI-4050 setting manual — scan a Start → toggle → End sequence; no host required.\n", - "3. Direct register writes via the ESC protocol — possible but the per-symbology byte layout is documented only in Denso's protocol manual.\n", + "1. **Denso's MDI Setup utility** (free Windows download from Denso ADC) \u2014 checkbox UI, talks to the reader over the same serial port, writes to NVRAM.\n", + "2. **Configuration barcodes** printed in the MDI-4050 setting manual \u2014 scan a Start \u2192 toggle \u2192 End sequence; no host required.\n", + "3. Direct register writes via the ESC protocol \u2014 possible but the per-symbology byte layout is documented only in Denso's protocol manual.\n", "\n", - "{class}`~pylabrobot.resources.barcode.Barcode` returned from `scan()` has `symbology=\"ANY 1D\"` by default — the reader doesn't emit a symbology ID unless prefix/suffix bytes are configured to include one." + "{class}`~pylabrobot.resources.barcode.Barcode` returned from `scan()` has `symbology=\"ANY 1D\"` by default \u2014 the reader doesn't emit a symbology ID unless prefix/suffix bytes are configured to include one." ] }, { @@ -636,7 +627,7 @@ "source": [ "print(await bcr.driver.get_software_version())\n", "\n", - "# Full NVRAM dump — model, firmware, all symbology enables, prefixes/suffixes, length limits.\n", + "# Full NVRAM dump \u2014 model, firmware, all symbology enables, prefixes/suffixes, length limits.\n", "config = await bcr.driver.dump_config()\n", "print(config[:200])\n", "\n", @@ -650,7 +641,7 @@ "source": [ "### Teardown\n", "\n", - "`stop()` sends `Y` (trigger off) and `Y2` (reset to a 2s read window), then closes the serial port — leaving the reader in a known state for the next session." + "`stop()` sends `Y` (trigger off) and `Y2` (reset to a 2s read window), then closes the serial port \u2014 leaving the reader in a known state for the next session." ] }, { @@ -702,4 +693,4 @@ }, "nbformat": 4, "nbformat_minor": 5 -} +} \ No newline at end of file diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index caf73b84653..4ed9d861692 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -362,8 +362,10 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: async def servo_gripper_close(self, closed_position: int = 0, check_plate_gripped=True) -> None: await self.motors_move_joint( {Axis.SERVO_GRIPPER: closed_position}, - cmd_vel_pct=100, - cmd_accel_pct=100, + cmd_linear_speed=None, + cmd_linear_acceleration=None, + cmd_rotary_speed=None, + cmd_rotary_acceleration=None, ) if check_plate_gripped: @@ -372,8 +374,10 @@ async def servo_gripper_close(self, closed_position: int = 0, check_plate_grippe async def servo_gripper_open(self, open_position: float) -> None: await self.motors_move_joint( {Axis.SERVO_GRIPPER: open_position}, - cmd_vel_pct=100, - cmd_accel_pct=100, + cmd_linear_speed=None, + cmd_linear_acceleration=None, + cmd_rotary_speed=None, + cmd_rotary_acceleration=None, ) async def drive_set_move_count_parameters( @@ -629,19 +633,22 @@ async def find_z_with_proximity_sensor( self, max_descent: float, z_start: Optional[float] = None, - vel_pct: float = 5.0, - accel_pct: float = 5.0, + speed: float = 5.0, + acceleration: float = 50.0, ) -> float: """Descend Z up to `max_descent`; halt when the IR breakbeam trips. - If `z_start` is given, first move Z to that height (same vel/accel) and - search from there; otherwise search from the current Z. Arms IL[4]= - StopForward so the Elmo drive halts the motor itself on the input edge - (sub-ms latency, no software in the loop). IL is restored to GeneralPurpose - afterwards even if the move raises. Returns the Z position where the drive - halted; raises RuntimeError if the beam never tripped. + `speed` is in mm/s, `acceleration` in mm/s^2. If `z_start` is given, + first move Z to that height (same speed/accel) and search from there; + otherwise search from the current Z. Arms IL[4]=StopForward so the Elmo + drive halts the motor itself on the input edge (sub-ms latency, no + software in the loop). IL is restored to GeneralPurpose afterwards even + if the move raises. Returns the Z position where the drive halted; + raises RuntimeError if the beam never tripped. """ - move_params = KX2ArmBackend.JointMoveParams(vel_pct=vel_pct, accel_pct=accel_pct) + move_params = KX2ArmBackend.JointMoveParams( + linear_speed=speed, linear_acceleration=acceleration, + ) # Pre-flight: force the drive back to Op Enabled. A prior failed search # could have left it in Fault/Quick Stop where new moves silently fail # (Z barely moves). Idempotent if the drive is already healthy. @@ -740,8 +747,10 @@ def _profile(dist: float, v: float, a: float) -> tuple: async def calculate_move_abs_all_axes( self, cmd_pos: Dict[Axis, float], - cmd_vel_pct: float, - cmd_accel_pct: float, + cmd_linear_speed: Optional[float], + cmd_linear_acceleration: Optional[float], + cmd_rotary_speed: Optional[float], + cmd_rotary_acceleration: Optional[float], ) -> Optional[MotorsMovePlan]: target = cmd_pos.copy() axes = list(target.keys()) @@ -752,10 +761,12 @@ async def calculate_move_abs_all_axes( skip_ax: Dict[Axis, bool] = {} # input validation / travel limits / done-wait logic - if cmd_vel_pct <= 0.0 or cmd_vel_pct > 100.0: - raise ValueError("CmdVel out of range") - if cmd_accel_pct <= 0.0 or cmd_accel_pct > 100.0: - raise ValueError("CmdAccel out of range") + for name, val in ( + ("linear_speed", cmd_linear_speed), ("linear_acceleration", cmd_linear_acceleration), + ("rotary_speed", cmd_rotary_speed), ("rotary_acceleration", cmd_rotary_acceleration), + ): + if val is not None and val <= 0.0: + raise ValueError(f"{name} must be positive, got {val}") # Convert elbow cmd from position->angle for planning math if Axis.ELBOW in axes: @@ -813,8 +824,16 @@ async def calculate_move_abs_all_axes( skip_ax[ax] = abs(dist[ax]) < 0.01 - v[ax] = (cmd_vel_pct / 100.0) * self._cfg.axes[ax].max_vel - a[ax] = (cmd_accel_pct / 100.0) * self._cfg.axes[ax].max_accel + axis_max_v = self._cfg.axes[ax].max_vel + axis_max_a = self._cfg.axes[ax].max_accel + if ax in _LINEAR_AXES: + chosen_v = cmd_linear_speed if cmd_linear_speed is not None else axis_max_v + chosen_a = cmd_linear_acceleration if cmd_linear_acceleration is not None else axis_max_a + else: + chosen_v = cmd_rotary_speed if cmd_rotary_speed is not None else axis_max_v + chosen_a = cmd_rotary_acceleration if cmd_rotary_acceleration is not None else axis_max_a + v[ax] = min(chosen_v, axis_max_v) + a[ax] = min(chosen_a, axis_max_a) if not skip_ax[ax] and a[ax] > 0: accel_time[ax] = v[ax] / a[ax] @@ -901,14 +920,18 @@ async def calculate_move_abs_all_axes( async def motors_move_joint( self, cmd_pos: Dict[Axis, float], - cmd_vel_pct: float, - cmd_accel_pct: float, + cmd_linear_speed: Optional[float], + cmd_linear_acceleration: Optional[float], + cmd_rotary_speed: Optional[float], + cmd_rotary_acceleration: Optional[float], ) -> None: logger.debug("motors_move_joint cmd_pos=%s", cmd_pos) plan = await self.calculate_move_abs_all_axes( cmd_pos=cmd_pos, - cmd_vel_pct=cmd_vel_pct, - cmd_accel_pct=cmd_accel_pct, + cmd_linear_speed=cmd_linear_speed, + cmd_linear_acceleration=cmd_linear_acceleration, + cmd_rotary_speed=cmd_rotary_speed, + cmd_rotary_acceleration=cmd_rotary_acceleration, ) if plan is None: # if every axis is skipped, exit @@ -944,13 +967,26 @@ async def _cart_to_joints( @dataclass class CartesianMoveParams(BackendParams): - vel_pct: float = 100.0 - accel_pct: float = 100.0 + """Per-axis speed/acceleration limits in physical units. + + `linear_*` applies to Z (and rail/gripper if commanded); `rotary_*` + applies to shoulder/elbow/wrist. `None` falls back to the axis maximum + read from the drive at setup. Values above the per-axis max are clamped + silently. + """ + linear_speed: Optional[float] = None # mm/s + linear_acceleration: Optional[float] = None # mm/s^2 + rotary_speed: Optional[float] = None # deg/s + rotary_acceleration: Optional[float] = None # deg/s^2 @dataclass class JointMoveParams(BackendParams): - vel_pct: float = 100.0 - accel_pct: float = 100.0 + """Per-axis speed/acceleration limits in physical units. Same shape as + `CartesianMoveParams` — see its docstring.""" + linear_speed: Optional[float] = None + linear_acceleration: Optional[float] = None + rotary_speed: Optional[float] = None + rotary_acceleration: Optional[float] = None @dataclass class GripParams(BackendParams): @@ -976,8 +1012,10 @@ async def open_gripper( ) -> None: await self.motors_move_joint( {Axis.SERVO_GRIPPER: gripper_width}, - cmd_vel_pct=100, - cmd_accel_pct=100, + cmd_linear_speed=None, + cmd_linear_acceleration=None, + cmd_rotary_speed=None, + cmd_rotary_acceleration=None, ) async def close_gripper( @@ -987,8 +1025,10 @@ async def close_gripper( backend_params = KX2ArmBackend.GripParams() await self.motors_move_joint( {Axis.SERVO_GRIPPER: gripper_width}, - cmd_vel_pct=100, - cmd_accel_pct=100, + cmd_linear_speed=None, + cmd_linear_acceleration=None, + cmd_rotary_speed=None, + cmd_rotary_acceleration=None, ) if backend_params.check_plate_gripped: await self.check_plate_gripped() @@ -1012,8 +1052,10 @@ async def move_to_gripper_location( joint_pos = await self._cart_to_joints(pose) await self.motors_move_joint( cmd_pos=joint_pos, - cmd_vel_pct=backend_params.vel_pct, - cmd_accel_pct=backend_params.accel_pct, + cmd_linear_speed=backend_params.linear_speed, + cmd_linear_acceleration=backend_params.linear_acceleration, + cmd_rotary_speed=backend_params.rotary_speed, + cmd_rotary_acceleration=backend_params.rotary_acceleration, ) async def move_to_location( @@ -1057,8 +1099,10 @@ async def move_to_joint_position( cmd_pos = {Axis(int(k)): float(v) for k, v in position.items()} await self.motors_move_joint( cmd_pos=cmd_pos, - cmd_vel_pct=backend_params.vel_pct, - cmd_accel_pct=backend_params.accel_pct, + cmd_linear_speed=backend_params.linear_speed, + cmd_linear_acceleration=backend_params.linear_acceleration, + cmd_rotary_speed=backend_params.rotary_speed, + cmd_rotary_acceleration=backend_params.rotary_acceleration, ) async def pick_up_at_joint_position( @@ -1090,6 +1134,19 @@ async def request_joint_position( Axis.SERVO_GRIPPER: await self.motor_get_current_position(Axis.SERVO_GRIPPER), } + def motion_limits(self) -> "_MotionLimits": + """Per-axis (max_speed, max_acceleration) read from the drives at setup. + + Linear axes (Z, rail, servo gripper) are mm/s, mm/s^2; rotary axes + (shoulder, elbow, wrist) are deg/s, deg/s^2. These are the upper bounds + `JointMoveParams` / `CartesianMoveParams` get clamped to. Returned as a + dict subclass that renders as a table in Jupyter and plain-text columns + in a terminal. + """ + return _MotionLimits( + {Axis(k): (cfg.max_vel, cfg.max_accel) for k, cfg in self._cfg.axes.items()}, + ) + async def start_freedrive_mode( self, free_axes: List[int], backend_params: Optional[BackendParams] = None ) -> None: @@ -1105,6 +1162,30 @@ async def stop_freedrive_mode(self, backend_params: Optional[BackendParams] = No MOTION_AXES = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) +# Axes that move in linear units (mm/s, mm/s^2). All others move in rotary +# units (deg/s, deg/s^2). Used to pick the right speed/acceleration from +# the linear/rotary split in JointMoveParams / CartesianMoveParams. +_LINEAR_AXES = frozenset({Axis.Z, Axis.RAIL, Axis.SERVO_GRIPPER}) + + +class _MotionLimits(Dict[Axis, tuple]): + """Pretty-printing dict for `KX2ArmBackend.motion_limits()`. Dict access + still works (`limits[Axis.Z]` -> `(max_speed, max_accel)`); `__repr__` + formats it as an aligned ASCII table for both terminals and notebooks. + """ + + def __repr__(self) -> str: + rows = [] + for ax, (v, a) in self.items(): + unit = "mm" if ax in _LINEAR_AXES else "deg" + rows.append((ax.name, f"{v:.2f} {unit}/s", f"{a:.2f} {unit}/s^2")) + headers = ("axis", "max speed", "max acceleration") + widths = [max(len(headers[i]), *(len(r[i]) for r in rows)) for i in range(3)] + fmt = " ".join(f"{{:<{w}}}" for w in widths) + out = [fmt.format(*headers), fmt.format(*("-" * w for w in widths))] + out.extend(fmt.format(*r) for r in rows) + return "\n".join(out) + # UI[5..10] code -> digital input role. _DIGITAL_INPUT_NAMES: Dict[int, str] = { 101: "ProximitySensor", diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index d7d8093b9bb..a51b30fb671 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -162,8 +162,8 @@ class MotorMoveParam: # CANopen node ID for this axis. Backend passes `int(self.Axis.X)`. node_id: int position: int - velocity: int - acceleration: int + velocity: int # encoder counts/sec (driver-internal; backend converts from mm/s or deg/s) + acceleration: int # encoder counts/sec^2 relative: bool = False direction: JointMoveDirection = JointMoveDirection.ShortestWay From 3101f1ae2a6d474b998333f92863c881d51af337 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 26 Apr 2026 19:11:30 -0700 Subject: [PATCH 40/93] productive work MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - KX2 setup: front-load E-stop check via SR-bit decode so a clear error appears before motor_enable's retry loop times out with a cryptic message. Clean up CAN on any setup failure so re-running setup doesn't trip PcanCanInitializationError. - KX2 motor_enable: bump retries from 5 to 20 with 0.5s between attempts. Drives sometimes need several seconds after a fault or power-rail bounce before they accept enable; the old window was under a second. - KX2 freedrive: make start_freedrive_mode.free_axes optional so it can be called with no args; defaults to all four motion axes. - Add KX2ArmBackend.very_dangerously_yeet — easter-egg throw routine that swings shoulder + wrist at firmware-max (VH[2]/1.01) with the wrist flick timed so peak ω lands at gripper release. `bump` parameter temporarily scales VH[2]/SP[2]/SD[0] on the swing axes, restored in finally. Confirmation prompt at the top before any motion. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/arm_backend.py | 358 +++++++++++++++++++++++++++++- pylabrobot/paa/kx2/driver.py | 38 +--- 2 files changed, 355 insertions(+), 41 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 4ed9d861692..0a38c6bbc65 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -25,6 +25,7 @@ ) from pylabrobot.paa.kx2.driver import ( CanError, + ElmoObjectDataType, InputLogic, JointMoveDirection, KX2Driver, @@ -75,11 +76,32 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): # Driver has already brought CAN up (connect + node discovery + PDO # mapping) via Device.setup(). Read per-drive config, then enable motion # axes and the servo gripper. - self._config = await self._read_config() - await asyncio.sleep(2) # let drives settle before motor enables - for axis in MOTION_AXES: - await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) - await self.servo_gripper_initialize() + # + # If anything below this line raises, tear CAN down so a retry can + # re-init. Otherwise the second setup() trips PcanCanInitializationError + # because the channel is still half-claimed from the first attempt. + try: + self._config = await self._read_config() + await asyncio.sleep(2) # let drives settle before motor enables + + # E-stop check: front-load a clear error before motor_enable's retry + # loop times out with a cryptic message. + if await self.get_estop_state(): + raise RuntimeError( + "KX2 setup failed: E-stop is engaged. Twist the red button to " + "release, then call setup() again. (If the button is out, the " + "safety-interlock loop or motor-power switch may also be open.)" + ) + + for axis in MOTION_AXES: + await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) + await self.servo_gripper_initialize() + except BaseException: + try: + await self.driver.stop() + except Exception: + logger.exception("KX2 setup cleanup: driver.stop() failed; ignoring") + raise # -- robot-level homing / estop (moved from driver) --------------------- @@ -214,7 +236,7 @@ async def home_motor( await self.driver.motor_enable(node_id=nid, state=True, use_ds402=nid in MOTION_AXES) - await self.driver.motors_move_absolute_execute( + await self.motors_move_absolute_execute( plan=MotorsMovePlan( moves=[ MotorMoveParam( @@ -232,7 +254,7 @@ async def home_motor( is_positive = hs_offset > 0 await self._motor_index_search(nid, abs(srch_vel), srch_acc, is_positive, timeout) - await self.driver.motors_move_absolute_execute( + await self.motors_move_absolute_execute( plan=MotorsMovePlan( moves=[ MotorMoveParam( @@ -768,6 +790,34 @@ async def calculate_move_abs_all_axes( if val is not None and val <= 0.0: raise ValueError(f"{name} must be positive, got {val}") + # Travel-limit bounds check. Mirrors C# MoveAbsoluteSingleAxisPrivate + # (KX2RobotControl.cs:4624-4649): snap if within 0.1 of the limit, raise + # otherwise. Without this, sending an out-of-range target (e.g. gripper + # width 600 when max_travel ~30) parks the drive trying to reach an + # unreachable position — MS never returns to 0 and every subsequent + # command on that axis hangs until full re-setup. Run before the elbow + # position->angle conversion so max_travel/min_travel are compared in + # the same space the user passed in. + for ax in axes: + ax_cfg = self._cfg.axes[ax] + if ax_cfg.unlimited_travel: + continue + t = target[ax] + if t > ax_cfg.max_travel: + if t - ax_cfg.max_travel < 0.1: + target[ax] = ax_cfg.max_travel + else: + raise ValueError( + f"Axis {ax.name} target {t} exceeds max_travel {ax_cfg.max_travel}" + ) + elif t < ax_cfg.min_travel: + if ax_cfg.min_travel - t < 0.1: + target[ax] = ax_cfg.min_travel + else: + raise ValueError( + f"Axis {ax.name} target {t} below min_travel {ax_cfg.min_travel}" + ) + # Convert elbow cmd from position->angle for planning math if Axis.ELBOW in axes: target[Axis.ELBOW] = self.convert_elbow_position_to_angle(target[Axis.ELBOW]) @@ -937,7 +987,42 @@ async def motors_move_joint( if plan is None: # if every axis is skipped, exit return - await self.driver.motors_move_absolute_execute(plan) + await self.motors_move_absolute_execute(plan) + + async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: + await self.driver._pvt_select_mode(False) + + print(f"[MOVE PLAN] move_time={plan.move_time:.3f}s, {len(plan.moves)} axes:") + for move in plan.moves: + print( + f" node={move.node_id} pos={move.position} vel={move.velocity} " + f"acc={move.acceleration} dir={move.direction.name}" + ) + + for move in plan.moves: + nid = int(move.node_id) + await self.driver._motor_set_move_direction(nid, move.direction) + # 0x607A = Target Position (24698 decimal) + await self.driver._can_sdo_download_elmo_object( + nid, 24698, 0, int(move.position), ElmoObjectDataType.INTEGER32, + ) + # 0x6081 = Profile Velocity (24705 decimal) + await self.driver._can_sdo_download_elmo_object( + nid, 24705, 0, int(move.velocity), ElmoObjectDataType.UNSIGNED32, + ) + acc = max(int(move.acceleration), 100) + # 0x6083 = Profile Acceleration (24707 decimal) + await self.driver._can_sdo_download_elmo_object( + nid, 24707, 0, acc, ElmoObjectDataType.UNSIGNED32 + ) + # 0x6084 = Profile Deceleration (24708 decimal) + await self.driver._can_sdo_download_elmo_object( + nid, 24708, 0, acc, ElmoObjectDataType.UNSIGNED32 + ) + + node_ids = [move.node_id for move in plan.moves] + await self.driver._motors_move_start(node_ids) + await self.driver.wait_for_moves_done(node_ids, timeout=plan.move_time + 2) async def _cart_to_joints( self, pose: kinematics.KX2GripperLocation @@ -1148,7 +1233,9 @@ def motion_limits(self) -> "_MotionLimits": ) async def start_freedrive_mode( - self, free_axes: List[int], backend_params: Optional[BackendParams] = None + self, + free_axes: Optional[List[int]] = None, + backend_params: Optional[BackendParams] = None, ) -> None: # KX2 frees all motion axes at once; free_axes is accepted for API parity. del free_axes @@ -1159,6 +1246,154 @@ async def stop_freedrive_mode(self, backend_params: Optional[BackendParams] = No for axis in MOTION_AXES: await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) + async def very_dangerously_yeet( + self, + min_z: float = 400.0, + bump: float = 1.25, + ) -> None: + """Easter egg — swing the arm at firmware-max and open the gripper at + peak velocity to throw whatever is being held. + + Call from your pickup pose. Sequence: auto-windup wrist to the inward + angle, swing shoulder 180° at firmware-max, fire gripper open near end + of cruise (with wrist flick at peak ω for extra tangential velocity), + return to pickup pose. + + ``bump`` scales VH[2]/SP[2]/SD[0] on shoulder + wrist for the swing's + duration (restored in finally). 1.0 = stock; 1.25 confirmed safe; + higher risks tracking-error faults that need Elmo Composer recovery. + """ + warning = ( + f"⚠️ very_dangerously_yeet: swing the arm at {bump:.2f}× firmware-max " + "and open the gripper mid-swing. Anything in the gripper will be " + "thrown. High bump can fault the drive. Type 'y' to continue: " + ) + if input(warning).strip().lower() != "y": + raise RuntimeError("very_dangerously_yeet: aborted by user") + + driver = self.driver + cfg = self._cfg + + z_now = await self.motor_get_current_position(Axis.Z) + if z_now < min_z: + raise RuntimeError( + f"yeet refused: Z={z_now:.0f}mm < min_z={min_z:.0f}mm; raise the arm first" + ) + + # Snapshot + bump VH[2]/SP[2]/SD[0] on swing axes; restore in finally. + saved_limits: Dict[Axis, dict] = {} + if bump != 1.0: + for ax in (Axis.SHOULDER, Axis.WRIST): + nid = int(ax) + s = { + "VH2": await driver.query_int(nid, "VH", 2), + "SP2": await driver.query_int(nid, "SP", 2), + "SD0": await driver.query_int(nid, "SD", 0), + "max_vel": cfg.axes[ax].max_vel, + "max_accel": cfg.axes[ax].max_accel, + } + saved_limits[ax] = s + new_vh2 = int(s["VH2"] * bump) + new_sp2 = int(s["SP2"] * bump) + new_sd0 = int(s["SD0"] * bump) + await driver.write(nid, "VH", 2, new_vh2) + await driver.write(nid, "SP", 2, new_sp2) + await driver.write(nid, "SD", 0, new_sd0) + conv = abs(cfg.axes[ax].motor_conversion_factor) + cfg.axes[ax].max_vel = (new_sp2 / 1.01) / conv + cfg.axes[ax].max_accel = (new_sd0 / 1.01) / conv + + try: + pickup_pose = await self.request_joint_position() + + # Auto-windup: rotate wrist to the inward angle (opposite of outward). + wrist_inward = 0.0 if cfg.gripper_finger_side == "barcode_reader" else 180.0 + while wrist_inward - pickup_pose[Axis.WRIST] > 180.0: + wrist_inward -= 360.0 + while wrist_inward - pickup_pose[Axis.WRIST] < -180.0: + wrist_inward += 360.0 + await self.motors_move_joint( + cmd_pos={Axis.WRIST: wrist_inward}, + cmd_linear_speed=_YEET_WINDUP_SPEED, + cmd_linear_acceleration=_YEET_WINDUP_ACC, + cmd_rotary_speed=_YEET_WINDUP_SPEED, + cmd_rotary_acceleration=_YEET_WINDUP_ACC, + ) + + joints0 = await self.request_joint_position() + + # Outward wrist = kinematic target (180° barcode_reader, 0° proximity). + wrist_outward = 180.0 if cfg.gripper_finger_side == "barcode_reader" else 0.0 + while wrist_outward - joints0[Axis.WRIST] > 180.0: + wrist_outward -= 360.0 + while wrist_outward - joints0[Axis.WRIST] < -180.0: + wrist_outward += 360.0 + + sh_move, sh_t_acc, sh_t_total, _ = await _yeet_build_axis_move( + self, Axis.SHOULDER, + joints0[Axis.SHOULDER], joints0[Axis.SHOULDER] + _YEET_SHOULDER_SWING_DEG, + ) + wr_move, wr_t_acc, wr_t_total, _ = await _yeet_build_axis_move( + self, Axis.WRIST, joints0[Axis.WRIST], wrist_outward, + ) + sh_plan = MotorsMovePlan(moves=[sh_move], move_time=sh_t_total) + wr_plan = MotorsMovePlan(moves=[wr_move], move_time=wr_t_total) + + # Release fires inside shoulder cruise. Wrist trigger is delayed so its + # accel ramp finishes at release (peak ω at the gripper offset). + sh_cruise_dur = max(0.0, sh_t_total - 2 * sh_t_acc) + release_t = sh_t_acc + sh_cruise_dur * _YEET_RELEASE_FRACTION + wrist_trigger_t = max(0.0, release_t - wr_t_acc) + + sg = int(Axis.SERVO_GRIPPER) + sg_cfg = cfg.axes[Axis.SERVO_GRIPPER] + open_pos = min(_YEET_OPEN_POSITION, sg_cfg.max_travel - _YEET_OPEN_SAFETY_MARGIN) + gripper_plan = MotorsMovePlan(moves=[MotorMoveParam( + node_id=sg, + position=int(round(open_pos * sg_cfg.motor_conversion_factor)), + velocity=int(round(sg_cfg.max_vel * abs(sg_cfg.motor_conversion_factor))), + acceleration=int(round(sg_cfg.max_accel * abs(sg_cfg.motor_conversion_factor))), + direction=sg_cfg.joint_move_direction, + )]) + + # Pre-arm so triggers are pure control-word writes (sub-ms), not SDOs. + await _yeet_arm_plan(driver, sh_plan) + await _yeet_arm_plan(driver, wr_plan) + + await driver._motors_move_start([int(Axis.SHOULDER)]) + t0 = time.monotonic() + await asyncio.sleep(max(0.0, wrist_trigger_t - (time.monotonic() - t0))) + await driver._motors_move_start([int(Axis.WRIST)]) + + await asyncio.sleep(max(0.0, release_t - (time.monotonic() - t0))) + await self.motors_move_absolute_execute(gripper_plan) + + # Settle slack: at higher bump, drives overshoot + ring before asserting + # target-reached; tight margin trips a CanError even though throw was OK. + swing_finish_t = max(sh_t_total, wrist_trigger_t + wr_t_total) + await driver.wait_for_moves_done( + [int(Axis.SHOULDER), int(Axis.WRIST)], timeout=swing_finish_t + 5, + ) + + await self.motors_move_joint( + cmd_pos={ + Axis.SHOULDER: pickup_pose[Axis.SHOULDER], + Axis.WRIST: pickup_pose[Axis.WRIST], + }, + cmd_linear_speed=_YEET_RETURN_SPEED, + cmd_linear_acceleration=_YEET_RETURN_ACC, + cmd_rotary_speed=_YEET_RETURN_SPEED, + cmd_rotary_acceleration=_YEET_RETURN_ACC, + ) + finally: + for ax, s in saved_limits.items(): + nid = int(ax) + await driver.write(nid, "VH", 2, s["VH2"]) + await driver.write(nid, "SP", 2, s["SP2"]) + await driver.write(nid, "SD", 0, s["SD0"]) + cfg.axes[ax].max_vel = s["max_vel"] + cfg.axes[ax].max_accel = s["max_accel"] + MOTION_AXES = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) @@ -1200,3 +1435,108 @@ def __repr__(self) -> str: 104: "IndicatorLight", 105: "Buzzer", } + + +# === very_dangerously_yeet helpers (easter egg) ============================ +# Constants and helpers for KX2ArmBackend.very_dangerously_yeet. Inlined +# here on purpose; do not split into another module. + +_YEET_SHOULDER_SWING_DEG = 180.0 +_YEET_RELEASE_FRACTION = 0.85 +# Gripper open target (mm). Clamped at runtime to drive's max_travel - margin. +_YEET_OPEN_POSITION = 30.0 +_YEET_OPEN_SAFETY_MARGIN = 1.0 +# Windup speed: arm holds the plate, don't whip. +_YEET_WINDUP_SPEED = 60.0 +_YEET_WINDUP_ACC = 120.0 +# Return speed: plate is gone, snap back faster than windup. +_YEET_RETURN_SPEED = 240.0 +_YEET_RETURN_ACC = 480.0 + + +def _yeet_profile_time(dist: float, v: float, a: float) -> tuple: + if dist <= 0 or a <= 0 or v <= 0: + return 0.0, 0.0 + t_acc = v / a + d_acc = 0.5 * a * t_acc * t_acc + if 2.0 * d_acc > dist: + t_acc = math.sqrt(dist / a) + return t_acc, 2.0 * t_acc + t_cruise = (dist - 2.0 * d_acc) / v + return t_acc, 2.0 * t_acc + t_cruise + + +def _yeet_wrap_to_range(x: float, lo: float, hi: float) -> float: + span = hi - lo + if span == 0: + return lo + k = math.trunc(x / span) + x = x - k * span + if x < lo: + x += span + if x == hi: + x -= span + return x + + +async def _yeet_build_axis_move( + backend: "KX2ArmBackend", ax: Axis, cur: float, target: float, +) -> tuple: + """Per-axis MotorMoveParam at firmware velocity limit (VH[2]/1.01). + Returns (move, t_acc, t_total, v_phys).""" + cfg = backend._cfg + ax_cfg = cfg.axes[ax] + conv = ax_cfg.motor_conversion_factor + vh2 = await backend.driver.query_int(int(ax), "VH", 2) + v_phys = vh2 / 1.01 / abs(conv) + a_phys = ax_cfg.max_accel + direction = ax_cfg.joint_move_direction + + d = target - cur + span = ax_cfg.max_travel - ax_cfg.min_travel + if span > 0 and ax_cfg.unlimited_travel: + if direction == JointMoveDirection.Clockwise and d > 0.01: + d -= span + elif direction == JointMoveDirection.Counterclockwise and d < -0.01: + d += span + elif direction == JointMoveDirection.ShortestWay: + if d > 180.0: + d -= span + elif d < -180.0: + d += span + dist = abs(d) + + if ax_cfg.unlimited_travel and direction != JointMoveDirection.Normal: + target = _yeet_wrap_to_range(target, ax_cfg.min_travel, ax_cfg.max_travel) + + t_acc, t_total = _yeet_profile_time(dist, v_phys, a_phys) + move = MotorMoveParam( + node_id=int(ax), + position=int(round(target * conv)), + velocity=max(int(round(v_phys * abs(conv))), 1), + acceleration=max(int(round(a_phys * abs(conv))), 1), + direction=direction, + ) + return move, t_acc, t_total, v_phys + + +async def _yeet_arm_plan(driver: KX2Driver, plan: MotorsMovePlan) -> None: + """Pre-load a plan onto the drives without triggering it. Splits SDO + setup latency from the move start so the timer can be accurate.""" + await driver._pvt_select_mode(False) + for move in plan.moves: + nid = int(move.node_id) + await driver._motor_set_move_direction(nid, move.direction) + await driver._can_sdo_download_elmo_object( + nid, 24698, 0, int(move.position), ElmoObjectDataType.INTEGER32, + ) + await driver._can_sdo_download_elmo_object( + nid, 24705, 0, int(move.velocity), ElmoObjectDataType.UNSIGNED32, + ) + acc = max(int(move.acceleration), 100) + await driver._can_sdo_download_elmo_object( + nid, 24707, 0, acc, ElmoObjectDataType.UNSIGNED32, + ) + await driver._can_sdo_download_elmo_object( + nid, 24708, 0, acc, ElmoObjectDataType.UNSIGNED32, + ) diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index a51b30fb671..ec8dc91ea53 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -766,7 +766,11 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N """ if state: self.EmcyMoveErrorReceived = False - max_attempts = 5 + # Drives sometimes need several seconds after a fault / power-rail + # bounce before they accept enable. Spread retries over ~10s rather + # than blasting 5 in <1s. + max_attempts = 20 + inter_attempt_sleep_s = 0.5 for attempt in range(1, max_attempts + 1): if not use_ds402: await self.write(node_id, "MO", 0, 1) @@ -784,6 +788,7 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N "motor_enable attempt %d/%d failed for node %d (MO=%s); retrying", attempt, max_attempts, node_id, left, ) + await asyncio.sleep(inter_attempt_sleep_s) else: raise CanError(f"Motor failed to enable (node_id = {node_id}) after {max_attempts} attempts") else: @@ -855,37 +860,6 @@ async def _motors_move_start( last = i == (len(node_ids) - 1) await self._control_word_set(int(nid), 47 + 0x10 + relative_bit, sync=last) - async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: - await self._pvt_select_mode(False) - - print(f"[MOVE PLAN] move_time={plan.move_time:.3f}s, {len(plan.moves)} axes:") - for move in plan.moves: - print( - f" node={move.node_id} pos={move.position} vel={move.velocity} " - f"acc={move.acceleration} dir={move.direction.name}" - ) - - for move in plan.moves: - nid = int(move.node_id) - await self._motor_set_move_direction(nid, move.direction) - # 0x607A = Target Position (24698 decimal) - await self._can_sdo_download_elmo_object( - nid, 24698, 0, int(move.position), ElmoObjectDataType.INTEGER32, - ) - # 0x6081 = Profile Velocity (24705 decimal) - await self._can_sdo_download_elmo_object( - nid, 24705, 0, int(move.velocity), ElmoObjectDataType.UNSIGNED32, - ) - acc = max(int(move.acceleration), 100) - # 0x6083 = Profile Acceleration (24707 decimal) - await self._can_sdo_download_elmo_object(nid, 24707, 0, acc, ElmoObjectDataType.UNSIGNED32) - # 0x6084 = Profile Deceleration (24708 decimal) - await self._can_sdo_download_elmo_object(nid, 24708, 0, acc, ElmoObjectDataType.UNSIGNED32) - - node_ids = [move.node_id for move in plan.moves] - await self._motors_move_start(node_ids) - await self.wait_for_moves_done(node_ids, timeout=plan.move_time + 2) - async def user_program_run( self, node_id: int, From 7d30a73699948a2e3549c1376433dcfdac9fcb75 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Mon, 27 Apr 2026 20:59:29 -0700 Subject: [PATCH 41/93] KX2: clear CI gates + review polish MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit CI fixes: - ruff: reorder imports above optional pyserial try/except (E402); drop unused `c = _config()` in kinematics tests (F841) - mypy: type `reply: bytes` in os_interpreter; bind `loop = self._loop` for closure narrowing in wait_for_moves_done; widen Home params to List[Union[int, float]] for invariance - pytest: rename kinematics_test.py -> kinematics_tests.py to match the *_tests.py collection pattern (was silently uncollected, 0 tests) Type-API: - kinematics.fk / ik / snap_to_current now use Dict[Axis, float] — keys are Axis values at runtime; matches what callers actually pass - arm_backend drops two redundant int<->Axis conversions Dependency: - declare `canopen = ["canopen"]` extra in pyproject (canopen pulls python-can transitively); add to `all` - gate `import canopen` in driver, raise ImportError at setup() with a clear install hint - list canopen in installation.md extras table Docs wiring: - add docs/user_guide/paa/index.md + register `paa/index` in the manufacturer toctree - add docs/api/pylabrobot.paa.rst + register `pylabrobot.paa` in the top-level API rst - clear notebook outputs Review polish: - promote driver methods used by backend to public: pvt_select_mode, motor_set_move_direction, can_sdo_download_elmo_object, motors_move_start - halt() now fires motor_emergency_stop on all motion axes concurrently via asyncio.gather (was serial — later axes coasted) - start_freedrive_mode honors the `free_axes` argument; default frees motion axes only (gripper stays enabled so a held plate doesn't drop); stop_freedrive_mode re-enables exactly the axes that were freed - KX2BarcodeReaderDriver raises ImportError (not RuntimeError) for missing pyserial — matches KX2Driver canopen style - move MOTION_AXES + _LINEAR_AXES to module top before KX2ArmBackend - replace stray `print()` move-plan debug spam with logger.debug --- docs/api/pylabrobot.paa.rst | 33 ++++ docs/api/pylabrobot.rst | 1 + .../_getting-started/installation.md | 1 + docs/user_guide/index.md | 1 + docs/user_guide/paa/index.md | 7 + docs/user_guide/paa/kx2/hello-world.ipynb | 143 +++++------------- pylabrobot/paa/kx2/arm_backend.py | 99 +++++++----- pylabrobot/paa/kx2/barcode_reader.py | 26 ++-- pylabrobot/paa/kx2/driver.py | 48 +++--- pylabrobot/paa/kx2/kinematics.py | 12 +- ...kinematics_test.py => kinematics_tests.py} | 1 - pyproject.toml | 3 +- 12 files changed, 191 insertions(+), 184 deletions(-) create mode 100644 docs/api/pylabrobot.paa.rst create mode 100644 docs/user_guide/paa/index.md rename pylabrobot/paa/kx2/{kinematics_test.py => kinematics_tests.py} (99%) diff --git a/docs/api/pylabrobot.paa.rst b/docs/api/pylabrobot.paa.rst new file mode 100644 index 00000000000..c9bbd3637d6 --- /dev/null +++ b/docs/api/pylabrobot.paa.rst @@ -0,0 +1,33 @@ +.. currentmodule:: pylabrobot.paa + +pylabrobot.paa package +====================== + +KX2 +--- + +.. currentmodule:: pylabrobot.paa.kx2 + +.. autosummary:: + :toctree: _autosummary + :nosignatures: + :recursive: + + KX2 + KX2Driver + KX2ArmBackend + KX2BarcodeReader + KX2BarcodeReaderDriver + KX2BarcodeReaderBackend + KX2Config + Axis + KX2GripperLocation + +.. autoclass:: pylabrobot.paa.kx2.arm_backend.KX2ArmBackend.CartesianMoveParams + :members: + +.. autoclass:: pylabrobot.paa.kx2.arm_backend.KX2ArmBackend.JointMoveParams + :members: + +.. autoclass:: pylabrobot.paa.kx2.arm_backend.KX2ArmBackend.GripParams + :members: diff --git a/docs/api/pylabrobot.rst b/docs/api/pylabrobot.rst index 72f1969330a..c4fbe299411 100644 --- a/docs/api/pylabrobot.rst +++ b/docs/api/pylabrobot.rst @@ -43,5 +43,6 @@ Manufacturers pylabrobot.mettler_toledo pylabrobot.molecular_devices pylabrobot.opentrons + pylabrobot.paa pylabrobot.qinstruments pylabrobot.thermo_fisher diff --git a/docs/user_guide/_getting-started/installation.md b/docs/user_guide/_getting-started/installation.md index bca89b7d671..6820560d65b 100644 --- a/docs/user_guide/_getting-started/installation.md +++ b/docs/user_guide/_getting-started/installation.md @@ -52,6 +52,7 @@ Different machines use different communication modes. Replace `[usb]` with one o | `cytation-microscopy` | numpy (1.26), opencv-python | Cytation imager | | `sila` | zeroconf, grpcio | SiLA devices | | `pico` | opencv-python, numpy, sila | ImageXpress Pico microscope | +| `canopen` | canopen (pulls python-can) | CANopen devices: e.g. PAA KX2 plate handler. Also requires a vendor CAN driver: PEAK PCAN-Basic on macOS/Windows, the in-tree `peak_pci` module on Linux | | `dev` | All of the above + testing/linting tools | Development | Or install all dependencies: diff --git a/docs/user_guide/index.md b/docs/user_guide/index.md index bb273f350cf..0bdbfc7f7cc 100644 --- a/docs/user_guide/index.md +++ b/docs/user_guide/index.md @@ -41,6 +41,7 @@ liconic/index mettler_toledo/index molecular_devices/index opentrons/index +paa/index qinstruments/index thermo_fisher/index ufactory/index diff --git a/docs/user_guide/paa/index.md b/docs/user_guide/paa/index.md new file mode 100644 index 00000000000..38cae5f216d --- /dev/null +++ b/docs/user_guide/paa/index.md @@ -0,0 +1,7 @@ +# PAA + +```{toctree} +:maxdepth: 1 + +kx2/hello-world +``` diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 2629addef3c..cc15e8fd725 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -48,9 +48,9 @@ "source": [ "## Arm capabilities\n", "\n", - "The KX2 exposes an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm` on `kx2.arm`, backed by {class}`~pylabrobot.paa.kx2.KX2ArmBackend`. Gripper yaw is controlled via a single `direction` float (degrees; 0\u00b0 = front). For the full arm API, see [Arms](../../capabilities/arms).\n", + "The KX2 exposes an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm` on `kx2.arm`, backed by {class}`~pylabrobot.paa.kx2.KX2ArmBackend`. Gripper yaw is controlled via a single `direction` float (degrees; 0° = front). For the full arm API, see [Arms](../../capabilities/arms).\n", "\n", - "The raw driver (`kx2.driver`, a `KX2Driver`) stays available for low-level access \u2014 motor commands, drive diagnostics, binary interpreter, etc." + "The raw driver (`kx2.driver`, a `KX2Driver`) stays available for low-level access — motor commands, drive diagnostics, binary interpreter, etc." ] }, { @@ -90,21 +90,10 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": null, "id": "kx2-gripper-status", "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "True" - ] - }, - "execution_count": 5, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ "await kx2.arm.is_gripper_closed()" ] @@ -177,33 +166,15 @@ "source": [ "### Motion limits\n", "\n", - "`motion_limits()` reads the per-axis maxima cached at setup. Pass values below these into ", - "`JointMoveParams` / `CartesianMoveParams`; anything above is silently clamped. Linear axes are mm/s and mm/s^2; rotary in deg/s and deg/s^2." + "`motion_limits()` reads the per-axis maxima cached at setup. Pass values below these into `JointMoveParams` / `CartesianMoveParams`; anything above is silently clamped. Linear axes are mm/s and mm/s^2; rotary in deg/s and deg/s^2." ] }, { "cell_type": "code", + "execution_count": null, "id": "kx2-motion-limits", "metadata": {}, - "execution_count": 3, - "outputs": [ - { - "data": { - "text/plain": [ - "axis max speed max acceleration\n", - "------------- ------------ ----------------\n", - "SHOULDER 145.00 deg/s 300.00 deg/s^2 \n", - "Z 750.00 mm/s 1000.00 mm/s^2 \n", - "ELBOW 80.00 deg/s 180.00 deg/s^2 \n", - "WRIST 500.00 deg/s 1000.04 deg/s^2 \n", - "SERVO_GRIPPER 47.00 mm/s 200.00 mm/s^2 " - ] - }, - "execution_count": 3, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ "kx2.arm.backend.motion_limits()" ] @@ -215,26 +186,15 @@ "source": [ "### Cartesian movement\n", "\n", - "Move the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only \u2014 the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity and acceleration." + "Move the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only — the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity and acceleration." ] }, { "cell_type": "code", - "execution_count": 7, + "execution_count": null, "id": "11b06c86", "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "GripperLocation(location=Coordinate(x=1.193, y=200.0123, z=300.0067), rotation=Rotation(x=0, y=0, z=99.65582378905646))" - ] - }, - "execution_count": 7, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ "await kx2.arm.request_gripper_location()" ] @@ -336,46 +296,20 @@ }, { "cell_type": "code", - "execution_count": 10, + "execution_count": null, "id": "kx2-query-joints", "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "{: 30.0002217292257,\n", - " : 150.00008664814723,\n", - " : 90.0000385241022,\n", - " : 0.0,\n", - " : 0.22776546532767158}" - ] - }, - "execution_count": 10, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ "await kx2.arm.backend.request_joint_position()" ] }, { "cell_type": "code", - "execution_count": 11, + "execution_count": null, "id": "kx2-query-cart", "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "GripperLocation(location=Coordinate(x=-120.829, y=209.2797, z=150.0001), rotation=Rotation(x=0, y=0, z=30.000264644559707))" - ] - }, - "execution_count": 11, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ "await kx2.arm.request_gripper_location()" ] @@ -387,7 +321,7 @@ "source": [ "### Pick and place\n", "\n", - "`pick_up_at_location` moves to the target pose and closes the gripper to `resource_width`. `drop_at_location` moves and opens the gripper. Approach and retract motions are the caller's responsibility \u2014 bracket these calls with your own pre- and post-moves." + "`pick_up_at_location` moves to the target pose and closes the gripper to `resource_width`. `drop_at_location` moves and opens the gripper. Approach and retract motions are the caller's responsibility — bracket these calls with your own pre- and post-moves." ] }, { @@ -420,7 +354,7 @@ "source": [ "### Freedrive (teaching mode)\n", "\n", - "Freedrive disables torque on the motion axes so you can push the arm by hand; the servo gripper stays powered. The KX2 frees all motion axes at once \u2014 the `free_axes` argument is accepted for API parity and ignored." + "Freedrive disables torque on the motion axes so you can push the arm by hand; the servo gripper stays powered. The KX2 frees all motion axes at once — the `free_axes` argument is accepted for API parity and ignored." ] }, { @@ -435,18 +369,10 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": null, "id": "kx2-free-read", "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "GripperLocation(location=Coordinate(x=353.1052, y=233.0403, z=140.0082), rotation=Rotation(x=0, y=0, z=121.77582182317468))\n" - ] - } - ], + "outputs": [], "source": [ "# Manually guide the arm to the desired pose, then read it:\n", "taught = await kx2.arm.request_gripper_location()\n", @@ -507,11 +433,12 @@ }, { "cell_type": "markdown", + "id": "9e9f0bed", "metadata": {}, "source": [ "## Barcode reader\n", "\n", - "The KX2's onboard barcode reader is a serial 1D laser scanner wired to the controller PC. It's fully independent of the CAN motor stack, so it works even with the arm e-stopped \u2014 exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`.\n", + "The KX2's onboard barcode reader is a serial 1D laser scanner wired to the controller PC. It's fully independent of the CAN motor stack, so it works even with the arm e-stopped — exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`.\n", "\n", "The model that ships with KX2 systems we've inspected is a **Denso MDI-4050** (firmware reports as `BD01J09`). Factory defaults: **9600 8N1**, no flow control. Find your port with `python -m serial.tools.list_ports -v`.\n", "\n", @@ -524,15 +451,15 @@ "open -a PL2303Serial # registers the system extension\n", "```\n", "\n", - "Then **System Settings \u2192 Privacy & Security \u2192 Allow** on the \"System software from PL2303Serial was blocked\" prompt, restart if asked, and replug the cable. Verify with `systemextensionsctl list`: `com.prolific.cdc.PLCdcFSDriver` should show `[activated enabled]`.\n", + "Then **System Settings → Privacy & Security → Allow** on the \"System software from PL2303Serial was blocked\" prompt, restart if asked, and replug the cable. Verify with `systemextensionsctl list`: `com.prolific.cdc.PLCdcFSDriver` should show `[activated enabled]`.\n", "\n", "### Symbologies\n", "\n", - "The MDI-4050 is a 1D laser, so **PDF417 / DataMatrix / QR Code aren't supported** \u2014 you'd need the 2D imager variants (e.g. MDI-4150) for those.\n", + "The MDI-4050 is a 1D laser, so **PDF417 / DataMatrix / QR Code aren't supported** — you'd need the 2D imager variants (e.g. MDI-4150) for those.\n", "\n", "Typical out-of-the-box 1D enables: UPC-A, UPC-E, EAN-13, EAN-8, Code 39, Tri-Optic, Codabar, Industrial 2/5, Interleaved 2/5, IATA, Code 128, Code 93, GS1 DataBar, GS1 DataBar Limited. Add-on (supplemental) codes, postal codes, MSI/Plessey, Telepen, UK/Plessey, and Code 11 are off by default.\n", "\n", - "You can read your unit's full configuration over the wire \u2014 model, firmware, every per-symbology enable flag, prefixes/suffixes, and length limits \u2014 via `dump_config()` (which sends `Z4`):\n", + "You can read your unit's full configuration over the wire — model, firmware, every per-symbology enable flag, prefixes/suffixes, and length limits — via `dump_config()` (which sends `Z4`):\n", "\n", "```python\n", "text = await bcr.driver.dump_config()\n", @@ -541,15 +468,16 @@ "\n", "To **change** which symbologies are enabled, three options in order of practicality:\n", "\n", - "1. **Denso's MDI Setup utility** (free Windows download from Denso ADC) \u2014 checkbox UI, talks to the reader over the same serial port, writes to NVRAM.\n", - "2. **Configuration barcodes** printed in the MDI-4050 setting manual \u2014 scan a Start \u2192 toggle \u2192 End sequence; no host required.\n", - "3. Direct register writes via the ESC protocol \u2014 possible but the per-symbology byte layout is documented only in Denso's protocol manual.\n", + "1. **Denso's MDI Setup utility** (free Windows download from Denso ADC) — checkbox UI, talks to the reader over the same serial port, writes to NVRAM.\n", + "2. **Configuration barcodes** printed in the MDI-4050 setting manual — scan a Start → toggle → End sequence; no host required.\n", + "3. Direct register writes via the ESC protocol — possible but the per-symbology byte layout is documented only in Denso's protocol manual.\n", "\n", - "{class}`~pylabrobot.resources.barcode.Barcode` returned from `scan()` has `symbology=\"ANY 1D\"` by default \u2014 the reader doesn't emit a symbology ID unless prefix/suffix bytes are configured to include one." + "{class}`~pylabrobot.resources.barcode.Barcode` returned from `scan()` has `symbology=\"ANY 1D\"` by default — the reader doesn't emit a symbology ID unless prefix/suffix bytes are configured to include one." ] }, { "cell_type": "markdown", + "id": "7d393b50", "metadata": {}, "source": [ "### Setup\n", @@ -560,6 +488,7 @@ { "cell_type": "code", "execution_count": null, + "id": "52e6a377", "metadata": {}, "outputs": [], "source": [ @@ -571,6 +500,7 @@ }, { "cell_type": "markdown", + "id": "665c811a", "metadata": {}, "source": [ "### Scan\n", @@ -581,6 +511,7 @@ { "cell_type": "code", "execution_count": null, + "id": "8ba1d44d", "metadata": {}, "outputs": [], "source": [ @@ -590,6 +521,7 @@ }, { "cell_type": "markdown", + "id": "5282deb9", "metadata": {}, "source": [ "### Configuration\n", @@ -600,6 +532,7 @@ { "cell_type": "code", "execution_count": null, + "id": "63db140e", "metadata": {}, "outputs": [], "source": [ @@ -612,6 +545,7 @@ }, { "cell_type": "markdown", + "id": "3b16ffac", "metadata": {}, "source": [ "### Driver-level access\n", @@ -622,12 +556,13 @@ { "cell_type": "code", "execution_count": null, + "id": "c0bcb7c9", "metadata": {}, "outputs": [], "source": [ "print(await bcr.driver.get_software_version())\n", "\n", - "# Full NVRAM dump \u2014 model, firmware, all symbology enables, prefixes/suffixes, length limits.\n", + "# Full NVRAM dump — model, firmware, all symbology enables, prefixes/suffixes, length limits.\n", "config = await bcr.driver.dump_config()\n", "print(config[:200])\n", "\n", @@ -637,16 +572,18 @@ }, { "cell_type": "markdown", + "id": "be3ecf8e", "metadata": {}, "source": [ "### Teardown\n", "\n", - "`stop()` sends `Y` (trigger off) and `Y2` (reset to a 2s read window), then closes the serial port \u2014 leaving the reader in a known state for the next session." + "`stop()` sends `Y` (trigger off) and `Y2` (reset to a 2s read window), then closes the serial port — leaving the reader in a known state for the next session." ] }, { "cell_type": "code", "execution_count": null, + "id": "c19e0921", "metadata": {}, "outputs": [], "source": [ @@ -693,4 +630,4 @@ }, "nbformat": 4, "nbformat_minor": 5 -} \ No newline at end of file +} diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 0a38c6bbc65..566f64b3c29 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -6,7 +6,7 @@ import warnings from dataclasses import dataclass from enum import IntEnum -from typing import Dict, List, Optional +from typing import Dict, List, Optional, Union from pylabrobot.capabilities.arms.backend import ( CanFreedrive, @@ -43,6 +43,14 @@ class HomeStatus(IntEnum): InitializedWithoutHoming = 2 +MOTION_AXES = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) + +# Axes that move in linear units (mm/s, mm/s^2). All others move in rotary +# units (deg/s, deg/s^2). Used to pick the right speed/acceleration from +# the linear/rotary split in JointMoveParams / CartesianMoveParams. +_LINEAR_AXES = frozenset({Axis.Z, Axis.RAIL, Axis.SERVO_GRIPPER}) + + class KX2ArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive): """Arm-capability backend for the PAA KX2. @@ -71,6 +79,7 @@ def __init__( self._gripper_z_offset = float(gripper_z_offset) self._gripper_finger_side: GripperFingerSide = gripper_finger_side self._config: Optional[KX2Config] = None + self._freedrive_axes: List[int] = [] async def _on_setup(self, backend_params: Optional[BackendParams] = None): # Driver has already brought CAN up (connect + node discovery + PDO @@ -152,7 +161,7 @@ async def _motor_hard_stop_search( await self.driver.write(nid, "JV", 0, srch_vel) try: - params = [int(hs_pe), int(timeout * 1000)] + params: List[Union[int, float]] = [int(hs_pe), int(timeout * 1000)] last_line = await self.driver.user_program_run( nid, "Home", params, int(timeout), True ) @@ -990,38 +999,42 @@ async def motors_move_joint( await self.motors_move_absolute_execute(plan) async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: - await self.driver._pvt_select_mode(False) + await self.driver.pvt_select_mode(False) - print(f"[MOVE PLAN] move_time={plan.move_time:.3f}s, {len(plan.moves)} axes:") - for move in plan.moves: - print( - f" node={move.node_id} pos={move.position} vel={move.velocity} " - f"acc={move.acceleration} dir={move.direction.name}" + if logger.isEnabledFor(logging.DEBUG): + logger.debug( + "move plan: move_time=%.3fs, %d axes:", plan.move_time, len(plan.moves) ) + for move in plan.moves: + logger.debug( + " node=%d pos=%s vel=%s acc=%s dir=%s", + move.node_id, move.position, move.velocity, + move.acceleration, move.direction.name, + ) for move in plan.moves: nid = int(move.node_id) - await self.driver._motor_set_move_direction(nid, move.direction) + await self.driver.motor_set_move_direction(nid, move.direction) # 0x607A = Target Position (24698 decimal) - await self.driver._can_sdo_download_elmo_object( + await self.driver.can_sdo_download_elmo_object( nid, 24698, 0, int(move.position), ElmoObjectDataType.INTEGER32, ) # 0x6081 = Profile Velocity (24705 decimal) - await self.driver._can_sdo_download_elmo_object( + await self.driver.can_sdo_download_elmo_object( nid, 24705, 0, int(move.velocity), ElmoObjectDataType.UNSIGNED32, ) acc = max(int(move.acceleration), 100) # 0x6083 = Profile Acceleration (24707 decimal) - await self.driver._can_sdo_download_elmo_object( + await self.driver.can_sdo_download_elmo_object( nid, 24707, 0, acc, ElmoObjectDataType.UNSIGNED32 ) # 0x6084 = Profile Deceleration (24708 decimal) - await self.driver._can_sdo_download_elmo_object( + await self.driver.can_sdo_download_elmo_object( nid, 24708, 0, acc, ElmoObjectDataType.UNSIGNED32 ) node_ids = [move.node_id for move in plan.moves] - await self.driver._motors_move_start(node_ids) + await self.driver.motors_move_start(node_ids) await self.driver.wait_for_moves_done(node_ids, timeout=plan.move_time + 2) async def _cart_to_joints( @@ -1034,19 +1047,17 @@ async def _cart_to_joints( snaps each rotary axis to the nearest 360° multiple of the current position, re-enforcing the wrist sign afterward. """ - current = await self.request_joint_position() - current_int = {int(k): v for k, v in current.items()} + current = {Axis(k): v for k, v in (await self.request_joint_position()).items()} # IK needs an explicit cw/ccw; for closest mode fill from the current # joint's sign so IK has a valid choice. Snap then runs with the # *original* pose.wrist — None disables sign re-enforce so the snap # actually picks the closest J4. ik_wrist = pose.wrist if pose.wrist is not None else ( - "ccw" if current_int[Axis.WRIST] >= 0 else "cw" + "ccw" if current[Axis.WRIST] >= 0 else "cw" ) resolved = dataclasses.replace(pose, wrist=ik_wrist) ik_joints = kinematics.ik(resolved, self._cfg) - snapped = kinematics.snap_to_current(ik_joints, current_int, pose.wrist) - return {Axis(k): v for k, v in snapped.items()} + return kinematics.snap_to_current(ik_joints, current, pose.wrist) # -- capability interface (OrientableGripperArmBackend + HasJoints + CanFreedrive) -- @@ -1078,8 +1089,11 @@ class GripParams(BackendParams): check_plate_gripped: bool = True async def halt(self, backend_params: Optional[BackendParams] = None) -> None: - for axis in MOTION_AXES: - await self.driver.motor_emergency_stop(node_id=axis) + # Fire MO=0 on every motion axis concurrently — serial halts let later + # axes coast for the duration of the earlier SDOs. + await asyncio.gather( + *(self.driver.motor_emergency_stop(node_id=axis) for axis in MOTION_AXES) + ) async def park(self, backend_params: Optional[BackendParams] = None) -> None: raise NotImplementedError( @@ -1090,7 +1104,8 @@ async def park(self, backend_params: Optional[BackendParams] = None) -> None: async def request_gripper_location( self, backend_params: Optional[BackendParams] = None ) -> GripperLocation: - return kinematics.fk(await self.request_joint_position(), self._cfg) + joints = {Axis(k): v for k, v in (await self.request_joint_position()).items()} + return kinematics.fk(joints, self._cfg) async def open_gripper( self, gripper_width: float, backend_params: Optional[BackendParams] = None @@ -1237,14 +1252,22 @@ async def start_freedrive_mode( free_axes: Optional[List[int]] = None, backend_params: Optional[BackendParams] = None, ) -> None: - # KX2 frees all motion axes at once; free_axes is accepted for API parity. - del free_axes - for axis in MOTION_AXES: + # Default: free all motion axes (shoulder/Z/elbow/wrist) but never the + # gripper, so a held plate doesn't drop. Caller can override with an + # explicit list; [0] means "all motion axes" per CanFreedrive convention. + if free_axes is None or free_axes == [0]: + axes: List[int] = [int(a) for a in MOTION_AXES] + else: + axes = [int(a) for a in free_axes] + for axis in axes: await self.driver.motor_enable(node_id=axis, state=False, use_ds402=True) + self._freedrive_axes = axes async def stop_freedrive_mode(self, backend_params: Optional[BackendParams] = None) -> None: - for axis in MOTION_AXES: + axes = self._freedrive_axes or list(MOTION_AXES) + for axis in axes: await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) + self._freedrive_axes = [] async def very_dangerously_yeet( self, @@ -1360,10 +1383,10 @@ async def very_dangerously_yeet( await _yeet_arm_plan(driver, sh_plan) await _yeet_arm_plan(driver, wr_plan) - await driver._motors_move_start([int(Axis.SHOULDER)]) + await driver.motors_move_start([int(Axis.SHOULDER)]) t0 = time.monotonic() await asyncio.sleep(max(0.0, wrist_trigger_t - (time.monotonic() - t0))) - await driver._motors_move_start([int(Axis.WRIST)]) + await driver.motors_move_start([int(Axis.WRIST)]) await asyncio.sleep(max(0.0, release_t - (time.monotonic() - t0))) await self.motors_move_absolute_execute(gripper_plan) @@ -1395,14 +1418,6 @@ async def very_dangerously_yeet( cfg.axes[ax].max_accel = s["max_accel"] -MOTION_AXES = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) - -# Axes that move in linear units (mm/s, mm/s^2). All others move in rotary -# units (deg/s, deg/s^2). Used to pick the right speed/acceleration from -# the linear/rotary split in JointMoveParams / CartesianMoveParams. -_LINEAR_AXES = frozenset({Axis.Z, Axis.RAIL, Axis.SERVO_GRIPPER}) - - class _MotionLimits(Dict[Axis, tuple]): """Pretty-printing dict for `KX2ArmBackend.motion_limits()`. Dict access still works (`limits[Axis.Z]` -> `(max_speed, max_accel)`); `__repr__` @@ -1523,20 +1538,20 @@ async def _yeet_build_axis_move( async def _yeet_arm_plan(driver: KX2Driver, plan: MotorsMovePlan) -> None: """Pre-load a plan onto the drives without triggering it. Splits SDO setup latency from the move start so the timer can be accurate.""" - await driver._pvt_select_mode(False) + await driver.pvt_select_mode(False) for move in plan.moves: nid = int(move.node_id) - await driver._motor_set_move_direction(nid, move.direction) - await driver._can_sdo_download_elmo_object( + await driver.motor_set_move_direction(nid, move.direction) + await driver.can_sdo_download_elmo_object( nid, 24698, 0, int(move.position), ElmoObjectDataType.INTEGER32, ) - await driver._can_sdo_download_elmo_object( + await driver.can_sdo_download_elmo_object( nid, 24705, 0, int(move.velocity), ElmoObjectDataType.UNSIGNED32, ) acc = max(int(move.acceleration), 100) - await driver._can_sdo_download_elmo_object( + await driver.can_sdo_download_elmo_object( nid, 24707, 0, acc, ElmoObjectDataType.UNSIGNED32, ) - await driver._can_sdo_download_elmo_object( + await driver.can_sdo_download_elmo_object( nid, 24708, 0, acc, ElmoObjectDataType.UNSIGNED32, ) diff --git a/pylabrobot/paa/kx2/barcode_reader.py b/pylabrobot/paa/kx2/barcode_reader.py index 12c6363e9ca..6733b92ef43 100644 --- a/pylabrobot/paa/kx2/barcode_reader.py +++ b/pylabrobot/paa/kx2/barcode_reader.py @@ -82,16 +82,6 @@ import time from typing import Literal, Optional -ReadMode = Literal["single", "multiple", "continuous"] - -try: - import serial as pyserial - - _HAS_SERIAL = True -except ImportError as _e: - _HAS_SERIAL = False - _SERIAL_IMPORT_ERROR = _e - from pylabrobot.capabilities.barcode_scanning import BarcodeScanner from pylabrobot.capabilities.barcode_scanning.backend import ( BarcodeScannerBackend, @@ -102,6 +92,16 @@ from pylabrobot.io.serial import Serial from pylabrobot.resources.barcode import Barcode +ReadMode = Literal["single", "multiple", "continuous"] + +try: + import serial as pyserial + + _HAS_SERIAL = True +except ImportError as _e: + _HAS_SERIAL = False + _SERIAL_IMPORT_ERROR = _e + logger = logging.getLogger(__name__) _ESC = b"\x1b" @@ -125,9 +125,9 @@ class KX2BarcodeReaderDriver(Driver): def __init__(self, port: str, baudrate: int = default_baudrate): if not _HAS_SERIAL: - raise RuntimeError( - "pyserial is not installed. Install with: pip install pylabrobot[serial]. " - f"Import error: {_SERIAL_IMPORT_ERROR}" + raise ImportError( + "pyserial is not installed. Install with `pip install pylabrobot[serial]` " + f"(import error: {_SERIAL_IMPORT_ERROR})" ) super().__init__() self.io = Serial( diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index ec8dc91ea53..d141cc47d21 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -20,11 +20,17 @@ from enum import IntEnum from typing import Dict, List, Optional, Tuple, Union -import canopen - from pylabrobot.capabilities.capability import BackendParams from pylabrobot.device import Driver +try: + import canopen + + _HAS_CANOPEN = True +except ImportError as _e: + _HAS_CANOPEN = False + _CANOPEN_IMPORT_ERROR = _e + def _u32_le(value: int) -> List[int]: return list((value & 0xFFFFFFFF).to_bytes(4, byteorder="little", signed=False)) @@ -144,7 +150,7 @@ class JointMoveDirection(IntEnum): """Move-direction hint used by the driver's move primitives. Lives here (not in the backend) because the driver's - `_motor_set_move_direction` primitive consumes it to program Elmo's modulo + `motor_set_move_direction` primitive consumes it to program Elmo's modulo mode register. Backend-side planning also uses it, but the canonical definition is the driver's. """ @@ -240,6 +246,11 @@ def __init__( async def setup(self, backend_params: Optional[BackendParams] = None) -> None: """Bring up the CAN bus, reset/start nodes, and configure PDO mapping.""" + if not _HAS_CANOPEN: + raise ImportError( + "canopen is not installed. Install with `pip install pylabrobot[canopen]` " + f"(import error: {_CANOPEN_IMPORT_ERROR})" + ) if self._network is not None: await self.stop() @@ -291,12 +302,12 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: # Elmo vendor objects: interpolation config for PVT mode. for nid in self.motion_node_ids: - await self._can_sdo_download_elmo_object(nid, 24768, 0, -1, ElmoObjectDataType.INTEGER16) - await self._can_sdo_download_elmo_object(nid, 24772, 2, 16, ElmoObjectDataType.UNSIGNED32) - await self._can_sdo_download_elmo_object(nid, 24772, 3, 0, ElmoObjectDataType.UNSIGNED8) - await self._can_sdo_download_elmo_object(nid, 24772, 5, 8, ElmoObjectDataType.UNSIGNED8) - await self._can_sdo_download_elmo_object(nid, 24770, 2, -3, ElmoObjectDataType.INTEGER8) - await self._can_sdo_download_elmo_object(nid, 24669, 0, 1, ElmoObjectDataType.INTEGER16) + await self.can_sdo_download_elmo_object(nid, 24768, 0, -1, ElmoObjectDataType.INTEGER16) + await self.can_sdo_download_elmo_object(nid, 24772, 2, 16, ElmoObjectDataType.UNSIGNED32) + await self.can_sdo_download_elmo_object(nid, 24772, 3, 0, ElmoObjectDataType.UNSIGNED8) + await self.can_sdo_download_elmo_object(nid, 24772, 5, 8, ElmoObjectDataType.UNSIGNED8) + await self.can_sdo_download_elmo_object(nid, 24770, 2, -3, ElmoObjectDataType.INTEGER8) + await self.can_sdo_download_elmo_object(nid, 24669, 0, 1, ElmoObjectDataType.INTEGER16) # RPDO1 = ControlWord (for DS402 enable), RPDO3 = interpolated target. for nid in self.motion_node_ids: @@ -311,7 +322,7 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: ) self._pvt_mode = True - await self._pvt_select_mode(False) + await self.pvt_select_mode(False) async def stop(self) -> None: if self._network is not None: @@ -432,7 +443,7 @@ async def _can_sdo_download( node = self._nodes[node_id] await asyncio.to_thread(node.sdo.download, index, sub_index, bytes(data_byte)) - async def _can_sdo_download_elmo_object( + async def can_sdo_download_elmo_object( self, node_id: int, elmo_object_int: int, @@ -648,7 +659,7 @@ async def _os_interpreter( return "" # 0x1023:3 = OSCommand.Reply (DOMAIN / string). Library handles segmented. - reply = await asyncio.to_thread(node.sdo.upload, 0x1023, 3) + reply: bytes = await asyncio.to_thread(node.sdo.upload, 0x1023, 3) return reply.decode("ascii", errors="replace").rstrip("\x00").rstrip() # --- raw CANopen sends (SYNC + RPDO1 controlword) ----------------------- @@ -684,14 +695,14 @@ async def motor_get_current_position(self, node_id: int, pu: bool = False) -> in async def motor_get_motion_status(self, node_id: int) -> int: return await self.query_int(node_id, "MS", 0) - async def _motor_set_move_direction( + async def motor_set_move_direction( self, node_id: int, direction: JointMoveDirection ) -> None: # Elmo modulo mode register: bit0 enables modulo; bits6..7 encode the # direction (0=Normal, 1=CW, 2=CCW, 3=Shortest). Packs to 1 + 64*direction # = 1/65/129/193. val = 1 + 64 * int(direction) - await self._can_sdo_download_elmo_object(node_id, 24818, 0, val, ElmoObjectDataType.UNSIGNED16) + await self.can_sdo_download_elmo_object(node_id, 24818, 0, val, ElmoObjectDataType.UNSIGNED16) async def motor_check_if_move_done(self, node_id: int) -> bool: ms_val = await self.query_int(node_id, "MS", 0) @@ -806,7 +817,7 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N # --- motion primitives -------------------------------------------------- - async def _pvt_select_mode(self, enable: bool) -> None: + async def pvt_select_mode(self, enable: bool) -> None: """Enable/disable PVT mode on all motion axes via standard SDO writes.""" if enable: if not self._pvt_mode: @@ -832,11 +843,12 @@ async def wait_for_moves_done( # Poll MS every 30ms after a 50ms warm-up. The warm-up avoids reading # MS=0 in the window between CW=63 and motion actually starting. assert self._loop is not None + loop = self._loop async def _poll_axis(nid: int) -> None: - deadline = self._loop.time() + timeout + deadline = loop.time() + timeout await asyncio.sleep(0.05) - while self._loop.time() < deadline: + while loop.time() < deadline: try: if await self.motor_check_if_move_done(int(nid)): return @@ -849,7 +861,7 @@ async def _poll_axis(nid: int) -> None: await asyncio.gather(*(_poll_axis(n) for n in node_ids)) - async def _motors_move_start( + async def motors_move_start( self, node_ids: List[int], *, relative: bool = False ) -> None: relative_bit = 0x40 if relative else 0 diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index 2204f71ea16..deef45c5703 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -53,11 +53,11 @@ class IKError(ValueError): """Target pose is unreachable (for now: non-Z rotation requested).""" -def fk(joints: Dict[int, float], c: KX2Config) -> KX2GripperLocation: +def fk(joints: Dict[Axis, float], c: KX2Config) -> KX2GripperLocation: """Forward kinematics. Args: - joints: {1: shoulder deg, 2: Z mm, 3: elbow mm, 4: wrist deg}. + joints: {Axis.SHOULDER: deg, Axis.Z: mm, Axis.ELBOW: mm, Axis.WRIST: deg}. c: arm configuration. Returns: KX2GripperLocation with the gripper clamp point and a wrist sign @@ -93,7 +93,7 @@ def fk(joints: Dict[int, float], c: KX2Config) -> KX2GripperLocation: ) -def ik(pose: KX2GripperLocation, c: KX2Config) -> Dict[int, float]: +def ik(pose: KX2GripperLocation, c: KX2Config) -> Dict[Axis, float]: """Inverse kinematics. Args: @@ -102,7 +102,7 @@ def ik(pose: KX2GripperLocation, c: KX2Config) -> Dict[int, float]: current joint's sign, then call `snap_to_current` after). c: arm configuration. Returns: - joints dict {1: shoulder deg, 2: Z mm, 3: elbow mm, 4: wrist deg}. + joints dict {Axis.SHOULDER: deg, Axis.Z: mm, Axis.ELBOW: mm, Axis.WRIST: deg}. J4 is in (-360°, 0°] when wrist="cw" and [0°, 360°) when wrist="ccw" (J4 ≈ 0 satisfies both, up to `c.eps`). @@ -148,8 +148,8 @@ def ik(pose: KX2GripperLocation, c: KX2Config) -> Dict[int, float]: def snap_to_current( - joints: Dict[int, float], current: Dict[int, float], wrist: Optional[Wrist] = None -) -> Dict[int, float]: + joints: Dict[Axis, float], current: Dict[Axis, float], wrist: Optional[Wrist] = None +) -> Dict[Axis, float]: """Shift rotary joints by 360° multiples toward `current`. Z and elbow are prismatic and pass through unchanged. diff --git a/pylabrobot/paa/kx2/kinematics_test.py b/pylabrobot/paa/kx2/kinematics_tests.py similarity index 99% rename from pylabrobot/paa/kx2/kinematics_test.py rename to pylabrobot/paa/kx2/kinematics_tests.py index 9ecff9f7c01..419aa999fb8 100644 --- a/pylabrobot/paa/kx2/kinematics_test.py +++ b/pylabrobot/paa/kx2/kinematics_tests.py @@ -115,7 +115,6 @@ def test_ccw_yields_non_negative_wrist(self): def test_wrist_near_zero_satisfies_both(self): """When the natural J4 is exactly 0, neither cw nor ccw should shift it.""" - c = _config() # Construct a pose where wrist - shoulder = 0 naturally: # rotation.z == shoulder. Pick rotation.z = -atan2(x, y) in degrees. x, y = 100.0, 100.0 diff --git a/pyproject.toml b/pyproject.toml index c873d4096bd..87f7a8a4fbe 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -22,7 +22,8 @@ sila = ["zeroconf>=0.131.0", "grpcio"] cytation-microscopy = ["numpy>=1.26", "opencv-python", "PyGObject"] pico = ["PyLabRobot[sila]", "opencv-python", "numpy"] xarm = ["xarm-python-sdk"] -all = ["PyLabRobot[serial,usb,ftdi,hid,modbus,websockets,visualizer,opentrons,sila,pico,xarm]"] +canopen = ["canopen"] +all = ["PyLabRobot[serial,usb,ftdi,hid,modbus,websockets,visualizer,opentrons,sila,pico,xarm,canopen]"] test = [ "pytest", "pytest-timeout", From 3968dc71ad47cf427820625e99e27a8011f38666 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Mon, 27 Apr 2026 21:45:45 -0700 Subject: [PATCH 42/93] KX2: expose has_rail/has_servo_gripper kwargs; async-safe yeet prompt MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - KX2.__init__ now plumbs has_rail and has_servo_gripper through to KX2Driver, but raises NotImplementedError when either differs from the tested defaults. The driver supports both topologies, but the backend setup path (servo_gripper_initialize, rail handling) has only been exercised against the standard 4-axis arm + servo gripper — surface that loudly instead of crashing inside _on_setup with a cryptic "Servo gripper not present". - very_dangerously_yeet's confirmation prompt now runs through asyncio.to_thread(input, ...) so the canopen RX listener and other coroutines stay live while the operator types. UX is unchanged. --- pylabrobot/paa/kx2/arm_backend.py | 5 ++++- pylabrobot/paa/kx2/kx2.py | 20 +++++++++++++++++++- 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 566f64b3c29..42522f19f26 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -1291,7 +1291,10 @@ async def very_dangerously_yeet( "and open the gripper mid-swing. Anything in the gripper will be " "thrown. High bump can fault the drive. Type 'y' to continue: " ) - if input(warning).strip().lower() != "y": + # Run the blocking prompt off the loop so the canopen RX listener and + # any other coroutines stay live while we wait for the operator. + answer = await asyncio.to_thread(input, warning) + if answer.strip().lower() != "y": raise RuntimeError("very_dangerously_yeet: aborted by user") driver = self.driver diff --git a/pylabrobot/paa/kx2/kx2.py b/pylabrobot/paa/kx2/kx2.py index 9e79e7c06f4..937525ed8d3 100644 --- a/pylabrobot/paa/kx2/kx2.py +++ b/pylabrobot/paa/kx2/kx2.py @@ -20,8 +20,26 @@ def __init__( gripper_length: float = 0.0, gripper_z_offset: float = 0.0, gripper_finger_side: GripperFingerSide = "barcode_reader", + has_rail: bool = False, + has_servo_gripper: bool = True, ) -> None: - driver = KX2Driver() + # The non-default topologies (rail-mounted KX2, gripper-less KX2) + # are accepted by the driver but the backend / capability layer has + # only been exercised against the standard 4-axis arm + servo + # gripper. setup() currently calls servo_gripper_initialize + # unconditionally, so has_servo_gripper=False crashes during init; + # rail support hasn't been validated end-to-end either. Surface that + # as a clear error rather than letting users hit cryptic failures + # downstream. + if has_rail or not has_servo_gripper: + raise NotImplementedError( + "KX2 has only been tested with the default 4-axis arm + servo " + "gripper topology (has_rail=False, has_servo_gripper=True). " + "Other configurations are wired through to the driver but the " + "backend setup path needs work — see KX2ArmBackend._on_setup " + "and servo_gripper_initialize." + ) + driver = KX2Driver(has_rail=has_rail, has_servo_gripper=has_servo_gripper) super().__init__(driver=driver) self.driver: KX2Driver = driver backend = KX2ArmBackend( From 9e3b5eb3e5178f71852cf1558741cfc0733f7990 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 28 Apr 2026 14:48:30 -0700 Subject: [PATCH 43/93] KX2 + CanFreedrive: review polish round 3 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - CanFreedrive.start_freedrive_mode: widen base signature so free_axes is Optional[List[int]] = None (was required List[int]). Backends' default behavior is "free all freeable axes" — making that the contract avoids overrides having to widen and brings the base in line with what brooks/kx2 already do. Update xArm6 override to match. - KX2ArmBackend.servo_gripper_initialize: stop swallowing motor_enable failures. The next step is homing, which faults with a confusing "homing failure" if the motor never came up; let the real cause propagate. - very_dangerously_yeet warning: drop the leading emoji + ×, plain ASCII so review tooling and terminals without emoji rendering don't trip on it. --- pylabrobot/capabilities/arms/backend.py | 10 ++++++++-- pylabrobot/paa/kx2/arm_backend.py | 18 +++++++----------- pylabrobot/ufactory/xarm6/backend.py | 4 +++- 3 files changed, 18 insertions(+), 14 deletions(-) diff --git a/pylabrobot/capabilities/arms/backend.py b/pylabrobot/capabilities/arms/backend.py index 817848958fa..3422f8525f6 100644 --- a/pylabrobot/capabilities/arms/backend.py +++ b/pylabrobot/capabilities/arms/backend.py @@ -37,12 +37,18 @@ class CanFreedrive(metaclass=ABCMeta): @abstractmethod async def start_freedrive_mode( - self, free_axes: List[int], backend_params: Optional[BackendParams] = None + self, + free_axes: Optional[List[int]] = None, + backend_params: Optional[BackendParams] = None, ) -> None: """Enter freedrive mode, allowing manual movement of the specified joints. Args: - free_axes: List of joint indices to free. Use [0] for all axes. + free_axes: List of joint indices to free. ``None`` or ``[0]`` mean + the backend's default ("all freeable axes" — typically all motion + axes, excluding load-bearing axes like a gripper that's holding a + plate). Backends may reject per-axis selection if they only + support all-or-nothing freedrive. """ @abstractmethod diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 42522f19f26..4f9eb909811 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -283,17 +283,13 @@ async def home_motor( # -- servo gripper ------------------------------------------------------ async def servo_gripper_initialize(self): - try: - await self.driver.motor_enable( - node_id=int(Axis.SERVO_GRIPPER), state=True, use_ds402=False - ) - except Exception as e: - logger.warning( - "Error enabling servo gripper motor on node %s: %s", Axis.SERVO_GRIPPER, e - ) - + # Don't swallow motor_enable failures here — homing is the next step + # and will fault with a confusing "homing failure" error if the motor + # never came up. Better to surface the real cause. + await self.driver.motor_enable( + node_id=int(Axis.SERVO_GRIPPER), state=True, use_ds402=False + ) await self.servo_gripper_home() - await self.servo_gripper_close() async def servo_gripper_home(self) -> None: @@ -1287,7 +1283,7 @@ async def very_dangerously_yeet( higher risks tracking-error faults that need Elmo Composer recovery. """ warning = ( - f"⚠️ very_dangerously_yeet: swing the arm at {bump:.2f}× firmware-max " + f"WARNING: very_dangerously_yeet: swing the arm at {bump:.2f}x firmware-max " "and open the gripper mid-swing. Anything in the gripper will be " "thrown. High bump can fault the drive. Type 'y' to continue: " ) diff --git a/pylabrobot/ufactory/xarm6/backend.py b/pylabrobot/ufactory/xarm6/backend.py index b4bfb62a7b7..358d830b434 100644 --- a/pylabrobot/ufactory/xarm6/backend.py +++ b/pylabrobot/ufactory/xarm6/backend.py @@ -277,7 +277,9 @@ async def drop_at_joint_position( # -- CanFreedrive ---------------------------------------------------------- async def start_freedrive_mode( - self, free_axes: List[int], backend_params: Optional[BackendParams] = None + self, + free_axes: Optional[List[int]] = None, + backend_params: Optional[BackendParams] = None, ) -> None: """Enter freedrive (manual teaching) mode. From f16707c3a0811a0826a38e2c9e20db6ecb49a930 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 28 Apr 2026 14:57:00 -0700 Subject: [PATCH 44/93] KX2 Axis: add is_motion / is_linear properties; drop _LINEAR_AXES Move axis classification onto the enum where it belongs. Three membership-style call sites (use_ds402 gate, _read_axis_config motion direction, motion_limits unit picker) now read directly off the axis: ``ax.is_motion`` / ``ax.is_linear``. ``_LINEAR_AXES`` is gone; ``MOTION_AXES`` survives as a one-line derived tuple at module top because four iteration sites still want a stable ordering. --- pylabrobot/paa/kx2/arm_backend.py | 17 +++++++---------- pylabrobot/paa/kx2/config.py | 14 ++++++++++++++ 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 4f9eb909811..335abfcca34 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -43,12 +43,9 @@ class HomeStatus(IntEnum): InitializedWithoutHoming = 2 -MOTION_AXES = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) - -# Axes that move in linear units (mm/s, mm/s^2). All others move in rotary -# units (deg/s, deg/s^2). Used to pick the right speed/acceleration from -# the linear/rotary split in JointMoveParams / CartesianMoveParams. -_LINEAR_AXES = frozenset({Axis.Z, Axis.RAIL, Axis.SERVO_GRIPPER}) +# Tuple of motion axes — derived from `Axis.is_motion`, kept around because +# iteration sites (setup, halt, freedrive) want a stable ordering. +MOTION_AXES = tuple(a for a in Axis if a.is_motion) class KX2ArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive): @@ -243,7 +240,7 @@ async def home_motor( raise RuntimeError(fault) raise e - await self.driver.motor_enable(node_id=nid, state=True, use_ds402=nid in MOTION_AXES) + await self.driver.motor_enable(node_id=nid, state=True, use_ds402=Axis(nid).is_motion) await self.motors_move_absolute_execute( plan=MotorsMovePlan( @@ -511,7 +508,7 @@ async def _read_axis_config(self, nid: int) -> AxisConfig: unlimited_travel = False elif xm1 > vl3 and xm2 < vh3: unlimited_travel = True - if nid in MOTION_AXES: + if Axis(nid).is_motion: joint_move_direction = JointMoveDirection.ShortestWay else: raise CanError( @@ -881,7 +878,7 @@ async def calculate_move_abs_all_axes( axis_max_v = self._cfg.axes[ax].max_vel axis_max_a = self._cfg.axes[ax].max_accel - if ax in _LINEAR_AXES: + if ax.is_linear: chosen_v = cmd_linear_speed if cmd_linear_speed is not None else axis_max_v chosen_a = cmd_linear_acceleration if cmd_linear_acceleration is not None else axis_max_a else: @@ -1426,7 +1423,7 @@ class _MotionLimits(Dict[Axis, tuple]): def __repr__(self) -> str: rows = [] for ax, (v, a) in self.items(): - unit = "mm" if ax in _LINEAR_AXES else "deg" + unit = "mm" if ax.is_linear else "deg" rows.append((ax.name, f"{v:.2f} {unit}/s", f"{a:.2f} {unit}/s^2")) headers = ("axis", "max speed", "max acceleration") widths = [max(len(headers[i]), *(len(r[i]) for r in rows)) for i in range(3)] diff --git a/pylabrobot/paa/kx2/config.py b/pylabrobot/paa/kx2/config.py index 89b6be5dbe1..4ebd453262e 100644 --- a/pylabrobot/paa/kx2/config.py +++ b/pylabrobot/paa/kx2/config.py @@ -27,6 +27,20 @@ class Axis(IntEnum): RAIL = 5 SERVO_GRIPPER = 6 + @property + def is_motion(self) -> bool: + """The four-axis arm proper. Excludes the rail (Cartesian carrier) and + the servo gripper (end-effector). Used for setup, halt, freedrive — any + operation that targets "the arm" without its peripherals.""" + return self in (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) + + @property + def is_linear(self) -> bool: + """Axis travels in linear units (mm/s, mm/s^2). All others are rotary + (deg/s, deg/s^2). Used to pick the right speed/acceleration from the + linear/rotary split in JointMoveParams / CartesianMoveParams.""" + return self in (Axis.Z, Axis.RAIL, Axis.SERVO_GRIPPER) + @dataclass class AxisConfig: From 89bb46647507e717dccd09f98926cd7f4d5f83a8 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 28 Apr 2026 15:08:43 -0700 Subject: [PATCH 45/93] KX2: collapse motors_move_joint kwargs into a single JointMoveParams MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit motors_move_joint and calculate_move_abs_all_axes used to take cmd_pos plus four positional Optional[float] limits (cmd_linear_speed, cmd_linear_acceleration, cmd_rotary_speed, cmd_rotary_acceleration). Every caller either passed all four as None (the four gripper helpers + open_gripper / close_gripper) or unpacked a JointMoveParams / CartesianMoveParams into the four kwargs. Both shapes match JointMoveParams already, so accept it directly. Net -28 lines; the four gripper / open / close call sites collapse from 6 lines to 1. CartesianMoveParams gets one explicit copy at the move_to_gripper_location boundary because it's a separate public type — same fields, different namespace. --- pylabrobot/paa/kx2/arm_backend.py | 112 +++++++++++------------------- 1 file changed, 42 insertions(+), 70 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 335abfcca34..a7b3cee988c 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -384,25 +384,13 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: ) async def servo_gripper_close(self, closed_position: int = 0, check_plate_gripped=True) -> None: - await self.motors_move_joint( - {Axis.SERVO_GRIPPER: closed_position}, - cmd_linear_speed=None, - cmd_linear_acceleration=None, - cmd_rotary_speed=None, - cmd_rotary_acceleration=None, - ) + await self.motors_move_joint({Axis.SERVO_GRIPPER: closed_position}) if check_plate_gripped: await self.check_plate_gripped() async def servo_gripper_open(self, open_position: float) -> None: - await self.motors_move_joint( - {Axis.SERVO_GRIPPER: open_position}, - cmd_linear_speed=None, - cmd_linear_acceleration=None, - cmd_rotary_speed=None, - cmd_rotary_acceleration=None, - ) + await self.motors_move_joint({Axis.SERVO_GRIPPER: open_position}) async def drive_set_move_count_parameters( self, @@ -771,11 +759,10 @@ def _profile(dist: float, v: float, a: float) -> tuple: async def calculate_move_abs_all_axes( self, cmd_pos: Dict[Axis, float], - cmd_linear_speed: Optional[float], - cmd_linear_acceleration: Optional[float], - cmd_rotary_speed: Optional[float], - cmd_rotary_acceleration: Optional[float], + params: Optional["KX2ArmBackend.JointMoveParams"] = None, ) -> Optional[MotorsMovePlan]: + if params is None: + params = KX2ArmBackend.JointMoveParams() target = cmd_pos.copy() axes = list(target.keys()) @@ -786,8 +773,10 @@ async def calculate_move_abs_all_axes( # input validation / travel limits / done-wait logic for name, val in ( - ("linear_speed", cmd_linear_speed), ("linear_acceleration", cmd_linear_acceleration), - ("rotary_speed", cmd_rotary_speed), ("rotary_acceleration", cmd_rotary_acceleration), + ("linear_speed", params.linear_speed), + ("linear_acceleration", params.linear_acceleration), + ("rotary_speed", params.rotary_speed), + ("rotary_acceleration", params.rotary_acceleration), ): if val is not None and val <= 0.0: raise ValueError(f"{name} must be positive, got {val}") @@ -879,11 +868,15 @@ async def calculate_move_abs_all_axes( axis_max_v = self._cfg.axes[ax].max_vel axis_max_a = self._cfg.axes[ax].max_accel if ax.is_linear: - chosen_v = cmd_linear_speed if cmd_linear_speed is not None else axis_max_v - chosen_a = cmd_linear_acceleration if cmd_linear_acceleration is not None else axis_max_a + chosen_v = params.linear_speed if params.linear_speed is not None else axis_max_v + chosen_a = ( + params.linear_acceleration if params.linear_acceleration is not None else axis_max_a + ) else: - chosen_v = cmd_rotary_speed if cmd_rotary_speed is not None else axis_max_v - chosen_a = cmd_rotary_acceleration if cmd_rotary_acceleration is not None else axis_max_a + chosen_v = params.rotary_speed if params.rotary_speed is not None else axis_max_v + chosen_a = ( + params.rotary_acceleration if params.rotary_acceleration is not None else axis_max_a + ) v[ax] = min(chosen_v, axis_max_v) a[ax] = min(chosen_a, axis_max_a) @@ -972,19 +965,10 @@ async def calculate_move_abs_all_axes( async def motors_move_joint( self, cmd_pos: Dict[Axis, float], - cmd_linear_speed: Optional[float], - cmd_linear_acceleration: Optional[float], - cmd_rotary_speed: Optional[float], - cmd_rotary_acceleration: Optional[float], + params: Optional["KX2ArmBackend.JointMoveParams"] = None, ) -> None: logger.debug("motors_move_joint cmd_pos=%s", cmd_pos) - plan = await self.calculate_move_abs_all_axes( - cmd_pos=cmd_pos, - cmd_linear_speed=cmd_linear_speed, - cmd_linear_acceleration=cmd_linear_acceleration, - cmd_rotary_speed=cmd_rotary_speed, - cmd_rotary_acceleration=cmd_rotary_acceleration, - ) + plan = await self.calculate_move_abs_all_axes(cmd_pos=cmd_pos, params=params) if plan is None: # if every axis is skipped, exit return @@ -1103,26 +1087,14 @@ async def request_gripper_location( async def open_gripper( self, gripper_width: float, backend_params: Optional[BackendParams] = None ) -> None: - await self.motors_move_joint( - {Axis.SERVO_GRIPPER: gripper_width}, - cmd_linear_speed=None, - cmd_linear_acceleration=None, - cmd_rotary_speed=None, - cmd_rotary_acceleration=None, - ) + await self.motors_move_joint({Axis.SERVO_GRIPPER: gripper_width}) async def close_gripper( self, gripper_width: float, backend_params: Optional[BackendParams] = None ) -> None: if not isinstance(backend_params, KX2ArmBackend.GripParams): backend_params = KX2ArmBackend.GripParams() - await self.motors_move_joint( - {Axis.SERVO_GRIPPER: gripper_width}, - cmd_linear_speed=None, - cmd_linear_acceleration=None, - cmd_rotary_speed=None, - cmd_rotary_acceleration=None, - ) + await self.motors_move_joint({Axis.SERVO_GRIPPER: gripper_width}) if backend_params.check_plate_gripped: await self.check_plate_gripped() @@ -1143,13 +1115,15 @@ async def move_to_gripper_location( if not isinstance(backend_params, KX2ArmBackend.CartesianMoveParams): backend_params = KX2ArmBackend.CartesianMoveParams() joint_pos = await self._cart_to_joints(pose) - await self.motors_move_joint( - cmd_pos=joint_pos, - cmd_linear_speed=backend_params.linear_speed, - cmd_linear_acceleration=backend_params.linear_acceleration, - cmd_rotary_speed=backend_params.rotary_speed, - cmd_rotary_acceleration=backend_params.rotary_acceleration, + # CartesianMoveParams and JointMoveParams have identical shape (linear/rotary + # speed + acceleration); copy the limits across to feed motors_move_joint. + joint_params = KX2ArmBackend.JointMoveParams( + linear_speed=backend_params.linear_speed, + linear_acceleration=backend_params.linear_acceleration, + rotary_speed=backend_params.rotary_speed, + rotary_acceleration=backend_params.rotary_acceleration, ) + await self.motors_move_joint(cmd_pos=joint_pos, params=joint_params) async def move_to_location( self, @@ -1190,13 +1164,7 @@ async def move_to_joint_position( if not isinstance(backend_params, KX2ArmBackend.JointMoveParams): backend_params = KX2ArmBackend.JointMoveParams() cmd_pos = {Axis(int(k)): float(v) for k, v in position.items()} - await self.motors_move_joint( - cmd_pos=cmd_pos, - cmd_linear_speed=backend_params.linear_speed, - cmd_linear_acceleration=backend_params.linear_acceleration, - cmd_rotary_speed=backend_params.rotary_speed, - cmd_rotary_acceleration=backend_params.rotary_acceleration, - ) + await self.motors_move_joint(cmd_pos=cmd_pos, params=backend_params) async def pick_up_at_joint_position( self, @@ -1333,10 +1301,12 @@ async def very_dangerously_yeet( wrist_inward += 360.0 await self.motors_move_joint( cmd_pos={Axis.WRIST: wrist_inward}, - cmd_linear_speed=_YEET_WINDUP_SPEED, - cmd_linear_acceleration=_YEET_WINDUP_ACC, - cmd_rotary_speed=_YEET_WINDUP_SPEED, - cmd_rotary_acceleration=_YEET_WINDUP_ACC, + params=KX2ArmBackend.JointMoveParams( + linear_speed=_YEET_WINDUP_SPEED, + linear_acceleration=_YEET_WINDUP_ACC, + rotary_speed=_YEET_WINDUP_SPEED, + rotary_acceleration=_YEET_WINDUP_ACC, + ), ) joints0 = await self.request_joint_position() @@ -1399,10 +1369,12 @@ async def very_dangerously_yeet( Axis.SHOULDER: pickup_pose[Axis.SHOULDER], Axis.WRIST: pickup_pose[Axis.WRIST], }, - cmd_linear_speed=_YEET_RETURN_SPEED, - cmd_linear_acceleration=_YEET_RETURN_ACC, - cmd_rotary_speed=_YEET_RETURN_SPEED, - cmd_rotary_acceleration=_YEET_RETURN_ACC, + params=KX2ArmBackend.JointMoveParams( + linear_speed=_YEET_RETURN_SPEED, + linear_acceleration=_YEET_RETURN_ACC, + rotary_speed=_YEET_RETURN_SPEED, + rotary_acceleration=_YEET_RETURN_ACC, + ), ) finally: for ax, s in saved_limits.items(): From 6a133cd0b720d52f6642f3cfb6404b2f89ffa75d Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 28 Apr 2026 15:13:45 -0700 Subject: [PATCH 46/93] KX2: drop duplicate trajectory math (_yeet_profile_time, _yeet_wrap_to_range) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Both helpers were near-copies of KX2ArmBackend._profile and KX2ArmBackend._wrap_to_range — same trapezoidal/triangular profile logic, same modulo-range wrap. The yeet versions just exposed a different return shape (t_acc, t_total) than _profile (v, a, t_total). Widen _profile to return (v, a, t_acc, t_total) so the yeet caller gets t_acc directly. As a side effect calculate_move_abs_all_axes also simplifies — the explicit `accel_time[ax] = v[ax] / a[ax]` line drops out (now part of the tuple return), the other two _profile call sites take a `_` for t_acc. Net -27 lines; one definition of profile + wrap, no math drift. --- pylabrobot/paa/kx2/arm_backend.py | 57 ++++++++----------------------- 1 file changed, 15 insertions(+), 42 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index a7b3cee988c..6b5fb3950f2 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -731,30 +731,28 @@ def _wrap_to_range(x: float, lo: float, hi: float) -> float: @staticmethod def _profile(dist: float, v: float, a: float) -> tuple: """ - Returns (v, a, t_total) after applying triangular fallback if needed. - If the distance is short, you cannot reach v before you must decelerate. + Returns (v, a, t_acc, t_total) after applying triangular fallback if + needed. If the distance is short, you cannot reach v before you must + decelerate. """ if dist <= 0: - return v, a, 0.0 + return v, a, 0.0, 0.0 if a <= 0: # degenerate; avoid crash - return max(v, 1e-9), 1e-9, dist / max(v, 1e-9) + return max(v, 1e-9), 1e-9, 0.0, dist / max(v, 1e-9) t_acc = v / a d_acc = 0.5 * a * t_acc * t_acc # triangular? if 2.0 * d_acc > dist: - d_acc = dist / 2.0 - t_acc = math.sqrt(2.0 * d_acc / a) + t_acc = math.sqrt(dist / a) v = a * t_acc - t_total = 2.0 * t_acc - return v, a, t_total + return v, a, t_acc, 2.0 * t_acc d_cruise = dist - 2.0 * d_acc t_cruise = d_cruise / max(v, 1e-9) - t_total = t_cruise + 2.0 * t_acc - return v, a, t_total + return v, a, t_acc, t_cruise + 2.0 * t_acc async def calculate_move_abs_all_axes( self, @@ -881,9 +879,9 @@ async def calculate_move_abs_all_axes( a[ax] = min(chosen_a, axis_max_a) if not skip_ax[ax] and a[ax] > 0: - accel_time[ax] = v[ax] / a[ax] - v[ax], a[ax], total_time[ax] = self._profile(dist[ax], v[ax], a[ax]) - accel_time[ax] = v[ax] / max(a[ax], 1e-9) + v[ax], a[ax], accel_time[ax], total_time[ax] = self._profile( + dist[ax], v[ax], a[ax] + ) else: total_time[ax] = 0.0 accel_time[ax] = 0.0 @@ -911,7 +909,7 @@ async def calculate_move_abs_all_axes( if skip_ax[ax]: total_time[ax] = 0.0 continue - v[ax], a[ax], total_time[ax] = self._profile(dist[ax], v[ax], a[ax]) + v[ax], a[ax], _, total_time[ax] = self._profile(dist[ax], v[ax], a[ax]) # Pick axis with max total_time; scale others to match its total_time lead_time_ax = max(axes, key=lambda ax: total_time[ax]) @@ -932,7 +930,7 @@ async def calculate_move_abs_all_axes( if skip_ax[ax]: total_time[ax] = 0.0 continue - v[ax], a[ax], total_time[ax] = self._profile(dist[ax], v[ax], a[ax]) + v[ax], a[ax], _, total_time[ax] = self._profile(dist[ax], v[ax], a[ax]) move_time = max(total_time[ax] for ax in axes) @@ -1437,31 +1435,6 @@ def __repr__(self) -> str: _YEET_RETURN_ACC = 480.0 -def _yeet_profile_time(dist: float, v: float, a: float) -> tuple: - if dist <= 0 or a <= 0 or v <= 0: - return 0.0, 0.0 - t_acc = v / a - d_acc = 0.5 * a * t_acc * t_acc - if 2.0 * d_acc > dist: - t_acc = math.sqrt(dist / a) - return t_acc, 2.0 * t_acc - t_cruise = (dist - 2.0 * d_acc) / v - return t_acc, 2.0 * t_acc + t_cruise - - -def _yeet_wrap_to_range(x: float, lo: float, hi: float) -> float: - span = hi - lo - if span == 0: - return lo - k = math.trunc(x / span) - x = x - k * span - if x < lo: - x += span - if x == hi: - x -= span - return x - - async def _yeet_build_axis_move( backend: "KX2ArmBackend", ax: Axis, cur: float, target: float, ) -> tuple: @@ -1490,9 +1463,9 @@ async def _yeet_build_axis_move( dist = abs(d) if ax_cfg.unlimited_travel and direction != JointMoveDirection.Normal: - target = _yeet_wrap_to_range(target, ax_cfg.min_travel, ax_cfg.max_travel) + target = KX2ArmBackend._wrap_to_range(target, ax_cfg.min_travel, ax_cfg.max_travel) - t_acc, t_total = _yeet_profile_time(dist, v_phys, a_phys) + _, _, t_acc, t_total = KX2ArmBackend._profile(dist, v_phys, a_phys) move = MotorMoveParam( node_id=int(ax), position=int(round(target * conv)), From d038c470327f84418dcd8eeb519161b697a3fb0a Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 28 Apr 2026 15:27:21 -0700 Subject: [PATCH 47/93] KX2: split GripperConfig out of KX2Config; kinematics takes (c, t) Tooling (gripper length / z-offset / finger side) is user-supplied geometry, not drive-read calibration, so it doesn't belong on KX2Config. Move it to a separate GripperConfig dataclass owned by the backend; fk/ik now take (joints, c, t) and (pose, c, t). Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/arm_backend.py | 24 +++---- pylabrobot/paa/kx2/config.py | 37 +++++++--- pylabrobot/paa/kx2/kinematics.py | 20 +++--- pylabrobot/paa/kx2/kinematics_tests.py | 95 ++++++++++++++------------ 4 files changed, 101 insertions(+), 75 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 6b5fb3950f2..3d8eed7df3e 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -20,6 +20,7 @@ Axis, AxisConfig, GripperFingerSide, + GripperConfig, KX2Config, ServoGripperConfig, ) @@ -70,11 +71,13 @@ def __init__( ) -> None: super().__init__() self.driver = driver - # Tooling offsets are user-supplied; everything else on the config is - # filled in from the drives during setup. - self._gripper_length = float(gripper_length) - self._gripper_z_offset = float(gripper_z_offset) - self._gripper_finger_side: GripperFingerSide = gripper_finger_side + # Tooling is user-supplied and known at construction; KX2Config (drive- + # read calibration) doesn't exist until setup runs. + self._gripper_config = GripperConfig( + length=float(gripper_length), + z_offset=float(gripper_z_offset), + finger_side=gripper_finger_side, + ) self._config: Optional[KX2Config] = None self._freedrive_axes: List[int] = [] @@ -459,9 +462,6 @@ async def _read_config(self) -> KX2Config: wrist_offset=await self.driver.query_float(sh, "UF", 8), elbow_offset=await self.driver.query_float(sh, "UF", 9), elbow_zero_offset=await self.driver.query_float(sh, "UF", 10), - gripper_length=self._gripper_length, - gripper_z_offset=self._gripper_z_offset, - gripper_finger_side=self._gripper_finger_side, axes=axes, base_to_gripper_clearance_z=await self.driver.query_float(sh, "UF", 6), base_to_gripper_clearance_arm=await self.driver.query_float(sh, "UF", 7), @@ -1031,7 +1031,7 @@ async def _cart_to_joints( "ccw" if current[Axis.WRIST] >= 0 else "cw" ) resolved = dataclasses.replace(pose, wrist=ik_wrist) - ik_joints = kinematics.ik(resolved, self._cfg) + ik_joints = kinematics.ik(resolved, self._cfg, self._gripper_config) return kinematics.snap_to_current(ik_joints, current, pose.wrist) # -- capability interface (OrientableGripperArmBackend + HasJoints + CanFreedrive) -- @@ -1080,7 +1080,7 @@ async def request_gripper_location( self, backend_params: Optional[BackendParams] = None ) -> GripperLocation: joints = {Axis(k): v for k, v in (await self.request_joint_position()).items()} - return kinematics.fk(joints, self._cfg) + return kinematics.fk(joints, self._cfg, self._gripper_config) async def open_gripper( self, gripper_width: float, backend_params: Optional[BackendParams] = None @@ -1292,7 +1292,7 @@ async def very_dangerously_yeet( pickup_pose = await self.request_joint_position() # Auto-windup: rotate wrist to the inward angle (opposite of outward). - wrist_inward = 0.0 if cfg.gripper_finger_side == "barcode_reader" else 180.0 + wrist_inward = 0.0 if self._gripper_config.finger_side == "barcode_reader" else 180.0 while wrist_inward - pickup_pose[Axis.WRIST] > 180.0: wrist_inward -= 360.0 while wrist_inward - pickup_pose[Axis.WRIST] < -180.0: @@ -1310,7 +1310,7 @@ async def very_dangerously_yeet( joints0 = await self.request_joint_position() # Outward wrist = kinematic target (180° barcode_reader, 0° proximity). - wrist_outward = 180.0 if cfg.gripper_finger_side == "barcode_reader" else 0.0 + wrist_outward = 180.0 if self._gripper_config.finger_side == "barcode_reader" else 0.0 while wrist_outward - joints0[Axis.WRIST] > 180.0: wrist_outward -= 360.0 while wrist_outward - joints0[Axis.WRIST] < -180.0: diff --git a/pylabrobot/paa/kx2/config.py b/pylabrobot/paa/kx2/config.py index 4ebd453262e..c0e22fbcd9c 100644 --- a/pylabrobot/paa/kx2/config.py +++ b/pylabrobot/paa/kx2/config.py @@ -67,6 +67,29 @@ class AxisConfig: outputs: Dict[int, str] +@dataclass +class GripperConfig: + """User-supplied gripper geometry — known at construction, never read + from the drives. Lives on :class:`KX2ArmBackend` + (``self._gripper_config``) and is passed into kinematics alongside + :class:`KX2Config`. + + Attributes: + length: Distance from the wrist axis to the gripper clamp point, in + mm. Sign tracks which finger is the radial "front" via + :attr:`finger_side`. + z_offset: Vertical offset from the wrist plate to the clamp point, + in mm. Positive = clamp sits below the wrist plate. + finger_side: Which finger is treated as the radial "front" by IK/FK. + The fingers are physically symmetric, so flipping side just + negates the gripper-length offset in the wrist frame. + """ + + length: float = 0.0 + z_offset: float = 0.0 + finger_side: GripperFingerSide = "barcode_reader" + + @dataclass class ServoGripperConfig: """Servo gripper params (axis 6 UF6..UF17). Only present when the @@ -88,21 +111,15 @@ class ServoGripperConfig: @dataclass class KX2Config: + """Drive-read calibration. Strictly contents pulled off the bus at + setup; tooling (gripper geometry) lives separately on + :class:`GripperConfig` and is owned by the backend.""" + # Geometry (read from the shoulder drive's UF8/UF9/UF10). wrist_offset: float elbow_offset: float elbow_zero_offset: float - # Tooling — supplied at backend construction, copied here so kinematics - # has a single source of truth. - gripper_length: float - gripper_z_offset: float - - # Which finger is treated as the radial "front" by IK/FK. The fingers are - # physically symmetric, so flipping side just negates the gripper_length - # offset in the wrist frame. Default matches the legacy convention. - gripper_finger_side: GripperFingerSide - # Per-axis params keyed by drive node-id (= Axis value). Iterating # `axes` (or `axes.keys()`) gives the axes present on this arm. axes: Dict[int, AxisConfig] diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index deef45c5703..2d535f3bf5f 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -27,7 +27,7 @@ from typing import Dict, Literal, Optional from pylabrobot.capabilities.arms.standard import GripperLocation -from pylabrobot.paa.kx2.config import Axis, KX2Config +from pylabrobot.paa.kx2.config import Axis, GripperConfig, KX2Config from pylabrobot.resources import Coordinate, Rotation @@ -53,12 +53,13 @@ class IKError(ValueError): """Target pose is unreachable (for now: non-Z rotation requested).""" -def fk(joints: Dict[Axis, float], c: KX2Config) -> KX2GripperLocation: +def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperConfig) -> KX2GripperLocation: """Forward kinematics. Args: joints: {Axis.SHOULDER: deg, Axis.Z: mm, Axis.ELBOW: mm, Axis.WRIST: deg}. - c: arm configuration. + c: arm configuration (drive-read calibration). + t: gripper tooling (user-supplied geometry). Returns: KX2GripperLocation with the gripper clamp point and a wrist sign derived from the joint configuration (J4 ≥ 0 → "ccw", else "cw"). @@ -81,10 +82,10 @@ def fk(joints: Dict[Axis, float], c: KX2Config) -> KX2GripperLocation: # so callers observe the gripper clamp point symmetric with what they # pass into ik. Sign tracks which finger is the radial "front". yaw = radians(yaw_deg) - gl = c.gripper_length if c.gripper_finger_side == "barcode_reader" else -c.gripper_length + gl = t.length if t.finger_side == "barcode_reader" else -t.length gripper_x = wrist_x + gl * sin(yaw) gripper_y = wrist_y - gl * cos(yaw) - gripper_z = wrist_z - c.gripper_z_offset + gripper_z = wrist_z - t.z_offset return KX2GripperLocation( location=Coordinate(x=gripper_x, y=gripper_y, z=gripper_z), @@ -93,14 +94,15 @@ def fk(joints: Dict[Axis, float], c: KX2Config) -> KX2GripperLocation: ) -def ik(pose: KX2GripperLocation, c: KX2Config) -> Dict[Axis, float]: +def ik(pose: KX2GripperLocation, c: KX2Config, t: GripperConfig) -> Dict[Axis, float]: """Inverse kinematics. Args: pose: KX2GripperLocation. Requires pose.wrist to be "cw" or "ccw" — "closest" semantics live in the backend wrapper (fill None with the current joint's sign, then call `snap_to_current` after). - c: arm configuration. + c: arm configuration (drive-read calibration). + t: gripper tooling (user-supplied geometry). Returns: joints dict {Axis.SHOULDER: deg, Axis.Z: mm, Axis.ELBOW: mm, Axis.WRIST: deg}. J4 is in (-360°, 0°] when wrist="cw" and [0°, 360°) when wrist="ccw" @@ -125,10 +127,10 @@ def ik(pose: KX2GripperLocation, c: KX2Config) -> Dict[Axis, float]: # the gripper z offset downward. Sign tracks which finger is the radial # "front". yaw = radians(pose.rotation.z) - gl = c.gripper_length if c.gripper_finger_side == "barcode_reader" else -c.gripper_length + gl = t.length if t.finger_side == "barcode_reader" else -t.length x = pose.location.x - gl * sin(yaw) y = pose.location.y + gl * cos(yaw) - wrist_z = pose.location.z + c.gripper_z_offset + wrist_z = pose.location.z + t.z_offset shoulder = -degrees(atan2(x, y)) if abs(shoulder + 180.0) < c.eps: diff --git a/pylabrobot/paa/kx2/kinematics_tests.py b/pylabrobot/paa/kx2/kinematics_tests.py index 419aa999fb8..54c2ed62146 100644 --- a/pylabrobot/paa/kx2/kinematics_tests.py +++ b/pylabrobot/paa/kx2/kinematics_tests.py @@ -2,7 +2,7 @@ import unittest from pylabrobot.paa.kx2 import kinematics -from pylabrobot.paa.kx2.config import Axis, AxisConfig, KX2Config +from pylabrobot.paa.kx2.config import Axis, AxisConfig, GripperConfig, KX2Config from pylabrobot.paa.kx2.driver import JointMoveDirection from pylabrobot.paa.kx2.kinematics import IKError, KX2GripperLocation from pylabrobot.resources import Coordinate, Rotation @@ -28,17 +28,11 @@ def _config( wrist_offset: float = 10.0, elbow_offset: float = 20.0, elbow_zero_offset: float = 5.0, - gripper_length: float = 15.0, - gripper_z_offset: float = 3.0, - gripper_finger_side: str = "barcode_reader", ) -> KX2Config: return KX2Config( wrist_offset=wrist_offset, elbow_offset=elbow_offset, elbow_zero_offset=elbow_zero_offset, - gripper_length=gripper_length, - gripper_z_offset=gripper_z_offset, - gripper_finger_side=gripper_finger_side, # type: ignore[arg-type] axes={a: _axis() for a in (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST)}, base_to_gripper_clearance_z=0.0, base_to_gripper_clearance_arm=0.0, @@ -54,13 +48,14 @@ def _approx(a: float, b: float, eps: float = 1e-9) -> bool: class FKIKRoundTrip(unittest.TestCase): def test_roundtrip_ccw(self): c = _config() + g = GripperConfig(length=15.0, z_offset=3.0) pose = KX2GripperLocation( location=Coordinate(x=100, y=200, z=50), rotation=Rotation(z=30), wrist="ccw", ) - joints = kinematics.ik(pose, c) - back = kinematics.fk(joints, c) + joints = kinematics.ik(pose, c, g) + back = kinematics.fk(joints, c, g) self.assertAlmostEqual(back.location.x, pose.location.x, places=9) self.assertAlmostEqual(back.location.y, pose.location.y, places=9) self.assertAlmostEqual(back.location.z, pose.location.z, places=9) @@ -69,27 +64,28 @@ def test_roundtrip_ccw(self): def test_roundtrip_cw_yields_same_world_pose(self): """cw and ccw produce J4 360° apart but represent the same physical pose.""" c = _config() + g = GripperConfig(length=15.0, z_offset=3.0) pose = KX2GripperLocation( location=Coordinate(x=100, y=200, z=50), rotation=Rotation(z=30), wrist="cw" ) - j_cw = kinematics.ik(pose, c) - j_ccw = kinematics.ik(KX2GripperLocation(**{**pose.__dict__, "wrist": "ccw"}), c) + j_cw = kinematics.ik(pose, c, g) + j_ccw = kinematics.ik(KX2GripperLocation(**{**pose.__dict__, "wrist": "ccw"}), c, g) self.assertAlmostEqual(j_ccw[Axis.WRIST] - j_cw[Axis.WRIST], 360.0, places=9) # FK on either should land at the original pose. for j in (j_cw, j_ccw): - back = kinematics.fk(j, c) + back = kinematics.fk(j, c, g) self.assertAlmostEqual(back.location.x, pose.location.x, places=9) self.assertAlmostEqual(back.location.y, pose.location.y, places=9) self.assertAlmostEqual(back.rotation.z, pose.rotation.z, places=9) def test_roundtrip_at_origin_yaw_zero(self): """Sanity: a pose at (0, R, Z, 0°) lands shoulder=0°.""" - c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0, gripper_length=0, - gripper_z_offset=0) + c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0) + g = GripperConfig() pose = KX2GripperLocation( location=Coordinate(x=0, y=300, z=10), rotation=Rotation(z=0), wrist="ccw" ) - joints = kinematics.ik(pose, c) + joints = kinematics.ik(pose, c, g) self.assertAlmostEqual(joints[Axis.SHOULDER], 0.0, places=9) self.assertAlmostEqual(joints[Axis.ELBOW], 300.0, places=9) self.assertAlmostEqual(joints[Axis.Z], 10.0, places=9) @@ -99,18 +95,20 @@ def test_roundtrip_at_origin_yaw_zero(self): class IKWristSign(unittest.TestCase): def test_cw_yields_non_positive_wrist(self): c = _config() + g = GripperConfig() pose = KX2GripperLocation( location=Coordinate(x=50, y=300, z=20), rotation=Rotation(z=45), wrist="cw" ) - joints = kinematics.ik(pose, c) + joints = kinematics.ik(pose, c, g) self.assertLessEqual(joints[Axis.WRIST], c.eps) def test_ccw_yields_non_negative_wrist(self): c = _config() + g = GripperConfig() pose = KX2GripperLocation( location=Coordinate(x=50, y=300, z=20), rotation=Rotation(z=45), wrist="ccw" ) - joints = kinematics.ik(pose, c) + joints = kinematics.ik(pose, c, g) self.assertGreaterEqual(joints[Axis.WRIST], -c.eps) def test_wrist_near_zero_satisfies_both(self): @@ -125,62 +123,68 @@ def test_wrist_near_zero_satisfies_both(self): wrist="ccw", ) # Use zero gripper offsets so location maps directly to wrist axis. - c0 = _config(gripper_length=0, gripper_z_offset=0) - joints = kinematics.ik(pose, c0) + c0 = _config() + g0 = GripperConfig() + joints = kinematics.ik(pose, c0, g0) self.assertAlmostEqual(joints[Axis.WRIST], 0.0, places=6) pose_cw = KX2GripperLocation(**{**pose.__dict__, "wrist": "cw"}) - joints_cw = kinematics.ik(pose_cw, c0) + joints_cw = kinematics.ik(pose_cw, c0, g0) self.assertAlmostEqual(joints_cw[Axis.WRIST], 0.0, places=6) class IKErrors(unittest.TestCase): def test_none_wrist_raises_valueerror(self): c = _config() + g = GripperConfig() pose = KX2GripperLocation( location=Coordinate(x=0, y=100, z=0), rotation=Rotation(z=0), wrist=None ) with self.assertRaises(ValueError): - kinematics.ik(pose, c) + kinematics.ik(pose, c, g) def test_invalid_wrist_string_raises_valueerror(self): c = _config() + g = GripperConfig() pose = KX2GripperLocation( location=Coordinate(x=0, y=100, z=0), rotation=Rotation(z=0), wrist="up", # type: ignore ) with self.assertRaises(ValueError): - kinematics.ik(pose, c) + kinematics.ik(pose, c, g) def test_x_rotation_raises_ikerror(self): c = _config() + g = GripperConfig() pose = KX2GripperLocation( location=Coordinate(x=0, y=100, z=0), rotation=Rotation(x=10, z=0), wrist="ccw", ) with self.assertRaises(IKError): - kinematics.ik(pose, c) + kinematics.ik(pose, c, g) def test_y_rotation_raises_ikerror(self): c = _config() + g = GripperConfig() pose = KX2GripperLocation( location=Coordinate(x=0, y=100, z=0), rotation=Rotation(y=10, z=0), wrist="ccw", ) with self.assertRaises(IKError): - kinematics.ik(pose, c) + kinematics.ik(pose, c, g) class FKResult(unittest.TestCase): def test_fk_sets_wrist_field_from_j4_sign(self): c = _config() + g = GripperConfig() j_pos = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 100.0, Axis.WRIST: 45.0} - self.assertEqual(kinematics.fk(j_pos, c).wrist, "ccw") + self.assertEqual(kinematics.fk(j_pos, c, g).wrist, "ccw") j_neg = {**j_pos, Axis.WRIST: -45.0} - self.assertEqual(kinematics.fk(j_neg, c).wrist, "cw") + self.assertEqual(kinematics.fk(j_neg, c, g).wrist, "cw") j_zero = {**j_pos, Axis.WRIST: 0.0} - self.assertEqual(kinematics.fk(j_zero, c).wrist, "ccw") # 0 ≥ 0 -> ccw + self.assertEqual(kinematics.fk(j_zero, c, g).wrist, "ccw") # 0 ≥ 0 -> ccw class SnapToCurrent(unittest.TestCase): @@ -240,11 +244,12 @@ def test_no_shift_when_already_close(self): class GripperFingerSide(unittest.TestCase): def test_proximity_side_negates_gripper_offset(self): """Same joints, opposite finger side -> clamp point reflected through wrist axis.""" - c_bc = _config(gripper_finger_side="barcode_reader") - c_pr = _config(gripper_finger_side="proximity_sensor") + c = _config() + g_bc = GripperConfig(length=15.0, z_offset=3.0, finger_side="barcode_reader") + g_pr = GripperConfig(length=15.0, z_offset=3.0, finger_side="proximity_sensor") j = {Axis.SHOULDER: 30.0, Axis.Z: 50.0, Axis.ELBOW: 100.0, Axis.WRIST: 15.0} - p_bc = kinematics.fk(j, c_bc) - p_pr = kinematics.fk(j, c_pr) + p_bc = kinematics.fk(j, c, g_bc) + p_pr = kinematics.fk(j, c, g_pr) # Wrist position is the midpoint between the two clamp points. wrist_x = (p_bc.location.x + p_pr.location.x) / 2 @@ -252,24 +257,25 @@ def test_proximity_side_negates_gripper_offset(self): yaw_deg = j[Axis.WRIST] + j[Axis.SHOULDER] yaw = math.radians(yaw_deg) self.assertAlmostEqual( - p_bc.location.x - wrist_x, c_bc.gripper_length * math.sin(yaw), delta=1e-5 + p_bc.location.x - wrist_x, g_bc.length * math.sin(yaw), delta=1e-5 ) self.assertAlmostEqual( - p_bc.location.y - wrist_y, -c_bc.gripper_length * math.cos(yaw), delta=1e-5 + p_bc.location.y - wrist_y, -g_bc.length * math.cos(yaw), delta=1e-5 ) # z and yaw are independent of finger side. self.assertAlmostEqual(p_bc.location.z, p_pr.location.z, places=9) self.assertAlmostEqual(p_bc.rotation.z, p_pr.rotation.z, places=9) def test_proximity_roundtrip(self): - c = _config(gripper_finger_side="proximity_sensor") + c = _config() + g = GripperConfig(length=15.0, z_offset=3.0, finger_side="proximity_sensor") pose = KX2GripperLocation( location=Coordinate(x=100, y=200, z=50), rotation=Rotation(z=30), wrist="ccw", ) - joints = kinematics.ik(pose, c) - back = kinematics.fk(joints, c) + joints = kinematics.ik(pose, c, g) + back = kinematics.fk(joints, c, g) self.assertAlmostEqual(back.location.x, pose.location.x, places=9) self.assertAlmostEqual(back.location.y, pose.location.y, places=9) self.assertAlmostEqual(back.location.z, pose.location.z, places=9) @@ -283,24 +289,25 @@ def test_ik_elbow_differs_by_twice_gripper_length(self): pose = KX2GripperLocation( location=Coordinate(x=0, y=300, z=0), rotation=Rotation(z=0), wrist="ccw" ) - c_bc = _config(gripper_finger_side="barcode_reader") - c_pr = _config(gripper_finger_side="proximity_sensor") - j_bc = kinematics.ik(pose, c_bc) - j_pr = kinematics.ik(pose, c_pr) + c = _config() + g_bc = GripperConfig(length=15.0, z_offset=3.0, finger_side="barcode_reader") + g_pr = GripperConfig(length=15.0, z_offset=3.0, finger_side="proximity_sensor") + j_bc = kinematics.ik(pose, c, g_bc) + j_pr = kinematics.ik(pose, c, g_pr) self.assertAlmostEqual(j_bc[Axis.SHOULDER], 0.0, places=9) self.assertAlmostEqual(j_pr[Axis.SHOULDER], 0.0, places=9) - self.assertAlmostEqual(j_bc[Axis.ELBOW] - j_pr[Axis.ELBOW], 2 * c_bc.gripper_length, places=9) + self.assertAlmostEqual(j_bc[Axis.ELBOW] - j_pr[Axis.ELBOW], 2 * g_bc.length, places=9) class ShoulderSnapAt180(unittest.TestCase): def test_negative_180_snaps_to_positive(self): """A pose pointing exactly along -y has shoulder = ±180; we snap to +180.""" - c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0, gripper_length=0, - gripper_z_offset=0) + c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0) + g = GripperConfig() pose = KX2GripperLocation( location=Coordinate(x=0, y=-100, z=0), rotation=Rotation(z=180), wrist="ccw" ) - joints = kinematics.ik(pose, c) + joints = kinematics.ik(pose, c, g) self.assertAlmostEqual(joints[Axis.SHOULDER], 180.0, places=9) From 45968dbcc9d735aa0f2f5e6ccf2a624447d88ec5 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 28 Apr 2026 15:42:36 -0700 Subject: [PATCH 48/93] KX2Config.axes: Dict[Axis, AxisConfig]; drop int<->Axis round-trips Mirrors the kinematics dict-key change. _read_config converts the driver's int node-id once at the boundary; motion_limits() drops the Axis(k) round-trip on the read side. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/arm_backend.py | 6 +++--- pylabrobot/paa/kx2/config.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 3d8eed7df3e..2caa6558f4f 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -453,9 +453,9 @@ async def _read_config(self) -> KX2Config: if has_rail: warnings.warn("Rails has not been tested for KX2 robots.") - axes: Dict[int, AxisConfig] = {} + axes: Dict[Axis, AxisConfig] = {} for nid in nodes: - axes[nid] = await self._read_axis_config(nid) + axes[Axis(nid)] = await self._read_axis_config(nid) sh = int(Axis.SHOULDER) return KX2Config( @@ -1203,7 +1203,7 @@ def motion_limits(self) -> "_MotionLimits": in a terminal. """ return _MotionLimits( - {Axis(k): (cfg.max_vel, cfg.max_accel) for k, cfg in self._cfg.axes.items()}, + {k: (cfg.max_vel, cfg.max_accel) for k, cfg in self._cfg.axes.items()}, ) async def start_freedrive_mode( diff --git a/pylabrobot/paa/kx2/config.py b/pylabrobot/paa/kx2/config.py index c0e22fbcd9c..f8dd9c194e3 100644 --- a/pylabrobot/paa/kx2/config.py +++ b/pylabrobot/paa/kx2/config.py @@ -120,9 +120,9 @@ class KX2Config: elbow_offset: float elbow_zero_offset: float - # Per-axis params keyed by drive node-id (= Axis value). Iterating - # `axes` (or `axes.keys()`) gives the axes present on this arm. - axes: Dict[int, AxisConfig] + # Per-axis params keyed by Axis. Iterating `axes` (or `axes.keys()`) + # gives the axes present on this arm. + axes: Dict[Axis, AxisConfig] # Robot-level clearance limits (shoulder UF6/UF7). base_to_gripper_clearance_z: float From bf6be163de3eee048e56300141c2f4df18a08201 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 28 Apr 2026 15:54:31 -0700 Subject: [PATCH 49/93] KX2: hide internal driver types; loop property; topology guard in driver - Underscore-prefix MotorMoveParam/MotorsMovePlan/JointMoveDirection/ ElmoObjectDataType/InputLogic so they don't read as supported API. - KX2Driver gains a `loop` property that asserts non-None; drops the dead `asyncio.get_event_loop()` fallback in send_bi. - Move the has_rail / has_servo_gripper NotImplementedError from KX2.__init__ down into KX2Driver.__init__ so direct backend users hit the same guard. - arm_backend: drop `from dataclasses import dataclass`; use `@dataclasses.dataclass` consistently (already imported the module). Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/arm_backend.py | 89 +++++++++++++------------- pylabrobot/paa/kx2/config.py | 4 +- pylabrobot/paa/kx2/driver.py | 74 +++++++++++++-------- pylabrobot/paa/kx2/kinematics_tests.py | 4 +- pylabrobot/paa/kx2/kx2.py | 16 ----- 5 files changed, 96 insertions(+), 91 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 2caa6558f4f..6c9ca2b3164 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -4,7 +4,6 @@ import math import time import warnings -from dataclasses import dataclass from enum import IntEnum from typing import Dict, List, Optional, Union @@ -26,12 +25,12 @@ ) from pylabrobot.paa.kx2.driver import ( CanError, - ElmoObjectDataType, - InputLogic, - JointMoveDirection, + _ElmoObjectDataType, + _InputLogic, + _JointMoveDirection, KX2Driver, - MotorMoveParam, - MotorsMovePlan, + _MotorMoveParam, + _MotorsMovePlan, ) from pylabrobot.resources import Coordinate, Rotation @@ -246,15 +245,15 @@ async def home_motor( await self.driver.motor_enable(node_id=nid, state=True, use_ds402=Axis(nid).is_motion) await self.motors_move_absolute_execute( - plan=MotorsMovePlan( + plan=_MotorsMovePlan( moves=[ - MotorMoveParam( + _MotorMoveParam( node_id=nid, position=hs_offset, velocity=offset_vel, acceleration=offset_acc, relative=False, - direction=JointMoveDirection.ShortestWay, + direction=_JointMoveDirection.ShortestWay, ) ], ) @@ -264,15 +263,15 @@ async def home_motor( await self._motor_index_search(nid, abs(srch_vel), srch_acc, is_positive, timeout) await self.motors_move_absolute_execute( - plan=MotorsMovePlan( + plan=_MotorsMovePlan( moves=[ - MotorMoveParam( + _MotorMoveParam( node_id=nid, position=ind_offset, velocity=offset_vel, acceleration=offset_acc, relative=False, - direction=JointMoveDirection.ShortestWay, + direction=_JointMoveDirection.ShortestWay, ) ] ) @@ -491,13 +490,13 @@ async def _read_axis_config(self, nid: int) -> AxisConfig: vh3 = await self.driver.query_int(nid, "VH", 3) vl3 = await self.driver.query_int(nid, "VL", 3) - joint_move_direction = JointMoveDirection.Normal + joint_move_direction = _JointMoveDirection.Normal if (xm1 == 0 and xm2 == 0) or (xm1 <= vl3 and xm2 >= vh3): unlimited_travel = False elif xm1 > vl3 and xm2 < vh3: unlimited_travel = True if Axis(nid).is_motion: - joint_move_direction = JointMoveDirection.ShortestWay + joint_move_direction = _JointMoveDirection.ShortestWay else: raise CanError( f"Invalid travel limits or modulo settings for axis {nid}: " @@ -674,7 +673,7 @@ async def find_z_with_proximity_sensor( f"clear the gripper or raise z_start before searching" ) await self.driver.configure_input_logic( - int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, InputLogic.StopForward, + int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, _InputLogic.StopForward, ) move_task = asyncio.create_task( self.move_to_joint_position({Axis.Z: z0 - max_descent}, backend_params=move_params) @@ -704,7 +703,7 @@ async def find_z_with_proximity_sensor( logger.warning("find_z: motor_stop failed: %s", e) try: await self.driver.configure_input_logic( - int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, InputLogic.GeneralPurpose, + int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose, ) except Exception as e: logger.warning("find_z: IL restore failed: %s", e) @@ -758,7 +757,7 @@ async def calculate_move_abs_all_axes( self, cmd_pos: Dict[Axis, float], params: Optional["KX2ArmBackend.JointMoveParams"] = None, - ) -> Optional[MotorsMovePlan]: + ) -> Optional[_MotorsMovePlan]: if params is None: params = KX2ArmBackend.JointMoveParams() target = cmd_pos.copy() @@ -830,7 +829,7 @@ async def calculate_move_abs_all_axes( for ax in axes: if ( self._cfg.axes[ax].unlimited_travel - and self._cfg.axes[ax].joint_move_direction != JointMoveDirection.Normal + and self._cfg.axes[ax].joint_move_direction != _JointMoveDirection.Normal ): target[ax] = self._wrap_to_range(target[ax], self._cfg.axes[ax].min_travel, self._cfg.axes[ax].max_travel) @@ -847,11 +846,11 @@ async def calculate_move_abs_all_axes( span = self._cfg.axes[ax].max_travel - self._cfg.axes[ax].min_travel dir_ = self._cfg.axes[ax].joint_move_direction - if dir_ == JointMoveDirection.Clockwise and d > 0.01: + if dir_ == _JointMoveDirection.Clockwise and d > 0.01: d -= span - elif dir_ == JointMoveDirection.Counterclockwise and d < -0.01: + elif dir_ == _JointMoveDirection.Counterclockwise and d < -0.01: d += span - elif dir_ == JointMoveDirection.ShortestWay: + elif dir_ == _JointMoveDirection.ShortestWay: if d > 180.0: d -= span elif d < -180.0: @@ -946,9 +945,9 @@ async def calculate_move_abs_all_axes( enc_vel[ax] = max(v[ax] * abs(conv), 1.0) enc_accel[ax] = max(a[ax] * abs(conv), 1.0) - return MotorsMovePlan( + return _MotorsMovePlan( moves=[ - MotorMoveParam( + _MotorMoveParam( node_id=int(ax), position=int(round(enc_pos[ax])), velocity=int(round(enc_vel[ax])), @@ -973,7 +972,7 @@ async def motors_move_joint( await self.motors_move_absolute_execute(plan) - async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: + async def motors_move_absolute_execute(self, plan: _MotorsMovePlan) -> None: await self.driver.pvt_select_mode(False) if logger.isEnabledFor(logging.DEBUG): @@ -992,20 +991,20 @@ async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: await self.driver.motor_set_move_direction(nid, move.direction) # 0x607A = Target Position (24698 decimal) await self.driver.can_sdo_download_elmo_object( - nid, 24698, 0, int(move.position), ElmoObjectDataType.INTEGER32, + nid, 24698, 0, int(move.position), _ElmoObjectDataType.INTEGER32, ) # 0x6081 = Profile Velocity (24705 decimal) await self.driver.can_sdo_download_elmo_object( - nid, 24705, 0, int(move.velocity), ElmoObjectDataType.UNSIGNED32, + nid, 24705, 0, int(move.velocity), _ElmoObjectDataType.UNSIGNED32, ) acc = max(int(move.acceleration), 100) # 0x6083 = Profile Acceleration (24707 decimal) await self.driver.can_sdo_download_elmo_object( - nid, 24707, 0, acc, ElmoObjectDataType.UNSIGNED32 + nid, 24707, 0, acc, _ElmoObjectDataType.UNSIGNED32 ) # 0x6084 = Profile Deceleration (24708 decimal) await self.driver.can_sdo_download_elmo_object( - nid, 24708, 0, acc, ElmoObjectDataType.UNSIGNED32 + nid, 24708, 0, acc, _ElmoObjectDataType.UNSIGNED32 ) node_ids = [move.node_id for move in plan.moves] @@ -1036,7 +1035,7 @@ async def _cart_to_joints( # -- capability interface (OrientableGripperArmBackend + HasJoints + CanFreedrive) -- - @dataclass + @dataclasses.dataclass class CartesianMoveParams(BackendParams): """Per-axis speed/acceleration limits in physical units. @@ -1050,7 +1049,7 @@ class CartesianMoveParams(BackendParams): rotary_speed: Optional[float] = None # deg/s rotary_acceleration: Optional[float] = None # deg/s^2 - @dataclass + @dataclasses.dataclass class JointMoveParams(BackendParams): """Per-axis speed/acceleration limits in physical units. Same shape as `CartesianMoveParams` — see its docstring.""" @@ -1059,7 +1058,7 @@ class JointMoveParams(BackendParams): rotary_speed: Optional[float] = None rotary_acceleration: Optional[float] = None - @dataclass + @dataclasses.dataclass class GripParams(BackendParams): check_plate_gripped: bool = True @@ -1323,8 +1322,8 @@ async def very_dangerously_yeet( wr_move, wr_t_acc, wr_t_total, _ = await _yeet_build_axis_move( self, Axis.WRIST, joints0[Axis.WRIST], wrist_outward, ) - sh_plan = MotorsMovePlan(moves=[sh_move], move_time=sh_t_total) - wr_plan = MotorsMovePlan(moves=[wr_move], move_time=wr_t_total) + sh_plan = _MotorsMovePlan(moves=[sh_move], move_time=sh_t_total) + wr_plan = _MotorsMovePlan(moves=[wr_move], move_time=wr_t_total) # Release fires inside shoulder cruise. Wrist trigger is delayed so its # accel ramp finishes at release (peak ω at the gripper offset). @@ -1335,7 +1334,7 @@ async def very_dangerously_yeet( sg = int(Axis.SERVO_GRIPPER) sg_cfg = cfg.axes[Axis.SERVO_GRIPPER] open_pos = min(_YEET_OPEN_POSITION, sg_cfg.max_travel - _YEET_OPEN_SAFETY_MARGIN) - gripper_plan = MotorsMovePlan(moves=[MotorMoveParam( + gripper_plan = _MotorsMovePlan(moves=[_MotorMoveParam( node_id=sg, position=int(round(open_pos * sg_cfg.motor_conversion_factor)), velocity=int(round(sg_cfg.max_vel * abs(sg_cfg.motor_conversion_factor))), @@ -1438,7 +1437,7 @@ def __repr__(self) -> str: async def _yeet_build_axis_move( backend: "KX2ArmBackend", ax: Axis, cur: float, target: float, ) -> tuple: - """Per-axis MotorMoveParam at firmware velocity limit (VH[2]/1.01). + """Per-axis _MotorMoveParam at firmware velocity limit (VH[2]/1.01). Returns (move, t_acc, t_total, v_phys).""" cfg = backend._cfg ax_cfg = cfg.axes[ax] @@ -1451,22 +1450,22 @@ async def _yeet_build_axis_move( d = target - cur span = ax_cfg.max_travel - ax_cfg.min_travel if span > 0 and ax_cfg.unlimited_travel: - if direction == JointMoveDirection.Clockwise and d > 0.01: + if direction == _JointMoveDirection.Clockwise and d > 0.01: d -= span - elif direction == JointMoveDirection.Counterclockwise and d < -0.01: + elif direction == _JointMoveDirection.Counterclockwise and d < -0.01: d += span - elif direction == JointMoveDirection.ShortestWay: + elif direction == _JointMoveDirection.ShortestWay: if d > 180.0: d -= span elif d < -180.0: d += span dist = abs(d) - if ax_cfg.unlimited_travel and direction != JointMoveDirection.Normal: + if ax_cfg.unlimited_travel and direction != _JointMoveDirection.Normal: target = KX2ArmBackend._wrap_to_range(target, ax_cfg.min_travel, ax_cfg.max_travel) _, _, t_acc, t_total = KX2ArmBackend._profile(dist, v_phys, a_phys) - move = MotorMoveParam( + move = _MotorMoveParam( node_id=int(ax), position=int(round(target * conv)), velocity=max(int(round(v_phys * abs(conv))), 1), @@ -1476,7 +1475,7 @@ async def _yeet_build_axis_move( return move, t_acc, t_total, v_phys -async def _yeet_arm_plan(driver: KX2Driver, plan: MotorsMovePlan) -> None: +async def _yeet_arm_plan(driver: KX2Driver, plan: _MotorsMovePlan) -> None: """Pre-load a plan onto the drives without triggering it. Splits SDO setup latency from the move start so the timer can be accurate.""" await driver.pvt_select_mode(False) @@ -1484,15 +1483,15 @@ async def _yeet_arm_plan(driver: KX2Driver, plan: MotorsMovePlan) -> None: nid = int(move.node_id) await driver.motor_set_move_direction(nid, move.direction) await driver.can_sdo_download_elmo_object( - nid, 24698, 0, int(move.position), ElmoObjectDataType.INTEGER32, + nid, 24698, 0, int(move.position), _ElmoObjectDataType.INTEGER32, ) await driver.can_sdo_download_elmo_object( - nid, 24705, 0, int(move.velocity), ElmoObjectDataType.UNSIGNED32, + nid, 24705, 0, int(move.velocity), _ElmoObjectDataType.UNSIGNED32, ) acc = max(int(move.acceleration), 100) await driver.can_sdo_download_elmo_object( - nid, 24707, 0, acc, ElmoObjectDataType.UNSIGNED32, + nid, 24707, 0, acc, _ElmoObjectDataType.UNSIGNED32, ) await driver.can_sdo_download_elmo_object( - nid, 24708, 0, acc, ElmoObjectDataType.UNSIGNED32, + nid, 24708, 0, acc, _ElmoObjectDataType.UNSIGNED32, ) diff --git a/pylabrobot/paa/kx2/config.py b/pylabrobot/paa/kx2/config.py index f8dd9c194e3..2155dfd4a25 100644 --- a/pylabrobot/paa/kx2/config.py +++ b/pylabrobot/paa/kx2/config.py @@ -11,7 +11,7 @@ from enum import IntEnum from typing import Dict, Literal, Optional -from pylabrobot.paa.kx2.driver import JointMoveDirection +from pylabrobot.paa.kx2.driver import _JointMoveDirection GripperFingerSide = Literal["barcode_reader", "proximity_sensor"] @@ -57,7 +57,7 @@ class AxisConfig: absolute_encoder: bool max_vel: float max_accel: float - joint_move_direction: JointMoveDirection + joint_move_direction: _JointMoveDirection # I/O pin assignments — channel -> human-readable name ("GripperSensor", # "Buzzer", "AuxPin42", or "" when unassigned). Probed from UI[5..16] but diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index d141cc47d21..f4200ef4a96 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -117,7 +117,7 @@ class TPDOMappedObject(IntEnum): DigitalInputs = 0x60FD0020 -class ElmoObjectDataType(IntEnum): +class _ElmoObjectDataType(IntEnum): UNSIGNED8 = 0 UNSIGNED16 = 1 UNSIGNED32 = 2 @@ -133,7 +133,7 @@ class CanError(Exception): """Custom exception for CAN motor errors.""" -class InputLogic(IntEnum): +class _InputLogic(IntEnum): """Elmo SimplIQ IL[N] codes. Even = active-low; odd (value+1) = active-high.""" GeneralPurpose = 0 StopForward = 2 @@ -146,7 +146,7 @@ class InputLogic(IntEnum): AbortMotion = 16 -class JointMoveDirection(IntEnum): +class _JointMoveDirection(IntEnum): """Move-direction hint used by the driver's move primitives. Lives here (not in the backend) because the driver's @@ -162,7 +162,7 @@ class JointMoveDirection(IntEnum): @dataclass -class MotorMoveParam: +class _MotorMoveParam: """One axis of a coordinated move, expressed purely in node-ID terms.""" # CANopen node ID for this axis. Backend passes `int(self.Axis.X)`. @@ -171,12 +171,12 @@ class MotorMoveParam: velocity: int # encoder counts/sec (driver-internal; backend converts from mm/s or deg/s) acceleration: int # encoder counts/sec^2 relative: bool = False - direction: JointMoveDirection = JointMoveDirection.ShortestWay + direction: _JointMoveDirection = _JointMoveDirection.ShortestWay @dataclass -class MotorsMovePlan: - moves: List[MotorMoveParam] = field(default_factory=list) +class _MotorsMovePlan: + moves: List[_MotorMoveParam] = field(default_factory=list) move_time: float = 10.0 @@ -211,6 +211,21 @@ def __init__( channel: Optional[str] = None, bitrate: int = 500000, ) -> None: + # The non-default topologies (rail-mounted KX2, gripper-less KX2) + # have shim code paths in this driver and the backend, but neither + # has been exercised against real hardware. KX2ArmBackend._on_setup + # also calls servo_gripper_initialize unconditionally. Refuse the + # configuration up front rather than letting users hit cryptic + # failures downstream. + if has_rail or not has_servo_gripper: + raise NotImplementedError( + "KX2 has only been tested with the default 4-axis arm + servo " + "gripper topology (has_rail=False, has_servo_gripper=True). " + "Other configurations have shim code paths but the setup / " + "homing layer needs work — see KX2ArmBackend._on_setup and " + "servo_gripper_initialize." + ) + super().__init__() self._interface = interface self._channel = channel @@ -242,6 +257,13 @@ def __init__( self._pvt_mode: bool = False self.EmcyMoveErrorReceived: bool = False + @property + def loop(self) -> asyncio.AbstractEventLoop: + """Event loop captured in setup(). Raises if accessed before setup().""" + if self._loop is None: + raise RuntimeError("KX2Driver event loop not initialized; call setup() first.") + return self._loop + # --- lifecycle ----------------------------------------------------------- async def setup(self, backend_params: Optional[BackendParams] = None) -> None: @@ -302,12 +324,12 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: # Elmo vendor objects: interpolation config for PVT mode. for nid in self.motion_node_ids: - await self.can_sdo_download_elmo_object(nid, 24768, 0, -1, ElmoObjectDataType.INTEGER16) - await self.can_sdo_download_elmo_object(nid, 24772, 2, 16, ElmoObjectDataType.UNSIGNED32) - await self.can_sdo_download_elmo_object(nid, 24772, 3, 0, ElmoObjectDataType.UNSIGNED8) - await self.can_sdo_download_elmo_object(nid, 24772, 5, 8, ElmoObjectDataType.UNSIGNED8) - await self.can_sdo_download_elmo_object(nid, 24770, 2, -3, ElmoObjectDataType.INTEGER8) - await self.can_sdo_download_elmo_object(nid, 24669, 0, 1, ElmoObjectDataType.INTEGER16) + await self.can_sdo_download_elmo_object(nid, 24768, 0, -1, _ElmoObjectDataType.INTEGER16) + await self.can_sdo_download_elmo_object(nid, 24772, 2, 16, _ElmoObjectDataType.UNSIGNED32) + await self.can_sdo_download_elmo_object(nid, 24772, 3, 0, _ElmoObjectDataType.UNSIGNED8) + await self.can_sdo_download_elmo_object(nid, 24772, 5, 8, _ElmoObjectDataType.UNSIGNED8) + await self.can_sdo_download_elmo_object(nid, 24770, 2, -3, _ElmoObjectDataType.INTEGER8) + await self.can_sdo_download_elmo_object(nid, 24669, 0, 1, _ElmoObjectDataType.INTEGER16) # RPDO1 = ControlWord (for DS402 enable), RPDO3 = interpolated target. for nid in self.motion_node_ids: @@ -449,18 +471,18 @@ async def can_sdo_download_elmo_object( elmo_object_int: int, sub_index: int, data: Union[int, float], - data_type: ElmoObjectDataType, + data_type: _ElmoObjectDataType, ) -> None: # Byte width + signedness derived from data_type; float inputs truncate to int. _SDO_ELMO_PACK = { - ElmoObjectDataType.UNSIGNED8: (1, False), - ElmoObjectDataType.UNSIGNED16: (2, False), - ElmoObjectDataType.UNSIGNED32: (4, False), - ElmoObjectDataType.UNSIGNED64: (8, False), - ElmoObjectDataType.INTEGER8: (1, True), - ElmoObjectDataType.INTEGER16: (2, True), - ElmoObjectDataType.INTEGER32: (4, True), - ElmoObjectDataType.INTEGER64: (8, True), + _ElmoObjectDataType.UNSIGNED8: (1, False), + _ElmoObjectDataType.UNSIGNED16: (2, False), + _ElmoObjectDataType.UNSIGNED32: (4, False), + _ElmoObjectDataType.UNSIGNED64: (8, False), + _ElmoObjectDataType.INTEGER8: (1, True), + _ElmoObjectDataType.INTEGER16: (2, True), + _ElmoObjectDataType.INTEGER32: (4, True), + _ElmoObjectDataType.INTEGER64: (8, True), } spec = _SDO_ELMO_PACK.get(data_type) if spec is None: @@ -546,7 +568,7 @@ async def _send_bi( old = self._pending_bi.pop(key, None) if old is not None and not old.done(): old.cancel() - fut = self._loop.create_future() if self._loop else asyncio.get_event_loop().create_future() + fut = self.loop.create_future() self._pending_bi[key] = fut futures.append(fut) @@ -696,13 +718,13 @@ async def motor_get_motion_status(self, node_id: int) -> int: return await self.query_int(node_id, "MS", 0) async def motor_set_move_direction( - self, node_id: int, direction: JointMoveDirection + self, node_id: int, direction: _JointMoveDirection ) -> None: # Elmo modulo mode register: bit0 enables modulo; bits6..7 encode the # direction (0=Normal, 1=CW, 2=CCW, 3=Shortest). Packs to 1 + 64*direction # = 1/65/129/193. val = 1 + 64 * int(direction) - await self.can_sdo_download_elmo_object(node_id, 24818, 0, val, ElmoObjectDataType.UNSIGNED16) + await self.can_sdo_download_elmo_object(node_id, 24818, 0, val, _ElmoObjectDataType.UNSIGNED16) async def motor_check_if_move_done(self, node_id: int) -> bool: ms_val = await self.query_int(node_id, "MS", 0) @@ -970,7 +992,7 @@ async def configure_input_logic( ) -> None: """Set IL[input_num]: drive auto-acts on input edges (e.g. halt motion). - Pass an `InputLogic` member or raw int for `logic`. With `StopForward` the + Pass an `_InputLogic` member or raw int for `logic`. With `StopForward` the drive halts the motor itself the instant the input trips during forward motion — no software in the loop. Skips the write if value already matches; settles 250ms after a real change (Elmo IL needs time to apply). diff --git a/pylabrobot/paa/kx2/kinematics_tests.py b/pylabrobot/paa/kx2/kinematics_tests.py index 54c2ed62146..e6935a8e67c 100644 --- a/pylabrobot/paa/kx2/kinematics_tests.py +++ b/pylabrobot/paa/kx2/kinematics_tests.py @@ -3,7 +3,7 @@ from pylabrobot.paa.kx2 import kinematics from pylabrobot.paa.kx2.config import Axis, AxisConfig, GripperConfig, KX2Config -from pylabrobot.paa.kx2.driver import JointMoveDirection +from pylabrobot.paa.kx2.driver import _JointMoveDirection from pylabrobot.paa.kx2.kinematics import IKError, KX2GripperLocation from pylabrobot.resources import Coordinate, Rotation @@ -17,7 +17,7 @@ def _axis() -> AxisConfig: absolute_encoder=True, max_vel=100.0, max_accel=100.0, - joint_move_direction=JointMoveDirection.Normal, + joint_move_direction=_JointMoveDirection.Normal, digital_inputs={}, analog_inputs={}, outputs={}, diff --git a/pylabrobot/paa/kx2/kx2.py b/pylabrobot/paa/kx2/kx2.py index 937525ed8d3..2c0e9300b70 100644 --- a/pylabrobot/paa/kx2/kx2.py +++ b/pylabrobot/paa/kx2/kx2.py @@ -23,22 +23,6 @@ def __init__( has_rail: bool = False, has_servo_gripper: bool = True, ) -> None: - # The non-default topologies (rail-mounted KX2, gripper-less KX2) - # are accepted by the driver but the backend / capability layer has - # only been exercised against the standard 4-axis arm + servo - # gripper. setup() currently calls servo_gripper_initialize - # unconditionally, so has_servo_gripper=False crashes during init; - # rail support hasn't been validated end-to-end either. Surface that - # as a clear error rather than letting users hit cryptic failures - # downstream. - if has_rail or not has_servo_gripper: - raise NotImplementedError( - "KX2 has only been tested with the default 4-axis arm + servo " - "gripper topology (has_rail=False, has_servo_gripper=True). " - "Other configurations are wired through to the driver but the " - "backend setup path needs work — see KX2ArmBackend._on_setup " - "and servo_gripper_initialize." - ) driver = KX2Driver(has_rail=has_rail, has_servo_gripper=has_servo_gripper) super().__init__(driver=driver) self.driver: KX2Driver = driver From 34e96e440a827d1e099f2f69434857e41247d189 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 28 Apr 2026 16:02:22 -0700 Subject: [PATCH 50/93] KX2 motion API: revert physical units back to vel_pct/accel_pct MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit JointMoveParams / CartesianMoveParams go back to the simpler percentage-of-axis-max API: a single vel_pct + accel_pct pair (default 100) clamped to (0, 100], applied uniformly. find_z_with_proximity_sensor takes vel_pct/accel_pct (defaults 5/5) again. yeet windup and return both run at 25% — return was 100% in the physical-units version but that's louder than it needs to be. motion_limits() and the _MotionLimits pretty-printer stay; the firmware caps in mm/s and deg/s are still useful as introspection of what 100% maps to. Notebook updated to the new (old) API; the markdown blurb now points at vel_pct/accel_pct rather than calling out linear/rotary splits. Co-Authored-By: Claude Opus 4.7 (1M context) --- docs/user_guide/paa/kx2/hello-world.ipynb | 33 +------ pylabrobot/paa/kx2/arm_backend.py | 104 ++++++++-------------- 2 files changed, 39 insertions(+), 98 deletions(-) diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index cc15e8fd725..2bc58850ca1 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -163,11 +163,7 @@ "cell_type": "markdown", "id": "kx2-motion-limits-md", "metadata": {}, - "source": [ - "### Motion limits\n", - "\n", - "`motion_limits()` reads the per-axis maxima cached at setup. Pass values below these into `JointMoveParams` / `CartesianMoveParams`; anything above is silently clamped. Linear axes are mm/s and mm/s^2; rotary in deg/s and deg/s^2." - ] + "source": "### Motion limits\n\n`motion_limits()` reads the per-axis maxima cached at setup. `JointMoveParams` / `CartesianMoveParams` take `vel_pct` and `accel_pct` (both default 100): each axis runs at that fraction of its own max. Linear axes are mm/s and mm/s^2; rotary in deg/s and deg/s^2." }, { "cell_type": "code", @@ -205,15 +201,7 @@ "id": "kx2-cartesian-code", "metadata": {}, "outputs": [], - "source": [ - "from pylabrobot.resources import Coordinate\n", - "\n", - "await kx2.arm.move_to_location(\n", - " location=Coordinate(x=330.124, y=210.552, z=320.0441),\n", - " direction=198,\n", - " backend_params=KX2ArmBackend.CartesianMoveParams(linear_speed=20, linear_acceleration=50, rotary_speed=15, rotary_acceleration=50),\n", - ")" - ] + "source": "from pylabrobot.resources import Coordinate\n\nawait kx2.arm.move_to_location(\n location=Coordinate(x=330.124, y=210.552, z=320.0441),\n direction=198,\n backend_params=KX2ArmBackend.CartesianMoveParams(vel_pct=20, accel_pct=20),\n)" }, { "cell_type": "markdown", @@ -235,20 +223,7 @@ "id": "kx2-joint-code", "metadata": {}, "outputs": [], - "source": [ - "from pylabrobot.paa.kx2 import KX2ArmBackend\n", - "\n", - "position = {\n", - " KX2ArmBackend.Axis.SHOULDER: 0.0,\n", - " KX2ArmBackend.Axis.Z: 150.0,\n", - " KX2ArmBackend.Axis.ELBOW: 90.0,\n", - " KX2ArmBackend.Axis.WRIST: 0.0,\n", - "}\n", - "await kx2.arm.backend.move_to_joint_position(\n", - " position,\n", - " backend_params=KX2ArmBackend.JointMoveParams(linear_speed=20, linear_acceleration=50, rotary_speed=15, rotary_acceleration=50),\n", - ")" - ] + "source": "from pylabrobot.paa.kx2 import KX2ArmBackend\n\nposition = {\n KX2ArmBackend.Axis.SHOULDER: 0.0,\n KX2ArmBackend.Axis.Z: 150.0,\n KX2ArmBackend.Axis.ELBOW: 90.0,\n KX2ArmBackend.Axis.WRIST: 0.0,\n}\nawait kx2.arm.backend.move_to_joint_position(\n position,\n backend_params=KX2ArmBackend.JointMoveParams(vel_pct=20, accel_pct=20),\n)" }, { "cell_type": "markdown", @@ -630,4 +605,4 @@ }, "nbformat": 4, "nbformat_minor": 5 -} +} \ No newline at end of file diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 6c9ca2b3164..ac9e62fa3ca 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -644,22 +644,20 @@ async def find_z_with_proximity_sensor( self, max_descent: float, z_start: Optional[float] = None, - speed: float = 5.0, - acceleration: float = 50.0, + vel_pct: float = 5.0, + accel_pct: float = 5.0, ) -> float: """Descend Z up to `max_descent`; halt when the IR breakbeam trips. - `speed` is in mm/s, `acceleration` in mm/s^2. If `z_start` is given, - first move Z to that height (same speed/accel) and search from there; - otherwise search from the current Z. Arms IL[4]=StopForward so the Elmo - drive halts the motor itself on the input edge (sub-ms latency, no - software in the loop). IL is restored to GeneralPurpose afterwards even - if the move raises. Returns the Z position where the drive halted; - raises RuntimeError if the beam never tripped. + If `z_start` is given, first move Z to that height (same vel/accel) and + search from there; otherwise search from the current Z. Arms IL[4]= + StopForward so the Elmo drive halts the motor itself on the input edge + (sub-ms latency, no software in the loop). IL is restored to + GeneralPurpose afterwards even if the move raises. Returns the Z + position where the drive halted; raises RuntimeError if the beam never + tripped. """ - move_params = KX2ArmBackend.JointMoveParams( - linear_speed=speed, linear_acceleration=acceleration, - ) + move_params = KX2ArmBackend.JointMoveParams(vel_pct=vel_pct, accel_pct=accel_pct) # Pre-flight: force the drive back to Op Enabled. A prior failed search # could have left it in Fault/Quick Stop where new moves silently fail # (Z barely moves). Idempotent if the drive is already healthy. @@ -769,14 +767,10 @@ async def calculate_move_abs_all_axes( skip_ax: Dict[Axis, bool] = {} # input validation / travel limits / done-wait logic - for name, val in ( - ("linear_speed", params.linear_speed), - ("linear_acceleration", params.linear_acceleration), - ("rotary_speed", params.rotary_speed), - ("rotary_acceleration", params.rotary_acceleration), - ): - if val is not None and val <= 0.0: - raise ValueError(f"{name} must be positive, got {val}") + if params.vel_pct <= 0.0 or params.vel_pct > 100.0: + raise ValueError(f"vel_pct out of range (0, 100], got {params.vel_pct}") + if params.accel_pct <= 0.0 or params.accel_pct > 100.0: + raise ValueError(f"accel_pct out of range (0, 100], got {params.accel_pct}") # Travel-limit bounds check. Mirrors C# MoveAbsoluteSingleAxisPrivate # (KX2RobotControl.cs:4624-4649): snap if within 0.1 of the limit, raise @@ -862,20 +856,8 @@ async def calculate_move_abs_all_axes( skip_ax[ax] = abs(dist[ax]) < 0.01 - axis_max_v = self._cfg.axes[ax].max_vel - axis_max_a = self._cfg.axes[ax].max_accel - if ax.is_linear: - chosen_v = params.linear_speed if params.linear_speed is not None else axis_max_v - chosen_a = ( - params.linear_acceleration if params.linear_acceleration is not None else axis_max_a - ) - else: - chosen_v = params.rotary_speed if params.rotary_speed is not None else axis_max_v - chosen_a = ( - params.rotary_acceleration if params.rotary_acceleration is not None else axis_max_a - ) - v[ax] = min(chosen_v, axis_max_v) - a[ax] = min(chosen_a, axis_max_a) + v[ax] = (params.vel_pct / 100.0) * self._cfg.axes[ax].max_vel + a[ax] = (params.accel_pct / 100.0) * self._cfg.axes[ax].max_accel if not skip_ax[ax] and a[ax] > 0: v[ax], a[ax], accel_time[ax], total_time[ax] = self._profile( @@ -1037,26 +1019,21 @@ async def _cart_to_joints( @dataclasses.dataclass class CartesianMoveParams(BackendParams): - """Per-axis speed/acceleration limits in physical units. + """Speed/acceleration as a percentage of each axis's firmware max. - `linear_*` applies to Z (and rail/gripper if commanded); `rotary_*` - applies to shoulder/elbow/wrist. `None` falls back to the axis maximum - read from the drive at setup. Values above the per-axis max are clamped - silently. + Applied uniformly to every axis in the move — `vel_pct=50` means each + axis runs at 50% of its own `max_vel` (read from the drive at setup, + surfaced via `motion_limits()`). Range (0, 100]. """ - linear_speed: Optional[float] = None # mm/s - linear_acceleration: Optional[float] = None # mm/s^2 - rotary_speed: Optional[float] = None # deg/s - rotary_acceleration: Optional[float] = None # deg/s^2 + vel_pct: float = 50.0 + accel_pct: float = 50.0 @dataclasses.dataclass class JointMoveParams(BackendParams): - """Per-axis speed/acceleration limits in physical units. Same shape as - `CartesianMoveParams` — see its docstring.""" - linear_speed: Optional[float] = None - linear_acceleration: Optional[float] = None - rotary_speed: Optional[float] = None - rotary_acceleration: Optional[float] = None + """Speed/acceleration as a percentage of each axis's firmware max. Same + shape as `CartesianMoveParams` — see its docstring.""" + vel_pct: float = 50.0 + accel_pct: float = 50.0 @dataclasses.dataclass class GripParams(BackendParams): @@ -1112,13 +1089,8 @@ async def move_to_gripper_location( if not isinstance(backend_params, KX2ArmBackend.CartesianMoveParams): backend_params = KX2ArmBackend.CartesianMoveParams() joint_pos = await self._cart_to_joints(pose) - # CartesianMoveParams and JointMoveParams have identical shape (linear/rotary - # speed + acceleration); copy the limits across to feed motors_move_joint. joint_params = KX2ArmBackend.JointMoveParams( - linear_speed=backend_params.linear_speed, - linear_acceleration=backend_params.linear_acceleration, - rotary_speed=backend_params.rotary_speed, - rotary_acceleration=backend_params.rotary_acceleration, + vel_pct=backend_params.vel_pct, accel_pct=backend_params.accel_pct, ) await self.motors_move_joint(cmd_pos=joint_pos, params=joint_params) @@ -1299,10 +1271,7 @@ async def very_dangerously_yeet( await self.motors_move_joint( cmd_pos={Axis.WRIST: wrist_inward}, params=KX2ArmBackend.JointMoveParams( - linear_speed=_YEET_WINDUP_SPEED, - linear_acceleration=_YEET_WINDUP_ACC, - rotary_speed=_YEET_WINDUP_SPEED, - rotary_acceleration=_YEET_WINDUP_ACC, + vel_pct=_YEET_WINDUP_VEL_PCT, accel_pct=_YEET_WINDUP_ACC_PCT, ), ) @@ -1367,10 +1336,7 @@ async def very_dangerously_yeet( Axis.WRIST: pickup_pose[Axis.WRIST], }, params=KX2ArmBackend.JointMoveParams( - linear_speed=_YEET_RETURN_SPEED, - linear_acceleration=_YEET_RETURN_ACC, - rotary_speed=_YEET_RETURN_SPEED, - rotary_acceleration=_YEET_RETURN_ACC, + vel_pct=_YEET_RETURN_VEL_PCT, accel_pct=_YEET_RETURN_ACC_PCT, ), ) finally: @@ -1426,12 +1392,12 @@ def __repr__(self) -> str: # Gripper open target (mm). Clamped at runtime to drive's max_travel - margin. _YEET_OPEN_POSITION = 30.0 _YEET_OPEN_SAFETY_MARGIN = 1.0 -# Windup speed: arm holds the plate, don't whip. -_YEET_WINDUP_SPEED = 60.0 -_YEET_WINDUP_ACC = 120.0 -# Return speed: plate is gone, snap back faster than windup. -_YEET_RETURN_SPEED = 240.0 -_YEET_RETURN_ACC = 480.0 +# Windup: arm holds the plate, don't whip. Percentages of axis max. +_YEET_WINDUP_VEL_PCT = 25.0 +_YEET_WINDUP_ACC_PCT = 25.0 +# Return: plate is gone, but still keep it gentle. +_YEET_RETURN_VEL_PCT = 25.0 +_YEET_RETURN_ACC_PCT = 25.0 async def _yeet_build_axis_move( From 6d93d02ff6a205b6205c8503fb56b10740149ad9 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 28 Apr 2026 22:13:47 -0700 Subject: [PATCH 51/93] KX2: cap gripper Cartesian speed via FD-based generic helper New: pylabrobot.capabilities.arms.kinematics with three helpers generic over any FK callable: gripper_speed (central FD, two FK evals regardless of DOF), sample_gripper_speed_along_trajectory, and joint_velocities_for_max_gripper_speed (single-sweep solve since |J*q_dot| is linear in q_dot). Tests included. KX2 wiring: gripper_position(joints, c, t) -> _GripperXYZ extracted from fk so the FD operates on raw floats (Coordinate rounds to 0.1 micron and breaks small-eps differences). JointMoveParams / CartesianMoveParams gain max_gripper_speed and max_gripper_acceleration (Cartesian mm/s and mm/s^2; None means run at firmware max). Planner uses 1000-sample peak detection. find_z_with_proximity_sensor and yeet constants updated to the new cap. Future work captured in pvt.todo.md: current joint-linear interp leaves avg/peak around 0.5-0.7 because |J*q_dot| varies along the path; PVT mode would let move_to_location trace a Cartesian straight line and hit avg ~ peak ~ cap. --- docs/user_guide/paa/kx2/hello-world.ipynb | 6 +- pvt.todo.md | 185 ++++++++++++++ pylabrobot/capabilities/arms/kinematics.py | 160 ++++++++++++ .../capabilities/arms/kinematics_tests.py | 237 ++++++++++++++++++ pylabrobot/paa/kx2/arm_backend.py | 146 ++++++++--- pylabrobot/paa/kx2/kinematics.py | 15 +- 6 files changed, 701 insertions(+), 48 deletions(-) create mode 100644 pvt.todo.md create mode 100644 pylabrobot/capabilities/arms/kinematics.py create mode 100644 pylabrobot/capabilities/arms/kinematics_tests.py diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 2bc58850ca1..770aae87c29 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -163,7 +163,7 @@ "cell_type": "markdown", "id": "kx2-motion-limits-md", "metadata": {}, - "source": "### Motion limits\n\n`motion_limits()` reads the per-axis maxima cached at setup. `JointMoveParams` / `CartesianMoveParams` take `vel_pct` and `accel_pct` (both default 100): each axis runs at that fraction of its own max. Linear axes are mm/s and mm/s^2; rotary in deg/s and deg/s^2." + "source": "### Motion limits\n\n`motion_limits()` reads the per-axis firmware maxima cached at setup. `JointMoveParams` / `CartesianMoveParams` take `max_gripper_speed` (mm/s) and `max_gripper_acceleration` (mm/s^2), which cap the worst-case Cartesian speed/acceleration at the gripper along the trajectory. `None` (the default) means no Cartesian cap — joints run at firmware max. Joint speeds get scaled uniformly so the gripper stays under the cap." }, { "cell_type": "code", @@ -201,7 +201,7 @@ "id": "kx2-cartesian-code", "metadata": {}, "outputs": [], - "source": "from pylabrobot.resources import Coordinate\n\nawait kx2.arm.move_to_location(\n location=Coordinate(x=330.124, y=210.552, z=320.0441),\n direction=198,\n backend_params=KX2ArmBackend.CartesianMoveParams(vel_pct=20, accel_pct=20),\n)" + "source": "from pylabrobot.resources import Coordinate\n\nawait kx2.arm.move_to_location(\n location=Coordinate(x=330.124, y=210.552, z=320.0441),\n direction=198,\n backend_params=KX2ArmBackend.CartesianMoveParams(\n max_gripper_speed=200.0, max_gripper_acceleration=1000.0,\n ),\n)" }, { "cell_type": "markdown", @@ -223,7 +223,7 @@ "id": "kx2-joint-code", "metadata": {}, "outputs": [], - "source": "from pylabrobot.paa.kx2 import KX2ArmBackend\n\nposition = {\n KX2ArmBackend.Axis.SHOULDER: 0.0,\n KX2ArmBackend.Axis.Z: 150.0,\n KX2ArmBackend.Axis.ELBOW: 90.0,\n KX2ArmBackend.Axis.WRIST: 0.0,\n}\nawait kx2.arm.backend.move_to_joint_position(\n position,\n backend_params=KX2ArmBackend.JointMoveParams(vel_pct=20, accel_pct=20),\n)" + "source": "from pylabrobot.paa.kx2 import KX2ArmBackend\n\nposition = {\n KX2ArmBackend.Axis.SHOULDER: 0.0,\n KX2ArmBackend.Axis.Z: 150.0,\n KX2ArmBackend.Axis.ELBOW: 90.0,\n KX2ArmBackend.Axis.WRIST: 0.0,\n}\nawait kx2.arm.backend.move_to_joint_position(\n position,\n backend_params=KX2ArmBackend.JointMoveParams(\n max_gripper_speed=200.0, max_gripper_acceleration=1000.0,\n ),\n)" }, { "cell_type": "markdown", diff --git a/pvt.todo.md b/pvt.todo.md new file mode 100644 index 00000000000..d4f2f951958 --- /dev/null +++ b/pvt.todo.md @@ -0,0 +1,185 @@ +# PVT mode — interpolated motion for KX2 + +## Why bother + +`move_to_location` today is joint-linear: `_cart_to_joints(target)` then a single +`motors_move_joint`. Cart-space path is curved; |J(q)·q_dot| varies along it. +Capping peak gripper speed = cap leaves mean ≈ 0.5–0.7·cap (peak/mean ratio of +the path). Validator confirms peak ≤ cap is hit exactly, but avg/cap stays at +0.1–0.7 even on long moves. There is no fix inside a single trapezoidal profile. + +**Goal**: avg gripper speed ≈ peak ≈ cap during cruise. Two ways: +1. Cartesian-linear interpolation in `move_to_location` (gripper traces a cart + straight line; |J·q_dot| ~ constant = cap). +2. Joint-linear path but time-varying joint velocity in PVT. + +Either way, we need PVT (or PP-on-the-fly) on the drive — single-trapezoid PP +mode can't vary speed mid-move. + +## What we already have + +`pylabrobot/paa/kx2/driver.py` is more set up than I expected: + +- `pvt_select_mode(enable)` — switches mode object `0x6060` between PP (1) and + IP (7), and toggles the interpolation-buffer enable (`0x60C4 sub 6`). + Currently called with `False` before every PP move (driver.py:842). +- `setup()` configures Elmo vendor objects for PVT (driver.py:325–332): + - `24768.0 = -1` — interpolation submode (Elmo-specific) + - `24772.2 = 16` — buffer size (16 deep) + - `24772.5 = 8` — points-per-block? (verify in Elmo docs) + - `24770.2 = -3` — time-base exponent (10⁻³ s = ms ticks) + - `24669.0 = 1` — enable interpolation +- RPDO3 is already mapped to `(0x60C1.01 TargetPositionIP, 0x60C1.02 TargetVelocityIP)` + with EventDrivenDev transmission — so we can shove a new PVT point with one + PDO write per axis. +- TPDO mapping exists for `PVTHeadPointer (0x2F11)` and `PVTTailPointer (0x2F12)` + — buffer occupancy is observable. + +So the protocol-level scaffolding is in place. The missing pieces are +**trajectory generation**, **buffer-fill loop**, and **planner integration**. + +## Two implementation flavors + +### Flavor A — Cartesian-linear `move_to_location` + +Scope: `move_to_location` only. `move_to_joint_position` stays joint-linear +(joint-linear is the right thing for a joint-space move). + +1. Sample the cart line at fine resolution (every ~5 mm or so). +2. IK each sample, snapping wrist to the previous so we don't flip mid-move. +3. Compute joint velocities at each sample by finite-difference of consecutive + joint positions divided by segment time. Time per segment = segment cart + length / cap. +4. Stream `(joint_pos, joint_vel, dt)` per axis into the PVT FIFO. + +avg/cap ≈ 1 modulo accel ramps at the endpoints, which we still pay (start at +v=0, end at v=0). For long moves this overhead is negligible. + +### Flavor B — joint-linear with constant-cart-speed PVT + +Scope: cap helper. The path is the same joint-linear path the planner already +uses; just vary joint velocity over time so |J·q_dot| stays at cap. + +1. Sample the joint-linear path at high resolution. +2. At each sample, compute the joint velocity scale that pins gripper speed at + cap: `q_dot(α) = q_dot_unit · cap / |J(q(α)) · q_dot_unit|`, capped by + firmware ceiling per axis. +3. Convert to (joint_pos, joint_vel, dt) tuples (dt = arc-length-in-joint / + |q_dot|), stream into PVT. + +Easier in some ways (no extra IK calls — the trajectory IS the joint line), +but the gripper's cart path is still curved, so this only fixes avg vs peak, +not "the gripper moves in a straight line." Probably less useful for actual +plate handling than A. + +**Recommended default: A.** Plate handling cares about predictable cart +trajectories. B is theoretically interesting but doesn't change *where* the +gripper goes, only how fast it goes there. + +## Implementation sketch + +### `KX2Driver` (driver.py) + +New methods: + +```python +async def pvt_push_point( + self, node_id: int, position: int, velocity: int, dt_ms: int, +) -> None: ... + +async def pvt_buffer_free(self, node_id: int) -> int: ... + # Read head - tail (modulo buffer size) from TPDO cache. + +async def pvt_start(self, node_ids: List[int]) -> None: ... + # Switch to mode 7, enable IP buffer (0x60C4 sub 6 = 1), set CW = enable. + +async def pvt_wait_until_drained(self, node_ids: List[int], timeout: float): ... + # Done = head == tail and motion complete. + +async def pvt_stop(self, node_ids: List[int]) -> None: ... + # Disable buffer, switch back to mode 1 (PP). +``` + +Buffer fill: tight async loop that watches `pvt_buffer_free()` per axis, pushes +points whenever there's headroom, sleeps a few ms otherwise. PVT FIFO is 16 +deep per axis — refill should comfortably keep up at the planner-side rates. + +### `KX2ArmBackend` (arm_backend.py) + +New planner sibling to `calculate_move_abs_all_axes`: + +```python +async def calculate_move_pvt_cartesian( + self, + start_pose: KX2GripperLocation, + end_pose: KX2GripperLocation, + cap: float, +) -> _PVTPlan: ... +``` + +Returns per-axis lists of (pos, vel, t) tuples in encoder/firmware units. The +existing `_profile`-based planner is untouched. + +`move_to_location` decision tree: +- `backend_params.linear_cart_motion = True` (new flag, default True): use PVT. +- Else: existing joint-linear PP. + +`move_to_joint_position` always uses PP. (Joint moves don't have a "straight +line" semantic.) + +### Tests + +- New unit test for trajectory generator: feed a known cart line, check + resulting joint waypoints reproject (FK) within tolerance to the line. +- Integration test on real arm: random A→B Cartesian moves at random caps, + validator measures peak via fine-resolution FK sweep over the actual + PVT joint trajectory; expect avg/cap ≥ 0.9 on moves >100 mm. + +## Gotchas / unknowns + +- **Segment time quantum.** Vendor object `24770.2 = -3` sets time exponent to + 10⁻³ s, so segment dt is in milliseconds. Verify the resolution and the + max representable dt in one PDO before designing the segmentation step. +- **Velocity unit.** `0x60C1.02 TargetVelocityIP` units may differ from + `0x60FF Target Velocity` in PV mode (Elmo sometimes uses + position-units-per-IP-period vs counts/sec). Check before scaling. +- **First/last segment.** PVT cubic-Hermite interp wants v at both ends. First + segment must start at v=0 (drive is at rest). Last segment must end at v=0. + In between, velocity is whatever the cart-speed-at-this-config calls for. +- **IK continuity.** Wrist sign and J1/J4 unwrapping must stay consistent + across consecutive cart samples. `snap_to_current` handles single-call + snapping; for streamed points we'd snap each to the *previous PVT sample*, + not to current measured joint pos. +- **Buffer underrun.** If we push slow and the FIFO drains, the drive faults + (Elmo MF for IP underflow). Fill aggressively — point preparation is much + faster than CAN throughput so we should be safe, but worth instrumenting. +- **Buffer overrun.** Drive ignores writes past `head + buffer_size`. Must + read-back head/tail before each push. +- **Mid-move halt.** `halt()` must work in PVT mode too — switch back to PP + with current commanded position as the new target, or just rely on CW=quick- + stop. Verify on hardware. +- **Sync across axes.** Each axis has its own PVT FIFO; they advance on the + drive's IP-period clock. With per-axis EventDrivenDev RPDOs, alignment + between axes is best-effort. May need SynchronousCyclic with SYNC frames if + jitter shows up. Prototype event-driven first; only switch to SYNC if cart + path drifts from the intended line. + +## Out of scope / future + +- Blending across multiple `move_to_location` calls (so a pick→approach→drop + sequence becomes one continuous PVT stream with no stop between sub-moves). + Big UX win for assays but a lot more state to manage. +- Speed/accel limits beyond the gripper-speed cap (per-axis torque/jerk caps). +- Online replanning if the path is blocked (collision avoidance) — way out of + scope, but PVT does enable it because we control velocity in real time. + +## References + +- CiA 402 §10.4 (Interpolated Position Mode) — defines `0x60C0`, `0x60C1`, + `0x60C4`. +- Elmo Application Note "Interpolated Position Mode (PVT)" — vendor + objects 0x21C0–0x21C7 documenting the buffer protocol and mode submodes. +- Existing PP planner: `KX2ArmBackend.calculate_move_abs_all_axes` + (arm_backend.py:759). +- Existing PVT scaffolding: `pvt_select_mode` (driver.py:842), setup at + driver.py:325–347. diff --git a/pylabrobot/capabilities/arms/kinematics.py b/pylabrobot/capabilities/arms/kinematics.py new file mode 100644 index 00000000000..3e1963619c7 --- /dev/null +++ b/pylabrobot/capabilities/arms/kinematics.py @@ -0,0 +1,160 @@ +"""Generic kinematic helpers shared across arm backends. + +Per-arm forward/inverse kinematics live in the arm's own module +(e.g. ``pylabrobot.paa.kx2.kinematics``); this file is for utilities +that work over *any* forward-kinematics function regardless of DOF +count, joint topology, or link layout. +""" + +from math import sqrt +from typing import Callable, Dict, Hashable, Iterator, Tuple, TypeVar + +from pylabrobot.resources import Coordinate + + +K = TypeVar("K", bound=Hashable) + + +def gripper_speed( + fk: Callable[[Dict[K, float]], Coordinate], + joints: Dict[K, float], + joint_velocities: Dict[K, float], + eps: float = 1e-6, +) -> float: + """Cartesian gripper speed at this joint snapshot. + + Computes |d/dt fk(joints(t))| via a central finite difference along the + joint-velocity direction -- two ``fk`` evaluations regardless of DOF + count. Equivalent to forming the Jacobian and computing |J*q_dot|, but + avoids materializing J. + + Args: + fk: forward kinematics, joints dict -> ``Coordinate``. + joints: current joint positions, in whatever units ``fk`` expects. + joint_velocities: per-joint rate (same units / second). Missing keys + are treated as zero, so callers can pass only the moving joints. + eps: finite-difference step in joint units. ``Coordinate`` rounds + to 0.1 micron, so ``eps * q_dot`` must shift the coordinate by + more than that or the diff collapses to zero -- pass ``eps=1e-3`` + for typical mm/deg-scale arms. Default ``1e-6`` is fine for + non-rounding fixtures (tests). + + Returns: + Magnitude of the Cartesian velocity in whatever length units ``fk`` + returns, per second. + """ + plus = fk({k: q + eps * joint_velocities.get(k, 0.0) for k, q in joints.items()}) + minus = fk({k: q - eps * joint_velocities.get(k, 0.0) for k, q in joints.items()}) + inv_2eps = 1.0 / (2.0 * eps) + dx = (plus.x - minus.x) * inv_2eps + dy = (plus.y - minus.y) * inv_2eps + dz = (plus.z - minus.z) * inv_2eps + return sqrt(dx * dx + dy * dy + dz * dz) + + +def sample_gripper_speed_along_trajectory( + fk: Callable[[Dict[K, float]], Coordinate], + joints_start: Dict[K, float], + joint_deltas: Dict[K, float], + joint_velocities: Dict[K, float], + num_samples: int = 20, + eps: float = 1e-6, +) -> Iterator[Tuple[float, float]]: + """Yield ``(alpha, gripper_speed)`` along the joint-space path + ``joints_start + alpha * joint_deltas`` for ``alpha`` in ``[0, 1]``. + + ``joint_deltas`` is the *direction-aware* per-axis change (signed): + for unlimited-travel rotary axes the caller should resolve the + shortest-way / always-CW / always-CCW convention before passing in, so + this helper walks the same path the arm physically takes (e.g. a + ShortestWay move from 350 deg to 10 deg has ``delta = +20``, not + ``-340``). + + ``joint_velocities`` is held constant across all samples (representing + cruise-phase joint rates); each sample returns the gripper speed at + that pose. + + Use ``alpha`` to plot, find the worst-case sample, etc.:: + + max_speed = max(s for _, s in sample_gripper_speed_along_trajectory(...)) + + Missing keys in either ``joints_start`` or ``joint_deltas`` are + treated as zero, so callers can pass only moving joints. + """ + if num_samples < 2: + raise ValueError(f"num_samples must be >= 2, got {num_samples}") + keys = set(joints_start) | set(joint_deltas) + for i in range(num_samples): + alpha = i / (num_samples - 1) + sample = { + k: joints_start.get(k, 0.0) + alpha * joint_deltas.get(k, 0.0) for k in keys + } + yield alpha, gripper_speed(fk, sample, joint_velocities, eps=eps) + + +def joint_velocities_for_max_gripper_speed( + fk: Callable[[Dict[K, float]], Coordinate], + joints_start: Dict[K, float], + joint_deltas: Dict[K, float], + joint_max_velocities: Dict[K, float], + max_gripper_speed: float, + num_samples: int = 20, + eps: float = 1e-6, +) -> Dict[K, float]: + """Signed joint velocities such that worst-case gripper speed along + the joint-space path ``joints_start + alpha * joint_deltas`` is at + most ``max_gripper_speed`` -- or each axis's firmware ceiling, + whichever is tighter. + + Sign of each output velocity is taken from ``joint_deltas`` (axes + with zero delta get zero velocity). The caller is responsible for + resolving any wrap-around / shortest-way / direction conventions when + computing ``joint_deltas`` -- this helper just walks the path it's + handed. + + Gripper speed is linear in joint velocities (the Jacobian is), so a + single sweep determines the scale factor exactly: if running every + axis at ``joint_max_velocities`` produces a worst-case path speed + ``M``, the returned velocities are scaled by ``min(1, max_gripper_speed / M)``. + No iteration. + + Equally applicable to acceleration -- pass joint max accelerations as + ``joint_max_velocities`` and the gripper-accel cap as + ``max_gripper_speed``; the math is identical because both reduce to + ``|J * rate|`` at each pose. + + Args: + fk: forward kinematics, joints dict -> ``Coordinate``. + joints_start: starting joint positions. + joint_deltas: signed per-axis change along the trajectory. Axis sign + determines output velocity direction; zero means "don't move". + joint_max_velocities: per-axis firmware ceiling, *unsigned*. + max_gripper_speed: Cartesian cap, in whatever length units ``fk`` + returns, per second. + num_samples: poses sampled along the path. 20 is plenty for a SCARA. + eps: see :func:`gripper_speed`. + + Returns: + Dict of *signed* joint velocities, one per key in + ``joint_max_velocities``. + """ + signed_max: Dict[K, float] = {} + for k in joint_max_velocities: + delta = joint_deltas.get(k, 0.0) + if delta > 0: + signed_max[k] = joint_max_velocities[k] + elif delta < 0: + signed_max[k] = -joint_max_velocities[k] + else: + signed_max[k] = 0.0 + + max_v = max( + s + for _, s in sample_gripper_speed_along_trajectory( + fk, joints_start, joint_deltas, signed_max, num_samples=num_samples, eps=eps + ) + ) + if max_v == 0.0: + return signed_max + scale = min(1.0, max_gripper_speed / max_v) + return {k: scale * v for k, v in signed_max.items()} diff --git a/pylabrobot/capabilities/arms/kinematics_tests.py b/pylabrobot/capabilities/arms/kinematics_tests.py new file mode 100644 index 00000000000..2a7ca6ccca0 --- /dev/null +++ b/pylabrobot/capabilities/arms/kinematics_tests.py @@ -0,0 +1,237 @@ +import math +import unittest +from dataclasses import dataclass + +from pylabrobot.capabilities.arms.kinematics import ( + gripper_speed, + joint_velocities_for_max_gripper_speed, + sample_gripper_speed_along_trajectory, +) + + +@dataclass +class _XYZ: + """Non-rounding x/y/z holder for tests; Coordinate rounds to 0.1 micron + which makes finite differences degenerate.""" + + x: float + y: float + z: float + + +class GripperSpeedTests(unittest.TestCase): + def test_static_pose_zero_speed(self): + """All velocities zero -> gripper speed zero, regardless of FK.""" + + def fk(j): + return _XYZ(x=math.cos(j["a"]), y=math.sin(j["a"]), z=0.0) + + self.assertAlmostEqual(gripper_speed(fk, {"a": 1.5}, {"a": 0.0}), 0.0, places=10) + + def test_pure_translation(self): + """fk(q) = (q, 0, 0); dx/dt = q_dot => |v| = |q_dot|.""" + + def fk(j): + return _XYZ(x=j["q"], y=0.0, z=0.0) + + self.assertAlmostEqual(gripper_speed(fk, {"q": 0.0}, {"q": 7.0}), 7.0, places=6) + self.assertAlmostEqual(gripper_speed(fk, {"q": 100.0}, {"q": -3.5}), 3.5, places=6) + + def test_unit_circle_tangential_speed(self): + """fk(theta) on the unit circle; |v| = r * |theta_dot|. r=1, theta_dot=2 => |v|=2.""" + + def fk(j): + return _XYZ(x=math.cos(j["t"]), y=math.sin(j["t"]), z=0.0) + + self.assertAlmostEqual(gripper_speed(fk, {"t": 0.7}, {"t": 2.0}), 2.0, places=6) + + def test_circle_with_radius(self): + """fk(theta) on a circle of radius 5; |v| = 5 * |theta_dot|.""" + + def fk(j): + return _XYZ(x=5.0 * math.cos(j["t"]), y=5.0 * math.sin(j["t"]), z=0.0) + + self.assertAlmostEqual(gripper_speed(fk, {"t": 1.2}, {"t": 3.0}), 15.0, places=5) + + def test_two_dof_quadrature(self): + """Two independent linear axes; speeds add in quadrature.""" + + def fk(j): + return _XYZ(x=j["a"], y=j["b"], z=0.0) + + self.assertAlmostEqual( + gripper_speed(fk, {"a": 0, "b": 0}, {"a": 3.0, "b": 4.0}), 5.0, places=6 + ) + + def test_missing_velocity_key_is_zero(self): + """Velocity keys can be a subset of joint keys.""" + + def fk(j): + return _XYZ(x=j["a"] + j["b"], y=0.0, z=0.0) + + self.assertAlmostEqual(gripper_speed(fk, {"a": 0, "b": 0}, {"a": 2.0}), 2.0, places=6) + + +class SampleAlongTrajectoryTests(unittest.TestCase): + def test_alpha_endpoints(self): + """First sample at alpha=0, last at alpha=1.""" + + def fk(j): + return _XYZ(x=j["q"], y=0.0, z=0.0) + + samples = list( + sample_gripper_speed_along_trajectory( + fk, joints_start={"q": 0}, joint_deltas={"q": 10}, joint_velocities={"q": 1.0}, num_samples=5 + ) + ) + self.assertEqual(len(samples), 5) + self.assertAlmostEqual(samples[0][0], 0.0) + self.assertAlmostEqual(samples[-1][0], 1.0) + + def test_constant_speed_along_linear_fk(self): + """For a linear fk, speed is constant along the trajectory.""" + + def fk(j): + return _XYZ(x=j["q"], y=0.0, z=0.0) + + speeds = [ + s + for _, s in sample_gripper_speed_along_trajectory( + fk, {"q": -5}, {"q": 10}, {"q": 2.0}, num_samples=10 + ) + ] + for s in speeds: + self.assertAlmostEqual(s, 2.0, places=6) + + def test_max_speed_at_extension_for_radial_arm(self): + """fk(elbow, shoulder) = (r * cos, r * sin) where r = elbow. + + With elbow ramping from 1 to 5 (delta=4) and only shoulder moving + (theta_dot=1), tangential speed = r * theta_dot grows monotonically; + max at the end.""" + + def fk(j): + r = j["e"] + return _XYZ(x=r * math.cos(j["s"]), y=r * math.sin(j["s"]), z=0.0) + + samples = list( + sample_gripper_speed_along_trajectory( + fk, + joints_start={"e": 1.0, "s": 0.0}, + joint_deltas={"e": 4.0, "s": 0.0}, + joint_velocities={"s": 1.0}, + num_samples=20, + ) + ) + speeds = [s for _, s in samples] + self.assertAlmostEqual(speeds[0], 1.0, places=5) + self.assertAlmostEqual(speeds[-1], 5.0, places=5) + self.assertEqual(max(speeds), speeds[-1]) + + def test_wrap_around_path(self): + """Direction-aware deltas: a ShortestWay move from 350 deg to 10 deg + walks through 0 (delta=+20), not through 180 (delta=-340). The unit + circle FK has constant speed = |theta_dot|, but the path is + different -- we sanity-check that the start/end poses are correct.""" + + def fk(j): + r = math.radians(j["t"]) + return _XYZ(x=math.cos(r), y=math.sin(r), z=0.0) + + samples = list( + sample_gripper_speed_along_trajectory( + fk, joints_start={"t": 350.0}, joint_deltas={"t": 20.0}, + joint_velocities={"t": 1.0}, num_samples=11, + ) + ) + # alpha=0 -> 350 deg (cos=cos(-10), sin=-sin(10)) + # alpha=1 -> 370 deg = 10 deg (cos=cos(10), sin=sin(10)) + # alpha=0.5 -> 360 deg = 0 deg (cos=1, sin=0); proves we walked through 0, not 180. + mid_speed = samples[5][1] + self.assertAlmostEqual(mid_speed, math.radians(1.0), places=6) + + def test_too_few_samples_raises(self): + def fk(j): + return _XYZ(x=0.0, y=0.0, z=0.0) + + with self.assertRaises(ValueError): + list(sample_gripper_speed_along_trajectory(fk, {}, {}, {}, num_samples=1)) + + +class JointVelocitiesForMaxGripperSpeedTests(unittest.TestCase): + def test_under_cap_returns_signed_max(self): + """Cap not binding -> joints run at signed firmware max.""" + + def fk(j): + return _XYZ(x=j["q"], y=0.0, z=0.0) + + v = joint_velocities_for_max_gripper_speed( + fk, joints_start={"q": 0}, joint_deltas={"q": 10}, + joint_max_velocities={"q": 2.0}, max_gripper_speed=5.0, + ) + self.assertAlmostEqual(v["q"], 2.0, places=6) + + def test_negative_delta_negates_velocity(self): + """Negative delta -> velocity is negative.""" + + def fk(j): + return _XYZ(x=j["q"], y=0.0, z=0.0) + + v = joint_velocities_for_max_gripper_speed( + fk, joints_start={"q": 10}, joint_deltas={"q": -10}, + joint_max_velocities={"q": 2.0}, max_gripper_speed=5.0, + ) + self.assertAlmostEqual(v["q"], -2.0, places=6) + + def test_zero_delta_zero_velocity(self): + """Axes with zero delta get velocity 0, regardless of max.""" + + def fk(j): + return _XYZ(x=j["a"] + j["b"], y=0.0, z=0.0) + + v = joint_velocities_for_max_gripper_speed( + fk, + joints_start={"a": 0, "b": 5}, + joint_deltas={"a": 10, "b": 0}, + joint_max_velocities={"a": 1.0, "b": 1.0}, + max_gripper_speed=100.0, + ) + self.assertAlmostEqual(v["a"], 1.0, places=6) + self.assertAlmostEqual(v["b"], 0.0, places=6) + + def test_over_cap_scales_linearly(self): + """firmware_max=10 gives gripper speed 10; cap=2 -> v=0.2*10=2.""" + + def fk(j): + return _XYZ(x=j["q"], y=0.0, z=0.0) + + v = joint_velocities_for_max_gripper_speed( + fk, joints_start={"q": 0}, joint_deltas={"q": 100}, + joint_max_velocities={"q": 10.0}, max_gripper_speed=2.0, + ) + self.assertAlmostEqual(v["q"], 2.0, places=6) + + def test_radial_arm_caps_at_full_extension(self): + """Radial arm with elbow + shoulder both moving. Verifies (a) the cap + is met along the path and (b) every axis is scaled by the same + factor (proportions to joint_max preserved).""" + + def fk(j): + r = j["e"] + return _XYZ(x=r * math.cos(j["s"]), y=r * math.sin(j["s"]), z=0.0) + + start = {"e": 0.0, "s": 0.0} + delta = {"e": 5.0, "s": 1.0} + joint_max = {"e": 1.0, "s": 2.0} + v = joint_velocities_for_max_gripper_speed( + fk, start, delta, joint_max, max_gripper_speed=6.0, num_samples=50, + ) + max_speed = max( + s for _, s in sample_gripper_speed_along_trajectory(fk, start, delta, v, num_samples=50) + ) + self.assertAlmostEqual(max_speed, 6.0, places=3) + self.assertAlmostEqual(v["e"] / joint_max["e"], v["s"] / joint_max["s"], places=6) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index ac9e62fa3ca..37d8509a7f1 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -7,6 +7,7 @@ from enum import IntEnum from typing import Dict, List, Optional, Union +from pylabrobot.capabilities.arms import kinematics as arm_kinematics from pylabrobot.capabilities.arms.backend import ( CanFreedrive, HasJoints, @@ -644,20 +645,24 @@ async def find_z_with_proximity_sensor( self, max_descent: float, z_start: Optional[float] = None, - vel_pct: float = 5.0, - accel_pct: float = 5.0, + max_gripper_speed: float = 25.0, + max_gripper_acceleration: float = 100.0, ) -> float: """Descend Z up to `max_descent`; halt when the IR breakbeam trips. - If `z_start` is given, first move Z to that height (same vel/accel) and - search from there; otherwise search from the current Z. Arms IL[4]= - StopForward so the Elmo drive halts the motor itself on the input edge - (sub-ms latency, no software in the loop). IL is restored to - GeneralPurpose afterwards even if the move raises. Returns the Z - position where the drive halted; raises RuntimeError if the beam never - tripped. + `max_gripper_speed`/`max_gripper_acceleration` cap Z descent in mm/s + and mm/s^2 (Z is linear, so gripper speed equals |v_z|). If `z_start` + is given, first move Z to that height (same caps) and search from + there; otherwise search from the current Z. Arms IL[4]=StopForward so + the Elmo drive halts the motor itself on the input edge (sub-ms + latency, no software in the loop). IL is restored to GeneralPurpose + afterwards even if the move raises. Returns the Z position where the + drive halted; raises RuntimeError if the beam never tripped. """ - move_params = KX2ArmBackend.JointMoveParams(vel_pct=vel_pct, accel_pct=accel_pct) + move_params = KX2ArmBackend.JointMoveParams( + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ) # Pre-flight: force the drive back to Op Enabled. A prior failed search # could have left it in Fault/Quick Stop where new moves silently fail # (Z barely moves). Idempotent if the drive is already healthy. @@ -767,10 +772,14 @@ async def calculate_move_abs_all_axes( skip_ax: Dict[Axis, bool] = {} # input validation / travel limits / done-wait logic - if params.vel_pct <= 0.0 or params.vel_pct > 100.0: - raise ValueError(f"vel_pct out of range (0, 100], got {params.vel_pct}") - if params.accel_pct <= 0.0 or params.accel_pct > 100.0: - raise ValueError(f"accel_pct out of range (0, 100], got {params.accel_pct}") + if params.max_gripper_speed is not None and params.max_gripper_speed <= 0.0: + raise ValueError( + f"max_gripper_speed must be positive, got {params.max_gripper_speed}" + ) + if params.max_gripper_acceleration is not None and params.max_gripper_acceleration <= 0.0: + raise ValueError( + f"max_gripper_acceleration must be positive, got {params.max_gripper_acceleration}" + ) # Travel-limit bounds check. Mirrors C# MoveAbsoluteSingleAxisPrivate # (KX2RobotControl.cs:4624-4649): snap if within 0.1 of the limit, raise @@ -800,6 +809,11 @@ async def calculate_move_abs_all_axes( f"Axis {ax.name} target {t} below min_travel {ax_cfg.min_travel}" ) + # Snapshot in cmd_pos units (elbow as linear extension, not angle) for + # the gripper-speed cap helper, which needs joints in `kinematics.fk`'s + # natural units. + target_cmd_units = dict(target) + # Convert elbow cmd from position->angle for planning math if Axis.ELBOW in axes: target[Axis.ELBOW] = self.convert_elbow_position_to_angle(target[Axis.ELBOW]) @@ -814,6 +828,7 @@ async def calculate_move_abs_all_axes( # Determine current/start positions curr = await self.request_joint_position() + curr_cmd_units = dict(curr) # Elbow: convert both target and start to angle for distance math if Axis.ELBOW in curr: @@ -834,6 +849,64 @@ async def calculate_move_abs_all_axes( accel_time: Dict[Axis, float] = {} total_time: Dict[Axis, float] = {} + # Direction-aware deltas in cmd_pos units (elbow as linear extension). + # Identical logic to the dist computation below, but evaluated against + # the un-converted joints so the cap helper sees the trajectory the + # arm actually walks. Skipped axes get delta 0. + cap_deltas: Dict[Axis, float] = {} + for ax in axes: + if self._cfg.axes[ax].unlimited_travel: + d = target_cmd_units[ax] - curr_cmd_units.get(ax, 0.0) + span = self._cfg.axes[ax].max_travel - self._cfg.axes[ax].min_travel + dir_ = self._cfg.axes[ax].joint_move_direction + if dir_ == _JointMoveDirection.Clockwise and d > 0.01: + d -= span + elif dir_ == _JointMoveDirection.Counterclockwise and d < -0.01: + d += span + elif dir_ == _JointMoveDirection.ShortestWay: + if d > 180.0: + d -= span + elif d < -180.0: + d += span + cap_deltas[ax] = d + else: + cap_deltas[ax] = target_cmd_units[ax] - curr_cmd_units.get(ax, 0.0) + + # Per-axis caps from the gripper-speed limit. Helper iterates the joint + # path in `kinematics.fk`'s native units (elbow as linear extension, + # everything else as the user specified). Servo gripper isn't in fk, so + # always run it at firmware max regardless of cap. Pass start joints + # filled in with curr_cmd_units defaults so fk has all four arm axes + # even when the move only touches a subset. + arm_axes = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) + fk_start = {ax: curr_cmd_units[ax] for ax in arm_axes if ax in curr_cmd_units} + fk_deltas = {ax: cap_deltas.get(ax, 0.0) for ax in arm_axes if ax in fk_start} + capped_v: Dict[Axis, float] = {} + capped_a: Dict[Axis, float] = {} + fk = lambda j: kinematics.fk(j, self._cfg, self._gripper_config).location + if params.max_gripper_speed is not None and fk_start: + result = arm_kinematics.joint_velocities_for_max_gripper_speed( + fk=fk, + joints_start=fk_start, + joint_deltas=fk_deltas, + joint_max_velocities={ax: self._cfg.axes[ax].max_vel for ax in fk_start}, + max_gripper_speed=params.max_gripper_speed, + num_samples=1000, + eps=1e-3, + ) + capped_v = {ax: abs(v) for ax, v in result.items()} + if params.max_gripper_acceleration is not None and fk_start: + result = arm_kinematics.joint_velocities_for_max_gripper_speed( + fk=fk, + joints_start=fk_start, + joint_deltas=fk_deltas, + joint_max_velocities={ax: self._cfg.axes[ax].max_accel for ax in fk_start}, + max_gripper_speed=params.max_gripper_acceleration, + num_samples=1000, + eps=1e-3, + ) + capped_a = {ax: abs(a) for ax, a in result.items()} + for ax in axes: if self._cfg.axes[ax].unlimited_travel: d = target[ax] - curr[ax] @@ -856,8 +929,8 @@ async def calculate_move_abs_all_axes( skip_ax[ax] = abs(dist[ax]) < 0.01 - v[ax] = (params.vel_pct / 100.0) * self._cfg.axes[ax].max_vel - a[ax] = (params.accel_pct / 100.0) * self._cfg.axes[ax].max_accel + v[ax] = capped_v.get(ax, self._cfg.axes[ax].max_vel) + a[ax] = capped_a.get(ax, self._cfg.axes[ax].max_accel) if not skip_ax[ax] and a[ax] > 0: v[ax], a[ax], accel_time[ax], total_time[ax] = self._profile( @@ -1019,21 +1092,19 @@ async def _cart_to_joints( @dataclasses.dataclass class CartesianMoveParams(BackendParams): - """Speed/acceleration as a percentage of each axis's firmware max. - - Applied uniformly to every axis in the move — `vel_pct=50` means each - axis runs at 50% of its own `max_vel` (read from the drive at setup, - surfaced via `motion_limits()`). Range (0, 100]. - """ - vel_pct: float = 50.0 - accel_pct: float = 50.0 + """Cartesian gripper-speed/acceleration cap during the move (mm/s, + mm/s^2). ``None`` means run joints at firmware max with no Cartesian + cap. Joint speeds are scaled uniformly so the worst-case Cartesian + gripper speed along the trajectory stays at or under + ``max_gripper_speed``.""" + max_gripper_speed: Optional[float] = None + max_gripper_acceleration: Optional[float] = None @dataclasses.dataclass class JointMoveParams(BackendParams): - """Speed/acceleration as a percentage of each axis's firmware max. Same - shape as `CartesianMoveParams` — see its docstring.""" - vel_pct: float = 50.0 - accel_pct: float = 50.0 + """Same shape as ``CartesianMoveParams`` — see its docstring.""" + max_gripper_speed: Optional[float] = None + max_gripper_acceleration: Optional[float] = None @dataclasses.dataclass class GripParams(BackendParams): @@ -1090,7 +1161,8 @@ async def move_to_gripper_location( backend_params = KX2ArmBackend.CartesianMoveParams() joint_pos = await self._cart_to_joints(pose) joint_params = KX2ArmBackend.JointMoveParams( - vel_pct=backend_params.vel_pct, accel_pct=backend_params.accel_pct, + max_gripper_speed=backend_params.max_gripper_speed, + max_gripper_acceleration=backend_params.max_gripper_acceleration, ) await self.motors_move_joint(cmd_pos=joint_pos, params=joint_params) @@ -1271,7 +1343,8 @@ async def very_dangerously_yeet( await self.motors_move_joint( cmd_pos={Axis.WRIST: wrist_inward}, params=KX2ArmBackend.JointMoveParams( - vel_pct=_YEET_WINDUP_VEL_PCT, accel_pct=_YEET_WINDUP_ACC_PCT, + max_gripper_speed=_YEET_WINDUP_GRIPPER_SPEED, + max_gripper_acceleration=_YEET_WINDUP_GRIPPER_ACC, ), ) @@ -1336,7 +1409,8 @@ async def very_dangerously_yeet( Axis.WRIST: pickup_pose[Axis.WRIST], }, params=KX2ArmBackend.JointMoveParams( - vel_pct=_YEET_RETURN_VEL_PCT, accel_pct=_YEET_RETURN_ACC_PCT, + max_gripper_speed=_YEET_RETURN_GRIPPER_SPEED, + max_gripper_acceleration=_YEET_RETURN_GRIPPER_ACC, ), ) finally: @@ -1392,12 +1466,12 @@ def __repr__(self) -> str: # Gripper open target (mm). Clamped at runtime to drive's max_travel - margin. _YEET_OPEN_POSITION = 30.0 _YEET_OPEN_SAFETY_MARGIN = 1.0 -# Windup: arm holds the plate, don't whip. Percentages of axis max. -_YEET_WINDUP_VEL_PCT = 25.0 -_YEET_WINDUP_ACC_PCT = 25.0 +# Windup: arm holds the plate, don't whip. +_YEET_WINDUP_GRIPPER_SPEED = 100.0 # mm/s +_YEET_WINDUP_GRIPPER_ACC = 500.0 # mm/s^2 # Return: plate is gone, but still keep it gentle. -_YEET_RETURN_VEL_PCT = 25.0 -_YEET_RETURN_ACC_PCT = 25.0 +_YEET_RETURN_GRIPPER_SPEED = 100.0 # mm/s +_YEET_RETURN_GRIPPER_ACC = 500.0 # mm/s^2 async def _yeet_build_axis_move( diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index 2d535f3bf5f..86e1d726ee6 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -62,7 +62,7 @@ def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperConfig) -> KX2GripperL t: gripper tooling (user-supplied geometry). Returns: KX2GripperLocation with the gripper clamp point and a wrist sign - derived from the joint configuration (J4 ≥ 0 → "ccw", else "cw"). + derived from the joint configuration (J4 >= 0 -> "ccw", else "cw"). """ r = c.wrist_offset + c.elbow_offset + c.elbow_zero_offset + joints[Axis.ELBOW] sh_deg = joints[Axis.SHOULDER] @@ -78,17 +78,14 @@ def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperConfig) -> KX2GripperL if yaw_deg < -180.0: yaw_deg += 360.0 - # Wrist -> gripper: inverse of the gripper -> wrist translation in ik, - # so callers observe the gripper clamp point symmetric with what they - # pass into ik. Sign tracks which finger is the radial "front". yaw = radians(yaw_deg) gl = t.length if t.finger_side == "barcode_reader" else -t.length - gripper_x = wrist_x + gl * sin(yaw) - gripper_y = wrist_y - gl * cos(yaw) - gripper_z = wrist_z - t.z_offset - return KX2GripperLocation( - location=Coordinate(x=gripper_x, y=gripper_y, z=gripper_z), + location=Coordinate( + x=wrist_x + gl * sin(yaw), + y=wrist_y - gl * cos(yaw), + z=wrist_z - t.z_offset, + ), rotation=Rotation(z=yaw_deg), wrist="ccw" if joints[Axis.WRIST] >= 0 else "cw", ) From e6bdaddf54f7d08b001bd4f51a7a13ce37bd21e4 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 28 Apr 2026 22:57:09 -0700 Subject: [PATCH 52/93] KX2: poll setpoint_ack in motors_move_start to fix dropped moves MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Profile Position Mode requires a CiA 402 handshake on every new setpoint: drop CW bit 4, wait for SW bit 12 (setpoint_ack) to clear, raise CW bit 4, wait for bit 12 to rise. The previous implementation sent CW=47 then CW=63 back-to-back without checking bit 12, so ~5-30% of single-axis moves silently dropped — the rising edge landed before the drive had cleared bit 12 from the prior trigger, and the new setpoint was never latched. Multi-axis moves accidentally worked because the per-axis SDO writes between the two SYNCs gave the drive enough time. Now we run the handshake per drive with up to 4 retries, and raise CanError if bit 12 never goes high after the rising edge (rather than letting motion silently drop). Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/driver.py | 80 +++++++++++++++++++++++++++++++++--- 1 file changed, 74 insertions(+), 6 deletions(-) diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index f4200ef4a96..133785a1847 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -886,13 +886,81 @@ async def _poll_axis(nid: int) -> None: async def motors_move_start( self, node_ids: List[int], *, relative: bool = False ) -> None: + # CiA 402 Profile Position Mode trigger handshake. Per drive: + # 1. CW bit 4 = 0 (new_setpoint cleared) -- bit 5 stays high so the + # drive treats the trigger as "change set immediately". + # 2. wait SW bit 12 (setpoint_ack) low -- drive ack of step 1. + # 3. CW bit 4 = 1 -- rising edge latches 0x607A; motion starts. + # 4. wait SW bit 12 high -- drive ack of step 3. If it doesn't go + # high, the rising edge was missed (RPDO/SDO race or drive busy) + # and we retry the cycle. + # Without (4) the failure rate is ~5-10% on this Elmo firmware: bit 4 + # falls and rises within milliseconds and the drive doesn't always see + # the edge. Polling bit 12 high is the only authoritative confirmation + # that the new setpoint was actually latched. relative_bit = 0x40 if relative else 0 - for i, nid in enumerate(node_ids): - last = i == (len(node_ids) - 1) - await self._control_word_set(int(nid), 47 + relative_bit, sync=last) - for i, nid in enumerate(node_ids): - last = i == (len(node_ids) - 1) - await self._control_word_set(int(nid), 47 + 0x10 + relative_bit, sync=last) + cw_low = 47 + relative_bit + cw_high = 47 + 0x10 + relative_bit + for nid in node_ids: + await self._trigger_new_setpoint(int(nid), cw_low, cw_high) + + async def _trigger_new_setpoint( + self, + node_id: int, + cw_low: int, + cw_high: int, + *, + max_attempts: int = 4, + ) -> None: + """Run the CiA 402 PPM new-setpoint handshake on one drive. + + Each attempt: drop CW bit 4, wait SW bit 12 low, set CW bit 4, wait + SW bit 12 high. Retries up to ``max_attempts`` if bit 12 doesn't go + high (= drive missed the rising edge). Raises on persistent failure + rather than letting motion silently drop.""" + for attempt in range(1, max_attempts + 1): + await self._control_word_set(node_id, cw_low, sync=True) + cleared = await self._wait_setpoint_ack(node_id, want_high=False) + if not cleared: + logger.debug( + "node %d: setpoint_ack didn't clear (attempt %d/%d)", + node_id, attempt, max_attempts, + ) + continue + await self._control_word_set(node_id, cw_high, sync=True) + raised = await self._wait_setpoint_ack(node_id, want_high=True) + if raised: + if attempt > 1: + logger.debug( + "node %d: new setpoint accepted on attempt %d", node_id, attempt + ) + return + logger.debug( + "node %d: setpoint_ack didn't go high (attempt %d/%d); retrying", + node_id, attempt, max_attempts, + ) + raise CanError( + f"node {node_id}: drive did not accept new PPM setpoint after " + f"{max_attempts} attempts (SW bit 12 never went high after CW bit 4 " + f"rising edge)" + ) + + async def _wait_setpoint_ack( + self, node_id: int, *, want_high: bool, timeout: float = 0.05 + ) -> bool: + """Poll 0x6041 bit 12 until it matches ``want_high`` (or timeout). + + Returns True on success, False on timeout. 50ms is plenty: this drive + flips bit 12 within a servo cycle (~1-2ms) when it sees the edge.""" + assert self._loop is not None + deadline = self._loop.time() + timeout + while self._loop.time() < deadline: + raw = await self._can_sdo_upload(node_id, 0x60, 0x41, 0x00) + sw = int.from_bytes(raw[:2], "little") + if bool(sw & (1 << 12)) == want_high: + return True + await asyncio.sleep(0.001) + return False async def user_program_run( self, From d053f29b4b77760dd9d8cb2740544f3eb3d1926b Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 15:19:50 -0700 Subject: [PATCH 53/93] KX2: motion lock, int(axis) at driver boundary, fix params silent drop - Reentrant _motion_guard (asyncio.Lock + task-identity reentry) wraps every motion-mutating capability method. Compound ops hold the guard for their full duration; leaves use _locked variants. halt() bypasses. - find_z spawns its move task using _motors_move_joint_locked since the spawned task isn't the lock owner. - Cast int(axis) at every driver boundary (home_motor and the homing helpers, _on_setup motor_enable loop, motor_get_current_position, read_input, halt, drive_set_move_count_parameters). Driver contract is node_id: int; Axis(IntEnum)==10 would collide with _GROUP_NODE_ID. - Add PickUpParams / DropParams so pick_up_at_*/drop_at_* stop forwarding the wrong-shape backend_params to close_gripper/open_gripper (the silent drop of check_plate_gripped). Leaf methods keep PLR's isinstance-or-default convention. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/arm_backend.py | 626 ++++++++++++++++++------------ 1 file changed, 369 insertions(+), 257 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 37d8509a7f1..20375cb4380 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -4,6 +4,7 @@ import math import time import warnings +from contextlib import asynccontextmanager from enum import IntEnum from typing import Dict, List, Optional, Union @@ -80,6 +81,28 @@ def __init__( ) self._config: Optional[KX2Config] = None self._freedrive_axes: List[int] = [] + # Reentrant motion guard. Two callers staging move setpoints + # (target/vel/accel SDOs) on the same drives interleave and the drives + # execute some random mix. Compound ops (pick_up_at_location, + # find_z_with_proximity_sensor, home_motor) hold this for their full + # duration; the inner motion primitive (motors_move_joint / + # motors_move_absolute_execute) re-enters via _motion_owner. halt() + # deliberately skips the lock — emergency-stop must interrupt regardless. + self._motion_lock = asyncio.Lock() + self._motion_owner: Optional[asyncio.Task] = None + + @asynccontextmanager + async def _motion_guard(self): + current = asyncio.current_task() + if self._motion_owner is current: + yield + return + async with self._motion_lock: + self._motion_owner = current + try: + yield + finally: + self._motion_owner = None async def _on_setup(self, backend_params: Optional[BackendParams] = None): # Driver has already brought CAN up (connect + node discovery + PDO @@ -103,7 +126,7 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): ) for axis in MOTION_AXES: - await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) + await self.driver.motor_enable(node_id=int(axis), state=True, use_ds402=True) await self.servo_gripper_initialize() except BaseException: try: @@ -128,20 +151,21 @@ async def get_estop_state(self) -> bool: return (not b14) and (not b15) async def _motor_set_homed_status(self, axis: Axis, status: HomeStatus) -> None: - await self.driver.write(axis, "UI", 3, int(status)) + await self.driver.write(int(axis), "UI", 3, int(status)) async def motor_get_homed_status(self, axis: Axis) -> HomeStatus: - return HomeStatus(await self.driver.query_int(axis, "UI", 3)) + return HomeStatus(await self.driver.query_int(int(axis), "UI", 3)) async def _motor_reset_encoder_position(self, axis: Axis, position: float) -> None: - await self.driver.write(axis, "HM", 1, 0) - await self.driver.write(axis, "HM", 3, 0) - await self.driver.write(axis, "HM", 4, 0) - await self.driver.write(axis, "HM", 5, 0) + nid = int(axis) + await self.driver.write(nid, "HM", 1, 0) + await self.driver.write(nid, "HM", 3, 0) + await self.driver.write(nid, "HM", 4, 0) + await self.driver.write(nid, "HM", 5, 0) # Old code packed `position` as int32 via `int(round(float(str(position))))`; # preserve that rounding semantic for callers that pass fractional values. - await self.driver.write(axis, "HM", 2, int(round(position))) - await self.driver.write(axis, "HM", 1, 1) + await self.driver.write(nid, "HM", 2, int(round(position))) + await self.driver.write(nid, "HM", 1, 1) async def _motor_hard_stop_search( self, @@ -152,7 +176,11 @@ async def _motor_hard_stop_search( hs_pe: int, timeout: float, ) -> None: - nid = axis + # int() at every driver call: driver's contract is `node_id: int`. Axis + # is IntEnum so it works by accident, but Axis.value collides with + # _GROUP_NODE_ID == 10 if anyone ever adds Axis.AUX = 10 — broadcast + # would silently dispatch to all motion nodes. + nid = int(axis) await self.driver.write(nid, "ER", 3, max_pe * 10) await self.driver.write(nid, "AC", 0, srch_acc) await self.driver.write(nid, "DC", 0, srch_acc) @@ -187,7 +215,7 @@ async def _motor_index_search( positive_direction: bool, timeout: float, ) -> tuple: - nid = axis + nid = int(axis) await self.driver.write(nid, "HM", 1, 0) one_revolution = await self.driver.query_int(nid, "CA", 18) @@ -229,56 +257,57 @@ async def home_motor( offset_acc: int, timeout: float, ) -> None: - nid = axis + nid = int(axis) - left = await self.driver.query_int(nid, "CA", 41) - if left == 24: - raise RuntimeError("Error 43") + async with self._motion_guard(): + left = await self.driver.query_int(nid, "CA", 41) + if left == 24: + raise RuntimeError("Error 43") - try: - await self._motor_hard_stop_search(nid, srch_vel, srch_acc, max_pe, hs_pe, timeout) - except Exception as e: - fault = await self.driver.motor_get_fault(nid) - if fault is not None: - raise RuntimeError(fault) - raise e - - await self.driver.motor_enable(node_id=nid, state=True, use_ds402=Axis(nid).is_motion) - - await self.motors_move_absolute_execute( - plan=_MotorsMovePlan( - moves=[ - _MotorMoveParam( - node_id=nid, - position=hs_offset, - velocity=offset_vel, - acceleration=offset_acc, - relative=False, - direction=_JointMoveDirection.ShortestWay, - ) - ], + try: + await self._motor_hard_stop_search(axis, srch_vel, srch_acc, max_pe, hs_pe, timeout) + except Exception as e: + fault = await self.driver.motor_get_fault(nid) + if fault is not None: + raise RuntimeError(fault) + raise e + + await self.driver.motor_enable(node_id=nid, state=True, use_ds402=axis.is_motion) + + await self._motors_move_absolute_execute_locked( + plan=_MotorsMovePlan( + moves=[ + _MotorMoveParam( + node_id=nid, + position=hs_offset, + velocity=offset_vel, + acceleration=offset_acc, + relative=False, + direction=_JointMoveDirection.ShortestWay, + ) + ], + ) ) - ) - is_positive = hs_offset > 0 - await self._motor_index_search(nid, abs(srch_vel), srch_acc, is_positive, timeout) - - await self.motors_move_absolute_execute( - plan=_MotorsMovePlan( - moves=[ - _MotorMoveParam( - node_id=nid, - position=ind_offset, - velocity=offset_vel, - acceleration=offset_acc, - relative=False, - direction=_JointMoveDirection.ShortestWay, - ) - ] + is_positive = hs_offset > 0 + await self._motor_index_search(axis, abs(srch_vel), srch_acc, is_positive, timeout) + + await self._motors_move_absolute_execute_locked( + plan=_MotorsMovePlan( + moves=[ + _MotorMoveParam( + node_id=nid, + position=ind_offset, + velocity=offset_vel, + acceleration=offset_acc, + relative=False, + direction=_JointMoveDirection.ShortestWay, + ) + ] + ) ) - ) - await self._motor_reset_encoder_position(nid, home_pos) - await self._motor_set_homed_status(nid, HomeStatus.Homed) + await self._motor_reset_encoder_position(axis, home_pos) + await self._motor_set_homed_status(axis, HomeStatus.Homed) # -- servo gripper ------------------------------------------------------ @@ -387,13 +416,14 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: ) async def servo_gripper_close(self, closed_position: int = 0, check_plate_gripped=True) -> None: - await self.motors_move_joint({Axis.SERVO_GRIPPER: closed_position}) - - if check_plate_gripped: - await self.check_plate_gripped() + async with self._motion_guard(): + await self._motors_move_joint_locked({Axis.SERVO_GRIPPER: closed_position}) + if check_plate_gripped: + await self.check_plate_gripped() async def servo_gripper_open(self, open_position: float) -> None: - await self.motors_move_joint({Axis.SERVO_GRIPPER: open_position}) + async with self._motion_guard(): + await self._motors_move_joint_locked({Axis.SERVO_GRIPPER: open_position}) async def drive_set_move_count_parameters( self, @@ -418,8 +448,8 @@ async def drive_set_move_count_parameters( else: pairs = zip(self._cfg.axes, travel) - for node_id, dist in pairs: - await self.driver.write(node_id, "UF", 5, float(dist)) + for axis_key, dist in pairs: + await self.driver.write(int(axis_key), "UF", 5, float(dist)) # LastMaintenancePerformed -> Z axis, UF index 6 await self.driver.write(z, "UF", 6, float(last_maintenance_performed)) @@ -607,7 +637,9 @@ def convert_elbow_angle_to_position(self, elbow_angle_deg: float) -> float: return elbow_pos async def motor_get_current_position(self, axis: Axis) -> float: - raw = await self.driver.motor_get_current_position(node_id=axis, pu=self._cfg.axes[axis].unlimited_travel) + raw = await self.driver.motor_get_current_position( + node_id=int(axis), pu=self._cfg.axes[axis].unlimited_travel, + ) c = self._cfg.axes[axis].motor_conversion_factor if axis == Axis.ELBOW: return self.convert_elbow_angle_to_position(elbow_angle_deg=raw / c) @@ -619,7 +651,7 @@ async def motor_get_current_position(self, axis: Axis) -> float: return raw / c async def read_input(self, axis: Axis, input_num: int) -> bool: - return await self.driver.read_input(node_id=axis, input_num=0x10 + input_num) + return await self.driver.read_input(node_id=int(axis), input_num=0x10 + input_num) # IR breakbeam between the gripper fingers, wired to the Z-drive's IO. # True = beam interrupted (object present). @@ -663,59 +695,68 @@ async def find_z_with_proximity_sensor( max_gripper_speed=max_gripper_speed, max_gripper_acceleration=max_gripper_acceleration, ) - # Pre-flight: force the drive back to Op Enabled. A prior failed search - # could have left it in Fault/Quick Stop where new moves silently fail - # (Z barely moves). Idempotent if the drive is already healthy. - await self.driver.motor_enable(node_id=int(Axis.Z), state=True, use_ds402=True) - if z_start is not None: - await self.move_to_joint_position({Axis.Z: z_start}, backend_params=move_params) - z0 = await self.motor_get_current_position(Axis.Z) - if await self.read_proximity_sensor(): - raise RuntimeError( - f"proximity sensor already tripped at start (Z {z0:.2f}); " - f"clear the gripper or raise z_start before searching" + # Hold the motion guard for the whole find: a parallel caller's move + # could land between IL=StopForward and the descent, or between the + # descent and IL restore. The spawned move task uses + # `_motors_move_joint_locked` (no guard reacquire) since asyncio.Lock + # is task-aware and our reentrance check uses task identity — the + # spawned task isn't the owner. + async with self._motion_guard(): + # Pre-flight: force the drive back to Op Enabled. A prior failed + # search could have left it in Fault/Quick Stop where new moves + # silently fail (Z barely moves). Idempotent if drive is healthy. + await self.driver.motor_enable(node_id=int(Axis.Z), state=True, use_ds402=True) + if z_start is not None: + await self._motors_move_joint_locked({Axis.Z: z_start}, params=move_params) + z0 = await self.motor_get_current_position(Axis.Z) + if await self.read_proximity_sensor(): + raise RuntimeError( + f"proximity sensor already tripped at start (Z {z0:.2f}); " + f"clear the gripper or raise z_start before searching" + ) + await self.driver.configure_input_logic( + int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, _InputLogic.StopForward, ) - await self.driver.configure_input_logic( - int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, _InputLogic.StopForward, - ) - move_task = asyncio.create_task( - self.move_to_joint_position({Axis.Z: z0 - max_descent}, backend_params=move_params) - ) - tripped = False - try: - # The drive halts itself via IL the moment the beam breaks. We poll the - # sensor in parallel so we can stop waiting for "move done" (which never - # arrives — the drive halted short of target). - while not move_task.done(): - if await self.read_proximity_sensor(): - tripped = True - break - await asyncio.sleep(0.01) - move_task.cancel() - try: - await move_task - except (asyncio.CancelledError, CanError): - pass - finally: - # Match C# search cleanup (KX2RobotControl.cs:8650-8658): halt motor - # FIRST, then restore IL. Reverse order would let the drive surge toward - # the unreached target during the gap. - try: - await self.driver.motor_stop(int(Axis.Z)) - except Exception as e: - logger.warning("find_z: motor_stop failed: %s", e) - try: - await self.driver.configure_input_logic( - int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose, + move_task = asyncio.create_task( + self._motors_move_joint_locked( + {Axis.Z: z0 - max_descent}, params=move_params ) - except Exception as e: - logger.warning("find_z: IL restore failed: %s", e) - if not tripped: - z_end = await self.motor_get_current_position(Axis.Z) - raise RuntimeError( - f"proximity sensor never tripped within {max_descent} (Z {z0:.2f} -> {z_end:.2f})" ) - return await self.motor_get_current_position(Axis.Z) + tripped = False + try: + # The drive halts itself via IL the moment the beam breaks. We poll + # the sensor in parallel so we can stop waiting for "move done" + # (which never arrives — the drive halted short of target). + while not move_task.done(): + if await self.read_proximity_sensor(): + tripped = True + break + await asyncio.sleep(0.01) + move_task.cancel() + try: + await move_task + except (asyncio.CancelledError, CanError): + pass + finally: + # Match C# search cleanup (KX2RobotControl.cs:8650-8658): halt + # motor FIRST, then restore IL. Reverse order would let the drive + # surge toward the unreached target during the gap. + try: + await self.driver.motor_stop(int(Axis.Z)) + except Exception as e: + logger.warning("find_z: motor_stop failed: %s", e) + try: + await self.driver.configure_input_logic( + int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose, + ) + except Exception as e: + logger.warning("find_z: IL restore failed: %s", e) + if not tripped: + z_end = await self.motor_get_current_position(Axis.Z) + raise RuntimeError( + f"proximity sensor never tripped within {max_descent} (Z {z0:.2f} -> {z_end:.2f})" + ) + return await self.motor_get_current_position(Axis.Z) @staticmethod def _wrap_to_range(x: float, lo: float, hi: float) -> float: @@ -1019,15 +1060,30 @@ async def motors_move_joint( cmd_pos: Dict[Axis, float], params: Optional["KX2ArmBackend.JointMoveParams"] = None, ) -> None: + async with self._motion_guard(): + await self._motors_move_joint_locked(cmd_pos, params) + + async def _motors_move_joint_locked( + self, + cmd_pos: Dict[Axis, float], + params: Optional["KX2ArmBackend.JointMoveParams"] = None, + ) -> None: + """Caller MUST hold _motion_guard. Used by find_z's spawned poll/move + task, which can't re-enter the guard from a fresh asyncio Task.""" logger.debug("motors_move_joint cmd_pos=%s", cmd_pos) plan = await self.calculate_move_abs_all_axes(cmd_pos=cmd_pos, params=params) if plan is None: # if every axis is skipped, exit return - await self.motors_move_absolute_execute(plan) + await self._motors_move_absolute_execute_locked(plan) async def motors_move_absolute_execute(self, plan: _MotorsMovePlan) -> None: + async with self._motion_guard(): + await self._motors_move_absolute_execute_locked(plan) + + async def _motors_move_absolute_execute_locked(self, plan: _MotorsMovePlan) -> None: + """Caller MUST hold _motion_guard.""" await self.driver.pvt_select_mode(False) if logger.isEnabledFor(logging.DEBUG): @@ -1110,11 +1166,27 @@ class JointMoveParams(BackendParams): class GripParams(BackendParams): check_plate_gripped: bool = True + @dataclasses.dataclass + class PickUpParams(BackendParams): + """Move-leg caps + grip-leg knob for the pick_up_* compounds.""" + max_gripper_speed: Optional[float] = None + max_gripper_acceleration: Optional[float] = None + check_plate_gripped: bool = True + + @dataclasses.dataclass + class DropParams(BackendParams): + """Move-leg caps for the drop_at_* compounds.""" + max_gripper_speed: Optional[float] = None + max_gripper_acceleration: Optional[float] = None + async def halt(self, backend_params: Optional[BackendParams] = None) -> None: # Fire MO=0 on every motion axis concurrently — serial halts let later # axes coast for the duration of the earlier SDOs. + # + # Deliberately NOT guarded by _motion_guard: emergency-stop must + # interrupt regardless of who's holding the lock. await asyncio.gather( - *(self.driver.motor_emergency_stop(node_id=axis) for axis in MOTION_AXES) + *(self.driver.motor_emergency_stop(node_id=int(axis)) for axis in MOTION_AXES) ) async def park(self, backend_params: Optional[BackendParams] = None) -> None: @@ -1132,16 +1204,18 @@ async def request_gripper_location( async def open_gripper( self, gripper_width: float, backend_params: Optional[BackendParams] = None ) -> None: - await self.motors_move_joint({Axis.SERVO_GRIPPER: gripper_width}) + async with self._motion_guard(): + await self._motors_move_joint_locked({Axis.SERVO_GRIPPER: gripper_width}) async def close_gripper( self, gripper_width: float, backend_params: Optional[BackendParams] = None ) -> None: if not isinstance(backend_params, KX2ArmBackend.GripParams): backend_params = KX2ArmBackend.GripParams() - await self.motors_move_joint({Axis.SERVO_GRIPPER: gripper_width}) - if backend_params.check_plate_gripped: - await self.check_plate_gripped() + async with self._motion_guard(): + await self._motors_move_joint_locked({Axis.SERVO_GRIPPER: gripper_width}) + if backend_params.check_plate_gripped: + await self.check_plate_gripped() async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool: pos = await self.motor_get_current_position(Axis.SERVO_GRIPPER) @@ -1159,12 +1233,13 @@ async def move_to_gripper_location( """ if not isinstance(backend_params, KX2ArmBackend.CartesianMoveParams): backend_params = KX2ArmBackend.CartesianMoveParams() - joint_pos = await self._cart_to_joints(pose) - joint_params = KX2ArmBackend.JointMoveParams( - max_gripper_speed=backend_params.max_gripper_speed, - max_gripper_acceleration=backend_params.max_gripper_acceleration, - ) - await self.motors_move_joint(cmd_pos=joint_pos, params=joint_params) + async with self._motion_guard(): + joint_pos = await self._cart_to_joints(pose) + joint_params = KX2ArmBackend.JointMoveParams( + max_gripper_speed=backend_params.max_gripper_speed, + max_gripper_acceleration=backend_params.max_gripper_acceleration, + ) + await self._motors_move_joint_locked(cmd_pos=joint_pos, params=joint_params) async def move_to_location( self, @@ -1184,8 +1259,16 @@ async def pick_up_at_location( resource_width: float, backend_params: Optional[BackendParams] = None, ) -> None: - await self.move_to_location(location, direction, backend_params=backend_params) - await self.close_gripper(resource_width, backend_params=backend_params) + if not isinstance(backend_params, KX2ArmBackend.PickUpParams): + backend_params = KX2ArmBackend.PickUpParams() + move_params = KX2ArmBackend.CartesianMoveParams( + max_gripper_speed=backend_params.max_gripper_speed, + max_gripper_acceleration=backend_params.max_gripper_acceleration, + ) + grip_params = KX2ArmBackend.GripParams(check_plate_gripped=backend_params.check_plate_gripped) + async with self._motion_guard(): + await self.move_to_location(location, direction, backend_params=move_params) + await self.close_gripper(resource_width, backend_params=grip_params) async def drop_at_location( self, @@ -1194,8 +1277,15 @@ async def drop_at_location( resource_width: float, backend_params: Optional[BackendParams] = None, ) -> None: - await self.move_to_location(location, direction, backend_params=backend_params) - await self.open_gripper(resource_width, backend_params=backend_params) + if not isinstance(backend_params, KX2ArmBackend.DropParams): + backend_params = KX2ArmBackend.DropParams() + move_params = KX2ArmBackend.CartesianMoveParams( + max_gripper_speed=backend_params.max_gripper_speed, + max_gripper_acceleration=backend_params.max_gripper_acceleration, + ) + async with self._motion_guard(): + await self.move_to_location(location, direction, backend_params=move_params) + await self.open_gripper(resource_width) async def move_to_joint_position( self, @@ -1204,8 +1294,9 @@ async def move_to_joint_position( ) -> None: if not isinstance(backend_params, KX2ArmBackend.JointMoveParams): backend_params = KX2ArmBackend.JointMoveParams() - cmd_pos = {Axis(int(k)): float(v) for k, v in position.items()} - await self.motors_move_joint(cmd_pos=cmd_pos, params=backend_params) + async with self._motion_guard(): + cmd_pos = {Axis(int(k)): float(v) for k, v in position.items()} + await self._motors_move_joint_locked(cmd_pos=cmd_pos, params=backend_params) async def pick_up_at_joint_position( self, @@ -1213,8 +1304,16 @@ async def pick_up_at_joint_position( resource_width: float, backend_params: Optional[BackendParams] = None, ) -> None: - await self.move_to_joint_position(position, backend_params=backend_params) - await self.close_gripper(resource_width, backend_params=backend_params) + if not isinstance(backend_params, KX2ArmBackend.PickUpParams): + backend_params = KX2ArmBackend.PickUpParams() + move_params = KX2ArmBackend.JointMoveParams( + max_gripper_speed=backend_params.max_gripper_speed, + max_gripper_acceleration=backend_params.max_gripper_acceleration, + ) + grip_params = KX2ArmBackend.GripParams(check_plate_gripped=backend_params.check_plate_gripped) + async with self._motion_guard(): + await self.move_to_joint_position(position, backend_params=move_params) + await self.close_gripper(resource_width, backend_params=grip_params) async def drop_at_joint_position( self, @@ -1222,8 +1321,15 @@ async def drop_at_joint_position( resource_width: float, backend_params: Optional[BackendParams] = None, ) -> None: - await self.move_to_joint_position(position, backend_params=backend_params) - await self.open_gripper(resource_width, backend_params=backend_params) + if not isinstance(backend_params, KX2ArmBackend.DropParams): + backend_params = KX2ArmBackend.DropParams() + move_params = KX2ArmBackend.JointMoveParams( + max_gripper_speed=backend_params.max_gripper_speed, + max_gripper_acceleration=backend_params.max_gripper_acceleration, + ) + async with self._motion_guard(): + await self.move_to_joint_position(position, backend_params=move_params) + await self.open_gripper(resource_width) async def request_joint_position( self, backend_params: Optional[BackendParams] = None @@ -1261,15 +1367,17 @@ async def start_freedrive_mode( axes: List[int] = [int(a) for a in MOTION_AXES] else: axes = [int(a) for a in free_axes] - for axis in axes: - await self.driver.motor_enable(node_id=axis, state=False, use_ds402=True) - self._freedrive_axes = axes + async with self._motion_guard(): + for axis in axes: + await self.driver.motor_enable(node_id=axis, state=False, use_ds402=True) + self._freedrive_axes = axes async def stop_freedrive_mode(self, backend_params: Optional[BackendParams] = None) -> None: - axes = self._freedrive_axes or list(MOTION_AXES) - for axis in axes: - await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) - self._freedrive_axes = [] + axes: List[int] = self._freedrive_axes or [int(a) for a in MOTION_AXES] + async with self._motion_guard(): + for axis in axes: + await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) + self._freedrive_axes = [] async def very_dangerously_yeet( self, @@ -1302,125 +1410,129 @@ async def very_dangerously_yeet( driver = self.driver cfg = self._cfg - z_now = await self.motor_get_current_position(Axis.Z) - if z_now < min_z: - raise RuntimeError( - f"yeet refused: Z={z_now:.0f}mm < min_z={min_z:.0f}mm; raise the arm first" - ) + # Hold the motion guard for the whole yeet — bumped VH/SP/SD on + # SHOULDER+WRIST is a global drive-state mutation that mustn't overlap + # with another caller's move. + async with self._motion_guard(): + z_now = await self.motor_get_current_position(Axis.Z) + if z_now < min_z: + raise RuntimeError( + f"yeet refused: Z={z_now:.0f}mm < min_z={min_z:.0f}mm; raise the arm first" + ) - # Snapshot + bump VH[2]/SP[2]/SD[0] on swing axes; restore in finally. - saved_limits: Dict[Axis, dict] = {} - if bump != 1.0: - for ax in (Axis.SHOULDER, Axis.WRIST): - nid = int(ax) - s = { - "VH2": await driver.query_int(nid, "VH", 2), - "SP2": await driver.query_int(nid, "SP", 2), - "SD0": await driver.query_int(nid, "SD", 0), - "max_vel": cfg.axes[ax].max_vel, - "max_accel": cfg.axes[ax].max_accel, - } - saved_limits[ax] = s - new_vh2 = int(s["VH2"] * bump) - new_sp2 = int(s["SP2"] * bump) - new_sd0 = int(s["SD0"] * bump) - await driver.write(nid, "VH", 2, new_vh2) - await driver.write(nid, "SP", 2, new_sp2) - await driver.write(nid, "SD", 0, new_sd0) - conv = abs(cfg.axes[ax].motor_conversion_factor) - cfg.axes[ax].max_vel = (new_sp2 / 1.01) / conv - cfg.axes[ax].max_accel = (new_sd0 / 1.01) / conv + # Snapshot + bump VH[2]/SP[2]/SD[0] on swing axes; restore in finally. + saved_limits: Dict[Axis, dict] = {} + if bump != 1.0: + for ax in (Axis.SHOULDER, Axis.WRIST): + nid = int(ax) + s = { + "VH2": await driver.query_int(nid, "VH", 2), + "SP2": await driver.query_int(nid, "SP", 2), + "SD0": await driver.query_int(nid, "SD", 0), + "max_vel": cfg.axes[ax].max_vel, + "max_accel": cfg.axes[ax].max_accel, + } + saved_limits[ax] = s + new_vh2 = int(s["VH2"] * bump) + new_sp2 = int(s["SP2"] * bump) + new_sd0 = int(s["SD0"] * bump) + await driver.write(nid, "VH", 2, new_vh2) + await driver.write(nid, "SP", 2, new_sp2) + await driver.write(nid, "SD", 0, new_sd0) + conv = abs(cfg.axes[ax].motor_conversion_factor) + cfg.axes[ax].max_vel = (new_sp2 / 1.01) / conv + cfg.axes[ax].max_accel = (new_sd0 / 1.01) / conv - try: - pickup_pose = await self.request_joint_position() - - # Auto-windup: rotate wrist to the inward angle (opposite of outward). - wrist_inward = 0.0 if self._gripper_config.finger_side == "barcode_reader" else 180.0 - while wrist_inward - pickup_pose[Axis.WRIST] > 180.0: - wrist_inward -= 360.0 - while wrist_inward - pickup_pose[Axis.WRIST] < -180.0: - wrist_inward += 360.0 - await self.motors_move_joint( - cmd_pos={Axis.WRIST: wrist_inward}, - params=KX2ArmBackend.JointMoveParams( - max_gripper_speed=_YEET_WINDUP_GRIPPER_SPEED, - max_gripper_acceleration=_YEET_WINDUP_GRIPPER_ACC, - ), - ) + try: + pickup_pose = await self.request_joint_position() + + # Auto-windup: rotate wrist to the inward angle (opposite of outward). + wrist_inward = 0.0 if self._gripper_config.finger_side == "barcode_reader" else 180.0 + while wrist_inward - pickup_pose[Axis.WRIST] > 180.0: + wrist_inward -= 360.0 + while wrist_inward - pickup_pose[Axis.WRIST] < -180.0: + wrist_inward += 360.0 + await self._motors_move_joint_locked( + cmd_pos={Axis.WRIST: wrist_inward}, + params=KX2ArmBackend.JointMoveParams( + max_gripper_speed=_YEET_WINDUP_GRIPPER_SPEED, + max_gripper_acceleration=_YEET_WINDUP_GRIPPER_ACC, + ), + ) - joints0 = await self.request_joint_position() + joints0 = await self.request_joint_position() - # Outward wrist = kinematic target (180° barcode_reader, 0° proximity). - wrist_outward = 180.0 if self._gripper_config.finger_side == "barcode_reader" else 0.0 - while wrist_outward - joints0[Axis.WRIST] > 180.0: - wrist_outward -= 360.0 - while wrist_outward - joints0[Axis.WRIST] < -180.0: - wrist_outward += 360.0 + # Outward wrist = kinematic target (180° barcode_reader, 0° proximity). + wrist_outward = 180.0 if self._gripper_config.finger_side == "barcode_reader" else 0.0 + while wrist_outward - joints0[Axis.WRIST] > 180.0: + wrist_outward -= 360.0 + while wrist_outward - joints0[Axis.WRIST] < -180.0: + wrist_outward += 360.0 - sh_move, sh_t_acc, sh_t_total, _ = await _yeet_build_axis_move( - self, Axis.SHOULDER, - joints0[Axis.SHOULDER], joints0[Axis.SHOULDER] + _YEET_SHOULDER_SWING_DEG, - ) - wr_move, wr_t_acc, wr_t_total, _ = await _yeet_build_axis_move( - self, Axis.WRIST, joints0[Axis.WRIST], wrist_outward, - ) - sh_plan = _MotorsMovePlan(moves=[sh_move], move_time=sh_t_total) - wr_plan = _MotorsMovePlan(moves=[wr_move], move_time=wr_t_total) - - # Release fires inside shoulder cruise. Wrist trigger is delayed so its - # accel ramp finishes at release (peak ω at the gripper offset). - sh_cruise_dur = max(0.0, sh_t_total - 2 * sh_t_acc) - release_t = sh_t_acc + sh_cruise_dur * _YEET_RELEASE_FRACTION - wrist_trigger_t = max(0.0, release_t - wr_t_acc) - - sg = int(Axis.SERVO_GRIPPER) - sg_cfg = cfg.axes[Axis.SERVO_GRIPPER] - open_pos = min(_YEET_OPEN_POSITION, sg_cfg.max_travel - _YEET_OPEN_SAFETY_MARGIN) - gripper_plan = _MotorsMovePlan(moves=[_MotorMoveParam( - node_id=sg, - position=int(round(open_pos * sg_cfg.motor_conversion_factor)), - velocity=int(round(sg_cfg.max_vel * abs(sg_cfg.motor_conversion_factor))), - acceleration=int(round(sg_cfg.max_accel * abs(sg_cfg.motor_conversion_factor))), - direction=sg_cfg.joint_move_direction, - )]) - - # Pre-arm so triggers are pure control-word writes (sub-ms), not SDOs. - await _yeet_arm_plan(driver, sh_plan) - await _yeet_arm_plan(driver, wr_plan) - - await driver.motors_move_start([int(Axis.SHOULDER)]) - t0 = time.monotonic() - await asyncio.sleep(max(0.0, wrist_trigger_t - (time.monotonic() - t0))) - await driver.motors_move_start([int(Axis.WRIST)]) - - await asyncio.sleep(max(0.0, release_t - (time.monotonic() - t0))) - await self.motors_move_absolute_execute(gripper_plan) - - # Settle slack: at higher bump, drives overshoot + ring before asserting - # target-reached; tight margin trips a CanError even though throw was OK. - swing_finish_t = max(sh_t_total, wrist_trigger_t + wr_t_total) - await driver.wait_for_moves_done( - [int(Axis.SHOULDER), int(Axis.WRIST)], timeout=swing_finish_t + 5, - ) + sh_move, sh_t_acc, sh_t_total, _ = await _yeet_build_axis_move( + self, Axis.SHOULDER, + joints0[Axis.SHOULDER], joints0[Axis.SHOULDER] + _YEET_SHOULDER_SWING_DEG, + ) + wr_move, wr_t_acc, wr_t_total, _ = await _yeet_build_axis_move( + self, Axis.WRIST, joints0[Axis.WRIST], wrist_outward, + ) + sh_plan = _MotorsMovePlan(moves=[sh_move], move_time=sh_t_total) + wr_plan = _MotorsMovePlan(moves=[wr_move], move_time=wr_t_total) + + # Release fires inside shoulder cruise. Wrist trigger is delayed so its + # accel ramp finishes at release (peak ω at the gripper offset). + sh_cruise_dur = max(0.0, sh_t_total - 2 * sh_t_acc) + release_t = sh_t_acc + sh_cruise_dur * _YEET_RELEASE_FRACTION + wrist_trigger_t = max(0.0, release_t - wr_t_acc) + + sg = int(Axis.SERVO_GRIPPER) + sg_cfg = cfg.axes[Axis.SERVO_GRIPPER] + open_pos = min(_YEET_OPEN_POSITION, sg_cfg.max_travel - _YEET_OPEN_SAFETY_MARGIN) + gripper_plan = _MotorsMovePlan(moves=[_MotorMoveParam( + node_id=sg, + position=int(round(open_pos * sg_cfg.motor_conversion_factor)), + velocity=int(round(sg_cfg.max_vel * abs(sg_cfg.motor_conversion_factor))), + acceleration=int(round(sg_cfg.max_accel * abs(sg_cfg.motor_conversion_factor))), + direction=sg_cfg.joint_move_direction, + )]) + + # Pre-arm so triggers are pure control-word writes (sub-ms), not SDOs. + await _yeet_arm_plan(driver, sh_plan) + await _yeet_arm_plan(driver, wr_plan) + + await driver.motors_move_start([int(Axis.SHOULDER)]) + t0 = time.monotonic() + await asyncio.sleep(max(0.0, wrist_trigger_t - (time.monotonic() - t0))) + await driver.motors_move_start([int(Axis.WRIST)]) + + await asyncio.sleep(max(0.0, release_t - (time.monotonic() - t0))) + await self._motors_move_absolute_execute_locked(gripper_plan) + + # Settle slack: at higher bump, drives overshoot + ring before asserting + # target-reached; tight margin trips a CanError even though throw was OK. + swing_finish_t = max(sh_t_total, wrist_trigger_t + wr_t_total) + await driver.wait_for_moves_done( + [int(Axis.SHOULDER), int(Axis.WRIST)], timeout=swing_finish_t + 5, + ) - await self.motors_move_joint( - cmd_pos={ - Axis.SHOULDER: pickup_pose[Axis.SHOULDER], - Axis.WRIST: pickup_pose[Axis.WRIST], - }, - params=KX2ArmBackend.JointMoveParams( - max_gripper_speed=_YEET_RETURN_GRIPPER_SPEED, - max_gripper_acceleration=_YEET_RETURN_GRIPPER_ACC, - ), - ) - finally: - for ax, s in saved_limits.items(): - nid = int(ax) - await driver.write(nid, "VH", 2, s["VH2"]) - await driver.write(nid, "SP", 2, s["SP2"]) - await driver.write(nid, "SD", 0, s["SD0"]) - cfg.axes[ax].max_vel = s["max_vel"] - cfg.axes[ax].max_accel = s["max_accel"] + await self._motors_move_joint_locked( + cmd_pos={ + Axis.SHOULDER: pickup_pose[Axis.SHOULDER], + Axis.WRIST: pickup_pose[Axis.WRIST], + }, + params=KX2ArmBackend.JointMoveParams( + max_gripper_speed=_YEET_RETURN_GRIPPER_SPEED, + max_gripper_acceleration=_YEET_RETURN_GRIPPER_ACC, + ), + ) + finally: + for ax, s in saved_limits.items(): + nid = int(ax) + await driver.write(nid, "VH", 2, s["VH2"]) + await driver.write(nid, "SP", 2, s["SP2"]) + await driver.write(nid, "SD", 0, s["SD0"]) + cfg.axes[ax].max_vel = s["max_vel"] + cfg.axes[ax].max_accel = s["max_accel"] class _MotionLimits(Dict[Axis, tuple]): From ac0d6d14fad170a7b3befb173f6cd7ebe754010e Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 15:31:55 -0700 Subject: [PATCH 54/93] =?UTF-8?q?KX2:=20fix=20public=20surface=20=E2=80=94?= =?UTF-8?q?=20rename=20leaked=20types,=20fix=20=5F=5Finit=5F=5F=20exports?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Three driver types were underscore-prefixed but appeared in public backend signatures or public dataclass field annotations — the underscore was a lie. Promoted to public: _JointMoveDirection -> JointMoveDirection (AxisConfig.joint_move_direction) _MotorMoveParam -> MotorMoveParam (MotorsMovePlan.moves) _MotorsMovePlan -> MotorsMovePlan (motors_move_absolute_execute) _ElmoObjectDataType and _InputLogic stay underscored — they only appear in method bodies, not in any backend public signature. __init__.py: drop KX2Driver, KX2BarcodeReaderBackend, KX2BarcodeReaderDriver (non-user-facing). Add AxisConfig, GripperConfig, ServoGripperConfig, GripperFingerSide, IKError. Add __all__. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/__init__.py | 30 +++++++++--- pylabrobot/paa/kx2/arm_backend.py | 66 +++++++++++++------------- pylabrobot/paa/kx2/config.py | 4 +- pylabrobot/paa/kx2/driver.py | 12 ++--- pylabrobot/paa/kx2/kinematics_tests.py | 4 +- 5 files changed, 66 insertions(+), 50 deletions(-) diff --git a/pylabrobot/paa/kx2/__init__.py b/pylabrobot/paa/kx2/__init__.py index b628da773f4..72d4c758926 100644 --- a/pylabrobot/paa/kx2/__init__.py +++ b/pylabrobot/paa/kx2/__init__.py @@ -1,10 +1,26 @@ from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend -from pylabrobot.paa.kx2.barcode_reader import ( - KX2BarcodeReader, - KX2BarcodeReaderBackend, - KX2BarcodeReaderDriver, +from pylabrobot.paa.kx2.barcode_reader import KX2BarcodeReader +from pylabrobot.paa.kx2.config import ( + Axis, + AxisConfig, + GripperConfig, + GripperFingerSide, + KX2Config, + ServoGripperConfig, ) -from pylabrobot.paa.kx2.config import Axis, KX2Config -from pylabrobot.paa.kx2.driver import KX2Driver -from pylabrobot.paa.kx2.kinematics import KX2GripperLocation +from pylabrobot.paa.kx2.kinematics import IKError, KX2GripperLocation from pylabrobot.paa.kx2.kx2 import KX2 + +__all__ = [ + "Axis", + "AxisConfig", + "GripperConfig", + "GripperFingerSide", + "IKError", + "KX2", + "KX2ArmBackend", + "KX2BarcodeReader", + "KX2Config", + "KX2GripperLocation", + "ServoGripperConfig", +] diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 20375cb4380..01c363aa48e 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -29,10 +29,10 @@ CanError, _ElmoObjectDataType, _InputLogic, - _JointMoveDirection, + JointMoveDirection, KX2Driver, - _MotorMoveParam, - _MotorsMovePlan, + MotorMoveParam, + MotorsMovePlan, ) from pylabrobot.resources import Coordinate, Rotation @@ -275,15 +275,15 @@ async def home_motor( await self.driver.motor_enable(node_id=nid, state=True, use_ds402=axis.is_motion) await self._motors_move_absolute_execute_locked( - plan=_MotorsMovePlan( + plan=MotorsMovePlan( moves=[ - _MotorMoveParam( + MotorMoveParam( node_id=nid, position=hs_offset, velocity=offset_vel, acceleration=offset_acc, relative=False, - direction=_JointMoveDirection.ShortestWay, + direction=JointMoveDirection.ShortestWay, ) ], ) @@ -293,15 +293,15 @@ async def home_motor( await self._motor_index_search(axis, abs(srch_vel), srch_acc, is_positive, timeout) await self._motors_move_absolute_execute_locked( - plan=_MotorsMovePlan( + plan=MotorsMovePlan( moves=[ - _MotorMoveParam( + MotorMoveParam( node_id=nid, position=ind_offset, velocity=offset_vel, acceleration=offset_acc, relative=False, - direction=_JointMoveDirection.ShortestWay, + direction=JointMoveDirection.ShortestWay, ) ] ) @@ -521,13 +521,13 @@ async def _read_axis_config(self, nid: int) -> AxisConfig: vh3 = await self.driver.query_int(nid, "VH", 3) vl3 = await self.driver.query_int(nid, "VL", 3) - joint_move_direction = _JointMoveDirection.Normal + joint_move_direction = JointMoveDirection.Normal if (xm1 == 0 and xm2 == 0) or (xm1 <= vl3 and xm2 >= vh3): unlimited_travel = False elif xm1 > vl3 and xm2 < vh3: unlimited_travel = True if Axis(nid).is_motion: - joint_move_direction = _JointMoveDirection.ShortestWay + joint_move_direction = JointMoveDirection.ShortestWay else: raise CanError( f"Invalid travel limits or modulo settings for axis {nid}: " @@ -801,7 +801,7 @@ async def calculate_move_abs_all_axes( self, cmd_pos: Dict[Axis, float], params: Optional["KX2ArmBackend.JointMoveParams"] = None, - ) -> Optional[_MotorsMovePlan]: + ) -> Optional[MotorsMovePlan]: if params is None: params = KX2ArmBackend.JointMoveParams() target = cmd_pos.copy() @@ -879,7 +879,7 @@ async def calculate_move_abs_all_axes( for ax in axes: if ( self._cfg.axes[ax].unlimited_travel - and self._cfg.axes[ax].joint_move_direction != _JointMoveDirection.Normal + and self._cfg.axes[ax].joint_move_direction != JointMoveDirection.Normal ): target[ax] = self._wrap_to_range(target[ax], self._cfg.axes[ax].min_travel, self._cfg.axes[ax].max_travel) @@ -900,11 +900,11 @@ async def calculate_move_abs_all_axes( d = target_cmd_units[ax] - curr_cmd_units.get(ax, 0.0) span = self._cfg.axes[ax].max_travel - self._cfg.axes[ax].min_travel dir_ = self._cfg.axes[ax].joint_move_direction - if dir_ == _JointMoveDirection.Clockwise and d > 0.01: + if dir_ == JointMoveDirection.Clockwise and d > 0.01: d -= span - elif dir_ == _JointMoveDirection.Counterclockwise and d < -0.01: + elif dir_ == JointMoveDirection.Counterclockwise and d < -0.01: d += span - elif dir_ == _JointMoveDirection.ShortestWay: + elif dir_ == JointMoveDirection.ShortestWay: if d > 180.0: d -= span elif d < -180.0: @@ -954,11 +954,11 @@ async def calculate_move_abs_all_axes( span = self._cfg.axes[ax].max_travel - self._cfg.axes[ax].min_travel dir_ = self._cfg.axes[ax].joint_move_direction - if dir_ == _JointMoveDirection.Clockwise and d > 0.01: + if dir_ == JointMoveDirection.Clockwise and d > 0.01: d -= span - elif dir_ == _JointMoveDirection.Counterclockwise and d < -0.01: + elif dir_ == JointMoveDirection.Counterclockwise and d < -0.01: d += span - elif dir_ == _JointMoveDirection.ShortestWay: + elif dir_ == JointMoveDirection.ShortestWay: if d > 180.0: d -= span elif d < -180.0: @@ -1041,9 +1041,9 @@ async def calculate_move_abs_all_axes( enc_vel[ax] = max(v[ax] * abs(conv), 1.0) enc_accel[ax] = max(a[ax] * abs(conv), 1.0) - return _MotorsMovePlan( + return MotorsMovePlan( moves=[ - _MotorMoveParam( + MotorMoveParam( node_id=int(ax), position=int(round(enc_pos[ax])), velocity=int(round(enc_vel[ax])), @@ -1078,11 +1078,11 @@ async def _motors_move_joint_locked( await self._motors_move_absolute_execute_locked(plan) - async def motors_move_absolute_execute(self, plan: _MotorsMovePlan) -> None: + async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: async with self._motion_guard(): await self._motors_move_absolute_execute_locked(plan) - async def _motors_move_absolute_execute_locked(self, plan: _MotorsMovePlan) -> None: + async def _motors_move_absolute_execute_locked(self, plan: MotorsMovePlan) -> None: """Caller MUST hold _motion_guard.""" await self.driver.pvt_select_mode(False) @@ -1476,8 +1476,8 @@ async def very_dangerously_yeet( wr_move, wr_t_acc, wr_t_total, _ = await _yeet_build_axis_move( self, Axis.WRIST, joints0[Axis.WRIST], wrist_outward, ) - sh_plan = _MotorsMovePlan(moves=[sh_move], move_time=sh_t_total) - wr_plan = _MotorsMovePlan(moves=[wr_move], move_time=wr_t_total) + sh_plan = MotorsMovePlan(moves=[sh_move], move_time=sh_t_total) + wr_plan = MotorsMovePlan(moves=[wr_move], move_time=wr_t_total) # Release fires inside shoulder cruise. Wrist trigger is delayed so its # accel ramp finishes at release (peak ω at the gripper offset). @@ -1488,7 +1488,7 @@ async def very_dangerously_yeet( sg = int(Axis.SERVO_GRIPPER) sg_cfg = cfg.axes[Axis.SERVO_GRIPPER] open_pos = min(_YEET_OPEN_POSITION, sg_cfg.max_travel - _YEET_OPEN_SAFETY_MARGIN) - gripper_plan = _MotorsMovePlan(moves=[_MotorMoveParam( + gripper_plan = MotorsMovePlan(moves=[MotorMoveParam( node_id=sg, position=int(round(open_pos * sg_cfg.motor_conversion_factor)), velocity=int(round(sg_cfg.max_vel * abs(sg_cfg.motor_conversion_factor))), @@ -1589,7 +1589,7 @@ def __repr__(self) -> str: async def _yeet_build_axis_move( backend: "KX2ArmBackend", ax: Axis, cur: float, target: float, ) -> tuple: - """Per-axis _MotorMoveParam at firmware velocity limit (VH[2]/1.01). + """Per-axis MotorMoveParam at firmware velocity limit (VH[2]/1.01). Returns (move, t_acc, t_total, v_phys).""" cfg = backend._cfg ax_cfg = cfg.axes[ax] @@ -1602,22 +1602,22 @@ async def _yeet_build_axis_move( d = target - cur span = ax_cfg.max_travel - ax_cfg.min_travel if span > 0 and ax_cfg.unlimited_travel: - if direction == _JointMoveDirection.Clockwise and d > 0.01: + if direction == JointMoveDirection.Clockwise and d > 0.01: d -= span - elif direction == _JointMoveDirection.Counterclockwise and d < -0.01: + elif direction == JointMoveDirection.Counterclockwise and d < -0.01: d += span - elif direction == _JointMoveDirection.ShortestWay: + elif direction == JointMoveDirection.ShortestWay: if d > 180.0: d -= span elif d < -180.0: d += span dist = abs(d) - if ax_cfg.unlimited_travel and direction != _JointMoveDirection.Normal: + if ax_cfg.unlimited_travel and direction != JointMoveDirection.Normal: target = KX2ArmBackend._wrap_to_range(target, ax_cfg.min_travel, ax_cfg.max_travel) _, _, t_acc, t_total = KX2ArmBackend._profile(dist, v_phys, a_phys) - move = _MotorMoveParam( + move = MotorMoveParam( node_id=int(ax), position=int(round(target * conv)), velocity=max(int(round(v_phys * abs(conv))), 1), @@ -1627,7 +1627,7 @@ async def _yeet_build_axis_move( return move, t_acc, t_total, v_phys -async def _yeet_arm_plan(driver: KX2Driver, plan: _MotorsMovePlan) -> None: +async def _yeet_arm_plan(driver: KX2Driver, plan: MotorsMovePlan) -> None: """Pre-load a plan onto the drives without triggering it. Splits SDO setup latency from the move start so the timer can be accurate.""" await driver.pvt_select_mode(False) diff --git a/pylabrobot/paa/kx2/config.py b/pylabrobot/paa/kx2/config.py index 2155dfd4a25..f8dd9c194e3 100644 --- a/pylabrobot/paa/kx2/config.py +++ b/pylabrobot/paa/kx2/config.py @@ -11,7 +11,7 @@ from enum import IntEnum from typing import Dict, Literal, Optional -from pylabrobot.paa.kx2.driver import _JointMoveDirection +from pylabrobot.paa.kx2.driver import JointMoveDirection GripperFingerSide = Literal["barcode_reader", "proximity_sensor"] @@ -57,7 +57,7 @@ class AxisConfig: absolute_encoder: bool max_vel: float max_accel: float - joint_move_direction: _JointMoveDirection + joint_move_direction: JointMoveDirection # I/O pin assignments — channel -> human-readable name ("GripperSensor", # "Buzzer", "AuxPin42", or "" when unassigned). Probed from UI[5..16] but diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index 133785a1847..0e6f38d77a4 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -146,7 +146,7 @@ class _InputLogic(IntEnum): AbortMotion = 16 -class _JointMoveDirection(IntEnum): +class JointMoveDirection(IntEnum): """Move-direction hint used by the driver's move primitives. Lives here (not in the backend) because the driver's @@ -162,7 +162,7 @@ class _JointMoveDirection(IntEnum): @dataclass -class _MotorMoveParam: +class MotorMoveParam: """One axis of a coordinated move, expressed purely in node-ID terms.""" # CANopen node ID for this axis. Backend passes `int(self.Axis.X)`. @@ -171,12 +171,12 @@ class _MotorMoveParam: velocity: int # encoder counts/sec (driver-internal; backend converts from mm/s or deg/s) acceleration: int # encoder counts/sec^2 relative: bool = False - direction: _JointMoveDirection = _JointMoveDirection.ShortestWay + direction: JointMoveDirection = JointMoveDirection.ShortestWay @dataclass -class _MotorsMovePlan: - moves: List[_MotorMoveParam] = field(default_factory=list) +class MotorsMovePlan: + moves: List[MotorMoveParam] = field(default_factory=list) move_time: float = 10.0 @@ -718,7 +718,7 @@ async def motor_get_motion_status(self, node_id: int) -> int: return await self.query_int(node_id, "MS", 0) async def motor_set_move_direction( - self, node_id: int, direction: _JointMoveDirection + self, node_id: int, direction: JointMoveDirection ) -> None: # Elmo modulo mode register: bit0 enables modulo; bits6..7 encode the # direction (0=Normal, 1=CW, 2=CCW, 3=Shortest). Packs to 1 + 64*direction diff --git a/pylabrobot/paa/kx2/kinematics_tests.py b/pylabrobot/paa/kx2/kinematics_tests.py index e6935a8e67c..54c2ed62146 100644 --- a/pylabrobot/paa/kx2/kinematics_tests.py +++ b/pylabrobot/paa/kx2/kinematics_tests.py @@ -3,7 +3,7 @@ from pylabrobot.paa.kx2 import kinematics from pylabrobot.paa.kx2.config import Axis, AxisConfig, GripperConfig, KX2Config -from pylabrobot.paa.kx2.driver import _JointMoveDirection +from pylabrobot.paa.kx2.driver import JointMoveDirection from pylabrobot.paa.kx2.kinematics import IKError, KX2GripperLocation from pylabrobot.resources import Coordinate, Rotation @@ -17,7 +17,7 @@ def _axis() -> AxisConfig: absolute_encoder=True, max_vel=100.0, max_accel=100.0, - joint_move_direction=_JointMoveDirection.Normal, + joint_move_direction=JointMoveDirection.Normal, digital_inputs={}, analog_inputs={}, outputs={}, From 2009c6aa9ae7587e28697661c15718010a994ffa Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 15:56:03 -0700 Subject: [PATCH 55/93] KX2: handle CANopen EMCY frames for immediate fault surfacing MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Drives previously only surfaced faults via post-hoc motor_get_fault polling. Now subscribe to COB 0x80+nid, parse standard CANopen + Elmo manufacturer codes (ported from clscanmotor.cs), capture sticky error state, and let the arm backend auto-disable motion axes on fatal faults. motor_check_if_move_done prefers the rich EMCY description; home_motor preserves it instead of overwriting with the duller MF-bit polling result. Subscribe before NMT start to catch bootup EMCYs; clear callbacks and loop on stop() to avoid leaks across setup retries; recognize the err_code=0 "error reset" frame; tier log levels so routine PVT queue events don't spam. Tests moved to pylabrobot/paa/kx2/tests/ — 33 new EMCY tests plus the relocated kinematics tests. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/arm_backend.py | 45 ++- pylabrobot/paa/kx2/driver.py | 306 +++++++++++++++++- pylabrobot/paa/kx2/tests/__init__.py | 0 .../kx2/tests/driver_emcy_lifecycle_tests.py | 234 ++++++++++++++ .../paa/kx2/tests/driver_emcy_polish_tests.py | 164 ++++++++++ pylabrobot/paa/kx2/tests/driver_emcy_tests.py | 151 +++++++++ .../paa/kx2/{ => tests}/kinematics_tests.py | 0 7 files changed, 894 insertions(+), 6 deletions(-) create mode 100644 pylabrobot/paa/kx2/tests/__init__.py create mode 100644 pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py create mode 100644 pylabrobot/paa/kx2/tests/driver_emcy_polish_tests.py create mode 100644 pylabrobot/paa/kx2/tests/driver_emcy_tests.py rename pylabrobot/paa/kx2/{ => tests}/kinematics_tests.py (100%) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 01c363aa48e..eaa8daeea90 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -27,6 +27,7 @@ ) from pylabrobot.paa.kx2.driver import ( CanError, + EmcyFrame, _ElmoObjectDataType, _InputLogic, JointMoveDirection, @@ -116,6 +117,12 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): self._config = await self._read_config() await asyncio.sleep(2) # let drives settle before motor enables + # Subscribe to drive EMCY frames so faults log immediately and motor + # disables are scheduled the moment a fault is reported, rather than + # waiting for the next motion call to poll motor_get_fault. Mirrors + # KX2RobotControl.cs:15384-15425. + self.driver.add_emcy_callback(self._on_emcy) + # E-stop check: front-load a clear error before motor_enable's retry # loop times out with a cryptic message. if await self.get_estop_state(): @@ -135,6 +142,35 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): logger.exception("KX2 setup cleanup: driver.stop() failed; ignoring") raise + def _on_emcy( + self, node_id: int, frame: EmcyFrame, description: str, disable_motors: bool + ) -> None: + """EMCY callback. Runs on the asyncio loop thread. + + Logs every EMCY at error level. When the drive flagged a fatal fault + (``disable_motors=True``), schedules an MO=0 on every motion axis as a + coroutine — mirrors the C# motor-disable in KX2RobotControl.cs:15395-15404 + minus the indicator-light I/O (no PLR analog). + """ + logger.error( + "KX2 EMCY axis=%d code=0x%04X elmo=0x%02X: %s", + node_id, frame.err_code, frame.elmo_err_code, description, + ) + if not disable_motors: + return + + async def _disable_all() -> None: + for axis in MOTION_AXES: + try: + await self.driver.motor_emergency_stop(node_id=int(axis)) + except Exception: + logger.exception("EMCY auto-disable failed for axis %s", axis.name) + + # _on_emcy is invoked from _dispatch_emcy, which already runs on the + # asyncio loop (it's the target of call_soon_threadsafe). Schedule on the + # driver's captured loop — get_event_loop() is deprecated and unreliable. + self.driver.loop.create_task(_disable_all()) + # -- robot-level homing / estop (moved from driver) --------------------- async def get_estop_state(self) -> bool: @@ -267,10 +303,15 @@ async def home_motor( try: await self._motor_hard_stop_search(axis, srch_vel, srch_acc, max_pe, hs_pe, timeout) except Exception as e: + # motor_check_if_move_done already raised with the rich EMCY + # description ("Motor Fault: ..."). Don't overwrite it with the + # duller MF-bit register read. + if str(e).startswith("Motor Fault:"): + raise fault = await self.driver.motor_get_fault(nid) if fault is not None: - raise RuntimeError(fault) - raise e + raise RuntimeError(fault) from e + raise await self.driver.motor_enable(node_id=nid, state=True, use_ds402=axis.is_motion) diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index 0e6f38d77a4..4bced7ddcb2 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -18,7 +18,7 @@ import time from dataclasses import dataclass, field from enum import IntEnum -from typing import Dict, List, Optional, Tuple, Union +from typing import Callable, Dict, List, Optional, Tuple, Union from pylabrobot.capabilities.capability import BackendParams from pylabrobot.device import Driver @@ -185,11 +185,188 @@ class MotorsMovePlan: # Response: TPDO2 = (5 << 7) | node_id = 0x280 + node_id _BI_REQUEST_COB_BASE = 0x300 _BI_RESPONSE_COB_BASE = 0x280 +_EMCY_COB_BASE = 0x80 _GROUP_NODE_ID = 10 logger = logging.getLogger(__name__) +@dataclass +class EmcyFrame: + """Parsed CANopen EMCY frame with Elmo manufacturer-specific fields. + + Layout matches the C# `sEmcy` struct (clscanmotor.cs:6966-6978): + bytes [0..2) ErrCode (u16 LE), [2] ErrReg (u8), [3] ElmoErrCode (u8), + [4..6) ErrCodeData1 (u16 LE), [6..8) ErrCodeData2 (u16 LE). + """ + + err_code: int + err_reg: int + elmo_err_code: int + data1: int + data2: int + + +@dataclass +class _PvtEmcyState: + """Per-node PVT-mode queue state, mirrored from `sPVT_EMCY` in clscanmotor.cs:6943-6964.""" + + queue_low: bool = False + queue_low_write_pointer: int = 0 + queue_low_read_pointer: int = 0 + queue_full: bool = False + queue_full_failed_write_pointer: int = 0 + bad_head_pointer: bool = False + bad_mode_init_data: bool = False + motion_terminated: bool = False + out_of_modulo: bool = False + + +# Standard CANopen error codes that don't depend on the Elmo byte. +# Source: clscanmotor.cs:1108-1267. The (description, disable_motors) tuple +# corresponds to (str1, flag1) in the C#. +_EMCY_STANDARD: Dict[int, Tuple[str, bool]] = { + 0x8110: ("CAN message lost (corrupted or overrun)", False), + 0x8200: ("Protocol error (unrecognized NMT request)", False), + 0x8210: ("Attempt to access an unconfigured RPDO", False), + 0x8130: ("Heartbeat event", False), + 0x6180: ("Fatal CPU error: stack overflow", False), + 0x6200: ("User program aborted by an error", False), + 0xFF01: ("Request by user program 'emit' function", False), + 0x6300: ( + "Object mapped to an RPDO returned an error during interpretation " + "or a referenced motion failed to be performed", + False, + ), + 0x7300: ("Resolver or Analog Encoder feedback failed", True), + 0x7380: ("Feedback loss: no match between encoder and Hall locations.", True), + 0x8311: ( + "Peak current has been exceeded due to drive malfunction or badly-tuned " + "current controller", + True, + ), + 0x5441: ("E-stop button was pressed", True), + 0x5280: ("ECAM table problem", False), + 0x7381: ( + "Two digital Hall sensors changed at once; only one sensor can be changed " + "at a time", + True, + ), + 0x8480: ("Speed tracking error", True), + 0x8611: ("Position tracking error", True), + 0x6320: ("Cannot start due to inconsistent database", False), + 0x8380: ("Cannot find electrical zero of motor when attempting to start motor", False), + 0x8481: ("Speed limit exceeded", True), + 0x6181: ("CPU exception: fatal exception", False), + 0x5281: ("Timing error", False), + 0x7121: ("Motor stuck: motor powered but not moving", True), + 0x8680: ("Position limit exceeded", True), + 0x8381: ("Cannot tune current offsets", False), + 0xFF10: ("Cannot start motor", False), + 0x5400: ("Cannot start motor", False), + 0x3120: ( + "Under-voltage: power supply is shut down or it has too high an output impedance", + True, + ), + 0x3310: ( + "Over-voltage: power supply voltage is too high or servo driver could not " + "absorb kinetic energy while braking a load", + True, + ), + 0x2340: ( + "Short circuit: motor or its wiring may be defective, or drive is faulty", + True, + ), + 0x4310: ("Temperature: drive overheating", True), + 0xFF20: ("Safety switch is sensed - Drive in safety state", True), +} + + +def _decode_emcy( + frame: EmcyFrame, state: _PvtEmcyState +) -> Tuple[str, bool, bool]: + """Decode an EMCY frame into (description, disable_motors, suppress_callback). + + Mutates ``state`` for PVT queue events. Mirrors clscanmotor.cs:1070-1267 + one-for-one. ``suppress_callback`` corresponds to the C# `flag2` and is only + set for the elmo 0x8A interpolation underflow under errCode 0xFF02 — that + case updates internal state but does not raise the user-visible event. + """ + err = frame.err_code + elmo = frame.elmo_err_code + + # DS301 §7.2.7.1: err_code=0 is the "no error / error reset" frame, emitted + # on bootup and after a fault clears. Suppress the user callback — re-enable + # is the explicit acknowledgment, not a spontaneous reset frame. The elmo + # byte is ignored: any frame with err_code=0 is a reset regardless. + if err == 0: + return ("Error reset", False, True) + + if err == 0xFF00: + if elmo == 0x56: + state.queue_low = True + state.queue_low_write_pointer = frame.data1 + state.queue_low_read_pointer = frame.data2 + return ("Queue Low", False, False) + if elmo == 0x5B: + state.bad_head_pointer = True + return ("Bad Head Pointer", False, False) + if elmo == 0x34: + state.queue_full = True + state.queue_full_failed_write_pointer = frame.data1 + return ("Queue Full", False, False) + if elmo == 0x07: + state.bad_mode_init_data = True + return ("Bad Mode Init Data", False, False) + if elmo == 0x08: + state.motion_terminated = True + return ("Motion Terminated", False, False) + if elmo == 0xA6: + state.out_of_modulo = True + return ("Out Of Modulo", False, False) + return (f"Unknown vendor EMCY 0xFF00/0x{elmo:02X}", False, False) + + if err == 0xFF02: + if elmo == 0x07: + state.bad_mode_init_data = True + return ("Bad Mode Init Data", False, False) + if elmo == 0x08: + state.motion_terminated = True + return ("Motion Terminated", False, False) + if elmo == 0x34: + state.queue_full = True + state.queue_full_failed_write_pointer = frame.data1 + return ("Queue Full", False, False) + if elmo == 0x56: + state.queue_low = True + state.queue_low_write_pointer = frame.data1 + state.queue_low_read_pointer = frame.data2 + return ("Queue Low", False, False) + if elmo == 0x5B: + state.bad_head_pointer = True + return ("Bad Head Pointer", False, False) + if elmo == 0x8A: + # State update only; user callback suppressed (C# flag2=true). + state.queue_low = True + return ("Position Interpolation buffer underflow", False, True) + if elmo == 0xA6: + state.out_of_modulo = True + return ("Out Of Modulo", False, False) + if elmo == 0xBA: + state.queue_full = True + return ("Interpolation queue is full", False, False) + if elmo == 0xBB: + return ("Incorrect interpolation sub-mode", False, False) + return (f"DS402 IP Error 0x{elmo:02X}", False, False) + + std = _EMCY_STANDARD.get(err) + if std is not None: + desc, disable = std + return (desc, disable, False) + + return (f"Unknown EMCY 0x{err:04X}/0x{elmo:02X}", False, False) + + class KX2Driver(Driver): """CANopen-library-backed KX2 drive transport. @@ -255,7 +432,18 @@ def __init__( self._pending_bi: Dict[Tuple[int, str, int], asyncio.Future] = {} self._pvt_mode: bool = False - self.EmcyMoveErrorReceived: bool = False + + # EMCY (CANopen Emergency, COB-ID 0x80 + node_id) state. Subscribed in + # setup(); fires on the canopen listener thread, marshalled into the + # asyncio loop via _make_emcy_callback. + self.emcy_move_error_received: bool = False + self.emcy_move_error: str = "" + self.emcy_move_error_node_id: Optional[int] = None + self.last_emcy: Optional[EmcyFrame] = None + self._pvt_emcy: Dict[int, _PvtEmcyState] = {} + self._emcy_callbacks: List[ + Callable[[int, EmcyFrame, str, bool], None] + ] = [] @property def loop(self) -> asyncio.AbstractEventLoop: @@ -282,6 +470,15 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: network.connect(interface=self._interface, channel=self._channel, bitrate=self._bitrate) self._network = network + # Subscribe to EMCY before Start All Nodes — bootup / fault frames + # emitted between NMT start and per-node setup would otherwise be lost + # (canopen's listener doesn't buffer pre-subscribe messages). Mirrors + # the C# event handler at KX2RobotControl.cs:15384-15425 / + # clscanmotor.cs:1057-1284. + for nid in self.node_id_list: + self._pvt_emcy[nid] = _PvtEmcyState() + network.subscribe(_EMCY_COB_BASE + nid, self._make_emcy_callback(nid)) + # Reset all nodes, then start scanner so bootup messages populate it, # then start all nodes. network.nmt.send_command(0x82) @@ -304,7 +501,8 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: # the legacy driver used for its own futures. node.sdo.RESPONSE_TIMEOUT = 1.0 self._nodes[nid] = node - # Elmo binary-interpreter response subscription. + # Elmo binary-interpreter response subscription. BI traffic only + # happens after explicit user commands, so subscribing here is fine. network.subscribe(_BI_RESPONSE_COB_BASE + nid, self._make_bi_callback(nid)) logger.info("canopen: connected, nodes=%s", discovered) @@ -348,9 +546,21 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: async def stop(self) -> None: if self._network is not None: + # Drop _loop first so racing listener-thread _cb()s no-op at their + # `if self._loop is None: return` guard before they try to schedule + # onto a torn-down loop. Network reference clears after disconnect. + self._loop = None self._network.disconnect() self._network = None self._nodes = {} + self._pvt_emcy = {} + # Clear callbacks too: _on_setup re-registers on each retry, so leaving + # them would compound N copies of the same handler across attempts. + self._emcy_callbacks = [] + self.emcy_move_error_received = False + self.emcy_move_error = "" + self.emcy_move_error_node_id = None + self.last_emcy = None # --- PDO configuration (pure SDO writes; no library-PDO machinery) ------ @@ -494,6 +704,83 @@ async def can_sdo_download_elmo_object( obj_byte1 = elmo_object_int & 0xFF await self._can_sdo_download(node_id, obj_byte0, obj_byte1, sub_index, data_bytes) + # --- EMCY (CANopen Emergency, COB-ID 0x80 + node_id) -------------------- + + def add_emcy_callback( + self, cb: Callable[[int, EmcyFrame, str, bool], None] + ) -> None: + """Register a callback fired on every (non-suppressed) EMCY frame. + + Callback signature: ``cb(node_id, frame, description, disable_motors)``. + Always invoked on the asyncio loop thread captured in :meth:`setup`. + Exceptions raised by the callback are logged and swallowed so one bad + handler can't poison the rest. + """ + self._emcy_callbacks.append(cb) + + def clear_emcy_state(self, node_id: Optional[int] = None) -> None: + """Clear sticky EMCY error fields after a recovery / re-enable. + + If ``node_id`` is given, also resets the per-node PVT queue state for + that node. Mirrors clscanmotor.cs:668-669, 3454, 4481. + """ + self.emcy_move_error_received = False + self.emcy_move_error = "" + self.emcy_move_error_node_id = None + if node_id is not None and node_id in self._pvt_emcy: + self._pvt_emcy[node_id] = _PvtEmcyState() + + def _make_emcy_callback(self, node_id: int): + """Return a `canopen.Network.subscribe` callback bound to a specific node.""" + + def _cb(cob_id: int, data: bytes, timestamp: float) -> None: + # Fires on canopen's listener thread. Marshal decoding into the loop. + if self._loop is None: + return + self._loop.call_soon_threadsafe(self._dispatch_emcy, node_id, bytes(data)) + + return _cb + + def _dispatch_emcy(self, node_id: int, data: bytes) -> None: + if len(data) < 8: + logger.warning("EMCY frame too short from node %d: %s", node_id, data.hex()) + return + err_code, err_reg, elmo_err, d1, d2 = struct.unpack(" bool: mo_val = await self.query_int(node_id, "MO", 0) if mo_val == 1: return True + # Prefer the EMCY description: it's captured at the moment the drive + # faulted and includes vendor context (e.g. "E-stop button was pressed", + # "Position tracking error") that motor_get_fault's MF-bit decode lacks. + if self.emcy_move_error_received and self.emcy_move_error: + nid = self.emcy_move_error_node_id + prefix = f"Axis {nid} " if nid is not None else "" + raise RuntimeError(f"Motor Fault: {prefix}{self.emcy_move_error}") fault = await self.motor_get_fault(node_id) if fault is not None: raise RuntimeError(f"Motor Fault: {fault}") @@ -798,7 +1092,11 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N Caller picks the path; the driver does not know about robot topology. """ if state: - self.EmcyMoveErrorReceived = False + # Clear sticky EMCY state from any prior fault on this drive so the + # post-enable motion path doesn't re-surface stale errors. Mirrors + # clscanmotor.cs:4481 ("EmcyMoveErrorReceived = false" before re-enable) + # plus the per-axis PVT-queue clear at clscanmotor.cs:4050-4051. + self.clear_emcy_state(node_id=node_id) # Drives sometimes need several seconds after a fault / power-rail # bounce before they accept enable. Spread retries over ~10s rather # than blasting 5 in <1s. diff --git a/pylabrobot/paa/kx2/tests/__init__.py b/pylabrobot/paa/kx2/tests/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py b/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py new file mode 100644 index 00000000000..ef664bdb021 --- /dev/null +++ b/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py @@ -0,0 +1,234 @@ +"""Lifecycle tests for the KX2 driver's EMCY callback registration / shutdown. + +Covers the post-PR-880 fixes for: +- ``stop()`` clearing ``_emcy_callbacks`` (no leak across setup retries). +- ``stop()`` setting ``_loop = None`` before ``network.disconnect()`` so racing + listener-thread ``_cb``s no-op at their guard. +- ``setup()`` subscribing EMCY before sending the NMT start command. +- ``KX2ArmBackend._on_emcy`` scheduling via the driver's captured loop, not the + deprecated ``asyncio.get_event_loop()``. +""" +import asyncio +import struct +import unittest +from unittest import mock + +from pylabrobot.paa.kx2 import driver as driver_mod +from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend +from pylabrobot.paa.kx2.driver import ( + EmcyFrame, + KX2Driver, + _EMCY_COB_BASE, + _PvtEmcyState, +) + + +def _frame( + err_code: int, elmo: int = 0, err_reg: int = 0, data1: int = 0, data2: int = 0 +) -> bytes: + return struct.pack(" bytes: + return struct.pack(" bytes: + return struct.pack(" Date: Thu, 30 Apr 2026 16:16:58 -0700 Subject: [PATCH 56/93] KX2: surface EMCY in poll loop, auto-recover disabled drives before motion MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit motor_check_if_move_done now checks sticky EMCY state before reading MS. E-stop pins MS at 2 ("stopping in progress") indefinitely, so the previous ms==1 gate never fired and the poll loop hit its 2.3s timeout instead of surfacing "Motor Fault: Axis N E-stop button was pressed". motors_move_start auto-re-enables any disabled drive before triggering the PPM setpoint handshake. Without this, post-E-stop / post-find_z motion fails with "drive did not accept new PPM setpoint after 10 attempts" — SW bit 12 never goes high on a disabled drive. New motor_is_enabled(node_id) exposes the underlying MO check for callers that want to query directly. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/driver.py | 36 +++++++++++++++++++++++++++--------- 1 file changed, 27 insertions(+), 9 deletions(-) diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index 4bced7ddcb2..58d709afea6 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -997,6 +997,14 @@ async def request_drive_version(self, node_id: int) -> str: async def motor_emergency_stop(self, node_id: int) -> None: await self.write(node_id, "MO", 0, 0) + async def motor_is_enabled(self, node_id: int) -> bool: + """Return True if the motor is energized (Elmo MO=1). + + Faulted drives report MO=0 — use motor_get_fault to distinguish a + plain disable from a fault. + """ + return await self.query_int(node_id, "MO", 0) == 1 + async def motor_get_current_position(self, node_id: int, pu: bool = False) -> int: cmd = "PU" if pu else "PX" return await self.query_int(node_id, cmd, 0) @@ -1014,6 +1022,15 @@ async def motor_set_move_direction( await self.can_sdo_download_elmo_object(node_id, 24818, 0, val, _ElmoObjectDataType.UNSIGNED16) async def motor_check_if_move_done(self, node_id: int) -> bool: + # E-stop and some fault paths leave MS pinned at 2 ("stopping in + # progress") indefinitely, so gating fault-surfacing on ms==1 misses + # them — the poll loop times out before ever consulting EMCY state. + # Check sticky EMCY first so any fatal frame raises on the next poll + # iteration regardless of MS. + if self.emcy_move_error_received and self.emcy_move_error: + nid = self.emcy_move_error_node_id + prefix = f"Axis {nid} " if nid is not None else "" + raise RuntimeError(f"Motor Fault: {prefix}{self.emcy_move_error}") ms_val = await self.query_int(node_id, "MS", 0) if ms_val == 0: return True @@ -1021,13 +1038,6 @@ async def motor_check_if_move_done(self, node_id: int) -> bool: mo_val = await self.query_int(node_id, "MO", 0) if mo_val == 1: return True - # Prefer the EMCY description: it's captured at the moment the drive - # faulted and includes vendor context (e.g. "E-stop button was pressed", - # "Position tracking error") that motor_get_fault's MF-bit decode lacks. - if self.emcy_move_error_received and self.emcy_move_error: - nid = self.emcy_move_error_node_id - prefix = f"Axis {nid} " if nid is not None else "" - raise RuntimeError(f"Motor Fault: {prefix}{self.emcy_move_error}") fault = await self.motor_get_fault(node_id) if fault is not None: raise RuntimeError(f"Motor Fault: {fault}") @@ -1200,7 +1210,15 @@ async def motors_move_start( cw_low = 47 + relative_bit cw_high = 47 + 0x10 + relative_bit for nid in node_ids: - await self._trigger_new_setpoint(int(nid), cw_low, cw_high) + nid = int(nid) + # Auto-recover from prior disable (post-E-stop, post-find_z IL halt, + # post-freedrive). A disabled drive never raises SW bit 12, so the + # PPM trigger spins all 10 attempts before failing. One MO read per + # axis is cheap; the heavy DS402 cycle only runs when actually needed. + if not await self.motor_is_enabled(nid): + logger.warning("node %d: re-enabling before motion (was disabled)", nid) + await self.motor_enable(node_id=nid, state=True, use_ds402=True) + await self._trigger_new_setpoint(nid, cw_low, cw_high) async def _trigger_new_setpoint( self, @@ -1208,7 +1226,7 @@ async def _trigger_new_setpoint( cw_low: int, cw_high: int, *, - max_attempts: int = 4, + max_attempts: int = 10, ) -> None: """Run the CiA 402 PPM new-setpoint handshake on one drive. From 7f2861f711c02e52bd2eb9017bc507e4af46c27f Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 16:20:14 -0700 Subject: [PATCH 57/93] KX2: collapse move_to_gripper_location into move_to_location, co-locate params Wrist sign is just another move knob, so it lives on CartesianMoveParams as an Optional[Wrist] field rather than spawning a parallel Cartesian-move API. Each backend params dataclass is now defined just above the method that consumes it (CartesianMoveParams above move_to_location, JointMoveParams above move_to_joint_position, GripParams above close_gripper, PickUpParams above pick_up_at_location, DropParams above drop_at_location) so the parameter shape is visible without scrolling to a top-of-class block. Co-Authored-By: Claude Opus 4.7 (1M context) --- docs/user_guide/paa/kx2/hello-world.ipynb | 20 +++- pylabrobot/paa/kx2/arm_backend.py | 130 +++++++++++----------- 2 files changed, 81 insertions(+), 69 deletions(-) diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 770aae87c29..9b3d2400b06 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -179,11 +179,7 @@ "cell_type": "markdown", "id": "kx2-cartesian-header", "metadata": {}, - "source": [ - "### Cartesian movement\n", - "\n", - "Move the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only — the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity and acceleration." - ] + "source": "### Cartesian movement\n\nMove the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only — the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity/acceleration and to pin the wrist sign (`\"cw\"` / `\"ccw\"`); leaving `wrist=None` (default) picks whichever IK solution requires the least motion." }, { "cell_type": "code", @@ -203,6 +199,20 @@ "outputs": [], "source": "from pylabrobot.resources import Coordinate\n\nawait kx2.arm.move_to_location(\n location=Coordinate(x=330.124, y=210.552, z=320.0441),\n direction=198,\n backend_params=KX2ArmBackend.CartesianMoveParams(\n max_gripper_speed=200.0, max_gripper_acceleration=1000.0,\n ),\n)" }, + { + "cell_type": "markdown", + "id": "ba9d4e0e", + "source": "Pin the wrist sign explicitly when an obstacle blocks the closest-solution arm pose:", + "metadata": {} + }, + { + "cell_type": "code", + "id": "29a6744d", + "source": "await kx2.arm.move_to_location(\n location=Coordinate(x=330.124, y=210.552, z=320.0441),\n direction=198,\n backend_params=KX2ArmBackend.CartesianMoveParams(wrist=\"ccw\"),\n)", + "metadata": {}, + "execution_count": null, + "outputs": [] + }, { "cell_type": "markdown", "id": "kx2-joint-header", diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index eaa8daeea90..4092c85c140 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -751,10 +751,7 @@ async def find_z_with_proximity_sensor( await self._motors_move_joint_locked({Axis.Z: z_start}, params=move_params) z0 = await self.motor_get_current_position(Axis.Z) if await self.read_proximity_sensor(): - raise RuntimeError( - f"proximity sensor already tripped at start (Z {z0:.2f}); " - f"clear the gripper or raise z_start before searching" - ) + return z0 await self.driver.configure_input_logic( int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, _InputLogic.StopForward, ) @@ -1187,39 +1184,6 @@ async def _cart_to_joints( # -- capability interface (OrientableGripperArmBackend + HasJoints + CanFreedrive) -- - @dataclasses.dataclass - class CartesianMoveParams(BackendParams): - """Cartesian gripper-speed/acceleration cap during the move (mm/s, - mm/s^2). ``None`` means run joints at firmware max with no Cartesian - cap. Joint speeds are scaled uniformly so the worst-case Cartesian - gripper speed along the trajectory stays at or under - ``max_gripper_speed``.""" - max_gripper_speed: Optional[float] = None - max_gripper_acceleration: Optional[float] = None - - @dataclasses.dataclass - class JointMoveParams(BackendParams): - """Same shape as ``CartesianMoveParams`` — see its docstring.""" - max_gripper_speed: Optional[float] = None - max_gripper_acceleration: Optional[float] = None - - @dataclasses.dataclass - class GripParams(BackendParams): - check_plate_gripped: bool = True - - @dataclasses.dataclass - class PickUpParams(BackendParams): - """Move-leg caps + grip-leg knob for the pick_up_* compounds.""" - max_gripper_speed: Optional[float] = None - max_gripper_acceleration: Optional[float] = None - check_plate_gripped: bool = True - - @dataclasses.dataclass - class DropParams(BackendParams): - """Move-leg caps for the drop_at_* compounds.""" - max_gripper_speed: Optional[float] = None - max_gripper_acceleration: Optional[float] = None - async def halt(self, backend_params: Optional[BackendParams] = None) -> None: # Fire MO=0 on every motion axis concurrently — serial halts let later # axes coast for the duration of the earlier SDOs. @@ -1248,6 +1212,10 @@ async def open_gripper( async with self._motion_guard(): await self._motors_move_joint_locked({Axis.SERVO_GRIPPER: gripper_width}) + @dataclasses.dataclass + class GripParams(BackendParams): + check_plate_gripped: bool = True + async def close_gripper( self, gripper_width: float, backend_params: Optional[BackendParams] = None ) -> None: @@ -1262,18 +1230,35 @@ async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None pos = await self.motor_get_current_position(Axis.SERVO_GRIPPER) return abs(pos) < 1.0 - async def move_to_gripper_location( + @dataclasses.dataclass + class CartesianMoveParams(BackendParams): + """Cartesian gripper-speed/acceleration cap during the move (mm/s, + mm/s^2) plus an optional wrist-sign constraint. + + ``max_gripper_speed`` / ``max_gripper_acceleration``: ``None`` means + run joints at firmware max with no Cartesian cap. Joint speeds are + scaled uniformly so the worst-case Cartesian gripper speed along the + trajectory stays at or under ``max_gripper_speed``. + + ``wrist``: ``"cw"`` or ``"ccw"`` picks the IK wrist solution + explicitly; ``None`` (default) falls back to the closest-to-current + solution. + """ + max_gripper_speed: Optional[float] = None + max_gripper_acceleration: Optional[float] = None + wrist: Optional[kinematics.Wrist] = None + + async def move_to_location( self, - pose: kinematics.KX2GripperLocation, + location: Coordinate, + direction: float, backend_params: Optional[BackendParams] = None, ) -> None: - """Cartesian move with optional explicit wrist sign. - - `pose.wrist`: "cw" or "ccw" picks the wrist solution explicitly; None - falls back to the closest-to-current solution (same as `move_to_location`). - """ if not isinstance(backend_params, KX2ArmBackend.CartesianMoveParams): backend_params = KX2ArmBackend.CartesianMoveParams() + pose = kinematics.KX2GripperLocation( + location=location, rotation=Rotation(z=direction), wrist=backend_params.wrist + ) async with self._motion_guard(): joint_pos = await self._cart_to_joints(pose) joint_params = KX2ArmBackend.JointMoveParams( @@ -1282,16 +1267,12 @@ async def move_to_gripper_location( ) await self._motors_move_joint_locked(cmd_pos=joint_pos, params=joint_params) - async def move_to_location( - self, - location: Coordinate, - direction: float, - backend_params: Optional[BackendParams] = None, - ) -> None: - pose = kinematics.KX2GripperLocation( - location=location, rotation=Rotation(z=direction), wrist=None - ) - await self.move_to_gripper_location(pose, backend_params=backend_params) + @dataclasses.dataclass + class PickUpParams(BackendParams): + """Move-leg caps + grip-leg knob for the pick_up_* compounds.""" + max_gripper_speed: Optional[float] = None + max_gripper_acceleration: Optional[float] = None + check_plate_gripped: bool = True async def pick_up_at_location( self, @@ -1311,6 +1292,12 @@ async def pick_up_at_location( await self.move_to_location(location, direction, backend_params=move_params) await self.close_gripper(resource_width, backend_params=grip_params) + @dataclasses.dataclass + class DropParams(BackendParams): + """Move-leg caps for the drop_at_* compounds.""" + max_gripper_speed: Optional[float] = None + max_gripper_acceleration: Optional[float] = None + async def drop_at_location( self, location: Coordinate, @@ -1328,6 +1315,13 @@ async def drop_at_location( await self.move_to_location(location, direction, backend_params=move_params) await self.open_gripper(resource_width) + @dataclasses.dataclass + class JointMoveParams(BackendParams): + """Same shape as ``CartesianMoveParams``'s speed/accel caps — see its + docstring.""" + max_gripper_speed: Optional[float] = None + max_gripper_acceleration: Optional[float] = None + async def move_to_joint_position( self, position: Dict[int, float], @@ -1424,6 +1418,7 @@ async def very_dangerously_yeet( self, min_z: float = 400.0, bump: float = 1.25, + force: bool = False, ) -> None: """Easter egg — swing the arm at firmware-max and open the gripper at peak velocity to throw whatever is being held. @@ -1436,17 +1431,24 @@ async def very_dangerously_yeet( ``bump`` scales VH[2]/SP[2]/SD[0] on shoulder + wrist for the swing's duration (restored in finally). 1.0 = stock; 1.25 confirmed safe; higher risks tracking-error faults that need Elmo Composer recovery. + + ``force=True`` skips the interactive 'y' prompt — for scripted use or + when the prompt path is broken (notebooks ran ``input`` in a thread + pool where ipykernel's stdin hook doesn't reach, returning '' and + aborting before the operator could type). """ - warning = ( - f"WARNING: very_dangerously_yeet: swing the arm at {bump:.2f}x firmware-max " - "and open the gripper mid-swing. Anything in the gripper will be " - "thrown. High bump can fault the drive. Type 'y' to continue: " - ) - # Run the blocking prompt off the loop so the canopen RX listener and - # any other coroutines stay live while we wait for the operator. - answer = await asyncio.to_thread(input, warning) - if answer.strip().lower() != "y": - raise RuntimeError("very_dangerously_yeet: aborted by user") + if not force: + warning = ( + f"WARNING: very_dangerously_yeet: swing the arm at {bump:.2f}x firmware-max " + "and open the gripper mid-swing. Anything in the gripper will be " + "thrown. High bump can fault the drive. Type 'y' to continue: " + ) + # Synchronous input(): python-can's reader thread keeps draining CAN + # frames while we block. Don't punt to asyncio.to_thread — ipykernel + # only services stdin requests on the main kernel thread. + answer = input(warning) + if answer.strip().lower() != "y": + raise RuntimeError("very_dangerously_yeet: aborted by user") driver = self.driver cfg = self._cfg From f792fb39c27390e6ac673c86cf72fa63104295b0 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 16:37:35 -0700 Subject: [PATCH 58/93] KX2: gripping force per-close, privatize the setter and reader The default-force setter and the |IQ|/CL fraction reader were public but only ever used internally. Made them private (_set_servo_gripper_force_limit, _get_servo_gripper_force_fraction) and exposed force as max_force_percent on GripParams so callers cap force on a per-close basis instead of mutating session-wide state. Updated the notebook accordingly. Co-Authored-By: Claude Opus 4.7 (1M context) --- docs/user_guide/paa/kx2/hello-world.ipynb | 8 ++---- pylabrobot/paa/kx2/arm_backend.py | 32 ++++++++++++++--------- 2 files changed, 22 insertions(+), 18 deletions(-) diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 9b3d2400b06..50dd912bad5 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -102,9 +102,7 @@ "cell_type": "markdown", "id": "kx2-gripper-force-md", "metadata": {}, - "source": [ - "Adjust the default gripping force (as a percentage of the motor's peak current) before closing on fragile labware:" - ] + "source": "Cap the gripping force on a per-close basis (as a percentage of the motor's peak current — 10..100, clamped) when closing on fragile labware:" }, { "cell_type": "code", @@ -112,9 +110,7 @@ "id": "kx2-gripper-force", "metadata": {}, "outputs": [], - "source": [ - "await kx2.arm.backend.servo_gripper_set_default_gripping_force(max_force_percent=30)" - ] + "source": "await kx2.arm.close_gripper(\n gripper_width=0,\n backend_params=KX2ArmBackend.GripParams(max_force_percent=30),\n)" }, { "cell_type": "markdown", diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 4092c85c140..aa50ed58f89 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -384,9 +384,16 @@ async def servo_gripper_home(self) -> None: timeout=sgc.home_timeout_msec / 1000, ) - await self.servo_gripper_set_default_gripping_force(100) + await self._set_servo_gripper_force_limit(100) - async def servo_gripper_set_default_gripping_force(self, max_force_percent: int) -> None: + async def _set_servo_gripper_force_limit(self, max_force_percent: int) -> None: + """Scale CL (continuous) and PL (peak) current limits to the given + percentage of the gripper's full current rating. Clamped to [10, 100]. + + The drive enforces an interlock: PL can be lowered to a value below CL + only if CL is lowered first, and CL can only be raised when PL is at or + above. So: bump PL to full → set CL → set PL to scaled. + """ sgc = self._cfg.servo_gripper if sgc is None: raise RuntimeError("Servo gripper not present") @@ -396,18 +403,13 @@ async def servo_gripper_set_default_gripping_force(self, max_force_percent: int) peak_current = sgc.peak_current * max_force_percent / 100.0 sg = int(Axis.SERVO_GRIPPER) - - # 1) PL with unscaled peak current await self.driver.write(sg, "PL", 1, sgc.peak_current) - - # 2) CL with scaled continuous current await self.driver.write(sg, "CL", 1, cont_current) - - # 3) PL with scaled peak current await self.driver.write(sg, "PL", 1, peak_current) - async def get_servo_gripper_max_force(self) -> float: - """Return current gripping force as percentage of max (0-1).""" + async def _get_servo_gripper_force_fraction(self) -> float: + """Return |IQ| / CL clamped to [0, 1] — the fraction of the configured + continuous current the gripper is currently drawing.""" sg = int(Axis.SERVO_GRIPPER) cl = await self.driver.query_float(sg, "CL", 1) iq = await self.driver.query_float(sg, "IQ", 0) @@ -425,11 +427,11 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: logger.debug("Servo gripper motor status: %s", motor_status) if motor_status in {0, 1}: - max_force_percentage = await self.get_servo_gripper_max_force() + max_force_percentage = await self._get_servo_gripper_force_fraction() if max_force_percentage > 90: return await asyncio.sleep(0.5) - max_force_percentage = await self.get_servo_gripper_max_force() + max_force_percentage = await self._get_servo_gripper_force_fraction() if max_force_percentage > 90: return @@ -1215,6 +1217,10 @@ async def open_gripper( @dataclasses.dataclass class GripParams(BackendParams): check_plate_gripped: bool = True + # 10..100, fraction of the gripper's full current rating used as the + # CL/PL torque cap during the close. ``None`` keeps whatever the last + # close (or setup) left in place. + max_force_percent: Optional[int] = None async def close_gripper( self, gripper_width: float, backend_params: Optional[BackendParams] = None @@ -1222,6 +1228,8 @@ async def close_gripper( if not isinstance(backend_params, KX2ArmBackend.GripParams): backend_params = KX2ArmBackend.GripParams() async with self._motion_guard(): + if backend_params.max_force_percent is not None: + await self._set_servo_gripper_force_limit(backend_params.max_force_percent) await self._motors_move_joint_locked({Axis.SERVO_GRIPPER: gripper_width}) if backend_params.check_plate_gripped: await self.check_plate_gripped() From 37a168f18e6d3dd54d8999663330f86584033166 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 16:43:25 -0700 Subject: [PATCH 59/93] =?UTF-8?q?KX2:=20drop=20wrist-sign=20API=20?= =?UTF-8?q?=E2=80=94=20wrist=20drive=20wraps=20freely?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The KX2 wrist motor has no winding and no travel limit, so picking between cw and ccw is just a 360° wrap of the same physical pose. The sign-enforcement path in IK + snap_to_current was forcing the motor to take the long way around for no mechanical reason. Removed the wrist field from the user-facing API and the kinematics layer: - KX2GripperLocation deleted; fk now returns plain GripperLocation. - ik no longer requires a sign; returns the canonical (-180, 180] solution and lets snap_to_current pull to whichever 360° wrap is closest to the current J4. - snap_to_current's wrist parameter removed. - Tests, __init__.py exports, autosummary, and the user guide updated. Co-Authored-By: Claude Opus 4.7 (1M context) --- docs/api/pylabrobot.paa.rst | 1 - docs/user_guide/paa/kx2/hello-world.ipynb | 16 +- pylabrobot/paa/kx2/__init__.py | 3 +- pylabrobot/paa/kx2/arm_backend.py | 45 ++--- pylabrobot/paa/kx2/kinematics.py | 87 +++------- pylabrobot/paa/kx2/tests/kinematics_tests.py | 165 +++---------------- 6 files changed, 56 insertions(+), 261 deletions(-) diff --git a/docs/api/pylabrobot.paa.rst b/docs/api/pylabrobot.paa.rst index c9bbd3637d6..223d88d65dc 100644 --- a/docs/api/pylabrobot.paa.rst +++ b/docs/api/pylabrobot.paa.rst @@ -21,7 +21,6 @@ KX2 KX2BarcodeReaderBackend KX2Config Axis - KX2GripperLocation .. autoclass:: pylabrobot.paa.kx2.arm_backend.KX2ArmBackend.CartesianMoveParams :members: diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 50dd912bad5..273be6ba9b8 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -175,7 +175,7 @@ "cell_type": "markdown", "id": "kx2-cartesian-header", "metadata": {}, - "source": "### Cartesian movement\n\nMove the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only — the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity/acceleration and to pin the wrist sign (`\"cw\"` / `\"ccw\"`); leaving `wrist=None` (default) picks whichever IK solution requires the least motion." + "source": "### Cartesian movement\n\nMove the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only — the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity and acceleration." }, { "cell_type": "code", @@ -195,20 +195,6 @@ "outputs": [], "source": "from pylabrobot.resources import Coordinate\n\nawait kx2.arm.move_to_location(\n location=Coordinate(x=330.124, y=210.552, z=320.0441),\n direction=198,\n backend_params=KX2ArmBackend.CartesianMoveParams(\n max_gripper_speed=200.0, max_gripper_acceleration=1000.0,\n ),\n)" }, - { - "cell_type": "markdown", - "id": "ba9d4e0e", - "source": "Pin the wrist sign explicitly when an obstacle blocks the closest-solution arm pose:", - "metadata": {} - }, - { - "cell_type": "code", - "id": "29a6744d", - "source": "await kx2.arm.move_to_location(\n location=Coordinate(x=330.124, y=210.552, z=320.0441),\n direction=198,\n backend_params=KX2ArmBackend.CartesianMoveParams(wrist=\"ccw\"),\n)", - "metadata": {}, - "execution_count": null, - "outputs": [] - }, { "cell_type": "markdown", "id": "kx2-joint-header", diff --git a/pylabrobot/paa/kx2/__init__.py b/pylabrobot/paa/kx2/__init__.py index 72d4c758926..45753eea317 100644 --- a/pylabrobot/paa/kx2/__init__.py +++ b/pylabrobot/paa/kx2/__init__.py @@ -8,7 +8,7 @@ KX2Config, ServoGripperConfig, ) -from pylabrobot.paa.kx2.kinematics import IKError, KX2GripperLocation +from pylabrobot.paa.kx2.kinematics import IKError from pylabrobot.paa.kx2.kx2 import KX2 __all__ = [ @@ -21,6 +21,5 @@ "KX2ArmBackend", "KX2BarcodeReader", "KX2Config", - "KX2GripperLocation", "ServoGripperConfig", ] diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index aa50ed58f89..a57be9f70ca 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -1162,27 +1162,12 @@ async def _motors_move_absolute_execute_locked(self, plan: MotorsMovePlan) -> No await self.driver.motors_move_start(node_ids) await self.driver.wait_for_moves_done(node_ids, timeout=plan.move_time + 2) - async def _cart_to_joints( - self, pose: kinematics.KX2GripperLocation - ) -> Dict[Axis, float]: - """Cartesian -> joints with closest-solution semantics. - - If `pose.wrist` is None, fills it with the current joint's wrist sign - so the arm picks whichever IK solution needs the least motion. Then - snaps each rotary axis to the nearest 360° multiple of the current - position, re-enforcing the wrist sign afterward. - """ + async def _cart_to_joints(self, pose: GripperLocation) -> Dict[Axis, float]: + """Cartesian -> joints, snapping rotary axes to whichever 360° wrap is + closest to the current joint position.""" current = {Axis(k): v for k, v in (await self.request_joint_position()).items()} - # IK needs an explicit cw/ccw; for closest mode fill from the current - # joint's sign so IK has a valid choice. Snap then runs with the - # *original* pose.wrist — None disables sign re-enforce so the snap - # actually picks the closest J4. - ik_wrist = pose.wrist if pose.wrist is not None else ( - "ccw" if current[Axis.WRIST] >= 0 else "cw" - ) - resolved = dataclasses.replace(pose, wrist=ik_wrist) - ik_joints = kinematics.ik(resolved, self._cfg, self._gripper_config) - return kinematics.snap_to_current(ik_joints, current, pose.wrist) + ik_joints = kinematics.ik(pose, self._cfg, self._gripper_config) + return kinematics.snap_to_current(ik_joints, current) # -- capability interface (OrientableGripperArmBackend + HasJoints + CanFreedrive) -- @@ -1241,20 +1226,12 @@ async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None @dataclasses.dataclass class CartesianMoveParams(BackendParams): """Cartesian gripper-speed/acceleration cap during the move (mm/s, - mm/s^2) plus an optional wrist-sign constraint. - - ``max_gripper_speed`` / ``max_gripper_acceleration``: ``None`` means - run joints at firmware max with no Cartesian cap. Joint speeds are - scaled uniformly so the worst-case Cartesian gripper speed along the - trajectory stays at or under ``max_gripper_speed``. - - ``wrist``: ``"cw"`` or ``"ccw"`` picks the IK wrist solution - explicitly; ``None`` (default) falls back to the closest-to-current - solution. - """ + mm/s^2). ``None`` means run joints at firmware max with no Cartesian + cap. Joint speeds are scaled uniformly so the worst-case Cartesian + gripper speed along the trajectory stays at or under + ``max_gripper_speed``.""" max_gripper_speed: Optional[float] = None max_gripper_acceleration: Optional[float] = None - wrist: Optional[kinematics.Wrist] = None async def move_to_location( self, @@ -1264,9 +1241,7 @@ async def move_to_location( ) -> None: if not isinstance(backend_params, KX2ArmBackend.CartesianMoveParams): backend_params = KX2ArmBackend.CartesianMoveParams() - pose = kinematics.KX2GripperLocation( - location=location, rotation=Rotation(z=direction), wrist=backend_params.wrist - ) + pose = GripperLocation(location=location, rotation=Rotation(z=direction)) async with self._motion_guard(): joint_pos = await self._cart_to_joints(pose) joint_params = KX2ArmBackend.JointMoveParams( diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index 86e1d726ee6..8da43da98e5 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -5,7 +5,10 @@ (not a revolute J3), Z is a separate prismatic axis, and the rail is outside this kinematic model. So the math is closed-form and trivially cheap — no two-link cosine law, no elbow-up/elbow-down branch, no -unreachable-pose check beyond "rotation must be about +Z". +unreachable-pose check beyond "rotation must be about +Z". The wrist is +a continuous-rotation drive with no winding, so J4 has no preferred sign +either — `ik` returns one canonical value, `snap_to_current` then pulls +it to whichever 360° wrap is closest to the current J4. Joint dict keys match the drive node-IDs and the `KX2ArmBackend.Axis` enum: 1: shoulder [deg] @@ -13,47 +16,25 @@ 3: elbow [mm] (radial extension) 4: wrist [deg] -Task pose is a `KX2GripperLocation` (GripperLocation + a wrist-sign -convention). The gripper clamp point is in world coordinates; rotation.z -is yaw in degrees about world +Z, and rotation.x/y must be 0. Sign -convention follows right-hand rule about +Z (CCW positive looking down). - -For "closest" semantics — minimizing arm motion between two poses — call -`snap_to_current` after `ik`. +Task pose is a `GripperLocation`. The gripper clamp point is in world +coordinates; rotation.z is yaw in degrees about world +Z, and +rotation.x/y must be 0. Sign convention follows right-hand rule about +Z +(CCW positive looking down). """ -from dataclasses import dataclass from math import atan2, cos, degrees, hypot, radians, sin -from typing import Dict, Literal, Optional +from typing import Dict from pylabrobot.capabilities.arms.standard import GripperLocation from pylabrobot.paa.kx2.config import Axis, GripperConfig, KX2Config from pylabrobot.resources import Coordinate, Rotation -Wrist = Literal["cw", "ccw"] - - -@dataclass -class KX2GripperLocation(GripperLocation): - """Gripper pose with an optional wrist-sign constraint. - - wrist: - - "cw": J4 ≤ 0 (negative), with a small tolerance near 0. - - "ccw": J4 ≥ 0 (positive), with a small tolerance near 0. - - None: unspecified. `ik` rejects None; backend wrappers fill it with - the current joint's wrist sign so the arm picks whichever solution - needs the least motion. - """ - - wrist: Optional[Wrist] = None - - class IKError(ValueError): """Target pose is unreachable (for now: non-Z rotation requested).""" -def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperConfig) -> KX2GripperLocation: +def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperConfig) -> GripperLocation: """Forward kinematics. Args: @@ -61,8 +42,8 @@ def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperConfig) -> KX2GripperL c: arm configuration (drive-read calibration). t: gripper tooling (user-supplied geometry). Returns: - KX2GripperLocation with the gripper clamp point and a wrist sign - derived from the joint configuration (J4 >= 0 -> "ccw", else "cw"). + GripperLocation with the gripper clamp point and a yaw equivalent to + the joints' net world orientation. """ r = c.wrist_offset + c.elbow_offset + c.elbow_zero_offset + joints[Axis.ELBOW] sh_deg = joints[Axis.SHOULDER] @@ -80,41 +61,30 @@ def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperConfig) -> KX2GripperL yaw = radians(yaw_deg) gl = t.length if t.finger_side == "barcode_reader" else -t.length - return KX2GripperLocation( + return GripperLocation( location=Coordinate( x=wrist_x + gl * sin(yaw), y=wrist_y - gl * cos(yaw), z=wrist_z - t.z_offset, ), rotation=Rotation(z=yaw_deg), - wrist="ccw" if joints[Axis.WRIST] >= 0 else "cw", ) -def ik(pose: KX2GripperLocation, c: KX2Config, t: GripperConfig) -> Dict[Axis, float]: +def ik(pose: GripperLocation, c: KX2Config, t: GripperConfig) -> Dict[Axis, float]: """Inverse kinematics. Args: - pose: KX2GripperLocation. Requires pose.wrist to be "cw" or "ccw" — - "closest" semantics live in the backend wrapper (fill None with the - current joint's sign, then call `snap_to_current` after). + pose: target gripper pose. rotation.x/y must be 0. c: arm configuration (drive-read calibration). t: gripper tooling (user-supplied geometry). Returns: joints dict {Axis.SHOULDER: deg, Axis.Z: mm, Axis.ELBOW: mm, Axis.WRIST: deg}. - J4 is in (-360°, 0°] when wrist="cw" and [0°, 360°) when wrist="ccw" - (J4 ≈ 0 satisfies both, up to `c.eps`). - - Note: J4 is not constrained to wrist travel limits. The backend's - `_cart_to_joints` calls `snap_to_current` immediately after, which - pulls J4 toward the current (in-range) wrist position by 360° - multiples. Direct callers of `ik` must handle travel range themselves. + J4 is the canonical (-180°, 180°] solution; `snap_to_current` shifts + it to the closest 360° wrap of the current J4 for actual motion. Raises: - ValueError if pose.wrist is not "cw" or "ccw". IKError if the requested rotation has an x or y component. """ - if pose.wrist not in ("cw", "ccw"): - raise ValueError(f"pose.wrist must be 'cw' or 'ccw', got {pose.wrist!r}") if pose.rotation.x != 0 or pose.rotation.y != 0: raise IKError("Only Z rotation is supported for KX2") @@ -134,34 +104,17 @@ def ik(pose: KX2GripperLocation, c: KX2Config, t: GripperConfig) -> Dict[Axis, f shoulder = 180.0 elbow = hypot(x, y) - c.wrist_offset - c.elbow_offset - c.elbow_zero_offset - wrist = pose.rotation.z - shoulder - # Enforce requested sign. Tolerance on the check so J4 values within FP - # dust of 0 aren't pushed to ±360; J4 ≈ 0 satisfies both conventions. - if pose.wrist == "cw" and wrist > c.eps: - wrist -= 360.0 - elif pose.wrist == "ccw" and wrist < -c.eps: - wrist += 360.0 return {Axis.SHOULDER: shoulder, Axis.Z: wrist_z, Axis.ELBOW: elbow, Axis.WRIST: wrist} def snap_to_current( - joints: Dict[Axis, float], current: Dict[Axis, float], wrist: Optional[Wrist] = None + joints: Dict[Axis, float], current: Dict[Axis, float] ) -> Dict[Axis, float]: - """Shift rotary joints by 360° multiples toward `current`. Z and elbow are - prismatic and pass through unchanged. - - If `wrist` is "cw" or "ccw", re-enforce that sign half on J4 after the - shift (use this when the caller explicitly chose a sign and wants it - preserved even at the cost of extra motion). If `wrist` is None, the - shift is unconditional — the user wanted "closest", so trust the snap. - """ + """Shift rotary joints by 360° multiples toward `current`. Z and elbow + are prismatic and pass through unchanged.""" out = dict(joints) for axis in (Axis.SHOULDER, Axis.WRIST): out[axis] += 360 * round((current[axis] - out[axis]) / 360) - if wrist == "ccw" and out[Axis.WRIST] < 0: - out[Axis.WRIST] += 360 - elif wrist == "cw" and out[Axis.WRIST] > 0: - out[Axis.WRIST] -= 360 return out diff --git a/pylabrobot/paa/kx2/tests/kinematics_tests.py b/pylabrobot/paa/kx2/tests/kinematics_tests.py index 54c2ed62146..ebcf1a469ac 100644 --- a/pylabrobot/paa/kx2/tests/kinematics_tests.py +++ b/pylabrobot/paa/kx2/tests/kinematics_tests.py @@ -1,10 +1,11 @@ import math import unittest +from pylabrobot.capabilities.arms.standard import GripperLocation from pylabrobot.paa.kx2 import kinematics from pylabrobot.paa.kx2.config import Axis, AxisConfig, GripperConfig, KX2Config from pylabrobot.paa.kx2.driver import JointMoveDirection -from pylabrobot.paa.kx2.kinematics import IKError, KX2GripperLocation +from pylabrobot.paa.kx2.kinematics import IKError from pylabrobot.resources import Coordinate, Rotation @@ -41,18 +42,12 @@ def _config( ) -def _approx(a: float, b: float, eps: float = 1e-9) -> bool: - return abs(a - b) < eps - - class FKIKRoundTrip(unittest.TestCase): - def test_roundtrip_ccw(self): + def test_roundtrip(self): c = _config() g = GripperConfig(length=15.0, z_offset=3.0) - pose = KX2GripperLocation( - location=Coordinate(x=100, y=200, z=50), - rotation=Rotation(z=30), - wrist="ccw", + pose = GripperLocation( + location=Coordinate(x=100, y=200, z=50), rotation=Rotation(z=30) ) joints = kinematics.ik(pose, c, g) back = kinematics.fk(joints, c, g) @@ -61,29 +56,12 @@ def test_roundtrip_ccw(self): self.assertAlmostEqual(back.location.z, pose.location.z, places=9) self.assertAlmostEqual(back.rotation.z, pose.rotation.z, places=9) - def test_roundtrip_cw_yields_same_world_pose(self): - """cw and ccw produce J4 360° apart but represent the same physical pose.""" - c = _config() - g = GripperConfig(length=15.0, z_offset=3.0) - pose = KX2GripperLocation( - location=Coordinate(x=100, y=200, z=50), rotation=Rotation(z=30), wrist="cw" - ) - j_cw = kinematics.ik(pose, c, g) - j_ccw = kinematics.ik(KX2GripperLocation(**{**pose.__dict__, "wrist": "ccw"}), c, g) - self.assertAlmostEqual(j_ccw[Axis.WRIST] - j_cw[Axis.WRIST], 360.0, places=9) - # FK on either should land at the original pose. - for j in (j_cw, j_ccw): - back = kinematics.fk(j, c, g) - self.assertAlmostEqual(back.location.x, pose.location.x, places=9) - self.assertAlmostEqual(back.location.y, pose.location.y, places=9) - self.assertAlmostEqual(back.rotation.z, pose.rotation.z, places=9) - def test_roundtrip_at_origin_yaw_zero(self): """Sanity: a pose at (0, R, Z, 0°) lands shoulder=0°.""" c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0) g = GripperConfig() - pose = KX2GripperLocation( - location=Coordinate(x=0, y=300, z=10), rotation=Rotation(z=0), wrist="ccw" + pose = GripperLocation( + location=Coordinate(x=0, y=300, z=10), rotation=Rotation(z=0) ) joints = kinematics.ik(pose, c, g) self.assertAlmostEqual(joints[Axis.SHOULDER], 0.0, places=9) @@ -92,73 +70,12 @@ def test_roundtrip_at_origin_yaw_zero(self): self.assertAlmostEqual(joints[Axis.WRIST], 0.0, places=9) -class IKWristSign(unittest.TestCase): - def test_cw_yields_non_positive_wrist(self): - c = _config() - g = GripperConfig() - pose = KX2GripperLocation( - location=Coordinate(x=50, y=300, z=20), rotation=Rotation(z=45), wrist="cw" - ) - joints = kinematics.ik(pose, c, g) - self.assertLessEqual(joints[Axis.WRIST], c.eps) - - def test_ccw_yields_non_negative_wrist(self): - c = _config() - g = GripperConfig() - pose = KX2GripperLocation( - location=Coordinate(x=50, y=300, z=20), rotation=Rotation(z=45), wrist="ccw" - ) - joints = kinematics.ik(pose, c, g) - self.assertGreaterEqual(joints[Axis.WRIST], -c.eps) - - def test_wrist_near_zero_satisfies_both(self): - """When the natural J4 is exactly 0, neither cw nor ccw should shift it.""" - # Construct a pose where wrist - shoulder = 0 naturally: - # rotation.z == shoulder. Pick rotation.z = -atan2(x, y) in degrees. - x, y = 100.0, 100.0 - yaw_deg = -math.degrees(math.atan2(x, y)) # ≈ -45° - pose = KX2GripperLocation( - location=Coordinate(x=x, y=y, z=0), - rotation=Rotation(z=yaw_deg), - wrist="ccw", - ) - # Use zero gripper offsets so location maps directly to wrist axis. - c0 = _config() - g0 = GripperConfig() - joints = kinematics.ik(pose, c0, g0) - self.assertAlmostEqual(joints[Axis.WRIST], 0.0, places=6) - - pose_cw = KX2GripperLocation(**{**pose.__dict__, "wrist": "cw"}) - joints_cw = kinematics.ik(pose_cw, c0, g0) - self.assertAlmostEqual(joints_cw[Axis.WRIST], 0.0, places=6) - - class IKErrors(unittest.TestCase): - def test_none_wrist_raises_valueerror(self): - c = _config() - g = GripperConfig() - pose = KX2GripperLocation( - location=Coordinate(x=0, y=100, z=0), rotation=Rotation(z=0), wrist=None - ) - with self.assertRaises(ValueError): - kinematics.ik(pose, c, g) - - def test_invalid_wrist_string_raises_valueerror(self): - c = _config() - g = GripperConfig() - pose = KX2GripperLocation( - location=Coordinate(x=0, y=100, z=0), rotation=Rotation(z=0), wrist="up", # type: ignore - ) - with self.assertRaises(ValueError): - kinematics.ik(pose, c, g) - def test_x_rotation_raises_ikerror(self): c = _config() g = GripperConfig() - pose = KX2GripperLocation( - location=Coordinate(x=0, y=100, z=0), - rotation=Rotation(x=10, z=0), - wrist="ccw", + pose = GripperLocation( + location=Coordinate(x=0, y=100, z=0), rotation=Rotation(x=10, z=0) ) with self.assertRaises(IKError): kinematics.ik(pose, c, g) @@ -166,78 +83,47 @@ def test_x_rotation_raises_ikerror(self): def test_y_rotation_raises_ikerror(self): c = _config() g = GripperConfig() - pose = KX2GripperLocation( - location=Coordinate(x=0, y=100, z=0), - rotation=Rotation(y=10, z=0), - wrist="ccw", + pose = GripperLocation( + location=Coordinate(x=0, y=100, z=0), rotation=Rotation(y=10, z=0) ) with self.assertRaises(IKError): kinematics.ik(pose, c, g) -class FKResult(unittest.TestCase): - def test_fk_sets_wrist_field_from_j4_sign(self): - c = _config() - g = GripperConfig() - j_pos = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 100.0, Axis.WRIST: 45.0} - self.assertEqual(kinematics.fk(j_pos, c, g).wrist, "ccw") - j_neg = {**j_pos, Axis.WRIST: -45.0} - self.assertEqual(kinematics.fk(j_neg, c, g).wrist, "cw") - j_zero = {**j_pos, Axis.WRIST: 0.0} - self.assertEqual(kinematics.fk(j_zero, c, g).wrist, "ccw") # 0 ≥ 0 -> ccw - - class SnapToCurrent(unittest.TestCase): def test_shifts_wrist_toward_current(self): """J4=20 with current=380 should snap to 380 (closer than 20).""" j = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 20.0} cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 380.0} - out = kinematics.snap_to_current(j, cur, "ccw") + out = kinematics.snap_to_current(j, cur) self.assertAlmostEqual(out[Axis.WRIST], 380.0) def test_shifts_shoulder_toward_current(self): """Shoulder is rotary too — also snaps.""" j = {Axis.SHOULDER: -170.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0} cur = {Axis.SHOULDER: 200.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0} - out = kinematics.snap_to_current(j, cur, "ccw") + out = kinematics.snap_to_current(j, cur) self.assertAlmostEqual(out[Axis.SHOULDER], 190.0) # -170 + 360 def test_prismatic_axes_pass_through(self): """Z and elbow are not 360°-modulo; they should be untouched.""" j = {Axis.SHOULDER: 0.0, Axis.Z: 100.0, Axis.ELBOW: 50.0, Axis.WRIST: 0.0} cur = {Axis.SHOULDER: 0.0, Axis.Z: 999.0, Axis.ELBOW: 999.0, Axis.WRIST: 0.0} - out = kinematics.snap_to_current(j, cur, "ccw") + out = kinematics.snap_to_current(j, cur) self.assertEqual(out[Axis.Z], 100.0) self.assertEqual(out[Axis.ELBOW], 50.0) - def test_re_enforces_ccw_after_snap(self): - """Explicit ccw: snap might land J4 negative; ccw re-enforce shifts +360.""" - j = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 350.0} - cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0} - out = kinematics.snap_to_current(j, cur, "ccw") - # snap shifts 350 -> -10 (toward current 0); ccw re-enforce -> +350 - self.assertAlmostEqual(out[Axis.WRIST], 350.0) - - def test_re_enforces_cw_after_snap(self): - """Symmetric: explicit cw re-enforce shifts a positive J4 negative.""" - j = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: -350.0} - cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0} - out = kinematics.snap_to_current(j, cur, "cw") - # snap shifts -350 -> 10 (toward current 0); cw re-enforce -> -350 - self.assertAlmostEqual(out[Axis.WRIST], -350.0) - - def test_closest_mode_does_not_re_enforce(self): - """wrist=None means closest: trust the snap, don't shift back.""" + def test_pure_closest_no_re_enforce(self): + """The wrist drive wraps freely — snap pulls to whichever 360° wrap is closest.""" j = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 350.0} cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0} - out = kinematics.snap_to_current(j, cur, None) - # Pure shift toward current: 350 - 360 = -10. No re-enforce. + out = kinematics.snap_to_current(j, cur) self.assertAlmostEqual(out[Axis.WRIST], -10.0) def test_no_shift_when_already_close(self): j = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 30.0} cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 25.0} - out = kinematics.snap_to_current(j, cur, "ccw") + out = kinematics.snap_to_current(j, cur) self.assertAlmostEqual(out[Axis.WRIST], 30.0) @@ -262,17 +148,14 @@ def test_proximity_side_negates_gripper_offset(self): self.assertAlmostEqual( p_bc.location.y - wrist_y, -g_bc.length * math.cos(yaw), delta=1e-5 ) - # z and yaw are independent of finger side. self.assertAlmostEqual(p_bc.location.z, p_pr.location.z, places=9) self.assertAlmostEqual(p_bc.rotation.z, p_pr.rotation.z, places=9) def test_proximity_roundtrip(self): c = _config() g = GripperConfig(length=15.0, z_offset=3.0, finger_side="proximity_sensor") - pose = KX2GripperLocation( - location=Coordinate(x=100, y=200, z=50), - rotation=Rotation(z=30), - wrist="ccw", + pose = GripperLocation( + location=Coordinate(x=100, y=200, z=50), rotation=Rotation(z=30) ) joints = kinematics.ik(pose, c, g) back = kinematics.fk(joints, c, g) @@ -286,8 +169,8 @@ def test_ik_elbow_differs_by_twice_gripper_length(self): shoulder=0 but the wrist sits 2*gripper_length further out for barcode_reader (gripper points +y away from base) than for proximity_sensor (gripper points -y back toward base).""" - pose = KX2GripperLocation( - location=Coordinate(x=0, y=300, z=0), rotation=Rotation(z=0), wrist="ccw" + pose = GripperLocation( + location=Coordinate(x=0, y=300, z=0), rotation=Rotation(z=0) ) c = _config() g_bc = GripperConfig(length=15.0, z_offset=3.0, finger_side="barcode_reader") @@ -304,8 +187,8 @@ def test_negative_180_snaps_to_positive(self): """A pose pointing exactly along -y has shoulder = ±180; we snap to +180.""" c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0) g = GripperConfig() - pose = KX2GripperLocation( - location=Coordinate(x=0, y=-100, z=0), rotation=Rotation(z=180), wrist="ccw" + pose = GripperLocation( + location=Coordinate(x=0, y=-100, z=0), rotation=Rotation(z=180) ) joints = kinematics.ik(pose, c, g) self.assertAlmostEqual(joints[Axis.SHOULDER], 180.0, places=9) From de8773e1f9e154a57cfc9b3d3e06fbe3d393face Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 17:01:51 -0700 Subject: [PATCH 60/93] KX2: motor_enable retry covers both directions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The disable path single-shot wrote CW=7/CW=6, slept 100 ms, raised if MO didn't go to 0. Same drives that need several seconds to come up after a fault can lag past a single settle on the way down too — a failed disable mid-start_freedrive_mode aborted the loop with shoulder+Z free and elbow+wrist locked, arm gravity-dependent. Lifted the retry loop and the verify-MO out of the if state: branch so enable and disable share the 20-attempt / ~10 s budget. Disable now raises CanError instead of RuntimeError, matching the enable path; the failure mode is the same class. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/driver.py | 57 +++++++++++++++++------------------- 1 file changed, 27 insertions(+), 30 deletions(-) diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index 58d709afea6..ff4ce20d80b 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -1100,6 +1100,11 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N rail and the servo gripper. Caller picks the path; the driver does not know about robot topology. + + Drives sometimes need several seconds after a fault / power-rail bounce + before they accept enable, and disable can lag past a single 100 ms + settle for the same reason — the retry budget covers both directions + so a slow drive doesn't leave the arm half-enabled mid-freedrive. """ if state: # Clear sticky EMCY state from any prior fault on this drive so the @@ -1107,43 +1112,35 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N # clscanmotor.cs:4481 ("EmcyMoveErrorReceived = false" before re-enable) # plus the per-axis PVT-queue clear at clscanmotor.cs:4050-4051. self.clear_emcy_state(node_id=node_id) - # Drives sometimes need several seconds after a fault / power-rail - # bounce before they accept enable. Spread retries over ~10s rather - # than blasting 5 in <1s. - max_attempts = 20 - inter_attempt_sleep_s = 0.5 - for attempt in range(1, max_attempts + 1): - if not use_ds402: - await self.write(node_id, "MO", 0, 1) - else: - # DS402 enable sequence: Fault -> Shutdown -> Switched On -> Op Enabled. - # Matches the C# reference (clscanmotor.cs:4495-4509): back-to-back - # CW writes, a single 100 ms settle at the end, then MO query. - for cw in (0, 128, 6, 7, 15): - await self._control_word_set(node_id=node_id, value=cw) - await asyncio.sleep(0.1) - left = await self.query_int(node_id, "MO", 0) - if left == 1: - break - logger.warning( - "motor_enable attempt %d/%d failed for node %d (MO=%s); retrying", - attempt, max_attempts, node_id, left, - ) - await asyncio.sleep(inter_attempt_sleep_s) - else: - raise CanError(f"Motor failed to enable (node_id = {node_id}) after {max_attempts} attempts") - else: + + want = 1 if state else 0 + max_attempts = 20 + inter_attempt_sleep_s = 0.5 + for attempt in range(1, max_attempts + 1): if not use_ds402: - await self.write(node_id, "MO", 0, 0) + await self.write(node_id, "MO", 0, want) + elif state: + # DS402 enable sequence: Fault -> Shutdown -> Switched On -> Op Enabled. + # Matches the C# reference (clscanmotor.cs:4495-4509): back-to-back + # CW writes, a single 100 ms settle at the end, then MO query. + for cw in (0, 128, 6, 7, 15): + await self._control_word_set(node_id=node_id, value=cw) else: # DS402 disable: Op Enabled -> Switched On -> Ready to Switch On. # Matches C# (clscanmotor.cs:4540-4543) — back-to-back, no inter-CW sleep. await self._control_word_set(node_id=node_id, value=7) await self._control_word_set(node_id=node_id, value=6) await asyncio.sleep(0.1) - left = await self.query_int(node_id, "MO", 0) - if left != 0: - raise RuntimeError(f"Motor failed to disable (node_id = {node_id})") + mo = await self.query_int(node_id, "MO", 0) + if mo == want: + return + logger.warning( + "motor_enable(state=%s) attempt %d/%d failed for node %d (MO=%s); retrying", + state, attempt, max_attempts, node_id, mo, + ) + await asyncio.sleep(inter_attempt_sleep_s) + verb = "enable" if state else "disable" + raise CanError(f"Motor failed to {verb} (node_id = {node_id}) after {max_attempts} attempts") # --- motion primitives -------------------------------------------------- From 5bf36e64f8b7f45fbb8925be14c9a09fccadb4a1 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 17:18:59 -0700 Subject: [PATCH 61/93] KX2: move trajectory planner from arm_backend.py into kinematics.py MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit calculate_move_abs_all_axes was 256 lines of pure math sitting on the async backend because it called request_joint_position internally. Lifted the driver round-trip up to _motors_move_joint_locked and dropped the math into kinematics.py as a pure function plan_joint_move(current, target, cfg, gripper_cfg, *, max_gripper_speed, max_gripper_acceleration). Backend orchestrator now reads current, calls the planner, dispatches the plan — no behavior change, planner is unit-testable without a driver. The encoder/joint elbow asin conversions (convert_elbow_position_to_angle / _to_position) and the _wrap_to_range / _profile helpers also moved — all pure-math kin-adjacent code now lives in kinematics.py. arm_backend.py shrinks by ~330 lines, kinematics.py grows by ~290. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/arm_backend.py | 356 ++---------------------------- pylabrobot/paa/kx2/kinematics.py | 305 ++++++++++++++++++++++++- 2 files changed, 322 insertions(+), 339 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index a57be9f70ca..ccf402584b5 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -1,14 +1,12 @@ import asyncio import dataclasses import logging -import math import time import warnings from contextlib import asynccontextmanager from enum import IntEnum from typing import Dict, List, Optional, Union -from pylabrobot.capabilities.arms import kinematics as arm_kinematics from pylabrobot.capabilities.arms.backend import ( CanFreedrive, HasJoints, @@ -654,44 +652,17 @@ def _cfg(self) -> KX2Config: raise RuntimeError("KX2 not set up — call setup() first") return self._config - def convert_elbow_position_to_angle(self, elbow_pos: float) -> float: - max_travel = self._cfg.axes[Axis.ELBOW].max_travel - denom = max_travel + self._cfg.elbow_zero_offset - - if elbow_pos > max_travel: - x = (2.0 * max_travel - elbow_pos + self._cfg.elbow_zero_offset) / denom - angle = math.asin(x) * (180.0 / math.pi) - elbow_angle = 90.0 + angle - else: - x = (elbow_pos + self._cfg.elbow_zero_offset) / denom - angle = math.asin(x) * (180.0 / math.pi) - elbow_angle = angle - - return elbow_angle - - def convert_elbow_angle_to_position(self, elbow_angle_deg: float) -> float: - elbow_pos = (self._cfg.axes[Axis.ELBOW].max_travel + self._cfg.elbow_zero_offset) * math.sin( - elbow_angle_deg * (math.pi / 180.0) - ) - self._cfg.elbow_zero_offset - - if elbow_angle_deg > 90.0: - elbow_pos = 2.0 * self._cfg.axes[Axis.ELBOW].max_travel - elbow_pos - - return elbow_pos - async def motor_get_current_position(self, axis: Axis) -> float: raw = await self.driver.motor_get_current_position( node_id=int(axis), pu=self._cfg.axes[axis].unlimited_travel, ) c = self._cfg.axes[axis].motor_conversion_factor if axis == Axis.ELBOW: - return self.convert_elbow_angle_to_position(elbow_angle_deg=raw / c) - else: - if c == 0: - logger.warning("Axis %s has conversion factor of 0", axis) - return 0.0 - else: - return raw / c + return kinematics.convert_elbow_angle_to_position(self._cfg, raw / c) + if c == 0: + logger.warning("Axis %s has conversion factor of 0", axis) + return 0.0 + return raw / c async def read_input(self, axis: Axis, input_num: int) -> bool: return await self.driver.read_input(node_id=int(axis), input_num=0x10 + input_num) @@ -798,303 +769,6 @@ async def find_z_with_proximity_sensor( ) return await self.motor_get_current_position(Axis.Z) - @staticmethod - def _wrap_to_range(x: float, lo: float, hi: float) -> float: - span = hi - lo - if span == 0: - return lo - k = math.trunc(x / span) - x = x - k * span - if x < lo: - x += span - if x == hi: - x -= span - return x - - @staticmethod - def _profile(dist: float, v: float, a: float) -> tuple: - """ - Returns (v, a, t_acc, t_total) after applying triangular fallback if - needed. If the distance is short, you cannot reach v before you must - decelerate. - """ - if dist <= 0: - return v, a, 0.0, 0.0 - if a <= 0: - # degenerate; avoid crash - return max(v, 1e-9), 1e-9, 0.0, dist / max(v, 1e-9) - - t_acc = v / a - d_acc = 0.5 * a * t_acc * t_acc - - # triangular? - if 2.0 * d_acc > dist: - t_acc = math.sqrt(dist / a) - v = a * t_acc - return v, a, t_acc, 2.0 * t_acc - - d_cruise = dist - 2.0 * d_acc - t_cruise = d_cruise / max(v, 1e-9) - return v, a, t_acc, t_cruise + 2.0 * t_acc - - async def calculate_move_abs_all_axes( - self, - cmd_pos: Dict[Axis, float], - params: Optional["KX2ArmBackend.JointMoveParams"] = None, - ) -> Optional[MotorsMovePlan]: - if params is None: - params = KX2ArmBackend.JointMoveParams() - target = cmd_pos.copy() - axes = list(target.keys()) - - enc_pos: Dict[Axis, float] = {} - enc_vel: Dict[Axis, float] = {} - enc_accel: Dict[Axis, float] = {} - skip_ax: Dict[Axis, bool] = {} - - # input validation / travel limits / done-wait logic - if params.max_gripper_speed is not None and params.max_gripper_speed <= 0.0: - raise ValueError( - f"max_gripper_speed must be positive, got {params.max_gripper_speed}" - ) - if params.max_gripper_acceleration is not None and params.max_gripper_acceleration <= 0.0: - raise ValueError( - f"max_gripper_acceleration must be positive, got {params.max_gripper_acceleration}" - ) - - # Travel-limit bounds check. Mirrors C# MoveAbsoluteSingleAxisPrivate - # (KX2RobotControl.cs:4624-4649): snap if within 0.1 of the limit, raise - # otherwise. Without this, sending an out-of-range target (e.g. gripper - # width 600 when max_travel ~30) parks the drive trying to reach an - # unreachable position — MS never returns to 0 and every subsequent - # command on that axis hangs until full re-setup. Run before the elbow - # position->angle conversion so max_travel/min_travel are compared in - # the same space the user passed in. - for ax in axes: - ax_cfg = self._cfg.axes[ax] - if ax_cfg.unlimited_travel: - continue - t = target[ax] - if t > ax_cfg.max_travel: - if t - ax_cfg.max_travel < 0.1: - target[ax] = ax_cfg.max_travel - else: - raise ValueError( - f"Axis {ax.name} target {t} exceeds max_travel {ax_cfg.max_travel}" - ) - elif t < ax_cfg.min_travel: - if ax_cfg.min_travel - t < 0.1: - target[ax] = ax_cfg.min_travel - else: - raise ValueError( - f"Axis {ax.name} target {t} below min_travel {ax_cfg.min_travel}" - ) - - # Snapshot in cmd_pos units (elbow as linear extension, not angle) for - # the gripper-speed cap helper, which needs joints in `kinematics.fk`'s - # natural units. - target_cmd_units = dict(target) - - # Convert elbow cmd from position->angle for planning math - if Axis.ELBOW in axes: - target[Axis.ELBOW] = self.convert_elbow_position_to_angle(target[Axis.ELBOW]) - - # Clearance check - if Axis.Z in axes: - if ( - target[Axis.Z] < self._cfg.axes[Axis.Z].min_travel + self._cfg.base_to_gripper_clearance_z - and target[Axis.ELBOW] < self._cfg.base_to_gripper_clearance_arm - ): - raise ValueError("Base-to-gripper clearance violated") - - # Determine current/start positions - curr = await self.request_joint_position() - curr_cmd_units = dict(curr) - - # Elbow: convert both target and start to angle for distance math - if Axis.ELBOW in curr: - curr[Axis.ELBOW] = self.convert_elbow_position_to_angle(curr[Axis.ELBOW]) - - # Handle unlimited travel normalization when direction != NORMAL - for ax in axes: - if ( - self._cfg.axes[ax].unlimited_travel - and self._cfg.axes[ax].joint_move_direction != JointMoveDirection.Normal - ): - target[ax] = self._wrap_to_range(target[ax], self._cfg.axes[ax].min_travel, self._cfg.axes[ax].max_travel) - - # Distances, skip flags, initial v/a per axis - dist: Dict[Axis, float] = {} - v: Dict[Axis, float] = {} - a: Dict[Axis, float] = {} - accel_time: Dict[Axis, float] = {} - total_time: Dict[Axis, float] = {} - - # Direction-aware deltas in cmd_pos units (elbow as linear extension). - # Identical logic to the dist computation below, but evaluated against - # the un-converted joints so the cap helper sees the trajectory the - # arm actually walks. Skipped axes get delta 0. - cap_deltas: Dict[Axis, float] = {} - for ax in axes: - if self._cfg.axes[ax].unlimited_travel: - d = target_cmd_units[ax] - curr_cmd_units.get(ax, 0.0) - span = self._cfg.axes[ax].max_travel - self._cfg.axes[ax].min_travel - dir_ = self._cfg.axes[ax].joint_move_direction - if dir_ == JointMoveDirection.Clockwise and d > 0.01: - d -= span - elif dir_ == JointMoveDirection.Counterclockwise and d < -0.01: - d += span - elif dir_ == JointMoveDirection.ShortestWay: - if d > 180.0: - d -= span - elif d < -180.0: - d += span - cap_deltas[ax] = d - else: - cap_deltas[ax] = target_cmd_units[ax] - curr_cmd_units.get(ax, 0.0) - - # Per-axis caps from the gripper-speed limit. Helper iterates the joint - # path in `kinematics.fk`'s native units (elbow as linear extension, - # everything else as the user specified). Servo gripper isn't in fk, so - # always run it at firmware max regardless of cap. Pass start joints - # filled in with curr_cmd_units defaults so fk has all four arm axes - # even when the move only touches a subset. - arm_axes = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) - fk_start = {ax: curr_cmd_units[ax] for ax in arm_axes if ax in curr_cmd_units} - fk_deltas = {ax: cap_deltas.get(ax, 0.0) for ax in arm_axes if ax in fk_start} - capped_v: Dict[Axis, float] = {} - capped_a: Dict[Axis, float] = {} - fk = lambda j: kinematics.fk(j, self._cfg, self._gripper_config).location - if params.max_gripper_speed is not None and fk_start: - result = arm_kinematics.joint_velocities_for_max_gripper_speed( - fk=fk, - joints_start=fk_start, - joint_deltas=fk_deltas, - joint_max_velocities={ax: self._cfg.axes[ax].max_vel for ax in fk_start}, - max_gripper_speed=params.max_gripper_speed, - num_samples=1000, - eps=1e-3, - ) - capped_v = {ax: abs(v) for ax, v in result.items()} - if params.max_gripper_acceleration is not None and fk_start: - result = arm_kinematics.joint_velocities_for_max_gripper_speed( - fk=fk, - joints_start=fk_start, - joint_deltas=fk_deltas, - joint_max_velocities={ax: self._cfg.axes[ax].max_accel for ax in fk_start}, - max_gripper_speed=params.max_gripper_acceleration, - num_samples=1000, - eps=1e-3, - ) - capped_a = {ax: abs(a) for ax, a in result.items()} - - for ax in axes: - if self._cfg.axes[ax].unlimited_travel: - d = target[ax] - curr[ax] - span = self._cfg.axes[ax].max_travel - self._cfg.axes[ax].min_travel - dir_ = self._cfg.axes[ax].joint_move_direction - - if dir_ == JointMoveDirection.Clockwise and d > 0.01: - d -= span - elif dir_ == JointMoveDirection.Counterclockwise and d < -0.01: - d += span - elif dir_ == JointMoveDirection.ShortestWay: - if d > 180.0: - d -= span - elif d < -180.0: - d += span - - dist[ax] = abs(d) - else: - dist[ax] = abs(target[ax] - curr[ax]) - - skip_ax[ax] = abs(dist[ax]) < 0.01 - - v[ax] = capped_v.get(ax, self._cfg.axes[ax].max_vel) - a[ax] = capped_a.get(ax, self._cfg.axes[ax].max_accel) - - if not skip_ax[ax] and a[ax] > 0: - v[ax], a[ax], accel_time[ax], total_time[ax] = self._profile( - dist[ax], v[ax], a[ax] - ) - else: - total_time[ax] = 0.0 - accel_time[ax] = 0.0 - - if all(skip_ax[ax] for ax in axes): - return None # nothing to do - - # Pick axis with max accel_time among non-skipped; match others to that accel_time - lead_acc_ax = max( - (ax for ax in axes if not skip_ax[ax]), - key=lambda ax: accel_time[ax], - ) - lead_acc_t = accel_time[lead_acc_ax] - - for ax in axes: - if ax == lead_acc_ax or skip_ax[ax]: - continue - if accel_time[ax] > lead_acc_t: - v[ax] = lead_acc_t * a[ax] - elif accel_time[ax] < lead_acc_t: - a[ax] = v[ax] / max(lead_acc_t, 1e-9) - - # Recompute times after accel sync - for ax in axes: - if skip_ax[ax]: - total_time[ax] = 0.0 - continue - v[ax], a[ax], _, total_time[ax] = self._profile(dist[ax], v[ax], a[ax]) - - # Pick axis with max total_time; scale others to match its total_time - lead_time_ax = max(axes, key=lambda ax: total_time[ax]) - lead_T = total_time[lead_time_ax] - - for ax in axes: - if ax == lead_time_ax or skip_ax[ax]: - continue - denom = v[ax] * (lead_T - (v[ax] / max(a[ax], 1e-9))) - if abs(denom) < 1e-12: - continue - k = dist[ax] / denom - v[ax] *= k - a[ax] *= k - - # Final recompute and final move time - for ax in axes: - if skip_ax[ax]: - total_time[ax] = 0.0 - continue - v[ax], a[ax], _, total_time[ax] = self._profile(dist[ax], v[ax], a[ax]) - - move_time = max(total_time[ax] for ax in axes) - - # Convert back to encoder units (and elbow back to "position" space) - for ax in axes: - conv = self._cfg.axes[ax].motor_conversion_factor - enc_pos[ax] = target[ax] * conv - - if skip_ax[ax]: - enc_vel[ax] = 1000.0 - enc_accel[ax] = 1000.0 - else: - enc_vel[ax] = max(v[ax] * abs(conv), 1.0) - enc_accel[ax] = max(a[ax] * abs(conv), 1.0) - - return MotorsMovePlan( - moves=[ - MotorMoveParam( - node_id=int(ax), - position=int(round(enc_pos[ax])), - velocity=int(round(enc_vel[ax])), - acceleration=int(round(enc_accel[ax])), - direction=self._cfg.axes[ax].joint_move_direction, - ) - for ax in axes - ], - move_time=move_time, - ) - async def motors_move_joint( self, cmd_pos: Dict[Axis, float], @@ -1111,11 +785,19 @@ async def _motors_move_joint_locked( """Caller MUST hold _motion_guard. Used by find_z's spawned poll/move task, which can't re-enter the guard from a fresh asyncio Task.""" logger.debug("motors_move_joint cmd_pos=%s", cmd_pos) - plan = await self.calculate_move_abs_all_axes(cmd_pos=cmd_pos, params=params) - - if plan is None: # if every axis is skipped, exit + if params is None: + params = KX2ArmBackend.JointMoveParams() + current = {Axis(k): v for k, v in (await self.request_joint_position()).items()} + plan = kinematics.plan_joint_move( + current=current, + target=cmd_pos, + cfg=self._cfg, + gripper_cfg=self._gripper_config, + max_gripper_speed=params.max_gripper_speed, + max_gripper_acceleration=params.max_gripper_acceleration, + ) + if plan is None: # every axis a no-op return - await self._motors_move_absolute_execute_locked(plan) async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: @@ -1640,9 +1322,9 @@ async def _yeet_build_axis_move( dist = abs(d) if ax_cfg.unlimited_travel and direction != JointMoveDirection.Normal: - target = KX2ArmBackend._wrap_to_range(target, ax_cfg.min_travel, ax_cfg.max_travel) + target = kinematics._wrap_to_range(target, ax_cfg.min_travel, ax_cfg.max_travel) - _, _, t_acc, t_total = KX2ArmBackend._profile(dist, v_phys, a_phys) + _, _, t_acc, t_total = kinematics._profile(dist, v_phys, a_phys) move = MotorMoveParam( node_id=int(ax), position=int(round(target * conv)), diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index 8da43da98e5..b2da894a6e6 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -22,11 +22,17 @@ (CCW positive looking down). """ -from math import atan2, cos, degrees, hypot, radians, sin -from typing import Dict +from math import asin, atan2, cos, degrees, hypot, pi, radians, sin, sqrt, trunc +from typing import Dict, Optional +from pylabrobot.capabilities.arms import kinematics as arm_kinematics from pylabrobot.capabilities.arms.standard import GripperLocation from pylabrobot.paa.kx2.config import Axis, GripperConfig, KX2Config +from pylabrobot.paa.kx2.driver import ( + JointMoveDirection, + MotorMoveParam, + MotorsMovePlan, +) from pylabrobot.resources import Coordinate, Rotation @@ -118,3 +124,298 @@ def snap_to_current( for axis in (Axis.SHOULDER, Axis.WRIST): out[axis] += 360 * round((current[axis] - out[axis]) / 360) return out + + +# --- elbow encoder/joint frame conversion ---------------------------------- +# +# The elbow is exposed to FK/IK as a linear radial extension (mm), but the +# physical motor is a rotary actuator driving a sine linkage — so the encoder +# count is sin-related to the mm-space joint value. These two functions sit +# at the encoder/joint boundary; FK/IK above and the planner below both +# operate in the linear (mm) domain. Mirrors C# `ConvertElbowPositionToAngle` +# / `ConvertElbowAngleToPosition` (KX2RobotControl.cs:2944, 2974). + +def convert_elbow_position_to_angle(cfg: KX2Config, elbow_pos: float) -> float: + max_travel = cfg.axes[Axis.ELBOW].max_travel + denom = max_travel + cfg.elbow_zero_offset + if elbow_pos > max_travel: + x = (2.0 * max_travel - elbow_pos + cfg.elbow_zero_offset) / denom + return 90.0 + asin(x) * (180.0 / pi) + x = (elbow_pos + cfg.elbow_zero_offset) / denom + return asin(x) * (180.0 / pi) + + +def convert_elbow_angle_to_position(cfg: KX2Config, elbow_angle_deg: float) -> float: + max_travel = cfg.axes[Axis.ELBOW].max_travel + elbow_pos = (max_travel + cfg.elbow_zero_offset) * sin(elbow_angle_deg * (pi / 180.0)) - cfg.elbow_zero_offset + if elbow_angle_deg > 90.0: + elbow_pos = 2.0 * max_travel - elbow_pos + return elbow_pos + + +# --- trajectory planning --------------------------------------------------- + +def _wrap_to_range(x: float, lo: float, hi: float) -> float: + span = hi - lo + if span == 0: + return lo + k = trunc(x / span) + x = x - k * span + if x < lo: + x += span + if x == hi: + x -= span + return x + + +def _profile(dist: float, v: float, a: float) -> tuple: + """Return (v, a, t_acc, t_total) with triangular fallback. If the + distance is short, you can't reach v before you must decelerate.""" + if dist <= 0: + return v, a, 0.0, 0.0 + if a <= 0: + # degenerate; avoid crash + return max(v, 1e-9), 1e-9, 0.0, dist / max(v, 1e-9) + t_acc = v / a + d_acc = 0.5 * a * t_acc * t_acc + if 2.0 * d_acc > dist: # triangular + t_acc = sqrt(dist / a) + v = a * t_acc + return v, a, t_acc, 2.0 * t_acc + d_cruise = dist - 2.0 * d_acc + t_cruise = d_cruise / max(v, 1e-9) + return v, a, t_acc, t_cruise + 2.0 * t_acc + + +def plan_joint_move( + current: Dict[Axis, float], + target: Dict[Axis, float], + cfg: KX2Config, + gripper_cfg: GripperConfig, + *, + max_gripper_speed: Optional[float] = None, + max_gripper_acceleration: Optional[float] = None, +) -> Optional[MotorsMovePlan]: + """Pure planner: joint-space target -> per-axis encoder plan. + + Caller owns the driver round-trip — pass ``current`` from + ``request_joint_position`` (linear-extension units for elbow). Returns + ``None`` if every axis would be a no-op (within 0.01 of current). + + ``max_gripper_speed`` / ``max_gripper_acceleration`` cap joint velocities + so the worst-case Cartesian gripper speed/accel along the trajectory + stays at or under the cap. + """ + if max_gripper_speed is not None and max_gripper_speed <= 0.0: + raise ValueError(f"max_gripper_speed must be positive, got {max_gripper_speed}") + if max_gripper_acceleration is not None and max_gripper_acceleration <= 0.0: + raise ValueError(f"max_gripper_acceleration must be positive, got {max_gripper_acceleration}") + + target = dict(target) + axes = list(target.keys()) + + # Travel-limit bounds check. Mirrors C# MoveAbsoluteSingleAxisPrivate + # (KX2RobotControl.cs:4624-4649): snap if within 0.1 of the limit, raise + # otherwise. Without this, an out-of-range target (e.g. gripper width 600 + # when max_travel ~30) parks the drive trying to reach an unreachable + # position — MS never returns to 0 and every subsequent command on that + # axis hangs until full re-setup. Run before the elbow position->angle + # conversion so max/min_travel are compared in the units the user passed. + for ax in axes: + ax_cfg = cfg.axes[ax] + if ax_cfg.unlimited_travel: + continue + t = target[ax] + if t > ax_cfg.max_travel: + if t - ax_cfg.max_travel < 0.1: + target[ax] = ax_cfg.max_travel + else: + raise ValueError(f"Axis {ax.name} target {t} exceeds max_travel {ax_cfg.max_travel}") + elif t < ax_cfg.min_travel: + if ax_cfg.min_travel - t < 0.1: + target[ax] = ax_cfg.min_travel + else: + raise ValueError(f"Axis {ax.name} target {t} below min_travel {ax_cfg.min_travel}") + + # Snapshot in cmd_pos units (elbow as linear extension) for the gripper- + # speed cap helper, which iterates the path in `fk`'s natural units. + target_cmd_units = dict(target) + curr_cmd_units = dict(current) + + # Convert elbow target+current from position->angle for planning math — + # the motor's vel/accel limits are encoder-rate, which is angle-rate, not + # mm-rate. Time math is done in angle units; we convert back at the end. + if Axis.ELBOW in axes: + target[Axis.ELBOW] = convert_elbow_position_to_angle(cfg, target[Axis.ELBOW]) + curr = dict(current) + if Axis.ELBOW in curr: + curr[Axis.ELBOW] = convert_elbow_position_to_angle(cfg, curr[Axis.ELBOW]) + + # Clearance check (in angle space for elbow, since base_to_gripper_clearance_arm + # is defined in the same domain in C#). + if Axis.Z in axes: + if ( + target[Axis.Z] < cfg.axes[Axis.Z].min_travel + cfg.base_to_gripper_clearance_z + and target[Axis.ELBOW] < cfg.base_to_gripper_clearance_arm + ): + raise ValueError("Base-to-gripper clearance violated") + + # Unlimited-travel normalization for non-NORMAL direction modes. + for ax in axes: + ax_cfg = cfg.axes[ax] + if ax_cfg.unlimited_travel and ax_cfg.joint_move_direction != JointMoveDirection.Normal: + target[ax] = _wrap_to_range(target[ax], ax_cfg.min_travel, ax_cfg.max_travel) + + # Direction-aware deltas in cmd_pos units (elbow as linear extension), for + # the gripper-speed cap helper. Identical logic to the dist computation + # below, but evaluated against the un-converted joints so the cap helper + # sees the trajectory the arm actually walks. + cap_deltas: Dict[Axis, float] = {} + for ax in axes: + ax_cfg = cfg.axes[ax] + if ax_cfg.unlimited_travel: + d = target_cmd_units[ax] - curr_cmd_units.get(ax, 0.0) + span = ax_cfg.max_travel - ax_cfg.min_travel + dir_ = ax_cfg.joint_move_direction + if dir_ == JointMoveDirection.Clockwise and d > 0.01: + d -= span + elif dir_ == JointMoveDirection.Counterclockwise and d < -0.01: + d += span + elif dir_ == JointMoveDirection.ShortestWay: + if d > 180.0: + d -= span + elif d < -180.0: + d += span + cap_deltas[ax] = d + else: + cap_deltas[ax] = target_cmd_units[ax] - curr_cmd_units.get(ax, 0.0) + + # Per-axis caps from the gripper-speed limit. Helper iterates the joint + # path in `fk`'s natural units. Servo gripper isn't in fk, so it always + # runs at firmware max regardless of cap. + arm_axes = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) + fk_start = {ax: curr_cmd_units[ax] for ax in arm_axes if ax in curr_cmd_units} + fk_deltas = {ax: cap_deltas.get(ax, 0.0) for ax in arm_axes if ax in fk_start} + capped_v: Dict[Axis, float] = {} + capped_a: Dict[Axis, float] = {} + fk_loc = lambda j: fk(j, cfg, gripper_cfg).location + if max_gripper_speed is not None and fk_start: + result = arm_kinematics.joint_velocities_for_max_gripper_speed( + fk=fk_loc, + joints_start=fk_start, + joint_deltas=fk_deltas, + joint_max_velocities={ax: cfg.axes[ax].max_vel for ax in fk_start}, + max_gripper_speed=max_gripper_speed, + num_samples=1000, + eps=1e-3, + ) + capped_v = {ax: abs(vv) for ax, vv in result.items()} + if max_gripper_acceleration is not None and fk_start: + result = arm_kinematics.joint_velocities_for_max_gripper_speed( + fk=fk_loc, + joints_start=fk_start, + joint_deltas=fk_deltas, + joint_max_velocities={ax: cfg.axes[ax].max_accel for ax in fk_start}, + max_gripper_speed=max_gripper_acceleration, + num_samples=1000, + eps=1e-3, + ) + capped_a = {ax: abs(aa) for ax, aa in result.items()} + + # Distances + initial v/a per axis (planning units = angle for elbow). + dist: Dict[Axis, float] = {} + v: Dict[Axis, float] = {} + a: Dict[Axis, float] = {} + accel_time: Dict[Axis, float] = {} + total_time: Dict[Axis, float] = {} + skip_ax: Dict[Axis, bool] = {} + for ax in axes: + ax_cfg = cfg.axes[ax] + if ax_cfg.unlimited_travel: + d = target[ax] - curr[ax] + span = ax_cfg.max_travel - ax_cfg.min_travel + dir_ = ax_cfg.joint_move_direction + if dir_ == JointMoveDirection.Clockwise and d > 0.01: + d -= span + elif dir_ == JointMoveDirection.Counterclockwise and d < -0.01: + d += span + elif dir_ == JointMoveDirection.ShortestWay: + if d > 180.0: + d -= span + elif d < -180.0: + d += span + dist[ax] = abs(d) + else: + dist[ax] = abs(target[ax] - curr[ax]) + + skip_ax[ax] = abs(dist[ax]) < 0.01 + v[ax] = capped_v.get(ax, ax_cfg.max_vel) + a[ax] = capped_a.get(ax, ax_cfg.max_accel) + if not skip_ax[ax] and a[ax] > 0: + v[ax], a[ax], accel_time[ax], total_time[ax] = _profile(dist[ax], v[ax], a[ax]) + else: + total_time[ax] = 0.0 + accel_time[ax] = 0.0 + + if all(skip_ax[ax] for ax in axes): + return None + + # Sync accel times to the lead axis so all axes ramp together. + lead_acc_ax = max((ax for ax in axes if not skip_ax[ax]), key=lambda ax: accel_time[ax]) + lead_acc_t = accel_time[lead_acc_ax] + for ax in axes: + if ax == lead_acc_ax or skip_ax[ax]: + continue + if accel_time[ax] > lead_acc_t: + v[ax] = lead_acc_t * a[ax] + elif accel_time[ax] < lead_acc_t: + a[ax] = v[ax] / max(lead_acc_t, 1e-9) + + for ax in axes: + if skip_ax[ax]: + total_time[ax] = 0.0 + continue + v[ax], a[ax], _, total_time[ax] = _profile(dist[ax], v[ax], a[ax]) + + # Sync total times to the lead axis. + lead_time_ax = max(axes, key=lambda ax: total_time[ax]) + lead_T = total_time[lead_time_ax] + for ax in axes: + if ax == lead_time_ax or skip_ax[ax]: + continue + denom = v[ax] * (lead_T - (v[ax] / max(a[ax], 1e-9))) + if abs(denom) < 1e-12: + continue + k = dist[ax] / denom + v[ax] *= k + a[ax] *= k + + for ax in axes: + if skip_ax[ax]: + total_time[ax] = 0.0 + continue + v[ax], a[ax], _, total_time[ax] = _profile(dist[ax], v[ax], a[ax]) + + move_time = max(total_time[ax] for ax in axes) + + # Convert back to encoder units. Elbow target is still in angle space here + # — the encoder count for an elbow joint is angle * conv, not mm * conv. + moves = [] + for ax in axes: + ax_cfg = cfg.axes[ax] + conv = ax_cfg.motor_conversion_factor + enc_pos = target[ax] * conv + if skip_ax[ax]: + enc_vel = 1000.0 + enc_accel = 1000.0 + else: + enc_vel = max(v[ax] * abs(conv), 1.0) + enc_accel = max(a[ax] * abs(conv), 1.0) + moves.append(MotorMoveParam( + node_id=int(ax), + position=int(round(enc_pos)), + velocity=int(round(enc_vel)), + acceleration=int(round(enc_accel)), + direction=ax_cfg.joint_move_direction, + )) + return MotorsMovePlan(moves=moves, move_time=move_time) From f9ca36312a7039ab4717af7c1d39a83434e41b1e Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 17:21:01 -0700 Subject: [PATCH 62/93] KX2: rename GripperConfig -> GripperParams to disambiguate from ServoGripperConfig MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit GripperConfig was the user-supplied tooling (length, z_offset, finger_side) held in self._gripper_config. ServoGripperConfig is drive-read motor calibration for the servo axis (currents, home params) held in KX2Config.servo_gripper. Both ending in "Config" with one a substring of the other suggested they were variants of one concept; they aren't. Rest of the file uses "Config" for drive-read calibration (KX2Config, AxisConfig, ServoGripperConfig). Gripper tooling is user-supplied at construction, not loaded from the bus — "Params" matches the codebase's convention for user-supplied dataclasses (PickUpParams, GripParams, JointMoveParams, CartesianMoveParams, DropParams). Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/__init__.py | 4 ++-- pylabrobot/paa/kx2/arm_backend.py | 14 ++++++------- pylabrobot/paa/kx2/config.py | 11 +++++----- pylabrobot/paa/kx2/kinematics.py | 10 ++++----- pylabrobot/paa/kx2/tests/kinematics_tests.py | 22 ++++++++++---------- 5 files changed, 31 insertions(+), 30 deletions(-) diff --git a/pylabrobot/paa/kx2/__init__.py b/pylabrobot/paa/kx2/__init__.py index 45753eea317..3ff522e3c49 100644 --- a/pylabrobot/paa/kx2/__init__.py +++ b/pylabrobot/paa/kx2/__init__.py @@ -3,7 +3,7 @@ from pylabrobot.paa.kx2.config import ( Axis, AxisConfig, - GripperConfig, + GripperParams, GripperFingerSide, KX2Config, ServoGripperConfig, @@ -14,7 +14,7 @@ __all__ = [ "Axis", "AxisConfig", - "GripperConfig", + "GripperParams", "GripperFingerSide", "IKError", "KX2", diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index ccf402584b5..0e91282fae8 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -19,7 +19,7 @@ Axis, AxisConfig, GripperFingerSide, - GripperConfig, + GripperParams, KX2Config, ServoGripperConfig, ) @@ -73,7 +73,7 @@ def __init__( self.driver = driver # Tooling is user-supplied and known at construction; KX2Config (drive- # read calibration) doesn't exist until setup runs. - self._gripper_config = GripperConfig( + self._gripper_params = GripperParams( length=float(gripper_length), z_offset=float(gripper_z_offset), finger_side=gripper_finger_side, @@ -792,7 +792,7 @@ async def _motors_move_joint_locked( current=current, target=cmd_pos, cfg=self._cfg, - gripper_cfg=self._gripper_config, + gripper_params=self._gripper_params, max_gripper_speed=params.max_gripper_speed, max_gripper_acceleration=params.max_gripper_acceleration, ) @@ -848,7 +848,7 @@ async def _cart_to_joints(self, pose: GripperLocation) -> Dict[Axis, float]: """Cartesian -> joints, snapping rotary axes to whichever 360° wrap is closest to the current joint position.""" current = {Axis(k): v for k, v in (await self.request_joint_position()).items()} - ik_joints = kinematics.ik(pose, self._cfg, self._gripper_config) + ik_joints = kinematics.ik(pose, self._cfg, self._gripper_params) return kinematics.snap_to_current(ik_joints, current) # -- capability interface (OrientableGripperArmBackend + HasJoints + CanFreedrive) -- @@ -873,7 +873,7 @@ async def request_gripper_location( self, backend_params: Optional[BackendParams] = None ) -> GripperLocation: joints = {Axis(k): v for k, v in (await self.request_joint_position()).items()} - return kinematics.fk(joints, self._cfg, self._gripper_config) + return kinematics.fk(joints, self._cfg, self._gripper_params) async def open_gripper( self, gripper_width: float, backend_params: Optional[BackendParams] = None @@ -1155,7 +1155,7 @@ async def very_dangerously_yeet( pickup_pose = await self.request_joint_position() # Auto-windup: rotate wrist to the inward angle (opposite of outward). - wrist_inward = 0.0 if self._gripper_config.finger_side == "barcode_reader" else 180.0 + wrist_inward = 0.0 if self._gripper_params.finger_side == "barcode_reader" else 180.0 while wrist_inward - pickup_pose[Axis.WRIST] > 180.0: wrist_inward -= 360.0 while wrist_inward - pickup_pose[Axis.WRIST] < -180.0: @@ -1171,7 +1171,7 @@ async def very_dangerously_yeet( joints0 = await self.request_joint_position() # Outward wrist = kinematic target (180° barcode_reader, 0° proximity). - wrist_outward = 180.0 if self._gripper_config.finger_side == "barcode_reader" else 0.0 + wrist_outward = 180.0 if self._gripper_params.finger_side == "barcode_reader" else 0.0 while wrist_outward - joints0[Axis.WRIST] > 180.0: wrist_outward -= 360.0 while wrist_outward - joints0[Axis.WRIST] < -180.0: diff --git a/pylabrobot/paa/kx2/config.py b/pylabrobot/paa/kx2/config.py index f8dd9c194e3..89cfa30c0d5 100644 --- a/pylabrobot/paa/kx2/config.py +++ b/pylabrobot/paa/kx2/config.py @@ -68,11 +68,12 @@ class AxisConfig: @dataclass -class GripperConfig: - """User-supplied gripper geometry — known at construction, never read +class GripperParams: + """User-supplied gripper tooling — known at construction, never read from the drives. Lives on :class:`KX2ArmBackend` - (``self._gripper_config``) and is passed into kinematics alongside - :class:`KX2Config`. + (``self._gripper_params``) and is passed into kinematics alongside + :class:`KX2Config`. Distinct from :class:`ServoGripperConfig`, which + is drive-read motor calibration for the servo gripper itself. Attributes: length: Distance from the wrist axis to the gripper clamp point, in @@ -113,7 +114,7 @@ class ServoGripperConfig: class KX2Config: """Drive-read calibration. Strictly contents pulled off the bus at setup; tooling (gripper geometry) lives separately on - :class:`GripperConfig` and is owned by the backend.""" + :class:`GripperParams` and is owned by the backend.""" # Geometry (read from the shoulder drive's UF8/UF9/UF10). wrist_offset: float diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index b2da894a6e6..1e2afedbf82 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -27,7 +27,7 @@ from pylabrobot.capabilities.arms import kinematics as arm_kinematics from pylabrobot.capabilities.arms.standard import GripperLocation -from pylabrobot.paa.kx2.config import Axis, GripperConfig, KX2Config +from pylabrobot.paa.kx2.config import Axis, GripperParams, KX2Config from pylabrobot.paa.kx2.driver import ( JointMoveDirection, MotorMoveParam, @@ -40,7 +40,7 @@ class IKError(ValueError): """Target pose is unreachable (for now: non-Z rotation requested).""" -def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperConfig) -> GripperLocation: +def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperParams) -> GripperLocation: """Forward kinematics. Args: @@ -77,7 +77,7 @@ def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperConfig) -> GripperLoca ) -def ik(pose: GripperLocation, c: KX2Config, t: GripperConfig) -> Dict[Axis, float]: +def ik(pose: GripperLocation, c: KX2Config, t: GripperParams) -> Dict[Axis, float]: """Inverse kinematics. Args: @@ -191,7 +191,7 @@ def plan_joint_move( current: Dict[Axis, float], target: Dict[Axis, float], cfg: KX2Config, - gripper_cfg: GripperConfig, + gripper_params: GripperParams, *, max_gripper_speed: Optional[float] = None, max_gripper_acceleration: Optional[float] = None, @@ -298,7 +298,7 @@ def plan_joint_move( fk_deltas = {ax: cap_deltas.get(ax, 0.0) for ax in arm_axes if ax in fk_start} capped_v: Dict[Axis, float] = {} capped_a: Dict[Axis, float] = {} - fk_loc = lambda j: fk(j, cfg, gripper_cfg).location + fk_loc = lambda j: fk(j, cfg, gripper_params).location if max_gripper_speed is not None and fk_start: result = arm_kinematics.joint_velocities_for_max_gripper_speed( fk=fk_loc, diff --git a/pylabrobot/paa/kx2/tests/kinematics_tests.py b/pylabrobot/paa/kx2/tests/kinematics_tests.py index ebcf1a469ac..8557208caa9 100644 --- a/pylabrobot/paa/kx2/tests/kinematics_tests.py +++ b/pylabrobot/paa/kx2/tests/kinematics_tests.py @@ -3,7 +3,7 @@ from pylabrobot.capabilities.arms.standard import GripperLocation from pylabrobot.paa.kx2 import kinematics -from pylabrobot.paa.kx2.config import Axis, AxisConfig, GripperConfig, KX2Config +from pylabrobot.paa.kx2.config import Axis, AxisConfig, GripperParams, KX2Config from pylabrobot.paa.kx2.driver import JointMoveDirection from pylabrobot.paa.kx2.kinematics import IKError from pylabrobot.resources import Coordinate, Rotation @@ -45,7 +45,7 @@ def _config( class FKIKRoundTrip(unittest.TestCase): def test_roundtrip(self): c = _config() - g = GripperConfig(length=15.0, z_offset=3.0) + g = GripperParams(length=15.0, z_offset=3.0) pose = GripperLocation( location=Coordinate(x=100, y=200, z=50), rotation=Rotation(z=30) ) @@ -59,7 +59,7 @@ def test_roundtrip(self): def test_roundtrip_at_origin_yaw_zero(self): """Sanity: a pose at (0, R, Z, 0°) lands shoulder=0°.""" c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0) - g = GripperConfig() + g = GripperParams() pose = GripperLocation( location=Coordinate(x=0, y=300, z=10), rotation=Rotation(z=0) ) @@ -73,7 +73,7 @@ def test_roundtrip_at_origin_yaw_zero(self): class IKErrors(unittest.TestCase): def test_x_rotation_raises_ikerror(self): c = _config() - g = GripperConfig() + g = GripperParams() pose = GripperLocation( location=Coordinate(x=0, y=100, z=0), rotation=Rotation(x=10, z=0) ) @@ -82,7 +82,7 @@ def test_x_rotation_raises_ikerror(self): def test_y_rotation_raises_ikerror(self): c = _config() - g = GripperConfig() + g = GripperParams() pose = GripperLocation( location=Coordinate(x=0, y=100, z=0), rotation=Rotation(y=10, z=0) ) @@ -131,8 +131,8 @@ class GripperFingerSide(unittest.TestCase): def test_proximity_side_negates_gripper_offset(self): """Same joints, opposite finger side -> clamp point reflected through wrist axis.""" c = _config() - g_bc = GripperConfig(length=15.0, z_offset=3.0, finger_side="barcode_reader") - g_pr = GripperConfig(length=15.0, z_offset=3.0, finger_side="proximity_sensor") + g_bc = GripperParams(length=15.0, z_offset=3.0, finger_side="barcode_reader") + g_pr = GripperParams(length=15.0, z_offset=3.0, finger_side="proximity_sensor") j = {Axis.SHOULDER: 30.0, Axis.Z: 50.0, Axis.ELBOW: 100.0, Axis.WRIST: 15.0} p_bc = kinematics.fk(j, c, g_bc) p_pr = kinematics.fk(j, c, g_pr) @@ -153,7 +153,7 @@ def test_proximity_side_negates_gripper_offset(self): def test_proximity_roundtrip(self): c = _config() - g = GripperConfig(length=15.0, z_offset=3.0, finger_side="proximity_sensor") + g = GripperParams(length=15.0, z_offset=3.0, finger_side="proximity_sensor") pose = GripperLocation( location=Coordinate(x=100, y=200, z=50), rotation=Rotation(z=30) ) @@ -173,8 +173,8 @@ def test_ik_elbow_differs_by_twice_gripper_length(self): location=Coordinate(x=0, y=300, z=0), rotation=Rotation(z=0) ) c = _config() - g_bc = GripperConfig(length=15.0, z_offset=3.0, finger_side="barcode_reader") - g_pr = GripperConfig(length=15.0, z_offset=3.0, finger_side="proximity_sensor") + g_bc = GripperParams(length=15.0, z_offset=3.0, finger_side="barcode_reader") + g_pr = GripperParams(length=15.0, z_offset=3.0, finger_side="proximity_sensor") j_bc = kinematics.ik(pose, c, g_bc) j_pr = kinematics.ik(pose, c, g_pr) self.assertAlmostEqual(j_bc[Axis.SHOULDER], 0.0, places=9) @@ -186,7 +186,7 @@ class ShoulderSnapAt180(unittest.TestCase): def test_negative_180_snaps_to_positive(self): """A pose pointing exactly along -y has shoulder = ±180; we snap to +180.""" c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0) - g = GripperConfig() + g = GripperParams() pose = GripperLocation( location=Coordinate(x=0, y=-100, z=0), rotation=Rotation(z=180) ) From 83f129a9326b0c861d9b5691ee2fd8d4f6511863 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 18:11:29 -0700 Subject: [PATCH 63/93] =?UTF-8?q?KX2:=20small=20polish=20=E2=80=94=20gathe?= =?UTF-8?q?r=20joint=20reads,=20drop=20int(Axis)=20casts,=20op-mode=20read?= =?UTF-8?q?back?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - request_joint_position pipelines five BI queries via asyncio.gather. Distinct (node_id, msg_type, msg_index) keys mean concurrent in-flight requests don't collide in _send_bi's pending-future map; the bus serializes the frames at the wire level. ~25 ms → ~5 ms on the planner hot path. - Drop int(Axis.X) casts at every backend->driver boundary. Axis is already IntEnum, so the cast was cosmetic; it didn't add safety (Axis.AUX = 10 would still collide with _GROUP_NODE_ID via int()). ~30 sites cleaned. - motor_get_fault: bits 13/14/15 boolean arithmetic replaced with a 3-bit triplet table indexed by (b15<<2 | b14<<1 | b13). Same encoding, reads as data instead of conditionals. - New _set_op_mode(node_id, mode): writes 0x6060 then polls 0x6061 (modes_of_operation_display) until the drive acks the requested mode. CiA 402 §6.2 — without the readback, the first move issued in the new mode races the drive's mode change. pvt_select_mode now uses it for every 0x6060 write. - kinematics planner: total-time sync comment gains "so all axes finish together", mirroring the accel-sync comment one block up. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/arm_backend.py | 94 ++++++++++++++++--------------- pylabrobot/paa/kx2/driver.py | 63 +++++++++++++-------- pylabrobot/paa/kx2/kinematics.py | 2 +- 3 files changed, 89 insertions(+), 70 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 0e91282fae8..8921a3dd003 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -131,7 +131,7 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): ) for axis in MOTION_AXES: - await self.driver.motor_enable(node_id=int(axis), state=True, use_ds402=True) + await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) await self.servo_gripper_initialize() except BaseException: try: @@ -160,7 +160,7 @@ def _on_emcy( async def _disable_all() -> None: for axis in MOTION_AXES: try: - await self.driver.motor_emergency_stop(node_id=int(axis)) + await self.driver.motor_emergency_stop(node_id=axis) except Exception: logger.exception("EMCY auto-disable failed for axis %s", axis.name) @@ -177,7 +177,7 @@ async def get_estop_state(self) -> bool: Reads the shoulder drive's SR (status register) via the binary interpreter. Bits 14/15 encode the stop/safety state. """ - r = await self.driver.query_int(int(Axis.SHOULDER), "SR", 1) + r = await self.driver.query_int(Axis.SHOULDER, "SR", 1) if r != 8438016: logger.warning("get_estop_state: SR register unexpected value %d (expected 8438016)", r) b14 = (r & 0x4000) == 0x4000 @@ -185,13 +185,13 @@ async def get_estop_state(self) -> bool: return (not b14) and (not b15) async def _motor_set_homed_status(self, axis: Axis, status: HomeStatus) -> None: - await self.driver.write(int(axis), "UI", 3, int(status)) + await self.driver.write(axis, "UI", 3, int(status)) async def motor_get_homed_status(self, axis: Axis) -> HomeStatus: - return HomeStatus(await self.driver.query_int(int(axis), "UI", 3)) + return HomeStatus(await self.driver.query_int(axis, "UI", 3)) async def _motor_reset_encoder_position(self, axis: Axis, position: float) -> None: - nid = int(axis) + nid = axis await self.driver.write(nid, "HM", 1, 0) await self.driver.write(nid, "HM", 3, 0) await self.driver.write(nid, "HM", 4, 0) @@ -214,7 +214,7 @@ async def _motor_hard_stop_search( # is IntEnum so it works by accident, but Axis.value collides with # _GROUP_NODE_ID == 10 if anyone ever adds Axis.AUX = 10 — broadcast # would silently dispatch to all motion nodes. - nid = int(axis) + nid = axis await self.driver.write(nid, "ER", 3, max_pe * 10) await self.driver.write(nid, "AC", 0, srch_acc) await self.driver.write(nid, "DC", 0, srch_acc) @@ -249,7 +249,7 @@ async def _motor_index_search( positive_direction: bool, timeout: float, ) -> tuple: - nid = int(axis) + nid = axis await self.driver.write(nid, "HM", 1, 0) one_revolution = await self.driver.query_int(nid, "CA", 18) @@ -291,7 +291,7 @@ async def home_motor( offset_acc: int, timeout: float, ) -> None: - nid = int(axis) + nid = axis async with self._motion_guard(): left = await self.driver.query_int(nid, "CA", 41) @@ -355,7 +355,7 @@ async def servo_gripper_initialize(self): # and will fault with a confusing "homing failure" error if the motor # never came up. Better to surface the real cause. await self.driver.motor_enable( - node_id=int(Axis.SERVO_GRIPPER), state=True, use_ds402=False + node_id=Axis.SERVO_GRIPPER, state=True, use_ds402=False ) await self.servo_gripper_home() await self.servo_gripper_close() @@ -364,7 +364,7 @@ async def servo_gripper_home(self) -> None: sgc = self._cfg.servo_gripper if sgc is None: raise RuntimeError("Servo gripper not present") - sg = int(Axis.SERVO_GRIPPER) + sg = Axis.SERVO_GRIPPER await self.driver.write(sg, "PL", 1, sgc.peak_current) await self.driver.write(sg, "CL", 1, sgc.continuous_current) @@ -400,7 +400,7 @@ async def _set_servo_gripper_force_limit(self, max_force_percent: int) -> None: cont_current = sgc.continuous_current * max_force_percent / 100.0 peak_current = sgc.peak_current * max_force_percent / 100.0 - sg = int(Axis.SERVO_GRIPPER) + sg = Axis.SERVO_GRIPPER await self.driver.write(sg, "PL", 1, sgc.peak_current) await self.driver.write(sg, "CL", 1, cont_current) await self.driver.write(sg, "PL", 1, peak_current) @@ -408,7 +408,7 @@ async def _set_servo_gripper_force_limit(self, max_force_percent: int) -> None: async def _get_servo_gripper_force_fraction(self) -> float: """Return |IQ| / CL clamped to [0, 1] — the fraction of the configured continuous current the gripper is currently drawing.""" - sg = int(Axis.SERVO_GRIPPER) + sg = Axis.SERVO_GRIPPER cl = await self.driver.query_float(sg, "CL", 1) iq = await self.driver.query_float(sg, "IQ", 0) @@ -420,7 +420,7 @@ async def _get_servo_gripper_force_fraction(self) -> float: async def check_plate_gripped(self, num_attempts: int = 5) -> None: for _ in range(num_attempts): motor_status = await self.driver.query_int( - int(Axis.SERVO_GRIPPER), "MS", 1 + Axis.SERVO_GRIPPER, "MS", 1 ) logger.debug("Servo gripper motor status: %s", motor_status) @@ -443,7 +443,7 @@ async def check_plate_gripped(self, num_attempts: int = 5) -> None: return elif motor_status == 2: - motor_fault = await self.driver.motor_get_fault(int(Axis.SERVO_GRIPPER)) + motor_fault = await self.driver.motor_get_fault(Axis.SERVO_GRIPPER) if motor_fault is None: raise RuntimeError("Error querying whether plate is gripped. Error querying motor fault.") raise RuntimeError( @@ -477,7 +477,7 @@ async def drive_set_move_count_parameters( maintenance_required_rail: bool, last_maintenance_performed_date_rail: int, ) -> None: - z = int(Axis.Z) + z = Axis.Z # MoveCount -> Z axis, UI index 22 await self.driver.write(z, "UI", 22, int(move_count)) @@ -503,7 +503,7 @@ async def drive_set_move_count_parameters( # Rail (if present) if self._cfg.robot_on_rail: - rail = int(Axis.RAIL) + rail = Axis.RAIL await self.driver.write(rail, "UF", 6, float(last_maintenance_performed_rail)) await self.driver.write(rail, "UI", 23, 1 if maintenance_required_rail else 0) await self.driver.write(rail, "UI", 21, int(last_maintenance_performed_date_rail)) @@ -528,7 +528,7 @@ async def _read_config(self) -> KX2Config: for nid in nodes: axes[Axis(nid)] = await self._read_axis_config(nid) - sh = int(Axis.SHOULDER) + sh = Axis.SHOULDER return KX2Config( wrist_offset=await self.driver.query_float(sh, "UF", 8), elbow_offset=await self.driver.query_float(sh, "UF", 9), @@ -630,7 +630,7 @@ async def _read_io_names( return out async def _read_servo_gripper_config(self) -> ServoGripperConfig: - sg = int(Axis.SERVO_GRIPPER) + sg = Axis.SERVO_GRIPPER return ServoGripperConfig( home_pos=int(await self.driver.query_float(sg, "UF", 6)), home_search_vel=int(await self.driver.query_float(sg, "UF", 7)), @@ -654,7 +654,7 @@ def _cfg(self) -> KX2Config: async def motor_get_current_position(self, axis: Axis) -> float: raw = await self.driver.motor_get_current_position( - node_id=int(axis), pu=self._cfg.axes[axis].unlimited_travel, + node_id=axis, pu=self._cfg.axes[axis].unlimited_travel, ) c = self._cfg.axes[axis].motor_conversion_factor if axis == Axis.ELBOW: @@ -665,7 +665,7 @@ async def motor_get_current_position(self, axis: Axis) -> float: return raw / c async def read_input(self, axis: Axis, input_num: int) -> bool: - return await self.driver.read_input(node_id=int(axis), input_num=0x10 + input_num) + return await self.driver.read_input(node_id=axis, input_num=0x10 + input_num) # IR breakbeam between the gripper fingers, wired to the Z-drive's IO. # True = beam interrupted (object present). @@ -719,14 +719,14 @@ async def find_z_with_proximity_sensor( # Pre-flight: force the drive back to Op Enabled. A prior failed # search could have left it in Fault/Quick Stop where new moves # silently fail (Z barely moves). Idempotent if drive is healthy. - await self.driver.motor_enable(node_id=int(Axis.Z), state=True, use_ds402=True) + await self.driver.motor_enable(node_id=Axis.Z, state=True, use_ds402=True) if z_start is not None: await self._motors_move_joint_locked({Axis.Z: z_start}, params=move_params) z0 = await self.motor_get_current_position(Axis.Z) if await self.read_proximity_sensor(): return z0 await self.driver.configure_input_logic( - int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, _InputLogic.StopForward, + self._PROXIMITY_SENSOR_AXIS, self._PROXIMITY_SENSOR_INPUT, _InputLogic.StopForward, ) move_task = asyncio.create_task( self._motors_move_joint_locked( @@ -753,12 +753,12 @@ async def find_z_with_proximity_sensor( # motor FIRST, then restore IL. Reverse order would let the drive # surge toward the unreached target during the gap. try: - await self.driver.motor_stop(int(Axis.Z)) + await self.driver.motor_stop(Axis.Z) except Exception as e: logger.warning("find_z: motor_stop failed: %s", e) try: await self.driver.configure_input_logic( - int(self._PROXIMITY_SENSOR_AXIS), self._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose, + self._PROXIMITY_SENSOR_AXIS, self._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose, ) except Exception as e: logger.warning("find_z: IL restore failed: %s", e) @@ -820,7 +820,7 @@ async def _motors_move_absolute_execute_locked(self, plan: MotorsMovePlan) -> No ) for move in plan.moves: - nid = int(move.node_id) + nid = move.node_id await self.driver.motor_set_move_direction(nid, move.direction) # 0x607A = Target Position (24698 decimal) await self.driver.can_sdo_download_elmo_object( @@ -860,7 +860,7 @@ async def halt(self, backend_params: Optional[BackendParams] = None) -> None: # Deliberately NOT guarded by _motion_guard: emergency-stop must # interrupt regardless of who's holding the lock. await asyncio.gather( - *(self.driver.motor_emergency_stop(node_id=int(axis)) for axis in MOTION_AXES) + *(self.driver.motor_emergency_stop(node_id=axis) for axis in MOTION_AXES) ) async def park(self, backend_params: Optional[BackendParams] = None) -> None: @@ -1034,13 +1034,15 @@ async def drop_at_joint_position( async def request_joint_position( self, backend_params: Optional[BackendParams] = None ) -> Dict[int, float]: - return { - Axis.SHOULDER: await self.motor_get_current_position(Axis.SHOULDER), - Axis.Z: await self.motor_get_current_position(Axis.Z), - Axis.ELBOW: await self.motor_get_current_position(Axis.ELBOW), - Axis.WRIST: await self.motor_get_current_position(Axis.WRIST), - Axis.SERVO_GRIPPER: await self.motor_get_current_position(Axis.SERVO_GRIPPER), - } + # Each motor_get_current_position is one BI query (one CAN round-trip, + # ~5ms). Distinct (node_id, msg_type, msg_index) keys mean concurrent + # in-flight requests can't collide in _send_bi's pending-future map, so + # gather pipelines all five into one round-trip's worth of latency. + axes = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST, Axis.SERVO_GRIPPER) + positions = await asyncio.gather( + *(self.motor_get_current_position(ax) for ax in axes) + ) + return dict(zip(axes, positions)) def motion_limits(self) -> "_MotionLimits": """Per-axis (max_speed, max_acceleration) read from the drives at setup. @@ -1064,16 +1066,16 @@ async def start_freedrive_mode( # gripper, so a held plate doesn't drop. Caller can override with an # explicit list; [0] means "all motion axes" per CanFreedrive convention. if free_axes is None or free_axes == [0]: - axes: List[int] = [int(a) for a in MOTION_AXES] + axes: List[int] = list(MOTION_AXES) else: - axes = [int(a) for a in free_axes] + axes = list(free_axes) async with self._motion_guard(): for axis in axes: await self.driver.motor_enable(node_id=axis, state=False, use_ds402=True) self._freedrive_axes = axes async def stop_freedrive_mode(self, backend_params: Optional[BackendParams] = None) -> None: - axes: List[int] = self._freedrive_axes or [int(a) for a in MOTION_AXES] + axes: List[int] = self._freedrive_axes or list(MOTION_AXES) async with self._motion_guard(): for axis in axes: await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) @@ -1132,7 +1134,7 @@ async def very_dangerously_yeet( saved_limits: Dict[Axis, dict] = {} if bump != 1.0: for ax in (Axis.SHOULDER, Axis.WRIST): - nid = int(ax) + nid = ax s = { "VH2": await driver.query_int(nid, "VH", 2), "SP2": await driver.query_int(nid, "SP", 2), @@ -1193,7 +1195,7 @@ async def very_dangerously_yeet( release_t = sh_t_acc + sh_cruise_dur * _YEET_RELEASE_FRACTION wrist_trigger_t = max(0.0, release_t - wr_t_acc) - sg = int(Axis.SERVO_GRIPPER) + sg = Axis.SERVO_GRIPPER sg_cfg = cfg.axes[Axis.SERVO_GRIPPER] open_pos = min(_YEET_OPEN_POSITION, sg_cfg.max_travel - _YEET_OPEN_SAFETY_MARGIN) gripper_plan = MotorsMovePlan(moves=[MotorMoveParam( @@ -1208,10 +1210,10 @@ async def very_dangerously_yeet( await _yeet_arm_plan(driver, sh_plan) await _yeet_arm_plan(driver, wr_plan) - await driver.motors_move_start([int(Axis.SHOULDER)]) + await driver.motors_move_start([Axis.SHOULDER]) t0 = time.monotonic() await asyncio.sleep(max(0.0, wrist_trigger_t - (time.monotonic() - t0))) - await driver.motors_move_start([int(Axis.WRIST)]) + await driver.motors_move_start([Axis.WRIST]) await asyncio.sleep(max(0.0, release_t - (time.monotonic() - t0))) await self._motors_move_absolute_execute_locked(gripper_plan) @@ -1220,7 +1222,7 @@ async def very_dangerously_yeet( # target-reached; tight margin trips a CanError even though throw was OK. swing_finish_t = max(sh_t_total, wrist_trigger_t + wr_t_total) await driver.wait_for_moves_done( - [int(Axis.SHOULDER), int(Axis.WRIST)], timeout=swing_finish_t + 5, + [Axis.SHOULDER, Axis.WRIST], timeout=swing_finish_t + 5, ) await self._motors_move_joint_locked( @@ -1235,7 +1237,7 @@ async def very_dangerously_yeet( ) finally: for ax, s in saved_limits.items(): - nid = int(ax) + nid = ax await driver.write(nid, "VH", 2, s["VH2"]) await driver.write(nid, "SP", 2, s["SP2"]) await driver.write(nid, "SD", 0, s["SD0"]) @@ -1302,7 +1304,7 @@ async def _yeet_build_axis_move( cfg = backend._cfg ax_cfg = cfg.axes[ax] conv = ax_cfg.motor_conversion_factor - vh2 = await backend.driver.query_int(int(ax), "VH", 2) + vh2 = await backend.driver.query_int(ax, "VH", 2) v_phys = vh2 / 1.01 / abs(conv) a_phys = ax_cfg.max_accel direction = ax_cfg.joint_move_direction @@ -1326,7 +1328,7 @@ async def _yeet_build_axis_move( _, _, t_acc, t_total = kinematics._profile(dist, v_phys, a_phys) move = MotorMoveParam( - node_id=int(ax), + node_id=ax, position=int(round(target * conv)), velocity=max(int(round(v_phys * abs(conv))), 1), acceleration=max(int(round(a_phys * abs(conv))), 1), @@ -1340,7 +1342,7 @@ async def _yeet_arm_plan(driver: KX2Driver, plan: MotorsMovePlan) -> None: setup latency from the move start so the timer can be accurate.""" await driver.pvt_select_mode(False) for move in plan.moves: - nid = int(move.node_id) + nid = move.node_id await driver.motor_set_move_direction(nid, move.direction) await driver.can_sdo_download_elmo_object( nid, 24698, 0, int(move.position), _ElmoObjectDataType.INTEGER32, diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index ff4ce20d80b..33c47a3053d 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -1048,8 +1048,9 @@ async def motor_get_fault(self, node_id: int) -> Optional[str]: val = await self.query_int(node_id, "MF", 0) if val == 0: return None - - faults: list[str] = [] + # Elmo MF register: most faults are independent single bits. Bits 13/14/15 + # are different — they form a 3-bit selector (b15<<2 | b14<<1 | b13) where + # only four combinations are real faults; the rest mean nothing. bit_msgs = { 0x0001: "Motor Hall sensor feedback angle not found yet.", 0x0004: "Feedback loss: no match between encoder and Hall location.", @@ -1070,22 +1071,16 @@ async def motor_get_fault(self, node_id: int) -> Optional[str]: 0x400000: "Position limit exceeded.", 0x20000000: "Cannot start motor.", } - for bit, msg in bit_msgs.items(): - if val & bit: - faults.append(msg) - - b13 = bool(val & 0x2000) - b14 = bool(val & 0x4000) - b15 = bool(val & 0x8000) - if (not b15) and (not b14) and b13: - faults.append("Power supply under voltage.") - if (not b15) and b14 and (not b13): - faults.append("Power supply over voltage.") - if b15 and (not b14) and b13: - faults.append("Motor lead short circuit or faulty drive.") - if b15 and b14 and (not b13): - faults.append("Drive overheated.") - + triplet_msgs = { + 0b001: "Power supply under voltage.", # b13 only + 0b010: "Power supply over voltage.", # b14 only + 0b101: "Motor lead short circuit or faulty drive.", # b13 + b15 + 0b110: "Drive overheated.", # b14 + b15 + } + faults = [msg for bit, msg in bit_msgs.items() if val & bit] + triplet = (val >> 13) & 0b111 + if triplet in triplet_msgs: + faults.append(triplet_msgs[triplet]) if not faults: return f"Unknown fault code: {val} (0x{val:08X})" return " ".join(faults) @@ -1144,6 +1139,30 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N # --- motion primitives -------------------------------------------------- + async def _set_op_mode(self, node_id: int, mode: int, timeout_s: float = 0.05) -> None: + """Write 0x6060 (modes_of_operation) and poll 0x6061 (modes_of_operation_display) + until the drive acknowledges. CiA 402 §6.2: 0x6060 is the request, 0x6061 is + the actual mode — issuing a move (0x607A or 0x60C1 write) before the drive + flips reads the actual mode races the mode change. Drives typically ack in + <5 ms; timeout is generous so a busy bus doesn't false-fail. + + See https://www.stober.jp/manual/manual-commissioning-instruction-cia402-443080-01-en.pdf + for a CiA 402 commissioning reference (object table, mode codes, state machine). + """ + await self._can_sdo_download(node_id, 0x60, 0x60, 0x00, [mode]) + deadline = asyncio.get_event_loop().time() + timeout_s + while True: + raw = await self._can_sdo_upload(node_id, 0x60, 0x61, 0x00) + actual = struct.unpack("= deadline: + raise CanError( + f"node {node_id}: 0x6061 modes_of_operation_display = {actual}, " + f"expected {mode} after {timeout_s * 1000:.0f}ms — drive didn't ack mode change" + ) + await asyncio.sleep(0.005) + async def pvt_select_mode(self, enable: bool) -> None: """Enable/disable PVT mode on all motion axes via standard SDO writes.""" if enable: @@ -1151,17 +1170,15 @@ async def pvt_select_mode(self, enable: bool) -> None: for nid in self.motion_node_ids: # 0x60C4 sub 6 = 0 (disable interpolation buffer) await self._can_sdo_download(nid, 0x60, 0xC4, 0x06, [0]) - # 0x6060 = 7 (interpolated position mode) - await self._can_sdo_download(nid, 0x60, 0x60, 0x00, [7]) + await self._set_op_mode(nid, 7) # interpolated position mode self._pvt_mode = True else: for nid in self.motion_node_ids: - await self._can_sdo_download(nid, 0x60, 0x60, 0x00, [1]) + await self._set_op_mode(nid, 1) else: if self._pvt_mode: for nid in self.motion_node_ids: - # 0x6060 = 1 (profile position mode) - await self._can_sdo_download(nid, 0x60, 0x60, 0x00, [1]) + await self._set_op_mode(nid, 1) # profile position mode self._pvt_mode = False async def wait_for_moves_done( diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index 1e2afedbf82..1c1477d3aaf 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -377,7 +377,7 @@ def plan_joint_move( continue v[ax], a[ax], _, total_time[ax] = _profile(dist[ax], v[ax], a[ax]) - # Sync total times to the lead axis. + # Sync total times to the lead axis so all axes finish together. lead_time_ax = max(axes, key=lambda ax: total_time[ax]) lead_T = total_time[lead_time_ax] for ax in axes: From e5ab70a6c20324d9da8c3e9510b323fbea913637 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 18:11:41 -0700 Subject: [PATCH 64/93] =?UTF-8?q?KX2:=20planner=20unit=20tests=20=E2=80=94?= =?UTF-8?q?=2038=20passing=20+=202=20xfail=20capturing=20latent=20bugs?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 40 tests for kinematics.plan_joint_move, the elbow asin conversions, and the direction-aware delta. All pure-Python, no driver, no CAN bus — the planner became unit-testable when 5bf36e64f made it a pure function. Two tests are xfail and pin real bugs uncovered while writing them: - test_open_clamp_at_max_travel_plus_epsilon_does_not_raise: convert_elbow_position_to_angle does asin(x) without clamping x to [-1, 1]. FP overshoot at the joint limit raises ValueError. - test_round_trip_above_90: the >90° branches of convert_elbow_position_to_angle and convert_elbow_angle_to_position are not strict inverses. The forward path uses (2*max - pos + zero_offset)/denom, the inverse reflects via (2*max - pos) without the matching + zero_offset. Round-trip drifts ~287 mm at pos=300.5, zero_offset=5. Latent today (the planner only calls pos→angle), but encoder-read flows through the buggy direction. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/tests/planner_tests.py | 629 ++++++++++++++++++++++ 1 file changed, 629 insertions(+) create mode 100644 pylabrobot/paa/kx2/tests/planner_tests.py diff --git a/pylabrobot/paa/kx2/tests/planner_tests.py b/pylabrobot/paa/kx2/tests/planner_tests.py new file mode 100644 index 00000000000..649bb56c570 --- /dev/null +++ b/pylabrobot/paa/kx2/tests/planner_tests.py @@ -0,0 +1,629 @@ +"""Unit tests for the KX2 trajectory planner and elbow conversions in +``pylabrobot.paa.kx2.kinematics``. + +Covers planner-level behaviour (``plan_joint_move``, sync passes, encoder +unit conversion, gripper-speed cap, clearance) and the elbow position<-> +angle round-trip. FK/IK/snap_to_current already have coverage in +``kinematics_tests.py`` -- this file deliberately stays disjoint. + +The planner is pure Python: no driver round-trip, no async, no CAN. We +build :class:`KX2Config` directly with synthetic per-axis configs. +""" + +import math +import unittest +from typing import Optional + +from pylabrobot.paa.kx2 import kinematics +from pylabrobot.paa.kx2.config import Axis, AxisConfig, GripperParams, KX2Config +from pylabrobot.paa.kx2.driver import JointMoveDirection, MotorsMovePlan + + +def _axis( + motor_conversion_factor: float = 1.0, + max_travel: float = 180.0, + min_travel: float = -180.0, + unlimited_travel: bool = False, + max_vel: float = 100.0, + max_accel: float = 100.0, + joint_move_direction: JointMoveDirection = JointMoveDirection.Normal, +) -> AxisConfig: + return AxisConfig( + motor_conversion_factor=motor_conversion_factor, + max_travel=max_travel, + min_travel=min_travel, + unlimited_travel=unlimited_travel, + absolute_encoder=True, + max_vel=max_vel, + max_accel=max_accel, + joint_move_direction=joint_move_direction, + digital_inputs={}, + analog_inputs={}, + outputs={}, + ) + + +def _config( + shoulder: Optional[AxisConfig] = None, + z: Optional[AxisConfig] = None, + elbow: Optional[AxisConfig] = None, + wrist: Optional[AxisConfig] = None, + servo_gripper: Optional[AxisConfig] = None, + wrist_offset: float = 10.0, + elbow_offset: float = 20.0, + elbow_zero_offset: float = 5.0, + base_to_gripper_clearance_z: float = 0.0, + base_to_gripper_clearance_arm: float = 0.0, +) -> KX2Config: + axes = { + Axis.SHOULDER: shoulder if shoulder is not None else _axis(), + Axis.Z: z if z is not None else _axis(min_travel=0.0, max_travel=400.0), + Axis.ELBOW: elbow if elbow is not None else _axis(min_travel=0.0, max_travel=300.0), + Axis.WRIST: wrist if wrist is not None else _axis(), + } + if servo_gripper is not None: + axes[Axis.SERVO_GRIPPER] = servo_gripper + return KX2Config( + wrist_offset=wrist_offset, + elbow_offset=elbow_offset, + elbow_zero_offset=elbow_zero_offset, + axes=axes, + base_to_gripper_clearance_z=base_to_gripper_clearance_z, + base_to_gripper_clearance_arm=base_to_gripper_clearance_arm, + robot_on_rail=False, + servo_gripper=None, + ) + + +def _move_for(plan: MotorsMovePlan, axis: Axis): + for m in plan.moves: + if m.node_id == int(axis): + return m + raise AssertionError(f"axis {axis.name} not in plan") + + +# --- 1. happy paths --------------------------------------------------------- + +class PlanJointMoveHappyPath(unittest.TestCase): + def test_single_axis_returns_plan_with_one_axis_entry(self): + cfg = _config() + g = GripperParams() + cur = {Axis.SHOULDER: 0.0} + plan = kinematics.plan_joint_move(cur, {Axis.SHOULDER: 30.0}, cfg, g) + self.assertIsInstance(plan, MotorsMovePlan) + assert plan is not None # type narrowing for mypy + self.assertEqual(len(plan.moves), 1) + + def test_single_axis_encoder_position_uses_conv_factor(self): + cfg = _config(shoulder=_axis(motor_conversion_factor=2.0)) + g = GripperParams() + plan = kinematics.plan_joint_move({Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + m = _move_for(plan, Axis.SHOULDER) + self.assertEqual(m.position, 60) + + def test_single_axis_velocity_floored_at_one(self): + """If conv * v < 1 (would round to 0), it's floored at 1.""" + cfg = _config(shoulder=_axis(motor_conversion_factor=1e-6, max_vel=0.001)) + g = GripperParams() + plan = kinematics.plan_joint_move({Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + m = _move_for(plan, Axis.SHOULDER) + self.assertGreaterEqual(m.velocity, 1) + + def test_single_axis_velocity_uses_abs_conv(self): + cfg = _config(shoulder=_axis(motor_conversion_factor=-3.0)) + g = GripperParams() + plan = kinematics.plan_joint_move({Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + m = _move_for(plan, Axis.SHOULDER) + self.assertGreater(m.velocity, 0) + + def test_single_axis_move_time_positive(self): + cfg = _config() + g = GripperParams() + plan = kinematics.plan_joint_move({Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + self.assertGreater(plan.move_time, 0.0) + + def test_multi_axis_all_axes_in_plan(self): + cfg = _config() + g = GripperParams() + cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0} + tgt = {Axis.SHOULDER: 30.0, Axis.Z: 50.0} + plan = kinematics.plan_joint_move(cur, tgt, cfg, g) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + nodes = {m.node_id for m in plan.moves} + self.assertEqual(nodes, {int(Axis.SHOULDER), int(Axis.Z)}) + + def test_multi_axis_move_time_equals_slowest(self): + """move_time should equal the slowest axis's total time.""" + cfg = _config( + shoulder=_axis(max_vel=10.0, max_accel=100.0), + z=_axis(min_travel=0.0, max_travel=400.0, max_vel=1000.0, max_accel=1000.0), + ) + g = GripperParams() + cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0} + tgt = {Axis.SHOULDER: 30.0, Axis.Z: 50.0} + plan = kinematics.plan_joint_move(cur, tgt, cfg, g) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + # shoulder is the slow lead axis; with v=10 deg/s and 30 deg, dominated time > 1s. + self.assertGreater(plan.move_time, 1.0) + + def test_no_op_returns_none(self): + cfg = _config() + g = GripperParams() + cur = {Axis.SHOULDER: 5.0, Axis.Z: 10.0} + tgt = {Axis.SHOULDER: 5.0, Axis.Z: 10.0} + self.assertIsNone(kinematics.plan_joint_move(cur, tgt, cfg, g)) + + def test_no_op_within_threshold_returns_none(self): + """Targets within 0.01 of current count as no-op.""" + cfg = _config() + g = GripperParams() + cur = {Axis.SHOULDER: 5.0} + tgt = {Axis.SHOULDER: 5.005} + self.assertIsNone(kinematics.plan_joint_move(cur, tgt, cfg, g)) + + +# --- 2. travel limit snap and raise ---------------------------------------- + +class TravelLimitSnap(unittest.TestCase): + def test_target_above_max_within_tolerance_snapped(self): + cfg = _config(shoulder=_axis(min_travel=-180.0, max_travel=180.0)) + g = GripperParams() + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 180.05}, cfg, g + ) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + m = _move_for(plan, Axis.SHOULDER) + # snapped to 180.0 -> position = 180 * 1.0 = 180 + self.assertEqual(m.position, 180) + + def test_target_above_max_out_of_tolerance_raises(self): + cfg = _config(shoulder=_axis(min_travel=-180.0, max_travel=180.0)) + g = GripperParams() + with self.assertRaises(ValueError) as ctx: + kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 180.5}, cfg, g + ) + self.assertIn("SHOULDER", str(ctx.exception)) + + def test_target_below_min_within_tolerance_snapped(self): + cfg = _config(shoulder=_axis(min_travel=-180.0, max_travel=180.0)) + g = GripperParams() + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: -180.05}, cfg, g + ) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + m = _move_for(plan, Axis.SHOULDER) + self.assertEqual(m.position, -180) + + def test_target_below_min_out_of_tolerance_raises(self): + cfg = _config(shoulder=_axis(min_travel=-180.0, max_travel=180.0)) + g = GripperParams() + with self.assertRaises(ValueError) as ctx: + kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: -180.5}, cfg, g + ) + self.assertIn("SHOULDER", str(ctx.exception)) + + def test_unlimited_travel_axis_accepts_out_of_range(self): + """An unlimited_travel axis (e.g. wrist) shouldn't raise on a 540° target.""" + cfg = _config( + wrist=_axis(min_travel=-180.0, max_travel=180.0, unlimited_travel=True), + ) + g = GripperParams() + plan = kinematics.plan_joint_move( + {Axis.WRIST: 0.0}, {Axis.WRIST: 540.0}, cfg, g + ) + self.assertIsNotNone(plan) + + +# --- 3. validation ---------------------------------------------------------- + +class GripperCapValidation(unittest.TestCase): + def test_negative_speed_raises(self): + cfg = _config() + g = GripperParams() + with self.assertRaises(ValueError) as ctx: + kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g, + max_gripper_speed=-1.0, + ) + self.assertIn("must be positive", str(ctx.exception)) + + def test_zero_speed_raises(self): + cfg = _config() + g = GripperParams() + with self.assertRaises(ValueError) as ctx: + kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g, + max_gripper_speed=0.0, + ) + self.assertIn("must be positive", str(ctx.exception)) + + def test_negative_acceleration_raises(self): + cfg = _config() + g = GripperParams() + with self.assertRaises(ValueError) as ctx: + kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g, + max_gripper_acceleration=-2.0, + ) + self.assertIn("must be positive", str(ctx.exception)) + + def test_zero_acceleration_raises(self): + cfg = _config() + g = GripperParams() + with self.assertRaises(ValueError) as ctx: + kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g, + max_gripper_acceleration=0.0, + ) + self.assertIn("must be positive", str(ctx.exception)) + + def test_both_none_runs_at_firmware_max(self): + cfg = _config() + g = GripperParams(length=15.0, z_offset=3.0) + # Need a full arm joint state for fk (the cap helper invokes fk). + cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 100.0, Axis.WRIST: 0.0} + tgt = {Axis.SHOULDER: 30.0, Axis.Z: 0.0, Axis.ELBOW: 100.0, Axis.WRIST: 0.0} + plan_uncapped = kinematics.plan_joint_move(cur, tgt, cfg, g) + self.assertIsNotNone(plan_uncapped) + assert plan_uncapped is not None # type narrowing for mypy + # Uncapped move time should match a profile run with the cap set very + # high (effectively no cap). Both should produce identical move_time. + plan_high_cap = kinematics.plan_joint_move( + cur, tgt, cfg, g, max_gripper_speed=1e9, max_gripper_acceleration=1e9 + ) + self.assertIsNotNone(plan_high_cap) + assert plan_high_cap is not None # type narrowing for mypy + self.assertAlmostEqual(plan_uncapped.move_time, plan_high_cap.move_time, places=6) + + +# --- 4. elbow conversions --------------------------------------------------- + +class ElbowConversion(unittest.TestCase): + def test_max_travel_lands_at_90_deg(self): + cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0) + self.assertAlmostEqual( + kinematics.convert_elbow_position_to_angle(cfg, 300.0), 90.0, places=9 + ) + + def test_zero_pos_lands_at_asin_of_zero_offset(self): + cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0) + expected = math.asin(5.0 / 305.0) * (180.0 / math.pi) + self.assertAlmostEqual( + kinematics.convert_elbow_position_to_angle(cfg, 0.0), expected, places=9 + ) + + def test_round_trip_below_90(self): + """pos -> angle -> pos must be identity for points where angle < 90.""" + cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0) + for pos in (0.0, 50.0, 100.0, 150.0, 200.0, 250.0, 299.0): + angle = kinematics.convert_elbow_position_to_angle(cfg, pos) + back = kinematics.convert_elbow_angle_to_position(cfg, angle) + self.assertAlmostEqual(back, pos, places=6, msg=f"pos={pos}") + + @unittest.expectedFailure + def test_round_trip_above_90(self): + """Same identity for the piecewise-reflected branch (angle > 90). + + The pos->angle formula uses ``(2*max - pos + zero_offset)``; the + angle->pos formula reflects via ``2*max - pos`` (no zero_offset + correction). The two are not strict inverses when zero_offset != 0, + so this test is currently expected to fail. The planner only calls + pos->angle, so this asymmetry is not on a hot path; flagging it + here as a regression candidate. + """ + cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0) + for pos in (300.5, 320.0, 380.0, 450.0, 550.0, 600.0): + angle = kinematics.convert_elbow_position_to_angle(cfg, pos) + back = kinematics.convert_elbow_angle_to_position(cfg, angle) + self.assertAlmostEqual(back, pos, places=6, msg=f"pos={pos}") + + def test_above_90_branch_returns_angle_above_90(self): + """At least confirm the piecewise branch is taken: pos > max_travel + => angle > 90°.""" + cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0) + for pos in (300.5, 320.0, 380.0, 450.0, 550.0, 599.0): + angle = kinematics.convert_elbow_position_to_angle(cfg, pos) + self.assertGreater(angle, 90.0, msg=f"pos={pos}") + + def test_angle_round_trip_at_90(self): + cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0) + pos = kinematics.convert_elbow_angle_to_position(cfg, 90.0) + self.assertAlmostEqual(pos, 300.0, places=9) + + @unittest.expectedFailure + def test_open_clamp_at_max_travel_plus_epsilon_does_not_raise(self): + """Regression for the open-clamp bug (PR-880 review finding 10). + + Today the function may produce x slightly outside [-1, 1] due to FP + rounding; the call should be clamped, not raise. Until that fix + lands, this test is expected to fail. TODO: clamp x to [-1, 1]. + """ + cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0) + # Try a few epsilons; any ValueError from asin should be considered a bug. + for eps in (1e-15, 1e-14, 1e-12, 1e-9): + try: + kinematics.convert_elbow_position_to_angle(cfg, 300.0 + eps) + except ValueError: + raise # expected failure today + # If none raised, force the failure flag off to surface that the bug is fixed. + self.fail("convert_elbow_position_to_angle no longer raises -- remove expectedFailure") + + +# --- 5. direction-aware delta in plan_joint_move ---------------------------- + +class DirectionAwareDelta(unittest.TestCase): + def _wrist_cfg(self, direction: JointMoveDirection) -> KX2Config: + return _config( + wrist=_axis( + min_travel=-180.0, + max_travel=180.0, + unlimited_travel=True, + joint_move_direction=direction, + max_vel=360.0, + max_accel=720.0, + ) + ) + + def test_clockwise_takes_long_way_when_target_ahead(self): + """Wrist at 0, target 10. Clockwise => long way around (-350). + + move_time should reflect the long-way distance, not 10. + """ + cfg_cw = self._wrist_cfg(JointMoveDirection.Clockwise) + cfg_short = self._wrist_cfg(JointMoveDirection.ShortestWay) + g = GripperParams() + cur = {Axis.WRIST: 0.0} + tgt = {Axis.WRIST: 10.0} + plan_cw = kinematics.plan_joint_move(cur, tgt, cfg_cw, g) + plan_short = kinematics.plan_joint_move(cur, tgt, cfg_short, g) + self.assertIsNotNone(plan_cw) + self.assertIsNotNone(plan_short) + assert plan_cw is not None and plan_short is not None # type narrowing for mypy + # Long way (350°) takes much longer than short way (10°). + self.assertGreater(plan_cw.move_time, plan_short.move_time * 5) + + def test_counterclockwise_takes_long_way_when_target_behind(self): + """Wrist at 0, target -10. Counterclockwise => long way (+350).""" + cfg_ccw = self._wrist_cfg(JointMoveDirection.Counterclockwise) + cfg_short = self._wrist_cfg(JointMoveDirection.ShortestWay) + g = GripperParams() + cur = {Axis.WRIST: 0.0} + tgt = {Axis.WRIST: -10.0} + plan_ccw = kinematics.plan_joint_move(cur, tgt, cfg_ccw, g) + plan_short = kinematics.plan_joint_move(cur, tgt, cfg_short, g) + self.assertIsNotNone(plan_ccw) + self.assertIsNotNone(plan_short) + assert plan_ccw is not None and plan_short is not None # type narrowing for mypy + self.assertGreater(plan_ccw.move_time, plan_short.move_time * 5) + + def test_shortest_way_wraps_at_180(self): + """A target +190 is reachable via -170 under ShortestWay -- the + abs distance traveled should equal 170, not 190.""" + cfg_short = self._wrist_cfg(JointMoveDirection.ShortestWay) + cfg_normal = self._wrist_cfg(JointMoveDirection.Normal) + g = GripperParams() + cur = {Axis.WRIST: 0.0} + plan_short = kinematics.plan_joint_move(cur, {Axis.WRIST: 190.0}, cfg_short, g) + # ShortestWay first wraps target into (-180, 180]: 190 -> -170. + plan_normal = kinematics.plan_joint_move(cur, {Axis.WRIST: -170.0}, cfg_normal, g) + self.assertIsNotNone(plan_short) + self.assertIsNotNone(plan_normal) + assert plan_short is not None and plan_normal is not None # type narrowing for mypy + self.assertAlmostEqual(plan_short.move_time, plan_normal.move_time, places=6) + + def test_normal_direction_uses_literal_delta(self): + """Normal mode = no wrap. delta is target - current, no shortcut.""" + cfg_normal = self._wrist_cfg(JointMoveDirection.Normal) + g = GripperParams() + plan_long = kinematics.plan_joint_move( + {Axis.WRIST: 0.0}, {Axis.WRIST: 170.0}, cfg_normal, g + ) + plan_short = kinematics.plan_joint_move( + {Axis.WRIST: 0.0}, {Axis.WRIST: 10.0}, cfg_normal, g + ) + self.assertIsNotNone(plan_long) + self.assertIsNotNone(plan_short) + assert plan_long is not None and plan_short is not None # type narrowing for mypy + # Move time roughly proportional to distance for the same v/a. + self.assertGreater(plan_long.move_time, plan_short.move_time) + + +# --- 6. accel sync ---------------------------------------------------------- + +class AccelSync(unittest.TestCase): + def test_slower_accel_axis_drives_accel_for_others(self): + """Two axes with very different max_accel should sync accel-time. + + The axis with the longer accel ramp (lower a, higher v) becomes the + lead; the other axis's acceleration should be reduced so the two + ramp together. + """ + # Same dist, same v, but very different a. + cfg = _config( + shoulder=_axis(max_vel=100.0, max_accel=10.0), # slow accel -> long ramp -> lead + z=_axis(min_travel=0.0, max_travel=400.0, max_vel=100.0, max_accel=1000.0), + ) + g = GripperParams() + cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0} + tgt = {Axis.SHOULDER: 50.0, Axis.Z: 50.0} + plan = kinematics.plan_joint_move(cur, tgt, cfg, g) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + z_move = _move_for(plan, Axis.Z) + # z's effective acceleration should be reduced from 1000 toward + # something far smaller after sync. Encoder accel = a * |conv|, conv=1. + self.assertLess(z_move.acceleration, 1000) + + +# --- 7. time sync ----------------------------------------------------------- + +class TimeSync(unittest.TestCase): + def test_shorter_time_axis_velocity_scaled_down(self): + """Two axes; one is much slower (long total time), the other should + have its v and a scaled by k = dist/denom < 1, never exceeding the + lead's time.""" + cfg = _config( + shoulder=_axis(max_vel=10.0, max_accel=20.0), # slow + z=_axis(min_travel=0.0, max_travel=400.0, max_vel=100.0, max_accel=200.0), + ) + g = GripperParams() + cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0} + tgt = {Axis.SHOULDER: 100.0, Axis.Z: 100.0} + plan = kinematics.plan_joint_move(cur, tgt, cfg, g) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + z_move = _move_for(plan, Axis.Z) + # z's commanded velocity should be far less than its firmware max + # because it's been time-synced down to the slow shoulder. + self.assertLess(z_move.velocity, 100) + + +# --- 8. encoder unit conversion at plan exit -------------------------------- + +class EncoderUnitConversion(unittest.TestCase): + def test_conv_factor_scales_position(self): + cfg = _config(shoulder=_axis(motor_conversion_factor=1000.0)) + g = GripperParams() + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g + ) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + m = _move_for(plan, Axis.SHOULDER) + # 30 deg * 1000 enc/deg = 30000 + self.assertEqual(m.position, 30000) + + def test_negative_conv_preserves_position_sign(self): + """Position uses the signed conv (so -1.0 flips sign), but velocity + and acceleration use abs(conv) (always positive).""" + cfg = _config(shoulder=_axis(motor_conversion_factor=-2.0)) + g = GripperParams() + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 30.0}, cfg, g + ) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + m = _move_for(plan, Axis.SHOULDER) + self.assertEqual(m.position, -60) + self.assertGreater(m.velocity, 0) + self.assertGreater(m.acceleration, 0) + + def test_skip_axis_emits_default_velocity_acceleration(self): + """Axes the planner skips (no-op) get hardcoded enc_vel/enc_accel.""" + cfg = _config() + g = GripperParams() + cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0} + tgt = {Axis.SHOULDER: 30.0, Axis.Z: 0.0} # Z is no-op + plan = kinematics.plan_joint_move(cur, tgt, cfg, g) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + z_move = _move_for(plan, Axis.Z) + self.assertEqual(z_move.velocity, 1000) + self.assertEqual(z_move.acceleration, 1000) + + +# --- 9. gripper-speed cap path --------------------------------------------- + +class GripperSpeedCapPath(unittest.TestCase): + def test_small_cap_reduces_velocity(self): + """A tight gripper-speed cap should drop joint velocities below the + uncapped baseline.""" + cfg = _config() + g = GripperParams(length=15.0, z_offset=3.0) + cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 100.0, Axis.WRIST: 0.0} + tgt = {Axis.SHOULDER: 30.0, Axis.Z: 50.0, Axis.ELBOW: 200.0, Axis.WRIST: 15.0} + + plan_uncapped = kinematics.plan_joint_move(cur, tgt, cfg, g) + plan_capped = kinematics.plan_joint_move( + cur, tgt, cfg, g, max_gripper_speed=10.0 + ) + self.assertIsNotNone(plan_uncapped) + self.assertIsNotNone(plan_capped) + assert plan_uncapped is not None and plan_capped is not None # type narrowing + # Capped plan should take strictly longer than uncapped. + self.assertGreater(plan_capped.move_time, plan_uncapped.move_time) + + def test_servo_gripper_axis_not_in_arm_axes_passes_through(self): + """The servo gripper axis (Axis.SERVO_GRIPPER) is not part of fk's + arm_axes set, so the gripper-speed cap helper shouldn't touch it.""" + cfg = _config( + servo_gripper=_axis(min_travel=0.0, max_travel=30.0, max_vel=50.0, max_accel=100.0), + ) + g = GripperParams() + cur = {Axis.SERVO_GRIPPER: 0.0} + tgt = {Axis.SERVO_GRIPPER: 20.0} + # Even with a tight gripper-speed cap, the servo gripper should still + # produce a plan -- the cap helper sees an empty fk_start and doesn't run. + plan = kinematics.plan_joint_move( + cur, tgt, cfg, g, max_gripper_speed=0.001 + ) + self.assertIsNotNone(plan) + assert plan is not None # type narrowing for mypy + self.assertEqual(len(plan.moves), 1) + self.assertEqual(plan.moves[0].node_id, int(Axis.SERVO_GRIPPER)) + + +# --- 10. clearance check ---------------------------------------------------- + +class ClearanceCheck(unittest.TestCase): + def test_violation_raises(self): + """Z below clearance AND elbow angle below arm clearance -> raise.""" + cfg = _config( + z=_axis(min_travel=0.0, max_travel=400.0), + elbow=_axis(min_travel=0.0, max_travel=300.0), + base_to_gripper_clearance_z=50.0, + base_to_gripper_clearance_arm=80.0, # 80 deg threshold (angle space) + ) + g = GripperParams() + # target Z = 10 < (0 + 50). Elbow position 50 -> angle ~asin(55/305) ~ 10° < 80°. + cur = {Axis.Z: 100.0, Axis.ELBOW: 200.0} + tgt = {Axis.Z: 10.0, Axis.ELBOW: 50.0} + with self.assertRaises(ValueError) as ctx: + kinematics.plan_joint_move(cur, tgt, cfg, g) + self.assertIn("clearance", str(ctx.exception).lower()) + + def test_no_violation_when_only_z_low(self): + """Z low but elbow angle above arm clearance -> no raise.""" + cfg = _config( + z=_axis(min_travel=0.0, max_travel=400.0), + elbow=_axis(min_travel=0.0, max_travel=300.0), + base_to_gripper_clearance_z=50.0, + base_to_gripper_clearance_arm=5.0, # very low threshold so any pose passes + ) + g = GripperParams() + cur = {Axis.Z: 100.0, Axis.ELBOW: 200.0} + tgt = {Axis.Z: 10.0, Axis.ELBOW: 200.0} + plan = kinematics.plan_joint_move(cur, tgt, cfg, g) + self.assertIsNotNone(plan) + + def test_no_violation_when_only_elbow_low(self): + """Elbow angle below threshold but Z high -> no raise.""" + cfg = _config( + z=_axis(min_travel=0.0, max_travel=400.0), + elbow=_axis(min_travel=0.0, max_travel=300.0), + base_to_gripper_clearance_z=50.0, + base_to_gripper_clearance_arm=80.0, + ) + g = GripperParams() + cur = {Axis.Z: 100.0, Axis.ELBOW: 200.0} + tgt = {Axis.Z: 200.0, Axis.ELBOW: 50.0} + plan = kinematics.plan_joint_move(cur, tgt, cfg, g) + self.assertIsNotNone(plan) + + +if __name__ == "__main__": + unittest.main() From c7b1488423eddb68b72496d1b190c6c29fe47379 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 18:11:48 -0700 Subject: [PATCH 65/93] KX2: fix mypy narrowing in tests Optional[MotorsMovePlan] dereferences and EMCY tuple-event recordings were mypy errors (31 across 3 test files). Standard idiom applied: assertIsNotNone followed by an assert for type narrowing; List[Tuple] for the heterogeneous EMCY event log. No semantic changes; same 84 passed + 2 xfail. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py | 6 ++++-- pylabrobot/paa/kx2/tests/driver_emcy_tests.py | 1 + 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py b/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py index ef664bdb021..f48fc05450a 100644 --- a/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py +++ b/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py @@ -11,6 +11,7 @@ import asyncio import struct import unittest +from typing import List, Tuple from unittest import mock from pylabrobot.paa.kx2 import driver as driver_mod @@ -107,8 +108,9 @@ class SetupOrdersEmcySubscribeBeforeNmtStartTests(unittest.TestCase): def test_emcy_subscribe_precedes_nmt_start(self): # Build a fake canopen.Network that records subscribe + nmt.send_command # in call order, and stub the rest of setup just enough to reach the - # ordering point we care about. - calls = [] + # ordering point we care about. Entries are heterogeneous tuples: some + # carry a payload (e.g. ("nmt", code)), others are tag-only (("scan",)). + calls: List[Tuple] = [] class _FakeNmt: def send_command(self_inner, code): diff --git a/pylabrobot/paa/kx2/tests/driver_emcy_tests.py b/pylabrobot/paa/kx2/tests/driver_emcy_tests.py index 816bed3c855..6f5cf78edef 100644 --- a/pylabrobot/paa/kx2/tests/driver_emcy_tests.py +++ b/pylabrobot/paa/kx2/tests/driver_emcy_tests.py @@ -93,6 +93,7 @@ def test_estop_sets_sticky_error_fields(self): self.assertEqual(self.driver.emcy_move_error, "E-stop button was pressed") self.assertEqual(self.driver.emcy_move_error_node_id, 1) self.assertIsNotNone(self.driver.last_emcy) + assert self.driver.last_emcy is not None # type narrowing for mypy self.assertEqual(self.driver.last_emcy.err_code, 0x5441) def test_non_fatal_does_not_set_sticky_error(self): From 5640cc4fbaa5f603f954e39b56238f6221e8a446 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 19:11:37 -0700 Subject: [PATCH 66/93] KX2: TPDO3-push setpoint-ack, SDO 16-bit index, pvt re-arm, planner contract MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/driver.py | 173 +++++++++++++------ pylabrobot/paa/kx2/kinematics.py | 25 ++- pylabrobot/paa/kx2/tests/kinematics_tests.py | 21 +++ 3 files changed, 161 insertions(+), 58 deletions(-) diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index 33c47a3053d..199f6a83c2b 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -186,6 +186,7 @@ class MotorsMovePlan: _BI_REQUEST_COB_BASE = 0x300 _BI_RESPONSE_COB_BASE = 0x280 _EMCY_COB_BASE = 0x80 +_TPDO3_COB_BASE = 0x380 _GROUP_NODE_ID = 10 logger = logging.getLogger(__name__) @@ -445,6 +446,13 @@ def __init__( Callable[[int, EmcyFrame, str, bool], None] ] = [] + # StatusWord (0x6041) push-cache from TPDO3, COB-ID 0x380+node_id. The + # canopen listener thread parses the 2-byte SW out of each TPDO3 frame and + # marshals (sw, set_event) onto the asyncio loop. _wait_setpoint_ack reads + # the cache + waits on the event instead of polling 0x6041 via SDO. + self._statusword: Dict[int, int] = {} + self._statusword_event: Dict[int, asyncio.Event] = {} + @property def loop(self) -> asyncio.AbstractEventLoop: """Event loop captured in setup(). Raises if accessed before setup().""" @@ -507,14 +515,24 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: logger.info("canopen: connected, nodes=%s", discovered) - # Unmap TPDO1, map TPDO3 (StatusWord, triggered on MotionComplete) and - # TPDO4 (DigitalInputs, triggered on edge). TPDO3 is kept mapped to match - # the C# reference config, but move completion is detected by polling MS - # (the MotionComplete event is unreliable on short moves). + # TPDO3 push for StatusWord (0x6041) so _wait_setpoint_ack can wait on + # the bit-12 transition without an SDO round-trip per poll. Subscribe + # before remapping so we don't lose the first frame the drive emits + # when the new event-trigger arms. 1 ms inhibit (10 * 100 us) caps + # bus traffic — SW changes happen at the ~1-2 ms servo cycle, so the + # inhibit doesn't lose edges in practice and keeps the bus quiet + # during PVT streaming if anyone resurrects the IPM runtime. + for nid in self.motion_node_ids: + self._statusword_event[nid] = asyncio.Event() + network.subscribe(_TPDO3_COB_BASE + nid, self._make_tpdo3_callback(nid)) + + # Unmap TPDO1, map TPDO3 (StatusWord, triggered on any SW change) and + # TPDO4 (DigitalInputs, triggered on edge). for node_id in self.node_id_list: await self._can_tpdo_unmap(TPDO.TPDO1, node_id) await self._tpdo_map( - TPDO.TPDO3, node_id, [TPDOMappedObject.StatusWord], TPDOTrigger.MotionComplete + TPDO.TPDO3, node_id, [TPDOMappedObject.StatusWord], + TPDOTrigger.StatusWordEvent, delay_100_us=10, ) await self._tpdo_map( TPDO.TPDO4, node_id, [TPDOMappedObject.DigitalInputs], TPDOTrigger.DigitalInputEvent @@ -561,6 +579,8 @@ async def stop(self) -> None: self.emcy_move_error = "" self.emcy_move_error_node_id = None self.last_emcy = None + self._statusword = {} + self._statusword_event = {} # --- PDO configuration (pure SDO writes; no library-PDO machinery) ------ @@ -573,8 +593,8 @@ async def _can_tpdo_unmap(self, tpdo: TPDO, node_id: int) -> None: node_id &= 0x7F num1 = ((cob_type_int & 0x01) << 7) | node_id num2 = (cob_type_int >> 1) & 0x07 - await self._can_sdo_download(node_id, 0x18, tpdo.value - 1, 1, [num1, num2, 0, 0xC0]) - await self._can_sdo_download(node_id, 0x1A, tpdo.value - 1, 0, [0, 0, 0, 0]) + await self._can_sdo_download(node_id, 0x1800 + tpdo.value - 1, 1, [num1, num2, 0, 0xC0]) + await self._can_sdo_download(node_id, 0x1A00 + tpdo.value - 1, 0, [0, 0, 0, 0]) async def _rpdo_map( self, @@ -590,22 +610,22 @@ async def _rpdo_map( cob_id_11 = ((int(cob_type) & 0x0F) << 7) | (node_id & 0x7F) # Disable PDO (bit 31 set) - await self._can_sdo_download(node_id, 0x14, rpdo_idx, 1, _u32_le(0x80000000 | cob_id_11)) + await self._can_sdo_download(node_id, 0x1400 + rpdo_idx, 1, _u32_le(0x80000000 | cob_id_11)) # Clear mapping count - await self._can_sdo_download(node_id, 0x16, rpdo_idx, 0, [0, 0, 0, 0]) + await self._can_sdo_download(node_id, 0x1600 + rpdo_idx, 0, [0, 0, 0, 0]) # Transmission type await self._can_sdo_download( - node_id, 0x14, rpdo_idx, 2, [int(transmission_type) & 0xFF, 0, 0, 0] + node_id, 0x1400 + rpdo_idx, 2, [int(transmission_type) & 0xFF, 0, 0, 0] ) # Mapped objects for i, mo in enumerate(mapped_objects): - await self._can_sdo_download(node_id, 0x16, rpdo_idx, i + 1, _u32_le(int(mo))) + await self._can_sdo_download(node_id, 0x1600 + rpdo_idx, i + 1, _u32_le(int(mo))) # Mapping count await self._can_sdo_download( - node_id, 0x16, rpdo_idx, 0, [len(mapped_objects) & 0xFF, 0, 0, 0] + node_id, 0x1600 + rpdo_idx, 0, [len(mapped_objects) & 0xFF, 0, 0, 0] ) # Re-enable (clear bit 31) - await self._can_sdo_download(node_id, 0x14, rpdo_idx, 1, _u32_le(cob_id_11)) + await self._can_sdo_download(node_id, 0x1400 + rpdo_idx, 1, _u32_le(cob_id_11)) async def _tpdo_map( self, @@ -625,55 +645,44 @@ async def _tpdo_map( event_mask = 1 << int(event_trigger) # Disable TPDO (bit 30 + 31) - await self._can_sdo_download(node_id, 0x18, tpdo_idx, 1, _u32_le(0xC0000000 | cob_id_11)) + await self._can_sdo_download(node_id, 0x1800 + tpdo_idx, 1, _u32_le(0xC0000000 | cob_id_11)) # Clear mapping count - await self._can_sdo_download(node_id, 0x1A, tpdo_idx, 0, [0, 0, 0, 0]) + await self._can_sdo_download(node_id, 0x1A00 + tpdo_idx, 0, [0, 0, 0, 0]) # Transmission type await self._can_sdo_download( - node_id, 0x18, tpdo_idx, 2, [int(transmission_type) & 0xFF, 0, 0, 0] + node_id, 0x1800 + tpdo_idx, 2, [int(transmission_type) & 0xFF, 0, 0, 0] ) # Inhibit / delay 100us - await self._can_sdo_download(node_id, 0x18, tpdo_idx, 3, [delay_100_us & 0xFF, 0, 0, 0]) + await self._can_sdo_download(node_id, 0x1800 + tpdo_idx, 3, [delay_100_us & 0xFF, 0, 0, 0]) # Event timer (ms) - await self._can_sdo_download(node_id, 0x18, tpdo_idx, 5, [event_timer_ms & 0xFF, 0, 0, 0]) + await self._can_sdo_download(node_id, 0x1800 + tpdo_idx, 5, [event_timer_ms & 0xFF, 0, 0, 0]) # Vendor event mask at 0x2F20: - await self._can_sdo_download(node_id, 0x2F, 0x20, int(tpdo) & 0xFF, _u32_le(event_mask)) + await self._can_sdo_download(node_id, 0x2F20, int(tpdo) & 0xFF, _u32_le(event_mask)) # Mapped objects for i, mo in enumerate(mapped_objects): - await self._can_sdo_download(node_id, 0x1A, tpdo_idx, i + 1, _u32_le(int(mo))) + await self._can_sdo_download(node_id, 0x1A00 + tpdo_idx, i + 1, _u32_le(int(mo))) # Mapping count await self._can_sdo_download( - node_id, 0x1A, tpdo_idx, 0, [len(mapped_objects) & 0xFF, 0, 0, 0] + node_id, 0x1A00 + tpdo_idx, 0, [len(mapped_objects) & 0xFF, 0, 0, 0] ) # Re-enable (clear bits 30 + 31) - await self._can_sdo_download(node_id, 0x18, tpdo_idx, 1, _u32_le(cob_id_11)) + await self._can_sdo_download(node_id, 0x1800 + tpdo_idx, 1, _u32_le(cob_id_11)) # --- SDO ----------------------------------------------------------------- async def _can_sdo_upload( - self, - node_id: int, - object_byte0: int, - object_byte1: int, - sub_index: int, + self, node_id: int, index: int, sub_index: int, ) -> bytes: - index = (object_byte0 << 8) | object_byte1 - node = self._nodes[node_id] # node.sdo.upload is blocking I/O (library handles expedited + segmented # transfers + abort codes); run off the event loop. - return await asyncio.to_thread(node.sdo.upload, index, sub_index) + return await asyncio.to_thread(self._nodes[node_id].sdo.upload, index, sub_index) async def _can_sdo_download( - self, - node_id: int, - object_byte0: int, - object_byte1: int, - sub_index: int, - data_byte: List[int], + self, node_id: int, index: int, sub_index: int, data: List[int], ) -> None: - index = (object_byte0 << 8) | object_byte1 - node = self._nodes[node_id] - await asyncio.to_thread(node.sdo.download, index, sub_index, bytes(data_byte)) + await asyncio.to_thread( + self._nodes[node_id].sdo.download, index, sub_index, bytes(data), + ) async def can_sdo_download_elmo_object( self, @@ -699,10 +708,7 @@ async def can_sdo_download_elmo_object( raise CanError(f"Unsupported data type for SDO Write: {data_type.name}") width, signed = spec data_bytes = list(int(data).to_bytes(width, "little", signed=signed)) - - obj_byte0 = elmo_object_int >> 8 - obj_byte1 = elmo_object_int & 0xFF - await self._can_sdo_download(node_id, obj_byte0, obj_byte1, sub_index, data_bytes) + await self._can_sdo_download(node_id, elmo_object_int, sub_index, data_bytes) # --- EMCY (CANopen Emergency, COB-ID 0x80 + node_id) -------------------- @@ -781,6 +787,32 @@ def _dispatch_emcy(self, node_id: int, data: bytes) -> None: except Exception: logger.exception("EMCY user callback raised; continuing") + def _make_tpdo3_callback(self, node_id: int): + """Return a callback that decodes TPDO3 (StatusWord) and signals waiters.""" + + def _cb(cob_id: int, data: bytes, timestamp: float) -> None: + if self._loop is None: + return + if len(data) < 2: + # StatusWord is 2 bytes — a shorter frame means the drive's TPDO3 + # mapping is wrong or a different sender is squatting on the COB-ID. + # _wait_setpoint_ack would silently fall back to SDO probing forever. + logger.warning( + "TPDO3 frame too short from node %d: %s (expected >=2 bytes)", + node_id, bytes(data).hex(), + ) + return + sw = int.from_bytes(bytes(data[:2]), "little") + self._loop.call_soon_threadsafe(self._dispatch_statusword, node_id, sw) + + return _cb + + def _dispatch_statusword(self, node_id: int, sw: int) -> None: + self._statusword[node_id] = sw + ev = self._statusword_event.get(node_id) + if ev is not None: + ev.set() + # --- Elmo binary interpreter (vendor protocol on TPDO2/RPDO2) ------------ def _make_bi_callback(self, node_id: int): @@ -1149,10 +1181,10 @@ async def _set_op_mode(self, node_id: int, mode: int, timeout_s: float = 0.05) - See https://www.stober.jp/manual/manual-commissioning-instruction-cia402-443080-01-en.pdf for a CiA 402 commissioning reference (object table, mode codes, state machine). """ - await self._can_sdo_download(node_id, 0x60, 0x60, 0x00, [mode]) + await self._can_sdo_download(node_id, 0x6060, 0x00, [mode]) deadline = asyncio.get_event_loop().time() + timeout_s while True: - raw = await self._can_sdo_upload(node_id, 0x60, 0x61, 0x00) + raw = await self._can_sdo_upload(node_id, 0x6061, 0x00) actual = struct.unpack(" None: if not self._pvt_mode: for nid in self.motion_node_ids: # 0x60C4 sub 6 = 0 (disable interpolation buffer) - await self._can_sdo_download(nid, 0x60, 0xC4, 0x06, [0]) + await self._can_sdo_download(nid, 0x60C4, 0x06, [0]) await self._set_op_mode(nid, 7) # interpolated position mode self._pvt_mode = True else: + # Re-arm: drop to PPM, reset the interpolation-buffer pointer, climb + # back into IPM. Mirrors C# PVTSelectMode true-and-already-in-PVT + # (clscanmotor.cs:6014-6031). Skipping any of the three steps leaves + # the drive in the wrong mode or with a stale IP buffer. for nid in self.motion_node_ids: await self._set_op_mode(nid, 1) + await self._can_sdo_download(nid, 0x60C4, 0x06, [0]) + await self._set_op_mode(nid, 7) else: if self._pvt_mode: for nid in self.motion_node_ids: @@ -1278,18 +1316,41 @@ async def _trigger_new_setpoint( async def _wait_setpoint_ack( self, node_id: int, *, want_high: bool, timeout: float = 0.05 ) -> bool: - """Poll 0x6041 bit 12 until it matches ``want_high`` (or timeout). - - Returns True on success, False on timeout. 50ms is plenty: this drive - flips bit 12 within a servo cycle (~1-2ms) when it sees the edge.""" + """Wait until 0x6041 bit 12 matches ``want_high`` (or timeout). + + TPDO3 maps StatusWord with the StatusWordEvent trigger; the canopen + listener thread parses each frame into self._statusword[node_id] and + signals self._statusword_event[node_id]. We wait on the event, with + a 5 ms grace before falling back to an SDO probe — covers the case + where the drive's event-trigger config didn't take and TPDO3 is + silent. 50 ms total is plenty: bit 12 flips within a servo cycle + (~1-2 ms) once the drive sees the edge. + """ assert self._loop is not None + ev = self._statusword_event.get(node_id) deadline = self._loop.time() + timeout while self._loop.time() < deadline: - raw = await self._can_sdo_upload(node_id, 0x60, 0x41, 0x00) - sw = int.from_bytes(raw[:2], "little") - if bool(sw & (1 << 12)) == want_high: + sw = self._statusword.get(node_id) + if sw is not None and bool(sw & (1 << 12)) == want_high: return True - await asyncio.sleep(0.001) + if ev is None: + # No subscription (drive outside motion_node_ids); SDO poll only. + raw = await self._can_sdo_upload(node_id, 0x6041, 0x00) + sw = int.from_bytes(raw[:2], "little") + if bool(sw & (1 << 12)) == want_high: + return True + await asyncio.sleep(0.001) + continue + ev.clear() + try: + remaining = max(0.0, deadline - self._loop.time()) + await asyncio.wait_for(ev.wait(), timeout=min(remaining, 0.005)) + except asyncio.TimeoutError: + # TPDO3 didn't fire within 5 ms — probe via SDO and update the + # cache so subsequent waits start from the latest known SW. + raw = await self._can_sdo_upload(node_id, 0x6041, 0x00) + sw = int.from_bytes(raw[:2], "little") + self._statusword[node_id] = sw return False async def user_program_run( @@ -1379,8 +1440,8 @@ async def motor_stop(self, node_id: int, settle: float = 0.1) -> None: """ await self._control_word_set(node_id, 271) await asyncio.sleep(settle) - await self._can_sdo_download(node_id, 0x60, 0x60, 0x00, [7]) - await self._can_sdo_download(node_id, 0x60, 0x60, 0x00, [1]) + await self._can_sdo_download(node_id, 0x6060, 0x00, [7]) + await self._can_sdo_download(node_id, 0x6060, 0x00, [1]) async def read_input_logic(self, node_id: int, input_num: int) -> int: return await self.query_int(node_id, "IL", input_num) diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index 1c1477d3aaf..878a4a32ad1 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -202,6 +202,17 @@ def plan_joint_move( ``request_joint_position`` (linear-extension units for elbow). Returns ``None`` if every axis would be a no-op (within 0.01 of current). + ``target`` may be a subset of axes (e.g. ``{Axis.Z: 100}``) — unspecified + axes don't move. + + ``current`` must include all four arm axes (SHOULDER/Z/ELBOW/WRIST) when + a gripper-speed/accel cap is given: the cap helper evaluates the FK + Jacobian at the start pose, and the column for any moving axis depends + on the absolute position of every other arm axis (the radius from the + shoulder enters the shoulder Jacobian, etc.). The orchestrator always + passes a full ``current`` from ``request_joint_position``; tests calling + this function directly must do the same. + ``max_gripper_speed`` / ``max_gripper_acceleration`` cap joint velocities so the worst-case Cartesian gripper speed/accel along the trajectory stays at or under the cap. @@ -294,10 +305,20 @@ def plan_joint_move( # path in `fk`'s natural units. Servo gripper isn't in fk, so it always # runs at firmware max regardless of cap. arm_axes = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) - fk_start = {ax: curr_cmd_units[ax] for ax in arm_axes if ax in curr_cmd_units} - fk_deltas = {ax: cap_deltas.get(ax, 0.0) for ax in arm_axes if ax in fk_start} capped_v: Dict[Axis, float] = {} capped_a: Dict[Axis, float] = {} + cap_requested = max_gripper_speed is not None or max_gripper_acceleration is not None + cap_relevant = any(ax in arm_axes for ax in axes) + if cap_requested and cap_relevant: + missing = [ax for ax in arm_axes if ax not in curr_cmd_units] + if missing: + raise ValueError( + f"max_gripper_speed/acceleration requires `current` to include all " + f"four arm axes (SHOULDER/Z/ELBOW/WRIST); missing: " + f"{[ax.name for ax in missing]}" + ) + fk_start = {ax: curr_cmd_units[ax] for ax in arm_axes if ax in curr_cmd_units} + fk_deltas = {ax: cap_deltas.get(ax, 0.0) for ax in arm_axes if ax in fk_start} fk_loc = lambda j: fk(j, cfg, gripper_params).location if max_gripper_speed is not None and fk_start: result = arm_kinematics.joint_velocities_for_max_gripper_speed( diff --git a/pylabrobot/paa/kx2/tests/kinematics_tests.py b/pylabrobot/paa/kx2/tests/kinematics_tests.py index 8557208caa9..b074152a130 100644 --- a/pylabrobot/paa/kx2/tests/kinematics_tests.py +++ b/pylabrobot/paa/kx2/tests/kinematics_tests.py @@ -69,6 +69,27 @@ def test_roundtrip_at_origin_yaw_zero(self): self.assertAlmostEqual(joints[Axis.Z], 10.0, places=9) self.assertAlmostEqual(joints[Axis.WRIST], 0.0, places=9) + def test_z_offset_sign_convention(self): + """Pin the documented convention: positive z_offset = clamp sits below + the wrist plate (in world frame, +Z up). FK with the same Z joint and + a larger z_offset must produce a *lower* gripper z; IK must require a + *higher* wrist Z to reach the same gripper z.""" + c = _config() + joints = {Axis.SHOULDER: 0.0, Axis.Z: 100.0, Axis.ELBOW: 50.0, Axis.WRIST: 0.0} + g_low = GripperParams(z_offset=0.0) + g_high = GripperParams(z_offset=10.0) + self.assertGreater( + kinematics.fk(joints, c, g_low).location.z, + kinematics.fk(joints, c, g_high).location.z, + "FK: increasing z_offset should lower the gripper", + ) + pose = GripperLocation(location=Coordinate(x=0, y=300, z=50), rotation=Rotation(z=0)) + self.assertGreater( + kinematics.ik(pose, c, g_high)[Axis.Z], + kinematics.ik(pose, c, g_low)[Axis.Z], + "IK: increasing z_offset should raise the wrist target", + ) + class IKErrors(unittest.TestCase): def test_x_rotation_raises_ikerror(self): From c5b28826fbd7193b218c9c703266f88d78aaa477 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 19:43:05 -0700 Subject: [PATCH 67/93] =?UTF-8?q?KX2:=20small=20polish=20=E2=80=94=20=5Fse?= =?UTF-8?q?nd=5Fbi=20race=20fix,=20asin=20clamp,=20skip-axis=20vel,=20=5FE?= =?UTF-8?q?PS,=20tests?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/config.py | 2 - pylabrobot/paa/kx2/driver.py | 69 +++++++++++++------- pylabrobot/paa/kx2/kinematics.py | 26 +++++--- pylabrobot/paa/kx2/tests/kinematics_tests.py | 33 ++++++++++ pylabrobot/paa/kx2/tests/planner_tests.py | 29 +++----- 5 files changed, 106 insertions(+), 53 deletions(-) diff --git a/pylabrobot/paa/kx2/config.py b/pylabrobot/paa/kx2/config.py index 89cfa30c0d5..9030ae55a59 100644 --- a/pylabrobot/paa/kx2/config.py +++ b/pylabrobot/paa/kx2/config.py @@ -133,5 +133,3 @@ class KX2Config: # None if no servo gripper is present on the bus. servo_gripper: Optional[ServoGripperConfig] - - eps: float = 1e-6 diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index 199f6a83c2b..44289e9346f 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -13,6 +13,7 @@ from __future__ import annotations import asyncio +import contextlib import logging import struct import time @@ -432,6 +433,15 @@ def __init__( # this dict directly. self._pending_bi: Dict[Tuple[int, str, int], asyncio.Future] = {} + # Per-(node, cmd, idx) lock around the future-install + send + await + # cycle in _send_bi. Without it, two coroutines firing the same + # request would clobber each other's pending-future entry; with the + # motion lock above mitigating most of it but direct driver callers + # (notebook diagnostics) and gathered queries with overlapping keys + # still hit the race. setdefault(asyncio.Lock()) is fine because the + # garbage Lock from a missed-race insert is immediately discarded. + self._bi_locks: Dict[Tuple[int, str, int], asyncio.Lock] = {} + self._pvt_mode: bool = False # EMCY (CANopen Emergency, COB-ID 0x80 + node_id) state. Subscribed in @@ -879,28 +889,39 @@ async def _send_bi( targets = ( list(self.motion_node_ids) if node_id == _GROUP_NODE_ID else [node_id] ) + keys = [(nid, cmd, cmd_index) for nid in targets] + + # Acquire one lock per target key first, so a second caller with the + # same key waits at the gate instead of clobbering our pending future. + # AsyncExitStack releases the locks in LIFO order on any exit path. + async with contextlib.AsyncExitStack() as stack: + for key in keys: + await stack.enter_async_context( + self._bi_locks.setdefault(key, asyncio.Lock()) + ) - futures: List[asyncio.Future] = [] - for nid in targets: - key = (nid, cmd, cmd_index) - # If a stale pending future exists for the same (node, cmd, index), drop it. - old = self._pending_bi.pop(key, None) - if old is not None and not old.done(): - old.cancel() - fut = self.loop.create_future() - self._pending_bi[key] = fut - futures.append(fut) - - self._network.send_message(_BI_REQUEST_COB_BASE + node_id, data_to_send) - - try: - return await asyncio.wait_for(asyncio.gather(*futures), timeout=timeout) - except asyncio.TimeoutError: - for nid in targets: - self._pending_bi.pop((nid, cmd, cmd_index), None) - raise CanError( - f"Timeout waiting for response to {cmd}[{cmd_index}] from node {node_id}" - ) + futures: List[asyncio.Future] = [] + for key in keys: + fut = self.loop.create_future() + self._pending_bi[key] = fut + futures.append(fut) + + self._network.send_message(_BI_REQUEST_COB_BASE + node_id, data_to_send) + + try: + return await asyncio.wait_for(asyncio.gather(*futures), timeout=timeout) + except asyncio.TimeoutError: + raise CanError( + f"Timeout waiting for response to {cmd}[{cmd_index}] from node {node_id}" + ) + finally: + # Defensive: clear pending futures on any exit (success, timeout, + # other exception). Without finally a non-Timeout exception could + # leave stale futures keyed on (nid, cmd, cmd_index) that the next + # caller would await indefinitely — the dispatch resolves only the + # most recent future at a given key. + for key in keys: + self._pending_bi.pop(key, None) async def query_int(self, node_id: int, cmd: str, cmd_index: int) -> int: """Query an int-typed Elmo parameter. Returns the drive's current value.""" @@ -1234,8 +1255,10 @@ async def _poll_axis(nid: int) -> None: try: if await self.motor_check_if_move_done(int(nid)): return - except CanError: - pass + except CanError as e: + # Transient bus error — keep polling. Visible at DEBUG so a wedged + # bus shows up in logs instead of just burning the full timeout. + logger.debug("wait_for_moves_done node %d: transient CAN error: %s", nid, e) await asyncio.sleep(0.03) # Final authoritative check; propagates CanError / motor-fault. if not await self.motor_check_if_move_done(int(nid)): diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index 878a4a32ad1..589aa8b754e 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -40,6 +40,10 @@ class IKError(ValueError): """Target pose is unreachable (for now: non-Z rotation requested).""" +# Floating-point fudge for boundary checks (e.g. snap shoulder to ±180°). +_EPS = 1e-6 + + def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperParams) -> GripperLocation: """Forward kinematics. @@ -106,7 +110,7 @@ def ik(pose: GripperLocation, c: KX2Config, t: GripperParams) -> Dict[Axis, floa wrist_z = pose.location.z + t.z_offset shoulder = -degrees(atan2(x, y)) - if abs(shoulder + 180.0) < c.eps: + if abs(shoulder + 180.0) < _EPS: shoulder = 180.0 elbow = hypot(x, y) - c.wrist_offset - c.elbow_offset - c.elbow_zero_offset @@ -138,10 +142,13 @@ def snap_to_current( def convert_elbow_position_to_angle(cfg: KX2Config, elbow_pos: float) -> float: max_travel = cfg.axes[Axis.ELBOW].max_travel denom = max_travel + cfg.elbow_zero_offset + # Clamp to [-1, 1]: floating-point overshoot at the joint limit (e.g. + # encoder reads max_travel + 1e-12) would otherwise raise ValueError + # from asin even though the position is physically reachable. if elbow_pos > max_travel: - x = (2.0 * max_travel - elbow_pos + cfg.elbow_zero_offset) / denom + x = max(-1.0, min(1.0, (2.0 * max_travel - elbow_pos + cfg.elbow_zero_offset) / denom)) return 90.0 + asin(x) * (180.0 / pi) - x = (elbow_pos + cfg.elbow_zero_offset) / denom + x = max(-1.0, min(1.0, (elbow_pos + cfg.elbow_zero_offset) / denom)) return asin(x) * (180.0 / pi) @@ -426,12 +433,13 @@ def plan_joint_move( ax_cfg = cfg.axes[ax] conv = ax_cfg.motor_conversion_factor enc_pos = target[ax] * conv - if skip_ax[ax]: - enc_vel = 1000.0 - enc_accel = 1000.0 - else: - enc_vel = max(v[ax] * abs(conv), 1.0) - enc_accel = max(a[ax] * abs(conv), 1.0) + # Skipped axes get firmware max — same formula as moving axes. They + # don't move (dist < 0.01), so vel/accel are nominal; the previous + # 1000.0 constant was 0.03–4% of firmware max across axes, leaving + # the drive's profile registers in a pathologically slow state if a + # follow-up step ever picked them up. + enc_vel = max(v[ax] * abs(conv), 1.0) + enc_accel = max(a[ax] * abs(conv), 1.0) moves.append(MotorMoveParam( node_id=int(ax), position=int(round(enc_pos)), diff --git a/pylabrobot/paa/kx2/tests/kinematics_tests.py b/pylabrobot/paa/kx2/tests/kinematics_tests.py index b074152a130..b60806b048a 100644 --- a/pylabrobot/paa/kx2/tests/kinematics_tests.py +++ b/pylabrobot/paa/kx2/tests/kinematics_tests.py @@ -90,6 +90,39 @@ def test_z_offset_sign_convention(self): "IK: increasing z_offset should raise the wrist target", ) + def test_ik_shoulder_branch_convention(self): + """Pin shoulder = -degrees(atan2(x, y)) (CW from +Y, standard for KX2). + + The C# original computes this with four quadrant cases hand-rolling + atan2 (KX2RobotControl.cs:7195-7268); Python uses the standard + library `atan2`. The two are identical at every reachable pose modulo + the -Y boundary, where Python snaps -180 -> +180 (kinematics.py:113) + to match C#. This test pins one point per quadrant + every axis + crossing so a future drive-by simplification can't silently flip a + sign or drop the boundary snap. + """ + c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0) + g = GripperParams() + cases = [ + ("+Y axis", 0.0, 300.0, 0.0), + ("Q1 (+x, +y)", 100.0, 100.0, -45.0), + ("+X axis", 300.0, 0.0, -90.0), + ("Q4 (+x, -y)", 100.0, -100.0, -135.0), + ("-Y axis", 0.0, -300.0, 180.0), # boundary: snapped from -180 + ("Q3 (-x, -y)", -100.0, -100.0, 135.0), + ("-X axis", -300.0, 0.0, 90.0), + ("Q2 (-x, +y)", -100.0, 100.0, 45.0), + ] + for label, x, y, expected_shoulder in cases: + pose = GripperLocation( + location=Coordinate(x=x, y=y, z=0), rotation=Rotation(z=0), + ) + joints = kinematics.ik(pose, c, g) + self.assertAlmostEqual( + joints[Axis.SHOULDER], expected_shoulder, places=9, + msg=f"{label}: x={x}, y={y}", + ) + class IKErrors(unittest.TestCase): def test_x_rotation_raises_ikerror(self): diff --git a/pylabrobot/paa/kx2/tests/planner_tests.py b/pylabrobot/paa/kx2/tests/planner_tests.py index 649bb56c570..4c8c2fc857c 100644 --- a/pylabrobot/paa/kx2/tests/planner_tests.py +++ b/pylabrobot/paa/kx2/tests/planner_tests.py @@ -344,23 +344,12 @@ def test_angle_round_trip_at_90(self): pos = kinematics.convert_elbow_angle_to_position(cfg, 90.0) self.assertAlmostEqual(pos, 300.0, places=9) - @unittest.expectedFailure def test_open_clamp_at_max_travel_plus_epsilon_does_not_raise(self): - """Regression for the open-clamp bug (PR-880 review finding 10). - - Today the function may produce x slightly outside [-1, 1] due to FP - rounding; the call should be clamped, not raise. Until that fix - lands, this test is expected to fail. TODO: clamp x to [-1, 1]. - """ + """Floating-point overshoot at the joint limit must not raise. The asin + argument is clamped to [-1, 1] before the call.""" cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0) - # Try a few epsilons; any ValueError from asin should be considered a bug. for eps in (1e-15, 1e-14, 1e-12, 1e-9): - try: - kinematics.convert_elbow_position_to_angle(cfg, 300.0 + eps) - except ValueError: - raise # expected failure today - # If none raised, force the failure flag off to surface that the bug is fixed. - self.fail("convert_elbow_position_to_angle no longer raises -- remove expectedFailure") + kinematics.convert_elbow_position_to_angle(cfg, 300.0 + eps) # --- 5. direction-aware delta in plan_joint_move ---------------------------- @@ -522,9 +511,11 @@ def test_negative_conv_preserves_position_sign(self): self.assertGreater(m.velocity, 0) self.assertGreater(m.acceleration, 0) - def test_skip_axis_emits_default_velocity_acceleration(self): - """Axes the planner skips (no-op) get hardcoded enc_vel/enc_accel.""" - cfg = _config() + def test_skip_axis_emits_firmware_max_velocity_acceleration(self): + """Axes the planner skips (no-op) still write vel/accel to the drive's + profile registers — set them to firmware max so a stale register can't + leave a follow-up move pathologically slow.""" + cfg = _config() # default _axis: max_vel=100, max_accel=100, conv=1.0 g = GripperParams() cur = {Axis.SHOULDER: 0.0, Axis.Z: 0.0} tgt = {Axis.SHOULDER: 30.0, Axis.Z: 0.0} # Z is no-op @@ -532,8 +523,8 @@ def test_skip_axis_emits_default_velocity_acceleration(self): self.assertIsNotNone(plan) assert plan is not None # type narrowing for mypy z_move = _move_for(plan, Axis.Z) - self.assertEqual(z_move.velocity, 1000) - self.assertEqual(z_move.acceleration, 1000) + self.assertEqual(z_move.velocity, 100) # max_vel * |conv| + self.assertEqual(z_move.acceleration, 100) # --- 9. gripper-speed cap path --------------------------------------------- From b6ca5d4fd183a86e125a97688f25513e044f6b11 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 19:50:36 -0700 Subject: [PATCH 68/93] KX2: tag MF-polling and PPM-setpoint errors with Axis {nid} MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/driver.py | 6 +++--- pylabrobot/paa/kx2/kinematics.py | 3 +++ 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index 44289e9346f..7d545a80b9c 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -1093,8 +1093,8 @@ async def motor_check_if_move_done(self, node_id: int) -> bool: return True fault = await self.motor_get_fault(node_id) if fault is not None: - raise RuntimeError(f"Motor Fault: {fault}") - raise RuntimeError("Motor Fault (Unknown)") + raise RuntimeError(f"Motor Fault: Axis {node_id} {fault}") + raise RuntimeError(f"Motor Fault: Axis {node_id} (Unknown)") return False async def motor_get_fault(self, node_id: int) -> Optional[str]: @@ -1331,7 +1331,7 @@ async def _trigger_new_setpoint( node_id, attempt, max_attempts, ) raise CanError( - f"node {node_id}: drive did not accept new PPM setpoint after " + f"Axis {node_id}: drive did not accept new PPM setpoint after " f"{max_attempts} attempts (SW bit 12 never went high after CW bit 4 " f"rising edge)" ) diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index 589aa8b754e..cd73e0cd2fb 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -109,6 +109,9 @@ def ik(pose: GripperLocation, c: KX2Config, t: GripperParams) -> Dict[Axis, floa y = pose.location.y + gl * cos(yaw) wrist_z = pose.location.z + t.z_offset + # atan2 returns (-π, π]; on the -Y axis it yields -180°. Snap to +180° + # so the boundary lands on the "in-range" side of an exclusive max + # convention and downstream sign comparisons are stable. shoulder = -degrees(atan2(x, y)) if abs(shoulder + 180.0) < _EPS: shoulder = 180.0 From d6dd639bfca2df12df24a7366097f565a07bce8f Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 30 Apr 2026 20:03:54 -0700 Subject: [PATCH 69/93] KX2: pin exact sync-algorithm values in planner tests MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/tests/planner_tests.py | 124 ++++++++++++++++++++++ 1 file changed, 124 insertions(+) diff --git a/pylabrobot/paa/kx2/tests/planner_tests.py b/pylabrobot/paa/kx2/tests/planner_tests.py index 4c8c2fc857c..03ef771d92a 100644 --- a/pylabrobot/paa/kx2/tests/planner_tests.py +++ b/pylabrobot/paa/kx2/tests/planner_tests.py @@ -616,5 +616,129 @@ def test_no_violation_when_only_elbow_low(self): self.assertIsNotNone(plan) +# --- 11. exact-value pins for the sync algorithm ---------------------------- +# These tests pin the *exact* encoder velocity, acceleration, and move_time +# the planner produces for representative cases. They're the contract for any +# refactor of the per-axis profile + accel-sync + time-sync chain — directional +# asserts (assertLess/Greater) are too loose to catch a value-shifting change. +# Tolerance: ±1 on int encoder values (rounding noise from float ops); 4 +# decimals on move_time. + +class SyncAlgorithmExactValues(unittest.TestCase): + def _assert_enc_close(self, actual, expected): + self.assertLessEqual( + abs(actual - expected), 1, f"{actual} not within 1 of {expected}" + ) + + def test_single_axis_trapezoidal(self): + """dist=100, v_max=100, a_max=200; t_acc=0.5, d_acc=25, t_cruise=0.5.""" + cfg = _config(shoulder=_axis(max_vel=100.0, max_accel=200.0)) + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 100.0}, cfg, GripperParams(), + ) + self.assertIsNotNone(plan) + assert plan is not None + m = _move_for(plan, Axis.SHOULDER) + self.assertEqual(m.position, 100) + self.assertEqual(m.velocity, 100) + self.assertEqual(m.acceleration, 200) + self.assertAlmostEqual(plan.move_time, 1.5, places=4) + + def test_single_axis_triangular(self): + """dist=10, v_max=100, a_max=200; can't reach v_max -> triangular. + t_acc = sqrt(10/200) = 0.2236, v_actual = a*t_acc = 44.72 -> 45.""" + cfg = _config(shoulder=_axis(max_vel=100.0, max_accel=200.0)) + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0}, {Axis.SHOULDER: 10.0}, cfg, GripperParams(), + ) + self.assertIsNotNone(plan) + assert plan is not None + m = _move_for(plan, Axis.SHOULDER) + self.assertEqual(m.position, 10) + self._assert_enc_close(m.velocity, 45) + self.assertEqual(m.acceleration, 200) + self.assertAlmostEqual(plan.move_time, 0.4472136, places=4) + + def test_accel_sync_two_axis_exact(self): + """Shoulder is the slow-accel lead (v=100, a=10); Z's accel scales down.""" + cfg = _config( + shoulder=_axis(max_vel=100.0, max_accel=10.0), + z=_axis(min_travel=0.0, max_travel=400.0, max_vel=100.0, max_accel=1000.0), + ) + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0, Axis.Z: 0.0}, + {Axis.SHOULDER: 50.0, Axis.Z: 50.0}, + cfg, GripperParams(), + ) + self.assertIsNotNone(plan) + assert plan is not None + sh = _move_for(plan, Axis.SHOULDER) + z = _move_for(plan, Axis.Z) + self._assert_enc_close(sh.velocity, 22) + self.assertEqual(sh.acceleration, 10) + self._assert_enc_close(z.velocity, 15) + self._assert_enc_close(z.acceleration, 14) + self.assertAlmostEqual(plan.move_time, 4.472136, places=4) + + def test_time_sync_two_axis_exact(self): + """Shoulder is the slow-vel lead (v=10, a=20); Z's v scales down to match + move_time = dist/v + v/a = 100/10 + 10/20 = 10.5s.""" + cfg = _config( + shoulder=_axis(max_vel=10.0, max_accel=20.0), + z=_axis(min_travel=0.0, max_travel=400.0, max_vel=100.0, max_accel=200.0), + ) + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0, Axis.Z: 0.0}, + {Axis.SHOULDER: 100.0, Axis.Z: 100.0}, + cfg, GripperParams(), + ) + self.assertIsNotNone(plan) + assert plan is not None + sh = _move_for(plan, Axis.SHOULDER) + z = _move_for(plan, Axis.Z) + self.assertEqual(sh.velocity, 10) + self.assertEqual(sh.acceleration, 20) + self.assertEqual(z.velocity, 10) + self.assertEqual(z.acceleration, 20) + self.assertAlmostEqual(plan.move_time, 10.5, places=4) + + def test_four_axis_real_conv_exact(self): + """Full 4-axis coordinated move with realistic KX2 conv factors. Pins + the entire pipeline (elbow asin + sync + encoder packing) end-to-end.""" + cfg = _config( + shoulder=_axis(max_vel=145.0, max_accel=300.0, motor_conversion_factor=23301.694), + z=_axis(min_travel=0.0, max_travel=750.0, max_vel=750.0, max_accel=1000.0, + motor_conversion_factor=3997.838), + elbow=_axis(min_travel=0.0, max_travel=300.0, max_vel=80.0, max_accel=180.0, + motor_conversion_factor=18204.444), + wrist=_axis(min_travel=-180.0, max_travel=180.0, max_vel=500.0, max_accel=1000.0, + motor_conversion_factor=45.511, unlimited_travel=True), + ) + plan = kinematics.plan_joint_move( + {Axis.SHOULDER: 0.0, Axis.Z: 50.0, Axis.ELBOW: 100.0, Axis.WRIST: 0.0}, + {Axis.SHOULDER: 30.0, Axis.Z: 200.0, Axis.ELBOW: 150.0, Axis.WRIST: 90.0}, + cfg, GripperParams(), + ) + self.assertIsNotNone(plan) + assert plan is not None + sh = _move_for(plan, Axis.SHOULDER) + z = _move_for(plan, Axis.Z) + el = _move_for(plan, Axis.ELBOW) + wr = _move_for(plan, Axis.WRIST) + self._assert_enc_close(sh.position, 699051) + self._assert_enc_close(sh.velocity, 1646247) + self._assert_enc_close(sh.acceleration, 4704052) + self._assert_enc_close(z.position, 799568) + self._assert_enc_close(z.velocity, 1548356) + self._assert_enc_close(z.acceleration, 3997838) + self._assert_enc_close(el.position, 556033) + self._assert_enc_close(el.velocity, 403583) + self._assert_enc_close(el.acceleration, 1322501) + self._assert_enc_close(wr.position, 4096) + self._assert_enc_close(wr.velocity, 9444) + self._assert_enc_close(wr.acceleration, 27705) + self.assertAlmostEqual(plan.move_time, 0.774597, places=3) + + if __name__ == "__main__": unittest.main() From f4427fbd4f94239c08ba129c2c4fa963cbfa6389 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sat, 2 May 2026 20:26:18 -0700 Subject: [PATCH 70/93] KX2: collapse duplicate direction-aware delta + parameterize cap-helper calls MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 d6dd639bf. Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/kinematics.py | 187 +++++++++++++------------------ 1 file changed, 81 insertions(+), 106 deletions(-) diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index cd73e0cd2fb..f556e2853d2 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -197,6 +197,45 @@ def _profile(dist: float, v: float, a: float) -> tuple: return v, a, t_acc, t_cruise + 2.0 * t_acc +def _directional_delta(target: float, current: float, ax_cfg) -> float: + """Signed joint delta honoring `unlimited_travel` direction modes. + + For non-`unlimited_travel` axes the delta is the literal `target − current`. + For unlimited axes the delta is rewritten to walk the direction the config + asks for (CW=negative, CCW=positive, ShortestWay=≤180°).""" + d = target - current + if not ax_cfg.unlimited_travel: + return d + span = ax_cfg.max_travel - ax_cfg.min_travel + dir_ = ax_cfg.joint_move_direction + if dir_ == JointMoveDirection.Clockwise and d > 0.01: + d -= span + elif dir_ == JointMoveDirection.Counterclockwise and d < -0.01: + d += span + elif dir_ == JointMoveDirection.ShortestWay: + if d > 180.0: + d -= span + elif d < -180.0: + d += span + return d + + +def _stretch_to_time(dist: float, v: float, a: float, T: float) -> tuple: + """Slow a single-axis trapezoidal/triangular profile so it lands at time T. + + Given the pass-1 profile (v, a) for `dist`, scale (v, a) by the same factor + k so the new profile takes time T. Preserving the v/a ratio keeps the + acceleration-time fixed; only the cruise leg stretches. Returns (v, a) for + the stretched profile.""" + if dist <= 0 or a <= 0 or v <= 0: + return v, a + denom = v * (T - v / a) + if abs(denom) < 1e-12: + return v, a + k = dist / denom + return v * k, a * k + + def plan_joint_move( current: Dict[Axis, float], target: Dict[Axis, float], @@ -288,35 +327,15 @@ def plan_joint_move( target[ax] = _wrap_to_range(target[ax], ax_cfg.min_travel, ax_cfg.max_travel) # Direction-aware deltas in cmd_pos units (elbow as linear extension), for - # the gripper-speed cap helper. Identical logic to the dist computation - # below, but evaluated against the un-converted joints so the cap helper - # sees the trajectory the arm actually walks. - cap_deltas: Dict[Axis, float] = {} - for ax in axes: - ax_cfg = cfg.axes[ax] - if ax_cfg.unlimited_travel: - d = target_cmd_units[ax] - curr_cmd_units.get(ax, 0.0) - span = ax_cfg.max_travel - ax_cfg.min_travel - dir_ = ax_cfg.joint_move_direction - if dir_ == JointMoveDirection.Clockwise and d > 0.01: - d -= span - elif dir_ == JointMoveDirection.Counterclockwise and d < -0.01: - d += span - elif dir_ == JointMoveDirection.ShortestWay: - if d > 180.0: - d -= span - elif d < -180.0: - d += span - cap_deltas[ax] = d - else: - cap_deltas[ax] = target_cmd_units[ax] - curr_cmd_units.get(ax, 0.0) - - # Per-axis caps from the gripper-speed limit. Helper iterates the joint - # path in `fk`'s natural units. Servo gripper isn't in fk, so it always - # runs at firmware max regardless of cap. + # the gripper-speed cap helper that walks the path in `fk`'s natural units. + cap_deltas: Dict[Axis, float] = { + ax: _directional_delta(target_cmd_units[ax], curr_cmd_units.get(ax, 0.0), cfg.axes[ax]) + for ax in axes + } + + # Per-axis caps from the gripper-speed/accel limits. Servo gripper isn't in + # fk, so it always runs at firmware max regardless of cap. arm_axes = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) - capped_v: Dict[Axis, float] = {} - capped_a: Dict[Axis, float] = {} cap_requested = max_gripper_speed is not None or max_gripper_acceleration is not None cap_relevant = any(ax in arm_axes for ax in axes) if cap_requested and cap_relevant: @@ -330,104 +349,60 @@ def plan_joint_move( fk_start = {ax: curr_cmd_units[ax] for ax in arm_axes if ax in curr_cmd_units} fk_deltas = {ax: cap_deltas.get(ax, 0.0) for ax in arm_axes if ax in fk_start} fk_loc = lambda j: fk(j, cfg, gripper_params).location - if max_gripper_speed is not None and fk_start: - result = arm_kinematics.joint_velocities_for_max_gripper_speed( - fk=fk_loc, - joints_start=fk_start, - joint_deltas=fk_deltas, - joint_max_velocities={ax: cfg.axes[ax].max_vel for ax in fk_start}, - max_gripper_speed=max_gripper_speed, - num_samples=1000, - eps=1e-3, - ) - capped_v = {ax: abs(vv) for ax, vv in result.items()} - if max_gripper_acceleration is not None and fk_start: + capped: Dict[str, Dict[Axis, float]] = {"v": {}, "a": {}} + for kind, cap, field in ( + ("v", max_gripper_speed, "max_vel"), + ("a", max_gripper_acceleration, "max_accel"), + ): + if cap is None or not fk_start: + continue result = arm_kinematics.joint_velocities_for_max_gripper_speed( fk=fk_loc, joints_start=fk_start, joint_deltas=fk_deltas, - joint_max_velocities={ax: cfg.axes[ax].max_accel for ax in fk_start}, - max_gripper_speed=max_gripper_acceleration, + joint_max_velocities={ax: getattr(cfg.axes[ax], field) for ax in fk_start}, + max_gripper_speed=cap, num_samples=1000, eps=1e-3, ) - capped_a = {ax: abs(aa) for ax, aa in result.items()} + capped[kind] = {ax: abs(x) for ax, x in result.items()} + capped_v, capped_a = capped["v"], capped["a"] - # Distances + initial v/a per axis (planning units = angle for elbow). + # Per-axis trajectory (planning units = angle for elbow). Three-pass sync: + # (1) profile each axis at its firmware max; (2) match accel-times to the + # slowest-accel axis (shrink `a` so all ramps end together); (3) scale (v,a) + # together so all axes finish together at the slowest total time. dist: Dict[Axis, float] = {} v: Dict[Axis, float] = {} a: Dict[Axis, float] = {} - accel_time: Dict[Axis, float] = {} - total_time: Dict[Axis, float] = {} - skip_ax: Dict[Axis, bool] = {} + ta: Dict[Axis, float] = {} + tt: Dict[Axis, float] = {} for ax in axes: ax_cfg = cfg.axes[ax] - if ax_cfg.unlimited_travel: - d = target[ax] - curr[ax] - span = ax_cfg.max_travel - ax_cfg.min_travel - dir_ = ax_cfg.joint_move_direction - if dir_ == JointMoveDirection.Clockwise and d > 0.01: - d -= span - elif dir_ == JointMoveDirection.Counterclockwise and d < -0.01: - d += span - elif dir_ == JointMoveDirection.ShortestWay: - if d > 180.0: - d -= span - elif d < -180.0: - d += span - dist[ax] = abs(d) - else: - dist[ax] = abs(target[ax] - curr[ax]) - - skip_ax[ax] = abs(dist[ax]) < 0.01 + dist[ax] = abs(_directional_delta(target[ax], curr[ax], ax_cfg)) v[ax] = capped_v.get(ax, ax_cfg.max_vel) a[ax] = capped_a.get(ax, ax_cfg.max_accel) - if not skip_ax[ax] and a[ax] > 0: - v[ax], a[ax], accel_time[ax], total_time[ax] = _profile(dist[ax], v[ax], a[ax]) + if dist[ax] >= 0.01 and a[ax] > 0: + v[ax], a[ax], ta[ax], tt[ax] = _profile(dist[ax], v[ax], a[ax]) else: - total_time[ax] = 0.0 - accel_time[ax] = 0.0 - - if all(skip_ax[ax] for ax in axes): + ta[ax] = tt[ax] = 0.0 + moving = [ax for ax in axes if tt[ax] > 0.0] + if not moving: return None - # Sync accel times to the lead axis so all axes ramp together. - lead_acc_ax = max((ax for ax in axes if not skip_ax[ax]), key=lambda ax: accel_time[ax]) - lead_acc_t = accel_time[lead_acc_ax] - for ax in axes: - if ax == lead_acc_ax or skip_ax[ax]: - continue - if accel_time[ax] > lead_acc_t: - v[ax] = lead_acc_t * a[ax] - elif accel_time[ax] < lead_acc_t: - a[ax] = v[ax] / max(lead_acc_t, 1e-9) + lead_acc_t = max(ta[ax] for ax in moving) + for ax in moving: + if ta[ax] < lead_acc_t: + a[ax] = v[ax] / lead_acc_t + v[ax], a[ax], _, tt[ax] = _profile(dist[ax], v[ax], a[ax]) - for ax in axes: - if skip_ax[ax]: - total_time[ax] = 0.0 - continue - v[ax], a[ax], _, total_time[ax] = _profile(dist[ax], v[ax], a[ax]) - - # Sync total times to the lead axis so all axes finish together. - lead_time_ax = max(axes, key=lambda ax: total_time[ax]) - lead_T = total_time[lead_time_ax] - for ax in axes: - if ax == lead_time_ax or skip_ax[ax]: - continue - denom = v[ax] * (lead_T - (v[ax] / max(a[ax], 1e-9))) - if abs(denom) < 1e-12: - continue - k = dist[ax] / denom - v[ax] *= k - a[ax] *= k - - for ax in axes: - if skip_ax[ax]: - total_time[ax] = 0.0 - continue - v[ax], a[ax], _, total_time[ax] = _profile(dist[ax], v[ax], a[ax]) + lead_T = max(tt[ax] for ax in moving) + for ax in moving: + if tt[ax] < lead_T: + v[ax], a[ax] = _stretch_to_time(dist[ax], v[ax], a[ax], lead_T) + v[ax], a[ax], _, tt[ax] = _profile(dist[ax], v[ax], a[ax]) - move_time = max(total_time[ax] for ax in axes) + move_time = max(tt[ax] for ax in moving) # Convert back to encoder units. Elbow target is still in angle space here # — the encoder count for an elbow joint is angle * conv, not mm * conv. From 4c7f1ce9a038cd2af940a723e43312569e030f06 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sat, 2 May 2026 20:47:53 -0700 Subject: [PATCH 71/93] KX2: DS402 fault-reset edge-pulse via SW polling MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/driver.py | 63 ++++++++++++++++++++++++++---------- 1 file changed, 46 insertions(+), 17 deletions(-) diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index 7d545a80b9c..f3dc2c39939 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -1168,11 +1168,22 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N if not use_ds402: await self.write(node_id, "MO", 0, want) elif state: - # DS402 enable sequence: Fault -> Shutdown -> Switched On -> Op Enabled. - # Matches the C# reference (clscanmotor.cs:4495-4509): back-to-back - # CW writes, a single 100 ms settle at the end, then MO query. - for cw in (0, 128, 6, 7, 15): - await self._control_word_set(node_id=node_id, value=cw) + # DS402 enable: edge-pulsed CW writes with SW confirmation between + # transitions. CiA 402 §6.1: Fault -> Switch on disabled fires on + # the rising edge of CW bit 7 (Fault Reset). RPDO1 is mapped + # SynchronousCyclic, so back-to-back writes within one servo cycle + # can be coalesced and the drive never sees the edge — fault never + # clears and the retry loop spins. Polling SW between transitions + # forces each CW write to land on the wire before the next one. + await self._control_word_set(node_id=node_id, value=0x00) # clear bits + await self._control_word_set(node_id=node_id, value=0x80) # fault reset + await self._wait_sw_bit(node_id, bit_mask=1 << 3, want_high=False) + await self._control_word_set(node_id=node_id, value=0x06) # Shutdown + await self._wait_sw_bit(node_id, bit_mask=1 << 0, want_high=True) + await self._control_word_set(node_id=node_id, value=0x07) # Switch on + await self._wait_sw_bit(node_id, bit_mask=1 << 1, want_high=True) + await self._control_word_set(node_id=node_id, value=0x0F) # Enable op + # Bit 2 (Operation enabled) is confirmed by the MO query below. else: # DS402 disable: Op Enabled -> Switched On -> Ready to Switch On. # Matches C# (clscanmotor.cs:4540-4543) — back-to-back, no inter-CW sleep. @@ -1336,31 +1347,36 @@ async def _trigger_new_setpoint( f"rising edge)" ) - async def _wait_setpoint_ack( - self, node_id: int, *, want_high: bool, timeout: float = 0.05 + async def _wait_sw_bit( + self, + node_id: int, + *, + bit_mask: int, + want_high: bool, + timeout_s: float = 0.05, ) -> bool: - """Wait until 0x6041 bit 12 matches ``want_high`` (or timeout). + """Wait until ``self._statusword[node_id] & bit_mask`` matches ``want_high``. - TPDO3 maps StatusWord with the StatusWordEvent trigger; the canopen - listener thread parses each frame into self._statusword[node_id] and - signals self._statusword_event[node_id]. We wait on the event, with + TPDO3 maps StatusWord (0x6041) with the StatusWordEvent trigger; the + canopen listener thread parses each frame into self._statusword[node_id] + and signals self._statusword_event[node_id]. We wait on the event, with a 5 ms grace before falling back to an SDO probe — covers the case - where the drive's event-trigger config didn't take and TPDO3 is - silent. 50 ms total is plenty: bit 12 flips within a servo cycle - (~1-2 ms) once the drive sees the edge. + where the drive's event-trigger config didn't take and TPDO3 is silent. + Returns True on a match, False on timeout. """ assert self._loop is not None ev = self._statusword_event.get(node_id) - deadline = self._loop.time() + timeout + deadline = self._loop.time() + timeout_s while self._loop.time() < deadline: sw = self._statusword.get(node_id) - if sw is not None and bool(sw & (1 << 12)) == want_high: + if sw is not None and bool(sw & bit_mask) == want_high: return True if ev is None: # No subscription (drive outside motion_node_ids); SDO poll only. raw = await self._can_sdo_upload(node_id, 0x6041, 0x00) sw = int.from_bytes(raw[:2], "little") - if bool(sw & (1 << 12)) == want_high: + self._statusword[node_id] = sw + if bool(sw & bit_mask) == want_high: return True await asyncio.sleep(0.001) continue @@ -1376,6 +1392,19 @@ async def _wait_setpoint_ack( self._statusword[node_id] = sw return False + async def _wait_setpoint_ack( + self, node_id: int, *, want_high: bool, timeout: float = 0.05 + ) -> bool: + """Wait until 0x6041 bit 12 (setpoint_ack) matches ``want_high``. + + Thin specialization of :meth:`_wait_sw_bit` for the PPM trigger + handshake. 50 ms total is plenty: bit 12 flips within a servo cycle + (~1-2 ms) once the drive sees the CW bit-4 edge. + """ + return await self._wait_sw_bit( + node_id, bit_mask=1 << 12, want_high=want_high, timeout_s=timeout + ) + async def user_program_run( self, node_id: int, From 5917e4fb9a61b2f284128c46706850f8f5ad7d30 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sat, 2 May 2026 20:48:07 -0700 Subject: [PATCH 72/93] =?UTF-8?q?KX2:=20fix=20elbow=20>90=C2=B0=20round-tr?= =?UTF-8?q?ip=20=E2=80=94=20missing=20zero=5Foffset=20in=20inverse=20refle?= =?UTF-8?q?ction?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/kinematics.py | 10 +++++++--- pylabrobot/paa/kx2/tests/planner_tests.py | 14 +++++--------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index f556e2853d2..d0018bb9a3a 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -157,10 +157,14 @@ def convert_elbow_position_to_angle(cfg: KX2Config, elbow_pos: float) -> float: def convert_elbow_angle_to_position(cfg: KX2Config, elbow_angle_deg: float) -> float: max_travel = cfg.axes[Axis.ELBOW].max_travel - elbow_pos = (max_travel + cfg.elbow_zero_offset) * sin(elbow_angle_deg * (pi / 180.0)) - cfg.elbow_zero_offset + denom = max_travel + cfg.elbow_zero_offset if elbow_angle_deg > 90.0: - elbow_pos = 2.0 * max_travel - elbow_pos - return elbow_pos + # Inverse of `90 + asin((2·max − pos + zero)/(max + zero))`. The `+ zero` + # term has to appear on both sides of the reflection or the round-trip + # drifts by ~zero·(max+zero)/max — silent encoder-read miscalibration + # when the joint is past peak extension. + return 2.0 * max_travel + cfg.elbow_zero_offset - denom * sin((elbow_angle_deg - 90.0) * (pi / 180.0)) + return denom * sin(elbow_angle_deg * (pi / 180.0)) - cfg.elbow_zero_offset # --- trajectory planning --------------------------------------------------- diff --git a/pylabrobot/paa/kx2/tests/planner_tests.py b/pylabrobot/paa/kx2/tests/planner_tests.py index 03ef771d92a..52a15bd1aa9 100644 --- a/pylabrobot/paa/kx2/tests/planner_tests.py +++ b/pylabrobot/paa/kx2/tests/planner_tests.py @@ -314,16 +314,12 @@ def test_round_trip_below_90(self): back = kinematics.convert_elbow_angle_to_position(cfg, angle) self.assertAlmostEqual(back, pos, places=6, msg=f"pos={pos}") - @unittest.expectedFailure def test_round_trip_above_90(self): - """Same identity for the piecewise-reflected branch (angle > 90). - - The pos->angle formula uses ``(2*max - pos + zero_offset)``; the - angle->pos formula reflects via ``2*max - pos`` (no zero_offset - correction). The two are not strict inverses when zero_offset != 0, - so this test is currently expected to fail. The planner only calls - pos->angle, so this asymmetry is not on a hot path; flagging it - here as a regression candidate. + """Inverse identity for the piecewise-reflected branch (angle > 90). + + Forward maps pos > max via ``90 + asin((2*max - pos + zero) / (max + zero))``; + the inverse must include the matching ``+ zero`` term so that pos -> angle + -> pos is the identity for any pos in the second-half domain. """ cfg = _config(elbow=_axis(min_travel=0.0, max_travel=300.0), elbow_zero_offset=5.0) for pos in (300.5, 320.0, 380.0, 450.0, 550.0, 600.0): From c8a60b6d5c40f0905fd4feea43166823cf6a91aa Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sat, 2 May 2026 20:48:20 -0700 Subject: [PATCH 73/93] =?UTF-8?q?KX2:=20home=20routines=20are=20gripper-on?= =?UTF-8?q?ly=20=E2=80=94=20drop=20the=20axis=20arg,=20rename=20helpers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/arm_backend.py | 205 ++++++++++++------------------ 1 file changed, 81 insertions(+), 124 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 8921a3dd003..b2653f299a6 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -83,7 +83,7 @@ def __init__( # Reentrant motion guard. Two callers staging move setpoints # (target/vel/accel SDOs) on the same drives interleave and the drives # execute some random mix. Compound ops (pick_up_at_location, - # find_z_with_proximity_sensor, home_motor) hold this for their full + # find_z_with_proximity_sensor, _home_servo_gripper) hold this for their full # duration; the inner motion primitive (motors_move_joint / # motors_move_absolute_execute) re-enters via _motion_owner. halt() # deliberately skips the lock — emergency-stop must interrupt regardless. @@ -184,169 +184,138 @@ async def get_estop_state(self) -> bool: b15 = (r & 0x8000) == 0x8000 return (not b14) and (not b15) - async def _motor_set_homed_status(self, axis: Axis, status: HomeStatus) -> None: - await self.driver.write(axis, "UI", 3, int(status)) + async def gripper_get_homed_status(self) -> HomeStatus: + return HomeStatus(await self.driver.query_int(Axis.SERVO_GRIPPER, "UI", 3)) - async def motor_get_homed_status(self, axis: Axis) -> HomeStatus: - return HomeStatus(await self.driver.query_int(axis, "UI", 3)) + async def _gripper_set_homed_status(self, status: HomeStatus) -> None: + await self.driver.write(Axis.SERVO_GRIPPER, "UI", 3, int(status)) - async def _motor_reset_encoder_position(self, axis: Axis, position: float) -> None: - nid = axis - await self.driver.write(nid, "HM", 1, 0) - await self.driver.write(nid, "HM", 3, 0) - await self.driver.write(nid, "HM", 4, 0) - await self.driver.write(nid, "HM", 5, 0) + async def _gripper_reset_encoder_position(self, position: float) -> None: + sg = Axis.SERVO_GRIPPER + await self.driver.write(sg, "HM", 1, 0) + await self.driver.write(sg, "HM", 3, 0) + await self.driver.write(sg, "HM", 4, 0) + await self.driver.write(sg, "HM", 5, 0) # Old code packed `position` as int32 via `int(round(float(str(position))))`; # preserve that rounding semantic for callers that pass fractional values. - await self.driver.write(nid, "HM", 2, int(round(position))) - await self.driver.write(nid, "HM", 1, 1) + await self.driver.write(sg, "HM", 2, int(round(position))) + await self.driver.write(sg, "HM", 1, 1) - async def _motor_hard_stop_search( - self, - axis: Axis, - srch_vel: int, - srch_acc: int, - max_pe: int, - hs_pe: int, - timeout: float, + async def _gripper_hard_stop_search( + self, srch_vel: int, srch_acc: int, max_pe: int, hs_pe: int, timeout: float, ) -> None: - # int() at every driver call: driver's contract is `node_id: int`. Axis - # is IntEnum so it works by accident, but Axis.value collides with - # _GROUP_NODE_ID == 10 if anyone ever adds Axis.AUX = 10 — broadcast - # would silently dispatch to all motion nodes. - nid = axis - await self.driver.write(nid, "ER", 3, max_pe * 10) - await self.driver.write(nid, "AC", 0, srch_acc) - await self.driver.write(nid, "DC", 0, srch_acc) + sg = Axis.SERVO_GRIPPER + await self.driver.write(sg, "ER", 3, max_pe * 10) + await self.driver.write(sg, "AC", 0, srch_acc) + await self.driver.write(sg, "DC", 0, srch_acc) for i in [3, 4, 5, 2]: - await self.driver.write(nid, "HM", i, 0) - await self.driver.write(nid, "JV", 0, srch_vel) + await self.driver.write(sg, "HM", i, 0) + await self.driver.write(sg, "JV", 0, srch_vel) try: params: List[Union[int, float]] = [int(hs_pe), int(timeout * 1000)] last_line = await self.driver.user_program_run( - nid, "Home", params, int(timeout), True + sg, "Home", params, int(timeout), True ) if last_line in [1, 2, 3]: raise RuntimeError(f"Homing Script Error {34 + last_line}") - curr_pos = await self.driver.motor_get_current_position(nid) - await self.driver.write(nid, "PA", 0, curr_pos) - await self.driver.write(nid, "SP", 0, srch_vel) - await self.driver.write(nid, "AC", 0, srch_acc) - await self.driver.write(nid, "DC", 0, srch_acc) + curr_pos = await self.driver.motor_get_current_position(sg) + await self.driver.write(sg, "PA", 0, curr_pos) + await self.driver.write(sg, "SP", 0, srch_vel) + await self.driver.write(sg, "AC", 0, srch_acc) + await self.driver.write(sg, "DC", 0, srch_acc) finally: await asyncio.sleep(0.3) - await self.driver.execute(nid, "BG", 0) + await self.driver.execute(sg, "BG", 0) await asyncio.sleep(0.3) - await self.driver.write(nid, "ER", 3, int(max_pe)) + await self.driver.write(sg, "ER", 3, int(max_pe)) - async def _motor_index_search( - self, - axis: Axis, - srch_vel: int, - srch_acc: int, - positive_direction: bool, - timeout: float, + async def _gripper_index_search( + self, srch_vel: int, srch_acc: int, positive_direction: bool, timeout: float, ) -> tuple: - nid = axis - await self.driver.write(nid, "HM", 1, 0) + sg = Axis.SERVO_GRIPPER + await self.driver.write(sg, "HM", 1, 0) - one_revolution = await self.driver.query_int(nid, "CA", 18) + one_revolution = await self.driver.query_int(sg, "CA", 18) if not positive_direction: one_revolution *= -1 - await self.driver.write(nid, "PR", 1, one_revolution) - await self.driver.write(nid, "SP", 0, srch_vel) - await self.driver.write(nid, "AC", 0, srch_acc) - await self.driver.write(nid, "DC", 0, srch_acc) + await self.driver.write(sg, "PR", 1, one_revolution) + await self.driver.write(sg, "SP", 0, srch_vel) + await self.driver.write(sg, "AC", 0, srch_acc) + await self.driver.write(sg, "DC", 0, srch_acc) - await self.driver.write(nid, "HM", 3, 3) # index only - await self.driver.write(nid, "HM", 4, 0) - await self.driver.write(nid, "HM", 5, 0) - await self.driver.write(nid, "HM", 2, 0) - await self.driver.write(nid, "HM", 1, 1) # arm + await self.driver.write(sg, "HM", 3, 3) # index only + await self.driver.write(sg, "HM", 4, 0) + await self.driver.write(sg, "HM", 5, 0) + await self.driver.write(sg, "HM", 2, 0) + await self.driver.write(sg, "HM", 1, 1) # arm - await self.driver.execute(nid, "BG", 0) - await self.driver.wait_for_moves_done([nid], timeout) + await self.driver.execute(sg, "BG", 0) + await self.driver.wait_for_moves_done([sg], timeout) - left = await self.driver.query_int(nid, "HM", 1) + left = await self.driver.query_int(sg, "HM", 1) if left != 0: raise RuntimeError("Homing Failure: Failed to finish index pulse search.") - captured_position = await self.driver.query_int(nid, "HM", 7) + captured_position = await self.driver.query_int(sg, "HM", 7) return one_revolution, captured_position - async def home_motor( - self, - axis: Axis, - hs_offset: int, - ind_offset: int, - home_pos: int, - srch_vel: int, - srch_acc: int, - max_pe: int, - hs_pe: int, - offset_vel: int, - offset_acc: int, - timeout: float, - ) -> None: - nid = axis + async def _home_servo_gripper(self, sgc: ServoGripperConfig) -> None: + """Hard-stop + index-pulse home for the servo gripper.""" + sg = Axis.SERVO_GRIPPER + timeout = sgc.home_timeout_msec / 1000 async with self._motion_guard(): - left = await self.driver.query_int(nid, "CA", 41) - if left == 24: - raise RuntimeError("Error 43") + ca41 = await self.driver.query_int(sg, "CA", 41) + if ca41 == 24: + raise RuntimeError( + f"Servo gripper not ready to home (drive reports CA[41]={ca41}). " + "Power-cycle the drive or re-check homing config." + ) try: - await self._motor_hard_stop_search(axis, srch_vel, srch_acc, max_pe, hs_pe, timeout) + await self._gripper_hard_stop_search( + sgc.home_search_vel, sgc.home_search_accel, + sgc.home_default_position_error, sgc.home_hard_stop_position_error, + timeout, + ) except Exception as e: # motor_check_if_move_done already raised with the rich EMCY # description ("Motor Fault: ..."). Don't overwrite it with the # duller MF-bit register read. if str(e).startswith("Motor Fault:"): raise - fault = await self.driver.motor_get_fault(nid) + fault = await self.driver.motor_get_fault(sg) if fault is not None: raise RuntimeError(fault) from e raise - await self.driver.motor_enable(node_id=nid, state=True, use_ds402=axis.is_motion) + await self.driver.motor_enable(node_id=sg, state=True, use_ds402=False) await self._motors_move_absolute_execute_locked( - plan=MotorsMovePlan( - moves=[ - MotorMoveParam( - node_id=nid, - position=hs_offset, - velocity=offset_vel, - acceleration=offset_acc, - relative=False, - direction=JointMoveDirection.ShortestWay, - ) - ], - ) + plan=MotorsMovePlan(moves=[MotorMoveParam( + node_id=sg, position=sgc.home_hard_stop_offset, + velocity=sgc.home_offset_vel, acceleration=sgc.home_offset_accel, + relative=False, direction=JointMoveDirection.ShortestWay, + )]) ) - is_positive = hs_offset > 0 - await self._motor_index_search(axis, abs(srch_vel), srch_acc, is_positive, timeout) + is_positive = sgc.home_hard_stop_offset > 0 + await self._gripper_index_search( + abs(sgc.home_search_vel), sgc.home_search_accel, is_positive, timeout, + ) await self._motors_move_absolute_execute_locked( - plan=MotorsMovePlan( - moves=[ - MotorMoveParam( - node_id=nid, - position=ind_offset, - velocity=offset_vel, - acceleration=offset_acc, - relative=False, - direction=JointMoveDirection.ShortestWay, - ) - ] - ) + plan=MotorsMovePlan(moves=[MotorMoveParam( + node_id=sg, position=sgc.home_index_offset, + velocity=sgc.home_offset_vel, acceleration=sgc.home_offset_accel, + relative=False, direction=JointMoveDirection.ShortestWay, + )]) ) - await self._motor_reset_encoder_position(axis, home_pos) - await self._motor_set_homed_status(axis, HomeStatus.Homed) + await self._gripper_reset_encoder_position(sgc.home_pos) + await self._gripper_set_homed_status(HomeStatus.Homed) # -- servo gripper ------------------------------------------------------ @@ -368,19 +337,7 @@ async def servo_gripper_home(self) -> None: await self.driver.write(sg, "PL", 1, sgc.peak_current) await self.driver.write(sg, "CL", 1, sgc.continuous_current) - await self.home_motor( - axis=Axis.SERVO_GRIPPER, - hs_offset=sgc.home_hard_stop_offset, - ind_offset=sgc.home_index_offset, - home_pos=sgc.home_pos, - srch_vel=sgc.home_search_vel, - srch_acc=sgc.home_search_accel, - max_pe=sgc.home_default_position_error, - hs_pe=sgc.home_hard_stop_position_error, - offset_vel=sgc.home_offset_vel, - offset_acc=sgc.home_offset_accel, - timeout=sgc.home_timeout_msec / 1000, - ) + await self._home_servo_gripper(sgc) await self._set_servo_gripper_force_limit(100) From e854279e3d3f68ae49fea4b31dbe2dae08338f66 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sat, 2 May 2026 20:48:31 -0700 Subject: [PATCH 74/93] KX2: FK/IK anchor tests pinning real-value behavior MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/tests/kinematics_tests.py | 119 +++++++++++++++++++ 1 file changed, 119 insertions(+) diff --git a/pylabrobot/paa/kx2/tests/kinematics_tests.py b/pylabrobot/paa/kx2/tests/kinematics_tests.py index b60806b048a..31130addf20 100644 --- a/pylabrobot/paa/kx2/tests/kinematics_tests.py +++ b/pylabrobot/paa/kx2/tests/kinematics_tests.py @@ -124,6 +124,125 @@ def test_ik_shoulder_branch_convention(self): ) +class FKIKAnchors(unittest.TestCase): + """Anchor points pinning FK and IK output values for representative + joints/poses. Values were snapshotted from the current implementation — + if you change the FK or IK formula and these tests fail, the new + output is provably different from the old one and needs explicit + hardware verification.""" + + def _cfg(self): + def ax(min_travel=-180.0, max_travel=180.0, unlimited_travel=False): + return AxisConfig( + motor_conversion_factor=1.0, max_travel=max_travel, min_travel=min_travel, + unlimited_travel=unlimited_travel, absolute_encoder=True, + max_vel=100.0, max_accel=100.0, + joint_move_direction=JointMoveDirection.Normal, + digital_inputs={}, analog_inputs={}, outputs={}, + ) + return KX2Config( + wrist_offset=10.0, elbow_offset=20.0, elbow_zero_offset=5.0, + axes={ + Axis.SHOULDER: ax(), + Axis.Z: ax(min_travel=0.0, max_travel=750.0), + Axis.ELBOW: ax(min_travel=0.0, max_travel=300.0), + Axis.WRIST: ax(unlimited_travel=True), + }, + base_to_gripper_clearance_z=0.0, base_to_gripper_clearance_arm=0.0, + robot_on_rail=False, servo_gripper=None, + ) + + def _gripper(self): + return GripperParams(length=15.0, z_offset=3.0, finger_side="barcode_reader") + + def test_fk_zero_pose(self): + """All joints at 0 → gripper at (0, 20, -3, 0°) for the default + barcode-reader gripper (15 mm length, 3 mm z_offset).""" + p = kinematics.fk( + {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 0.0, Axis.WRIST: 0.0}, + self._cfg(), self._gripper(), + ) + self.assertAlmostEqual(p.location.x, 0.0, places=6) + self.assertAlmostEqual(p.location.y, 20.0, places=6) + self.assertAlmostEqual(p.location.z, -3.0, places=6) + self.assertAlmostEqual(p.rotation.z, 0.0, places=6) + + def test_fk_q1_mid(self): + p = kinematics.fk( + {Axis.SHOULDER: 30.0, Axis.Z: 100.0, Axis.ELBOW: 80.0, Axis.WRIST: 0.0}, + self._cfg(), self._gripper(), + ) + self.assertAlmostEqual(p.location.x, -50.0, places=6) + self.assertAlmostEqual(p.location.y, 86.6025, places=4) + self.assertAlmostEqual(p.location.z, 97.0, places=6) + self.assertAlmostEqual(p.rotation.z, 30.0, places=6) + + def test_fk_q2_mid(self): + p = kinematics.fk( + {Axis.SHOULDER: -45.0, Axis.Z: 50.0, Axis.ELBOW: 120.0, Axis.WRIST: 30.0}, + self._cfg(), self._gripper(), + ) + self.assertAlmostEqual(p.location.x, 105.7193, places=4) + self.assertAlmostEqual(p.location.y, 95.1127, places=4) + self.assertAlmostEqual(p.location.z, 47.0, places=6) + self.assertAlmostEqual(p.rotation.z, -15.0, places=6) + + def test_fk_max_extension_with_wrist(self): + p = kinematics.fk( + {Axis.SHOULDER: 90.0, Axis.Z: 200.0, Axis.ELBOW: 250.0, Axis.WRIST: -45.0}, + self._cfg(), self._gripper(), + ) + self.assertAlmostEqual(p.location.x, -274.3934, places=4) + self.assertAlmostEqual(p.location.y, -10.6066, places=4) + self.assertAlmostEqual(p.location.z, 197.0, places=6) + self.assertAlmostEqual(p.rotation.z, 45.0, places=6) + + def test_fk_wrist_180_keeps_yaw_in_range(self): + """Wrist at 180° + shoulder at 0° gives yaw=180° (not -180° via the + boundary snap).""" + p = kinematics.fk( + {Axis.SHOULDER: 0.0, Axis.Z: 0.0, Axis.ELBOW: 100.0, Axis.WRIST: 180.0}, + self._cfg(), self._gripper(), + ) + self.assertAlmostEqual(p.location.x, 0.0, places=6) + self.assertAlmostEqual(p.location.y, 150.0, places=6) + self.assertAlmostEqual(p.rotation.z, 180.0, places=6) + + def test_ik_on_plus_y_axis(self): + """Target on +Y axis → shoulder = 0; elbow = y - sum_of_offsets.""" + pose = GripperLocation(location=Coordinate(x=0, y=300, z=50), rotation=Rotation(z=0)) + j = kinematics.ik(pose, self._cfg(), self._gripper()) + self.assertAlmostEqual(j[Axis.SHOULDER], 0.0, places=6) + self.assertAlmostEqual(j[Axis.Z], 53.0, places=6) + self.assertAlmostEqual(j[Axis.ELBOW], 280.0, places=4) + self.assertAlmostEqual(j[Axis.WRIST], 0.0, places=6) + + def test_ik_q1_target(self): + pose = GripperLocation(location=Coordinate(x=100, y=200, z=100), rotation=Rotation(z=30)) + j = kinematics.ik(pose, self._cfg(), self._gripper()) + self.assertAlmostEqual(j[Axis.SHOULDER], -23.474916, places=4) + self.assertAlmostEqual(j[Axis.Z], 103.0, places=6) + self.assertAlmostEqual(j[Axis.ELBOW], 197.209286, places=4) + self.assertAlmostEqual(j[Axis.WRIST], 53.474916, places=4) + + def test_ik_q2_target(self): + pose = GripperLocation(location=Coordinate(x=-150, y=150, z=75), rotation=Rotation(z=-45)) + j = kinematics.ik(pose, self._cfg(), self._gripper()) + self.assertAlmostEqual(j[Axis.SHOULDER], 40.955309, places=4) + self.assertAlmostEqual(j[Axis.Z], 78.0, places=6) + self.assertAlmostEqual(j[Axis.ELBOW], 177.661703, places=4) + self.assertAlmostEqual(j[Axis.WRIST], -85.955309, places=4) + + def test_ik_negative_quadrant(self): + """Target in negative-Y region — shoulder rotates past 90°.""" + pose = GripperLocation(location=Coordinate(x=-50, y=-200, z=120), rotation=Rotation(z=90)) + j = kinematics.ik(pose, self._cfg(), self._gripper()) + self.assertAlmostEqual(j[Axis.SHOULDER], 161.995838, places=4) + self.assertAlmostEqual(j[Axis.Z], 123.0, places=6) + self.assertAlmostEqual(j[Axis.ELBOW], 175.297408, places=4) + self.assertAlmostEqual(j[Axis.WRIST], -71.995838, places=4) + + class IKErrors(unittest.TestCase): def test_x_rotation_raises_ikerror(self): c = _config() From 54c69e3561ec40c83a5d92c10cdaadeccc80504f Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sat, 2 May 2026 20:59:09 -0700 Subject: [PATCH 75/93] =?UTF-8?q?KX2:=20driver-side=20tests=20=E2=80=94=20?= =?UTF-8?q?=5Ftrigger=5Fnew=5Fsetpoint,=20=5Fread=5Faxis=5Fconfig,=20find?= =?UTF-8?q?=5Fz?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- .../arm_backend_find_z_proximity_tests.py | 267 ++++++++++++++++++ .../arm_backend_read_axis_config_tests.py | 243 ++++++++++++++++ .../tests/driver_setpoint_trigger_tests.py | 194 +++++++++++++ 3 files changed, 704 insertions(+) create mode 100644 pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py create mode 100644 pylabrobot/paa/kx2/tests/arm_backend_read_axis_config_tests.py create mode 100644 pylabrobot/paa/kx2/tests/driver_setpoint_trigger_tests.py diff --git a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py new file mode 100644 index 00000000000..3a7fe7b260c --- /dev/null +++ b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py @@ -0,0 +1,267 @@ +"""Unit tests for ``KX2ArmBackend.find_z_with_proximity_sensor``. + +Covers the four interesting paths through the descent + IL-arm + cancel-on-trip +state machine: + +* Sensor trips mid-descent -> task is cancelled, motor_stop + IL-restore run. +* Sensor never trips -> RuntimeError mentioning the descent range. +* Move task raises a CanError mid-descent -> cleanup still runs, no exception + escapes from the swallow at ``await move_task``. +* Sensor already tripped at start -> short-circuit returns z0; no IL change, + no descent task spawned. + +The fake backend stubs the driver methods the function actually calls +(``motor_enable``, ``motor_stop``, ``configure_input_logic``, +``motor_get_current_position`` via ``read_input``-style scripts) and replaces +``_motors_move_joint_locked`` with an async sleeper so the test controls +when the descent "finishes". The motion guard is initialised so +``async with self._motion_guard()`` works without a real lock setup race. +""" +import asyncio +import unittest +from typing import Any, Dict, List, NamedTuple, Optional, Tuple + +from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend +from pylabrobot.paa.kx2.config import Axis +from pylabrobot.paa.kx2.driver import CanError, _InputLogic + + +class _FakeDriver: + """Records every method ``find_z_with_proximity_sensor`` calls. Each + method is async so ``await`` works as in production.""" + + def __init__(self) -> None: + self.motor_enable_calls: List[Tuple[Any, bool, bool]] = [] + self.motor_stop_calls: List[Axis] = [] + self.configure_il_calls: List[Tuple[Any, int, _InputLogic]] = [] + self.motor_stop_should_raise: Optional[Exception] = None + + async def motor_enable(self, *, node_id: Any, state: bool, use_ds402: bool) -> None: + self.motor_enable_calls.append((node_id, state, use_ds402)) + + async def motor_stop(self, axis: Axis) -> None: + self.motor_stop_calls.append(axis) + if self.motor_stop_should_raise is not None: + raise self.motor_stop_should_raise + + async def configure_input_logic( + self, axis: Any, input_num: int, logic: _InputLogic, + ) -> None: + self.configure_il_calls.append((axis, input_num, logic)) + + +class _FindZHarness(NamedTuple): + """Bundle of test handles. Returning a typed tuple keeps mypy happy + (fake_driver is _FakeDriver, not KX2Driver) without polluting the + KX2ArmBackend instance with ad-hoc attributes.""" + backend: KX2ArmBackend + fake_driver: _FakeDriver + move_calls: List[Dict[str, Any]] + + +def _build_harness( + *, + proximity_script: List[bool], + z_positions: List[float], + move_behavior: str = "long_sleep", + move_exception: Optional[Exception] = None, +) -> _FindZHarness: + """Construct a KX2ArmBackend wired for find_z testing. + + Args: + proximity_script: Sequence of bools returned by ``read_proximity_sensor``; + one popped per call. The last element is sticky once exhausted. + z_positions: Sequence returned by ``motor_get_current_position`` calls. + Same exhaust-then-sticky behavior. + move_behavior: ``"long_sleep"`` (default; awaits ~10 s so the test cancels + it via the trip path) or ``"completes_immediately"`` (returns as soon + as the task is scheduled — used for the "sensor never trips" test). + move_exception: If set, ``_motors_move_joint_locked`` raises this from + inside the spawned task before it would otherwise sleep. + """ + backend = KX2ArmBackend.__new__(KX2ArmBackend) + fake_driver = _FakeDriver() + backend.driver = fake_driver # type: ignore[assignment] + backend._motion_lock = asyncio.Lock() + backend._motion_owner = None + backend._gripper_params = None # type: ignore[assignment] # not consumed by find_z + + prox = list(proximity_script) + zs = list(z_positions) + + async def _read_proximity() -> bool: + # Yield once so any concurrently scheduled task (the descent move) gets + # a chance to run before we observe the next sensor value. Without this + # an immediate-True script can race the move task and cancel it before + # it has a chance to record its call. + await asyncio.sleep(0) + val = prox[0] if len(prox) == 1 else prox.pop(0) + return val + + async def _get_z(axis: Axis) -> float: + val = zs[0] if len(zs) == 1 else zs.pop(0) + return float(val) + + move_calls: List[Dict[str, Any]] = [] + + async def _fake_move(cmd_pos: Dict[Axis, float], params: Any = None) -> None: + move_calls.append({"cmd_pos": dict(cmd_pos), "params": params}) + if move_exception is not None: + raise move_exception + if move_behavior == "completes_immediately": + return + # "long_sleep": stays awaitable until cancelled by find_z's trip handler. + await asyncio.sleep(10.0) + + backend.read_proximity_sensor = _read_proximity # type: ignore[assignment] + backend.motor_get_current_position = _get_z # type: ignore[assignment] + backend._motors_move_joint_locked = _fake_move # type: ignore[assignment] + return _FindZHarness(backend=backend, fake_driver=fake_driver, move_calls=move_calls) + + +class FindZSensorTripsCleanupTests(unittest.TestCase): + def test_trip_cancels_descent_and_runs_cleanup(self): + """Sensor returns False on the pre-check, then True after one poll + cycle. Verify: descent task is spawned, cancelled, motor_stop runs, + IL is restored to GeneralPurpose.""" + h = _build_harness( + # Pre-check (False), then one poll iteration True (trip). + proximity_script=[False, True], + # z0 read after motor_enable; final z read after the task is cancelled. + z_positions=[100.0, 95.5], + ) + + z = asyncio.run(h.backend.find_z_with_proximity_sensor(max_descent=20.0)) + + self.assertAlmostEqual(z, 95.5, places=9) + # IL was armed once, then restored once. + arm = (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.StopForward) + restore = (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose) + self.assertEqual(h.fake_driver.configure_il_calls, [arm, restore]) + # motor_stop ran exactly once on the Z axis. + self.assertEqual(h.fake_driver.motor_stop_calls, [Axis.Z]) + # Pre-flight motor_enable still happened. + self.assertEqual(h.fake_driver.motor_enable_calls, [(Axis.Z, True, True)]) + # Descent target is z0 - max_descent. + self.assertEqual(len(h.move_calls), 1) + self.assertAlmostEqual(h.move_calls[0]["cmd_pos"][Axis.Z], 80.0, places=9) + + +class FindZSensorNeverTripsTests(unittest.TestCase): + def test_never_trips_raises_runtime_error_with_range(self): + """Move completes (no trip), sensor stayed False -> raises with the + descent range and start/end Z values in the message.""" + h = _build_harness( + proximity_script=[False], # always False + z_positions=[100.0, 79.5], # z0=100, z_end after descent=79.5 + move_behavior="completes_immediately", + ) + + with self.assertRaises(RuntimeError) as ctx: + asyncio.run(h.backend.find_z_with_proximity_sensor(max_descent=20.0)) + + msg = str(ctx.exception) + self.assertIn("proximity sensor never tripped", msg) + self.assertIn("20.0", msg) + self.assertIn("100.00", msg) + self.assertIn("79.50", msg) + # Cleanup still ran. + self.assertEqual(h.fake_driver.motor_stop_calls, [Axis.Z]) + self.assertIn( + (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose), + h.fake_driver.configure_il_calls, + ) + + +class FindZMoveRaisesCanErrorTests(unittest.TestCase): + def test_canerror_in_descent_is_swallowed_cleanup_runs(self): + """The descent task raises CanError. ``await move_task`` swallows it + (per the ``except (asyncio.CancelledError, CanError): pass``). Sensor + stayed False -> the function then raises the standard RuntimeError + *after* cleanup. motor_stop + IL restore still ran.""" + h = _build_harness( + proximity_script=[False], + z_positions=[100.0, 100.0], + move_exception=CanError("synthetic descent failure"), + ) + + with self.assertRaises(RuntimeError) as ctx: + asyncio.run(h.backend.find_z_with_proximity_sensor(max_descent=20.0)) + + # The post-cleanup "never tripped" branch runs because the swallow + # at `await move_task` masked the CanError; sensor stayed False so + # `tripped` is False. + self.assertIn("never tripped", str(ctx.exception)) + self.assertEqual(h.fake_driver.motor_stop_calls, [Axis.Z]) + self.assertEqual( + h.fake_driver.configure_il_calls[-1], + (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose), + ) + + def test_motor_stop_failure_is_logged_not_raised(self): + """Even if motor_stop fails during cleanup, IL restore still runs and + the function still completes its happy-path return (sensor tripped).""" + h = _build_harness( + proximity_script=[False, True], + z_positions=[100.0, 95.5], + ) + h.fake_driver.motor_stop_should_raise = CanError("synthetic stop failure") + + z = asyncio.run(h.backend.find_z_with_proximity_sensor(max_descent=20.0)) + + self.assertAlmostEqual(z, 95.5, places=9) + # IL restore still ran despite motor_stop raising. + self.assertEqual( + h.fake_driver.configure_il_calls[-1], + (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose), + ) + + +class FindZAlreadyTrippedTests(unittest.TestCase): + def test_already_tripped_short_circuits_no_il_change(self): + """The pre-check sees the sensor already tripped: returns z0 without + configuring IL or spawning a descent task.""" + h = _build_harness( + proximity_script=[True], # tripped on the very first read + z_positions=[42.0], + ) + + z = asyncio.run(h.backend.find_z_with_proximity_sensor(max_descent=20.0)) + + self.assertAlmostEqual(z, 42.0, places=9) + # No IL configure calls (neither arm nor restore) — short-circuit hits + # before the StopForward arm. + self.assertEqual(h.fake_driver.configure_il_calls, []) + # No motor_stop, no descent move spawned. + self.assertEqual(h.fake_driver.motor_stop_calls, []) + self.assertEqual(h.move_calls, []) + # motor_enable still ran (it's the pre-flight before anything else). + self.assertEqual(h.fake_driver.motor_enable_calls, [(Axis.Z, True, True)]) + + def test_z_start_provided_runs_pre_descent_move_then_short_circuits(self): + """Same as above but with z_start: the pre-positioning move runs, then + the sensor reads tripped on the pre-check -> still no IL arm or + descent task.""" + h = _build_harness( + proximity_script=[True], + z_positions=[55.0], + # Pre-positioning move must complete on its own (it's awaited inline, + # not spawned as a cancellable task). long_sleep would deadlock. + move_behavior="completes_immediately", + ) + + z = asyncio.run(h.backend.find_z_with_proximity_sensor( + max_descent=20.0, z_start=80.0, + )) + + self.assertAlmostEqual(z, 55.0, places=9) + # The pre-position move ran (z_start path), but no descent move. + self.assertEqual(len(h.move_calls), 1) + self.assertAlmostEqual(h.move_calls[0]["cmd_pos"][Axis.Z], 80.0, places=9) + # Still no IL change, still no motor_stop. + self.assertEqual(h.fake_driver.configure_il_calls, []) + self.assertEqual(h.fake_driver.motor_stop_calls, []) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/paa/kx2/tests/arm_backend_read_axis_config_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_read_axis_config_tests.py new file mode 100644 index 00000000000..5658b23b529 --- /dev/null +++ b/pylabrobot/paa/kx2/tests/arm_backend_read_axis_config_tests.py @@ -0,0 +1,243 @@ +"""Unit tests for ``KX2ArmBackend._read_axis_config``. + +Covers the per-axis Elmo register read and its validation branches: + +* UF1 / UF2 -> motor conversion factor (zero on either is invalid). +* XM[1..2], VH[3], VL[3] -> bounded vs unlimited travel decision. +* CA[45] -> encoder socket index in 1..4. +* CA[40+ca45] -> encoder type code (1 / 2 / 24 are the only supported). + +The test populates a per-(cmd, idx) dict that a fake driver reads from. +No CAN, no asyncio loop concerns beyond a one-shot ``asyncio.run``. +""" +import asyncio +import unittest +from typing import Any, Dict, Tuple + +from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend +from pylabrobot.paa.kx2.config import Axis, AxisConfig +from pylabrobot.paa.kx2.driver import CanError, JointMoveDirection + + +def _baseline_register_map() -> Dict[Tuple[str, int], Any]: + """Plausible "all-good, bounded-travel" register snapshot for one axis. + + Values picked so: + * UF1/UF2 give a non-zero conv factor (UF1=10000, UF2=1 -> 10000). + * XM/VH/VL satisfy the bounded-travel branch (xm1==xm2==0). + * CA[45]==1 (valid socket), CA[41]==24 (absolute encoder). + * SP[2] != 100000 so max_vel takes the SP[2]/denom branch. + """ + reg: Dict[Tuple[str, int], Any] = {} + # _read_io_names: UI[5..10] (digital), UI[11..12] (analog), UI[13..16] (outputs) + for idx in range(5, 17): + reg[("UI", idx)] = 0 # all-unassigned -> "" entries + reg[("UI", 24)] = 12345 # serial, value irrelevant + # Conversion factor: UF1/UF2. + reg[("UF", 1)] = 10000.0 + reg[("UF", 2)] = 1.0 + # Bounded travel: xm1==xm2==0 hits the "(xm1==0 and xm2==0)" branch. + reg[("XM", 1)] = 0 + reg[("XM", 2)] = 0 + reg[("UF", 3)] = 180.0 # max_travel + reg[("UF", 4)] = -180.0 # min_travel + reg[("VH", 3)] = 1000 + reg[("VL", 3)] = -1000 + # Encoder socket + type: socket 1, type 24 (absolute). + reg[("CA", 45)] = 1 + reg[("CA", 41)] = 24 + reg[("CA", 46)] = 1 # equals ca45 -> num3 = 1.0 (skips FF[3] read) + # Velocity / accel. + reg[("SP", 2)] = 50000 + reg[("VH", 2)] = 200000 + reg[("SD", 0)] = 100000 + reg[("FF", 3)] = 1.0 # only used if ca45 != ca46 + return reg + + +class _FakeDriver: + """Minimal stand-in for KX2Driver; only resolves query_int / query_float + against a (cmd, idx) -> value dict. Raises KeyError on unmapped reads so + test misconfig surfaces loudly.""" + + def __init__(self, reg: Dict[Tuple[str, int], Any]): + self.reg = reg + self.calls: list = [] + + async def query_int(self, node_id: int, cmd: str, idx: int) -> int: + self.calls.append(("int", node_id, cmd, idx)) + return int(self.reg[(cmd, idx)]) + + async def query_float(self, node_id: int, cmd: str, idx: int) -> float: + self.calls.append(("float", node_id, cmd, idx)) + return float(self.reg[(cmd, idx)]) + + +def _build_backend(reg: Dict[Tuple[str, int], Any]) -> KX2ArmBackend: + """Bypass __init__ — _read_axis_config only touches self.driver.""" + backend = KX2ArmBackend.__new__(KX2ArmBackend) + backend.driver = _FakeDriver(reg) # type: ignore[assignment] + return backend + + +class ReadAxisConfigHappyPathTests(unittest.TestCase): + def test_returns_axis_config_with_expected_fields(self): + """Full register snapshot -> AxisConfig with the values derived from + the snapshot.""" + reg = _baseline_register_map() + backend = _build_backend(reg) + cfg = asyncio.run(backend._read_axis_config(Axis.SHOULDER)) + + self.assertIsInstance(cfg, AxisConfig) + self.assertEqual(cfg.motor_conversion_factor, 10000.0) + self.assertEqual(cfg.max_travel, 180.0) + self.assertEqual(cfg.min_travel, -180.0) + self.assertFalse(cfg.unlimited_travel) + self.assertTrue(cfg.absolute_encoder) + self.assertEqual(cfg.joint_move_direction, JointMoveDirection.Normal) + # max_vel = SP[2] / (conv * num3) = 50000 / (10000 * 1.0) = 5.0 + self.assertAlmostEqual(cfg.max_vel, 5.0, places=9) + # max_accel = SD[0] / 1.01 / (conv * num3) = 100000 / 1.01 / 10000 + self.assertAlmostEqual(cfg.max_accel, 100000 / 1.01 / 10000.0, places=9) + # Default I/O dicts: all-empty channel labels (codes were 0). + self.assertEqual(cfg.digital_inputs, {1: "", 2: "", 3: "", 4: "", 5: "", 6: ""}) + self.assertEqual(cfg.analog_inputs, {1: "", 2: ""}) + self.assertEqual(cfg.outputs, {1: "", 2: "", 3: "", 4: ""}) + + def test_unlimited_travel_branch_sets_shortest_way_for_motion_axis(self): + """xm1 > vl3 and xm2 < vh3 -> unlimited travel; for motion axes, the + joint_move_direction flips to ShortestWay.""" + reg = _baseline_register_map() + # xm range strictly inside vl/vh range. + reg[("XM", 1)] = -500 + reg[("XM", 2)] = 500 + reg[("VH", 3)] = 1000 + reg[("VL", 3)] = -1000 + backend = _build_backend(reg) + cfg = asyncio.run(backend._read_axis_config(Axis.SHOULDER)) + self.assertTrue(cfg.unlimited_travel) + self.assertEqual(cfg.joint_move_direction, JointMoveDirection.ShortestWay) + + def test_unlimited_travel_keeps_normal_direction_on_non_motion_axis(self): + """Same unlimited-travel path on a non-motion axis (e.g. RAIL) keeps + the joint_move_direction at Normal.""" + reg = _baseline_register_map() + reg[("XM", 1)] = -500 + reg[("XM", 2)] = 500 + backend = _build_backend(reg) + cfg = asyncio.run(backend._read_axis_config(Axis.RAIL)) + self.assertTrue(cfg.unlimited_travel) + self.assertEqual(cfg.joint_move_direction, JointMoveDirection.Normal) + + def test_sp2_is_100000_uses_vh2_path_for_max_vel(self): + """SP[2] == 100000 sentinel -> max_vel comes from VH[2]/1.01/denom.""" + reg = _baseline_register_map() + reg[("SP", 2)] = 100000 + reg[("VH", 2)] = 200000 + backend = _build_backend(reg) + cfg = asyncio.run(backend._read_axis_config(Axis.SHOULDER)) + # 200000 / 1.01 / (10000 * 1.0) + self.assertAlmostEqual(cfg.max_vel, 200000 / 1.01 / 10000.0, places=9) + + def test_ca45_neq_ca46_pulls_ff3_into_denom(self): + """When the position encoder (CA[46]) differs from the commutation + encoder (CA[45]), num3 is read from FF[3] instead of defaulting to 1.""" + reg = _baseline_register_map() + reg[("CA", 46)] = 2 # different from CA[45]=1 + reg[("FF", 3)] = 4.0 + backend = _build_backend(reg) + cfg = asyncio.run(backend._read_axis_config(Axis.SHOULDER)) + # denom = 10000 * 4.0; max_vel = 50000 / 40000 = 1.25 + self.assertAlmostEqual(cfg.max_vel, 1.25, places=9) + + +class ReadAxisConfigValidationTests(unittest.TestCase): + """One test per validation branch: a future contributor flipping a guard + by mistake should fail at least one of these.""" + + def test_uf1_zero_raises(self): + reg = _baseline_register_map() + reg[("UF", 1)] = 0.0 + backend = _build_backend(reg) + with self.assertRaises(CanError) as ctx: + asyncio.run(backend._read_axis_config(Axis.SHOULDER)) + self.assertIn("Invalid motor conversion factor", str(ctx.exception)) + self.assertIn(f"axis {int(Axis.SHOULDER)}", str(ctx.exception)) + self.assertIn("UF[1]=0.0", str(ctx.exception)) + + def test_uf2_zero_raises(self): + reg = _baseline_register_map() + reg[("UF", 2)] = 0.0 + backend = _build_backend(reg) + with self.assertRaises(CanError) as ctx: + asyncio.run(backend._read_axis_config(Axis.SHOULDER)) + self.assertIn("Invalid motor conversion factor", str(ctx.exception)) + self.assertIn("UF[2]=0.0", str(ctx.exception)) + + def test_invalid_travel_limits_raises(self): + """Neither bounded-travel nor unlimited-travel branch matches: e.g. + xm1 > vl3 but xm2 >= vh3 -> hits the else.""" + reg = _baseline_register_map() + reg[("XM", 1)] = -500 # > VL[3] = -1000 + reg[("XM", 2)] = 2000 # >= VH[3] = 1000 -> not unlimited, not bounded + backend = _build_backend(reg) + with self.assertRaises(CanError) as ctx: + asyncio.run(backend._read_axis_config(Axis.SHOULDER)) + msg = str(ctx.exception) + self.assertIn("Invalid travel limits or modulo settings", msg) + self.assertIn("VH[3]=1000", msg) + self.assertIn("VL[3]=-1000", msg) + self.assertIn("XM[1]=-500", msg) + self.assertIn("XM[2]=2000", msg) + + def test_ca45_zero_raises(self): + """CA[45] == 0 fails the (0 < ca45 <= 4) guard.""" + reg = _baseline_register_map() + reg[("CA", 45)] = 0 + backend = _build_backend(reg) + with self.assertRaises(CanError) as ctx: + asyncio.run(backend._read_axis_config(Axis.SHOULDER)) + self.assertIn("Invalid encoder socket", str(ctx.exception)) + self.assertIn("CA[45]=0", str(ctx.exception)) + + def test_ca45_too_large_raises(self): + """CA[45] > 4 also fails the guard (only sockets 1..4 wired).""" + reg = _baseline_register_map() + reg[("CA", 45)] = 5 + backend = _build_backend(reg) + with self.assertRaises(CanError) as ctx: + asyncio.run(backend._read_axis_config(Axis.SHOULDER)) + self.assertIn("Invalid encoder socket", str(ctx.exception)) + self.assertIn("CA[45]=5", str(ctx.exception)) + + def test_unsupported_encoder_type_raises(self): + """enc_type not in {1, 2, 24} -> raises with the offending CA register.""" + reg = _baseline_register_map() + reg[("CA", 45)] = 1 + reg[("CA", 41)] = 7 # neither incremental (1/2) nor absolute (24) + backend = _build_backend(reg) + with self.assertRaises(CanError) as ctx: + asyncio.run(backend._read_axis_config(Axis.SHOULDER)) + msg = str(ctx.exception) + self.assertIn("Unsupported encoder type", msg) + self.assertIn("CA[41]=7", msg) + + def test_incremental_encoder_type_1_sets_absolute_false(self): + reg = _baseline_register_map() + reg[("CA", 45)] = 1 + reg[("CA", 41)] = 1 + backend = _build_backend(reg) + cfg = asyncio.run(backend._read_axis_config(Axis.SHOULDER)) + self.assertFalse(cfg.absolute_encoder) + + def test_incremental_encoder_type_2_sets_absolute_false(self): + reg = _baseline_register_map() + reg[("CA", 45)] = 1 + reg[("CA", 41)] = 2 + backend = _build_backend(reg) + cfg = asyncio.run(backend._read_axis_config(Axis.SHOULDER)) + self.assertFalse(cfg.absolute_encoder) + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/paa/kx2/tests/driver_setpoint_trigger_tests.py b/pylabrobot/paa/kx2/tests/driver_setpoint_trigger_tests.py new file mode 100644 index 00000000000..215f6dc91ef --- /dev/null +++ b/pylabrobot/paa/kx2/tests/driver_setpoint_trigger_tests.py @@ -0,0 +1,194 @@ +"""Unit tests for the KX2 driver's CiA 402 PPM new-setpoint handshake. + +Covers ``KX2Driver._trigger_new_setpoint``: the four-step CW/SW dance +(drop bit 4, wait bit 12 low, raise bit 4, wait bit 12 high) and its +retry-on-missed-edge behavior. The drive's SW state is faked by setting +``self._statusword[nid]`` directly and signalling +``self._statusword_event[nid]`` — same shape as the real TPDO3 callback, +without any CAN traffic. +""" +import asyncio +import unittest +from typing import List, Tuple + +from pylabrobot.paa.kx2.driver import CanError, KX2Driver + + +SW_BIT_12 = 1 << 12 + + +class _SwScript: + """Drives the fake SW state machine. + + Each step is one of: + ("set_low",) -> clear bit 12 immediately + ("set_high",) -> raise bit 12 immediately + ("hang",) -> leave SW unchanged so _wait_setpoint_ack times out + + The script is consumed in order on each ``_control_word_set`` call. + """ + + def __init__(self, steps: List[Tuple[str, ...]]): + self.steps = list(steps) + self.calls: List[int] = [] # values passed to _control_word_set + + def consume(self) -> Tuple[str, ...]: + if not self.steps: + raise AssertionError("script exhausted but _control_word_set was called again") + return self.steps.pop(0) + + +def _build_driver(nid: int, script: _SwScript) -> KX2Driver: + drv = KX2Driver() + loop = asyncio.get_event_loop() + drv._loop = loop + drv._statusword = {nid: 0} + drv._statusword_event = {nid: asyncio.Event()} + + async def _fake_cw_set(node_id: int, value: int, sync: bool = True) -> None: + script.calls.append(value) + step = script.consume() + tag = step[0] + if tag == "set_low": + drv._statusword[node_id] = drv._statusword.get(node_id, 0) & ~SW_BIT_12 + drv._statusword_event[node_id].set() + elif tag == "set_high": + drv._statusword[node_id] = drv._statusword.get(node_id, 0) | SW_BIT_12 + drv._statusword_event[node_id].set() + elif tag == "hang": + # Leave SW unchanged. _wait_sw_bit will exhaust its 50 ms timeout + # waiting on the event + falling back to SDO; we also stub the SDO + # path below to return the cached SW (= still wrong) so it always + # times out. + pass + else: + raise AssertionError(f"unknown step {tag!r}") + + async def _fake_sdo_upload(node_id: int, idx: int, sub: int) -> bytes: + return drv._statusword.get(node_id, 0).to_bytes(2, "little") + b"\x00\x00" + + drv._control_word_set = _fake_cw_set # type: ignore[assignment] + drv._can_sdo_upload = _fake_sdo_upload # type: ignore[assignment] + return drv + + +class TriggerNewSetpointHappyPathTests(unittest.TestCase): + def test_first_attempt_succeeds(self): + """SW bit 12 clears, then rises promptly -> returns, no raise. + `_control_word_set` is called exactly twice (cw_low then cw_high).""" + nid = 1 + script = _SwScript([("set_low",), ("set_high",)]) + + async def _go(): + drv = _build_driver(nid, script) + await drv._trigger_new_setpoint(nid, cw_low=47, cw_high=63) + + asyncio.new_event_loop().run_until_complete(_go()) + self.assertEqual(script.calls, [47, 63]) + self.assertEqual(script.steps, []) # script fully consumed + + +class TriggerNewSetpointRetryThenSuccessTests(unittest.TestCase): + def test_second_attempt_succeeds(self): + """First attempt: bit 12 clears but never rises (timeout). Second + attempt: clears + rises -> returns. CW is set 4 times total.""" + nid = 1 + # Attempt 1: cw_low clears bit 12, cw_high hangs (bit 12 doesn't rise) + # Attempt 2: cw_low clears, cw_high raises. + script = _SwScript([ + ("set_low",), ("hang",), + ("set_low",), ("set_high",), + ]) + + async def _go(): + drv = _build_driver(nid, script) + # Use a shorter wait timeout indirectly via the script: ("hang",) just + # means "leave SW unchanged"; the wait will spin its 50 ms timeout. + await drv._trigger_new_setpoint(nid, cw_low=47, cw_high=63) + + asyncio.new_event_loop().run_until_complete(_go()) + self.assertEqual(script.calls, [47, 63, 47, 63]) + self.assertEqual(script.steps, []) + + +class TriggerNewSetpointAllAttemptsFailTests(unittest.TestCase): + def test_raises_after_max_attempts(self): + """SW bit 12 never rises -> raises CanError tagged with the axis nid. + With max_attempts=2 the handshake runs twice (4 CW writes) then gives up.""" + nid = 3 + script = _SwScript([ + ("set_low",), ("hang",), + ("set_low",), ("hang",), + ]) + + async def _go(): + drv = _build_driver(nid, script) + await drv._trigger_new_setpoint(nid, cw_low=47, cw_high=63, max_attempts=2) + + with self.assertRaises(CanError) as ctx: + asyncio.new_event_loop().run_until_complete(_go()) + msg = str(ctx.exception) + self.assertIn(f"Axis {nid}", msg) + self.assertIn("did not accept new PPM setpoint", msg) + self.assertIn("after 2 attempts", msg) + self.assertEqual(script.calls, [47, 63, 47, 63]) + + +class TriggerNewSetpointBit12StuckHighTests(unittest.TestCase): + def test_recovers_when_bit12_does_not_clear_on_first_cw_low(self): + """Pre-existing bit 12 high. Attempt 1 cw_low fails to clear it -> the + inner ``cleared`` check is False, the attempt restarts (continues the + loop) without writing cw_high. Attempt 2's cw_low clears, cw_high raises. + + State machine recovery proof: cw_high is NOT written on attempt 1 + (script only sees [cw_low, cw_low, cw_high]).""" + nid = 2 + # Attempt 1: cw_low call -> "hang" (bit 12 stays high; cleared==False). + # Attempt 2: cw_low call -> set_low (clears), cw_high call -> set_high. + script = _SwScript([ + ("hang",), + ("set_low",), ("set_high",), + ]) + + async def _go(): + drv = _build_driver(nid, script) + drv._statusword[nid] = SW_BIT_12 # bit 12 high before any handshake + await drv._trigger_new_setpoint(nid, cw_low=47, cw_high=63) + + asyncio.new_event_loop().run_until_complete(_go()) + # Attempt 1 wrote cw_low only; attempt 2 wrote cw_low + cw_high. + self.assertEqual(script.calls, [47, 47, 63]) + self.assertEqual(script.steps, []) + + +class TriggerNewSetpointMaxAttemptsOneTests(unittest.TestCase): + def test_single_attempt_failure_raises_immediately(self): + """With ``max_attempts=1`` the handshake gets exactly one shot. If + SW bit 12 doesn't rise, raises straight away (no second attempt).""" + nid = 4 + script = _SwScript([("set_low",), ("hang",)]) + + async def _go(): + drv = _build_driver(nid, script) + await drv._trigger_new_setpoint(nid, cw_low=47, cw_high=63, max_attempts=1) + + with self.assertRaises(CanError) as ctx: + asyncio.new_event_loop().run_until_complete(_go()) + self.assertIn("after 1 attempts", str(ctx.exception)) + self.assertEqual(script.calls, [47, 63]) + + def test_single_attempt_success_returns(self): + """``max_attempts=1`` happy path: still works on a clean handshake.""" + nid = 4 + script = _SwScript([("set_low",), ("set_high",)]) + + async def _go(): + drv = _build_driver(nid, script) + await drv._trigger_new_setpoint(nid, cw_low=47, cw_high=63, max_attempts=1) + + asyncio.new_event_loop().run_until_complete(_go()) + self.assertEqual(script.calls, [47, 63]) + + +if __name__ == "__main__": + unittest.main() From ebd74ac10aead0c1e2bb4b479c8a0c0f217c9dcc Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sat, 2 May 2026 22:32:10 -0700 Subject: [PATCH 76/93] KX2: Cartesian-linear path via IPM/PVT streaming, opt-in path='linear' MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/arm_backend.py | 183 ++++++++++- pylabrobot/paa/kx2/driver.py | 267 +++++++++++++--- pylabrobot/paa/kx2/kinematics.py | 203 +++++++++++- .../arm_backend_find_z_proximity_tests.py | 14 +- .../tests/arm_backend_linear_path_tests.py | 291 ++++++++++++++++++ .../kx2/tests/driver_emcy_lifecycle_tests.py | 10 +- .../paa/kx2/tests/driver_emcy_polish_tests.py | 14 +- pylabrobot/paa/kx2/tests/driver_emcy_tests.py | 36 ++- pylabrobot/paa/kx2/tests/kinematics_tests.py | 155 ++++++++++ 9 files changed, 1082 insertions(+), 91 deletions(-) create mode 100644 pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index b2653f299a6..b83fbcc787f 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -5,7 +5,7 @@ import warnings from contextlib import asynccontextmanager from enum import IntEnum -from typing import Dict, List, Optional, Union +from typing import Dict, List, Literal, Optional, Union from pylabrobot.capabilities.arms.backend import ( CanFreedrive, @@ -130,8 +130,7 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None): "safety-interlock loop or motor-power switch may also be open.)" ) - for axis in MOTION_AXES: - await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) + await self.driver.motors_ensure_enabled([int(a) for a in MOTION_AXES]) await self.servo_gripper_initialize() except BaseException: try: @@ -675,8 +674,8 @@ async def find_z_with_proximity_sensor( async with self._motion_guard(): # Pre-flight: force the drive back to Op Enabled. A prior failed # search could have left it in Fault/Quick Stop where new moves - # silently fail (Z barely moves). Idempotent if drive is healthy. - await self.driver.motor_enable(node_id=Axis.Z, state=True, use_ds402=True) + # silently fail (Z barely moves). + await self.driver.motors_ensure_enabled([int(Axis.Z)]) if z_start is not None: await self._motors_move_joint_locked({Axis.Z: z_start}, params=move_params) z0 = await self.motor_get_current_position(Axis.Z) @@ -763,7 +762,7 @@ async def motors_move_absolute_execute(self, plan: MotorsMovePlan) -> None: async def _motors_move_absolute_execute_locked(self, plan: MotorsMovePlan) -> None: """Caller MUST hold _motion_guard.""" - await self.driver.pvt_select_mode(False) + await self.driver.ipm_select_mode(False) if logger.isEnabledFor(logging.DEBUG): logger.debug( @@ -798,7 +797,7 @@ async def _motors_move_absolute_execute_locked(self, plan: MotorsMovePlan) -> No ) node_ids = [move.node_id for move in plan.moves] - await self.driver.motors_move_start(node_ids) + await self.driver.ppm_begin_motion(node_ids) await self.driver.wait_for_moves_done(node_ids, timeout=plan.move_time + 2) async def _cart_to_joints(self, pose: GripperLocation) -> Dict[Axis, float]: @@ -808,6 +807,137 @@ async def _cart_to_joints(self, pose: GripperLocation) -> Dict[Axis, float]: ik_joints = kinematics.ik(pose, self._cfg, self._gripper_params) return kinematics.snap_to_current(ik_joints, current) + async def _run_linear_path( + self, + target_pose: GripperLocation, + params: "KX2ArmBackend.CartesianMoveParams", + ) -> None: + """Stream a Cartesian-linear trajectory through the drive's IPM buffer. + + Caller MUST hold ``_motion_guard``. Picks up the current pose from the + drives, samples a straight-line tool-tip path to ``target_pose`` at the + fixed IPM cadence (``_LINEAR_PATH_DT_MS``), and feeds (P, V) points to + each axis 8 frames ahead of the drive's read pointer. + + On cancel/exception the drive is brought back to PPM through a + ``finally`` block (``ipm_stop`` then ``ipm_select_mode(False)``), so a + halt mid-stream leaves the arm in a state that a subsequent joint move + can resume from. + + NOTE: cancel/halt drops ip-enable and lets the drive consume already- + buffered points before stopping. Coast can run up to + ``_LINEAR_PATH_PRELOAD * dt_ms`` (~64 ms at 8 ms / 8 frames) past the + cancel. If true zero-coast halt is needed, fall through to the + existing ``halt()`` (MO=0 across all motion axes), which is independent + of the linear path. + """ + if params.max_gripper_speed is None or params.max_gripper_acceleration is None: + raise ValueError( + "CartesianMoveParams(path='linear') requires max_gripper_speed and " + "max_gripper_acceleration: the Cartesian profile is built from them " + "directly (no firmware fallback for streamed motion)." + ) + + current_joints = { + Axis(k): v for k, v in (await self.request_joint_position()).items() + } + start_pose = kinematics.fk(current_joints, self._cfg, self._gripper_params) + samples = kinematics.sample_linear_path( + cfg=self._cfg, + gripper_params=self._gripper_params, + start_pose=start_pose, + end_pose=target_pose, + vel_mm_per_s=params.max_gripper_speed, + accel_mm_per_s2=params.max_gripper_acceleration, + dt_s=self._LINEAR_PATH_DT_MS / 1000.0, + current_joints=current_joints, + ) + if len(samples) < 2: + # Zero-length path: same pose. Defensively flip mode back to PPM in + # case a prior linear move's cleanup failed and left _ipm_mode=True. + await self.driver.ipm_select_mode(False) + return + + active_axes = [int(ax) for ax in (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST)] + dt_s = self._LINEAR_PATH_DT_MS / 1000.0 + preload = min(self._LINEAR_PATH_PRELOAD, len(samples)) + + await self.driver.ipm_select_mode(True) + try: + await self.driver.ipm_set_time_interval(self._LINEAR_PATH_DT_MS) + + # Preload PRELOAD frames per axis. The drive starts consuming as soon + # as begin_motion fires; preload lets the producer fall behind by up + # to (PRELOAD-1) * dt without underflowing the queue. + for i in range(preload): + for nid in active_axes: + ax = Axis(nid) + self.driver.ipm_send_pvt_point( + nid, + samples[i].encoder_position[ax], + samples[i].encoder_velocity[ax], + ) + # Surface any preload-induced fault (queue_full from a malformed + # first point) before we kick off motion. + self.driver.ipm_check_queue_fault(active_axes) + + # Capture the pacing reference *before* begin_motion. The drive starts + # consuming the moment SYNC arrives inside begin_motion; capturing + # after means we underestimate elapsed drive-time and pace too late, + # eating into the underflow margin. Capturing before is conservative + # (we send slightly early), wasting a few buffer slots — strictly + # safer. + start = time.monotonic() + await self.driver.ipm_begin_motion(active_axes) + + # Pace the rest: send sample i so it lands at t = (i - (PRELOAD-1)) * dt + # after the captured start. + for i in range(preload, len(samples)): + target_t = (i - (preload - 1)) * dt_s + while time.monotonic() - start < target_t: + await asyncio.sleep(0) + for nid in active_axes: + ax = Axis(nid) + self.driver.ipm_send_pvt_point( + nid, + samples[i].encoder_position[ax], + samples[i].encoder_velocity[ax], + ) + self.driver.ipm_check_queue_fault(active_axes) + + # Wait for SW bit-10 (target reached) on every axis. Bit-10 goes high + # once the IP buffer drains and the trajectory generator settles on + # the last commanded position. MS-based polling (`wait_for_moves_done`) + # would race the buffer drain — bit-10 is the authoritative IPM + # done-signal. + total_t = (len(samples) - 1) * dt_s + timeout = total_t + 2.0 + await self.driver.ipm_wait_motion_complete(active_axes, timeout_s=timeout) + # One more fault check after drain — covers an underflow in the very + # last frames that bit-10 wouldn't reflect. + self.driver.ipm_check_queue_fault(active_axes) + finally: + try: + await self.driver.ipm_stop(active_axes) + except Exception: + # Log with EMCY state for post-mortem: if the drive was already + # mid-fault when we tried to stop, the EMCY counters tell us why. + emcy_snap = { + ax: vars(self.driver._ipm_emcy[ax]) for ax in active_axes + if ax in self.driver._ipm_emcy + } + logger.exception( + "ipm_stop cleanup failed; drive may still be in IPM with ip-enable " + "high. EMCY state per axis: %s", emcy_snap, + ) + try: + await self.driver.ipm_select_mode(False) + except Exception: + logger.exception( + "ipm_select_mode(False) cleanup failed; next motion call will " + "fresh-arm IPM via the re-arm path" + ) + # -- capability interface (OrientableGripperArmBackend + HasJoints + CanFreedrive) -- async def halt(self, backend_params: Optional[BackendParams] = None) -> None: @@ -868,9 +998,33 @@ class CartesianMoveParams(BackendParams): mm/s^2). ``None`` means run joints at firmware max with no Cartesian cap. Joint speeds are scaled uniformly so the worst-case Cartesian gripper speed along the trajectory stays at or under - ``max_gripper_speed``.""" + ``max_gripper_speed``. + + ``path`` selects the trajectory shape: + - ``"joint"`` (default): every axis ramps in parallel through its own + trapezoid. Tool tip traces a curvy path; speed is firmware-bounded + with optional gripper-speed cap. + - ``"linear"``: tool tip traces a straight line from current to + target pose, sampled and streamed via the drive's interpolation + buffer (PVT). Requires ``max_gripper_speed`` and + ``max_gripper_acceleration`` since they directly drive the + Cartesian profile (no cap == no speed to stream). + """ max_gripper_speed: Optional[float] = None max_gripper_acceleration: Optional[float] = None + path: Literal["joint", "linear"] = "joint" + + # PVT streaming cadence. 8 ms / 125 Hz: small enough that the 16-deep drive + # buffer holds ~125 ms of motion, long enough that producer scheduling + # jitter doesn't underflow the queue. Buffered up to PRELOAD frames ahead + # of the drive's read pointer. + # + # If you bump these, check: (a) DT_MS fits UINT8 — 0x60C2:01 is UNSIGNED8; + # `ipm_set_time_interval` validates this at the wire level. (b) PRELOAD < + # drive IP buffer depth (16, set by the 24772:2=16 write at setup) — + # otherwise the (PRELOAD+1)th frame queue_fulls immediately. + _LINEAR_PATH_DT_MS: int = 8 + _LINEAR_PATH_PRELOAD: int = 8 async def move_to_location( self, @@ -881,6 +1035,10 @@ async def move_to_location( if not isinstance(backend_params, KX2ArmBackend.CartesianMoveParams): backend_params = KX2ArmBackend.CartesianMoveParams() pose = GripperLocation(location=location, rotation=Rotation(z=direction)) + if backend_params.path == "linear": + async with self._motion_guard(): + await self._run_linear_path(pose, backend_params) + return async with self._motion_guard(): joint_pos = await self._cart_to_joints(pose) joint_params = KX2ArmBackend.JointMoveParams( @@ -1034,8 +1192,7 @@ async def start_freedrive_mode( async def stop_freedrive_mode(self, backend_params: Optional[BackendParams] = None) -> None: axes: List[int] = self._freedrive_axes or list(MOTION_AXES) async with self._motion_guard(): - for axis in axes: - await self.driver.motor_enable(node_id=axis, state=True, use_ds402=True) + await self.driver.motors_ensure_enabled([int(a) for a in axes]) self._freedrive_axes = [] async def very_dangerously_yeet( @@ -1167,10 +1324,10 @@ async def very_dangerously_yeet( await _yeet_arm_plan(driver, sh_plan) await _yeet_arm_plan(driver, wr_plan) - await driver.motors_move_start([Axis.SHOULDER]) + await driver.ppm_begin_motion([Axis.SHOULDER]) t0 = time.monotonic() await asyncio.sleep(max(0.0, wrist_trigger_t - (time.monotonic() - t0))) - await driver.motors_move_start([Axis.WRIST]) + await driver.ppm_begin_motion([Axis.WRIST]) await asyncio.sleep(max(0.0, release_t - (time.monotonic() - t0))) await self._motors_move_absolute_execute_locked(gripper_plan) @@ -1297,7 +1454,7 @@ async def _yeet_build_axis_move( async def _yeet_arm_plan(driver: KX2Driver, plan: MotorsMovePlan) -> None: """Pre-load a plan onto the drives without triggering it. Splits SDO setup latency from the move start so the timer can be accurate.""" - await driver.pvt_select_mode(False) + await driver.ipm_select_mode(False) for move in plan.moves: nid = move.node_id await driver.motor_set_move_direction(nid, move.direction) diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index f3dc2c39939..e1f08e7f4e6 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -210,14 +210,22 @@ class EmcyFrame: @dataclass -class _PvtEmcyState: - """Per-node PVT-mode queue state, mirrored from `sPVT_EMCY` in clscanmotor.cs:6943-6964.""" +class _IpmEmcyState: + """Per-node IPM queue state, mirrored from `sPVT_EMCY` in clscanmotor.cs:6943-6964. + + ``queue_low`` is a proactive warning (drive's IP buffer is approaching + empty); ``underflow`` is post-fact (drive ran the buffer dry and + short-stopped the trajectory). The streaming runtime treats underflow as + a fatal error; queue_low is informational unless we move to event-driven + pacing. + """ queue_low: bool = False queue_low_write_pointer: int = 0 queue_low_read_pointer: int = 0 queue_full: bool = False queue_full_failed_write_pointer: int = 0 + underflow: bool = False bad_head_pointer: bool = False bad_mode_init_data: bool = False motion_terminated: bool = False @@ -285,11 +293,11 @@ class _PvtEmcyState: def _decode_emcy( - frame: EmcyFrame, state: _PvtEmcyState + frame: EmcyFrame, state: _IpmEmcyState ) -> Tuple[str, bool, bool]: """Decode an EMCY frame into (description, disable_motors, suppress_callback). - Mutates ``state`` for PVT queue events. Mirrors clscanmotor.cs:1070-1267 + Mutates ``state`` for IPM queue events. Mirrors clscanmotor.cs:1070-1267 one-for-one. ``suppress_callback`` corresponds to the C# `flag2` and is only set for the elmo 0x8A interpolation underflow under errCode 0xFF02 — that case updates internal state but does not raise the user-visible event. @@ -348,8 +356,10 @@ def _decode_emcy( state.bad_head_pointer = True return ("Bad Head Pointer", False, False) if elmo == 0x8A: - # State update only; user callback suppressed (C# flag2=true). - state.queue_low = True + # State update only; user callback suppressed (C# flag2=true). Sets + # `underflow` (post-fact: drive already ran out of points) — distinct + # from `queue_low` (proactive: more data needed soon). + state.underflow = True return ("Position Interpolation buffer underflow", False, True) if elmo == 0xA6: state.out_of_modulo = True @@ -442,7 +452,7 @@ def __init__( # garbage Lock from a missed-race insert is immediately discarded. self._bi_locks: Dict[Tuple[int, str, int], asyncio.Lock] = {} - self._pvt_mode: bool = False + self._ipm_mode: bool = False # EMCY (CANopen Emergency, COB-ID 0x80 + node_id) state. Subscribed in # setup(); fires on the canopen listener thread, marshalled into the @@ -451,7 +461,7 @@ def __init__( self.emcy_move_error: str = "" self.emcy_move_error_node_id: Optional[int] = None self.last_emcy: Optional[EmcyFrame] = None - self._pvt_emcy: Dict[int, _PvtEmcyState] = {} + self._ipm_emcy: Dict[int, _IpmEmcyState] = {} self._emcy_callbacks: List[ Callable[[int, EmcyFrame, str, bool], None] ] = [] @@ -494,7 +504,7 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: # the C# event handler at KX2RobotControl.cs:15384-15425 / # clscanmotor.cs:1057-1284. for nid in self.node_id_list: - self._pvt_emcy[nid] = _PvtEmcyState() + self._ipm_emcy[nid] = _IpmEmcyState() network.subscribe(_EMCY_COB_BASE + nid, self._make_emcy_callback(nid)) # Reset all nodes, then start scanner so bootup messages populate it, @@ -531,7 +541,7 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: # when the new event-trigger arms. 1 ms inhibit (10 * 100 us) caps # bus traffic — SW changes happen at the ~1-2 ms servo cycle, so the # inhibit doesn't lose edges in practice and keeps the bus quiet - # during PVT streaming if anyone resurrects the IPM runtime. + # during IPM streaming if anyone resurrects the IPM runtime. for nid in self.motion_node_ids: self._statusword_event[nid] = asyncio.Event() network.subscribe(_TPDO3_COB_BASE + nid, self._make_tpdo3_callback(nid)) @@ -548,7 +558,7 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: TPDO.TPDO4, node_id, [TPDOMappedObject.DigitalInputs], TPDOTrigger.DigitalInputEvent ) - # Elmo vendor objects: interpolation config for PVT mode. + # Elmo vendor objects: interpolation config for IPM. for nid in self.motion_node_ids: await self.can_sdo_download_elmo_object(nid, 24768, 0, -1, _ElmoObjectDataType.INTEGER16) await self.can_sdo_download_elmo_object(nid, 24772, 2, 16, _ElmoObjectDataType.UNSIGNED32) @@ -569,8 +579,8 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: PDOTransmissionType.EventDrivenDev, ) - self._pvt_mode = True - await self.pvt_select_mode(False) + self._ipm_mode = True + await self.ipm_select_mode(False) async def stop(self) -> None: if self._network is not None: @@ -581,7 +591,7 @@ async def stop(self) -> None: self._network.disconnect() self._network = None self._nodes = {} - self._pvt_emcy = {} + self._ipm_emcy = {} # Clear callbacks too: _on_setup re-registers on each retry, so leaving # them would compound N copies of the same handler across attempts. self._emcy_callbacks = [] @@ -737,14 +747,14 @@ def add_emcy_callback( def clear_emcy_state(self, node_id: Optional[int] = None) -> None: """Clear sticky EMCY error fields after a recovery / re-enable. - If ``node_id`` is given, also resets the per-node PVT queue state for + If ``node_id`` is given, also resets the per-node IPM queue state for that node. Mirrors clscanmotor.cs:668-669, 3454, 4481. """ self.emcy_move_error_received = False self.emcy_move_error = "" self.emcy_move_error_node_id = None - if node_id is not None and node_id in self._pvt_emcy: - self._pvt_emcy[node_id] = _PvtEmcyState() + if node_id is not None and node_id in self._ipm_emcy: + self._ipm_emcy[node_id] = _IpmEmcyState() def _make_emcy_callback(self, node_id: int): """Return a `canopen.Network.subscribe` callback bound to a specific node.""" @@ -765,9 +775,9 @@ def _dispatch_emcy(self, node_id: int, data: bytes) -> None: frame = EmcyFrame(err_code, err_reg, elmo_err, d1, d2) self.last_emcy = frame - state = self._pvt_emcy.setdefault(node_id, _PvtEmcyState()) + state = self._ipm_emcy.setdefault(node_id, _IpmEmcyState()) desc, disable_motors, suppress = _decode_emcy(frame, state) - # Tier the level so PVT housekeeping (queue-low/underflow) doesn't drown + # Tier the level so IPM housekeeping (queue-low/underflow) doesn't drown # ops logs while real faults stay loud. Unknown codes warn so we notice. if disable_motors: level = logging.ERROR @@ -1158,7 +1168,7 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N # Clear sticky EMCY state from any prior fault on this drive so the # post-enable motion path doesn't re-surface stale errors. Mirrors # clscanmotor.cs:4481 ("EmcyMoveErrorReceived = false" before re-enable) - # plus the per-axis PVT-queue clear at clscanmotor.cs:4050-4051. + # plus the per-axis IPM-queue clear at clscanmotor.cs:4050-4051. self.clear_emcy_state(node_id=node_id) want = 1 if state else 0 @@ -1201,6 +1211,35 @@ async def motor_enable(self, node_id: int, state: bool, *, use_ds402: bool) -> N verb = "enable" if state else "disable" raise CanError(f"Motor failed to {verb} (node_id = {node_id}) after {max_attempts} attempts") + async def motors_ensure_enabled( + self, node_ids: List[int], *, use_ds402: bool = True, + ) -> None: + """Enable every drive in ``node_ids`` that isn't already enabled. + + One ``motor_is_enabled`` SDO read per axis (~5 ms); only drives reading + MO=0 pay the full DS402 enable cycle. Per-axis work runs concurrently — + each node ID has its own SDO channel so they don't serialize on the bus. + + The cheap path (drive already enabled) is the common case after the + first move; the slow path covers post-halt, post-fault, post-freedrive + where the drive deliberately landed in Switch On Disabled. Used by + every motion-trigger site (PPM, IPM begin) and lifecycle transitions + (setup, stop_freedrive_mode, find_z preflight) so they all share one + recovery contract. + """ + # Sequential, not gather: motor_enable's DS402 ladder writes intermediate + # CW values (0x06/0x07/0x0F) interleaved with SW reads. If one axis + # raises mid-cycle, gather cancels the others mid-sequence — leaving + # them in an indeterminate state (e.g. 0x07 written but 0x0F never sent) + # that the per-call retry budget can't reliably recover from. Sequential + # finishes one axis fully before touching the next. + for nid in node_ids: + nid_int = int(nid) + if await self.motor_is_enabled(nid_int): + continue + logger.warning("node %d: re-enabling (was disabled)", nid_int) + await self.motor_enable(node_id=nid_int, state=True, use_ds402=use_ds402) + # --- motion primitives -------------------------------------------------- async def _set_op_mode(self, node_id: int, mode: int, timeout_s: float = 0.05) -> None: @@ -1227,29 +1266,173 @@ async def _set_op_mode(self, node_id: int, mode: int, timeout_s: float = 0.05) - ) await asyncio.sleep(0.005) - async def pvt_select_mode(self, enable: bool) -> None: - """Enable/disable PVT mode on all motion axes via standard SDO writes.""" + async def ipm_select_mode(self, enable: bool) -> None: + """Enable/disable IPM (Interpolated Position Mode, 0x6060=7) on all motion + axes via standard SDO writes. + + The ``_ipm_mode`` bookkeeping flag is set *pessimistically*: True before + the SDO writes on enable, False after them on disable. So a partial + failure mid-sequence still leaves the next caller with an accurate + "we tried to be in IPM" signal — they can re-arm rather than assume + we cleanly stayed in PPM. + """ if enable: - if not self._pvt_mode: + # Set the flag first so a partial failure (some axes flipped to mode 7, + # others not) leaves bookkeeping consistent with "drive may be in IPM". + already_armed = self._ipm_mode + self._ipm_mode = True + if not already_armed: for nid in self.motion_node_ids: - # 0x60C4 sub 6 = 0 (disable interpolation buffer) - await self._can_sdo_download(nid, 0x60C4, 0x06, [0]) + await self.ipm_clear_queue(nid) await self._set_op_mode(nid, 7) # interpolated position mode - self._pvt_mode = True else: # Re-arm: drop to PPM, reset the interpolation-buffer pointer, climb - # back into IPM. Mirrors C# PVTSelectMode true-and-already-in-PVT - # (clscanmotor.cs:6014-6031). Skipping any of the three steps leaves - # the drive in the wrong mode or with a stale IP buffer. + # back into IPM. Skipping any of the three steps leaves the drive in + # the wrong mode or with a stale IP buffer. for nid in self.motion_node_ids: await self._set_op_mode(nid, 1) - await self._can_sdo_download(nid, 0x60C4, 0x06, [0]) + await self.ipm_clear_queue(nid) await self._set_op_mode(nid, 7) else: - if self._pvt_mode: - for nid in self.motion_node_ids: - await self._set_op_mode(nid, 1) # profile position mode - self._pvt_mode = False + # Always attempt to revert — the cheap mode-display poll inside + # _set_op_mode is the authoritative confirmation. Skip the writes only + # if we know we never armed (cleaner test semantics; idempotent on + # the wire either way). + if self._ipm_mode: + try: + for nid in self.motion_node_ids: + await self._set_op_mode(nid, 1) # profile position mode + finally: + # Clear bookkeeping even on partial failure — the alternative is + # leaving _ipm_mode=True after a teardown attempt, which would + # cause the next select_mode(True) to take the re-arm branch + # against drives possibly already in PPM. + self._ipm_mode = False + + async def ipm_clear_queue(self, node_id: int) -> None: + """Reset the drive's interpolation buffer head/tail (0x60C4 sub 6 = 0). + Used by `ipm_select_mode` re-arm and post-cancel cleanup so a stale tail + pointer doesn't replay old points on the next IPM enable.""" + await self._can_sdo_download(int(node_id), 0x60C4, 0x06, [0]) + + async def ipm_set_time_interval(self, ms: int) -> None: + """Program 0x60C2:01 = ms on every motion axis. The 0x60C2:02 = -3 written + at setup means the unit is milliseconds; this just sets the count. + + Drive-level: integer ms only (UNSIGNED8). Caller picks dt; runtime passes + the same value used to build the trajectory so position/velocity scaling + matches what the drive integrates between points.""" + if not 0 <= int(ms) <= 255: + raise ValueError(f"ipm_set_time_interval: ms must fit in UINT8, got {ms}") + for nid in self.motion_node_ids: + await self.can_sdo_download_elmo_object( + nid, 24770, 1, int(ms), _ElmoObjectDataType.UNSIGNED8, + ) + + def ipm_send_pvt_point( + self, node_id: int, position_enc: int, velocity_enc_per_s: int, + ) -> None: + """Append one PVT (P, V) data point to the drive's interpolation buffer + via RPDO3. + + Wire layout (8 bytes, little-endian, RPDO3 COB-ID = 0x400 + node_id): + [0..3] TargetPositionIP (INT32 encoder counts) + [4..7] TargetVelocityIP (INT32 encoder counts/sec) + + Synchronous — `network.send_message` queues to the kernel CAN buffer in + microseconds with no I/O wait, so there's nothing to await. Marking it + `async` would mislead callers about the cost (no event-loop yield) and + add a coroutine-frame allocation per send. + + RPDO3 is mapped EventDriven (no SYNC needed) at setup. Caller paces the + feed; sending faster than the buffer drains raises EMCY 0x34 from the + drive (queue-full).""" + if self._network is None: + raise CanError("ipm_send_pvt_point called before setup()") + payload = ( + int(position_enc).to_bytes(4, "little", signed=True) + + int(velocity_enc_per_s).to_bytes(4, "little", signed=True) + ) + cob_id = (int(COBType.RPDO3) << 7) | (int(node_id) & 0x7F) + self._network.send_message(cob_id, payload) + + async def ipm_begin_motion(self, node_ids: List[int]) -> None: + """Start IPM streaming on the listed axes. CW=0x1F (op-enabled + + ip-enable) per axis via RPDO1; one SYNC at the end so the + SynchronousCyclic-mapped RPDO1s latch together. + + Auto-recovers from a prior disable (post-halt, post-fault) via + ``motors_ensure_enabled``. Single-shot 0x1F to a drive in Switch On + Disabled is silently dropped — the state machine needs the 6→7→15 + transitions visible to its poll loop. + + Resets the per-axis IPM queue counters BEFORE issuing the CW edge so a + queue_full/underflow fired between preload and begin (e.g. malformed + first point) is preserved for the runtime to inspect and surface.""" + ids = [int(n) for n in node_ids] + if not ids: + return + await self.motors_ensure_enabled(ids) + for nid in ids: + if nid in self._ipm_emcy: + self._ipm_emcy[nid] = _IpmEmcyState() + for nid in ids[:-1]: + await self._control_word_set(nid, 0x1F, sync=False) + await self._control_word_set(ids[-1], 0x1F, sync=True) + + async def ipm_stop(self, node_ids: Optional[List[int]] = None) -> None: + """Drop ip-enable on the listed axes (CW=0x0F via RPDO1, SYNC on last). + The drive consumes any already-buffered points before halting — coast + can run up to (queued_points * dt_ms) ms past the request. Defaults to + every motion axis when ``node_ids`` is None.""" + ids = [int(n) for n in node_ids] if node_ids is not None else list(self.motion_node_ids) + if not ids: + return + for nid in ids[:-1]: + await self._control_word_set(nid, 0x0F, sync=False) + await self._control_word_set(ids[-1], 0x0F, sync=True) + + def ipm_check_queue_fault(self, node_ids: List[int]) -> None: + """Raise CanError if any axis has a fatal IPM queue condition. + + Inspects the EMCY-driven ``_ipm_emcy`` state for ``queue_full`` (drive + rejected our point) or ``underflow`` (drive ran the buffer dry). + Either condition means the trajectory is no longer trustworthy — the + streaming runtime calls this after each send-batch to surface the fault + promptly instead of letting the move silently degrade. + """ + bad: List[str] = [] + for nid in node_ids: + st = self._ipm_emcy.get(int(nid)) + if st is None: + continue + if st.queue_full: + bad.append(f"Axis {nid} queue_full (failed write @ {st.queue_full_failed_write_pointer})") + if st.underflow: + bad.append(f"Axis {nid} underflow") + if bad: + raise CanError("IPM queue fault: " + "; ".join(bad)) + + async def ipm_wait_motion_complete( + self, node_ids: List[int], timeout_s: float, + ) -> None: + """Wait until SW bit-10 (motion_complete / target_reached) goes high on + every listed axis. The bit goes high once the IP buffer drains and the + drive's trajectory generator settles on the last commanded position. + + Polls via the TPDO3-backed StatusWord cache; falls back to SDO probe + if the cache hasn't seen a frame within 5 ms. Raises CanError on + timeout, naming the offending axis.""" + async def _wait_one(nid: int) -> None: + ok = await self._wait_sw_bit( + int(nid), bit_mask=1 << 10, want_high=True, timeout_s=timeout_s, + ) + if not ok: + raise CanError( + f"Axis {nid}: SW bit-10 (target reached) never went high within " + f"{timeout_s:.1f}s — IPM trajectory did not complete" + ) + await asyncio.gather(*(_wait_one(int(n)) for n in node_ids)) async def wait_for_moves_done( self, node_ids: List[int], timeout: float @@ -1277,7 +1460,7 @@ async def _poll_axis(nid: int) -> None: await asyncio.gather(*(_poll_axis(n) for n in node_ids)) - async def motors_move_start( + async def ppm_begin_motion( self, node_ids: List[int], *, relative: bool = False ) -> None: # CiA 402 Profile Position Mode trigger handshake. Per drive: @@ -1295,16 +1478,12 @@ async def motors_move_start( relative_bit = 0x40 if relative else 0 cw_low = 47 + relative_bit cw_high = 47 + 0x10 + relative_bit + # Auto-recover from prior disable (post-E-stop, post-find_z IL halt, + # post-freedrive). A disabled drive never raises SW bit 12, so the PPM + # trigger spins all 10 attempts before failing. + await self.motors_ensure_enabled([int(n) for n in node_ids]) for nid in node_ids: - nid = int(nid) - # Auto-recover from prior disable (post-E-stop, post-find_z IL halt, - # post-freedrive). A disabled drive never raises SW bit 12, so the - # PPM trigger spins all 10 attempts before failing. One MO read per - # axis is cheap; the heavy DS402 cycle only runs when actually needed. - if not await self.motor_is_enabled(nid): - logger.warning("node %d: re-enabling before motion (was disabled)", nid) - await self.motor_enable(node_id=nid, state=True, use_ds402=True) - await self._trigger_new_setpoint(nid, cw_low, cw_high) + await self._trigger_new_setpoint(int(nid), cw_low, cw_high) async def _trigger_new_setpoint( self, diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index d0018bb9a3a..4c853cd7ef7 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -22,8 +22,9 @@ (CCW positive looking down). """ -from math import asin, atan2, cos, degrees, hypot, pi, radians, sin, sqrt, trunc -from typing import Dict, Optional +from dataclasses import dataclass, field +from math import asin, atan2, ceil, cos, degrees, hypot, pi, radians, sin, sqrt, trunc +from typing import Dict, List, Optional, Tuple from pylabrobot.capabilities.arms import kinematics as arm_kinematics from pylabrobot.capabilities.arms.standard import GripperLocation @@ -430,3 +431,201 @@ def plan_joint_move( direction=ax_cfg.joint_move_direction, )) return MotorsMovePlan(moves=moves, move_time=move_time) + + +# --- Cartesian-linear path sampling (for IPM/PVT streaming) ---------------- +# +# `plan_joint_move` generates a *joint*-space trapezoid: every axis ramps in +# parallel through its own (v, a) profile, which gives a curvy tool-tip path. +# `sample_linear_path` instead generates a *Cartesian*-linear path: the +# gripper travels the straight line from start to end, sampled at fixed dt, +# and IK at each sample yields the joint-space trajectory. The KX2ArmBackend +# streams the result into the drive's interpolation buffer (PVT mode) when +# `CartesianMoveParams(path='linear')` is requested. + +# Arm axes that show up in FK/IK and therefore have an entry in every +# `LinearPathSample.joints`. Servo gripper and rail are not Cartesian-driven. +_LINEAR_PATH_AXES: Tuple[Axis, ...] = (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST) + + +@dataclass +class LinearPathSample: + """One frame of a Cartesian-linear trajectory. + + ``joints`` is in planning units (mm for Z, deg for SHOULDER/WRIST, + *linear extension mm* for ELBOW). ``encoder_position`` / + ``encoder_velocity`` are post-conversion: ELBOW is converted through + ``convert_elbow_position_to_angle`` first because the encoder counts are + driven by the rotary actuator, not the linear projection. The runtime + feeds these straight into ``ipm_send_pvt_point``. + """ + + time_s: float + joints: Dict[Axis, float] = field(default_factory=dict) + encoder_position: Dict[Axis, int] = field(default_factory=dict) + encoder_velocity: Dict[Axis, int] = field(default_factory=dict) + + +def _arc_length_trapezoid_times( + length: float, vel: float, accel: float, dt_s: float, +) -> List[float]: + """Return arc-length s(t_i) at evenly-spaced t_i = i*dt covering a trapezoidal + profile from 0 to ``length`` with peak speed ``vel`` and peak accel ``accel``. + + Falls back to triangular when the path is too short to reach ``vel``. + + The trajectory is **dt-aligned**: the last sample lands at exactly + ``ceil(t_total / dt) * dt`` with s = length, and one extra "hold" sample + at length is appended so the central-difference velocity at the final + point is exactly zero. This matters for IPM streaming — the drive + integrates a cubic Hermite spline through (P_i, V_i) → (P_{i+1}, V_{i+1}) + over each dt; without the hold, the FD-derived V at the last sample + would be small-but-nonzero, and the cubic would overshoot the target + before snapping back. + """ + if length <= 0 or dt_s <= 0: + return [0.0] + v, a, t_acc, t_total = _profile(length, vel, accel) + if t_total <= 0: + return [0.0] + d_acc = 0.5 * a * t_acc * t_acc + n_motion = max(2, ceil(t_total / dt_s) + 1) + out: List[float] = [] + for i in range(n_motion): + t = i * dt_s + if t >= t_total: + out.append(length) + elif t < t_acc: + out.append(0.5 * a * t * t) + elif t < t_total - t_acc: + out.append(d_acc + v * (t - t_acc)) + else: + td = t_total - t + out.append(length - 0.5 * a * td * td) + out.append(length) # trailing hold so FD-derived final V is exactly 0 + return out + + +def _yaw_lerp(yaw_a_deg: float, yaw_b_deg: float, alpha: float) -> float: + """Interpolate yaw the short way around the circle. Avoids a 359° unwind + when start=+179°, end=-179° (true delta = 2°, naive lerp delta = 358°).""" + delta = (yaw_b_deg - yaw_a_deg + 540.0) % 360.0 - 180.0 + return yaw_a_deg + alpha * delta + + +def sample_linear_path( + cfg: KX2Config, + gripper_params: GripperParams, + start_pose: GripperLocation, + end_pose: GripperLocation, + *, + vel_mm_per_s: float, + accel_mm_per_s2: float, + dt_s: float, + current_joints: Optional[Dict[Axis, float]] = None, +) -> List[LinearPathSample]: + """Sample a straight tool-tip path from ``start_pose`` to ``end_pose`` at dt. + + Args: + cfg: drive-read calibration. + gripper_params: tooling geometry. + start_pose, end_pose: Cartesian endpoints (gripper clamp point + yaw). + vel_mm_per_s: peak Cartesian speed along the path. + accel_mm_per_s2: peak Cartesian acceleration along the path. + dt_s: sample period (matches the dt fed to ``ipm_set_time_interval``). + current_joints: pre-move joint snapshot. If supplied, the first sample's + WRIST/SHOULDER are 360°-snapped toward ``current_joints`` so the + trajectory doesn't begin with a full unwind. ELBOW position passes + through unchanged (linear axis). + + Returns: + Dense list of samples, one per dt step. The last sample lands exactly + on ``end_pose``. ``encoder_position`` / ``encoder_velocity`` are ready + to feed straight into ``ipm_send_pvt_point``. Velocity is the central + finite difference of encoder positions; the endpoints use one-sided + differences and are clamped to zero at the very last sample so the + drive ends stationary. + """ + if vel_mm_per_s <= 0 or accel_mm_per_s2 <= 0 or dt_s <= 0: + raise ValueError( + f"sample_linear_path: vel/accel/dt must be positive (got " + f"{vel_mm_per_s}, {accel_mm_per_s2}, {dt_s})" + ) + + sx, sy, sz = start_pose.location.x, start_pose.location.y, start_pose.location.z + ex, ey, ez = end_pose.location.x, end_pose.location.y, end_pose.location.z + start_yaw, end_yaw = start_pose.rotation.z, end_pose.rotation.z + length = sqrt((ex - sx) ** 2 + (ey - sy) ** 2 + (ez - sz) ** 2) + + # Pure rotation in place is unsupported here: the speed/accel caps are + # mm/s and mm/s², so reusing them as deg/s for a wrist spin would silently + # command rotational rates the caller didn't intend. Use a joint-space + # move for orientation-only changes. + rot_delta = abs(((end_yaw - start_yaw + 540.0) % 360.0) - 180.0) + if length <= _EPS and rot_delta > _EPS: + raise NotImplementedError( + "sample_linear_path: pure rotation (no translation) is not supported. " + f"Translation length={length:.4f} mm, rotation delta={rot_delta:.2f}°. " + "Use move_to_joint_position with the target wrist angle for " + "orientation-only changes." + ) + + # Cartesian arc-length trapezoid. Yaw rides the same s/length ratio so + # rotation lands together with translation. + s_seq = _arc_length_trapezoid_times(length, vel_mm_per_s, accel_mm_per_s2, dt_s) + + prev_for_snap: Optional[Dict[Axis, float]] = ( + dict(current_joints) if current_joints is not None else None + ) + poses: List[Dict[Axis, float]] = [] + for s in s_seq: + alpha = (s / length) if length > 0 else 1.0 + pose_i = GripperLocation( + location=Coordinate( + x=sx + alpha * (ex - sx), + y=sy + alpha * (ey - sy), + z=sz + alpha * (ez - sz), + ), + rotation=Rotation(z=_yaw_lerp(start_yaw, end_yaw, alpha)), + ) + joints = ik(pose_i, cfg, gripper_params) + if prev_for_snap is not None: + joints = snap_to_current(joints, prev_for_snap) + poses.append(joints) + prev_for_snap = joints + + # Encoder-space sequence per axis. Elbow's encoder is angle-driven (sine + # linkage), so convert position to angle before scaling by motor factor. + enc_pos: Dict[Axis, List[int]] = {ax: [] for ax in _LINEAR_PATH_AXES} + for joints in poses: + for ax in _LINEAR_PATH_AXES: + conv = cfg.axes[ax].motor_conversion_factor + val = joints[ax] + if ax is Axis.ELBOW: + val = convert_elbow_position_to_angle(cfg, val) + enc_pos[ax].append(int(round(val * conv))) + + # Central finite difference for velocity; one-sided at endpoints. The + # trajectory is dt-aligned with a trailing hold sample (s = length), + # so the one-sided FD at the last point is naturally zero — no need + # to force it. Forcing zero against a non-trivial v[n-2] would create + # the cubic-Hermite overshoot the trailing hold is meant to avoid. + n = len(poses) + enc_vel: Dict[Axis, List[int]] = {ax: [0] * n for ax in _LINEAR_PATH_AXES} + for ax in _LINEAR_PATH_AXES: + seq = enc_pos[ax] + if n >= 2: + enc_vel[ax][0] = int(round((seq[1] - seq[0]) / dt_s)) + for i in range(1, n - 1): + enc_vel[ax][i] = int(round((seq[i + 1] - seq[i - 1]) / (2.0 * dt_s))) + enc_vel[ax][n - 1] = int(round((seq[n - 1] - seq[n - 2]) / dt_s)) + + out: List[LinearPathSample] = [] + for i in range(n): + out.append(LinearPathSample( + time_s=i * dt_s, + joints=poses[i], + encoder_position={ax: enc_pos[ax][i] for ax in _LINEAR_PATH_AXES}, + encoder_velocity={ax: enc_vel[ax][i] for ax in _LINEAR_PATH_AXES}, + )) + return out diff --git a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py index 3a7fe7b260c..2dae4db82d7 100644 --- a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py +++ b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py @@ -35,10 +35,16 @@ def __init__(self) -> None: self.motor_stop_calls: List[Axis] = [] self.configure_il_calls: List[Tuple[Any, int, _InputLogic]] = [] self.motor_stop_should_raise: Optional[Exception] = None + self.ensure_enabled_calls: List[Tuple[int, ...]] = [] async def motor_enable(self, *, node_id: Any, state: bool, use_ds402: bool) -> None: self.motor_enable_calls.append((node_id, state, use_ds402)) + async def motors_ensure_enabled( + self, node_ids: List[int], *, use_ds402: bool = True, + ) -> None: + self.ensure_enabled_calls.append(tuple(int(n) for n in node_ids)) + async def motor_stop(self, axis: Axis) -> None: self.motor_stop_calls.append(axis) if self.motor_stop_should_raise is not None: @@ -140,8 +146,8 @@ def test_trip_cancels_descent_and_runs_cleanup(self): self.assertEqual(h.fake_driver.configure_il_calls, [arm, restore]) # motor_stop ran exactly once on the Z axis. self.assertEqual(h.fake_driver.motor_stop_calls, [Axis.Z]) - # Pre-flight motor_enable still happened. - self.assertEqual(h.fake_driver.motor_enable_calls, [(Axis.Z, True, True)]) + # Pre-flight ensured Z was enabled before descent. + self.assertEqual(h.fake_driver.ensure_enabled_calls, [(int(Axis.Z),)]) # Descent target is z0 - max_descent. self.assertEqual(len(h.move_calls), 1) self.assertAlmostEqual(h.move_calls[0]["cmd_pos"][Axis.Z], 80.0, places=9) @@ -235,8 +241,8 @@ def test_already_tripped_short_circuits_no_il_change(self): # No motor_stop, no descent move spawned. self.assertEqual(h.fake_driver.motor_stop_calls, []) self.assertEqual(h.move_calls, []) - # motor_enable still ran (it's the pre-flight before anything else). - self.assertEqual(h.fake_driver.motor_enable_calls, [(Axis.Z, True, True)]) + # ensure_enabled still ran (it's the pre-flight before anything else). + self.assertEqual(h.fake_driver.ensure_enabled_calls, [(int(Axis.Z),)]) def test_z_start_provided_runs_pre_descent_move_then_short_circuits(self): """Same as above but with z_start: the pre-positioning move runs, then diff --git a/pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py new file mode 100644 index 00000000000..6a36e9b6b12 --- /dev/null +++ b/pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py @@ -0,0 +1,291 @@ +"""Unit tests for ``KX2ArmBackend._run_linear_path``. + +Exercises the IPM streaming runtime that backs ``CartesianMoveParams(path='linear')``: + +* Preload + begin_motion call ordering. Preload writes 8 frames into the + drive's interpolation buffer *before* CW=0x1F is issued so the first sync + doesn't underflow the queue. +* Per-sample (P, V) frames go to all four arm axes (SHOULDER/Z/ELBOW/WRIST) + in lockstep. +* Cancellation mid-stream runs the finally-block cleanup: ``ipm_stop`` then + ``ipm_select_mode(False)``, both even if the cancel arrives before the + trapezoid is exhausted. +* Validation: requesting ``path='linear'`` without ``max_gripper_speed`` or + ``max_gripper_acceleration`` raises ValueError. + +Driver methods are stubbed on a fake recorder; ``request_joint_position`` is +shimmed so the runtime computes start_pose without going to hardware. Motion +guard is initialised the same way the find_z tests do it (manual Lock + +``__new__``). +""" +import asyncio +import unittest +from typing import Any, Dict, List, Optional, Tuple + +from pylabrobot.capabilities.arms.standard import GripperLocation +from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend +from pylabrobot.paa.kx2.config import ( + Axis, AxisConfig, GripperParams, KX2Config, +) +from pylabrobot.paa.kx2.driver import JointMoveDirection +from pylabrobot.resources import Coordinate, Rotation + + +def _axis() -> AxisConfig: + return AxisConfig( + motor_conversion_factor=1000.0, + max_travel=180.0, + min_travel=-180.0, + unlimited_travel=False, + absolute_encoder=True, + max_vel=100.0, + max_accel=100.0, + joint_move_direction=JointMoveDirection.Normal, + digital_inputs={}, + analog_inputs={}, + outputs={}, + ) + + +def _config() -> KX2Config: + return KX2Config( + wrist_offset=10.0, + elbow_offset=20.0, + elbow_zero_offset=5.0, + axes={a: _axis() for a in ( + Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST, + )}, + base_to_gripper_clearance_z=0.0, + base_to_gripper_clearance_arm=0.0, + robot_on_rail=False, + servo_gripper=None, + ) + + +class _FakeDriver: + """Records every IPM-related call. Order matters: tests assert the + preload-then-begin-motion sequencing.""" + + def __init__(self) -> None: + self.calls: List[Tuple[str, Tuple[Any, ...]]] = [] + self.send_calls: List[Tuple[int, int, int]] = [] + self.cancel_after_sends: Optional[int] = None + + async def ipm_select_mode(self, enable: bool) -> None: + self.calls.append(("ipm_select_mode", (enable,))) + + async def ipm_set_time_interval(self, ms: int) -> None: + self.calls.append(("ipm_set_time_interval", (ms,))) + + def ipm_send_pvt_point( + self, node_id: int, position_enc: int, velocity_enc_per_s: int, + ) -> None: + self.send_calls.append((node_id, position_enc, velocity_enc_per_s)) + self.calls.append(( + "ipm_send_pvt_point", (node_id, position_enc, velocity_enc_per_s), + )) + if ( + self.cancel_after_sends is not None + and len(self.send_calls) >= self.cancel_after_sends + ): + raise asyncio.CancelledError("test injection") + + async def ipm_begin_motion(self, node_ids: List[int]) -> None: + self.calls.append(("ipm_begin_motion", (tuple(node_ids),))) + + async def ipm_stop(self, node_ids: Optional[List[int]] = None) -> None: + self.calls.append(( + "ipm_stop", (tuple(node_ids) if node_ids is not None else None,), + )) + + def ipm_check_queue_fault(self, node_ids: List[int]) -> None: + # Synchronous helper; record the call so tests can verify backpressure + # checks happen at the expected points. + self.calls.append(("ipm_check_queue_fault", (tuple(int(n) for n in node_ids),))) + + async def ipm_wait_motion_complete( + self, node_ids: List[int], timeout_s: float, + ) -> None: + self.calls.append(( + "ipm_wait_motion_complete", (tuple(int(n) for n in node_ids), timeout_s), + )) + + +def _build_backend( + fake_driver: _FakeDriver, + *, + current_joints: Optional[Dict[int, float]] = None, +) -> KX2ArmBackend: + backend = KX2ArmBackend.__new__(KX2ArmBackend) + backend.driver = fake_driver # type: ignore[assignment] + backend._motion_lock = asyncio.Lock() + backend._motion_owner = None + backend._config = _config() + backend._gripper_params = GripperParams(length=15.0, z_offset=3.0) + + joints = current_joints if current_joints is not None else { + int(Axis.SHOULDER): 30.0, + int(Axis.Z): 50.0, + int(Axis.ELBOW): 60.0, + int(Axis.WRIST): 0.0, + int(Axis.SERVO_GRIPPER): 0.0, + } + + async def _request_joint_position() -> Dict[int, float]: + return dict(joints) + + backend.request_joint_position = _request_joint_position # type: ignore[assignment] + return backend + + +def _target_pose() -> GripperLocation: + """A pose ~30 mm away from the harness's current_joints — long enough that + the arc-length trapezoid produces well over the 8-frame preload.""" + return GripperLocation( + location=Coordinate(x=20.0, y=110.0, z=80.0), + rotation=Rotation(z=15.0), + ) + + +class LinearPathHappyPath(unittest.TestCase): + def test_preload_then_begin_then_stream(self): + """Preload 8 frames per axis (32 sends), then ipm_begin_motion, then + further per-frame sends in lockstep across the four arm axes.""" + fake = _FakeDriver() + backend = _build_backend(fake) + params = KX2ArmBackend.CartesianMoveParams( + max_gripper_speed=20.0, + max_gripper_acceleration=200.0, + path="linear", + ) + asyncio.run(backend._run_linear_path(_target_pose(), params)) + + # Find begin_motion in the call log + begin_idx = next( + i for i, (n, _) in enumerate(fake.calls) if n == "ipm_begin_motion" + ) + pre = fake.calls[:begin_idx] + pre_sends = [c for c in pre if c[0] == "ipm_send_pvt_point"] + # 4 axes * 8 preload frames = 32 sends before begin_motion + self.assertEqual(len(pre_sends), 4 * backend._LINEAR_PATH_PRELOAD) + + # begin_motion received the four arm axes + _, (axes,) = fake.calls[begin_idx] + self.assertEqual(set(axes), { + int(Axis.SHOULDER), int(Axis.Z), int(Axis.ELBOW), int(Axis.WRIST), + }) + + def test_dt_set_to_8ms_default(self): + fake = _FakeDriver() + backend = _build_backend(fake) + params = KX2ArmBackend.CartesianMoveParams( + max_gripper_speed=20.0, max_gripper_acceleration=200.0, path="linear", + ) + asyncio.run(backend._run_linear_path(_target_pose(), params)) + dt_calls = [c for c in fake.calls if c[0] == "ipm_set_time_interval"] + self.assertEqual(len(dt_calls), 1) + self.assertEqual(dt_calls[0][1], (backend._LINEAR_PATH_DT_MS,)) + + def test_select_mode_true_then_false(self): + """Mode is flipped on at start, off in cleanup. The drive must end in + PPM so subsequent joint moves don't try to issue PPM triggers in IPM.""" + fake = _FakeDriver() + backend = _build_backend(fake) + params = KX2ArmBackend.CartesianMoveParams( + max_gripper_speed=20.0, max_gripper_acceleration=200.0, path="linear", + ) + asyncio.run(backend._run_linear_path(_target_pose(), params)) + selects = [c[1] for c in fake.calls if c[0] == "ipm_select_mode"] + self.assertEqual(selects[0], (True,)) + self.assertEqual(selects[-1], (False,)) + + def test_wait_uses_ipm_specific_motion_complete(self): + """End-of-motion wait must poll SW bit-10 (target reached), not MS. + MS goes to 0 transiently between buffered points; bit-10 is the + authoritative IPM-done signal.""" + fake = _FakeDriver() + backend = _build_backend(fake) + params = KX2ArmBackend.CartesianMoveParams( + max_gripper_speed=20.0, max_gripper_acceleration=200.0, path="linear", + ) + asyncio.run(backend._run_linear_path(_target_pose(), params)) + # Must NOT use the MS-based generic waiter. + self.assertFalse([c for c in fake.calls if c[0] == "wait_for_moves_done"]) + waits = [c for c in fake.calls if c[0] == "ipm_wait_motion_complete"] + self.assertEqual(len(waits), 1) + (axes, _) = waits[0][1] + self.assertEqual(set(axes), { + int(Axis.SHOULDER), int(Axis.Z), int(Axis.ELBOW), int(Axis.WRIST), + }) + + def test_queue_fault_checked_after_preload_and_each_send(self): + """After preload and after every streamed send, the runtime must + inspect _ipm_emcy via ipm_check_queue_fault — otherwise queue_full / + underflow EMCYs are silently swallowed and the move appears to + succeed even when the drive rejected our points.""" + fake = _FakeDriver() + backend = _build_backend(fake) + params = KX2ArmBackend.CartesianMoveParams( + max_gripper_speed=20.0, max_gripper_acceleration=200.0, path="linear", + ) + asyncio.run(backend._run_linear_path(_target_pose(), params)) + fault_checks = [c for c in fake.calls if c[0] == "ipm_check_queue_fault"] + # At least one after preload, then one after each streamed send, plus + # one after the wait — total ≥ 3. + self.assertGreaterEqual(len(fault_checks), 3) + + +class LinearPathCancellationCleanup(unittest.TestCase): + def test_cancel_mid_stream_runs_ipm_stop_and_ipm_select_false(self): + """Inject a CancelledError after a few sends. Both cleanup steps must + execute regardless: ipm_stop dropping ip-enable, then ipm_select_mode + flipping back to PPM.""" + fake = _FakeDriver() + fake.cancel_after_sends = 12 # 8 preload + a few streamed + backend = _build_backend(fake) + params = KX2ArmBackend.CartesianMoveParams( + max_gripper_speed=20.0, max_gripper_acceleration=200.0, path="linear", + ) + with self.assertRaises(asyncio.CancelledError): + asyncio.run(backend._run_linear_path(_target_pose(), params)) + + names = [c[0] for c in fake.calls] + self.assertIn("ipm_stop", names) + self.assertEqual(names[-1], "ipm_select_mode") + self.assertEqual(fake.calls[-1][1], (False,)) + + +class LinearPathValidation(unittest.TestCase): + def test_missing_caps_raises(self): + """No max_gripper_speed -> we can't build the Cartesian profile. + Surface a clear ValueError before any drive interaction.""" + fake = _FakeDriver() + backend = _build_backend(fake) + params_no_speed = KX2ArmBackend.CartesianMoveParams( + max_gripper_speed=None, + max_gripper_acceleration=200.0, + path="linear", + ) + with self.assertRaises(ValueError): + asyncio.run(backend._run_linear_path(_target_pose(), params_no_speed)) + self.assertEqual(len(fake.calls), 0) + + params_no_accel = KX2ArmBackend.CartesianMoveParams( + max_gripper_speed=20.0, + max_gripper_acceleration=None, + path="linear", + ) + with self.assertRaises(ValueError): + asyncio.run(backend._run_linear_path(_target_pose(), params_no_accel)) + + +class LinearPathDispatchDefault(unittest.TestCase): + def test_default_path_is_joint(self): + """``CartesianMoveParams()`` defaults to joint-space planning. Verify + the field is unset (no surprise switch to linear).""" + p = KX2ArmBackend.CartesianMoveParams() + self.assertEqual(p.path, "joint") + + +if __name__ == "__main__": + unittest.main() diff --git a/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py b/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py index f48fc05450a..2d5c549d31e 100644 --- a/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py +++ b/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py @@ -20,7 +20,7 @@ EmcyFrame, KX2Driver, _EMCY_COB_BASE, - _PvtEmcyState, + _IpmEmcyState, ) @@ -88,7 +88,7 @@ def test_stale_listener_cb_noops_after_stop(self): drv._network = mock.MagicMock() loop = asyncio.new_event_loop() drv._loop = loop - drv._pvt_emcy[1] = _PvtEmcyState() + drv._ipm_emcy[1] = _IpmEmcyState() try: loop.run_until_complete(drv.stop()) finally: @@ -100,7 +100,7 @@ def test_stale_listener_cb_noops_after_stop(self): self.assertFalse(drv.emcy_move_error_received) self.assertEqual(drv.emcy_move_error, "") - self.assertEqual(drv._pvt_emcy, {}) + self.assertEqual(drv._ipm_emcy, {}) self.assertEqual(drv._emcy_callbacks, []) @@ -159,7 +159,7 @@ def send_message(self_inner, *a, **k): drv = KX2Driver() # Short-circuit the parts of setup we don't need (PDO mapping, Elmo - # vendor-object writes, pvt_select_mode). The fake's add_node returns a + # vendor-object writes, ipm_select_mode). The fake's add_node returns a # node whose sdo download/upload are no-ops, so these would technically # work, but skipping keeps the test fast and focused on call ordering. async def _noop(*a, **k): @@ -171,7 +171,7 @@ async def _noop(*a, **k): mock.patch.object(KX2Driver, "_tpdo_map", _noop), \ mock.patch.object(KX2Driver, "_rpdo_map", _noop), \ mock.patch.object(KX2Driver, "can_sdo_download_elmo_object", _noop), \ - mock.patch.object(KX2Driver, "pvt_select_mode", _noop), \ + mock.patch.object(KX2Driver, "ipm_select_mode", _noop), \ mock.patch("asyncio.sleep", _noop): asyncio.new_event_loop().run_until_complete(drv.setup()) diff --git a/pylabrobot/paa/kx2/tests/driver_emcy_polish_tests.py b/pylabrobot/paa/kx2/tests/driver_emcy_polish_tests.py index f974255c46a..566dfc29b7d 100644 --- a/pylabrobot/paa/kx2/tests/driver_emcy_polish_tests.py +++ b/pylabrobot/paa/kx2/tests/driver_emcy_polish_tests.py @@ -16,7 +16,7 @@ from pylabrobot.paa.kx2.driver import ( EmcyFrame, KX2Driver, - _PvtEmcyState, + _IpmEmcyState, _decode_emcy, ) @@ -29,14 +29,14 @@ def _frame( class DecodeErrorResetTests(unittest.TestCase): def test_zero_err_code_is_error_reset_and_suppresses(self): - state = _PvtEmcyState() + state = _IpmEmcyState() desc, disable, suppress = _decode_emcy(EmcyFrame(0, 0, 0, 0, 0), state) self.assertEqual(desc, "Error reset") self.assertFalse(disable) self.assertTrue(suppress) def test_zero_err_code_does_not_mutate_state(self): - state = _PvtEmcyState() + state = _IpmEmcyState() state.queue_low = False state.queue_full = False _decode_emcy(EmcyFrame(0, 0, 0, 0, 0), state) @@ -51,7 +51,7 @@ def test_zero_err_code_does_not_mutate_state(self): def test_zero_err_code_with_nonzero_elmo_still_treated_as_reset(self): # Some drives stamp a stale elmo byte on the reset frame; err_code=0 is # the canonical signal per DS301 so we treat it as reset regardless. - state = _PvtEmcyState() + state = _IpmEmcyState() desc, disable, suppress = _decode_emcy( EmcyFrame(0, 0, 0xFF, 0, 0), state ) @@ -61,18 +61,18 @@ def test_zero_err_code_with_nonzero_elmo_still_treated_as_reset(self): def test_existing_unknown_code_path_unaffected(self): # Sanity: the err_code=0 branch must not steal the unknown-code path. - state = _PvtEmcyState() + state = _IpmEmcyState() desc, _, _ = _decode_emcy(EmcyFrame(0x1234, 0, 0xAB, 0, 0), state) self.assertEqual(desc, "Unknown EMCY 0x1234/0xAB") class DispatchEmcyLogLevelTests(unittest.TestCase): - """Verify the level matrix in ``_dispatch_emcy`` so PVT housekeeping events + """Verify the level matrix in ``_dispatch_emcy`` so IPM housekeeping events don't drown ops logs while real faults stay loud.""" def setUp(self): self.driver = KX2Driver() - self.driver._pvt_emcy[1] = _PvtEmcyState() + self.driver._ipm_emcy[1] = _IpmEmcyState() self.logger_name = "pylabrobot.paa.kx2.driver" def _dispatch_at(self, payload: bytes): diff --git a/pylabrobot/paa/kx2/tests/driver_emcy_tests.py b/pylabrobot/paa/kx2/tests/driver_emcy_tests.py index 6f5cf78edef..3099c988e19 100644 --- a/pylabrobot/paa/kx2/tests/driver_emcy_tests.py +++ b/pylabrobot/paa/kx2/tests/driver_emcy_tests.py @@ -10,7 +10,7 @@ from pylabrobot.paa.kx2.driver import ( EmcyFrame, KX2Driver, - _PvtEmcyState, + _IpmEmcyState, _decode_emcy, ) @@ -22,8 +22,8 @@ def _frame( class DecodeEmcyTests(unittest.TestCase): - def test_pvt_queue_low_ff00(self): - state = _PvtEmcyState() + def test_ipm_queue_low_ff00(self): + state = _IpmEmcyState() desc, disable, suppress = _decode_emcy( EmcyFrame(0xFF00, 0, 0x56, 0x1234, 0x0042), state ) @@ -34,8 +34,8 @@ def test_pvt_queue_low_ff00(self): self.assertEqual(state.queue_low_write_pointer, 0x1234) self.assertEqual(state.queue_low_read_pointer, 0x0042) - def test_pvt_queue_full_ff02(self): - state = _PvtEmcyState() + def test_ipm_queue_full_ff02(self): + state = _IpmEmcyState() desc, disable, suppress = _decode_emcy( EmcyFrame(0xFF02, 0, 0x34, 0x55AA, 0), state ) @@ -46,34 +46,36 @@ def test_pvt_queue_full_ff02(self): self.assertEqual(state.queue_full_failed_write_pointer, 0x55AA) def test_estop_disables_motors(self): - state = _PvtEmcyState() + state = _IpmEmcyState() desc, disable, suppress = _decode_emcy(EmcyFrame(0x5441, 0, 0, 0, 0), state) self.assertEqual(desc, "E-stop button was pressed") self.assertTrue(disable) self.assertFalse(suppress) def test_interpolation_underflow_suppresses_callback(self): - state = _PvtEmcyState() + state = _IpmEmcyState() desc, disable, suppress = _decode_emcy(EmcyFrame(0xFF02, 0, 0x8A, 0, 0), state) self.assertEqual(desc, "Position Interpolation buffer underflow") self.assertFalse(disable) self.assertTrue(suppress) - self.assertTrue(state.queue_low) + # Underflow is post-fact, distinct from proactive queue_low. + self.assertTrue(state.underflow) + self.assertFalse(state.queue_low) def test_unknown_code_falls_back_to_hex(self): - state = _PvtEmcyState() + state = _IpmEmcyState() desc, disable, suppress = _decode_emcy(EmcyFrame(0x1234, 0, 0xAB, 0, 0), state) self.assertEqual(desc, "Unknown EMCY 0x1234/0xAB") self.assertFalse(disable) self.assertFalse(suppress) def test_ff02_unknown_elmo_is_ds402_ip_error(self): - state = _PvtEmcyState() + state = _IpmEmcyState() desc, _, _ = _decode_emcy(EmcyFrame(0xFF02, 0, 0xCC, 0, 0), state) self.assertEqual(desc, "DS402 IP Error 0xCC") def test_position_tracking_error_disables_motors(self): - state = _PvtEmcyState() + state = _IpmEmcyState() desc, disable, _ = _decode_emcy(EmcyFrame(0x8611, 0, 0, 0, 0), state) self.assertEqual(desc, "Position tracking error") self.assertTrue(disable) @@ -85,7 +87,7 @@ def setUp(self): # _dispatch_emcy normally runs on the asyncio loop after # call_soon_threadsafe; here we drive it synchronously since the method # itself is sync and doesn't touch the network. - self.driver._pvt_emcy[1] = _PvtEmcyState() + self.driver._ipm_emcy[1] = _IpmEmcyState() def test_estop_sets_sticky_error_fields(self): self.driver._dispatch_emcy(1, _frame(0x5441)) @@ -100,8 +102,8 @@ def test_non_fatal_does_not_set_sticky_error(self): self.driver._dispatch_emcy(1, _frame(0xFF00, 0x56, data1=10, data2=5)) self.assertFalse(self.driver.emcy_move_error_received) self.assertEqual(self.driver.emcy_move_error, "") - self.assertTrue(self.driver._pvt_emcy[1].queue_low) - self.assertEqual(self.driver._pvt_emcy[1].queue_low_write_pointer, 10) + self.assertTrue(self.driver._ipm_emcy[1].queue_low) + self.assertEqual(self.driver._ipm_emcy[1].queue_low_write_pointer, 10) def test_callback_invoked(self): received = [] @@ -120,7 +122,9 @@ def test_callback_suppressed_for_interpolation_underflow(self): self.driver.add_emcy_callback(lambda *args: received.append(args)) self.driver._dispatch_emcy(1, _frame(0xFF02, 0x8A)) self.assertEqual(received, []) - self.assertTrue(self.driver._pvt_emcy[1].queue_low) + # 0x8A is post-fact underflow, not proactive queue_low. + self.assertTrue(self.driver._ipm_emcy[1].underflow) + self.assertFalse(self.driver._ipm_emcy[1].queue_low) def test_callback_exception_does_not_break_dispatch(self): received = [] @@ -140,7 +144,7 @@ def test_clear_emcy_state_resets_node(self): self.assertFalse(self.driver.emcy_move_error_received) self.assertEqual(self.driver.emcy_move_error, "") self.assertIsNone(self.driver.emcy_move_error_node_id) - self.assertFalse(self.driver._pvt_emcy[1].queue_low) + self.assertFalse(self.driver._ipm_emcy[1].queue_low) def test_short_frame_logged_not_raised(self): # Should warn and return without raising. diff --git a/pylabrobot/paa/kx2/tests/kinematics_tests.py b/pylabrobot/paa/kx2/tests/kinematics_tests.py index 31130addf20..3db6545bf1e 100644 --- a/pylabrobot/paa/kx2/tests/kinematics_tests.py +++ b/pylabrobot/paa/kx2/tests/kinematics_tests.py @@ -367,5 +367,160 @@ def test_negative_180_snaps_to_positive(self): self.assertAlmostEqual(joints[Axis.SHOULDER], 180.0, places=9) +def _linear_pose(x: float, y: float, z: float, yaw: float = 0.0) -> GripperLocation: + return GripperLocation( + location=Coordinate(x=x, y=y, z=z), rotation=Rotation(z=yaw), + ) + + +class SampleLinearPath(unittest.TestCase): + """Sampler-level checks for `kinematics.sample_linear_path`. The straight + Cartesian line is the contract; we verify endpoints land exact, intermediate + samples stay on the line, dt drives sample density, and finite-difference + velocity respects the central-difference rule.""" + + def setUp(self) -> None: + self.cfg = _config() + self.gp = GripperParams(length=15.0, z_offset=3.0) + self.start = _linear_pose(50.0, 100.0, 30.0, yaw=10.0) + self.end = _linear_pose(80.0, 130.0, 60.0, yaw=40.0) + + def test_endpoints_exact_via_fk_roundtrip(self): + """First sample's joints FK back to start_pose; last sample's to end_pose. + Tightest correctness check — the streamed start/end land where requested.""" + samples = kinematics.sample_linear_path( + self.cfg, self.gp, self.start, self.end, + vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008, + ) + self.assertGreater(len(samples), 2) + first_back = kinematics.fk(samples[0].joints, self.cfg, self.gp) + last_back = kinematics.fk(samples[-1].joints, self.cfg, self.gp) + for got, want in ( + (first_back.location.x, self.start.location.x), + (first_back.location.y, self.start.location.y), + (first_back.location.z, self.start.location.z), + (last_back.location.x, self.end.location.x), + (last_back.location.y, self.end.location.y), + (last_back.location.z, self.end.location.z), + ): + self.assertAlmostEqual(got, want, places=6) + + def test_samples_stay_on_straight_line(self): + """FK every sample. Cross-product of (sample - start) with (end - start) + is zero iff the sample is colinear; tolerance handles floating-point.""" + samples = kinematics.sample_linear_path( + self.cfg, self.gp, self.start, self.end, + vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008, + ) + sx, sy, sz = self.start.location.x, self.start.location.y, self.start.location.z + ex, ey, ez = self.end.location.x, self.end.location.y, self.end.location.z + dx, dy, dz = ex - sx, ey - sy, ez - sz + L = math.sqrt(dx * dx + dy * dy + dz * dz) + for sample in samples: + back = kinematics.fk(sample.joints, self.cfg, self.gp) + px, py, pz = back.location.x - sx, back.location.y - sy, back.location.z - sz + cx = py * dz - pz * dy + cy = pz * dx - px * dz + cz = px * dy - py * dx + cross_norm = math.sqrt(cx * cx + cy * cy + cz * cz) + self.assertLess(cross_norm / L, 1e-6, + msg=f"sample at t={sample.time_s:.4f}s off line by {cross_norm:.6f}") + + def test_dt_halves_doubles_sample_count(self): + """Two-fold dt change should ~halve sample count. ±2 slop for the trapezoid + rounding where the trailing partial step varies.""" + fast = kinematics.sample_linear_path( + self.cfg, self.gp, self.start, self.end, + vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.004, + ) + slow = kinematics.sample_linear_path( + self.cfg, self.gp, self.start, self.end, + vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008, + ) + self.assertAlmostEqual(len(fast) / len(slow), 2.0, delta=0.2) + + def test_last_sample_velocity_zero(self): + """Drive integrates V over dt; non-zero final V would push past target. + Last sample's encoder velocity must be exactly zero on every axis.""" + samples = kinematics.sample_linear_path( + self.cfg, self.gp, self.start, self.end, + vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008, + ) + for ax, v in samples[-1].encoder_velocity.items(): + self.assertEqual(v, 0, msg=f"axis {ax.name} final velocity {v} != 0") + + def test_zero_length_path_returns_short(self): + """Same start and end pose: no motion. Sampler returns a degenerate + list (≤1 sample). Runtime guards on len(samples) < 2 and bails.""" + samples = kinematics.sample_linear_path( + self.cfg, self.gp, self.start, self.start, + vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008, + ) + self.assertLessEqual(len(samples), 1) + + def test_invalid_caps_raise(self): + """Non-positive vel/accel/dt is a programmer error — fail loud.""" + with self.assertRaises(ValueError): + kinematics.sample_linear_path( + self.cfg, self.gp, self.start, self.end, + vel_mm_per_s=0.0, accel_mm_per_s2=100.0, dt_s=0.008, + ) + with self.assertRaises(ValueError): + kinematics.sample_linear_path( + self.cfg, self.gp, self.start, self.end, + vel_mm_per_s=20.0, accel_mm_per_s2=-1.0, dt_s=0.008, + ) + with self.assertRaises(ValueError): + kinematics.sample_linear_path( + self.cfg, self.gp, self.start, self.end, + vel_mm_per_s=20.0, accel_mm_per_s2=100.0, dt_s=0.0, + ) + + def test_velocity_finite_difference_matches_position(self): + """Interior samples use central difference: v[i] ≈ (p[i+1] - p[i-1]) / 2dt. + Verify the relationship holds for shoulder over an interior sample.""" + samples = kinematics.sample_linear_path( + self.cfg, self.gp, self.start, self.end, + vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008, + ) + if len(samples) < 4: + self.skipTest("not enough samples for interior check") + i = len(samples) // 2 + expected_v = ( + samples[i + 1].encoder_position[Axis.SHOULDER] + - samples[i - 1].encoder_position[Axis.SHOULDER] + ) / (2.0 * 0.008) + self.assertAlmostEqual( + samples[i].encoder_velocity[Axis.SHOULDER], int(round(expected_v)), delta=1, + ) + + def test_yaw_lerp_takes_short_way(self): + """Yaw 179° -> -179° while ALSO translating is a 2° rotation, not 358°. + Sampler must interpolate yaw via the short arc, otherwise wrist angles + spin a full unwind alongside the linear translation.""" + start = _linear_pose(50.0, 100.0, 30.0, yaw=179.0) + end = _linear_pose(80.0, 130.0, 60.0, yaw=-179.0) + samples = kinematics.sample_linear_path( + self.cfg, self.gp, start, end, + vel_mm_per_s=10.0, accel_mm_per_s2=100.0, dt_s=0.008, + ) + self.assertGreaterEqual(len(samples), 3) + mid_back = kinematics.fk(samples[len(samples) // 2].joints, self.cfg, self.gp) + # Short way through ±180° -> midpoint near ±180°, not near 0°. + self.assertGreater(abs(mid_back.rotation.z), 170.0) + + def test_pure_rotation_in_place_raises(self): + """No translation + non-zero rotation: caps are mm/s, repurposing them + as deg/s is a footgun. Surface clearly so the caller switches to + move_to_joint_position.""" + start = _linear_pose(50.0, 100.0, 30.0, yaw=10.0) + end = _linear_pose(50.0, 100.0, 30.0, yaw=40.0) + with self.assertRaises(NotImplementedError): + kinematics.sample_linear_path( + self.cfg, self.gp, start, end, + vel_mm_per_s=20.0, accel_mm_per_s2=200.0, dt_s=0.008, + ) + + if __name__ == "__main__": unittest.main() From 85c7d59405dd40439f9545842893296a363d8e19 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 3 May 2026 16:36:31 -0700 Subject: [PATCH 77/93] KX2: wire barcode reader as an optional capability on KX2 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/kx2.py | 44 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 43 insertions(+), 1 deletion(-) diff --git a/pylabrobot/paa/kx2/kx2.py b/pylabrobot/paa/kx2/kx2.py index 2c0e9300b70..c800c720bbb 100644 --- a/pylabrobot/paa/kx2/kx2.py +++ b/pylabrobot/paa/kx2/kx2.py @@ -2,9 +2,14 @@ from typing import Optional from pylabrobot.capabilities.arms.orientable_arm import OrientableArm +from pylabrobot.capabilities.barcode_scanning import BarcodeScanner from pylabrobot.capabilities.capability import BackendParams from pylabrobot.device import Device from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend +from pylabrobot.paa.kx2.barcode_reader import ( + KX2BarcodeReaderBackend, + KX2BarcodeReaderDriver, +) from pylabrobot.paa.kx2.config import GripperFingerSide from pylabrobot.paa.kx2.driver import KX2Driver from pylabrobot.resources.resource import Resource @@ -22,6 +27,9 @@ def __init__( gripper_finger_side: GripperFingerSide = "barcode_reader", has_rail: bool = False, has_servo_gripper: bool = True, + barcode_port: Optional[str] = None, + barcode_read_time: int = 2, + barcode_baudrate: int = KX2BarcodeReaderDriver.default_baudrate, ) -> None: driver = KX2Driver(has_rail=has_rail, has_servo_gripper=has_servo_gripper) super().__init__(driver=driver) @@ -36,6 +44,22 @@ def __init__( self.arm = OrientableArm(backend=backend, reference_resource=self.reference) self._capabilities = [self.arm] + # Onboard barcode reader is a serial device on a separate USB-serial port, + # entirely independent of the CAN bus that drives the motors. Wire it in + # only when a port is given; the standalone `KX2BarcodeReader` Device + # remains available for users who prefer to manage it as a sibling. + self._bcr_driver: Optional[KX2BarcodeReaderDriver] = None + self.barcode_scanning: Optional[BarcodeScanner] = None + if barcode_port is not None: + self._bcr_driver = KX2BarcodeReaderDriver( + port=barcode_port, baudrate=barcode_baudrate, + ) + bcr_backend = KX2BarcodeReaderBackend( + self._bcr_driver, read_time=barcode_read_time, + ) + self.barcode_scanning = BarcodeScanner(backend=bcr_backend) + self._capabilities.append(self.barcode_scanning) + async def setup(self, backend_params: Optional[BackendParams] = None): # Idempotent: re-running setup on a live KX2 used to crash partway through # the homing/PDO sequence. If already up, stay up — call stop() first to @@ -43,4 +67,22 @@ async def setup(self, backend_params: Optional[BackendParams] = None): if self.setup_finished: logger.info("KX2.setup: already set up; skipping. Call stop() first to re-init.") return - await super().setup(backend_params=backend_params) + # Bring the barcode reader's serial port up before super().setup() so the + # barcode_scanning capability's _on_setup (version handshake) has an open + # port. If the BCR fails, abort before touching the CAN bus. + if self._bcr_driver is not None: + await self._bcr_driver.setup() + try: + await super().setup(backend_params=backend_params) + except BaseException: + if self._bcr_driver is not None: + try: + await self._bcr_driver.stop() + except Exception: + logger.exception("KX2.setup cleanup: BCR driver stop failed; ignoring") + raise + + async def stop(self): + await super().stop() + if self._bcr_driver is not None: + await self._bcr_driver.stop() From 8382742cde947c6d7d5c46c1f0d7b47346e798c3 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 3 May 2026 18:36:46 -0700 Subject: [PATCH 78/93] KX2: drop read_time from BCR object, take it per scan() call 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. --- docs/user_guide/paa/kx2/hello-world.ipynb | 38 +++--------------- pylabrobot/paa/kx2/barcode_reader.py | 47 +++++++++-------------- pylabrobot/paa/kx2/kx2.py | 5 +-- 3 files changed, 26 insertions(+), 64 deletions(-) diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 273be6ba9b8..9a3ea9591a2 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -446,11 +446,7 @@ "cell_type": "markdown", "id": "7d393b50", "metadata": {}, - "source": [ - "### Setup\n", - "\n", - "Pass the serial port path and (optionally) a read-window in seconds. `setup()` opens the port, does the `Z1` software-version handshake, and puts the reader into single-trigger mode." - ] + "source": "### Setup\n\nPass the serial port path. `setup()` opens the port, does the `Z1` software-version handshake, and puts the reader into single-trigger mode." }, { "cell_type": "code", @@ -458,22 +454,13 @@ "id": "52e6a377", "metadata": {}, "outputs": [], - "source": [ - "from pylabrobot.paa.kx2 import KX2BarcodeReader\n", - "\n", - "bcr = KX2BarcodeReader(port=\"/dev/tty.PL2303G-USBtoUART11240\", read_time=8)\n", - "await bcr.setup()" - ] + "source": "from pylabrobot.paa.kx2 import KX2BarcodeReader\n\nbcr = KX2BarcodeReader(port=\"/dev/tty.PL2303G-USBtoUART11240\")\nawait bcr.setup()" }, { "cell_type": "markdown", "id": "665c811a", "metadata": {}, - "source": [ - "### Scan\n", - "\n", - "`scan()` fires the trigger and waits up to `read_time + 1s` for the reader to decode something. Returns a {class}`~pylabrobot.resources.barcode.Barcode`." - ] + "source": "### Scan\n\n`scan(read_time=...)` fires the trigger and waits up to `read_time + 1s` for the reader to decode something. Omit `read_time` to use whatever read window is currently programmed on the reader. Returns a {class}`~pylabrobot.resources.barcode.Barcode`." }, { "cell_type": "code", @@ -481,20 +468,13 @@ "id": "8ba1d44d", "metadata": {}, "outputs": [], - "source": [ - "barcode = await bcr.barcode_scanning.scan()\n", - "print(barcode.data)" - ] + "source": "barcode = await bcr.barcode_scanning.scan(read_time=8)\nprint(barcode.data)" }, { "cell_type": "markdown", "id": "5282deb9", "metadata": {}, - "source": [ - "### Configuration\n", - "\n", - "Adjust the read window or trigger mode at runtime:" - ] + "source": "### Configuration\n\nChange the trigger mode at runtime:" }, { "cell_type": "code", @@ -502,13 +482,7 @@ "id": "63db140e", "metadata": {}, "outputs": [], - "source": [ - "# 1..9 seconds, or 0 for indefinite. Updates the reader and the scan timeout together.\n", - "await bcr.set_read_time(5)\n", - "\n", - "# \"single\" (default), \"multiple\" (two reads per trigger), or \"continuous\".\n", - "# await bcr.set_read_mode(\"continuous\")" - ] + "source": "# \"single\" (default), \"multiple\" (two reads per trigger), or \"continuous\".\n# await bcr.set_read_mode(\"continuous\")" }, { "cell_type": "markdown", diff --git a/pylabrobot/paa/kx2/barcode_reader.py b/pylabrobot/paa/kx2/barcode_reader.py index 6733b92ef43..b06ac1ef3a0 100644 --- a/pylabrobot/paa/kx2/barcode_reader.py +++ b/pylabrobot/paa/kx2/barcode_reader.py @@ -261,15 +261,13 @@ async def dump_config(self, timeout: float = 8.0) -> str: class KX2BarcodeReaderBackend(BarcodeScannerBackend): """Adapts :class:`KX2BarcodeReaderDriver` to the BarcodeScanner capability.""" - def __init__(self, driver: KX2BarcodeReaderDriver, read_time: int = 2): + # Wait bound when the caller doesn't specify a read_time. Long enough that + # any reasonable on-device read window (Y1..Y9) finishes inside it. + _DEFAULT_SCAN_WAIT = 10.0 + + def __init__(self, driver: KX2BarcodeReaderDriver): super().__init__() self.driver = driver - self._read_time = read_time - - async def set_read_time(self, seconds: int) -> None: - """Update both the reader's read window and our scan timeout.""" - await self.driver.set_read_time(seconds) - self._read_time = seconds async def _on_setup(self, backend_params: Optional[BackendParams] = None) -> None: # Handshake: version query (mirrors KX2RobotControl.cs:15617). @@ -281,14 +279,19 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None) -> Non ) logger.info("[KX2 BCR %s] software version: %s", self.driver.io.port, version) await self.driver.set_read_mode("single") - await self.set_read_time(self._read_time) - async def scan_barcode(self) -> Barcode: - # Fire the trigger, then listen for the decoded line. The reader delivers - # data asynchronously (not as a command-response), so we read-until-CR - # rather than sending another command. + async def scan_barcode(self, read_time: Optional[float] = None) -> Barcode: + # Reader's Y-command only takes integer seconds 1..9 (YM=indefinite). When + # the caller specifies read_time, push it to the device first so the + # on-device window matches our wait bound. Otherwise leave whatever's + # currently configured and wait long enough for any in-range setting. + if read_time is not None: + if read_time <= 0: + raise ValueError("read_time must be > 0") + await self.driver.set_read_time(int(round(read_time))) await self.driver.trigger(True) - data = await self.driver.read_decoded_barcode(timeout=float(self._read_time + 1)) + timeout = (read_time + 1.0) if read_time is not None else self._DEFAULT_SCAN_WAIT + data = await self.driver.read_decoded_barcode(timeout=timeout) if not data: raise BarcodeScannerError("KX2 barcode reader: no read within read-time window") return Barcode(data=data, symbology="ANY 1D", position_on_resource="front") @@ -302,16 +305,13 @@ class KX2BarcodeReader(Device): ``/dev/tty.PL2303G-USBtoUART`` (after the Prolific driver is installed and approved — see module docstring). On Linux it's usually ``/dev/ttyUSB``. - read_time: Default read window in seconds (1–9, or 0 for indefinite). - The reader returns the decoded barcode if it sees one within this - window, otherwise the scan call raises. baudrate: Serial baud rate; the reader's factory default is 9600. Usage:: - bcr = KX2BarcodeReader(port="/dev/tty.PL2303G-USBtoUART11240", read_time=8) + bcr = KX2BarcodeReader(port="/dev/tty.PL2303G-USBtoUART11240") await bcr.setup() - barcode = await bcr.barcode_scanning.scan() + barcode = await bcr.barcode_scanning.scan(read_time=8) print(barcode.data) await bcr.stop() """ @@ -319,24 +319,15 @@ class KX2BarcodeReader(Device): def __init__( self, port: str, - read_time: int = 2, baudrate: int = KX2BarcodeReaderDriver.default_baudrate, ): driver = KX2BarcodeReaderDriver(port=port, baudrate=baudrate) super().__init__(driver=driver) self.driver: KX2BarcodeReaderDriver = driver - self._backend = KX2BarcodeReaderBackend(driver, read_time=read_time) + self._backend = KX2BarcodeReaderBackend(driver) self.barcode_scanning = BarcodeScanner(backend=self._backend) self._capabilities = [self.barcode_scanning] - async def set_read_time(self, seconds: int) -> None: - """Set the read-window timeout (1–9 seconds, or 0 for indefinite). - - Updates both the reader and the scan-completion timeout in lockstep, - so subsequent ``barcode_scanning.scan()`` calls wait long enough. - """ - await self._backend.set_read_time(seconds) - async def set_read_mode(self, mode: ReadMode) -> None: """Set the trigger mode: ``"single"`` (default), ``"multiple"``, or ``"continuous"``.""" diff --git a/pylabrobot/paa/kx2/kx2.py b/pylabrobot/paa/kx2/kx2.py index c800c720bbb..7295b055089 100644 --- a/pylabrobot/paa/kx2/kx2.py +++ b/pylabrobot/paa/kx2/kx2.py @@ -28,7 +28,6 @@ def __init__( has_rail: bool = False, has_servo_gripper: bool = True, barcode_port: Optional[str] = None, - barcode_read_time: int = 2, barcode_baudrate: int = KX2BarcodeReaderDriver.default_baudrate, ) -> None: driver = KX2Driver(has_rail=has_rail, has_servo_gripper=has_servo_gripper) @@ -54,9 +53,7 @@ def __init__( self._bcr_driver = KX2BarcodeReaderDriver( port=barcode_port, baudrate=barcode_baudrate, ) - bcr_backend = KX2BarcodeReaderBackend( - self._bcr_driver, read_time=barcode_read_time, - ) + bcr_backend = KX2BarcodeReaderBackend(self._bcr_driver) self.barcode_scanning = BarcodeScanner(backend=bcr_backend) self._capabilities.append(self.barcode_scanning) From 9e60bbb3f7e323610a6e5f99430cbe9a4b80db0b Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 3 May 2026 19:28:44 -0700 Subject: [PATCH 79/93] KX2 BCR: return None on read-window timeout instead of raising 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. --- pylabrobot/paa/kx2/barcode_reader.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/pylabrobot/paa/kx2/barcode_reader.py b/pylabrobot/paa/kx2/barcode_reader.py index b06ac1ef3a0..5824e4543c9 100644 --- a/pylabrobot/paa/kx2/barcode_reader.py +++ b/pylabrobot/paa/kx2/barcode_reader.py @@ -280,7 +280,7 @@ async def _on_setup(self, backend_params: Optional[BackendParams] = None) -> Non logger.info("[KX2 BCR %s] software version: %s", self.driver.io.port, version) await self.driver.set_read_mode("single") - async def scan_barcode(self, read_time: Optional[float] = None) -> Barcode: + async def scan_barcode(self, read_time: Optional[float] = None) -> Optional[Barcode]: # Reader's Y-command only takes integer seconds 1..9 (YM=indefinite). When # the caller specifies read_time, push it to the device first so the # on-device window matches our wait bound. Otherwise leave whatever's @@ -291,9 +291,15 @@ async def scan_barcode(self, read_time: Optional[float] = None) -> Barcode: await self.driver.set_read_time(int(round(read_time))) await self.driver.trigger(True) timeout = (read_time + 1.0) if read_time is not None else self._DEFAULT_SCAN_WAIT - data = await self.driver.read_decoded_barcode(timeout=timeout) + try: + data = await self.driver.read_decoded_barcode(timeout=timeout) + except BarcodeScannerError: + # Driver raises on serial-read timeout. At this layer that's the + # "nothing decoded within the window" signal — return None instead of + # propagating. Real comms failures aren't reported via this path. + return None if not data: - raise BarcodeScannerError("KX2 barcode reader: no read within read-time window") + return None return Barcode(data=data, symbology="ANY 1D", position_on_resource="front") From 3fef4b9b5dbfd98fc72274a695abbded9d21eff3 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 3 May 2026 19:47:50 -0700 Subject: [PATCH 80/93] KX2: collapse EMCY state to one per-node struct; clear in find_z MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/arm_backend.py | 9 +- pylabrobot/paa/kx2/driver.py | 120 ++++++++++++------ .../arm_backend_find_z_proximity_tests.py | 4 + .../tests/arm_backend_linear_path_tests.py | 2 +- .../kx2/tests/driver_emcy_lifecycle_tests.py | 9 +- .../paa/kx2/tests/driver_emcy_polish_tests.py | 12 +- pylabrobot/paa/kx2/tests/driver_emcy_tests.py | 55 ++++---- 7 files changed, 132 insertions(+), 79 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index b83fbcc787f..79e3d87feab 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -718,6 +718,11 @@ async def find_z_with_proximity_sensor( ) except Exception as e: logger.warning("find_z: IL restore failed: %s", e) + # The IL-trip and the motor_stop fire EMCY frames that mark the + # move as a fault; the trip was *expected*, not a real fault, so + # clear the sticky state — otherwise the next motion call's + # `motor_check_if_move_done` raises immediately on a stale flag. + self.driver.clear_emcy_state(int(Axis.Z)) if not tripped: z_end = await self.motor_get_current_position(Axis.Z) raise RuntimeError( @@ -923,8 +928,8 @@ async def _run_linear_path( # Log with EMCY state for post-mortem: if the drive was already # mid-fault when we tried to stop, the EMCY counters tell us why. emcy_snap = { - ax: vars(self.driver._ipm_emcy[ax]) for ax in active_axes - if ax in self.driver._ipm_emcy + ax: vars(self.driver._emcy[ax]) for ax in active_axes + if ax in self.driver._emcy } logger.exception( "ipm_stop cleanup failed; drive may still be in IPM with ip-enable " diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index e1f08e7f4e6..4710a7a5de8 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -210,14 +210,21 @@ class EmcyFrame: @dataclass -class _IpmEmcyState: - """Per-node IPM queue state, mirrored from `sPVT_EMCY` in clscanmotor.cs:6943-6964. - - ``queue_low`` is a proactive warning (drive's IP buffer is approaching - empty); ``underflow`` is post-fact (drive ran the buffer dry and - short-stopped the trajectory). The streaming runtime treats underflow as - a fatal error; queue_low is informational unless we move to event-driven - pacing. +class _NodeEmcyState: + """All EMCY state we track for one drive node — single source of truth. + + Three flavors of consumer pull from this: + - **Streaming runtime** reads ``queue_full`` / ``underflow`` to detect + IPM backpressure. ``queue_low`` is a proactive warning (drive's IP + buffer approaching empty); ``underflow`` is post-fact (drive ran the + buffer dry and short-stopped the trajectory). + - **Motion-poll path** reads ``move_error`` to raise on the next + ``motor_check_if_move_done`` after a fault-class EMCY arrived. The + string is preformatted with axis context. + - **Diagnostics** read ``last_frame`` for the last raw EMCY received + from this node. + + Reset (whole struct) on re-enable / fault clear / find_z post-IL-trip. """ queue_low: bool = False @@ -230,6 +237,8 @@ class _IpmEmcyState: bad_mode_init_data: bool = False motion_terminated: bool = False out_of_modulo: bool = False + move_error: Optional[str] = None + last_frame: Optional["EmcyFrame"] = None # Standard CANopen error codes that don't depend on the Elmo byte. @@ -293,7 +302,7 @@ class _IpmEmcyState: def _decode_emcy( - frame: EmcyFrame, state: _IpmEmcyState + frame: EmcyFrame, state: _NodeEmcyState ) -> Tuple[str, bool, bool]: """Decode an EMCY frame into (description, disable_motors, suppress_callback). @@ -456,12 +465,14 @@ def __init__( # EMCY (CANopen Emergency, COB-ID 0x80 + node_id) state. Subscribed in # setup(); fires on the canopen listener thread, marshalled into the - # asyncio loop via _make_emcy_callback. - self.emcy_move_error_received: bool = False - self.emcy_move_error: str = "" - self.emcy_move_error_node_id: Optional[int] = None - self.last_emcy: Optional[EmcyFrame] = None - self._ipm_emcy: Dict[int, _IpmEmcyState] = {} + # asyncio loop via _make_emcy_callback. Per-node `_NodeEmcyState` is + # the single source of truth (queue counters, sticky fault, last raw + # frame); `emcy_move_error` and `last_emcy` are derived properties. + self._emcy: Dict[int, _NodeEmcyState] = {} + # Tracks which node fired the most recent EMCY frame, so `last_emcy` + # can resolve to the right node's `last_frame`. Cheaper than scanning + # the dict on every read. + self._last_emcy_node_id: Optional[int] = None self._emcy_callbacks: List[ Callable[[int, EmcyFrame, str, bool], None] ] = [] @@ -480,6 +491,30 @@ def loop(self) -> asyncio.AbstractEventLoop: raise RuntimeError("KX2Driver event loop not initialized; call setup() first.") return self._loop + @property + def emcy_move_error(self) -> Optional[str]: + """First pending fault across all nodes; ``None`` if no fault. + + Pre-formatted with axis context (``"Axis {nid} {description}"``). + `motor_check_if_move_done` raises on this; recovery paths clear it + via `clear_emcy_state`. + """ + for st in self._emcy.values(): + if st.move_error is not None: + return st.move_error + return None + + @property + def last_emcy(self) -> Optional[EmcyFrame]: + """Most recent EMCY frame received from any node; ``None`` if none yet. + + Diagnostic only — motion logic uses `emcy_move_error` for fault + detection and `_emcy[nid]` for per-axis IPM queue state.""" + if self._last_emcy_node_id is None: + return None + st = self._emcy.get(self._last_emcy_node_id) + return st.last_frame if st is not None else None + # --- lifecycle ----------------------------------------------------------- async def setup(self, backend_params: Optional[BackendParams] = None) -> None: @@ -504,7 +539,7 @@ async def setup(self, backend_params: Optional[BackendParams] = None) -> None: # the C# event handler at KX2RobotControl.cs:15384-15425 / # clscanmotor.cs:1057-1284. for nid in self.node_id_list: - self._ipm_emcy[nid] = _IpmEmcyState() + self._emcy[nid] = _NodeEmcyState() network.subscribe(_EMCY_COB_BASE + nid, self._make_emcy_callback(nid)) # Reset all nodes, then start scanner so bootup messages populate it, @@ -591,14 +626,11 @@ async def stop(self) -> None: self._network.disconnect() self._network = None self._nodes = {} - self._ipm_emcy = {} + self._emcy = {} + self._last_emcy_node_id = None # Clear callbacks too: _on_setup re-registers on each retry, so leaving # them would compound N copies of the same handler across attempts. self._emcy_callbacks = [] - self.emcy_move_error_received = False - self.emcy_move_error = "" - self.emcy_move_error_node_id = None - self.last_emcy = None self._statusword = {} self._statusword_event = {} @@ -745,16 +777,19 @@ def add_emcy_callback( self._emcy_callbacks.append(cb) def clear_emcy_state(self, node_id: Optional[int] = None) -> None: - """Clear sticky EMCY error fields after a recovery / re-enable. + """Clear EMCY state. - If ``node_id`` is given, also resets the per-node IPM queue state for - that node. Mirrors clscanmotor.cs:668-669, 3454, 4481. + With ``node_id``: reset that one node's `_NodeEmcyState` (queue + counters + sticky fault + last_frame). Without: clear the sticky + fault on every node — leaves queue counters intact since they're + stream-scoped and reset by `ipm_begin_motion`. """ - self.emcy_move_error_received = False - self.emcy_move_error = "" - self.emcy_move_error_node_id = None - if node_id is not None and node_id in self._ipm_emcy: - self._ipm_emcy[node_id] = _IpmEmcyState() + if node_id is not None: + if node_id in self._emcy: + self._emcy[node_id] = _NodeEmcyState() + return + for st in self._emcy.values(): + st.move_error = None def _make_emcy_callback(self, node_id: int): """Return a `canopen.Network.subscribe` callback bound to a specific node.""" @@ -773,9 +808,10 @@ def _dispatch_emcy(self, node_id: int, data: bytes) -> None: return err_code, err_reg, elmo_err, d1, d2 = struct.unpack(" None: ) if disable_motors: - self.emcy_move_error_received = True - self.emcy_move_error = desc - self.emcy_move_error_node_id = node_id + # Pre-format with axis context: motor_check_if_move_done just raises + # `f"Motor Fault: {emcy_move_error}"` and the consumer expects the + # axis to be named. + state.move_error = f"Axis {node_id} {desc}" if suppress: return @@ -1090,10 +1127,9 @@ async def motor_check_if_move_done(self, node_id: int) -> bool: # them — the poll loop times out before ever consulting EMCY state. # Check sticky EMCY first so any fatal frame raises on the next poll # iteration regardless of MS. - if self.emcy_move_error_received and self.emcy_move_error: - nid = self.emcy_move_error_node_id - prefix = f"Axis {nid} " if nid is not None else "" - raise RuntimeError(f"Motor Fault: {prefix}{self.emcy_move_error}") + pending = self.emcy_move_error + if pending is not None: + raise RuntimeError(f"Motor Fault: {pending}") ms_val = await self.query_int(node_id, "MS", 0) if ms_val == 0: return True @@ -1374,8 +1410,8 @@ async def ipm_begin_motion(self, node_ids: List[int]) -> None: return await self.motors_ensure_enabled(ids) for nid in ids: - if nid in self._ipm_emcy: - self._ipm_emcy[nid] = _IpmEmcyState() + if nid in self._emcy: + self._emcy[nid] = _NodeEmcyState() for nid in ids[:-1]: await self._control_word_set(nid, 0x1F, sync=False) await self._control_word_set(ids[-1], 0x1F, sync=True) @@ -1395,7 +1431,7 @@ async def ipm_stop(self, node_ids: Optional[List[int]] = None) -> None: def ipm_check_queue_fault(self, node_ids: List[int]) -> None: """Raise CanError if any axis has a fatal IPM queue condition. - Inspects the EMCY-driven ``_ipm_emcy`` state for ``queue_full`` (drive + Inspects the EMCY-driven ``_emcy`` state for ``queue_full`` (drive rejected our point) or ``underflow`` (drive ran the buffer dry). Either condition means the trajectory is no longer trustworthy — the streaming runtime calls this after each send-batch to surface the fault @@ -1403,7 +1439,7 @@ def ipm_check_queue_fault(self, node_ids: List[int]) -> None: """ bad: List[str] = [] for nid in node_ids: - st = self._ipm_emcy.get(int(nid)) + st = self._emcy.get(int(nid)) if st is None: continue if st.queue_full: diff --git a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py index 2dae4db82d7..d9b5dd5dd53 100644 --- a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py +++ b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py @@ -36,6 +36,7 @@ def __init__(self) -> None: self.configure_il_calls: List[Tuple[Any, int, _InputLogic]] = [] self.motor_stop_should_raise: Optional[Exception] = None self.ensure_enabled_calls: List[Tuple[int, ...]] = [] + self.clear_emcy_calls: List[Optional[int]] = [] async def motor_enable(self, *, node_id: Any, state: bool, use_ds402: bool) -> None: self.motor_enable_calls.append((node_id, state, use_ds402)) @@ -45,6 +46,9 @@ async def motors_ensure_enabled( ) -> None: self.ensure_enabled_calls.append(tuple(int(n) for n in node_ids)) + def clear_emcy_state(self, node_id: Optional[int] = None) -> None: + self.clear_emcy_calls.append(node_id) + async def motor_stop(self, axis: Axis) -> None: self.motor_stop_calls.append(axis) if self.motor_stop_should_raise is not None: diff --git a/pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py index 6a36e9b6b12..b8504a5a1d2 100644 --- a/pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py +++ b/pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py @@ -220,7 +220,7 @@ def test_wait_uses_ipm_specific_motion_complete(self): def test_queue_fault_checked_after_preload_and_each_send(self): """After preload and after every streamed send, the runtime must - inspect _ipm_emcy via ipm_check_queue_fault — otherwise queue_full / + inspect _emcy via ipm_check_queue_fault — otherwise queue_full / underflow EMCYs are silently swallowed and the move appears to succeed even when the drive rejected our points.""" fake = _FakeDriver() diff --git a/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py b/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py index 2d5c549d31e..cc6fc0e748e 100644 --- a/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py +++ b/pylabrobot/paa/kx2/tests/driver_emcy_lifecycle_tests.py @@ -20,7 +20,7 @@ EmcyFrame, KX2Driver, _EMCY_COB_BASE, - _IpmEmcyState, + _NodeEmcyState, ) @@ -88,7 +88,7 @@ def test_stale_listener_cb_noops_after_stop(self): drv._network = mock.MagicMock() loop = asyncio.new_event_loop() drv._loop = loop - drv._ipm_emcy[1] = _IpmEmcyState() + drv._emcy[1] = _NodeEmcyState() try: loop.run_until_complete(drv.stop()) finally: @@ -98,9 +98,8 @@ def test_stale_listener_cb_noops_after_stop(self): # resurrect any state. cb(_EMCY_COB_BASE + 1, _frame(0x5441), 0.0) - self.assertFalse(drv.emcy_move_error_received) - self.assertEqual(drv.emcy_move_error, "") - self.assertEqual(drv._ipm_emcy, {}) + self.assertIsNone(drv.emcy_move_error) + self.assertEqual(drv._emcy, {}) self.assertEqual(drv._emcy_callbacks, []) diff --git a/pylabrobot/paa/kx2/tests/driver_emcy_polish_tests.py b/pylabrobot/paa/kx2/tests/driver_emcy_polish_tests.py index 566dfc29b7d..ba49d450be0 100644 --- a/pylabrobot/paa/kx2/tests/driver_emcy_polish_tests.py +++ b/pylabrobot/paa/kx2/tests/driver_emcy_polish_tests.py @@ -16,7 +16,7 @@ from pylabrobot.paa.kx2.driver import ( EmcyFrame, KX2Driver, - _IpmEmcyState, + _NodeEmcyState, _decode_emcy, ) @@ -29,14 +29,14 @@ def _frame( class DecodeErrorResetTests(unittest.TestCase): def test_zero_err_code_is_error_reset_and_suppresses(self): - state = _IpmEmcyState() + state = _NodeEmcyState() desc, disable, suppress = _decode_emcy(EmcyFrame(0, 0, 0, 0, 0), state) self.assertEqual(desc, "Error reset") self.assertFalse(disable) self.assertTrue(suppress) def test_zero_err_code_does_not_mutate_state(self): - state = _IpmEmcyState() + state = _NodeEmcyState() state.queue_low = False state.queue_full = False _decode_emcy(EmcyFrame(0, 0, 0, 0, 0), state) @@ -51,7 +51,7 @@ def test_zero_err_code_does_not_mutate_state(self): def test_zero_err_code_with_nonzero_elmo_still_treated_as_reset(self): # Some drives stamp a stale elmo byte on the reset frame; err_code=0 is # the canonical signal per DS301 so we treat it as reset regardless. - state = _IpmEmcyState() + state = _NodeEmcyState() desc, disable, suppress = _decode_emcy( EmcyFrame(0, 0, 0xFF, 0, 0), state ) @@ -61,7 +61,7 @@ def test_zero_err_code_with_nonzero_elmo_still_treated_as_reset(self): def test_existing_unknown_code_path_unaffected(self): # Sanity: the err_code=0 branch must not steal the unknown-code path. - state = _IpmEmcyState() + state = _NodeEmcyState() desc, _, _ = _decode_emcy(EmcyFrame(0x1234, 0, 0xAB, 0, 0), state) self.assertEqual(desc, "Unknown EMCY 0x1234/0xAB") @@ -72,7 +72,7 @@ class DispatchEmcyLogLevelTests(unittest.TestCase): def setUp(self): self.driver = KX2Driver() - self.driver._ipm_emcy[1] = _IpmEmcyState() + self.driver._emcy[1] = _NodeEmcyState() self.logger_name = "pylabrobot.paa.kx2.driver" def _dispatch_at(self, payload: bytes): diff --git a/pylabrobot/paa/kx2/tests/driver_emcy_tests.py b/pylabrobot/paa/kx2/tests/driver_emcy_tests.py index 3099c988e19..62d70b08843 100644 --- a/pylabrobot/paa/kx2/tests/driver_emcy_tests.py +++ b/pylabrobot/paa/kx2/tests/driver_emcy_tests.py @@ -10,7 +10,7 @@ from pylabrobot.paa.kx2.driver import ( EmcyFrame, KX2Driver, - _IpmEmcyState, + _NodeEmcyState, _decode_emcy, ) @@ -23,7 +23,7 @@ def _frame( class DecodeEmcyTests(unittest.TestCase): def test_ipm_queue_low_ff00(self): - state = _IpmEmcyState() + state = _NodeEmcyState() desc, disable, suppress = _decode_emcy( EmcyFrame(0xFF00, 0, 0x56, 0x1234, 0x0042), state ) @@ -35,7 +35,7 @@ def test_ipm_queue_low_ff00(self): self.assertEqual(state.queue_low_read_pointer, 0x0042) def test_ipm_queue_full_ff02(self): - state = _IpmEmcyState() + state = _NodeEmcyState() desc, disable, suppress = _decode_emcy( EmcyFrame(0xFF02, 0, 0x34, 0x55AA, 0), state ) @@ -46,14 +46,14 @@ def test_ipm_queue_full_ff02(self): self.assertEqual(state.queue_full_failed_write_pointer, 0x55AA) def test_estop_disables_motors(self): - state = _IpmEmcyState() + state = _NodeEmcyState() desc, disable, suppress = _decode_emcy(EmcyFrame(0x5441, 0, 0, 0, 0), state) self.assertEqual(desc, "E-stop button was pressed") self.assertTrue(disable) self.assertFalse(suppress) def test_interpolation_underflow_suppresses_callback(self): - state = _IpmEmcyState() + state = _NodeEmcyState() desc, disable, suppress = _decode_emcy(EmcyFrame(0xFF02, 0, 0x8A, 0, 0), state) self.assertEqual(desc, "Position Interpolation buffer underflow") self.assertFalse(disable) @@ -63,19 +63,19 @@ def test_interpolation_underflow_suppresses_callback(self): self.assertFalse(state.queue_low) def test_unknown_code_falls_back_to_hex(self): - state = _IpmEmcyState() + state = _NodeEmcyState() desc, disable, suppress = _decode_emcy(EmcyFrame(0x1234, 0, 0xAB, 0, 0), state) self.assertEqual(desc, "Unknown EMCY 0x1234/0xAB") self.assertFalse(disable) self.assertFalse(suppress) def test_ff02_unknown_elmo_is_ds402_ip_error(self): - state = _IpmEmcyState() + state = _NodeEmcyState() desc, _, _ = _decode_emcy(EmcyFrame(0xFF02, 0, 0xCC, 0, 0), state) self.assertEqual(desc, "DS402 IP Error 0xCC") def test_position_tracking_error_disables_motors(self): - state = _IpmEmcyState() + state = _NodeEmcyState() desc, disable, _ = _decode_emcy(EmcyFrame(0x8611, 0, 0, 0, 0), state) self.assertEqual(desc, "Position tracking error") self.assertTrue(disable) @@ -87,23 +87,24 @@ def setUp(self): # _dispatch_emcy normally runs on the asyncio loop after # call_soon_threadsafe; here we drive it synchronously since the method # itself is sync and doesn't touch the network. - self.driver._ipm_emcy[1] = _IpmEmcyState() + self.driver._emcy[1] = _NodeEmcyState() def test_estop_sets_sticky_error_fields(self): self.driver._dispatch_emcy(1, _frame(0x5441)) - self.assertTrue(self.driver.emcy_move_error_received) - self.assertEqual(self.driver.emcy_move_error, "E-stop button was pressed") - self.assertEqual(self.driver.emcy_move_error_node_id, 1) + self.assertEqual( + self.driver.emcy_move_error, "Axis 1 E-stop button was pressed", + ) + self.assertEqual(self.driver._emcy[1].move_error, "Axis 1 E-stop button was pressed") self.assertIsNotNone(self.driver.last_emcy) assert self.driver.last_emcy is not None # type narrowing for mypy self.assertEqual(self.driver.last_emcy.err_code, 0x5441) def test_non_fatal_does_not_set_sticky_error(self): self.driver._dispatch_emcy(1, _frame(0xFF00, 0x56, data1=10, data2=5)) - self.assertFalse(self.driver.emcy_move_error_received) - self.assertEqual(self.driver.emcy_move_error, "") - self.assertTrue(self.driver._ipm_emcy[1].queue_low) - self.assertEqual(self.driver._ipm_emcy[1].queue_low_write_pointer, 10) + self.assertIsNone(self.driver.emcy_move_error) + self.assertIsNone(self.driver._emcy[1].move_error) + self.assertTrue(self.driver._emcy[1].queue_low) + self.assertEqual(self.driver._emcy[1].queue_low_write_pointer, 10) def test_callback_invoked(self): received = [] @@ -123,8 +124,8 @@ def test_callback_suppressed_for_interpolation_underflow(self): self.driver._dispatch_emcy(1, _frame(0xFF02, 0x8A)) self.assertEqual(received, []) # 0x8A is post-fact underflow, not proactive queue_low. - self.assertTrue(self.driver._ipm_emcy[1].underflow) - self.assertFalse(self.driver._ipm_emcy[1].queue_low) + self.assertTrue(self.driver._emcy[1].underflow) + self.assertFalse(self.driver._emcy[1].queue_low) def test_callback_exception_does_not_break_dispatch(self): received = [] @@ -141,15 +142,23 @@ def test_clear_emcy_state_resets_node(self): self.driver._dispatch_emcy(1, _frame(0xFF00, 0x56, data1=10, data2=5)) self.driver._dispatch_emcy(1, _frame(0x5441)) self.driver.clear_emcy_state(node_id=1) - self.assertFalse(self.driver.emcy_move_error_received) - self.assertEqual(self.driver.emcy_move_error, "") - self.assertIsNone(self.driver.emcy_move_error_node_id) - self.assertFalse(self.driver._ipm_emcy[1].queue_low) + self.assertIsNone(self.driver.emcy_move_error) + self.assertIsNone(self.driver._emcy[1].move_error) + self.assertFalse(self.driver._emcy[1].queue_low) + + def test_clear_emcy_state_no_node_clears_only_move_error(self): + """No-arg clear: drop sticky faults across all nodes, but keep the + queue counters (those are stream-scoped, reset by ipm_begin_motion).""" + self.driver._dispatch_emcy(1, _frame(0xFF00, 0x56, data1=10, data2=5)) + self.driver._dispatch_emcy(1, _frame(0x5441)) + self.driver.clear_emcy_state() + self.assertIsNone(self.driver.emcy_move_error) + self.assertTrue(self.driver._emcy[1].queue_low) # preserved def test_short_frame_logged_not_raised(self): # Should warn and return without raising. self.driver._dispatch_emcy(1, b"\x00\x00\x00") - self.assertFalse(self.driver.emcy_move_error_received) + self.assertIsNone(self.driver.emcy_move_error) if __name__ == "__main__": From 42f6ce43d58db0e0b8366ddedc006f03589b57cb Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 3 May 2026 19:51:33 -0700 Subject: [PATCH 81/93] KX2: split proximity sensor docs into its own notebook 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) --- docs/user_guide/paa/index.md | 1 + docs/user_guide/paa/kx2/hello-world.ipynb | 79 +++--------- .../user_guide/paa/kx2/proximity-sensor.ipynb | 120 ++++++++++++++++++ 3 files changed, 139 insertions(+), 61 deletions(-) create mode 100644 docs/user_guide/paa/kx2/proximity-sensor.ipynb diff --git a/docs/user_guide/paa/index.md b/docs/user_guide/paa/index.md index 38cae5f216d..75831b573de 100644 --- a/docs/user_guide/paa/index.md +++ b/docs/user_guide/paa/index.md @@ -4,4 +4,5 @@ :maxdepth: 1 kx2/hello-world +kx2/proximity-sensor ``` diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 9a3ea9591a2..9c3e2975b9d 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -48,9 +48,9 @@ "source": [ "## Arm capabilities\n", "\n", - "The KX2 exposes an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm` on `kx2.arm`, backed by {class}`~pylabrobot.paa.kx2.KX2ArmBackend`. Gripper yaw is controlled via a single `direction` float (degrees; 0° = front). For the full arm API, see [Arms](../../capabilities/arms).\n", + "The KX2 exposes an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm` on `kx2.arm`, backed by {class}`~pylabrobot.paa.kx2.KX2ArmBackend`. Gripper yaw is controlled via a single `direction` float (degrees; 0\u00b0 = front). For the full arm API, see [Arms](../../capabilities/arms).\n", "\n", - "The raw driver (`kx2.driver`, a `KX2Driver`) stays available for low-level access — motor commands, drive diagnostics, binary interpreter, etc." + "The raw driver (`kx2.driver`, a `KX2Driver`) stays available for low-level access \u2014 motor commands, drive diagnostics, binary interpreter, etc." ] }, { @@ -102,7 +102,7 @@ "cell_type": "markdown", "id": "kx2-gripper-force-md", "metadata": {}, - "source": "Cap the gripping force on a per-close basis (as a percentage of the motor's peak current — 10..100, clamped) when closing on fragile labware:" + "source": "Cap the gripping force on a per-close basis (as a percentage of the motor's peak current \u2014 10..100, clamped) when closing on fragile labware:" }, { "cell_type": "code", @@ -112,54 +112,11 @@ "outputs": [], "source": "await kx2.arm.close_gripper(\n gripper_width=0,\n backend_params=KX2ArmBackend.GripParams(max_force_percent=30),\n)" }, - { - "cell_type": "markdown", - "id": "kx2-proximity-header", - "metadata": {}, - "source": [ - "### Proximity sensor\n", - "\n", - "An IR breakbeam between the gripper fingers, wired to the Z drive's IO header. The sensor is binary (no distance, just present/absent) and is exposed via the auto-discovered `\"ProximitySensor\"` digital input on the Z axis (pin 4).\n", - "\n", - "`read_proximity_sensor()` returns `True` when the beam is interrupted. Useful for confirming an object is in the gripper before closing." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "kx2-proximity-read", - "metadata": {}, - "outputs": [], - "source": [ - "await kx2.arm.backend.read_proximity_sensor()" - ] - }, - { - "cell_type": "markdown", - "id": "kx2-proximity-search-md", - "metadata": {}, - "source": [ - "`find_z_with_proximity_sensor` descends Z up to `max_descent` and halts the instant the beam trips. It arms the Elmo drive's input-logic register so the drive itself stops the motor on the input edge (sub-ms reaction; no software in the loop). Returns the Z where the drive halted; raises `RuntimeError` if the beam never tripped.\n", - "\n", - "Pass `z_start` to first move Z to a known starting height before searching. Position the arm above an object and open the gripper wider than the object before running." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "kx2-proximity-search", - "metadata": {}, - "outputs": [], - "source": [ - "z_hit = await kx2.arm.backend.find_z_with_proximity_sensor(max_descent=100.0, z_start=400)\n", - "print(f\"object found at Z = {z_hit:.2f}\")" - ] - }, { "cell_type": "markdown", "id": "kx2-motion-limits-md", "metadata": {}, - "source": "### Motion limits\n\n`motion_limits()` reads the per-axis firmware maxima cached at setup. `JointMoveParams` / `CartesianMoveParams` take `max_gripper_speed` (mm/s) and `max_gripper_acceleration` (mm/s^2), which cap the worst-case Cartesian speed/acceleration at the gripper along the trajectory. `None` (the default) means no Cartesian cap — joints run at firmware max. Joint speeds get scaled uniformly so the gripper stays under the cap." + "source": "### Motion limits\n\n`motion_limits()` reads the per-axis firmware maxima cached at setup. `JointMoveParams` / `CartesianMoveParams` take `max_gripper_speed` (mm/s) and `max_gripper_acceleration` (mm/s^2), which cap the worst-case Cartesian speed/acceleration at the gripper along the trajectory. `None` (the default) means no Cartesian cap \u2014 joints run at firmware max. Joint speeds get scaled uniformly so the gripper stays under the cap." }, { "cell_type": "code", @@ -175,7 +132,7 @@ "cell_type": "markdown", "id": "kx2-cartesian-header", "metadata": {}, - "source": "### Cartesian movement\n\nMove the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only — the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity and acceleration." + "source": "### Cartesian movement\n\nMove the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only \u2014 the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity and acceleration." }, { "cell_type": "code", @@ -288,7 +245,7 @@ "source": [ "### Pick and place\n", "\n", - "`pick_up_at_location` moves to the target pose and closes the gripper to `resource_width`. `drop_at_location` moves and opens the gripper. Approach and retract motions are the caller's responsibility — bracket these calls with your own pre- and post-moves." + "`pick_up_at_location` moves to the target pose and closes the gripper to `resource_width`. `drop_at_location` moves and opens the gripper. Approach and retract motions are the caller's responsibility \u2014 bracket these calls with your own pre- and post-moves." ] }, { @@ -321,7 +278,7 @@ "source": [ "### Freedrive (teaching mode)\n", "\n", - "Freedrive disables torque on the motion axes so you can push the arm by hand; the servo gripper stays powered. The KX2 frees all motion axes at once — the `free_axes` argument is accepted for API parity and ignored." + "Freedrive disables torque on the motion axes so you can push the arm by hand; the servo gripper stays powered. The KX2 frees all motion axes at once \u2014 the `free_axes` argument is accepted for API parity and ignored." ] }, { @@ -405,7 +362,7 @@ "source": [ "## Barcode reader\n", "\n", - "The KX2's onboard barcode reader is a serial 1D laser scanner wired to the controller PC. It's fully independent of the CAN motor stack, so it works even with the arm e-stopped — exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`.\n", + "The KX2's onboard barcode reader is a serial 1D laser scanner wired to the controller PC. It's fully independent of the CAN motor stack, so it works even with the arm e-stopped \u2014 exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`.\n", "\n", "The model that ships with KX2 systems we've inspected is a **Denso MDI-4050** (firmware reports as `BD01J09`). Factory defaults: **9600 8N1**, no flow control. Find your port with `python -m serial.tools.list_ports -v`.\n", "\n", @@ -418,15 +375,15 @@ "open -a PL2303Serial # registers the system extension\n", "```\n", "\n", - "Then **System Settings → Privacy & Security → Allow** on the \"System software from PL2303Serial was blocked\" prompt, restart if asked, and replug the cable. Verify with `systemextensionsctl list`: `com.prolific.cdc.PLCdcFSDriver` should show `[activated enabled]`.\n", + "Then **System Settings \u2192 Privacy & Security \u2192 Allow** on the \"System software from PL2303Serial was blocked\" prompt, restart if asked, and replug the cable. Verify with `systemextensionsctl list`: `com.prolific.cdc.PLCdcFSDriver` should show `[activated enabled]`.\n", "\n", "### Symbologies\n", "\n", - "The MDI-4050 is a 1D laser, so **PDF417 / DataMatrix / QR Code aren't supported** — you'd need the 2D imager variants (e.g. MDI-4150) for those.\n", + "The MDI-4050 is a 1D laser, so **PDF417 / DataMatrix / QR Code aren't supported** \u2014 you'd need the 2D imager variants (e.g. MDI-4150) for those.\n", "\n", "Typical out-of-the-box 1D enables: UPC-A, UPC-E, EAN-13, EAN-8, Code 39, Tri-Optic, Codabar, Industrial 2/5, Interleaved 2/5, IATA, Code 128, Code 93, GS1 DataBar, GS1 DataBar Limited. Add-on (supplemental) codes, postal codes, MSI/Plessey, Telepen, UK/Plessey, and Code 11 are off by default.\n", "\n", - "You can read your unit's full configuration over the wire — model, firmware, every per-symbology enable flag, prefixes/suffixes, and length limits — via `dump_config()` (which sends `Z4`):\n", + "You can read your unit's full configuration over the wire \u2014 model, firmware, every per-symbology enable flag, prefixes/suffixes, and length limits \u2014 via `dump_config()` (which sends `Z4`):\n", "\n", "```python\n", "text = await bcr.driver.dump_config()\n", @@ -435,11 +392,11 @@ "\n", "To **change** which symbologies are enabled, three options in order of practicality:\n", "\n", - "1. **Denso's MDI Setup utility** (free Windows download from Denso ADC) — checkbox UI, talks to the reader over the same serial port, writes to NVRAM.\n", - "2. **Configuration barcodes** printed in the MDI-4050 setting manual — scan a Start → toggle → End sequence; no host required.\n", - "3. Direct register writes via the ESC protocol — possible but the per-symbology byte layout is documented only in Denso's protocol manual.\n", + "1. **Denso's MDI Setup utility** (free Windows download from Denso ADC) \u2014 checkbox UI, talks to the reader over the same serial port, writes to NVRAM.\n", + "2. **Configuration barcodes** printed in the MDI-4050 setting manual \u2014 scan a Start \u2192 toggle \u2192 End sequence; no host required.\n", + "3. Direct register writes via the ESC protocol \u2014 possible but the per-symbology byte layout is documented only in Denso's protocol manual.\n", "\n", - "{class}`~pylabrobot.resources.barcode.Barcode` returned from `scan()` has `symbology=\"ANY 1D\"` by default — the reader doesn't emit a symbology ID unless prefix/suffix bytes are configured to include one." + "{class}`~pylabrobot.resources.barcode.Barcode` returned from `scan()` has `symbology=\"ANY 1D\"` by default \u2014 the reader doesn't emit a symbology ID unless prefix/suffix bytes are configured to include one." ] }, { @@ -503,7 +460,7 @@ "source": [ "print(await bcr.driver.get_software_version())\n", "\n", - "# Full NVRAM dump — model, firmware, all symbology enables, prefixes/suffixes, length limits.\n", + "# Full NVRAM dump \u2014 model, firmware, all symbology enables, prefixes/suffixes, length limits.\n", "config = await bcr.driver.dump_config()\n", "print(config[:200])\n", "\n", @@ -518,7 +475,7 @@ "source": [ "### Teardown\n", "\n", - "`stop()` sends `Y` (trigger off) and `Y2` (reset to a 2s read window), then closes the serial port — leaving the reader in a known state for the next session." + "`stop()` sends `Y` (trigger off) and `Y2` (reset to a 2s read window), then closes the serial port \u2014 leaving the reader in a known state for the next session." ] }, { @@ -571,4 +528,4 @@ }, "nbformat": 4, "nbformat_minor": 5 -} \ No newline at end of file +} diff --git a/docs/user_guide/paa/kx2/proximity-sensor.ipynb b/docs/user_guide/paa/kx2/proximity-sensor.ipynb new file mode 100644 index 00000000000..10724f533db --- /dev/null +++ b/docs/user_guide/paa/kx2/proximity-sensor.ipynb @@ -0,0 +1,120 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "id": "proximity-intro", + "source": [ + "# KX2 Proximity Sensor\n", + "\n", + "The KX2's onboard IR breakbeam between the gripper fingers, wired ", + "to the Z drive's IO header (pin 4 \u2014 auto-discovered as ", + "`\"ProximitySensor\"`). Binary signal: `True` when the beam is ", + "broken (object present). Useful for confirming an object is in ", + "the gripper before closing, or for finding the Z height of an ", + "object below the gripper.\n" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "id": "proximity-setup-header", + "source": [ + "## Setup\n" + ] + }, + { + "cell_type": "code", + "metadata": {}, + "id": "proximity-setup-code", + "execution_count": null, + "outputs": [], + "source": [ + "from pylabrobot.paa.kx2 import KX2\n", + "\n", + "kx2 = KX2()\n", + "await kx2.setup()\n" + ] + }, + { + "cell_type": "markdown", + "id": "proximity-read-md", + "metadata": {}, + "source": [ + "An IR breakbeam between the gripper fingers, wired to the Z drive's IO header. The sensor is binary (no distance, just present/absent) and is exposed via the auto-discovered `\"ProximitySensor\"` digital input on the Z axis (pin 4).\n", + "\n", + "`read_proximity_sensor()` returns `True` when the beam is interrupted. Useful for confirming an object is in the gripper before closing." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "proximity-read", + "metadata": {}, + "outputs": [], + "source": [ + "await kx2.arm.backend.read_proximity_sensor()" + ] + }, + { + "cell_type": "markdown", + "id": "proximity-search-md", + "metadata": {}, + "source": [ + "`find_z_with_proximity_sensor` descends Z up to `max_descent` and halts the instant the beam trips. It arms the Elmo drive's input-logic register so the drive itself stops the motor on the input edge (sub-ms reaction; no software in the loop). Returns the Z where the drive halted; raises `RuntimeError` if the beam never tripped.\n", + "\n", + "Pass `z_start` to first move Z to a known starting height before searching. Position the arm above an object and open the gripper wider than the object before running." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "proximity-search", + "metadata": {}, + "outputs": [], + "source": [ + "z_hit = await kx2.arm.backend.find_z_with_proximity_sensor(max_descent=100.0, z_start=400)\n", + "print(f\"object found at Z = {z_hit:.2f}\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "id": "proximity-teardown-header", + "source": [ + "## Teardown\n" + ] + }, + { + "cell_type": "code", + "metadata": {}, + "id": "proximity-teardown-code", + "execution_count": null, + "outputs": [], + "source": [ + "await kx2.stop()\n" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "env", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.15" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} From 365ec70d03fb50061c9f0f320805606fc1f8d296 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 3 May 2026 19:56:50 -0700 Subject: [PATCH 82/93] KX2: find_z takes z_start/z_end instead of max_descent MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- .../user_guide/paa/kx2/proximity-sensor.ipynb | 61 +++++--- pylabrobot/paa/kx2/arm_backend.py | 45 +++--- .../arm_backend_find_z_proximity_tests.py | 131 +++++++++--------- 3 files changed, 136 insertions(+), 101 deletions(-) diff --git a/docs/user_guide/paa/kx2/proximity-sensor.ipynb b/docs/user_guide/paa/kx2/proximity-sensor.ipynb index 10724f533db..e0972fdb605 100644 --- a/docs/user_guide/paa/kx2/proximity-sensor.ipynb +++ b/docs/user_guide/paa/kx2/proximity-sensor.ipynb @@ -2,33 +2,45 @@ "cells": [ { "cell_type": "markdown", - "metadata": {}, "id": "proximity-intro", + "metadata": {}, "source": [ "# KX2 Proximity Sensor\n", "\n", - "The KX2's onboard IR breakbeam between the gripper fingers, wired ", - "to the Z drive's IO header (pin 4 \u2014 auto-discovered as ", - "`\"ProximitySensor\"`). Binary signal: `True` when the beam is ", - "broken (object present). Useful for confirming an object is in ", - "the gripper before closing, or for finding the Z height of an ", + "The KX2's onboard IR breakbeam between the gripper fingers, wired \n", + "to the Z drive's IO header (pin 4 \u2014 auto-discovered as \n", + "`\"ProximitySensor\"`). Binary signal: `True` when the beam is \n", + "broken (object present). Useful for confirming an object is in \n", + "the gripper before closing, or for finding the Z height of an \n", "object below the gripper.\n" ] }, { "cell_type": "markdown", - "metadata": {}, "id": "proximity-setup-header", + "metadata": {}, "source": [ "## Setup\n" ] }, { "cell_type": "code", - "metadata": {}, + "execution_count": 1, "id": "proximity-setup-code", - "execution_count": null, - "outputs": [], + "metadata": {}, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "uptime library not available, timestamps are relative to boot time and not to Epoch UTC\n", + "2026-05-03 19:52:43,005 - pylabrobot.paa.kx2.driver - INFO - canopen: connected, nodes=[1, 2, 3, 4, 6]\n", + "2026-05-03 19:52:45,964 - pylabrobot.paa.kx2.driver - WARNING - node 1: re-enabling (was disabled)\n", + "2026-05-03 19:52:46,081 - pylabrobot.paa.kx2.driver - WARNING - node 3: re-enabling (was disabled)\n", + "2026-05-03 19:52:46,246 - pylabrobot.paa.kx2.driver - WARNING - node 4: re-enabling (was disabled)\n" + ] + } + ], "source": [ "from pylabrobot.paa.kx2 import KX2\n", "\n", @@ -48,10 +60,21 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, "id": "proximity-read", "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "text/plain": [ + "False" + ] + }, + "execution_count": 2, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "await kx2.arm.backend.read_proximity_sensor()" ] @@ -61,9 +84,9 @@ "id": "proximity-search-md", "metadata": {}, "source": [ - "`find_z_with_proximity_sensor` descends Z up to `max_descent` and halts the instant the beam trips. It arms the Elmo drive's input-logic register so the drive itself stops the motor on the input edge (sub-ms reaction; no software in the loop). Returns the Z where the drive halted; raises `RuntimeError` if the beam never tripped.\n", + "`find_z_with_proximity_sensor(z_start, z_end)` pre-positions Z to `z_start`, then descends toward `z_end`, halting the instant the beam trips. The Elmo drive halts the motor itself on the input edge (sub-ms latency, no software in the loop). Returns the Z where the drive halted; raises `RuntimeError` if the beam never tripped over the full `z_start \u2192 z_end` range.\n", "\n", - "Pass `z_start` to first move Z to a known starting height before searching. Position the arm above an object and open the gripper wider than the object before running." + "Position the arm above an object and open the gripper wider than the object before running.\n" ] }, { @@ -73,23 +96,23 @@ "metadata": {}, "outputs": [], "source": [ - "z_hit = await kx2.arm.backend.find_z_with_proximity_sensor(max_descent=100.0, z_start=400)\n", - "print(f\"object found at Z = {z_hit:.2f}\")" + "z_hit = await kx2.arm.backend.find_z_with_proximity_sensor(z_start=400.0, z_end=300.0)\n", + "print(f\"object found at Z = {z_hit:.2f}\")\n" ] }, { "cell_type": "markdown", - "metadata": {}, "id": "proximity-teardown-header", + "metadata": {}, "source": [ "## Teardown\n" ] }, { "cell_type": "code", - "metadata": {}, - "id": "proximity-teardown-code", "execution_count": null, + "id": "proximity-teardown-code", + "metadata": {}, "outputs": [], "source": [ "await kx2.stop()\n" diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 79e3d87feab..6d7f0ca3fb4 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -645,22 +645,30 @@ async def wait_for_proximity_sensor( async def find_z_with_proximity_sensor( self, - max_descent: float, - z_start: Optional[float] = None, + z_start: float, + z_end: float, max_gripper_speed: float = 25.0, max_gripper_acceleration: float = 100.0, ) -> float: - """Descend Z up to `max_descent`; halt when the IR breakbeam trips. - - `max_gripper_speed`/`max_gripper_acceleration` cap Z descent in mm/s - and mm/s^2 (Z is linear, so gripper speed equals |v_z|). If `z_start` - is given, first move Z to that height (same caps) and search from - there; otherwise search from the current Z. Arms IL[4]=StopForward so - the Elmo drive halts the motor itself on the input edge (sub-ms - latency, no software in the loop). IL is restored to GeneralPurpose - afterwards even if the move raises. Returns the Z position where the - drive halted; raises RuntimeError if the beam never tripped. + """Pre-position to ``z_start``, then descend toward ``z_end``; halt when + the IR breakbeam trips. + + ``z_start > z_end`` (search descends in world frame). The search + bounds are absolute Z so the caller never has to compute a delta. + ``max_gripper_speed`` / ``max_gripper_acceleration`` cap Z motion in + mm/s and mm/s² (Z is linear, so gripper speed equals |v_z|). + + Arms IL[4]=StopForward so the drive halts the motor itself on the + input edge (sub-ms latency, no software in the loop). IL is restored + to GeneralPurpose afterwards even if the move raises. Returns the Z + position where the drive halted; raises RuntimeError if the beam + never tripped (descent ran the full ``z_start → z_end`` range). """ + if z_end >= z_start: + raise ValueError( + f"find_z_with_proximity_sensor: z_end ({z_end}) must be below " + f"z_start ({z_start}) — search descends." + ) move_params = KX2ArmBackend.JointMoveParams( max_gripper_speed=max_gripper_speed, max_gripper_acceleration=max_gripper_acceleration, @@ -676,17 +684,15 @@ async def find_z_with_proximity_sensor( # search could have left it in Fault/Quick Stop where new moves # silently fail (Z barely moves). await self.driver.motors_ensure_enabled([int(Axis.Z)]) - if z_start is not None: - await self._motors_move_joint_locked({Axis.Z: z_start}, params=move_params) - z0 = await self.motor_get_current_position(Axis.Z) + await self._motors_move_joint_locked({Axis.Z: z_start}, params=move_params) if await self.read_proximity_sensor(): - return z0 + return await self.motor_get_current_position(Axis.Z) await self.driver.configure_input_logic( self._PROXIMITY_SENSOR_AXIS, self._PROXIMITY_SENSOR_INPUT, _InputLogic.StopForward, ) move_task = asyncio.create_task( self._motors_move_joint_locked( - {Axis.Z: z0 - max_descent}, params=move_params + {Axis.Z: z_end}, params=move_params ) ) tripped = False @@ -724,9 +730,10 @@ async def find_z_with_proximity_sensor( # `motor_check_if_move_done` raises immediately on a stale flag. self.driver.clear_emcy_state(int(Axis.Z)) if not tripped: - z_end = await self.motor_get_current_position(Axis.Z) + z_actual_end = await self.motor_get_current_position(Axis.Z) raise RuntimeError( - f"proximity sensor never tripped within {max_descent} (Z {z0:.2f} -> {z_end:.2f})" + f"proximity sensor never tripped on Z {z_start:.2f} → {z_end:.2f} " + f"(stopped at {z_actual_end:.2f})" ) return await self.motor_get_current_position(Axis.Z) diff --git a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py index d9b5dd5dd53..4d98e32d444 100644 --- a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py +++ b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py @@ -75,6 +75,7 @@ def _build_harness( z_positions: List[float], move_behavior: str = "long_sleep", move_exception: Optional[Exception] = None, + pre_position_completes: bool = False, ) -> _FindZHarness: """Construct a KX2ArmBackend wired for find_z testing. @@ -88,6 +89,10 @@ def _build_harness( as the task is scheduled — used for the "sensor never trips" test). move_exception: If set, ``_motors_move_joint_locked`` raises this from inside the spawned task before it would otherwise sleep. + pre_position_completes: If True, the first move call (the pre-position + to ``z_start``) completes immediately regardless of ``move_behavior``; + subsequent moves (the descent) follow ``move_behavior``. Without this, + a ``long_sleep`` behavior would block on the awaited pre-position. """ backend = KX2ArmBackend.__new__(KX2ArmBackend) fake_driver = _FakeDriver() @@ -116,6 +121,9 @@ async def _get_z(axis: Axis) -> float: async def _fake_move(cmd_pos: Dict[Axis, float], params: Any = None) -> None: move_calls.append({"cmd_pos": dict(cmd_pos), "params": params}) + is_first = len(move_calls) == 1 + if pre_position_completes and is_first: + return # Pre-position always completes when this flag is set. if move_exception is not None: raise move_exception if move_behavior == "completes_immediately": @@ -132,50 +140,53 @@ async def _fake_move(cmd_pos: Dict[Axis, float], params: Any = None) -> None: class FindZSensorTripsCleanupTests(unittest.TestCase): def test_trip_cancels_descent_and_runs_cleanup(self): """Sensor returns False on the pre-check, then True after one poll - cycle. Verify: descent task is spawned, cancelled, motor_stop runs, - IL is restored to GeneralPurpose.""" + cycle. Verify: pre-position move ran, descent task spawned then + cancelled, motor_stop ran, IL restored to GeneralPurpose.""" h = _build_harness( - # Pre-check (False), then one poll iteration True (trip). + # Pre-position move (completes_immediately so the descent path is + # reached), pre-check sensor=False, poll sensor=True (trip). proximity_script=[False, True], - # z0 read after motor_enable; final z read after the task is cancelled. - z_positions=[100.0, 95.5], + # Z read once at the end after cleanup, returns the stop position. + z_positions=[95.5], + pre_position_completes=True, ) - z = asyncio.run(h.backend.find_z_with_proximity_sensor(max_descent=20.0)) + z = asyncio.run(h.backend.find_z_with_proximity_sensor( + z_start=100.0, z_end=80.0, + )) self.assertAlmostEqual(z, 95.5, places=9) - # IL was armed once, then restored once. arm = (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.StopForward) restore = (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose) self.assertEqual(h.fake_driver.configure_il_calls, [arm, restore]) - # motor_stop ran exactly once on the Z axis. self.assertEqual(h.fake_driver.motor_stop_calls, [Axis.Z]) - # Pre-flight ensured Z was enabled before descent. self.assertEqual(h.fake_driver.ensure_enabled_calls, [(int(Axis.Z),)]) - # Descent target is z0 - max_descent. - self.assertEqual(len(h.move_calls), 1) - self.assertAlmostEqual(h.move_calls[0]["cmd_pos"][Axis.Z], 80.0, places=9) + # Two moves: pre-position to z_start, descent to z_end. + self.assertEqual(len(h.move_calls), 2) + self.assertAlmostEqual(h.move_calls[0]["cmd_pos"][Axis.Z], 100.0, places=9) + self.assertAlmostEqual(h.move_calls[1]["cmd_pos"][Axis.Z], 80.0, places=9) class FindZSensorNeverTripsTests(unittest.TestCase): def test_never_trips_raises_runtime_error_with_range(self): - """Move completes (no trip), sensor stayed False -> raises with the - descent range and start/end Z values in the message.""" + """Descent completes without a trip → raises with z_start, z_end, and + the actual stop position in the message.""" h = _build_harness( - proximity_script=[False], # always False - z_positions=[100.0, 79.5], # z0=100, z_end after descent=79.5 - move_behavior="completes_immediately", + proximity_script=[False], + z_positions=[79.5], + move_behavior="completes_immediately", # both moves complete immediately ) with self.assertRaises(RuntimeError) as ctx: - asyncio.run(h.backend.find_z_with_proximity_sensor(max_descent=20.0)) + asyncio.run(h.backend.find_z_with_proximity_sensor( + z_start=100.0, z_end=80.0, + )) msg = str(ctx.exception) - self.assertIn("proximity sensor never tripped", msg) - self.assertIn("20.0", msg) + self.assertIn("never tripped", msg) self.assertIn("100.00", msg) + self.assertIn("80.00", msg) self.assertIn("79.50", msg) - # Cleanup still ran. self.assertEqual(h.fake_driver.motor_stop_calls, [Axis.Z]) self.assertIn( (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose), @@ -185,22 +196,21 @@ def test_never_trips_raises_runtime_error_with_range(self): class FindZMoveRaisesCanErrorTests(unittest.TestCase): def test_canerror_in_descent_is_swallowed_cleanup_runs(self): - """The descent task raises CanError. ``await move_task`` swallows it - (per the ``except (asyncio.CancelledError, CanError): pass``). Sensor - stayed False -> the function then raises the standard RuntimeError - *after* cleanup. motor_stop + IL restore still ran.""" + """Descent task raises CanError. ``await move_task`` swallows it; sensor + stayed False, so the function raises RuntimeError *after* cleanup. + motor_stop + IL restore still ran.""" h = _build_harness( proximity_script=[False], - z_positions=[100.0, 100.0], + z_positions=[100.0], move_exception=CanError("synthetic descent failure"), + pre_position_completes=True, # Only the descent should raise, not the pre-position. ) with self.assertRaises(RuntimeError) as ctx: - asyncio.run(h.backend.find_z_with_proximity_sensor(max_descent=20.0)) + asyncio.run(h.backend.find_z_with_proximity_sensor( + z_start=100.0, z_end=80.0, + )) - # The post-cleanup "never tripped" branch runs because the swallow - # at `await move_task` masked the CanError; sensor stayed False so - # `tripped` is False. self.assertIn("never tripped", str(ctx.exception)) self.assertEqual(h.fake_driver.motor_stop_calls, [Axis.Z]) self.assertEqual( @@ -213,14 +223,16 @@ def test_motor_stop_failure_is_logged_not_raised(self): the function still completes its happy-path return (sensor tripped).""" h = _build_harness( proximity_script=[False, True], - z_positions=[100.0, 95.5], + z_positions=[95.5], + pre_position_completes=True, ) h.fake_driver.motor_stop_should_raise = CanError("synthetic stop failure") - z = asyncio.run(h.backend.find_z_with_proximity_sensor(max_descent=20.0)) + z = asyncio.run(h.backend.find_z_with_proximity_sensor( + z_start=100.0, z_end=80.0, + )) self.assertAlmostEqual(z, 95.5, places=9) - # IL restore still ran despite motor_stop raising. self.assertEqual( h.fake_driver.configure_il_calls[-1], (Axis.Z, h.backend._PROXIMITY_SENSOR_INPUT, _InputLogic.GeneralPurpose), @@ -228,49 +240,42 @@ def test_motor_stop_failure_is_logged_not_raised(self): class FindZAlreadyTrippedTests(unittest.TestCase): - def test_already_tripped_short_circuits_no_il_change(self): - """The pre-check sees the sensor already tripped: returns z0 without - configuring IL or spawning a descent task.""" - h = _build_harness( - proximity_script=[True], # tripped on the very first read - z_positions=[42.0], - ) - - z = asyncio.run(h.backend.find_z_with_proximity_sensor(max_descent=20.0)) - - self.assertAlmostEqual(z, 42.0, places=9) - # No IL configure calls (neither arm nor restore) — short-circuit hits - # before the StopForward arm. - self.assertEqual(h.fake_driver.configure_il_calls, []) - # No motor_stop, no descent move spawned. - self.assertEqual(h.fake_driver.motor_stop_calls, []) - self.assertEqual(h.move_calls, []) - # ensure_enabled still ran (it's the pre-flight before anything else). - self.assertEqual(h.fake_driver.ensure_enabled_calls, [(int(Axis.Z),)]) - - def test_z_start_provided_runs_pre_descent_move_then_short_circuits(self): - """Same as above but with z_start: the pre-positioning move runs, then - the sensor reads tripped on the pre-check -> still no IL arm or - descent task.""" + def test_pre_check_already_tripped_skips_descent(self): + """After the pre-position move, the sensor reads tripped: returns + current Z without arming IL or spawning a descent task.""" h = _build_harness( proximity_script=[True], z_positions=[55.0], - # Pre-positioning move must complete on its own (it's awaited inline, - # not spawned as a cancellable task). long_sleep would deadlock. - move_behavior="completes_immediately", + move_behavior="completes_immediately", # pre-position completes, then trip ) z = asyncio.run(h.backend.find_z_with_proximity_sensor( - max_descent=20.0, z_start=80.0, + z_start=80.0, z_end=20.0, )) self.assertAlmostEqual(z, 55.0, places=9) - # The pre-position move ran (z_start path), but no descent move. + # Pre-position ran, but no descent — only one move call. self.assertEqual(len(h.move_calls), 1) self.assertAlmostEqual(h.move_calls[0]["cmd_pos"][Axis.Z], 80.0, places=9) - # Still no IL change, still no motor_stop. + # No IL configure (short-circuit hits before the StopForward arm), + # no motor_stop. ensure_enabled still ran as pre-flight. self.assertEqual(h.fake_driver.configure_il_calls, []) self.assertEqual(h.fake_driver.motor_stop_calls, []) + self.assertEqual(h.fake_driver.ensure_enabled_calls, [(int(Axis.Z),)]) + + +class FindZArgValidationTests(unittest.TestCase): + def test_z_end_above_z_start_raises(self): + """Search must descend (z_end < z_start). Surface the typo before any + drive interaction.""" + h = _build_harness(proximity_script=[False], z_positions=[0.0]) + with self.assertRaises(ValueError): + asyncio.run(h.backend.find_z_with_proximity_sensor( + z_start=80.0, z_end=100.0, + )) + # Nothing happened on the drive side. + self.assertEqual(h.fake_driver.ensure_enabled_calls, []) + self.assertEqual(h.move_calls, []) if __name__ == "__main__": From acd491fbb471a6f638429e499c4521477ce896d7 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 3 May 2026 20:02:53 -0700 Subject: [PATCH 83/93] KX2: add find_with_proximity_sensor for generic Cartesian sweeps MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit `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) --- pylabrobot/paa/kx2/arm_backend.py | 81 +++++++++++ .../arm_backend_find_z_proximity_tests.py | 129 ++++++++++++++++++ 2 files changed, 210 insertions(+) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 6d7f0ca3fb4..648ecffe6e8 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -737,6 +737,87 @@ async def find_z_with_proximity_sensor( ) return await self.motor_get_current_position(Axis.Z) + async def find_with_proximity_sensor( + self, + start: Coordinate, + end: Coordinate, + direction: float, + *, + max_gripper_speed: float = 25.0, + max_gripper_acceleration: float = 100.0, + ) -> Coordinate: + """Sweep the gripper from ``start`` to ``end`` along a straight Cartesian + line; halt when the IR breakbeam trips. Yaw is held at ``direction`` + for the whole sweep. Returns the gripper location at halt; raises + ``RuntimeError`` if the beam never tripped over the full path. + + Generic Cartesian counterpart to :meth:`find_z_with_proximity_sensor`. + The Z-only descent has a hardware fast-path (``IL[4]=StopForward`` on + the Z drive halts the motor sub-ms on the input edge); X/Y motion can't + use that — the breakbeam is wired only to the Z drive's I/O — so this + method polls the sensor in software (~10 ms latency at 100 Hz). At the + default 25 mm/s sweep that's ~0.25 mm of overshoot before cancellation, + plus ~64 ms of post-cancel buffer-drain (8 PVT frames × 8 ms). For + Z-only descents where mm-precision into labware matters, prefer + ``find_z_with_proximity_sensor``. + """ + move_params = KX2ArmBackend.CartesianMoveParams( + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + path="linear", + ) + start_pose = GripperLocation(location=start, rotation=Rotation(z=direction)) + end_pose = GripperLocation(location=end, rotation=Rotation(z=direction)) + + async with self._motion_guard(): + await self.driver.motors_ensure_enabled([int(a) for a in MOTION_AXES]) + # Pre-position to start (joint move — path doesn't matter, just get there). + pre_pos = await self._cart_to_joints(start_pose) + await self._motors_move_joint_locked( + cmd_pos=pre_pos, + params=KX2ArmBackend.JointMoveParams( + max_gripper_speed=max_gripper_speed, + max_gripper_acceleration=max_gripper_acceleration, + ), + ) + if await self.read_proximity_sensor(): + return (await self.request_gripper_location()).location + + sweep_task = asyncio.create_task( + self._run_linear_path(end_pose, move_params) + ) + tripped = False + try: + # Drive runs the streamed PVT trajectory; we poll the sensor in + # parallel and cancel on trip. The sweep task's finally clause + # then sends ipm_stop + reverts to PPM, leaving the drive ready + # for the next motion call. + while not sweep_task.done(): + if await self.read_proximity_sensor(): + tripped = True + break + await asyncio.sleep(0.01) + sweep_task.cancel() + try: + await sweep_task + except (asyncio.CancelledError, CanError): + pass + finally: + # PVT-stop coasts up to 8 frames; halt() is too aggressive (drops + # all motion-axis MO=0). The sweep task's own cleanup already + # handled IPM teardown; clear sticky EMCY so next motion call's + # `motor_check_if_move_done` doesn't raise on the stop-induced frame. + self.driver.clear_emcy_state() + if not tripped: + end_loc = (await self.request_gripper_location()).location + raise RuntimeError( + f"proximity sensor never tripped on " + f"({start.x:.1f},{start.y:.1f},{start.z:.1f}) → " + f"({end.x:.1f},{end.y:.1f},{end.z:.1f}) " + f"(stopped at ({end_loc.x:.1f},{end_loc.y:.1f},{end_loc.z:.1f}))" + ) + return (await self.request_gripper_location()).location + async def motors_move_joint( self, cmd_pos: Dict[Axis, float], diff --git a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py index 4d98e32d444..c1a427744cd 100644 --- a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py +++ b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py @@ -278,5 +278,134 @@ def test_z_end_above_z_start_raises(self): self.assertEqual(h.move_calls, []) +class _FindGenericHarness(NamedTuple): + backend: KX2ArmBackend + fake_driver: _FakeDriver + joint_move_calls: List[Dict[str, Any]] + linear_move_calls: List[Dict[str, Any]] + + +def _build_generic_harness( + *, + proximity_script: List[bool], + gripper_locations: List[Any], + linear_behavior: str = "long_sleep", +) -> _FindGenericHarness: + """Harness for ``find_with_proximity_sensor``. Fakes _run_linear_path + (the IPM streaming runtime), _cart_to_joints (IK), and + request_gripper_location (FK) so the test exercises the polling-and- + cancel logic without spinning up a real driver.""" + from pylabrobot.capabilities.arms.standard import GripperLocation + from pylabrobot.resources import Rotation + + backend = KX2ArmBackend.__new__(KX2ArmBackend) + fake_driver = _FakeDriver() + backend.driver = fake_driver # type: ignore[assignment] + backend._motion_lock = asyncio.Lock() + backend._motion_owner = None + backend._gripper_params = None # type: ignore[assignment] + + prox = list(proximity_script) + locs = list(gripper_locations) + + async def _read_proximity() -> bool: + await asyncio.sleep(0) + return prox[0] if len(prox) == 1 else prox.pop(0) + + async def _request_gripper_location(): + val = locs[0] if len(locs) == 1 else locs.pop(0) + if isinstance(val, GripperLocation): + return val + return GripperLocation(location=val, rotation=Rotation(z=0)) + + async def _cart_to_joints(pose): + return {Axis.SHOULDER: 0.0, Axis.Z: pose.location.z, Axis.ELBOW: 100.0, + Axis.WRIST: pose.rotation.z} + + joint_calls: List[Dict[str, Any]] = [] + linear_calls: List[Dict[str, Any]] = [] + + async def _fake_joint_move(cmd_pos, params=None): + joint_calls.append({"cmd_pos": dict(cmd_pos), "params": params}) + + async def _fake_run_linear(target_pose, params): + linear_calls.append({"pose": target_pose, "params": params}) + if linear_behavior == "completes_immediately": + return + try: + await asyncio.sleep(10.0) + finally: + # Mirror the real method's finally-block behavior. + pass + + backend.read_proximity_sensor = _read_proximity # type: ignore[assignment] + backend.request_gripper_location = _request_gripper_location # type: ignore[assignment] + backend._cart_to_joints = _cart_to_joints # type: ignore[assignment] + backend._motors_move_joint_locked = _fake_joint_move # type: ignore[assignment] + backend._run_linear_path = _fake_run_linear # type: ignore[assignment] + return _FindGenericHarness(backend, fake_driver, joint_calls, linear_calls) + + +class FindGenericTests(unittest.TestCase): + def test_trip_cancels_sweep_returns_gripper_location(self): + """Sensor trips during the sweep: linear-path task is cancelled, + EMCY state cleared, and the gripper location at halt is returned.""" + from pylabrobot.resources import Coordinate + h = _build_generic_harness( + proximity_script=[False, True], + gripper_locations=[Coordinate(x=110.0, y=200.0, z=50.0)], + ) + + result = asyncio.run(h.backend.find_with_proximity_sensor( + start=Coordinate(x=100, y=200, z=50), + end=Coordinate(x=200, y=200, z=50), + direction=0.0, + )) + + self.assertAlmostEqual(result.x, 110.0, places=6) + self.assertEqual(len(h.joint_move_calls), 1) # Pre-position only. + self.assertEqual(len(h.linear_move_calls), 1) # Sweep itself. + # Sticky EMCY cleared so the next motion call doesn't raise on a + # halt-induced frame. + self.assertEqual(h.fake_driver.clear_emcy_calls, [None]) + + def test_never_trips_raises_with_endpoints(self): + from pylabrobot.resources import Coordinate + h = _build_generic_harness( + proximity_script=[False], + gripper_locations=[Coordinate(x=200.0, y=200.0, z=50.0)], + linear_behavior="completes_immediately", + ) + + with self.assertRaises(RuntimeError) as ctx: + asyncio.run(h.backend.find_with_proximity_sensor( + start=Coordinate(x=100, y=200, z=50), + end=Coordinate(x=200, y=200, z=50), + direction=0.0, + )) + + msg = str(ctx.exception) + self.assertIn("never tripped", msg) + self.assertIn("100.0", msg) + self.assertIn("200.0", msg) + + def test_pre_check_already_tripped_skips_sweep(self): + from pylabrobot.resources import Coordinate + h = _build_generic_harness( + proximity_script=[True], + gripper_locations=[Coordinate(x=100.0, y=200.0, z=50.0)], + ) + + result = asyncio.run(h.backend.find_with_proximity_sensor( + start=Coordinate(x=100, y=200, z=50), + end=Coordinate(x=200, y=200, z=50), + direction=0.0, + )) + + self.assertAlmostEqual(result.x, 100.0, places=6) + self.assertEqual(len(h.joint_move_calls), 1) # Pre-position ran. + self.assertEqual(len(h.linear_move_calls), 0) # No sweep. + + if __name__ == "__main__": unittest.main() From 7a495f73fa495526f958999a79c5bc0f488ac54f Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 3 May 2026 20:02:59 -0700 Subject: [PATCH 84/93] KX2: split barcode reader docs into its own notebook 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) --- docs/user_guide/paa/index.md | 1 + docs/user_guide/paa/kx2/barcode-reader.ipynb | 175 +++++++++++ docs/user_guide/paa/kx2/hello-world.ipynb | 299 +++++++++---------- 3 files changed, 321 insertions(+), 154 deletions(-) create mode 100644 docs/user_guide/paa/kx2/barcode-reader.ipynb diff --git a/docs/user_guide/paa/index.md b/docs/user_guide/paa/index.md index 75831b573de..12881ce2044 100644 --- a/docs/user_guide/paa/index.md +++ b/docs/user_guide/paa/index.md @@ -5,4 +5,5 @@ kx2/hello-world kx2/proximity-sensor +kx2/barcode-reader ``` diff --git a/docs/user_guide/paa/kx2/barcode-reader.ipynb b/docs/user_guide/paa/kx2/barcode-reader.ipynb new file mode 100644 index 00000000000..1e070c855f8 --- /dev/null +++ b/docs/user_guide/paa/kx2/barcode-reader.ipynb @@ -0,0 +1,175 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "kx2-bcr-intro", + "metadata": {}, + "source": [ + "# KX2 Barcode Reader\n", + "\n", + "The KX2's onboard barcode reader is a serial 1D laser scanner wired to the controller PC. It's fully independent of the CAN motor stack, so it works even with the arm e-stopped \u2014 exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`.\n", + "\n", + "The model that ships with KX2 systems we've inspected is a **Denso MDI-4050** (firmware reports as `BD01J09`). Factory defaults: **9600 8N1**, no flow control. Find your port with `python -m serial.tools.list_ports -v`.\n", + "\n", + "### macOS: USB-to-serial driver\n", + "\n", + "The reader cable is typically a Prolific PL2303. macOS bundles a driver for older variants but not the newer GC/HXN chip (USB ID `067b:23a3`). If the device doesn't show up at `/dev/tty.PL2303G-USBtoUART` after plugging it in:\n", + "\n", + "```bash\n", + "brew install --cask prolific-pl2303\n", + "open -a PL2303Serial # registers the system extension\n", + "```\n", + "\n", + "Then **System Settings \u2192 Privacy & Security \u2192 Allow** on the \"System software from PL2303Serial was blocked\" prompt, restart if asked, and replug the cable. Verify with `systemextensionsctl list`: `com.prolific.cdc.PLCdcFSDriver` should show `[activated enabled]`.\n", + "\n", + "### Symbologies\n", + "\n", + "The MDI-4050 is a 1D laser, so **PDF417 / DataMatrix / QR Code aren't supported** \u2014 you'd need the 2D imager variants (e.g. MDI-4150) for those.\n", + "\n", + "Typical out-of-the-box 1D enables: UPC-A, UPC-E, EAN-13, EAN-8, Code 39, Tri-Optic, Codabar, Industrial 2/5, Interleaved 2/5, IATA, Code 128, Code 93, GS1 DataBar, GS1 DataBar Limited. Add-on (supplemental) codes, postal codes, MSI/Plessey, Telepen, UK/Plessey, and Code 11 are off by default.\n", + "\n", + "You can read your unit's full configuration over the wire \u2014 model, firmware, every per-symbology enable flag, prefixes/suffixes, and length limits \u2014 via `dump_config()` (which sends `Z4`):\n", + "\n", + "```python\n", + "text = await bcr.driver.dump_config()\n", + "print(text[:200]) # header has MODEL / ROM Ver. / I/F\n", + "```\n", + "\n", + "To **change** which symbologies are enabled, three options in order of practicality:\n", + "\n", + "1. **Denso's MDI Setup utility** (free Windows download from Denso ADC) \u2014 checkbox UI, talks to the reader over the same serial port, writes to NVRAM.\n", + "2. **Configuration barcodes** printed in the MDI-4050 setting manual \u2014 scan a Start \u2192 toggle \u2192 End sequence; no host required.\n", + "3. Direct register writes via the ESC protocol \u2014 possible but the per-symbology byte layout is documented only in Denso's protocol manual.\n", + "\n", + "{class}`~pylabrobot.resources.barcode.Barcode` returned from `scan()` has `symbology=\"ANY 1D\"` by default \u2014 the reader doesn't emit a symbology ID unless prefix/suffix bytes are configured to include one." + ] + }, + { + "cell_type": "markdown", + "id": "kx2-bcr-setup-header", + "metadata": {}, + "source": [ + "## Setup\n", + "\n", + "Pass the serial port path. `setup()` opens the port, does the `Z1` software-version handshake, and puts the reader into single-trigger mode." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-bcr-setup-code", + "metadata": {}, + "outputs": [], + "source": [ + "from pylabrobot.paa.kx2 import KX2BarcodeReader\n", + "\n", + "bcr = KX2BarcodeReader(port=\"/dev/tty.PL2303G-USBtoUART11240\")\n", + "await bcr.setup()\n" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-bcr-scan-md", + "metadata": {}, + "source": [ + "`scan(read_time=...)` fires the trigger and waits up to `read_time + 1s` for the reader to decode something. Omit `read_time` to use whatever read window is currently programmed on the reader. Returns a {class}`~pylabrobot.resources.barcode.Barcode`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-bcr-scan-code", + "metadata": {}, + "outputs": [], + "source": [ + "barcode = await bcr.barcode_scanning.scan(read_time=8)\n", + "print(barcode.data)" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-bcr-config-md", + "metadata": {}, + "source": [ + "Change the trigger mode at runtime:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-bcr-config-code", + "metadata": {}, + "outputs": [], + "source": [ + "# \"single\" (default), \"multiple\" (two reads per trigger), or \"continuous\".\n", + "# await bcr.set_read_mode(\"continuous\")" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-bcr-driver-md", + "metadata": {}, + "source": [ + "For raw vendor commands or the auto-trigger mode (reader fires on its own), go through `bcr.driver`:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-bcr-driver-code", + "metadata": {}, + "outputs": [], + "source": [ + "print(await bcr.driver.get_software_version())\n", + "\n", + "# Full NVRAM dump \u2014 model, firmware, all symbology enables, prefixes/suffixes, length limits.\n", + "config = await bcr.driver.dump_config()\n", + "print(config[:200])\n", + "\n", + "# await bcr.driver.set_auto_trigger(True) # reader scans on its own\n", + "# await bcr.driver.set_auto_trigger(False)" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-bcr-teardown-header", + "metadata": {}, + "source": [ + "## Teardown\n", + "\n", + "`stop()` sends `Y` (trigger off) and `Y2` (reset to a 2s read window), then closes the serial port \u2014 leaving the reader in a known state for the next session." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "kx2-bcr-teardown-code", + "metadata": {}, + "outputs": [], + "source": [ + "await bcr.stop()\n" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "env", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.15" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 9c3e2975b9d..190da015787 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -30,10 +30,23 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, "id": "kx2-setup-code", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "uptime library not available, timestamps are relative to boot time and not to Epoch UTC\n", + "2026-05-03 19:56:58,933 - pylabrobot.paa.kx2.driver - INFO - canopen: connected, nodes=[1, 2, 3, 4, 6]\n", + "2026-05-03 19:57:01,871 - pylabrobot.paa.kx2.driver - WARNING - node 1: re-enabling (was disabled)\n", + "2026-05-03 19:57:02,082 - pylabrobot.paa.kx2.driver - WARNING - motor_enable(state=True) attempt 1/20 failed for node 1 (MO=0); retrying\n", + "2026-05-03 19:57:02,704 - pylabrobot.paa.kx2.driver - WARNING - node 3: re-enabling (was disabled)\n", + "2026-05-03 19:57:02,835 - pylabrobot.paa.kx2.driver - WARNING - node 4: re-enabling (was disabled)\n" + ] + } + ], "source": [ "from pylabrobot.paa.kx2 import KX2\n", "\n", @@ -65,17 +78,17 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 4, "id": "kx2-gripper-open", "metadata": {}, "outputs": [], "source": [ - "await kx2.arm.open_gripper(gripper_width=30)" + "await kx2.arm.open_gripper(gripper_width=25)" ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 5, "id": "kx2-gripper-close", "metadata": {}, "outputs": [], @@ -90,10 +103,21 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 6, "id": "kx2-gripper-status", "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "text/plain": [ + "True" + ] + }, + "execution_count": 6, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "await kx2.arm.is_gripper_closed()" ] @@ -102,28 +126,56 @@ "cell_type": "markdown", "id": "kx2-gripper-force-md", "metadata": {}, - "source": "Cap the gripping force on a per-close basis (as a percentage of the motor's peak current \u2014 10..100, clamped) when closing on fragile labware:" + "source": [ + "Cap the gripping force on a per-close basis (as a percentage of the motor's peak current \u2014 10..100, clamped) when closing on fragile labware:" + ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 7, "id": "kx2-gripper-force", "metadata": {}, "outputs": [], - "source": "await kx2.arm.close_gripper(\n gripper_width=0,\n backend_params=KX2ArmBackend.GripParams(max_force_percent=30),\n)" + "source": [ + "await kx2.arm.close_gripper(\n", + " gripper_width=0,\n", + " backend_params=KX2ArmBackend.GripParams(max_force_percent=30),\n", + ")" + ] }, { "cell_type": "markdown", "id": "kx2-motion-limits-md", "metadata": {}, - "source": "### Motion limits\n\n`motion_limits()` reads the per-axis firmware maxima cached at setup. `JointMoveParams` / `CartesianMoveParams` take `max_gripper_speed` (mm/s) and `max_gripper_acceleration` (mm/s^2), which cap the worst-case Cartesian speed/acceleration at the gripper along the trajectory. `None` (the default) means no Cartesian cap \u2014 joints run at firmware max. Joint speeds get scaled uniformly so the gripper stays under the cap." + "source": [ + "### Motion limits\n", + "\n", + "`motion_limits()` reads the per-axis firmware maxima cached at setup. `JointMoveParams` / `CartesianMoveParams` take `max_gripper_speed` (mm/s) and `max_gripper_acceleration` (mm/s^2), which cap the worst-case Cartesian speed/acceleration at the gripper along the trajectory. `None` (the default) means no Cartesian cap \u2014 joints run at firmware max. Joint speeds get scaled uniformly so the gripper stays under the cap." + ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 8, "id": "kx2-motion-limits", "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "text/plain": [ + "axis max speed max acceleration\n", + "------------- ------------ ----------------\n", + "SHOULDER 145.00 deg/s 300.00 deg/s^2 \n", + "Z 750.00 mm/s 1000.00 mm/s^2 \n", + "ELBOW 80.00 deg/s 180.00 deg/s^2 \n", + "WRIST 500.00 deg/s 1000.04 deg/s^2 \n", + "SERVO_GRIPPER 47.00 mm/s 200.00 mm/s^2 " + ] + }, + "execution_count": 8, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "kx2.arm.backend.motion_limits()" ] @@ -132,25 +184,70 @@ "cell_type": "markdown", "id": "kx2-cartesian-header", "metadata": {}, - "source": "### Cartesian movement\n\nMove the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only \u2014 the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity and acceleration." + "source": [ + "### Cartesian movement\n", + "\n", + "Move the arm to a Cartesian location. `direction` is the gripper yaw in degrees (Z-axis rotation only \u2014 the KX2 cannot roll or pitch). Use {class}`~pylabrobot.paa.kx2.KX2ArmBackend.CartesianMoveParams` to override velocity and acceleration." + ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 9, "id": "11b06c86", "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "text/plain": [ + "GripperLocation(location=Coordinate(x=312.2642, y=564.2383, z=333.7532), rotation=Rotation(x=0, y=0, z=265.67006324798695))" + ] + }, + "execution_count": 9, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ "await kx2.arm.request_gripper_location()" ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 15, "id": "kx2-cartesian-code", "metadata": {}, "outputs": [], - "source": "from pylabrobot.resources import Coordinate\n\nawait kx2.arm.move_to_location(\n location=Coordinate(x=330.124, y=210.552, z=320.0441),\n direction=198,\n backend_params=KX2ArmBackend.CartesianMoveParams(\n max_gripper_speed=200.0, max_gripper_acceleration=1000.0,\n ),\n)" + "source": [ + "from pylabrobot.resources import Coordinate\n", + "\n", + "await kx2.arm.move_to_location(\n", + " location=Coordinate(x=330.124, y=210.552, z=320.0441),\n", + " direction=198,\n", + " backend_params=KX2ArmBackend.CartesianMoveParams(\n", + " max_gripper_speed=200.0, max_gripper_acceleration=1000.0,\n", + " ),\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "0a41fece", + "metadata": {}, + "outputs": [], + "source": [ + "from pylabrobot.resources import Coordinate\n", + "\n", + "await kx2.arm.move_to_location(\n", + " location=Coordinate(x=130.124, y=210.552, z=420.0441),\n", + " direction=100,\n", + " backend_params=KX2ArmBackend.CartesianMoveParams(\n", + " max_gripper_speed=200.0,\n", + " max_gripper_acceleration=1000.0,\n", + " path=\"joint\"\n", + " ),\n", + ")" + ] }, { "cell_type": "markdown", @@ -168,11 +265,38 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "id": "kx2-joint-code", "metadata": {}, - "outputs": [], - "source": "from pylabrobot.paa.kx2 import KX2ArmBackend\n\nposition = {\n KX2ArmBackend.Axis.SHOULDER: 0.0,\n KX2ArmBackend.Axis.Z: 150.0,\n KX2ArmBackend.Axis.ELBOW: 90.0,\n KX2ArmBackend.Axis.WRIST: 0.0,\n}\nawait kx2.arm.backend.move_to_joint_position(\n position,\n backend_params=KX2ArmBackend.JointMoveParams(\n max_gripper_speed=200.0, max_gripper_acceleration=1000.0,\n ),\n)" + "outputs": [ + { + "ename": "AttributeError", + "evalue": "type object 'KX2ArmBackend' has no attribute 'Axis'", + "output_type": "error", + "traceback": [ + "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", + "\u001b[0;31mAttributeError\u001b[0m Traceback (most recent call last)", + "Cell \u001b[0;32mIn[1], line 4\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mpylabrobot\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mpaa\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mkx2\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m KX2ArmBackend\n\u001b[1;32m 3\u001b[0m position \u001b[38;5;241m=\u001b[39m {\n\u001b[0;32m----> 4\u001b[0m \u001b[43mKX2ArmBackend\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mAxis\u001b[49m\u001b[38;5;241m.\u001b[39mSHOULDER: \u001b[38;5;241m0.0\u001b[39m,\n\u001b[1;32m 5\u001b[0m KX2ArmBackend\u001b[38;5;241m.\u001b[39mAxis\u001b[38;5;241m.\u001b[39mZ: \u001b[38;5;241m150.0\u001b[39m,\n\u001b[1;32m 6\u001b[0m KX2ArmBackend\u001b[38;5;241m.\u001b[39mAxis\u001b[38;5;241m.\u001b[39mELBOW: \u001b[38;5;241m90.0\u001b[39m,\n\u001b[1;32m 7\u001b[0m KX2ArmBackend\u001b[38;5;241m.\u001b[39mAxis\u001b[38;5;241m.\u001b[39mWRIST: \u001b[38;5;241m0.0\u001b[39m,\n\u001b[1;32m 8\u001b[0m }\n\u001b[1;32m 9\u001b[0m \u001b[38;5;28;01mawait\u001b[39;00m kx2\u001b[38;5;241m.\u001b[39marm\u001b[38;5;241m.\u001b[39mbackend\u001b[38;5;241m.\u001b[39mmove_to_joint_position(\n\u001b[1;32m 10\u001b[0m position,\n\u001b[1;32m 11\u001b[0m backend_params\u001b[38;5;241m=\u001b[39mKX2ArmBackend\u001b[38;5;241m.\u001b[39mJointMoveParams(\n\u001b[1;32m 12\u001b[0m max_gripper_speed\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m200.0\u001b[39m, max_gripper_acceleration\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m1000.0\u001b[39m,\n\u001b[1;32m 13\u001b[0m ),\n\u001b[1;32m 14\u001b[0m )\n", + "\u001b[0;31mAttributeError\u001b[0m: type object 'KX2ArmBackend' has no attribute 'Axis'" + ] + } + ], + "source": [ + "from pylabrobot.paa.kx2 import KX2ArmBackend\n", + "\n", + "position = {\n", + " KX2ArmBackend.Axis.SHOULDER: 0.0,\n", + " KX2ArmBackend.Axis.Z: 150.0,\n", + " KX2ArmBackend.Axis.ELBOW: 90.0,\n", + " KX2ArmBackend.Axis.WRIST: 0.0,\n", + "}\n", + "await kx2.arm.backend.move_to_joint_position(\n", + " position,\n", + " backend_params=KX2ArmBackend.JointMoveParams(\n", + " max_gripper_speed=200.0, max_gripper_acceleration=1000.0,\n", + " ),\n", + ")" + ] }, { "cell_type": "markdown", @@ -355,139 +479,6 @@ "await kx2.driver.motor_get_fault(KX2ArmBackend.Axis.SHOULDER)" ] }, - { - "cell_type": "markdown", - "id": "9e9f0bed", - "metadata": {}, - "source": [ - "## Barcode reader\n", - "\n", - "The KX2's onboard barcode reader is a serial 1D laser scanner wired to the controller PC. It's fully independent of the CAN motor stack, so it works even with the arm e-stopped \u2014 exposed as its own {class}`~pylabrobot.paa.kx2.KX2BarcodeReader` `Device`.\n", - "\n", - "The model that ships with KX2 systems we've inspected is a **Denso MDI-4050** (firmware reports as `BD01J09`). Factory defaults: **9600 8N1**, no flow control. Find your port with `python -m serial.tools.list_ports -v`.\n", - "\n", - "### macOS: USB-to-serial driver\n", - "\n", - "The reader cable is typically a Prolific PL2303. macOS bundles a driver for older variants but not the newer GC/HXN chip (USB ID `067b:23a3`). If the device doesn't show up at `/dev/tty.PL2303G-USBtoUART` after plugging it in:\n", - "\n", - "```bash\n", - "brew install --cask prolific-pl2303\n", - "open -a PL2303Serial # registers the system extension\n", - "```\n", - "\n", - "Then **System Settings \u2192 Privacy & Security \u2192 Allow** on the \"System software from PL2303Serial was blocked\" prompt, restart if asked, and replug the cable. Verify with `systemextensionsctl list`: `com.prolific.cdc.PLCdcFSDriver` should show `[activated enabled]`.\n", - "\n", - "### Symbologies\n", - "\n", - "The MDI-4050 is a 1D laser, so **PDF417 / DataMatrix / QR Code aren't supported** \u2014 you'd need the 2D imager variants (e.g. MDI-4150) for those.\n", - "\n", - "Typical out-of-the-box 1D enables: UPC-A, UPC-E, EAN-13, EAN-8, Code 39, Tri-Optic, Codabar, Industrial 2/5, Interleaved 2/5, IATA, Code 128, Code 93, GS1 DataBar, GS1 DataBar Limited. Add-on (supplemental) codes, postal codes, MSI/Plessey, Telepen, UK/Plessey, and Code 11 are off by default.\n", - "\n", - "You can read your unit's full configuration over the wire \u2014 model, firmware, every per-symbology enable flag, prefixes/suffixes, and length limits \u2014 via `dump_config()` (which sends `Z4`):\n", - "\n", - "```python\n", - "text = await bcr.driver.dump_config()\n", - "print(text[:200]) # header has MODEL / ROM Ver. / I/F\n", - "```\n", - "\n", - "To **change** which symbologies are enabled, three options in order of practicality:\n", - "\n", - "1. **Denso's MDI Setup utility** (free Windows download from Denso ADC) \u2014 checkbox UI, talks to the reader over the same serial port, writes to NVRAM.\n", - "2. **Configuration barcodes** printed in the MDI-4050 setting manual \u2014 scan a Start \u2192 toggle \u2192 End sequence; no host required.\n", - "3. Direct register writes via the ESC protocol \u2014 possible but the per-symbology byte layout is documented only in Denso's protocol manual.\n", - "\n", - "{class}`~pylabrobot.resources.barcode.Barcode` returned from `scan()` has `symbology=\"ANY 1D\"` by default \u2014 the reader doesn't emit a symbology ID unless prefix/suffix bytes are configured to include one." - ] - }, - { - "cell_type": "markdown", - "id": "7d393b50", - "metadata": {}, - "source": "### Setup\n\nPass the serial port path. `setup()` opens the port, does the `Z1` software-version handshake, and puts the reader into single-trigger mode." - }, - { - "cell_type": "code", - "execution_count": null, - "id": "52e6a377", - "metadata": {}, - "outputs": [], - "source": "from pylabrobot.paa.kx2 import KX2BarcodeReader\n\nbcr = KX2BarcodeReader(port=\"/dev/tty.PL2303G-USBtoUART11240\")\nawait bcr.setup()" - }, - { - "cell_type": "markdown", - "id": "665c811a", - "metadata": {}, - "source": "### Scan\n\n`scan(read_time=...)` fires the trigger and waits up to `read_time + 1s` for the reader to decode something. Omit `read_time` to use whatever read window is currently programmed on the reader. Returns a {class}`~pylabrobot.resources.barcode.Barcode`." - }, - { - "cell_type": "code", - "execution_count": null, - "id": "8ba1d44d", - "metadata": {}, - "outputs": [], - "source": "barcode = await bcr.barcode_scanning.scan(read_time=8)\nprint(barcode.data)" - }, - { - "cell_type": "markdown", - "id": "5282deb9", - "metadata": {}, - "source": "### Configuration\n\nChange the trigger mode at runtime:" - }, - { - "cell_type": "code", - "execution_count": null, - "id": "63db140e", - "metadata": {}, - "outputs": [], - "source": "# \"single\" (default), \"multiple\" (two reads per trigger), or \"continuous\".\n# await bcr.set_read_mode(\"continuous\")" - }, - { - "cell_type": "markdown", - "id": "3b16ffac", - "metadata": {}, - "source": [ - "### Driver-level access\n", - "\n", - "For raw vendor commands or the auto-trigger mode (reader fires on its own), go through `bcr.driver`:" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "c0bcb7c9", - "metadata": {}, - "outputs": [], - "source": [ - "print(await bcr.driver.get_software_version())\n", - "\n", - "# Full NVRAM dump \u2014 model, firmware, all symbology enables, prefixes/suffixes, length limits.\n", - "config = await bcr.driver.dump_config()\n", - "print(config[:200])\n", - "\n", - "# await bcr.driver.set_auto_trigger(True) # reader scans on its own\n", - "# await bcr.driver.set_auto_trigger(False)" - ] - }, - { - "cell_type": "markdown", - "id": "be3ecf8e", - "metadata": {}, - "source": [ - "### Teardown\n", - "\n", - "`stop()` sends `Y` (trigger off) and `Y2` (reset to a 2s read window), then closes the serial port \u2014 leaving the reader in a known state for the next session." - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "c19e0921", - "metadata": {}, - "outputs": [], - "source": [ - "await bcr.stop()" - ] - }, { "cell_type": "markdown", "id": "kx2-teardown-header", From 6088c5b3b29e4688781b8371613dc2dd5ac3aacf Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 3 May 2026 20:06:40 -0700 Subject: [PATCH 85/93] KX2: drop direction arg from find_with_proximity_sensor MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/arm_backend.py | 15 +++++++++------ .../tests/arm_backend_find_z_proximity_tests.py | 3 --- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 648ecffe6e8..451c583d872 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -741,15 +741,17 @@ async def find_with_proximity_sensor( self, start: Coordinate, end: Coordinate, - direction: float, *, max_gripper_speed: float = 25.0, max_gripper_acceleration: float = 100.0, ) -> Coordinate: """Sweep the gripper from ``start`` to ``end`` along a straight Cartesian - line; halt when the IR breakbeam trips. Yaw is held at ``direction`` - for the whole sweep. Returns the gripper location at halt; raises - ``RuntimeError`` if the beam never tripped over the full path. + line; halt when the IR breakbeam trips. Yaw is held at whatever the + gripper is currently at — proximity sensing doesn't care about + orientation, and asking the caller to specify a direction would force + them to think about wrist angle they don't otherwise need to. Returns + the gripper location at halt; raises ``RuntimeError`` if the beam + never tripped over the full path. Generic Cartesian counterpart to :meth:`find_z_with_proximity_sensor`. The Z-only descent has a hardware fast-path (``IL[4]=StopForward`` on @@ -766,11 +768,12 @@ async def find_with_proximity_sensor( max_gripper_acceleration=max_gripper_acceleration, path="linear", ) - start_pose = GripperLocation(location=start, rotation=Rotation(z=direction)) - end_pose = GripperLocation(location=end, rotation=Rotation(z=direction)) async with self._motion_guard(): await self.driver.motors_ensure_enabled([int(a) for a in MOTION_AXES]) + current_yaw = (await self.request_gripper_location()).rotation.z + start_pose = GripperLocation(location=start, rotation=Rotation(z=current_yaw)) + end_pose = GripperLocation(location=end, rotation=Rotation(z=current_yaw)) # Pre-position to start (joint move — path doesn't matter, just get there). pre_pos = await self._cart_to_joints(start_pose) await self._motors_move_joint_locked( diff --git a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py index c1a427744cd..9de1f3174df 100644 --- a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py +++ b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py @@ -359,7 +359,6 @@ def test_trip_cancels_sweep_returns_gripper_location(self): result = asyncio.run(h.backend.find_with_proximity_sensor( start=Coordinate(x=100, y=200, z=50), end=Coordinate(x=200, y=200, z=50), - direction=0.0, )) self.assertAlmostEqual(result.x, 110.0, places=6) @@ -381,7 +380,6 @@ def test_never_trips_raises_with_endpoints(self): asyncio.run(h.backend.find_with_proximity_sensor( start=Coordinate(x=100, y=200, z=50), end=Coordinate(x=200, y=200, z=50), - direction=0.0, )) msg = str(ctx.exception) @@ -399,7 +397,6 @@ def test_pre_check_already_tripped_skips_sweep(self): result = asyncio.run(h.backend.find_with_proximity_sensor( start=Coordinate(x=100, y=200, z=50), end=Coordinate(x=200, y=200, z=50), - direction=0.0, )) self.assertAlmostEqual(result.x, 100.0, places=6) From e0e5896ae84d93856c9f50d6ce4dbe00d96d20b0 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 12 May 2026 15:35:48 -0700 Subject: [PATCH 86/93] KX2: get_estop_state checks only SR bits 14/15, drop full-register match Co-Authored-By: Claude Opus 4.7 (1M context) --- pylabrobot/paa/kx2/arm_backend.py | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 451c583d872..d3dcfd913a0 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -177,11 +177,7 @@ async def get_estop_state(self) -> bool: interpreter. Bits 14/15 encode the stop/safety state. """ r = await self.driver.query_int(Axis.SHOULDER, "SR", 1) - if r != 8438016: - logger.warning("get_estop_state: SR register unexpected value %d (expected 8438016)", r) - b14 = (r & 0x4000) == 0x4000 - b15 = (r & 0x8000) == 0x8000 - return (not b14) and (not b15) + return (r & 0x4000) == 0 or (r & 0x8000) == 0 async def gripper_get_homed_status(self) -> HomeStatus: return HomeStatus(await self.driver.query_int(Axis.SERVO_GRIPPER, "UI", 3)) From f1a80935b7c2a067ca34456400be055dacec42d3 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 12 May 2026 15:53:10 -0700 Subject: [PATCH 87/93] =?UTF-8?q?KX2:=20rename=20GripperLocation=20?= =?UTF-8?q?=E2=86=92=20CartesianPose;=20sync=20docs=20and=20notebooks?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Claude Opus 4.7 (1M context) --- docs/api/pylabrobot.arms.rst | 2 +- .../brooks/precise_flex/hello-world.ipynb | 4 +-- docs/user_guide/paa/kx2/hello-world.ipynb | 2 +- pylabrobot/paa/kx2/arm_backend.py | 14 ++++---- pylabrobot/paa/kx2/kinematics.py | 18 +++++------ .../arm_backend_find_z_proximity_tests.py | 6 ++-- .../tests/arm_backend_linear_path_tests.py | 6 ++-- pylabrobot/paa/kx2/tests/kinematics_tests.py | 32 +++++++++---------- 8 files changed, 42 insertions(+), 42 deletions(-) diff --git a/docs/api/pylabrobot.arms.rst b/docs/api/pylabrobot.arms.rst index 60a7050203d..02a48eceb76 100644 --- a/docs/api/pylabrobot.arms.rst +++ b/docs/api/pylabrobot.arms.rst @@ -38,5 +38,5 @@ Types :nosignatures: :recursive: - standard.GripperLocation + standard.CartesianPose standard.GripDirection diff --git a/docs/user_guide/brooks/precise_flex/hello-world.ipynb b/docs/user_guide/brooks/precise_flex/hello-world.ipynb index 236ba7be744..44a503a4a4b 100644 --- a/docs/user_guide/brooks/precise_flex/hello-world.ipynb +++ b/docs/user_guide/brooks/precise_flex/hello-world.ipynb @@ -126,7 +126,7 @@ "name": "stdout", "output_type": "stream", "text": [ - "PreciseFlexGripperLocation(location=Coordinate(x=262.0253, y=0.0042, z=1.501), rotation=Rotation(x=-180, y=90, z=0.0010000000000864489), rail=0.015, orientation='left', wrist='ccw')\n", + "PreciseFlexCartesianPose(location=Coordinate(x=262.0253, y=0.0042, z=1.501), rotation=Rotation(x=-180, y=90, z=0.0010000000000864489), rail=0.015, orientation='left', wrist='ccw')\n", "{: 0.015, : 1.501, : 72.977, : 199.323, : -272.299, : 79.8}\n" ] } @@ -173,7 +173,7 @@ { "data": { "text/plain": [ - "PreciseFlexGripperLocation(location=Coordinate(x=262.0253, y=0.0042, z=1.501), rotation=Rotation(x=-180, y=90, z=0.0010000000000864489), rail=0.015, orientation='right', wrist='ccw')" + "PreciseFlexCartesianPose(location=Coordinate(x=262.0253, y=0.0042, z=1.501), rotation=Rotation(x=-180, y=90, z=0.0010000000000864489), rail=0.015, orientation='right', wrist='ccw')" ] }, "execution_count": 7, diff --git a/docs/user_guide/paa/kx2/hello-world.ipynb b/docs/user_guide/paa/kx2/hello-world.ipynb index 190da015787..4273451898f 100644 --- a/docs/user_guide/paa/kx2/hello-world.ipynb +++ b/docs/user_guide/paa/kx2/hello-world.ipynb @@ -199,7 +199,7 @@ { "data": { "text/plain": [ - "GripperLocation(location=Coordinate(x=312.2642, y=564.2383, z=333.7532), rotation=Rotation(x=0, y=0, z=265.67006324798695))" + "CartesianPose(location=Coordinate(x=312.2642, y=564.2383, z=333.7532), rotation=Rotation(x=0, y=0, z=265.67006324798695))" ] }, "execution_count": 9, diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index d3dcfd913a0..3b714277287 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -12,7 +12,7 @@ HasJoints, OrientableGripperArmBackend, ) -from pylabrobot.capabilities.arms.standard import GripperLocation +from pylabrobot.capabilities.arms.standard import CartesianPose from pylabrobot.capabilities.capability import BackendParams from pylabrobot.paa.kx2 import kinematics from pylabrobot.paa.kx2.config import ( @@ -768,8 +768,8 @@ async def find_with_proximity_sensor( async with self._motion_guard(): await self.driver.motors_ensure_enabled([int(a) for a in MOTION_AXES]) current_yaw = (await self.request_gripper_location()).rotation.z - start_pose = GripperLocation(location=start, rotation=Rotation(z=current_yaw)) - end_pose = GripperLocation(location=end, rotation=Rotation(z=current_yaw)) + start_pose = CartesianPose(location=start, rotation=Rotation(z=current_yaw)) + end_pose = CartesianPose(location=end, rotation=Rotation(z=current_yaw)) # Pre-position to start (joint move — path doesn't matter, just get there). pre_pos = await self._cart_to_joints(start_pose) await self._motors_move_joint_locked( @@ -892,7 +892,7 @@ async def _motors_move_absolute_execute_locked(self, plan: MotorsMovePlan) -> No await self.driver.ppm_begin_motion(node_ids) await self.driver.wait_for_moves_done(node_ids, timeout=plan.move_time + 2) - async def _cart_to_joints(self, pose: GripperLocation) -> Dict[Axis, float]: + async def _cart_to_joints(self, pose: CartesianPose) -> Dict[Axis, float]: """Cartesian -> joints, snapping rotary axes to whichever 360° wrap is closest to the current joint position.""" current = {Axis(k): v for k, v in (await self.request_joint_position()).items()} @@ -901,7 +901,7 @@ async def _cart_to_joints(self, pose: GripperLocation) -> Dict[Axis, float]: async def _run_linear_path( self, - target_pose: GripperLocation, + target_pose: CartesianPose, params: "KX2ArmBackend.CartesianMoveParams", ) -> None: """Stream a Cartesian-linear trajectory through the drive's IPM buffer. @@ -1050,7 +1050,7 @@ async def park(self, backend_params: Optional[BackendParams] = None) -> None: async def request_gripper_location( self, backend_params: Optional[BackendParams] = None - ) -> GripperLocation: + ) -> CartesianPose: joints = {Axis(k): v for k, v in (await self.request_joint_position()).items()} return kinematics.fk(joints, self._cfg, self._gripper_params) @@ -1126,7 +1126,7 @@ async def move_to_location( ) -> None: if not isinstance(backend_params, KX2ArmBackend.CartesianMoveParams): backend_params = KX2ArmBackend.CartesianMoveParams() - pose = GripperLocation(location=location, rotation=Rotation(z=direction)) + pose = CartesianPose(location=location, rotation=Rotation(z=direction)) if backend_params.path == "linear": async with self._motion_guard(): await self._run_linear_path(pose, backend_params) diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index 4c853cd7ef7..93ea02377b5 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -16,7 +16,7 @@ 3: elbow [mm] (radial extension) 4: wrist [deg] -Task pose is a `GripperLocation`. The gripper clamp point is in world +Task pose is a `CartesianPose`. The gripper clamp point is in world coordinates; rotation.z is yaw in degrees about world +Z, and rotation.x/y must be 0. Sign convention follows right-hand rule about +Z (CCW positive looking down). @@ -27,7 +27,7 @@ from typing import Dict, List, Optional, Tuple from pylabrobot.capabilities.arms import kinematics as arm_kinematics -from pylabrobot.capabilities.arms.standard import GripperLocation +from pylabrobot.capabilities.arms.standard import CartesianPose from pylabrobot.paa.kx2.config import Axis, GripperParams, KX2Config from pylabrobot.paa.kx2.driver import ( JointMoveDirection, @@ -45,7 +45,7 @@ class IKError(ValueError): _EPS = 1e-6 -def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperParams) -> GripperLocation: +def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperParams) -> CartesianPose: """Forward kinematics. Args: @@ -53,7 +53,7 @@ def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperParams) -> GripperLoca c: arm configuration (drive-read calibration). t: gripper tooling (user-supplied geometry). Returns: - GripperLocation with the gripper clamp point and a yaw equivalent to + CartesianPose with the gripper clamp point and a yaw equivalent to the joints' net world orientation. """ r = c.wrist_offset + c.elbow_offset + c.elbow_zero_offset + joints[Axis.ELBOW] @@ -72,7 +72,7 @@ def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperParams) -> GripperLoca yaw = radians(yaw_deg) gl = t.length if t.finger_side == "barcode_reader" else -t.length - return GripperLocation( + return CartesianPose( location=Coordinate( x=wrist_x + gl * sin(yaw), y=wrist_y - gl * cos(yaw), @@ -82,7 +82,7 @@ def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperParams) -> GripperLoca ) -def ik(pose: GripperLocation, c: KX2Config, t: GripperParams) -> Dict[Axis, float]: +def ik(pose: CartesianPose, c: KX2Config, t: GripperParams) -> Dict[Axis, float]: """Inverse kinematics. Args: @@ -516,8 +516,8 @@ def _yaw_lerp(yaw_a_deg: float, yaw_b_deg: float, alpha: float) -> float: def sample_linear_path( cfg: KX2Config, gripper_params: GripperParams, - start_pose: GripperLocation, - end_pose: GripperLocation, + start_pose: CartesianPose, + end_pose: CartesianPose, *, vel_mm_per_s: float, accel_mm_per_s2: float, @@ -580,7 +580,7 @@ def sample_linear_path( poses: List[Dict[Axis, float]] = [] for s in s_seq: alpha = (s / length) if length > 0 else 1.0 - pose_i = GripperLocation( + pose_i = CartesianPose( location=Coordinate( x=sx + alpha * (ex - sx), y=sy + alpha * (ey - sy), diff --git a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py index 9de1f3174df..0954053fabb 100644 --- a/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py +++ b/pylabrobot/paa/kx2/tests/arm_backend_find_z_proximity_tests.py @@ -295,7 +295,7 @@ def _build_generic_harness( (the IPM streaming runtime), _cart_to_joints (IK), and request_gripper_location (FK) so the test exercises the polling-and- cancel logic without spinning up a real driver.""" - from pylabrobot.capabilities.arms.standard import GripperLocation + from pylabrobot.capabilities.arms.standard import CartesianPose from pylabrobot.resources import Rotation backend = KX2ArmBackend.__new__(KX2ArmBackend) @@ -314,9 +314,9 @@ async def _read_proximity() -> bool: async def _request_gripper_location(): val = locs[0] if len(locs) == 1 else locs.pop(0) - if isinstance(val, GripperLocation): + if isinstance(val, CartesianPose): return val - return GripperLocation(location=val, rotation=Rotation(z=0)) + return CartesianPose(location=val, rotation=Rotation(z=0)) async def _cart_to_joints(pose): return {Axis.SHOULDER: 0.0, Axis.Z: pose.location.z, Axis.ELBOW: 100.0, diff --git a/pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py index b8504a5a1d2..75d0fd80757 100644 --- a/pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py +++ b/pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py @@ -22,7 +22,7 @@ import unittest from typing import Any, Dict, List, Optional, Tuple -from pylabrobot.capabilities.arms.standard import GripperLocation +from pylabrobot.capabilities.arms.standard import CartesianPose from pylabrobot.paa.kx2.arm_backend import KX2ArmBackend from pylabrobot.paa.kx2.config import ( Axis, AxisConfig, GripperParams, KX2Config, @@ -138,10 +138,10 @@ async def _request_joint_position() -> Dict[int, float]: return backend -def _target_pose() -> GripperLocation: +def _target_pose() -> CartesianPose: """A pose ~30 mm away from the harness's current_joints — long enough that the arc-length trapezoid produces well over the 8-frame preload.""" - return GripperLocation( + return CartesianPose( location=Coordinate(x=20.0, y=110.0, z=80.0), rotation=Rotation(z=15.0), ) diff --git a/pylabrobot/paa/kx2/tests/kinematics_tests.py b/pylabrobot/paa/kx2/tests/kinematics_tests.py index 3db6545bf1e..7f98ee34105 100644 --- a/pylabrobot/paa/kx2/tests/kinematics_tests.py +++ b/pylabrobot/paa/kx2/tests/kinematics_tests.py @@ -1,7 +1,7 @@ import math import unittest -from pylabrobot.capabilities.arms.standard import GripperLocation +from pylabrobot.capabilities.arms.standard import CartesianPose from pylabrobot.paa.kx2 import kinematics from pylabrobot.paa.kx2.config import Axis, AxisConfig, GripperParams, KX2Config from pylabrobot.paa.kx2.driver import JointMoveDirection @@ -46,7 +46,7 @@ class FKIKRoundTrip(unittest.TestCase): def test_roundtrip(self): c = _config() g = GripperParams(length=15.0, z_offset=3.0) - pose = GripperLocation( + pose = CartesianPose( location=Coordinate(x=100, y=200, z=50), rotation=Rotation(z=30) ) joints = kinematics.ik(pose, c, g) @@ -60,7 +60,7 @@ def test_roundtrip_at_origin_yaw_zero(self): """Sanity: a pose at (0, R, Z, 0°) lands shoulder=0°.""" c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0) g = GripperParams() - pose = GripperLocation( + pose = CartesianPose( location=Coordinate(x=0, y=300, z=10), rotation=Rotation(z=0) ) joints = kinematics.ik(pose, c, g) @@ -83,7 +83,7 @@ def test_z_offset_sign_convention(self): kinematics.fk(joints, c, g_high).location.z, "FK: increasing z_offset should lower the gripper", ) - pose = GripperLocation(location=Coordinate(x=0, y=300, z=50), rotation=Rotation(z=0)) + pose = CartesianPose(location=Coordinate(x=0, y=300, z=50), rotation=Rotation(z=0)) self.assertGreater( kinematics.ik(pose, c, g_high)[Axis.Z], kinematics.ik(pose, c, g_low)[Axis.Z], @@ -114,7 +114,7 @@ def test_ik_shoulder_branch_convention(self): ("Q2 (-x, +y)", -100.0, 100.0, 45.0), ] for label, x, y, expected_shoulder in cases: - pose = GripperLocation( + pose = CartesianPose( location=Coordinate(x=x, y=y, z=0), rotation=Rotation(z=0), ) joints = kinematics.ik(pose, c, g) @@ -210,7 +210,7 @@ def test_fk_wrist_180_keeps_yaw_in_range(self): def test_ik_on_plus_y_axis(self): """Target on +Y axis → shoulder = 0; elbow = y - sum_of_offsets.""" - pose = GripperLocation(location=Coordinate(x=0, y=300, z=50), rotation=Rotation(z=0)) + pose = CartesianPose(location=Coordinate(x=0, y=300, z=50), rotation=Rotation(z=0)) j = kinematics.ik(pose, self._cfg(), self._gripper()) self.assertAlmostEqual(j[Axis.SHOULDER], 0.0, places=6) self.assertAlmostEqual(j[Axis.Z], 53.0, places=6) @@ -218,7 +218,7 @@ def test_ik_on_plus_y_axis(self): self.assertAlmostEqual(j[Axis.WRIST], 0.0, places=6) def test_ik_q1_target(self): - pose = GripperLocation(location=Coordinate(x=100, y=200, z=100), rotation=Rotation(z=30)) + pose = CartesianPose(location=Coordinate(x=100, y=200, z=100), rotation=Rotation(z=30)) j = kinematics.ik(pose, self._cfg(), self._gripper()) self.assertAlmostEqual(j[Axis.SHOULDER], -23.474916, places=4) self.assertAlmostEqual(j[Axis.Z], 103.0, places=6) @@ -226,7 +226,7 @@ def test_ik_q1_target(self): self.assertAlmostEqual(j[Axis.WRIST], 53.474916, places=4) def test_ik_q2_target(self): - pose = GripperLocation(location=Coordinate(x=-150, y=150, z=75), rotation=Rotation(z=-45)) + pose = CartesianPose(location=Coordinate(x=-150, y=150, z=75), rotation=Rotation(z=-45)) j = kinematics.ik(pose, self._cfg(), self._gripper()) self.assertAlmostEqual(j[Axis.SHOULDER], 40.955309, places=4) self.assertAlmostEqual(j[Axis.Z], 78.0, places=6) @@ -235,7 +235,7 @@ def test_ik_q2_target(self): def test_ik_negative_quadrant(self): """Target in negative-Y region — shoulder rotates past 90°.""" - pose = GripperLocation(location=Coordinate(x=-50, y=-200, z=120), rotation=Rotation(z=90)) + pose = CartesianPose(location=Coordinate(x=-50, y=-200, z=120), rotation=Rotation(z=90)) j = kinematics.ik(pose, self._cfg(), self._gripper()) self.assertAlmostEqual(j[Axis.SHOULDER], 161.995838, places=4) self.assertAlmostEqual(j[Axis.Z], 123.0, places=6) @@ -247,7 +247,7 @@ class IKErrors(unittest.TestCase): def test_x_rotation_raises_ikerror(self): c = _config() g = GripperParams() - pose = GripperLocation( + pose = CartesianPose( location=Coordinate(x=0, y=100, z=0), rotation=Rotation(x=10, z=0) ) with self.assertRaises(IKError): @@ -256,7 +256,7 @@ def test_x_rotation_raises_ikerror(self): def test_y_rotation_raises_ikerror(self): c = _config() g = GripperParams() - pose = GripperLocation( + pose = CartesianPose( location=Coordinate(x=0, y=100, z=0), rotation=Rotation(y=10, z=0) ) with self.assertRaises(IKError): @@ -327,7 +327,7 @@ def test_proximity_side_negates_gripper_offset(self): def test_proximity_roundtrip(self): c = _config() g = GripperParams(length=15.0, z_offset=3.0, finger_side="proximity_sensor") - pose = GripperLocation( + pose = CartesianPose( location=Coordinate(x=100, y=200, z=50), rotation=Rotation(z=30) ) joints = kinematics.ik(pose, c, g) @@ -342,7 +342,7 @@ def test_ik_elbow_differs_by_twice_gripper_length(self): shoulder=0 but the wrist sits 2*gripper_length further out for barcode_reader (gripper points +y away from base) than for proximity_sensor (gripper points -y back toward base).""" - pose = GripperLocation( + pose = CartesianPose( location=Coordinate(x=0, y=300, z=0), rotation=Rotation(z=0) ) c = _config() @@ -360,15 +360,15 @@ def test_negative_180_snaps_to_positive(self): """A pose pointing exactly along -y has shoulder = ±180; we snap to +180.""" c = _config(wrist_offset=0, elbow_offset=0, elbow_zero_offset=0) g = GripperParams() - pose = GripperLocation( + pose = CartesianPose( location=Coordinate(x=0, y=-100, z=0), rotation=Rotation(z=180) ) joints = kinematics.ik(pose, c, g) self.assertAlmostEqual(joints[Axis.SHOULDER], 180.0, places=9) -def _linear_pose(x: float, y: float, z: float, yaw: float = 0.0) -> GripperLocation: - return GripperLocation( +def _linear_pose(x: float, y: float, z: float, yaw: float = 0.0) -> CartesianPose: + return CartesianPose( location=Coordinate(x=x, y=y, z=z), rotation=Rotation(z=yaw), ) From 5e2a114e299ec5745d77e3eae4d20967380d6296 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 12 May 2026 21:11:27 -0700 Subject: [PATCH 88/93] KX2: fix FK/IK to model gripper hanging off wrist axis (location = grip center) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- pylabrobot/paa/kx2/arm_backend.py | 20 +++++ pylabrobot/paa/kx2/config.py | 26 ++++-- pylabrobot/paa/kx2/kinematics.py | 92 ++++++++++++++------ pylabrobot/paa/kx2/kx2.py | 25 ++++++ pylabrobot/paa/kx2/tests/kinematics_tests.py | 62 ++++++++----- 5 files changed, 169 insertions(+), 56 deletions(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index 3b714277287..8d1d4559b01 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -69,6 +69,26 @@ def __init__( gripper_z_offset: float = 0.0, gripper_finger_side: GripperFingerSide = "barcode_reader", ) -> None: + """ + Args: + driver: low-level KX2 CAN transport. + gripper_length: distance from the wrist axis to the gripper's + *grip center* (geometric midpoint of the jaws) in mm. + Non-negative; the side is encoded in ``gripper_finger_side``, + not the sign. The gripper assembly hangs off the wrist axis, + so for the same target ``(location, direction)`` IK puts the + wrist on opposite sides of the grip center for the two + finger-side choices (separated by ``2·gripper_length``). + gripper_z_offset: vertical offset from the wrist plate to the + grip center in mm. Positive = grip center below the wrist plate. + gripper_finger_side: which finger is treated as the gripper's + "front". The world yaw reported by + :meth:`request_gripper_location` (and the yaw accepted by + :meth:`move_to_location`) points at this finger. Flipping + side is a 180° relabel of which finger is "front" — for the + same joints the grip center is unchanged and only the + reported yaw shifts by 180°. + """ super().__init__() self.driver = driver # Tooling is user-supplied and known at construction; KX2Config (drive- diff --git a/pylabrobot/paa/kx2/config.py b/pylabrobot/paa/kx2/config.py index 9030ae55a59..7ef960a9802 100644 --- a/pylabrobot/paa/kx2/config.py +++ b/pylabrobot/paa/kx2/config.py @@ -76,14 +76,24 @@ class GripperParams: is drive-read motor calibration for the servo gripper itself. Attributes: - length: Distance from the wrist axis to the gripper clamp point, in - mm. Sign tracks which finger is the radial "front" via - :attr:`finger_side`. - z_offset: Vertical offset from the wrist plate to the clamp point, - in mm. Positive = clamp sits below the wrist plate. - finger_side: Which finger is treated as the radial "front" by IK/FK. - The fingers are physically symmetric, so flipping side just - negates the gripper-length offset in the wrist frame. + length: Distance from the wrist axis to the gripper's *grip center* + (the geometric midpoint between the jaws, where a held plate + sits), in mm. Always non-negative — the "which side" choice is + encoded in :attr:`finger_side`, not the sign of this length. + The gripper assembly hangs off the wrist axis along its + extension direction, with both fingers clustered around the + grip center; FK/IK route ``location`` through this offset. + z_offset: Vertical offset from the wrist plate to the grip center, + in mm. Positive = grip center sits below the wrist plate. + finger_side: Which finger is treated as the gripper's "front" — + i.e., the one the reported world yaw (``rotation.z``) points at. + Flipping side is just a 180° relabel: for the same joints the + grip center is unchanged and only the reported yaw shifts by + 180°. For the same target ``(location, rotation.z)``, IK puts + the wrist axis on opposite sides of the grip center (separated + by ``2·length`` along the front-finger axis) — the gripper + assembly has to swing around the wrist motor to point the + chosen finger forward. """ length: float = 0.0 diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index 93ea02377b5..6ae20c55dbe 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -16,10 +16,25 @@ 3: elbow [mm] (radial extension) 4: wrist [deg] -Task pose is a `CartesianPose`. The gripper clamp point is in world -coordinates; rotation.z is yaw in degrees about world +Z, and -rotation.x/y must be 0. Sign convention follows right-hand rule about +Z -(CCW positive looking down). +Task pose is a `CartesianPose`. ``location`` is the gripper's *grip +center* (the geometric midpoint between the two jaws, where a held +plate sits) in world coordinates; rotation.z is yaw in degrees about +world +Z, and rotation.x/y must be 0. Sign convention follows right-hand +rule about +Z (CCW positive looking down). + +`rotation.z` is the world direction in which the *front* finger +of the gripper points — where "front" is whichever finger +:attr:`GripperParams.finger_side` names. The gripper assembly hangs +``t.length`` away from the wrist axis along its extension direction; +the *grip center* is at that offset, and the two fingers cluster +around it. Flipping `finger_side` is just a 180° relabel of which +finger is the "front", so for the same joint state the grip center +stays put and only the reported yaw flips by 180°. For the same +(grip center, yaw) target, the wrist *axis* lands on opposite sides +of the grip center for the two side choices (separated by +``2·t.length`` along the front-finger axis), because the gripper +assembly has to swing around the wrist motor to point the chosen +finger forward. """ from dataclasses import dataclass, field @@ -53,8 +68,12 @@ def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperParams) -> CartesianPo c: arm configuration (drive-read calibration). t: gripper tooling (user-supplied geometry). Returns: - CartesianPose with the gripper clamp point and a yaw equivalent to - the joints' net world orientation. + CartesianPose where ``location`` is the gripper's *grip center* + (geometric midpoint of the jaws) and ``rotation.z`` is the world + yaw of the *front* finger (the one named by ``t.finger_side``). + Yaw is wrapped to ``[-180, 180]``. The grip center depends only + on the joint state — flipping ``t.finger_side`` for fixed joints + leaves it unchanged and shifts only the reported yaw by 180°. """ r = c.wrist_offset + c.elbow_offset + c.elbow_zero_offset + joints[Axis.ELBOW] sh_deg = joints[Axis.SHOULDER] @@ -64,18 +83,25 @@ def fk(joints: Dict[Axis, float], c: KX2Config, t: GripperParams) -> CartesianPo wrist_y = r * cos(sh) wrist_z = joints[Axis.Z] - yaw_deg = joints[Axis.WRIST] + sh_deg - if yaw_deg > 180.0: + # Gripper assembly hangs t.length off the wrist axis along the + # extension direction (= world angle of the wrist motor = WRIST+SHOULDER). + # finger_side just relabels which physical finger is "front", so it + # only shifts the reported yaw by 180°; the grip center is fixed. + ext_deg = joints[Axis.WRIST] + sh_deg + ext = radians(ext_deg) + + yaw_deg = ext_deg + if t.finger_side == "proximity_sensor": + yaw_deg += 180.0 + while yaw_deg > 180.0: yaw_deg -= 360.0 - if yaw_deg < -180.0: + while yaw_deg < -180.0: yaw_deg += 360.0 - yaw = radians(yaw_deg) - gl = t.length if t.finger_side == "barcode_reader" else -t.length return CartesianPose( location=Coordinate( - x=wrist_x + gl * sin(yaw), - y=wrist_y - gl * cos(yaw), + x=wrist_x + t.length * sin(ext), + y=wrist_y - t.length * cos(ext), z=wrist_z - t.z_offset, ), rotation=Rotation(z=yaw_deg), @@ -86,28 +112,40 @@ def ik(pose: CartesianPose, c: KX2Config, t: GripperParams) -> Dict[Axis, float] """Inverse kinematics. Args: - pose: target gripper pose. rotation.x/y must be 0. + pose: target gripper pose. ``location`` is the *grip center* + (geometric midpoint of the jaws). ``rotation.z`` is the world + direction the *front* finger should face (per ``t.finger_side``). + ``rotation.x/y`` must be 0. c: arm configuration (drive-read calibration). t: gripper tooling (user-supplied geometry). Returns: joints dict {Axis.SHOULDER: deg, Axis.Z: mm, Axis.ELBOW: mm, Axis.WRIST: deg}. - J4 is the canonical (-180°, 180°] solution; `snap_to_current` shifts - it to the closest 360° wrap of the current J4 for actual motion. + Shoulder and elbow differ between the two finger-side choices for + the same target — the gripper assembly swings around the wrist + motor, so the wrist axis lands on opposite sides of the grip + center (separated by ``2·t.length`` along the front-finger axis). + J4 is the canonical (-180°, 180°] solution; `snap_to_current` + then pulls it to whichever 360° wrap is closest to the current J4. Raises: IKError if the requested rotation has an x or y component. """ if pose.rotation.x != 0 or pose.rotation.y != 0: raise IKError("Only Z rotation is supported for KX2") - # Gripper -> wrist: the incoming pose describes the gripper clamp point; - # the joint-space math operates on the wrist axis. Rigid offset with the - # gripper length on the radial axis (governed by world rotation z) and - # the gripper z offset downward. Sign tracks which finger is the radial - # "front". - yaw = radians(pose.rotation.z) - gl = t.length if t.finger_side == "barcode_reader" else -t.length - x = pose.location.x - gl * sin(yaw) - y = pose.location.y + gl * cos(yaw) + # The incoming pose describes the grip center; the joint-space math + # operates on the wrist axis. The gripper assembly hangs ``t.length`` + # off the wrist axis along its *extension direction*, so the grip + # center sits at wrist + t.length·. For barcode_reader + # the extension direction == the front-finger direction; for + # proximity_sensor the front finger has been swung 180° to the other + # side, so the extension (and therefore grip-center offset) points + # 180° opposite the front finger. + ext_deg = pose.rotation.z + if t.finger_side == "proximity_sensor": + ext_deg -= 180.0 + ext = radians(ext_deg) + x = pose.location.x - t.length * sin(ext) + y = pose.location.y + t.length * cos(ext) wrist_z = pose.location.z + t.z_offset # atan2 returns (-π, π]; on the -Y axis it yields -180°. Snap to +180° @@ -118,7 +156,9 @@ def ik(pose: CartesianPose, c: KX2Config, t: GripperParams) -> Dict[Axis, float] shoulder = 180.0 elbow = hypot(x, y) - c.wrist_offset - c.elbow_offset - c.elbow_zero_offset - wrist = pose.rotation.z - shoulder + # Wrist motor's world angle == extension direction. Joint space: + # wrist_joint = ext_world - shoulder. + wrist = ext_deg - shoulder return {Axis.SHOULDER: shoulder, Axis.Z: wrist_z, Axis.ELBOW: elbow, Axis.WRIST: wrist} diff --git a/pylabrobot/paa/kx2/kx2.py b/pylabrobot/paa/kx2/kx2.py index 7295b055089..96fde2ba91d 100644 --- a/pylabrobot/paa/kx2/kx2.py +++ b/pylabrobot/paa/kx2/kx2.py @@ -30,6 +30,31 @@ def __init__( barcode_port: Optional[str] = None, barcode_baudrate: int = KX2BarcodeReaderDriver.default_baudrate, ) -> None: + """ + Args: + gripper_length: distance from the wrist axis to the gripper's + *grip center* (geometric midpoint of the jaws) in mm. + Non-negative; ``gripper_finger_side`` selects which side, not + the sign of ``gripper_length``. For the same target + ``(location, direction)``, IK puts the wrist on opposite sides + of the grip center for the two finger-side choices. + gripper_z_offset: vertical offset from the wrist plate to the + grip center in mm. Positive = grip center below the wrist plate. + gripper_finger_side: which finger is treated as the gripper's + "front". The world yaw returned by + :meth:`arm.request_gripper_location` (and the ``direction`` + argument to :meth:`arm.move_to_location`) points at this + finger. Flipping side is a 180° relabel of which finger is + "front" — for the same joints the grip center is unchanged + and only the reported yaw shifts by 180°. + has_rail: True if the arm is mounted on the optional linear + rail (axis 5). + has_servo_gripper: True if a servo-driven plate gripper is on + the bus (axis 6). + barcode_port: optional serial port of the onboard barcode reader. + When set, exposes :attr:`barcode_scanning`. + barcode_baudrate: barcode-reader serial baud rate. + """ driver = KX2Driver(has_rail=has_rail, has_servo_gripper=has_servo_gripper) super().__init__(driver=driver) self.driver: KX2Driver = driver diff --git a/pylabrobot/paa/kx2/tests/kinematics_tests.py b/pylabrobot/paa/kx2/tests/kinematics_tests.py index 7f98ee34105..4133f08c003 100644 --- a/pylabrobot/paa/kx2/tests/kinematics_tests.py +++ b/pylabrobot/paa/kx2/tests/kinematics_tests.py @@ -301,8 +301,11 @@ def test_no_shift_when_already_close(self): class GripperFingerSide(unittest.TestCase): - def test_proximity_side_negates_gripper_offset(self): - """Same joints, opposite finger side -> clamp point reflected through wrist axis.""" + def test_proximity_side_keeps_grip_center_flips_yaw(self): + """Same joints, opposite finger side -> identical grip center (the + location field is the geometric midpoint between the jaws, not a + finger tip), but reported yaw flips 180° because the front finger + swaps to the opposite physical sensor.""" c = _config() g_bc = GripperParams(length=15.0, z_offset=3.0, finger_side="barcode_reader") g_pr = GripperParams(length=15.0, z_offset=3.0, finger_side="proximity_sensor") @@ -310,19 +313,12 @@ def test_proximity_side_negates_gripper_offset(self): p_bc = kinematics.fk(j, c, g_bc) p_pr = kinematics.fk(j, c, g_pr) - # Wrist position is the midpoint between the two clamp points. - wrist_x = (p_bc.location.x + p_pr.location.x) / 2 - wrist_y = (p_bc.location.y + p_pr.location.y) / 2 - yaw_deg = j[Axis.WRIST] + j[Axis.SHOULDER] - yaw = math.radians(yaw_deg) - self.assertAlmostEqual( - p_bc.location.x - wrist_x, g_bc.length * math.sin(yaw), delta=1e-5 - ) - self.assertAlmostEqual( - p_bc.location.y - wrist_y, -g_bc.length * math.cos(yaw), delta=1e-5 - ) + self.assertAlmostEqual(p_bc.location.x, p_pr.location.x, places=9) + self.assertAlmostEqual(p_bc.location.y, p_pr.location.y, places=9) self.assertAlmostEqual(p_bc.location.z, p_pr.location.z, places=9) - self.assertAlmostEqual(p_bc.rotation.z, p_pr.rotation.z, places=9) + # Yaws are normalized to [-180, 180]; flipping finger_side rotates + # the *labelled* front by 180°, so the absolute difference is 180. + self.assertAlmostEqual(abs(p_bc.rotation.z - p_pr.rotation.z), 180.0, places=9) def test_proximity_roundtrip(self): c = _config() @@ -337,11 +333,13 @@ def test_proximity_roundtrip(self): self.assertAlmostEqual(back.location.z, pose.location.z, places=9) self.assertAlmostEqual(back.rotation.z, pose.rotation.z, places=9) - def test_ik_elbow_differs_by_twice_gripper_length(self): - """For a clamp point on the +y axis with yaw=0, both sides give - shoulder=0 but the wrist sits 2*gripper_length further out for - barcode_reader (gripper points +y away from base) than for - proximity_sensor (gripper points -y back toward base).""" + def test_ik_wrist_axis_lands_on_opposite_sides_of_grip_center(self): + """Same target ``(location, direction)``, opposite finger side -> + wrist axis lands on opposite sides of the grip center along the + front-finger axis, separated by ``2·t.length``. The gripper assembly + has to swing around the wrist motor so the chosen finger faces the + target direction; both solutions still place the grip center exactly + at the requested location (verified by FK round-trip).""" pose = CartesianPose( location=Coordinate(x=0, y=300, z=0), rotation=Rotation(z=0) ) @@ -350,9 +348,29 @@ def test_ik_elbow_differs_by_twice_gripper_length(self): g_pr = GripperParams(length=15.0, z_offset=3.0, finger_side="proximity_sensor") j_bc = kinematics.ik(pose, c, g_bc) j_pr = kinematics.ik(pose, c, g_pr) - self.assertAlmostEqual(j_bc[Axis.SHOULDER], 0.0, places=9) - self.assertAlmostEqual(j_pr[Axis.SHOULDER], 0.0, places=9) - self.assertAlmostEqual(j_bc[Axis.ELBOW] - j_pr[Axis.ELBOW], 2 * g_bc.length, places=9) + + # Both solutions land the grip center on the requested point. + back_bc = kinematics.fk(j_bc, c, g_bc) + back_pr = kinematics.fk(j_pr, c, g_pr) + for back in (back_bc, back_pr): + self.assertAlmostEqual(back.location.x, pose.location.x, places=9) + self.assertAlmostEqual(back.location.y, pose.location.y, places=9) + self.assertAlmostEqual(back.location.z, pose.location.z, places=9) + self.assertAlmostEqual(back.rotation.z, pose.rotation.z, places=9) + + # Wrist axes (= grip center − t.length·) sit on opposite + # sides of the grip center → distance between them is 2·t.length. + def wrist_xy(j): + r = ( + c.wrist_offset + c.elbow_offset + c.elbow_zero_offset + j[Axis.ELBOW] + ) + sh = math.radians(j[Axis.SHOULDER]) + return (-r * math.sin(sh), r * math.cos(sh)) + + wx_bc, wy_bc = wrist_xy(j_bc) + wx_pr, wy_pr = wrist_xy(j_pr) + sep = math.hypot(wx_bc - wx_pr, wy_bc - wy_pr) + self.assertAlmostEqual(sep, 2.0 * g_bc.length, places=6) class ShoulderSnapAt180(unittest.TestCase): From f3bccd590a9481d3e1f69f1f4089926f6057176b Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Tue, 12 May 2026 21:14:51 -0700 Subject: [PATCH 89/93] =?UTF-8?q?Arm:=20rename=20request=5Fgripper=5Flocat?= =?UTF-8?q?ion=20=E2=86=92=20request=5Fgripper=5Fpose?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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. --- .../brooks/precise_flex/hello-world.ipynb | 2 +- docs/user_guide/capabilities/arms.md | 2 +- docs/user_guide/ufactory/xarm6/hello-world.ipynb | 4 ++-- pylabrobot/brooks/pf400_test.ipynb | 16 ++++++++-------- pylabrobot/brooks/precise_flex.py | 2 +- pylabrobot/capabilities/arms/arm.py | 4 ++-- pylabrobot/capabilities/arms/backend.py | 4 ++-- pylabrobot/hamilton/liquid_handlers/star/core.py | 4 ++-- .../hamilton/liquid_handlers/star/iswap.py | 8 ++++---- .../arms/precise_flex/precise_flex_backend.py | 2 +- .../backends/hamilton/STAR_backend.py | 2 +- .../legacy/liquid_handling/liquid_handler.py | 4 ++-- pylabrobot/ufactory/xarm6/backend.py | 2 +- pylabrobot/ufactory/xarm6/backend_tests.py | 4 ++-- 14 files changed, 30 insertions(+), 30 deletions(-) diff --git a/docs/user_guide/brooks/precise_flex/hello-world.ipynb b/docs/user_guide/brooks/precise_flex/hello-world.ipynb index 236ba7be744..c798dd122c2 100644 --- a/docs/user_guide/brooks/precise_flex/hello-world.ipynb +++ b/docs/user_guide/brooks/precise_flex/hello-world.ipynb @@ -132,7 +132,7 @@ } ], "source": [ - "c = await pf.arm.backend.request_gripper_location()\n", + "c = await pf.arm.backend.request_gripper_pose()\n", "j = await pf.arm.backend.request_joint_position()\n", "print(c)\n", "print(j)" diff --git a/docs/user_guide/capabilities/arms.md b/docs/user_guide/capabilities/arms.md index f9a039122ae..38601786f54 100644 --- a/docs/user_guide/capabilities/arms.md +++ b/docs/user_guide/capabilities/arms.md @@ -65,7 +65,7 @@ await lh.iswap.drop_resource(reader, direction=GripDirection.FRONT) - **`pickup_distance_from_bottom`** controls how far up from the bottom of the resource the gripper grips. If `None`, the resource's `preferred_pickup_location` is used, or a default of 5 mm from the top (`size_z - 5`). - **Resource tree is updated automatically.** After a successful `drop_resource`, the resource is unassigned from its old parent and assigned to the destination. - **`GripOrientation`** is either a {class}`~pylabrobot.capabilities.arms.standard.GripDirection` enum (`FRONT`, `RIGHT`, `BACK`, `LEFT`) or a float in degrees. -- **`request_gripper_location()`** queries the hardware for the current end effector position. `get_picked_up_resource()` returns the internally tracked state (no hardware call). +- **`request_gripper_pose()`** queries the hardware for the current end effector position. `get_picked_up_resource()` returns the internally tracked state (no hardware call). ## Supported hardware diff --git a/docs/user_guide/ufactory/xarm6/hello-world.ipynb b/docs/user_guide/ufactory/xarm6/hello-world.ipynb index ee8ae92a67c..43cc3fe9b6e 100644 --- a/docs/user_guide/ufactory/xarm6/hello-world.ipynb +++ b/docs/user_guide/ufactory/xarm6/hello-world.ipynb @@ -107,7 +107,7 @@ { "cell_type": "code", "id": "xarm6-query-cart", - "source": "await xarm.arm.backend.request_gripper_location()", + "source": "await xarm.arm.backend.request_gripper_pose()", "metadata": {}, "execution_count": null, "outputs": [] @@ -171,7 +171,7 @@ { "cell_type": "code", "id": "d3d63cd0", - "source": "# Manually guide the arm to the desired pose, then read it:\ntaught = await xarm.arm.backend.request_gripper_location()\nprint(taught)", + "source": "# Manually guide the arm to the desired pose, then read it:\ntaught = await xarm.arm.backend.request_gripper_pose()\nprint(taught)", "metadata": {}, "execution_count": null, "outputs": [] diff --git a/pylabrobot/brooks/pf400_test.ipynb b/pylabrobot/brooks/pf400_test.ipynb index ae984684660..037b77b8efb 100644 --- a/pylabrobot/brooks/pf400_test.ipynb +++ b/pylabrobot/brooks/pf400_test.ipynb @@ -102,7 +102,7 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": "cart = await arm.request_gripper_location()\nprint(f\"Cartesian: x={cart.location.x:.1f}, y={cart.location.y:.1f}, z={cart.location.z:.1f}\")\nprint(\n f\"Rotation: yaw={cart.rotation.z:.1f}, pitch={cart.rotation.y:.1f}, roll={cart.rotation.x:.1f}\"\n)\nprint(f\"Elbow: {cart.orientation}\")\nprint()\njoints = await arm.request_joint_position()\nprint(f\"Joints: {joints}\")" + "source": "cart = await arm.request_gripper_pose()\nprint(f\"Cartesian: x={cart.location.x:.1f}, y={cart.location.y:.1f}, z={cart.location.z:.1f}\")\nprint(\n f\"Rotation: yaw={cart.rotation.z:.1f}, pitch={cart.rotation.y:.1f}, roll={cart.rotation.x:.1f}\"\n)\nprint(f\"Elbow: {cart.orientation}\")\nprint()\njoints = await arm.request_joint_position()\nprint(f\"Joints: {joints}\")" }, { "cell_type": "markdown", @@ -128,7 +128,7 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": "# Read position while in freedrive\ncart = await arm.request_gripper_location()\nprint(\n f\"x={cart.location.x:.1f}, y={cart.location.y:.1f}, z={cart.location.z:.1f}, \"\n f\"yaw={cart.rotation.z:.1f}, orientation={cart.orientation}\"\n)" + "source": "# Read position while in freedrive\ncart = await arm.request_gripper_pose()\nprint(\n f\"x={cart.location.x:.1f}, y={cart.location.y:.1f}, z={cart.location.z:.1f}, \"\n f\"yaw={cart.rotation.z:.1f}, orientation={cart.orientation}\"\n)" }, { "cell_type": "code", @@ -154,14 +154,14 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": "# Save current position with a name\npos = await arm.request_gripper_location()\nsave_position(\"home\", pos)" + "source": "# Save current position with a name\npos = await arm.request_gripper_pose()\nsave_position(\"home\", pos)" }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], - "source": "# Save another position\npos = await arm.request_gripper_location()\nsave_position(\"plate_pickup\", pos)" + "source": "# Save another position\npos = await arm.request_gripper_pose()\nsave_position(\"plate_pickup\", pos)" }, { "cell_type": "code", @@ -221,7 +221,7 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": "await arm.move_to_location(\n location=Coordinate(300, 0, 200),\n direction=Rotation(0, 0, 0),\n backend_params=PreciseFlexBackend.MoveToLocationParams(speed=20),\n)\nprint(await arm.request_gripper_location())" + "source": "await arm.move_to_location(\n location=Coordinate(300, 0, 200),\n direction=Rotation(0, 0, 0),\n backend_params=PreciseFlexBackend.MoveToLocationParams(speed=20),\n)\nprint(await arm.request_gripper_pose())" }, { "cell_type": "markdown", @@ -237,21 +237,21 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": "await arm.move_to_location(\n location=Coordinate(300, 0, 200),\n direction=Rotation(0, 0, 0),\n backend_params=PreciseFlexBackend.MoveToLocationParams(\n orientation=ElbowOrientation.RIGHT,\n speed=30,\n ),\n)\nprint(\"Righty:\", await arm.request_gripper_location())" + "source": "await arm.move_to_location(\n location=Coordinate(300, 0, 200),\n direction=Rotation(0, 0, 0),\n backend_params=PreciseFlexBackend.MoveToLocationParams(\n orientation=ElbowOrientation.RIGHT,\n speed=30,\n ),\n)\nprint(\"Righty:\", await arm.request_gripper_pose())" }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], - "source": "await arm.move_to_location(\n location=Coordinate(300, 0, 200),\n direction=Rotation(0, 0, 0),\n backend_params=PreciseFlexBackend.MoveToLocationParams(\n orientation=ElbowOrientation.LEFT,\n speed=30,\n ),\n)\nprint(\"Lefty:\", await arm.request_gripper_location())" + "source": "await arm.move_to_location(\n location=Coordinate(300, 0, 200),\n direction=Rotation(0, 0, 0),\n backend_params=PreciseFlexBackend.MoveToLocationParams(\n orientation=ElbowOrientation.LEFT,\n speed=30,\n ),\n)\nprint(\"Lefty:\", await arm.request_gripper_pose())" }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], - "source": "await backend.change_config()\nprint(\"Flipped:\", await arm.request_gripper_location())" + "source": "await backend.change_config()\nprint(\"Flipped:\", await arm.request_gripper_pose())" }, { "cell_type": "markdown", diff --git a/pylabrobot/brooks/precise_flex.py b/pylabrobot/brooks/precise_flex.py index 6ed9bb4598e..7497500f089 100644 --- a/pylabrobot/brooks/precise_flex.py +++ b/pylabrobot/brooks/precise_flex.py @@ -572,7 +572,7 @@ async def request_joint_position( raise PreciseFlexError(-1, "Unexpected response format from wherej command.") return self._parse_angles_response(parts) - async def request_gripper_location( + async def request_gripper_pose( self, backend_params: Optional[BackendParams] = None ) -> PreciseFlexGripperLocation: """Get the current pose using our kinematics model (no firmware `wherec`).""" diff --git a/pylabrobot/capabilities/arms/arm.py b/pylabrobot/capabilities/arms/arm.py index 82242803b4c..686828ce85b 100644 --- a/pylabrobot/capabilities/arms/arm.py +++ b/pylabrobot/capabilities/arms/arm.py @@ -72,11 +72,11 @@ async def park(self, backend_params: Optional[BackendParams] = None) -> None: """Park the arm to its default position.""" return await self.backend.park(backend_params=backend_params) - async def request_gripper_location( + async def request_gripper_pose( self, backend_params: Optional[BackendParams] = None ) -> GripperLocation: """Get the current location and rotation of the gripper.""" - return await self.backend.request_gripper_location(backend_params=backend_params) + return await self.backend.request_gripper_pose(backend_params=backend_params) # -- holding state ----------------------------------------------------------- diff --git a/pylabrobot/capabilities/arms/backend.py b/pylabrobot/capabilities/arms/backend.py index 817848958fa..26d35a2ce1f 100644 --- a/pylabrobot/capabilities/arms/backend.py +++ b/pylabrobot/capabilities/arms/backend.py @@ -10,7 +10,7 @@ # - pick_up_at_location # - drop_at_location # - move_to_location -# - request_gripper_location +# - request_gripper_pose # - is_holding_resource # CanGrip @@ -117,7 +117,7 @@ async def park(self, backend_params: Optional[BackendParams] = None) -> None: """Park the arm to its default position.""" @abstractmethod - async def request_gripper_location( + async def request_gripper_pose( self, backend_params: Optional[BackendParams] = None ) -> GripperLocation: """Get the current location and rotation of the gripper.""" diff --git a/pylabrobot/hamilton/liquid_handlers/star/core.py b/pylabrobot/hamilton/liquid_handlers/star/core.py index 33c4e8caa86..1cc69386fe8 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/core.py +++ b/pylabrobot/hamilton/liquid_handlers/star/core.py @@ -24,8 +24,8 @@ def __init__(self, driver: STARDriver): # -- lifecycle -------------------------------------------------------------- - async def request_gripper_location(self, backend_params=None) -> GripperLocation: - raise NotImplementedError("CoreGripper does not support request_gripper_location") + async def request_gripper_pose(self, backend_params=None) -> GripperLocation: + raise NotImplementedError("CoreGripper does not support request_gripper_pose") # -- ArmBackend interface --------------------------------------------------- diff --git a/pylabrobot/hamilton/liquid_handlers/star/iswap.py b/pylabrobot/hamilton/liquid_handlers/star/iswap.py index 969967ad3ac..691d614d7ad 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/iswap.py +++ b/pylabrobot/hamilton/liquid_handlers/star/iswap.py @@ -71,7 +71,7 @@ def use_traversal_height(self, height: float): finally: self.traversal_height = original - async def request_gripper_location(self, backend_params=None) -> GripperLocation: + async def request_gripper_pose(self, backend_params=None) -> GripperLocation: """Request iSWAP grip center position (C0 QG). Returns: @@ -224,17 +224,17 @@ async def rotation_drive_request_y(self) -> float: async def move_x(self, x: float) -> None: """Move iSWAP X to an absolute position [mm].""" - loc = (await self.request_gripper_location()).location + loc = (await self.request_gripper_pose()).location await self.move_relative_x(step_size=x - loc.x, allow_splitting=True) async def move_y(self, y: float) -> None: """Move iSWAP Y to an absolute position [mm].""" - loc = (await self.request_gripper_location()).location + loc = (await self.request_gripper_pose()).location await self.move_relative_y(step_size=y - loc.y, allow_splitting=True) async def move_z(self, z: float) -> None: """Move iSWAP Z to an absolute position [mm].""" - loc = (await self.request_gripper_location()).location + loc = (await self.request_gripper_pose()).location await self.move_relative_z(step_size=z - loc.z, allow_splitting=True) # -- rotation / wrist drive ------------------------------------------------ diff --git a/pylabrobot/legacy/arms/precise_flex/precise_flex_backend.py b/pylabrobot/legacy/arms/precise_flex/precise_flex_backend.py index 9e241af999b..912a4f7cf29 100644 --- a/pylabrobot/legacy/arms/precise_flex/precise_flex_backend.py +++ b/pylabrobot/legacy/arms/precise_flex/precise_flex_backend.py @@ -261,7 +261,7 @@ async def get_joint_position(self) -> Dict[int, float]: return await self._new_backend.request_joint_position() async def get_cartesian_position(self) -> PreciseFlexCartesianCoords: - result = await self._new_backend.request_gripper_location() + result = await self._new_backend.request_gripper_pose() return _from_new_coords(result) async def send_command(self, command: str) -> str: diff --git a/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py index 6c0be7d8b92..d04ca23deda 100644 --- a/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py @@ -7390,7 +7390,7 @@ async def request_plate_in_iswap(self) -> bool: async def request_iswap_position(self) -> Coordinate: """Deprecated: use ``star.iswap.get_gripper_location()``.""" - return (await self._iswap.request_gripper_location()).location + return (await self._iswap.request_gripper_pose()).location async def iswap_rotation_drive_request_y(self) -> float: """Deprecated: use ``star.iswap.rotation_drive_request_y()``.""" diff --git a/pylabrobot/legacy/liquid_handling/liquid_handler.py b/pylabrobot/legacy/liquid_handling/liquid_handler.py index 6966dbdbdb7..306a656853f 100644 --- a/pylabrobot/legacy/liquid_handling/liquid_handler.py +++ b/pylabrobot/legacy/liquid_handling/liquid_handler.py @@ -350,8 +350,8 @@ async def halt(self, backend_params=None): async def park(self, backend_params=None): pass - async def request_gripper_location(self, backend_params=None) -> GripperLocation: - raise NotImplementedError("request_gripper_location not available via legacy adapter") + async def request_gripper_pose(self, backend_params=None) -> GripperLocation: + raise NotImplementedError("request_gripper_pose not available via legacy adapter") async def open_gripper(self, gripper_width, backend_params=None): pass diff --git a/pylabrobot/ufactory/xarm6/backend.py b/pylabrobot/ufactory/xarm6/backend.py index b4bfb62a7b7..dc5fa3e0ba5 100644 --- a/pylabrobot/ufactory/xarm6/backend.py +++ b/pylabrobot/ufactory/xarm6/backend.py @@ -166,7 +166,7 @@ async def park(self, backend_params: Optional[BackendParams] = None) -> None: num_retries=1, ) - async def request_gripper_location( + async def request_gripper_pose( self, backend_params: Optional[BackendParams] = None ) -> GripperLocation: """Get the current gripper location and rotation.""" diff --git a/pylabrobot/ufactory/xarm6/backend_tests.py b/pylabrobot/ufactory/xarm6/backend_tests.py index 9977d277d5d..058f0b9b6bc 100644 --- a/pylabrobot/ufactory/xarm6/backend_tests.py +++ b/pylabrobot/ufactory/xarm6/backend_tests.py @@ -104,8 +104,8 @@ async def test_park_with_location(self): self.assertEqual(set_pos_calls[0].kwargs["y"], 0) self.assertEqual(set_pos_calls[0].kwargs["z"], 300) - async def test_request_gripper_location(self): - location = await self.backend.request_gripper_location() + async def test_request_gripper_pose(self): + location = await self.backend.request_gripper_pose() self.assertEqual(location.location.x, 100) self.assertEqual(location.location.y, 200) self.assertEqual(location.location.z, 300) From bc71517f782f8fb8f8c54dbd7acad5b86942d524 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sat, 16 May 2026 21:18:54 -0700 Subject: [PATCH 90/93] KX2: smooth PVT trajectories via move_parametric / move_through_waypoints MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- .../paa/kx2/continuous-motion.ipynb | 382 ++++++++++++++++++ pylabrobot/paa/kx2/arm_backend.py | 288 +++++++++---- pylabrobot/paa/kx2/driver.py | 34 +- pylabrobot/paa/kx2/kinematics.py | 251 +++++++++++- ...path_tests.py => arm_backend_pvt_tests.py} | 34 +- pylabrobot/paa/kx2/tests/kinematics_tests.py | 17 +- 6 files changed, 896 insertions(+), 110 deletions(-) create mode 100644 docs/user_guide/paa/kx2/continuous-motion.ipynb rename pylabrobot/paa/kx2/tests/{arm_backend_linear_path_tests.py => arm_backend_pvt_tests.py} (89%) diff --git a/docs/user_guide/paa/kx2/continuous-motion.ipynb b/docs/user_guide/paa/kx2/continuous-motion.ipynb new file mode 100644 index 00000000000..035b7c6d9df --- /dev/null +++ b/docs/user_guide/paa/kx2/continuous-motion.ipynb @@ -0,0 +1,382 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "kx2-cm-intro", + "metadata": {}, + "source": [ + "# Continuous motion (PVT)\n", + "\n", + "The KX2 supports streamed Cartesian trajectories via the Elmo drives' Interpolated Position Mode (IPM). The host samples a target path at the drive's interpolation cadence (8 ms / 125 Hz), pushes (position, velocity) frames into a 16-deep ring buffer ~8 frames ahead of the read pointer, and lets the drive interpolate cubic-Hermite splines between them. The result is smooth, continuous motion — no PPM-style stop-and-restart between segments.\n", + "\n", + "Three APIs use this transport:\n", + "\n", + "| API | Input | Use case |\n", + "|---|---|---|\n", + "| `arm.move_to_location(..., path='linear')` | one target pose | a single straight-line Cartesian move |\n", + "| `arm.backend.move_parametric(fn, duration_s)` | a callable `t → pose` | math-defined paths (figure-8, helix, Lissajous) |\n", + "| `arm.backend.move_through_waypoints(poses, speed, accel)` | a list of poses | smooth pass-through any sequence of points |\n", + "\n", + "All three share the same underlying streamer, EMCY handling, and IPM cleanup. See [hello-world](hello-world.ipynb) for the rest of the KX2 API." + ] + }, + { + "cell_type": "markdown", + "id": "kx2-cm-setup-header", + "metadata": {}, + "source": [ + "## Setup" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "id": "kx2-cm-setup", + "metadata": { + "execution": { + "iopub.execute_input": "2026-05-17T03:51:14.542746Z", + "iopub.status.busy": "2026-05-17T03:51:14.542612Z", + "iopub.status.idle": "2026-05-17T03:51:23.563072Z", + "shell.execute_reply": "2026-05-17T03:51:23.561338Z" + } + }, + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "uptime library not available, timestamps are relative to boot time and not to Epoch UTC\n", + "2026-05-16 21:16:47,364 - pylabrobot.paa.kx2.driver - INFO - canopen: connected, nodes=[1, 2, 3, 4, 6]\n" + ] + } + ], + "source": [ + "import math\n", + "from pylabrobot.paa.kx2 import KX2, KX2ArmBackend\n", + "from pylabrobot.capabilities.arms.standard import CartesianPose\n", + "from pylabrobot.resources import Coordinate, Rotation\n", + "\n", + "kx2 = KX2()\n", + "await kx2.setup()" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-cm-linear-header", + "metadata": {}, + "source": [ + "## 1. Straight-line move\n", + "\n", + "`path='linear'` on the existing `move_to_location` swaps PPM for the IPM streamer. The gripper tip traces a straight line from current pose to target, sampled at 8 ms intervals along a trapezoidal arc-length profile. `max_gripper_speed` (mm/s) and `max_gripper_acceleration` (mm/s²) are required — the host builds the profile directly from them (no firmware fallback for streamed motion).\n", + "\n", + "End-of-motion drops a few frames of the trajectory tail (~0.3 mm at 50 mm/s); for tighter terminal-position needs, fall through to the joint-mode `move_to_location` (default `path='joint'`)." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "kx2-cm-linear", + "metadata": { + "execution": { + "iopub.execute_input": "2026-05-17T03:51:23.567365Z", + "iopub.status.busy": "2026-05-17T03:51:23.567076Z", + "iopub.status.idle": "2026-05-17T03:51:24.343056Z", + "shell.execute_reply": "2026-05-17T03:51:24.342528Z" + } + }, + "outputs": [], + "source": [ + "start = await kx2.arm.request_gripper_pose()\n", + "target = Coordinate(x=start.location.x, y=start.location.y, z=start.location.z + 20)\n", + "await kx2.arm.move_to_location(\n", + " location=target,\n", + " direction=start.rotation.z,\n", + " backend_params=KX2ArmBackend.CartesianMoveParams(\n", + " max_gripper_speed=50.0, max_gripper_acceleration=200.0, path='linear',\n", + " ),\n", + ")" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-cm-parametric-header", + "metadata": {}, + "source": [ + "## 2. Parametric trajectories\n", + "\n", + "`move_parametric(path_fn, duration_s)` calls `path_fn(t)` at every IPM sample time (`t ∈ [0, duration_s]` seconds) and streams the resulting poses. The function defines *both* shape and pacing — there's no separate speed/accel cap, so the caller is responsible for keeping derivatives within drive limits.\n", + "\n", + "IK runs on every sample with mandatory joint-limit enforcement; an out-of-range pose anywhere along the trajectory raises `IKError` **before any motion starts**.\n", + "\n", + "### Figure 8 (lemniscate)" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "kx2-cm-fig8", + "metadata": { + "execution": { + "iopub.execute_input": "2026-05-17T03:51:24.344548Z", + "iopub.status.busy": "2026-05-17T03:51:24.344461Z", + "iopub.status.idle": "2026-05-17T03:51:34.491757Z", + "shell.execute_reply": "2026-05-17T03:51:34.491240Z" + } + }, + "outputs": [], + "source": [ + "start = await kx2.arm.request_gripper_pose()\n", + "Rx, Ry, H, T = 25.0, 12.0, 20.0, 10.0 # 50mm wide, 25mm tall lemniscate, 30mm Z arch, 8s\n", + "\n", + "def fig8(t):\n", + " s = t / T\n", + " theta = 2 * math.pi * s\n", + " return CartesianPose(\n", + " location=Coordinate(\n", + " x=start.location.x + Rx * math.sin(theta),\n", + " y=start.location.y + Ry * math.sin(2 * theta) / 2,\n", + " z=start.location.z + H * math.sin(math.pi * s),\n", + " ),\n", + " rotation=start.rotation,\n", + " )\n", + "\n", + "await kx2.arm.backend.move_parametric(fig8, duration_s=T)" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-cm-helix-header", + "metadata": {}, + "source": [ + "### Rising helix\n", + "\n", + "Constant-radius circle in XY, linear ramp in Z. Both endpoints land back at the start XY but the Z is offset." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "kx2-cm-helix", + "metadata": { + "execution": { + "iopub.execute_input": "2026-05-17T03:51:34.493405Z", + "iopub.status.busy": "2026-05-17T03:51:34.493307Z", + "iopub.status.idle": "2026-05-17T03:51:40.615193Z", + "shell.execute_reply": "2026-05-17T03:51:40.614590Z" + } + }, + "outputs": [], + "source": [ + "start = await kx2.arm.request_gripper_pose()\n", + "R, dZ, turns, T = 30.0, 40.0, 2.0, 6.0\n", + "\n", + "def helix(t):\n", + " s = t / T\n", + " theta = 2 * math.pi * turns * s\n", + " return CartesianPose(\n", + " location=Coordinate(\n", + " x=start.location.x + R * math.sin(theta),\n", + " y=start.location.y + R * (1 - math.cos(theta)),\n", + " z=start.location.z + dZ * s,\n", + " ),\n", + " rotation=start.rotation,\n", + " )\n", + "\n", + "await kx2.arm.backend.move_parametric(helix, duration_s=T)" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-cm-lissa-header", + "metadata": {}, + "source": [ + "### 3D Lissajous\n", + "\n", + "Different per-axis frequencies trace intricate closed (or quasi-closed) curves in space. Tune for the shape and for the size; conservative numbers keep elbow/shoulder joint velocity well inside drive limits." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "kx2-cm-lissa", + "metadata": { + "execution": { + "iopub.execute_input": "2026-05-17T03:51:40.616597Z", + "iopub.status.busy": "2026-05-17T03:51:40.616509Z", + "iopub.status.idle": "2026-05-17T03:51:56.740932Z", + "shell.execute_reply": "2026-05-17T03:51:56.740396Z" + } + }, + "outputs": [], + "source": [ + "start = await kx2.arm.request_gripper_pose()\n", + "Ax, Ay, Az = 12.0, 8.0, 8.0 # small amplitudes keep joint velocity inside drive limits\n", + "a, b, c = 2, 1, 1 # frequency ratios\n", + "T = 16.0 # seconds — long enough to keep speeds modest\n", + "\n", + "def lissa(t):\n", + " s = t / T\n", + " return CartesianPose(\n", + " location=Coordinate(\n", + " x=start.location.x + Ax * math.sin(2 * math.pi * a * s),\n", + " y=start.location.y + Ay * math.sin(2 * math.pi * b * s + math.pi / 4),\n", + " z=start.location.z + Az * math.sin(2 * math.pi * c * s),\n", + " ),\n", + " rotation=start.rotation,\n", + " )\n", + "\n", + "await kx2.arm.backend.move_parametric(lissa, duration_s=T)" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-cm-waypoints-header", + "metadata": {}, + "source": [ + "## 3. Smooth waypoints\n", + "\n", + "`move_through_waypoints(poses, speed, accel)` fits a centripetal Catmull-Rom spline through the list of poses and reparametrizes it with a trapezoidal arc-length profile capped at `speed` (mm/s) and `accel` (mm/s²). The curve is C¹ continuous — there's no stop at intermediate waypoints.\n", + "\n", + "Useful when you have a sequence of named locations (sample sites, plate corners, scan path) and want smooth motion through them." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "kx2-cm-waypoints", + "metadata": { + "execution": { + "iopub.execute_input": "2026-05-17T03:51:56.742312Z", + "iopub.status.busy": "2026-05-17T03:51:56.742223Z", + "iopub.status.idle": "2026-05-17T03:52:01.715157Z", + "shell.execute_reply": "2026-05-17T03:52:01.714719Z" + } + }, + "outputs": [], + "source": [ + "start = await kx2.arm.request_gripper_pose()\n", + "\n", + "# Square corners + a Z hop at one corner — gripper rounds each corner smoothly\n", + "# rather than stopping at each.\n", + "def at(dx=0, dy=0, dz=0):\n", + " return CartesianPose(\n", + " location=Coordinate(start.location.x + dx, start.location.y + dy, start.location.z + dz),\n", + " rotation=start.rotation,\n", + " )\n", + "\n", + "waypoints = [\n", + " at(),\n", + " at(dx=20),\n", + " at(dx=20, dy=20),\n", + " at(dx=20, dy=20, dz=15), # hop up\n", + " at(dx=20, dy=20), # back down\n", + " at(dy=20),\n", + " at(),\n", + "]\n", + "\n", + "await kx2.arm.backend.move_through_waypoints(waypoints, speed=25.0, accel=80.0)" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-cm-zigzag-header", + "metadata": {}, + "source": [ + "### Zig-zag scan\n", + "\n", + "Useful pattern for sweeping over a rectangular region (e.g. raster scanning a plate). The Catmull-Rom spline rounds each corner so the gripper never has to come to a stop." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "id": "kx2-cm-zigzag", + "metadata": { + "execution": { + "iopub.execute_input": "2026-05-17T03:52:01.716905Z", + "iopub.status.busy": "2026-05-17T03:52:01.716784Z", + "iopub.status.idle": "2026-05-17T03:52:10.558103Z", + "shell.execute_reply": "2026-05-17T03:52:10.557606Z" + } + }, + "outputs": [], + "source": [ + "start = await kx2.arm.request_gripper_pose()\n", + "rows, row_dy, sweep_dx = 4, 10.0, 50.0\n", + "\n", + "waypoints = []\n", + "for r in range(rows):\n", + " y = r * row_dy\n", + " x_left, x_right = (0.0, sweep_dx) if r % 2 == 0 else (sweep_dx, 0.0)\n", + " waypoints.append(CartesianPose(\n", + " location=Coordinate(start.location.x + x_left, start.location.y + y, start.location.z),\n", + " rotation=start.rotation,\n", + " ))\n", + " waypoints.append(CartesianPose(\n", + " location=Coordinate(start.location.x + x_right, start.location.y + y, start.location.z),\n", + " rotation=start.rotation,\n", + " ))\n", + "\n", + "await kx2.arm.backend.move_through_waypoints(waypoints, speed=30.0, accel=100.0)" + ] + }, + { + "cell_type": "markdown", + "id": "kx2-cm-safety-header", + "metadata": {}, + "source": [ + "## Safety notes\n", + "\n", + "- **Joint limits are enforced by IK**: any pose whose IK solution falls outside the per-axis `[min_travel, max_travel]` raises `IKError` before any frame leaves the host. Catches workspace-violating trajectories at sample time, not at the drive.\n", + "- **Drive velocity / acceleration limits are NOT enforced**. `move_parametric` lets the caller define the velocity profile, and `move_through_waypoints` only caps Cartesian speed/accel — neither checks per-axis encoder rates. A swirly trajectory near a kinematic singularity can demand joint speeds way above what the motors can produce; the drives will either lag or fault. Stay inside the workspace and use moderate speed/accel.\n", + "- **Cancel/halt during streaming** drops ip-enable and lets the drive consume already-buffered frames before stopping. Coast can run up to ~64 ms past the cancel. For true zero-coast stop, use `await kx2.arm.backend.halt()` (MO=0 on every motion axis)." + ] + }, + { + "cell_type": "markdown", + "id": "kx2-cm-teardown-header", + "metadata": {}, + "source": [ + "## Teardown" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "id": "kx2-cm-teardown", + "metadata": { + "execution": { + "iopub.execute_input": "2026-05-17T03:52:10.559447Z", + "iopub.status.busy": "2026-05-17T03:52:10.559377Z", + "iopub.status.idle": "2026-05-17T03:52:10.562092Z", + "shell.execute_reply": "2026-05-17T03:52:10.561785Z" + } + }, + "outputs": [], + "source": [ + "await kx2.stop()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "env", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.10.15" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index ea9e72ff022..a0f03087717 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -5,7 +5,7 @@ import warnings from contextlib import asynccontextmanager from enum import IntEnum -from typing import Dict, List, Literal, Optional, Union +from typing import Callable, Dict, List, Literal, Optional, Union from pylabrobot.capabilities.arms.backend import ( CanFreedrive, @@ -919,65 +919,66 @@ async def _cart_to_joints(self, pose: CartesianPose) -> Dict[Axis, float]: ik_joints = kinematics.ik(pose, self._cfg, self._gripper_params) return kinematics.snap_to_current(ik_joints, current) - async def _run_linear_path( - self, - target_pose: CartesianPose, - params: "KX2ArmBackend.CartesianMoveParams", + async def _stream_samples( + self, samples: "List[kinematics.LinearPathSample]" ) -> None: - """Stream a Cartesian-linear trajectory through the drive's IPM buffer. - - Caller MUST hold ``_motion_guard``. Picks up the current pose from the - drives, samples a straight-line tool-tip path to ``target_pose`` at the - fixed IPM cadence (``_LINEAR_PATH_DT_MS``), and feeds (P, V) points to - each axis 8 frames ahead of the drive's read pointer. + """Stream a pre-built list of LinearPathSamples through the drive's IPM + buffer. Caller MUST hold ``_motion_guard``. The samples' encoder positions + and velocities are fed to each axis 8 frames ahead of the drive's read + pointer at the fixed IPM cadence (``_PVT_DT_MS``). On cancel/exception the drive is brought back to PPM through a - ``finally`` block (``ipm_stop`` then ``ipm_select_mode(False)``), so a - halt mid-stream leaves the arm in a state that a subsequent joint move - can resume from. - - NOTE: cancel/halt drops ip-enable and lets the drive consume already- - buffered points before stopping. Coast can run up to - ``_LINEAR_PATH_PRELOAD * dt_ms`` (~64 ms at 8 ms / 8 frames) past the - cancel. If true zero-coast halt is needed, fall through to the - existing ``halt()`` (MO=0 across all motion axes), which is independent - of the linear path. + ``finally`` block. Coast on a cancel can run up to ``_PVT_PRELOAD + * dt_ms`` (~64 ms) past the cancel — see ``halt()`` for zero-coast stop. """ - if params.max_gripper_speed is None or params.max_gripper_acceleration is None: - raise ValueError( - "CartesianMoveParams(path='linear') requires max_gripper_speed and " - "max_gripper_acceleration: the Cartesian profile is built from them " - "directly (no firmware fallback for streamed motion)." - ) - - current_joints = { - Axis(k): v for k, v in (await self.request_joint_position()).items() - } - start_pose = kinematics.fk(current_joints, self._cfg, self._gripper_params) - samples = kinematics.sample_linear_path( - cfg=self._cfg, - gripper_params=self._gripper_params, - start_pose=start_pose, - end_pose=target_pose, - vel_mm_per_s=params.max_gripper_speed, - accel_mm_per_s2=params.max_gripper_acceleration, - dt_s=self._LINEAR_PATH_DT_MS / 1000.0, - current_joints=current_joints, - ) if len(samples) < 2: - # Zero-length path: same pose. Defensively flip mode back to PPM in - # case a prior linear move's cleanup failed and left _ipm_mode=True. await self.driver.ipm_select_mode(False) return - active_axes = [int(ax) for ax in (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST)] - dt_s = self._LINEAR_PATH_DT_MS / 1000.0 - preload = min(self._LINEAR_PATH_PRELOAD, len(samples)) + # Skip axes with small total motion (Δ ≤ _SKIP_AXIS_COUNTS). Two reasons: + # (1) the drive idles on sub-threshold motion and leaves frames stuck in + # the IP buffer; (2) rotary axes (shoulder, wrist) wrap — IK gives + # angles in (-180, 180] but the drive's encoder counts up across + # revolutions, so streaming raw IK values for a rotary axis that's + # currently many revolutions in causes the drive to interpret each + # command as a huge multi-rotation move and tracking-error fault. + all_axes = [int(ax) for ax in (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST)] + active_axes: List[int] = [] + for nid in all_axes: + seq = [s.encoder_position[Axis(nid)] for s in samples] + if max(seq) - min(seq) > self._SKIP_AXIS_COUNTS: + active_axes.append(nid) + if not active_axes: + return + + # Clear any sticky fault on the involved drives before streaming. + await self.driver.motors_ensure_enabled(active_axes) + + # Align the sample sequence to the drive's actual encoder positions. + # Rotary axes (shoulder, wrist) wrap — the drive's encoder counts up + # forever across full rotations, but IK gives joint angles in (-180, + # 180]; sample[0] therefore lands many revolutions away from the drive's + # actual position. Sending those raw triggers an immediate position- + # tracking fault (drive rejects RPDO3, looks like queue_full to us). + # Shift every sample's encoder_position by (actual_now - sample[0]) so + # the trajectory rides on top of the drive's current encoder, preserving + # all relative motion. + enc_now = {} + for nid in active_axes: + enc_now[nid] = await self.driver.motor_get_current_position(nid) + for nid in active_axes: + offset = enc_now[nid] - samples[0].encoder_position[Axis(nid)] + if offset == 0: + continue + for s in samples: + s.encoder_position[Axis(nid)] += offset + + dt_s = self._PVT_DT_MS / 1000.0 + preload = min(self._PVT_PRELOAD, len(samples)) await self.driver.ipm_select_mode(True) try: - await self.driver.ipm_set_time_interval(self._LINEAR_PATH_DT_MS) - + await self.driver.ipm_set_time_interval(self._PVT_DT_MS) # Preload PRELOAD frames per axis. The drive starts consuming as soon # as begin_motion fires; preload lets the producer fall behind by up # to (PRELOAD-1) * dt without underflowing the queue. @@ -989,21 +990,14 @@ async def _run_linear_path( samples[i].encoder_position[ax], samples[i].encoder_velocity[ax], ) - # Surface any preload-induced fault (queue_full from a malformed - # first point) before we kick off motion. self.driver.ipm_check_queue_fault(active_axes) - # Capture the pacing reference *before* begin_motion. The drive starts - # consuming the moment SYNC arrives inside begin_motion; capturing - # after means we underestimate elapsed drive-time and pace too late, - # eating into the underflow margin. Capturing before is conservative - # (we send slightly early), wasting a few buffer slots — strictly - # safer. + # Capture pacing reference *before* begin_motion (drive starts consuming + # on SYNC; capturing after underestimates elapsed drive-time and pace + # too late, eating into the underflow margin). start = time.monotonic() await self.driver.ipm_begin_motion(active_axes) - # Pace the rest: send sample i so it lands at t = (i - (PRELOAD-1)) * dt - # after the captured start. for i in range(preload, len(samples)): target_t = (i - (preload - 1)) * dt_s while time.monotonic() - start < target_t: @@ -1017,23 +1011,17 @@ async def _run_linear_path( ) self.driver.ipm_check_queue_fault(active_axes) - # Wait for SW bit-10 (target reached) on every axis. Bit-10 goes high - # once the IP buffer drains and the trajectory generator settles on - # the last commanded position. MS-based polling (`wait_for_moves_done`) - # would race the buffer drain — bit-10 is the authoritative IPM - # done-signal. - total_t = (len(samples) - 1) * dt_s - timeout = total_t + 2.0 - await self.driver.ipm_wait_motion_complete(active_axes, timeout_s=timeout) - # One more fault check after drain — covers an underflow in the very - # last frames that bit-10 wouldn't reflect. - self.driver.ipm_check_queue_fault(active_axes) + # Drop ip-enable immediately after the last frame. Elmo drives don't + # latch SW bit-10 (target_reached) while ip-enable is high — polling + # for it inside IP mode hangs forever — and leaving ip-enable asserted + # past the buffer drain raises EMCY 0x8A on the next tick. Mirrors C# + # MotorsMovePathExecute. Trade-off: drive halts ~0.3 mm short of the + # trajectory end at typical speeds (matches the vendor behaviour). + await self.driver.ipm_stop(active_axes) finally: try: await self.driver.ipm_stop(active_axes) except Exception: - # Log with EMCY state for post-mortem: if the drive was already - # mid-fault when we tried to stop, the EMCY counters tell us why. emcy_snap = { ax: vars(self.driver._emcy[ax]) for ax in active_axes if ax in self.driver._emcy @@ -1050,6 +1038,127 @@ async def _run_linear_path( "fresh-arm IPM via the re-arm path" ) + async def _run_linear_path( + self, + target_pose: CartesianPose, + params: "KX2ArmBackend.CartesianMoveParams", + ) -> None: + """Sample a straight tool-tip path to ``target_pose`` and stream it. + Caller MUST hold ``_motion_guard``.""" + if params.max_gripper_speed is None or params.max_gripper_acceleration is None: + raise ValueError( + "CartesianMoveParams(path='linear') requires max_gripper_speed and " + "max_gripper_acceleration: the Cartesian profile is built from them " + "directly (no firmware fallback for streamed motion)." + ) + current_joints = { + Axis(k): v for k, v in (await self.request_joint_position()).items() + } + start_pose = kinematics.fk(current_joints, self._cfg, self._gripper_params) + samples = kinematics.sample_linear_path( + cfg=self._cfg, + gripper_params=self._gripper_params, + start_pose=start_pose, + end_pose=target_pose, + vel_mm_per_s=params.max_gripper_speed, + accel_mm_per_s2=params.max_gripper_acceleration, + dt_s=self._PVT_DT_MS / 1000.0, + current_joints=current_joints, + ) + await self._stream_samples(samples) + + async def move_parametric( + self, + path_fn: "Callable[[float], CartesianPose]", + duration_s: float, + ) -> None: + """Stream a parametric Cartesian trajectory through IPM. + + ``path_fn(t)`` is called at every IPM sample time ``t ∈ [0, duration_s]`` + (seconds, evaluated at the drive's interpolation cadence, currently + 8 ms) and must return the absolute :class:`CartesianPose` the gripper + should occupy at that instant. Use the start pose if you need offsets: + + start = await arm.request_gripper_pose() + def fig8(t): + s = t / duration_s + theta = 2 * math.pi * s + return CartesianPose( + location=Coordinate(start.location.x + 50 * math.sin(theta), + start.location.y + 12.5 * math.sin(2*theta), + start.location.z + 30 * math.sin(math.pi * s)), + rotation=start.rotation, + ) + await arm.move_parametric(fig8, duration_s=8.0) + + No speed/accel cap — the parametrization sets the velocity profile; + callers are responsible for keeping derivatives within drive limits. + Joint travel limits are enforced via IK; out-of-range raises IKError + before any motion. Path continuity is the caller's responsibility — + discontinuities surface as drive faults. + """ + if duration_s <= 0: + raise ValueError(f"duration_s must be > 0, got {duration_s}") + async with self._motion_guard(): + current_joints = { + Axis(k): v for k, v in (await self.request_joint_position()).items() + } + samples = kinematics.sample_parametric_path( + cfg=self._cfg, + gripper_params=self._gripper_params, + path_fn=path_fn, + duration_s=duration_s, + dt_s=self._PVT_DT_MS / 1000.0, + current_joints=current_joints, + ) + await self._stream_samples(samples) + + async def move_through_waypoints( + self, + waypoints: "List[CartesianPose]", + *, + speed: float, + accel: float, + ) -> None: + """Stream a smooth Catmull-Rom spline through ``waypoints``. + + The curve passes through every waypoint with C¹ continuity (no stop at + intermediate waypoints). Tangents at each interior waypoint are derived + from neighbours; endpoint tangents are extrapolated from the first/last + segment. The whole spline is time-reparametrized into a trapezoidal + arc-length profile with peak Cartesian speed ``speed`` (mm/s) and peak + acceleration ``accel`` (mm/s²), then sampled at the IPM cadence. + + Args: + waypoints: at least 2 absolute Cartesian poses. The first should + match the current pose closely (or the path will start with a + Cartesian-linear segment to get there). + speed: peak Cartesian speed along the spline, mm/s. + accel: peak Cartesian acceleration, mm/s². + + Raises: + ValueError if waypoints < 2 or speed/accel <= 0. + IKError if any sample's joints are out of range. + """ + if len(waypoints) < 2: + raise ValueError(f"need at least 2 waypoints, got {len(waypoints)}") + if speed <= 0 or accel <= 0: + raise ValueError(f"speed and accel must be > 0, got {speed}, {accel}") + async with self._motion_guard(): + current_joints = { + Axis(k): v for k, v in (await self.request_joint_position()).items() + } + samples = kinematics.sample_waypoint_path( + cfg=self._cfg, + gripper_params=self._gripper_params, + waypoints=waypoints, + speed_mm_per_s=speed, + accel_mm_per_s2=accel, + dt_s=self._PVT_DT_MS / 1000.0, + current_joints=current_joints, + ) + await self._stream_samples(samples) + # -- capability interface (OrientableGripperArmBackend + HasJoints + CanFreedrive) -- async def halt(self, backend_params: Optional[BackendParams] = None) -> None: @@ -1062,10 +1171,31 @@ async def halt(self, backend_params: Optional[BackendParams] = None) -> None: *(self.driver.motor_emergency_stop(node_id=axis) for axis in MOTION_AXES) ) + # Park pose (centered, well inside workspace) and motion caps. + _PARK_JOINTS: Dict[Axis, float] = { + Axis.SHOULDER: 0.0, Axis.Z: 300.0, Axis.ELBOW: 250.0, Axis.WRIST: 0.0, + } + _PARK_SPEED: float = 80.0 + _PARK_ACCEL: float = 400.0 + async def park(self, backend_params: Optional[BackendParams] = None) -> None: - raise NotImplementedError( - "KX2 does not define a default park pose. Use move_to_joint_position with a " - "site-specific safe configuration." + """Move the arm to a centered safe pose via a Cartesian-linear (IPM) move. + + Uses IPM (`path='linear'`) rather than PPM (move_to_joint_position) so the + drives stay in mode 7 — back-to-back PPM → IPM transitions leave one or + more drives' IP buffers in a state where the next preload write hits + EMCY 0x34/0xBA (queue_full on first write). Sticking to IPM throughout + sidesteps that. + """ + park_pose = kinematics.fk(self._PARK_JOINTS, self._cfg, self._gripper_params) + await self.move_to_location( + location=park_pose.location, + direction=park_pose.rotation.z, + backend_params=KX2ArmBackend.CartesianMoveParams( + max_gripper_speed=self._PARK_SPEED, + max_gripper_acceleration=self._PARK_ACCEL, + path="linear", + ), ) async def request_gripper_pose( @@ -1135,8 +1265,12 @@ class CartesianMoveParams(BackendParams): # `ipm_set_time_interval` validates this at the wire level. (b) PRELOAD < # drive IP buffer depth (16, set by the 24772:2=16 write at setup) — # otherwise the (PRELOAD+1)th frame queue_fulls immediately. - _LINEAR_PATH_DT_MS: int = 8 - _LINEAR_PATH_PRELOAD: int = 8 + _PVT_DT_MS: int = 8 + _PVT_PRELOAD: int = 8 + # Below this trajectory delta (in encoder counts) an axis is considered + # "not moving" and skipped from IPM streaming — the drive idles on + # sub-threshold motion and leaves frames stuck in its IP buffer otherwise. + _SKIP_AXIS_COUNTS: int = 500 async def move_to_location( self, diff --git a/pylabrobot/paa/kx2/driver.py b/pylabrobot/paa/kx2/driver.py index 4710a7a5de8..712d3535862 100644 --- a/pylabrobot/paa/kx2/driver.py +++ b/pylabrobot/paa/kx2/driver.py @@ -1313,22 +1313,26 @@ async def ipm_select_mode(self, enable: bool) -> None: we cleanly stayed in PPM. """ if enable: - # Set the flag first so a partial failure (some axes flipped to mode 7, - # others not) leaves bookkeeping consistent with "drive may be in IPM". - already_armed = self._ipm_mode self._ipm_mode = True - if not already_armed: - for nid in self.motion_node_ids: - await self.ipm_clear_queue(nid) - await self._set_op_mode(nid, 7) # interpolated position mode - else: - # Re-arm: drop to PPM, reset the interpolation-buffer pointer, climb - # back into IPM. Skipping any of the three steps leaves the drive in - # the wrong mode or with a stale IP buffer. - for nid in self.motion_node_ids: - await self._set_op_mode(nid, 1) - await self.ipm_clear_queue(nid) - await self._set_op_mode(nid, 7) + # First, latch CW=0x0F on every axis (op-enabled, NO ip-enable/new- + # setpoint trigger). PPM leaves CW bit 4 high; in mode 7 bit 4 means + # "interpolation enabled" so an unreset CW makes the drive try to + # interpolate an empty buffer the moment we flip to mode 7, and the next + # RPDO3 preload hits EMCY 0x34 / 0xBA (queue_full on first write). + # RPDO1 is SynchronousCyclic so the writes need a SYNC to take effect. + for nid in self.motion_node_ids: + await self._control_word_set(nid, 0x0F, sync=False) + await self._can_sync() + # Now do the mode-bounce + buffer clear per axis. + for nid in self.motion_node_ids: + await self._set_op_mode(nid, 1) + await self.ipm_clear_queue(nid) + await self._set_op_mode(nid, 7) + # Let drives finish ingesting mode 7 + buffer reset before any RPDO3 + # write — without this, back-to-back IPM moves intermittently see the + # first preload write hit EMCY 0x34 / 0xBA (queue_full) against a + # buffer the drive still treats as "previous state". + await asyncio.sleep(0.05) else: # Always attempt to revert — the cheap mode-display poll inside # _set_op_mode is the authoritative confirmation. Skip the writes only diff --git a/pylabrobot/paa/kx2/kinematics.py b/pylabrobot/paa/kx2/kinematics.py index 6ae20c55dbe..6308d51dcc1 100644 --- a/pylabrobot/paa/kx2/kinematics.py +++ b/pylabrobot/paa/kx2/kinematics.py @@ -39,7 +39,7 @@ from dataclasses import dataclass, field from math import asin, atan2, ceil, cos, degrees, hypot, pi, radians, sin, sqrt, trunc -from typing import Dict, List, Optional, Tuple +from typing import Callable, Dict, List, Optional, Tuple from pylabrobot.capabilities.arms import kinematics as arm_kinematics from pylabrobot.capabilities.arms.standard import CartesianPose @@ -160,7 +160,39 @@ def ik(pose: CartesianPose, c: KX2Config, t: GripperParams) -> Dict[Axis, float] # wrist_joint = ext_world - shoulder. wrist = ext_deg - shoulder - return {Axis.SHOULDER: shoulder, Axis.Z: wrist_z, Axis.ELBOW: elbow, Axis.WRIST: wrist} + joints = {Axis.SHOULDER: shoulder, Axis.Z: wrist_z, Axis.ELBOW: elbow, Axis.WRIST: wrist} + + # Enforce per-axis joint travel limits. Rotary axes (shoulder, wrist) are + # 360°-periodic and a downstream `snap_to_current` may wrap them — check + # only the *non-wrappable* representative (the canonical solution in this + # call). Linear axes (Z, elbow) get a strict check. Raising here aborts + # the caller before any motion command leaves the host. + for ax in (Axis.Z, Axis.ELBOW): + cfg_ax = c.axes[ax] + val = joints[ax] + if val < cfg_ax.min_travel or val > cfg_ax.max_travel: + raise IKError( + f"{ax.name} out of range: {val:.3f} not in " + f"[{cfg_ax.min_travel:.3f}, {cfg_ax.max_travel:.3f}] " + f"for pose location={pose.location}, yaw={pose.rotation.z}" + ) + for ax in (Axis.SHOULDER, Axis.WRIST): + cfg_ax = c.axes[ax] + if cfg_ax.unlimited_travel: + continue + # Pull the canonical solution into the range straddling the limits before + # checking, so a -180° solution against a [-90, 270] axis doesn't false-fail. + val = joints[ax] + mid = 0.5 * (cfg_ax.min_travel + cfg_ax.max_travel) + val += 360.0 * round((mid - val) / 360.0) + if val < cfg_ax.min_travel or val > cfg_ax.max_travel: + raise IKError( + f"{ax.name} out of range: {val:.3f}° not in " + f"[{cfg_ax.min_travel:.3f}, {cfg_ax.max_travel:.3f}]° " + f"for pose location={pose.location}, yaw={pose.rotation.z}" + ) + + return joints def snap_to_current( @@ -651,11 +683,12 @@ def sample_linear_path( # to force it. Forcing zero against a non-trivial v[n-2] would create # the cubic-Hermite overshoot the trailing hold is meant to avoid. n = len(poses) + # V[0] forced to 0 (matches C# MotorsMovePath line 4242). Some drives reject + # the first preload frame if it has non-zero velocity. enc_vel: Dict[Axis, List[int]] = {ax: [0] * n for ax in _LINEAR_PATH_AXES} for ax in _LINEAR_PATH_AXES: seq = enc_pos[ax] if n >= 2: - enc_vel[ax][0] = int(round((seq[1] - seq[0]) / dt_s)) for i in range(1, n - 1): enc_vel[ax][i] = int(round((seq[i + 1] - seq[i - 1]) / (2.0 * dt_s))) enc_vel[ax][n - 1] = int(round((seq[n - 1] - seq[n - 2]) / dt_s)) @@ -669,3 +702,215 @@ def sample_linear_path( encoder_velocity={ax: enc_vel[ax][i] for ax in _LINEAR_PATH_AXES}, )) return out + + +def _build_samples_from_joints( + joints_seq: List[Dict[Axis, float]], + cfg: KX2Config, + dt_s: float, +) -> List[LinearPathSample]: + """Convert a joint-space sequence into encoder-domain PVT samples. + + Mirrors the encoder + central-diff velocity block of ``sample_linear_path``. + Shared by every sampler so they all produce identically-formed samples. + """ + n = len(joints_seq) + enc_pos: Dict[Axis, List[int]] = {ax: [] for ax in _LINEAR_PATH_AXES} + for joints in joints_seq: + for ax in _LINEAR_PATH_AXES: + conv = cfg.axes[ax].motor_conversion_factor + val = joints[ax] + if ax is Axis.ELBOW: + val = convert_elbow_position_to_angle(cfg, val) + enc_pos[ax].append(int(round(val * conv))) + # V[0]=0 (matches sample_linear_path and C# MotorsMovePath line 4242). + # Forward-diff initial velocity makes drives reject the first preload frame + # in some configurations (queue_full on first write). + enc_vel: Dict[Axis, List[int]] = {ax: [0] * n for ax in _LINEAR_PATH_AXES} + for ax in _LINEAR_PATH_AXES: + seq = enc_pos[ax] + if n >= 2: + for i in range(1, n - 1): + enc_vel[ax][i] = int(round((seq[i + 1] - seq[i - 1]) / (2.0 * dt_s))) + enc_vel[ax][n - 1] = int(round((seq[n - 1] - seq[n - 2]) / dt_s)) + return [ + LinearPathSample( + time_s=i * dt_s, + joints=joints_seq[i], + encoder_position={ax: enc_pos[ax][i] for ax in _LINEAR_PATH_AXES}, + encoder_velocity={ax: enc_vel[ax][i] for ax in _LINEAR_PATH_AXES}, + ) for i in range(n) + ] + + +def _ik_pose_sequence( + poses: List[CartesianPose], + cfg: KX2Config, + gripper_params: GripperParams, + current_joints: Optional[Dict[Axis, float]], +) -> List[Dict[Axis, float]]: + """IK every pose, snap rotary axes to the previous solution (avoiding 360° + unwinds across the trajectory). Raises IKError on the first out-of-range + pose with the trajectory-relative index in the message.""" + prev: Optional[Dict[Axis, float]] = ( + dict(current_joints) if current_joints is not None else None + ) + out: List[Dict[Axis, float]] = [] + for i, p in enumerate(poses): + try: + joints = ik(p, cfg, gripper_params) + except IKError as e: + raise IKError(f"sample {i}/{len(poses)}: {e}") from e + if prev is not None: + joints = snap_to_current(joints, prev) + out.append(joints) + prev = joints + return out + + +def sample_parametric_path( + cfg: KX2Config, + gripper_params: GripperParams, + path_fn: "Callable[[float], CartesianPose]", + duration_s: float, + dt_s: float, + current_joints: Optional[Dict[Axis, float]] = None, +) -> List[LinearPathSample]: + """Evaluate ``path_fn`` at every ``i * dt_s`` for ``i = 0..N`` (N chosen so + the last sample lands at or just past ``duration_s``), IK each pose, and + return encoder-domain samples ready for streaming. + + Caller owns the velocity profile — ``path_fn`` defines both shape and pacing. + IK enforces joint limits; any out-of-range pose raises IKError with the + sample index. + """ + if duration_s <= 0 or dt_s <= 0: + raise ValueError(f"duration_s and dt_s must be > 0 (got {duration_s}, {dt_s})") + n = max(2, int(duration_s / dt_s) + 1) + poses = [path_fn(min(i * dt_s, duration_s)) for i in range(n)] + joints_seq = _ik_pose_sequence(poses, cfg, gripper_params, current_joints) + return _build_samples_from_joints(joints_seq, cfg, dt_s) + + +def sample_waypoint_path( + cfg: KX2Config, + gripper_params: GripperParams, + waypoints: List[CartesianPose], + speed_mm_per_s: float, + accel_mm_per_s2: float, + dt_s: float, + current_joints: Optional[Dict[Axis, float]] = None, +) -> List[LinearPathSample]: + """Sample a Catmull-Rom spline through ``waypoints`` at the IPM cadence. + + Curve geometry: centripetal Catmull-Rom through the location component of + every waypoint, with C¹ continuity at interior knots. Yaw is interpolated + linearly between knot rotations (short-way around the circle). Endpoint + tangents are extrapolated from the first/last segment. + + Time parametrization: trapezoidal velocity profile over the spline's total + arc length, with peak speed ``speed_mm_per_s`` and peak acceleration + ``accel_mm_per_s2``. Returns to a triangular profile when the spline is too + short to reach peak speed. + + The spline is dense-sampled internally (50 sub-steps per segment) to build + an arc-length lookup table; the trajectory is then sampled by stepping + through that table at dt-spaced arc-length increments. + """ + if len(waypoints) < 2: + raise ValueError(f"need >= 2 waypoints, got {len(waypoints)}") + if speed_mm_per_s <= 0 or accel_mm_per_s2 <= 0 or dt_s <= 0: + raise ValueError( + f"speed, accel, dt_s must be > 0 (got {speed_mm_per_s}, " + f"{accel_mm_per_s2}, {dt_s})" + ) + + # Build extended control point list with mirrored endpoint tangents so the + # Catmull-Rom evaluator can use four consecutive points at every segment. + pts = [(w.location.x, w.location.y, w.location.z) for w in waypoints] + yaws = [w.rotation.z for w in waypoints] + ext = [tuple(2 * a - b for a, b in zip(pts[0], pts[1]))] + ext.extend(pts) + ext.append(tuple(2 * a - b for a, b in zip(pts[-1], pts[-2]))) + + def cr_eval(seg_idx: int, t: float) -> "Tuple[float, float, float]": + # Centripetal Catmull-Rom at local parameter t in [0, 1] within segment + # seg_idx (which goes between waypoints[seg_idx] and waypoints[seg_idx+1]). + p0, p1, p2, p3 = ext[seg_idx], ext[seg_idx + 1], ext[seg_idx + 2], ext[seg_idx + 3] + t2 = t * t + t3 = t2 * t + return tuple( + 0.5 * ( + (2 * p1[k]) + + (-p0[k] + p2[k]) * t + + (2 * p0[k] - 5 * p1[k] + 4 * p2[k] - p3[k]) * t2 + + (-p0[k] + 3 * p1[k] - 3 * p2[k] + p3[k]) * t3 + ) for k in range(3) + ) + + # Build (cumulative arc length → (segment, local t)) table by densely + # sampling the spline. + SUBSTEPS = 50 + arc_table: List[Tuple[float, int, float]] = [] # (s, seg, local_t) + total_len = 0.0 + prev_pt = pts[0] + arc_table.append((0.0, 0, 0.0)) + for seg in range(len(pts) - 1): + for k in range(1, SUBSTEPS + 1): + t_local = k / SUBSTEPS + p = cr_eval(seg, t_local) + total_len += sqrt(sum((p[i] - prev_pt[i]) ** 2 for i in range(3))) + arc_table.append((total_len, seg, t_local)) + prev_pt = p + + if total_len <= _EPS: + # All waypoints colinear at one point — bail out with a hold-in-place + # 2-sample trajectory so the streamer no-ops cleanly. + one_pose = waypoints[0] + return sample_parametric_path( + cfg=cfg, gripper_params=gripper_params, + path_fn=lambda _t: one_pose, duration_s=dt_s, dt_s=dt_s, + current_joints=current_joints, + ) + + # Trapezoidal time → arc-length profile. + s_seq = _arc_length_trapezoid_times(total_len, speed_mm_per_s, accel_mm_per_s2, dt_s) + n = len(s_seq) + + # Resolve each arc-length s into (seg, local_t) via binary search in the table. + poses: List[CartesianPose] = [] + for s in s_seq: + s_clamped = min(max(s, 0.0), total_len) + lo, hi = 0, len(arc_table) - 1 + while hi - lo > 1: + mid = (lo + hi) // 2 + if arc_table[mid][0] <= s_clamped: + lo = mid + else: + hi = mid + s0, seg0, t0 = arc_table[lo] + s1, seg1, t1 = arc_table[hi] + span = s1 - s0 + alpha = 0.0 if span <= _EPS else (s_clamped - s0) / span + # Linear interpolation in local-t space within the same segment; if the + # table spans a segment boundary, evaluate using the higher one. + if seg0 == seg1: + seg = seg0 + tl = t0 + alpha * (t1 - t0) + else: + seg = seg1 + tl = t1 * alpha + x, y, z = cr_eval(seg, tl) + # Yaw interpolation: chord-length parameter along the full polyline so + # yaw lerps across segment boundaries naturally. + yaw_alpha = s_clamped / total_len + # Map yaw_alpha (0..1 over whole polyline) onto waypoint indices linearly. + yaw_pos = yaw_alpha * (len(yaws) - 1) + yi = min(int(yaw_pos), len(yaws) - 2) + yf = yaw_pos - yi + yaw = _yaw_lerp(yaws[yi], yaws[yi + 1], yf) + poses.append(CartesianPose(location=Coordinate(x=x, y=y, z=z), + rotation=Rotation(z=yaw))) + + joints_seq = _ik_pose_sequence(poses, cfg, gripper_params, current_joints) + return _build_samples_from_joints(joints_seq, cfg, dt_s) diff --git a/pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py b/pylabrobot/paa/kx2/tests/arm_backend_pvt_tests.py similarity index 89% rename from pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py rename to pylabrobot/paa/kx2/tests/arm_backend_pvt_tests.py index 75d0fd80757..8b3aeb1ae32 100644 --- a/pylabrobot/paa/kx2/tests/arm_backend_linear_path_tests.py +++ b/pylabrobot/paa/kx2/tests/arm_backend_pvt_tests.py @@ -110,6 +110,17 @@ async def ipm_wait_motion_complete( "ipm_wait_motion_complete", (tuple(int(n) for n in node_ids), timeout_s), )) + async def motors_ensure_enabled( + self, node_ids: List[int], *, use_ds402: bool = True, + ) -> None: + self.calls.append(("motors_ensure_enabled", (tuple(int(n) for n in node_ids),))) + + def clear_emcy_state(self, node_id=None): + self.calls.append(("clear_emcy_state", (node_id,))) + + async def motor_get_current_position(self, node_id, pu=False): + return 0 + def _build_backend( fake_driver: _FakeDriver, @@ -167,7 +178,7 @@ def test_preload_then_begin_then_stream(self): pre = fake.calls[:begin_idx] pre_sends = [c for c in pre if c[0] == "ipm_send_pvt_point"] # 4 axes * 8 preload frames = 32 sends before begin_motion - self.assertEqual(len(pre_sends), 4 * backend._LINEAR_PATH_PRELOAD) + self.assertEqual(len(pre_sends), 4 * backend._PVT_PRELOAD) # begin_motion received the four arm axes _, (axes,) = fake.calls[begin_idx] @@ -184,7 +195,7 @@ def test_dt_set_to_8ms_default(self): asyncio.run(backend._run_linear_path(_target_pose(), params)) dt_calls = [c for c in fake.calls if c[0] == "ipm_set_time_interval"] self.assertEqual(len(dt_calls), 1) - self.assertEqual(dt_calls[0][1], (backend._LINEAR_PATH_DT_MS,)) + self.assertEqual(dt_calls[0][1], (backend._PVT_DT_MS,)) def test_select_mode_true_then_false(self): """Mode is flipped on at start, off in cleanup. The drive must end in @@ -199,21 +210,24 @@ def test_select_mode_true_then_false(self): self.assertEqual(selects[0], (True,)) self.assertEqual(selects[-1], (False,)) - def test_wait_uses_ipm_specific_motion_complete(self): - """End-of-motion wait must poll SW bit-10 (target reached), not MS. - MS goes to 0 transiently between buffered points; bit-10 is the - authoritative IPM-done signal.""" + def test_stream_end_drops_ip_enable_no_blocking_wait(self): + """After the last frame, the runtime must drop ip-enable (ipm_stop) and + NOT poll for any wait condition. The Elmo drive doesn't latch SW bit-10 + until ip-enable goes low — polling it inside IP mode hangs forever — and + leaving ip-enable asserted past the buffer drain raises EMCY 0x8A + (underflow). Mirrors C# MotorsMovePathExecute which proceeds straight to + PVTBeginMotion(false) once streaming completes.""" fake = _FakeDriver() backend = _build_backend(fake) params = KX2ArmBackend.CartesianMoveParams( max_gripper_speed=20.0, max_gripper_acceleration=200.0, path="linear", ) asyncio.run(backend._run_linear_path(_target_pose(), params)) - # Must NOT use the MS-based generic waiter. self.assertFalse([c for c in fake.calls if c[0] == "wait_for_moves_done"]) - waits = [c for c in fake.calls if c[0] == "ipm_wait_motion_complete"] - self.assertEqual(len(waits), 1) - (axes, _) = waits[0][1] + self.assertFalse([c for c in fake.calls if c[0] == "ipm_wait_motion_complete"]) + stops = [c for c in fake.calls if c[0] == "ipm_stop"] + self.assertGreaterEqual(len(stops), 1) + (axes,) = stops[0][1] self.assertEqual(set(axes), { int(Axis.SHOULDER), int(Axis.Z), int(Axis.ELBOW), int(Axis.WRIST), }) diff --git a/pylabrobot/paa/kx2/tests/kinematics_tests.py b/pylabrobot/paa/kx2/tests/kinematics_tests.py index 4133f08c003..81c81b3ca8c 100644 --- a/pylabrobot/paa/kx2/tests/kinematics_tests.py +++ b/pylabrobot/paa/kx2/tests/kinematics_tests.py @@ -9,12 +9,14 @@ from pylabrobot.resources import Coordinate, Rotation -def _axis() -> AxisConfig: +def _axis( + *, max_travel: float = 1e6, min_travel: float = -1e6, unlimited: bool = False, +) -> AxisConfig: return AxisConfig( motor_conversion_factor=1.0, - max_travel=180.0, - min_travel=-180.0, - unlimited_travel=False, + max_travel=max_travel, + min_travel=min_travel, + unlimited_travel=unlimited, absolute_encoder=True, max_vel=100.0, max_accel=100.0, @@ -34,7 +36,12 @@ def _config( wrist_offset=wrist_offset, elbow_offset=elbow_offset, elbow_zero_offset=elbow_zero_offset, - axes={a: _axis() for a in (Axis.SHOULDER, Axis.Z, Axis.ELBOW, Axis.WRIST)}, + axes={ + Axis.SHOULDER: _axis(unlimited=True), + Axis.Z: _axis(min_travel=0.0, max_travel=10000.0), + Axis.ELBOW: _axis(min_travel=-10000.0, max_travel=10000.0), + Axis.WRIST: _axis(unlimited=True), + }, base_to_gripper_clearance_z=0.0, base_to_gripper_clearance_arm=0.0, robot_on_rail=False, From 2719e0f3434d84d7b6127c770e6ae89566ed849b Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Thu, 14 May 2026 12:18:13 -0700 Subject: [PATCH 91/93] =?UTF-8?q?Arms:=20rename=20GripDirection=E2=86=92Gr?= =?UTF-8?q?ipperDirection=20(string=20literal);=20rotate=20convention=20so?= =?UTF-8?q?=200=C2=B0=3D+X=20(#1034)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Claude Opus 4.7 (1M context) --- docs/api/pylabrobot.arms.rst | 2 +- docs/user_guide/capabilities/arms.md | 20 ++++-- .../hamilton/star/hardware/replacing-iswap.md | 5 +- docs/user_guide/hamilton/star/iswap.ipynb | 55 +++------------ pylabrobot/arms/arm_tests.py | 19 ++--- pylabrobot/capabilities/arms/architecture.md | 26 +++++++ pylabrobot/capabilities/arms/arm.py | 4 +- pylabrobot/capabilities/arms/arm_tests.py | 19 ++--- .../capabilities/arms/orientable_arm.py | 59 ++++++++++------ pylabrobot/capabilities/arms/standard.py | 15 ++-- .../hamilton/liquid_handlers/star/iswap.py | 20 +++--- .../star/misc/iswap_test.ipynb | 69 +++---------------- .../liquid_handlers/star/tests/iswap_tests.py | 12 ++-- .../legacy/liquid_handling/liquid_handler.py | 9 ++- 14 files changed, 155 insertions(+), 179 deletions(-) diff --git a/docs/api/pylabrobot.arms.rst b/docs/api/pylabrobot.arms.rst index 02a48eceb76..23d07ab85a9 100644 --- a/docs/api/pylabrobot.arms.rst +++ b/docs/api/pylabrobot.arms.rst @@ -39,4 +39,4 @@ Types :recursive: standard.CartesianPose - standard.GripDirection + standard.GripperDirection diff --git a/docs/user_guide/capabilities/arms.md b/docs/user_guide/capabilities/arms.md index 38601786f54..33acb85bf53 100644 --- a/docs/user_guide/capabilities/arms.md +++ b/docs/user_guide/capabilities/arms.md @@ -52,19 +52,29 @@ await lh.iswap.move_resource( ### OrientableArm: grip direction -```python -from pylabrobot.capabilities.arms.standard import GripDirection +`direction` is the world yaw of the gripper's front finger, in degrees, +**CCW about +Z with 0° = +X**. You can pass a float, or one of the +cardinal-direction strings: -await lh.iswap.pick_up_resource(plate, direction=GripDirection.LEFT) -await lh.iswap.drop_resource(reader, direction=GripDirection.FRONT) +```python +await lh.iswap.pick_up_resource(plate, direction="left") # = 180° +await lh.iswap.drop_resource(reader, direction="front") # = 270° +await lh.iswap.move_to_location(coord, direction=45.0) # 45° CCW from +X ``` +| String | Degrees | World axis (deck frame) | +|:------------|--------:|:------------------------| +| `"right"` | `0°` | `+X` | +| `"back"` | `90°` | `+Y` | +| `"left"` | `180°` | `-X` | +| `"front"` | `270°` | `-Y` | + ## Tips and gotchas - **Coordinates are in the reference resource's frame** (typically the deck). The arm computes gripper target coordinates from the resource's position, dimensions, and the destination type. - **`pickup_distance_from_bottom`** controls how far up from the bottom of the resource the gripper grips. If `None`, the resource's `preferred_pickup_location` is used, or a default of 5 mm from the top (`size_z - 5`). - **Resource tree is updated automatically.** After a successful `drop_resource`, the resource is unassigned from its old parent and assigned to the destination. -- **`GripOrientation`** is either a {class}`~pylabrobot.capabilities.arms.standard.GripDirection` enum (`FRONT`, `RIGHT`, `BACK`, `LEFT`) or a float in degrees. +- **`GripperOrientation`** is either a {data}`~pylabrobot.capabilities.arms.standard.GripperDirection` string literal (`"front"`, `"right"`, `"back"`, `"left"`) or a float in degrees, measured CCW about world +Z with 0° = +X. - **`request_gripper_pose()`** queries the hardware for the current end effector position. `get_picked_up_resource()` returns the internally tracked state (no hardware call). ## Supported hardware diff --git a/docs/user_guide/hamilton/star/hardware/replacing-iswap.md b/docs/user_guide/hamilton/star/hardware/replacing-iswap.md index 9d9ae1bc383..94536a15d5e 100644 --- a/docs/user_guide/hamilton/star/hardware/replacing-iswap.md +++ b/docs/user_guide/hamilton/star/hardware/replacing-iswap.md @@ -84,11 +84,10 @@ await star.driver.iswap.reengage_break() # firmware command "R0BO" ```python from pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAPBackend -from pylabrobot.capabilities.arms.standard import GripDirection await star.driver.iswap.rotate( rotation_drive=iSWAPBackend.RotationDriveOrientation.RIGHT, - grip_direction=GripDirection.BACK, + grip_direction="back", ) ``` @@ -97,7 +96,7 @@ await star.driver.iswap.rotate( ```python await star.driver.iswap.rotate( rotation_drive=iSWAPBackend.RotationDriveOrientation.LEFT, - grip_direction=GripDirection.BACK, + grip_direction="back", ) ``` diff --git a/docs/user_guide/hamilton/star/iswap.ipynb b/docs/user_guide/hamilton/star/iswap.ipynb index ed5b6f42eca..e9719858414 100644 --- a/docs/user_guide/hamilton/star/iswap.ipynb +++ b/docs/user_guide/hamilton/star/iswap.ipynb @@ -160,47 +160,17 @@ }, { "cell_type": "code", - "execution_count": 7, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "2026-04-08 17:52:13,325 - pylabrobot.hamilton.liquid_handlers.star.iswap - INFO - [iSWAP] pick up plate: x=482.9, y=210.2, z=192.3, direction=270 deg, width=85.5 mm\n", - "2026-04-08 17:52:21,538 - pylabrobot.hamilton.liquid_handlers.star.iswap - INFO - [iSWAP] release plate: x=482.9, y=306.2, z=192.3, direction=270 deg, width=85.5 mm\n" - ] - } - ], - "source": [ - "from pylabrobot.capabilities.arms.standard import GripDirection\n", - "\n", - "await star.iswap.pick_up_resource(plate, direction=GripDirection.LEFT)\n", - "await star.iswap.drop_resource(plate_carrier[2], direction=GripDirection.LEFT)" - ] + "outputs": [], + "source": "await star.iswap.pick_up_resource(plate, direction=\"left\")\nawait star.iswap.drop_resource(plate_carrier[2], direction=\"left\")" }, { "cell_type": "code", - "execution_count": 8, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "2026-04-08 17:52:27,002 - pylabrobot.hamilton.liquid_handlers.star.iswap - INFO - [iSWAP] pick up plate: x=482.9, y=306.2, z=192.3, direction=90 deg, width=85.5 mm\n", - "2026-04-08 17:52:33,870 - pylabrobot.hamilton.liquid_handlers.star.iswap - INFO - [iSWAP] release plate: x=482.9, y=210.2, z=192.3, direction=90 deg, width=85.5 mm\n" - ] - } - ], - "source": [ - "await star.iswap.move_resource(\n", - " plate, plate_carrier[1],\n", - " pickup_backend_params=iSWAPBackend.PickUpParams(grip_strength=8),\n", - " pickup_direction=GripDirection.RIGHT,\n", - " drop_direction=GripDirection.RIGHT,\n", - ")" - ] + "outputs": [], + "source": "await star.iswap.move_resource(\n plate, plate_carrier[1],\n pickup_backend_params=iSWAPBackend.PickUpParams(grip_strength=8),\n pickup_direction=\"right\",\n drop_direction=\"right\",\n)" }, { "cell_type": "markdown", @@ -387,17 +357,10 @@ }, { "cell_type": "code", - "execution_count": 19, + "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "from pylabrobot.capabilities.arms.standard import GripDirection\n", - "\n", - "await star.driver.iswap.rotate(\n", - " rotation_drive=iSWAPBackend.RotationDriveOrientation.RIGHT,\n", - " grip_direction=GripDirection.LEFT,\n", - ")" - ] + "source": "await star.driver.iswap.rotate(\n rotation_drive=iSWAPBackend.RotationDriveOrientation.RIGHT,\n grip_direction=\"left\",\n)" }, { "cell_type": "markdown", @@ -656,4 +619,4 @@ }, "nbformat": 4, "nbformat_minor": 4 -} +} \ No newline at end of file diff --git a/pylabrobot/arms/arm_tests.py b/pylabrobot/arms/arm_tests.py index 7860c30f343..e391e4978df 100644 --- a/pylabrobot/arms/arm_tests.py +++ b/pylabrobot/arms/arm_tests.py @@ -7,7 +7,6 @@ OrientableGripperArmBackend, ) from pylabrobot.arms.orientable_arm import OrientableArm -from pylabrobot.arms.standard import GripDirection from pylabrobot.resources import Coordinate, Resource, ResourceHolder @@ -138,21 +137,23 @@ async def asyncSetUp(self): async def test_pick_up_front(self): await self.arm.pick_up_resource( - self.plate, pickup_distance_from_bottom=8, direction=GripDirection.FRONT + self.plate, pickup_distance_from_bottom=8, direction="front" ) call = self.mock_backend.pick_up_at_location.call_args _assert_location(self, call, 165, 145, 58) - self.assertAlmostEqual(call.kwargs["direction"], 0.0) - # FRONT → X width = 120 + # "front" = -Y in deck frame = 270° under the +X-is-zero convention. + self.assertAlmostEqual(call.kwargs["direction"], 270.0) + # "front" grips along the X axis → X width = 120 self.assertAlmostEqual(call.kwargs["resource_width"], 120) async def test_pick_up_right(self): await self.arm.pick_up_resource( - self.plate, pickup_distance_from_bottom=8, direction=GripDirection.RIGHT + self.plate, pickup_distance_from_bottom=8, direction="right" ) call = self.mock_backend.pick_up_at_location.call_args - self.assertAlmostEqual(call.kwargs["direction"], 90.0) - # RIGHT → Y width = 80 + # "right" = +X = 0° under the +X-is-zero convention. + self.assertAlmostEqual(call.kwargs["direction"], 0.0) + # "right" grips along the Y axis → Y width = 80 self.assertAlmostEqual(call.kwargs["resource_width"], 80) async def test_drop_at_location(self): @@ -173,9 +174,9 @@ async def test_move_to_location(self): async def test_move_plate(self): """Pick from site_a, drop at site_b.""" await self.arm.pick_up_resource( - self.plate, pickup_distance_from_bottom=8, direction=GripDirection.FRONT + self.plate, pickup_distance_from_bottom=8, direction="front" ) - await self.arm.drop_resource(self.site_b, direction=GripDirection.FRONT) + await self.arm.drop_resource(self.site_b, direction="front") drop_call = self.mock_backend.drop_at_location.call_args _assert_location(self, drop_call, 160, 340, 58) self.assertEqual(self.plate.parent.name, "site_b") diff --git a/pylabrobot/capabilities/arms/architecture.md b/pylabrobot/capabilities/arms/architecture.md index 99ccf47d5ee..ddae7dfd315 100644 --- a/pylabrobot/capabilities/arms/architecture.md +++ b/pylabrobot/capabilities/arms/architecture.md @@ -1,5 +1,31 @@ # Arms architecture +## Coordinate convention + +Backends share one rotation convention so swapping a Hamilton iSWAP for a +PreciseFlex (or any future arm) doesn't change what `direction` means at +the call site. + +`direction` (and `CartesianPose.rotation.z`) is the world yaw of the +gripper's *front finger*, in degrees, measured **CCW about world +Z +(right-hand rule, looking down)** with **0° = +X**: + +| `direction` | World axis | `GripperDirection` (deck frame) | +|------------:|:----------:|:--------------------------------| +| `0°` | `+X` | `"right"` | +| `90°` | `+Y` | `"back"` | +| `180°` | `-X` | `"left"` | +| `270°` | `-Y` | `"front"` | + +`GripperDirection = Literal["front", "back", "left", "right"]` is a +string-literal alias for these cardinal degrees, used wherever a deck- +relative label reads better than a raw number. + +The frontends accept `direction: Union[GripperDirection, float]` and +resolve the label to degrees before handing it to the backend, so +backend implementations only ever see the float — but every backend +must interpret that float under the convention above. + ## Frontend hierarchy (capabilities) ``` diff --git a/pylabrobot/capabilities/arms/arm.py b/pylabrobot/capabilities/arms/arm.py index 0186128f188..9d644a8e229 100644 --- a/pylabrobot/capabilities/arms/arm.py +++ b/pylabrobot/capabilities/arms/arm.py @@ -3,7 +3,7 @@ from typing import List, Literal, Optional, Tuple, Union from pylabrobot.capabilities.arms.backend import GripperArmBackend, _BaseArmBackend -from pylabrobot.capabilities.arms.standard import CartesianPose, GripDirection +from pylabrobot.capabilities.arms.standard import CartesianPose, GripperDirection from pylabrobot.capabilities.capability import BackendParams, Capability from pylabrobot.legacy.tilting.tilter import Tilter from pylabrobot.resources import ( @@ -20,7 +20,7 @@ logger = logging.getLogger(__name__) -GripOrientation = Union[GripDirection, float] +GripperOrientation = Union[GripperDirection, float] @dataclass diff --git a/pylabrobot/capabilities/arms/arm_tests.py b/pylabrobot/capabilities/arms/arm_tests.py index 56c65485982..b7a9aa622a5 100644 --- a/pylabrobot/capabilities/arms/arm_tests.py +++ b/pylabrobot/capabilities/arms/arm_tests.py @@ -7,7 +7,6 @@ OrientableGripperArmBackend, ) from pylabrobot.capabilities.arms.orientable_arm import OrientableArm -from pylabrobot.capabilities.arms.standard import GripDirection from pylabrobot.resources import Coordinate, Resource, ResourceHolder @@ -138,21 +137,23 @@ async def asyncSetUp(self): async def test_pick_up_front(self): await self.arm.pick_up_resource( - self.plate, pickup_distance_from_bottom=8, direction=GripDirection.FRONT + self.plate, pickup_distance_from_bottom=8, direction="front" ) call = self.mock_backend.pick_up_at_location.call_args _assert_location(self, call, 165, 145, 58) - self.assertAlmostEqual(call.kwargs["direction"], 0.0) - # FRONT → X width = 120 + # "front" = -Y in deck frame = 270° under the +X-is-zero convention. + self.assertAlmostEqual(call.kwargs["direction"], 270.0) + # "front" grips along the X axis → X width = 120 self.assertAlmostEqual(call.kwargs["resource_width"], 120) async def test_pick_up_right(self): await self.arm.pick_up_resource( - self.plate, pickup_distance_from_bottom=8, direction=GripDirection.RIGHT + self.plate, pickup_distance_from_bottom=8, direction="right" ) call = self.mock_backend.pick_up_at_location.call_args - self.assertAlmostEqual(call.kwargs["direction"], 90.0) - # RIGHT → Y width = 80 + # "right" = +X = 0° under the +X-is-zero convention. + self.assertAlmostEqual(call.kwargs["direction"], 0.0) + # "right" grips along the Y axis → Y width = 80 self.assertAlmostEqual(call.kwargs["resource_width"], 80) async def test_drop_at_location(self): @@ -173,9 +174,9 @@ async def test_move_to_location(self): async def test_move_plate(self): """Pick from site_a, drop at site_b.""" await self.arm.pick_up_resource( - self.plate, pickup_distance_from_bottom=8, direction=GripDirection.FRONT + self.plate, pickup_distance_from_bottom=8, direction="front" ) - await self.arm.drop_resource(self.site_b, direction=GripDirection.FRONT) + await self.arm.drop_resource(self.site_b, direction="front") drop_call = self.mock_backend.drop_at_location.call_args _assert_location(self, drop_call, 160, 340, 58) self.assertEqual(self.plate.parent.name, "site_b") diff --git a/pylabrobot/capabilities/arms/orientable_arm.py b/pylabrobot/capabilities/arms/orientable_arm.py index 848364c5fb5..831459ba6f1 100644 --- a/pylabrobot/capabilities/arms/orientable_arm.py +++ b/pylabrobot/capabilities/arms/orientable_arm.py @@ -1,23 +1,36 @@ -from typing import List, Optional, Union +from typing import Dict, List, Optional, Union -from pylabrobot.capabilities.arms.arm import GripOrientation, _BaseArm, _PickedUpState +from pylabrobot.capabilities.arms.arm import GripperOrientation, _BaseArm, _PickedUpState from pylabrobot.capabilities.arms.backend import OrientableGripperArmBackend -from pylabrobot.capabilities.arms.standard import GripDirection +from pylabrobot.capabilities.arms.standard import ( + _GRIPPER_DIRECTION_VALUES, + GripperDirection, +) from pylabrobot.capabilities.capability import BackendParams from pylabrobot.resources import Coordinate, Resource, ResourceHolder, ResourceStack from pylabrobot.resources.rotation import Rotation -_GRIP_DIRECTION_TO_DEGREES = { - GripDirection.FRONT: 0.0, - GripDirection.RIGHT: 90.0, - GripDirection.BACK: 180.0, - GripDirection.LEFT: 270.0, +# Cardinal-direction → degrees under the standard ``rotation.z = 0 → +X`` +# convention (CCW about world +Z, right-hand rule looking down). In the +# PLR deck frame (+X = right, +Y = back), this maps to: right=+X, +# back=+Y, left=-X, front=-Y. ``front`` lives at 270° (rather than -90°) +# so the table reads top-to-bottom in CCW order from +X. +_GRIPPER_DIRECTION_TO_DEGREES: Dict[GripperDirection, float] = { + "right": 0.0, + "back": 90.0, + "left": 180.0, + "front": 270.0, } -def _resolve_direction(direction: GripOrientation) -> float: - if isinstance(direction, GripDirection): - return _GRIP_DIRECTION_TO_DEGREES[direction] +def _resolve_direction(direction: GripperOrientation) -> float: + if isinstance(direction, str): + if direction not in _GRIPPER_DIRECTION_VALUES: + raise ValueError( + f"direction must be one of {sorted(_GRIPPER_DIRECTION_VALUES)} or a float, " + f"got {direction!r}" + ) + return _GRIPPER_DIRECTION_TO_DEGREES[direction] return direction @@ -43,17 +56,21 @@ async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None @staticmethod def _resource_width_for_direction(resource: Resource, direction: float) -> float: + # Front-finger axis is `direction`; jaws come together perpendicular to + # it. At 0°/180° the finger points along ±X (right/left), so the jaws + # grip the resource along Y; at 90°/270° the finger points along ±Y + # (back/front) so they grip along X. # TODO: resource rotation is not taken into account here. if direction % 180 == 0: - return resource.get_absolute_size_x() - else: return resource.get_absolute_size_y() + else: + return resource.get_absolute_size_x() async def pick_up_at_location( self, location: Coordinate, resource_width: float, - direction: GripOrientation = 0.0, + direction: GripperOrientation = 0.0, backend_params: Optional[BackendParams] = None, ): if self.holding: @@ -73,7 +90,7 @@ async def pick_up_resource( resource: Resource, offset: Coordinate = Coordinate.zero(), pickup_distance_from_bottom: Optional[float] = None, - direction: GripOrientation = GripDirection.FRONT, + direction: GripperOrientation = "front", backend_params: Optional[BackendParams] = None, ): location, pickup_distance_from_bottom = self._prepare_pickup( @@ -97,7 +114,7 @@ async def pick_up_resource( async def drop_at_location( self, location: Coordinate, - direction: GripOrientation, + direction: GripperOrientation, backend_params: Optional[BackendParams] = None, ): if self._holding_resource_width is None: @@ -114,7 +131,7 @@ async def drop_resource( self, destination: Union[ResourceStack, ResourceHolder, Resource, Coordinate], offset: Coordinate = Coordinate.zero(), - direction: GripOrientation = GripDirection.FRONT, + direction: GripperOrientation = "front", backend_params: Optional[BackendParams] = None, ): resource = self._prepare_drop(destination) @@ -135,7 +152,7 @@ async def drop_resource( async def move_to_location( self, location: Coordinate, - direction: GripOrientation = 0.0, + direction: GripperOrientation = 0.0, backend_params: Optional[BackendParams] = None, ): await self.backend.move_to_location( @@ -147,7 +164,7 @@ async def move_to_location( async def move_picked_up_resource( self, to: Coordinate, - direction: GripOrientation, + direction: GripperOrientation, offset: Coordinate = Coordinate.zero(), backend_params: Optional[BackendParams] = None, ): @@ -169,8 +186,8 @@ async def move_resource( pickup_offset: Coordinate = Coordinate.zero(), destination_offset: Coordinate = Coordinate.zero(), pickup_distance_from_bottom: Optional[float] = None, - pickup_direction: GripOrientation = GripDirection.FRONT, - drop_direction: GripOrientation = GripDirection.FRONT, + pickup_direction: GripperOrientation = "front", + drop_direction: GripperOrientation = "front", pickup_backend_params: Optional[BackendParams] = None, drop_backend_params: Optional[BackendParams] = None, ): diff --git a/pylabrobot/capabilities/arms/standard.py b/pylabrobot/capabilities/arms/standard.py index 4b3d983d60a..3adfa40c31e 100644 --- a/pylabrobot/capabilities/arms/standard.py +++ b/pylabrobot/capabilities/arms/standard.py @@ -1,6 +1,5 @@ -import enum from dataclasses import dataclass -from typing import Dict +from typing import Dict, Literal, get_args from pylabrobot.resources import Coordinate, Rotation @@ -15,11 +14,13 @@ class CartesianPose: rotation: Rotation -class GripDirection(enum.Enum): - FRONT = enum.auto() - BACK = enum.auto() - LEFT = enum.auto() - RIGHT = enum.auto() +# Cardinal directions in the deck frame. String-literal alias for +# ``OrientableArm`` ``direction`` arguments. Mapped to degrees by +# :data:`pylabrobot.capabilities.arms.orientable_arm._GRIPPER_DIRECTION_TO_DEGREES` +# under the standard ``rotation.z = 0 → +X (CCW about +Z)`` convention: +# ``"right" = 0°``, ``"back" = 90°``, ``"left" = 180°``, ``"front" = 270°``. +GripperDirection = Literal["front", "back", "left", "right"] +_GRIPPER_DIRECTION_VALUES = frozenset(get_args(GripperDirection)) @dataclass(frozen=True) diff --git a/pylabrobot/hamilton/liquid_handlers/star/iswap.py b/pylabrobot/hamilton/liquid_handlers/star/iswap.py index 3a64ca981fa..517d4c314f2 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/iswap.py +++ b/pylabrobot/hamilton/liquid_handlers/star/iswap.py @@ -7,7 +7,7 @@ from typing import TYPE_CHECKING, Literal, Optional, cast from pylabrobot.capabilities.arms.backend import OrientableGripperArmBackend -from pylabrobot.capabilities.arms.standard import CartesianPose, GripDirection +from pylabrobot.capabilities.arms.standard import CartesianPose, GripperDirection from pylabrobot.capabilities.capability import BackendParams from pylabrobot.resources import Coordinate from pylabrobot.resources.rotation import Rotation @@ -21,11 +21,11 @@ def _direction_degrees_to_grip_direction(degrees: float) -> int: """Convert rotation angle in degrees to firmware grip_direction (1-4). - Firmware: 1 = negative Y (front), 2 = positive X (right), - 3 = positive Y (back), 4 = negative X (left). + User-facing convention: ``rotation.z = 0 → +X (CCW about +Z)``. + Firmware: 1 = -Y (front), 2 = +X (right), 3 = +Y (back), 4 = -X (left). """ normalized = round(degrees) % 360 - mapping = {0: 1, 90: 2, 180: 3, 270: 4} + mapping = {0: 2, 90: 3, 180: 4, 270: 1} if normalized not in mapping: raise ValueError(f"grip direction must be a multiple of 90 degrees, got {degrees}") return mapping[normalized] @@ -301,7 +301,7 @@ async def request_wrist_drive_orientation(self) -> "iSWAPBackend.WristDriveOrien async def rotate( self, rotation_drive: "iSWAPBackend.RotationDriveOrientation", - grip_direction: GripDirection, + grip_direction: GripperDirection, gripper_velocity: int = 55_000, gripper_acceleration: int = 170, gripper_protection: Literal[0, 1, 2, 3, 4, 5, 6, 7] = 5, @@ -334,16 +334,16 @@ async def rotate( else: raise ValueError(f"Invalid rotation drive orientation: {rotation_drive}") - if grip_direction.value == GripDirection.FRONT.value: + if grip_direction == "front": position += 1 - elif grip_direction.value == GripDirection.RIGHT.value: + elif grip_direction == "right": position += 2 - elif grip_direction.value == GripDirection.BACK.value: + elif grip_direction == "back": position += 3 - elif grip_direction.value == GripDirection.LEFT.value: + elif grip_direction == "left": position += 4 else: - raise ValueError("Invalid grip direction") + raise ValueError(f"Invalid grip direction: {grip_direction!r}") await self.driver.send_command( module="R0", diff --git a/pylabrobot/hamilton/liquid_handlers/star/misc/iswap_test.ipynb b/pylabrobot/hamilton/liquid_handlers/star/misc/iswap_test.ipynb index c540015a33a..67a45fdd1c6 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/misc/iswap_test.ipynb +++ b/pylabrobot/hamilton/liquid_handlers/star/misc/iswap_test.ipynb @@ -24,15 +24,7 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "from pylabrobot.arms.orientable_arm import OrientableArm\n", - "from pylabrobot.arms.standard import GripDirection\n", - "from pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAP\n", - "from pylabrobot.legacy.liquid_handling.backends.hamilton.STAR_backend import STARBackend\n", - "from pylabrobot.resources.corning.plates import Cor_96_wellplate_360ul_Fb\n", - "from pylabrobot.resources.hamilton.hamilton_decks import STARLetDeck\n", - "from pylabrobot.resources.hamilton.plate_carriers import PLT_CAR_L5AC_A00" - ] + "source": "from pylabrobot.arms.orientable_arm import OrientableArm\nfrom pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAP\nfrom pylabrobot.legacy.liquid_handling.backends.hamilton.STAR_backend import STARBackend\nfrom pylabrobot.resources.corning.plates import Cor_96_wellplate_360ul_Fb\nfrom pylabrobot.resources.hamilton.hamilton_decks import STARLetDeck\nfrom pylabrobot.resources.hamilton.plate_carriers import PLT_CAR_L5AC_A00" }, { "cell_type": "markdown", @@ -164,33 +156,10 @@ }, { "cell_type": "code", - "execution_count": 10, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "cmd C0PPid0030xs04604xd0yj2102yd0zj1923zd0gr1th2800te2800gw4go1308gb1245gt20ga0gc0\n", - "Picked up 'my_plate'\n" - ] - } - ], - "source": [ - "await arm.pick_up_resource(\n", - " plate,\n", - " direction=GripDirection.FRONT,\n", - " backend_params=iSWAP.PickUpParams(\n", - " minimum_traverse_height=280.0,\n", - " z_position_at_end=280.0,\n", - " grip_strength=4,\n", - " plate_width_tolerance=2.0,\n", - " collision_control_level=0,\n", - " fold_up_at_end=False,\n", - " ),\n", - ")\n", - "print(f\"Picked up '{plate.name}'\")" - ] + "outputs": [], + "source": "await arm.pick_up_resource(\n plate,\n direction=\"front\",\n backend_params=iSWAP.PickUpParams(\n minimum_traverse_height=280.0,\n z_position_at_end=280.0,\n grip_strength=4,\n plate_width_tolerance=2.0,\n collision_control_level=0,\n fold_up_at_end=False,\n ),\n)\nprint(f\"Picked up '{plate.name}'\")" }, { "cell_type": "code", @@ -218,37 +187,17 @@ }, { "cell_type": "code", - "execution_count": 11, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "cmd C0PRid0031xs04604xd0yj3062yd0zj1923zd0th2800te2800gr1go1308ga0gc0\n", - "Plate 'my_plate' is now in: carrier-2\n", - "Plate absolute location: Coordinate(396.500, 263.500, 183.120)\n" - ] - } - ], - "source": [ - "await arm.drop_resource(carrier[2], direction=GripDirection.FRONT)\n", - "print(f\"Plate '{plate.name}' is now in: {plate.parent.name}\")\n", - "print(f\"Plate absolute location: {plate.get_absolute_location()}\")" - ] + "outputs": [], + "source": "await arm.drop_resource(carrier[2], direction=\"front\")\nprint(f\"Plate '{plate.name}' is now in: {plate.parent.name}\")\nprint(f\"Plate absolute location: {plate.get_absolute_location()}\")" }, { "cell_type": "code", - "execution_count": 13, + "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "await arm.pick_up_resource(\n", - " plate,\n", - " direction=GripDirection.LEFT,\n", - ")\n", - "await arm.drop_resource(carrier[2], direction=GripDirection.RIGHT)" - ] + "source": "await arm.pick_up_resource(\n plate,\n direction=\"left\",\n)\nawait arm.drop_resource(carrier[2], direction=\"right\")" }, { "cell_type": "markdown", diff --git a/pylabrobot/hamilton/liquid_handlers/star/tests/iswap_tests.py b/pylabrobot/hamilton/liquid_handlers/star/tests/iswap_tests.py index c2d9a173fab..d23572a1def 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/tests/iswap_tests.py +++ b/pylabrobot/hamilton/liquid_handlers/star/tests/iswap_tests.py @@ -16,9 +16,10 @@ async def asyncSetUp(self): async def test_pick_up_at_location(self): """C0PPid0001xs03479xd0yj1142yd0zj1874zd0gr1th2800te2800gw4go1308gb1245gt20ga0gc0""" + # direction=270° = -Y = "front" pickup; firmware gr=1 (front). await self.iswap.pick_up_at_location( location=Coordinate(347.9, 114.2, 187.4), - direction=0.0, + direction=270.0, resource_width=127.76, ) @@ -44,9 +45,10 @@ async def test_pick_up_at_location(self): async def test_pick_up_grip_direction_left(self): """C0PPid0003xs10427xd0yj3286yd0zj2063zd0gr4th2800te2800gw4go1308gb1245gt20ga0gc0""" + # direction=180° = -X = "left" pickup; firmware gr=4 (left). await self.iswap.pick_up_at_location( location=Coordinate(1042.7, 328.6, 206.3), - direction=270.0, + direction=180.0, resource_width=127.76, ) @@ -72,9 +74,10 @@ async def test_pick_up_grip_direction_left(self): async def test_drop_at_location(self): """C0PRid0002xs03479xd0yj3062yd0zj1874zd0th2800te2800gr1go1308ga0gc0""" + # direction=270° = -Y = "front" drop; firmware gr=1 (front). await self.iswap.drop_at_location( location=Coordinate(347.9, 306.2, 187.4), - direction=0.0, + direction=270.0, resource_width=127.76, ) @@ -97,9 +100,10 @@ async def test_drop_at_location(self): async def test_drop_grip_direction_left(self): """C0PRid0002xs10427xd0yj3286yd0zj2063zd0th2800te2800gr4go1308ga0gc0""" + # direction=180° = -X = "left" drop; firmware gr=4 (left). await self.iswap.drop_at_location( location=Coordinate(1042.7, 328.6, 206.3), - direction=270.0, + direction=180.0, resource_width=127.76, ) diff --git a/pylabrobot/legacy/liquid_handling/liquid_handler.py b/pylabrobot/legacy/liquid_handling/liquid_handler.py index 2f4ba43ca16..64cc7845aea 100644 --- a/pylabrobot/legacy/liquid_handling/liquid_handler.py +++ b/pylabrobot/legacy/liquid_handling/liquid_handler.py @@ -22,7 +22,7 @@ from pylabrobot.capabilities.arms.backend import OrientableGripperArmBackend from pylabrobot.capabilities.arms.orientable_arm import OrientableArm -from pylabrobot.capabilities.arms.standard import GripDirection as _NewGripDirection +from pylabrobot.capabilities.arms.standard import GripperDirection as _NewGripperDirection from pylabrobot.capabilities.arms.standard import CartesianPose from pylabrobot.capabilities.liquid_handling.head96 import Head96 from pylabrobot.capabilities.liquid_handling.head96_backend import ( @@ -305,7 +305,12 @@ async def dispense96( await self._legacy.dispense96(dispense=legacy_disp, **kw) -_LEGACY_TO_NEW_GRIP = {d: _NewGripDirection[d.name] for d in GripDirection} +_LEGACY_TO_NEW_GRIP: Dict[GripDirection, _NewGripperDirection] = { + GripDirection.FRONT: "front", + GripDirection.BACK: "back", + GripDirection.LEFT: "left", + GripDirection.RIGHT: "right", +} class _ArmAdapter(OrientableGripperArmBackend): From c1368ffa1cea01ce99b1a5cfebbdf6a43129c817 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 17 May 2026 15:27:13 -0700 Subject: [PATCH 92/93] Unify gripper API: move_gripper(width, force_sensing) + class hierarchy refactor (#1044) Co-authored-by: Claude Opus 4.7 (1M context) --- docs/api/pylabrobot.arms.rst | 42 ---- docs/api/pylabrobot.hamilton.rst | 2 +- docs/resources/index.md | 2 +- .../brooks/precise_flex/hello-world.ipynb | 52 +---- docs/user_guide/capabilities/arms.md | 46 ++++- .../hamilton/star/core-grippers.ipynb | 4 +- docs/user_guide/hamilton/star/iswap.ipynb | 68 ++----- .../ufactory/xarm6/hello-world.ipynb | 6 +- pylabrobot/arms/__init__.py | 9 - pylabrobot/arms/architecture.md | 88 --------- pylabrobot/arms/arm.py | 9 - pylabrobot/arms/arm_tests.py | 182 ------------------ pylabrobot/arms/articulated_arm.py | 10 - pylabrobot/arms/backend.py | 10 - pylabrobot/arms/orientable_arm.py | 10 - pylabrobot/arms/standard.py | 10 - pylabrobot/brooks/pf400_test.ipynb | 34 +--- pylabrobot/brooks/precise_flex.py | 66 +++++-- pylabrobot/brooks/precise_flex_tests.py | 75 ++++++++ pylabrobot/capabilities/arms/architecture.md | 38 ++-- pylabrobot/capabilities/arms/arm.py | 129 +++++++++++-- pylabrobot/capabilities/arms/arm_tests.py | 88 +++++++-- .../capabilities/arms/articulated_arm.py | 21 +- pylabrobot/capabilities/arms/backend.py | 56 +++++- .../capabilities/arms/orientable_arm.py | 24 +-- .../hamilton/liquid_handlers/star/core.py | 32 ++- .../hamilton/liquid_handlers/star/iswap.py | 58 +++--- .../liquid_handlers/star/misc/architecture.md | 4 +- .../liquid_handlers/star/misc/core_test.ipynb | 2 +- .../liquid_handlers/star/misc/demo.ipynb | 10 +- .../star/misc/iswap_test.ipynb | 12 +- .../hamilton/liquid_handlers/star/star.py | 12 +- .../liquid_handlers/star/tests/core_tests.py | 10 +- .../liquid_handlers/star/tests/iswap_tests.py | 22 ++- .../backends/hamilton/STAR_backend.py | 9 +- .../legacy/liquid_handling/liquid_handler.py | 8 +- pylabrobot/ufactory/xarm6/backend.py | 35 ++-- pylabrobot/ufactory/xarm6/backend_tests.py | 36 ++-- pylabrobot/ufactory/xarm6/xarm6.py | 6 +- 39 files changed, 606 insertions(+), 731 deletions(-) delete mode 100644 docs/api/pylabrobot.arms.rst delete mode 100644 pylabrobot/arms/__init__.py delete mode 100644 pylabrobot/arms/architecture.md delete mode 100644 pylabrobot/arms/arm.py delete mode 100644 pylabrobot/arms/arm_tests.py delete mode 100644 pylabrobot/arms/articulated_arm.py delete mode 100644 pylabrobot/arms/backend.py delete mode 100644 pylabrobot/arms/orientable_arm.py delete mode 100644 pylabrobot/arms/standard.py create mode 100644 pylabrobot/brooks/precise_flex_tests.py diff --git a/docs/api/pylabrobot.arms.rst b/docs/api/pylabrobot.arms.rst deleted file mode 100644 index 23d07ab85a9..00000000000 --- a/docs/api/pylabrobot.arms.rst +++ /dev/null @@ -1,42 +0,0 @@ -.. currentmodule:: pylabrobot.arms - -pylabrobot.arms package -======================= - -Arm capabilities for picking up, moving, and placing labware. - -.. autosummary:: - :toctree: _autosummary - :nosignatures: - :recursive: - - arm.GripperArm - orientable_arm.OrientableArm - - -Backends --------- - -.. autosummary:: - :toctree: _autosummary - :nosignatures: - :recursive: - - backend.GripperArmBackend - backend.OrientableGripperArmBackend - backend.ArticulatedGripperArmBackend - backend.CanFreedrive - backend.HasJoints - backend.CanGrip - - -Types ------ - -.. autosummary:: - :toctree: _autosummary - :nosignatures: - :recursive: - - standard.CartesianPose - standard.GripperDirection diff --git a/docs/api/pylabrobot.hamilton.rst b/docs/api/pylabrobot.hamilton.rst index a9ac7e4848e..50d6ae270a1 100644 --- a/docs/api/pylabrobot.hamilton.rst +++ b/docs/api/pylabrobot.hamilton.rst @@ -123,7 +123,7 @@ STAR Liquid Handler .. autoclass:: pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.ParkParams :members: -.. autoclass:: pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.CloseGripperParams +.. autoclass:: pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.GripParams :members: .. autoclass:: pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.PickUpParams diff --git a/docs/resources/index.md b/docs/resources/index.md index 82f30512712..11fdcfb7a80 100644 --- a/docs/resources/index.md +++ b/docs/resources/index.md @@ -55,7 +55,7 @@ PLR's `Resource` subclasses in the inheritance tree are: Resource ├── Arm - │ ├── ArticulatedArm + │ ├── ArticulatedGripperArm │ ├── CartesianArm │ └── SCARA diff --git a/docs/user_guide/brooks/precise_flex/hello-world.ipynb b/docs/user_guide/brooks/precise_flex/hello-world.ipynb index e2a5cb0638f..caf1737557c 100644 --- a/docs/user_guide/brooks/precise_flex/hello-world.ipynb +++ b/docs/user_guide/brooks/precise_flex/hello-world.ipynb @@ -40,24 +40,11 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": null, "id": "klqi0r4257k", "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "2026-04-20 21:20:11,081 - pylabrobot.brooks.precise_flex - INFO - [PreciseFlex 10.0.0.1] connected: port=10100\n" - ] - } - ], - "source": [ - "from pylabrobot.brooks import PreciseFlex400\n", - "\n", - "pf = PreciseFlex400(host=\"10.0.0.1\", port=10100, has_rail=True)\n", - "await pf.setup()" - ] + "outputs": [], + "source": "from pylabrobot.brooks import PreciseFlex400\n\n# closed_gripper_position is the firmware-unit value at min_gripper_width.\n# Calibrate against your specific gripper before first use.\npf = PreciseFlex400(host=\"10.0.0.1\", closed_gripper_position=500.0, port=10100, has_rail=True)\nawait pf.setup()" }, { "cell_type": "markdown", @@ -74,7 +61,7 @@ "source": [ "## Arm capabilities\n", "\n", - "The PreciseFlex exposes an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm` on `pf.arm`. For the full arm API, see [Arms](../../capabilities/arms)." + "The PreciseFlex exposes an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm` on `pf.arm`. For the full arm API, see [Arms](../../capabilities/arms)." ] }, { @@ -87,34 +74,11 @@ }, { "cell_type": "code", - "execution_count": 4, + "execution_count": null, "id": "trbcgxwrh6k", "metadata": {}, - "outputs": [ - { - "name": "stderr", - "output_type": "stream", - "text": [ - "2026-04-20 21:20:11,097 - pylabrobot.brooks.precise_flex - INFO - [PreciseFlex 10.0.0.1] open_gripper: width_mm=120\n", - "2026-04-20 21:20:11,233 - pylabrobot.brooks.precise_flex - INFO - [PreciseFlex 10.0.0.1] close_gripper: width_mm=80\n" - ] - }, - { - "data": { - "text/plain": [ - "False" - ] - }, - "execution_count": 4, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "await pf.arm.open_gripper(gripper_width=120)\n", - "await pf.arm.close_gripper(gripper_width=80)\n", - "await pf.arm.backend.is_gripper_closed()" - ] + "outputs": [], + "source": "await pf.arm.move_gripper(width=120, force_sensing=False)\nawait pf.arm.move_gripper(width=80, force_sensing=True)\nawait pf.arm.backend.is_gripper_closed()" }, { "cell_type": "code", @@ -391,4 +355,4 @@ }, "nbformat": 4, "nbformat_minor": 5 -} +} \ No newline at end of file diff --git a/docs/user_guide/capabilities/arms.md b/docs/user_guide/capabilities/arms.md index 33acb85bf53..0eb57ef1bb5 100644 --- a/docs/user_guide/capabilities/arms.md +++ b/docs/user_guide/capabilities/arms.md @@ -1,15 +1,16 @@ # Arms -Arms are capabilities for picking up, moving, and placing labware (plates, lids, etc.) on the deck. PLR provides two arm types: +Arms are capabilities for picking up, moving, and placing labware (plates, lids, etc.) on the deck. PLR provides three arm types: -- {class}`~pylabrobot.capabilities.arms.arm.GripperArm` -- a fixed-axis gripper arm (e.g. Hamilton core grippers). Grips along a single axis. -- {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm` -- a rotatable gripper arm (e.g. Hamilton iSWAP). Can grip from any direction. +- {class}`~pylabrobot.capabilities.arms.arm.FixedAxisGripperArm`: a fixed-axis gripper arm (e.g. Hamilton core grippers). Grips along a single deck-fixed axis. +- {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm`: a rotatable gripper arm (e.g. Hamilton iSWAP). Can grip from any direction. +- {class}`~pylabrobot.capabilities.arms.articulated_arm.ArticulatedGripperArm`: a fully articulated arm (e.g. UFACTORY xArm 6). Pick/drop with arbitrary 3D rotation. -Both inherit from `_BaseArm`, which is a {class}`~pylabrobot.capabilities.capability.Capability`. +All three inherit from {class}`~pylabrobot.capabilities.arms.arm.GripperArm` (the abstract base that owns gripper-width control), which in turn extends `_BaseArm`, a {class}`~pylabrobot.capabilities.capability.Capability`. ## When to use -Use arms to move plates, lids, and other labware between deck positions -- from a hotel to a reader, from a reader to a shaker, from a shaker to a centrifuge, etc. +Use arms to move plates, lids, and other labware between deck positions: from a hotel to a reader, from a reader to a shaker, from a shaker to a centrifuge, etc. ## Setup @@ -21,7 +22,7 @@ from pylabrobot.hamilton.star import STAR lh = STAR(name="star", ...) await lh.setup() -# the arm is at lh.iswap (OrientableArm) or lh.core_gripper (GripperArm) +# the arm is at lh.iswap (OrientableGripperArm) or lh.core_gripper (FixedAxisGripperArm) await lh.iswap.move_resource(plate, to=heater_shaker) ``` @@ -50,7 +51,7 @@ await lh.iswap.move_resource( ) ``` -### OrientableArm: grip direction +### OrientableGripperArm: grip direction `direction` is the world yaw of the gripper's front finger, in degrees, **CCW about +Z with 0° = +X**. You can pass a float, or one of the @@ -77,6 +78,35 @@ await lh.iswap.move_to_location(coord, direction=45.0) # 45° CCW from +X - **`GripperOrientation`** is either a {data}`~pylabrobot.capabilities.arms.standard.GripperDirection` string literal (`"front"`, `"right"`, `"back"`, `"left"`) or a float in degrees, measured CCW about world +Z with 0° = +X. - **`request_gripper_pose()`** queries the hardware for the current end effector position. `get_picked_up_resource()` returns the internally tracked state (no hardware call). +## Grippers + +Gripper actuation goes through a single fundamental call, `move_gripper(width, force_sensing)`, which has two modes: + +- `force_sensing=False`: drive the jaws to `width` mm without force feedback. The jaws reach exactly that width. +- `force_sensing=True`: close toward `width` mm with force feedback, stopping on contact. The final width may be larger than the target. + +Widths are always in mm. Each backend declares its hardware limits via `min_gripper_width` and `max_gripper_width` (either may be `None` if the gripper has no commandable open/close at that end). + +`open_gripper()` and `close_gripper()` are convenience wrappers built on top: `open_gripper()` calls `move_gripper(max_gripper_width, force_sensing=False)` and `close_gripper()` calls `move_gripper(min_gripper_width, force_sensing=True)`. They take no width but still forward `backend_params` so backend-specific options (e.g. `iSWAPBackend.GripParams`) work the same as with `move_gripper`. They raise `NotImplementedError` if the corresponding limit is `None`. + +## Migration + +The arm capability was reshaped in the v1b1 release. The old +`pylabrobot.arms` shim package has been removed; import from +`pylabrobot.capabilities.arms.*` instead. + +| Old | New | +|:----|:----| +| `pylabrobot.arms.GripperArm` (concrete) | {class}`~pylabrobot.capabilities.arms.arm.FixedAxisGripperArm` | +| `pylabrobot.arms.OrientableArm` | {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm` | +| `pylabrobot.arms.ArticulatedArm` | {class}`~pylabrobot.capabilities.arms.articulated_arm.ArticulatedGripperArm` | +| `iSWAPBackend.CloseGripperParams` | {class}`~pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.GripParams` | +| `open_gripper(width=...)` / `close_gripper(width=...)` | `move_gripper(width=..., force_sensing=...)` | + +The name `GripperArm` still exists in `pylabrobot.capabilities.arms.arm` but +now refers to the abstract base. Code that did `class MyArm(GripperArm)` over +the old concrete class should subclass `FixedAxisGripperArm` instead. + ## Supported hardware ```{supported-devices} arm @@ -84,4 +114,4 @@ await lh.iswap.move_to_location(coord, direction=45.0) # 45° CCW from +X ## API reference -See {class}`~pylabrobot.capabilities.arms.arm.GripperArm`, {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm`, {class}`~pylabrobot.capabilities.arms.backend.GripperArmBackend`, and {class}`~pylabrobot.capabilities.arms.backend.OrientableGripperArmBackend`. +See {class}`~pylabrobot.capabilities.arms.arm.GripperArm`, {class}`~pylabrobot.capabilities.arms.arm.FixedAxisGripperArm`, {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm`, {class}`~pylabrobot.capabilities.arms.articulated_arm.ArticulatedGripperArm`, {class}`~pylabrobot.capabilities.arms.backend.GripperArmBackend`, and {class}`~pylabrobot.capabilities.arms.backend.OrientableGripperArmBackend`. diff --git a/docs/user_guide/hamilton/star/core-grippers.ipynb b/docs/user_guide/hamilton/star/core-grippers.ipynb index ced20d7ae25..e51b851fdc1 100644 --- a/docs/user_guide/hamilton/star/core-grippers.ipynb +++ b/docs/user_guide/hamilton/star/core-grippers.ipynb @@ -67,7 +67,7 @@ "source": [ "## Moving resources\n", "\n", - "Use {meth}`~pylabrobot.hamilton.liquid_handlers.star.star.STAR.core_grippers` as an async context manager. It picks up the gripper tools when entering and returns them when exiting. The yielded {class}`~pylabrobot.capabilities.arms.arm.GripperArm` has two APIs:\n", + "Use {meth}`~pylabrobot.hamilton.liquid_handlers.star.star.STAR.core_grippers` as an async context manager. It picks up the gripper tools when entering and returns them when exiting. The yielded {class}`~pylabrobot.capabilities.arms.arm.FixedAxisGripperArm` has two APIs:\n", "\n", "- `move_resource`: a single call that picks up a resource and drops it at a destination.\n", "- `pick_up_resource` and `drop_resource`: two separate calls for more control over timing." @@ -157,4 +157,4 @@ }, "nbformat": 4, "nbformat_minor": 4 -} +} \ No newline at end of file diff --git a/docs/user_guide/hamilton/star/iswap.ipynb b/docs/user_guide/hamilton/star/iswap.ipynb index e9719858414..95a3bd358f3 100644 --- a/docs/user_guide/hamilton/star/iswap.ipynb +++ b/docs/user_guide/hamilton/star/iswap.ipynb @@ -6,7 +6,7 @@ "source": [ "# iSWAP Module\n", "\n", - "The iSWAP is a plate transport gripper arm on the Hamilton STAR(let). After {meth}`~pylabrobot.hamilton.liquid_handlers.star.star._HamiltonSTAR.setup`, it is available as `star.iswap` — an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm`." + "The iSWAP is a plate transport gripper arm on the Hamilton STAR(let). After {meth}`~pylabrobot.hamilton.liquid_handlers.star.star._HamiltonSTAR.setup`, it is available as `star.iswap` — an {class}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm`." ] }, { @@ -252,77 +252,41 @@ "await star.driver.iswap.move_relative_z(step_size=-10)" ] }, + { + "cell_type": "markdown", + "source": "### Gripper control\n\nTwo equivalent ways to actuate the gripper:\n\n```python\n# explicit width — pick when you need a specific target\nawait star.iswap.move_gripper(width=130, force_sensing=False)\nawait star.iswap.move_gripper(width=85, force_sensing=True)\n\n# shortcuts that read max/min from the backend\nawait star.iswap.open_gripper() # == move_gripper(max_gripper_width, force_sensing=False)\nawait star.iswap.close_gripper() # == move_gripper(min_gripper_width, force_sensing=True)\n```\n\nThe subsections below show the explicit-width form, which is what you'll usually want for plate handling. Use the shortcuts when you just need the gripper fully open or fully closed.", + "metadata": {} + }, { "cell_type": "markdown", "metadata": {}, - "source": [ - "### Opening the gripper\n", - "\n", - "Open the iSWAP gripper using {meth}`~pylabrobot.capabilities.arms.orientable_arm.OrientableArm.open_gripper`. **Warning**: this will release any object that is gripped. Useful for error recovery." - ] + "source": "### Opening the gripper\n\nOpen the iSWAP gripper using {meth}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm.open_gripper`, which drives to the backend's `max_gripper_width`. Useful for error recovery.\n\n```{warning}\nThis will release any object that is gripped.\n```\n\nFor an explicit width, use {meth}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm.move_gripper` with `force_sensing=False`." }, { "cell_type": "code", - "execution_count": 15, + "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "await star.iswap.open_gripper(gripper_width=130)" - ] + "source": "await star.iswap.move_gripper(width=130, force_sensing=False)" }, { "cell_type": "markdown", "metadata": {}, - "source": [ - "### Closing the gripper\n", - "\n", - "Close the iSWAP gripper. Pass {class}`~pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.CloseGripperParams` via `backend_params` to control grip strength and plate width tolerance." - ] + "source": "### Closing the gripper\n\nClose the iSWAP gripper with force feedback by calling {meth}`~pylabrobot.capabilities.arms.orientable_arm.OrientableGripperArm.move_gripper` with `force_sensing=True`. Pass {class}`~pylabrobot.hamilton.liquid_handlers.star.iswap.iSWAPBackend.GripParams` via `backend_params` to control grip strength and plate width tolerance." }, { "cell_type": "code", - "execution_count": 16, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "As expected (no plate gripped): {'ISWAP': NoElementError('Plate not found')}\n" - ] - } - ], - "source": [ - "from pylabrobot.hamilton.liquid_handlers.star.errors import STARFirmwareError\n", - "\n", - "try:\n", - " await star.iswap.close_gripper(gripper_width=120)\n", - "except STARFirmwareError as e:\n", - " print(f\"As expected (no plate gripped): {e.errors}\")" - ] + "outputs": [], + "source": "from pylabrobot.hamilton.liquid_handlers.star.errors import STARFirmwareError\n\ntry:\n await star.iswap.move_gripper(width=120, force_sensing=True)\nexcept STARFirmwareError as e:\n print(f\"As expected (no plate gripped): {e.errors}\")" }, { "cell_type": "code", - "execution_count": 17, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "As expected (no plate gripped): {'ISWAP': NoElementError('Plate not found')}\n" - ] - } - ], - "source": [ - "try:\n", - " await star.iswap.close_gripper(\n", - " gripper_width=85,\n", - " backend_params=iSWAPBackend.CloseGripperParams(grip_strength=8, plate_width_tolerance=1.0),\n", - " )\n", - "except STARFirmwareError as e:\n", - " print(f\"As expected (no plate gripped): {e.errors}\")" - ] + "outputs": [], + "source": "try:\n await star.iswap.move_gripper(\n width=85,\n force_sensing=True,\n backend_params=iSWAPBackend.GripParams(grip_strength=8, plate_width_tolerance=1.0),\n )\nexcept STARFirmwareError as e:\n print(f\"As expected (no plate gripped): {e.errors}\")" }, { "cell_type": "markdown", diff --git a/docs/user_guide/ufactory/xarm6/hello-world.ipynb b/docs/user_guide/ufactory/xarm6/hello-world.ipynb index 43cc3fe9b6e..413f8f2430c 100644 --- a/docs/user_guide/ufactory/xarm6/hello-world.ipynb +++ b/docs/user_guide/ufactory/xarm6/hello-world.ipynb @@ -29,7 +29,7 @@ { "cell_type": "markdown", "id": "xarm6-arm-header", - "source": "## Arm capabilities\n\nThe xArm 6 exposes an {class}`~pylabrobot.capabilities.arms.articulated_arm.ArticulatedArm` on `xarm.arm`, backed by {class}`~pylabrobot.ufactory.xarm6.backend.XArm6ArmBackend`. For the full arm API, see [Arms](../../capabilities/arms).", + "source": "## Arm capabilities\n\nThe xArm 6 exposes an {class}`~pylabrobot.capabilities.arms.articulated_arm.ArticulatedGripperArm` on `xarm.arm`, backed by {class}`~pylabrobot.ufactory.xarm6.backend.XArm6ArmBackend`. For the full arm API, see [Arms](../../capabilities/arms).", "metadata": {} }, { @@ -41,7 +41,7 @@ { "cell_type": "code", "id": "xarm6-gripper-code", - "source": "await xarm.arm.open_gripper(gripper_width=150)", + "source": "await xarm.arm.open_gripper()", "metadata": {}, "execution_count": null, "outputs": [] @@ -49,7 +49,7 @@ { "cell_type": "code", "id": "a730d6d4", - "source": "await xarm.arm.close_gripper(gripper_width=71)", + "source": "await xarm.arm.close_gripper()", "metadata": {}, "execution_count": null, "outputs": [] diff --git a/pylabrobot/arms/__init__.py b/pylabrobot/arms/__init__.py deleted file mode 100644 index 41f1ad785ce..00000000000 --- a/pylabrobot/arms/__init__.py +++ /dev/null @@ -1,9 +0,0 @@ -import warnings - -warnings.warn( - "Importing from pylabrobot.arms is deprecated. Use pylabrobot.capabilities.arms instead.", - DeprecationWarning, - stacklevel=2, -) - -from pylabrobot.capabilities.arms import * # noqa: F401,F403,E402 diff --git a/pylabrobot/arms/architecture.md b/pylabrobot/arms/architecture.md deleted file mode 100644 index 99ccf47d5ee..00000000000 --- a/pylabrobot/arms/architecture.md +++ /dev/null @@ -1,88 +0,0 @@ -# Arms architecture - -## Frontend hierarchy (capabilities) - -``` -_BaseArm(Capability) - │ halt(), park(), get_gripper_location() - │ resource tracking (pick_up/drop state) - │ - └── GripperArm - │ open/close_gripper, is_gripper_closed - │ pick_up/drop/move at location - │ pick_up_resource(), drop_resource(), move_resource() (convenience) - │ - └── OrientableArm - Arm with rotation. E.g. Hamilton iSWAP, PreciseFlex. - pick_up/drop/move with direction parameter -``` - -Frontend mirrors backend hierarchy exactly. -Joint-space methods are backend-only (robot-specific), accessed via `arm.backend`. - -## Backend hierarchy (capability backends) - -``` -_BaseArmBackend(CapabilityBackend) - │ halt(), park(), get_gripper_location() - │ - ├── GripperArmBackend - │ open/close_gripper, is_gripper_closed - │ pick_up/drop/move at location (no rotation) - │ - ├── OrientableGripperArmBackend - │ pick_up/drop/move with direction (float degrees) - │ - └── ArticulatedGripperArmBackend - pick_up/drop/move with full Rotation -``` - -## Mixins (backend) - -- `HasJoints` — joint-space control: pick_up/drop/move at joint position, get_joint_position -- `CanFreedrive` — freedrive (manual guidance) mode - -## Concrete implementations - -| Device | Driver | Arm Backend | Frontend | -|--------|--------|-------------|----------| -| Hamilton STAR (iSWAP) | STARDriver (shared) | `iSWAP(OrientableGripperArmBackend)` | `OrientableArm` | -| Hamilton STAR (core) | STARDriver (shared) | `CoreGripper(GripperArmBackend)` | `Arm` | -| PreciseFlex 400 | `PreciseFlexDriver` | `PreciseFlexArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive)` | `OrientableArm` | - -## Usage - -Arms are capabilities, not devices. They are owned by a Device: - -```python -class STAR(Device): - def __init__(self, ...): - driver = STARDriver(...) - super().__init__(driver=driver) - self.iswap = OrientableArm(backend=iSWAP(driver), reference_resource=deck) - self.core_gripper = GripperArm(backend=CoreGripper(driver), reference_resource=deck) - self._capabilities = [self.iswap, self.core_gripper] -``` - -A standalone arm (like PreciseFlex) is a Device with a single arm capability: - -```python -class PreciseFlex400(Device): - def __init__( - self, host, port=10100, has_rail=False, timeout=20, gripper_length=162.0, gripper_z_offset=0.0 - ): - driver = PreciseFlexDriver(host=host, port=port, timeout=timeout) - super().__init__(driver=driver) - backend = PreciseFlexArmBackend( - driver=driver, - has_rail=has_rail, - gripper_length=gripper_length, - gripper_z_offset=gripper_z_offset, - ) - self.arm = OrientableArm(backend=backend, reference_resource=self.reference) - self._capabilities = [self.arm] - -# Joint methods accessed via backend (robot-specific): -await pf.arm.backend.move_to_joint_position({1: 0, 2: 90, 3: 45}) -await pf.arm.backend.start_freedrive_mode(free_axes=[0]) -``` diff --git a/pylabrobot/arms/arm.py b/pylabrobot/arms/arm.py deleted file mode 100644 index b5b4154b806..00000000000 --- a/pylabrobot/arms/arm.py +++ /dev/null @@ -1,9 +0,0 @@ -import warnings - -warnings.warn( - "Importing from pylabrobot.arms.arm is deprecated. Use pylabrobot.capabilities.arms.arm instead.", - DeprecationWarning, - stacklevel=2, -) - -from pylabrobot.capabilities.arms.arm import * # noqa: F401,F403,E402 diff --git a/pylabrobot/arms/arm_tests.py b/pylabrobot/arms/arm_tests.py deleted file mode 100644 index e391e4978df..00000000000 --- a/pylabrobot/arms/arm_tests.py +++ /dev/null @@ -1,182 +0,0 @@ -import unittest -from unittest.mock import AsyncMock, MagicMock - -from pylabrobot.arms.arm import GripperArm -from pylabrobot.arms.backend import ( - GripperArmBackend, - OrientableGripperArmBackend, -) -from pylabrobot.arms.orientable_arm import OrientableArm -from pylabrobot.resources import Coordinate, Resource, ResourceHolder - - -def _assert_location(test, call, x, y, z, places=1): - """Assert the location kwarg of a mock call matches expected coordinates.""" - loc = call.kwargs["location"] - test.assertAlmostEqual(loc.x, x, places=places) - test.assertAlmostEqual(loc.y, y, places=places) - test.assertAlmostEqual(loc.z, z, places=places) - - -def _make_deck_with_sites(): - """Create a fictional deck with two sites and a plate. - - Deck: 1000x1000x0 at origin. - Site A at (100, 100, 50), site B at (100, 300, 50). - Plate: 120x80x10 assigned to site A. - """ - deck = Resource("deck", size_x=1000, size_y=1000, size_z=0) - - site_a = ResourceHolder("site_a", size_x=130, size_y=90, size_z=0) - deck.assign_child_resource(site_a, location=Coordinate(100, 100, 50)) - - site_b = ResourceHolder("site_b", size_x=130, size_y=90, size_z=0) - deck.assign_child_resource(site_b, location=Coordinate(100, 300, 50)) - - plate = Resource("plate", size_x=120, size_y=80, size_z=10) - site_a.assign_child_resource(plate, location=Coordinate(5, 5, 0)) - - return deck, site_a, site_b, plate - - -class TestArm(unittest.IsolatedAsyncioTestCase): - """Test Arm (ArmBackend, no rotation). E.g. Hamilton core grippers.""" - - async def asyncSetUp(self): - self.mock_backend = MagicMock(spec=GripperArmBackend) - for method_name in [ - "pick_up_at_location", - "drop_at_location", - "move_to_location", - "open_gripper", - "close_gripper", - "is_gripper_closed", - "halt", - "park", - ]: - setattr(self.mock_backend, method_name, AsyncMock()) - - self.deck, self.site_a, self.site_b, self.plate = _make_deck_with_sites() - self.arm = GripperArm(backend=self.mock_backend, reference_resource=self.deck) - - async def test_pick_up_resource(self): - # plate at site_a(100,100,50) + child_loc(5,5,0), center_xy=(60,40), size_z=10 - # pickup_distance_from_bottom=8 → z = 50 + 8 = 58 - await self.arm.pick_up_resource(self.plate, pickup_distance_from_bottom=8) - call = self.mock_backend.pick_up_at_location.call_args - _assert_location(self, call, 165, 145, 58) - # default grip_axis="x" → resource_width is X size = 120 - self.assertAlmostEqual(call.kwargs["resource_width"], 120) - - async def test_drop_resource(self): - await self.arm.pick_up_resource(self.plate, pickup_distance_from_bottom=8) - await self.arm.drop_resource(self.site_b) - call = self.mock_backend.drop_at_location.call_args - # site_b(100,300,50) + default_child_loc(0,0,0), size_z=10 - # pickup_distance_from_bottom=8 → z = 50 + 8 = 58 - _assert_location(self, call, 160, 340, 58) - self.assertEqual(self.plate.parent.name, "site_b") - - async def test_pick_up_at_location(self): - location = Coordinate(x=100, y=200, z=300) - await self.arm.pick_up_at_location(location, resource_width=80.0) - self.mock_backend.pick_up_at_location.assert_called_once_with( - location=location, resource_width=80.0, backend_params=None - ) - - async def test_drop_at_location(self): - location = Coordinate(x=100, y=200, z=300) - await self.arm.pick_up_at_location(location, resource_width=80.0) - await self.arm.drop_at_location(location) - self.mock_backend.drop_at_location.assert_called_once_with( - location=location, resource_width=80.0, backend_params=None - ) - - async def test_move_to_location(self): - location = Coordinate(x=100, y=200, z=300) - await self.arm.move_to_location(location) - self.mock_backend.move_to_location.assert_called_once_with( - location=location, backend_params=None - ) - - async def test_open_gripper(self): - await self.arm.open_gripper(gripper_width=50.0) - self.mock_backend.open_gripper.assert_called_once_with(gripper_width=50.0, backend_params=None) - - async def test_halt(self): - await self.arm.halt() - self.mock_backend.halt.assert_called_once() - - async def test_park(self): - await self.arm.park() - self.mock_backend.park.assert_called_once() - - async def test_grip_axis_y(self): - """With grip_axis='y', resource_width should be the Y size.""" - arm_y = GripperArm(backend=self.mock_backend, reference_resource=self.deck, grip_axis="y") - await arm_y.pick_up_resource(self.plate, pickup_distance_from_bottom=8) - call = self.mock_backend.pick_up_at_location.call_args - # plate size_y=80 - self.assertAlmostEqual(call.kwargs["resource_width"], 80) - - -class TestOrientableArm(unittest.IsolatedAsyncioTestCase): - """Test OrientableArm coordinate computation with fictional resources.""" - - async def asyncSetUp(self): - self.mock_backend = MagicMock(spec=OrientableGripperArmBackend) - for method_name in [ - "pick_up_at_location", - "drop_at_location", - "move_to_location", - ]: - setattr(self.mock_backend, method_name, AsyncMock()) - - self.deck, self.site_a, self.site_b, self.plate = _make_deck_with_sites() - self.arm = OrientableArm(backend=self.mock_backend, reference_resource=self.deck) - - async def test_pick_up_front(self): - await self.arm.pick_up_resource( - self.plate, pickup_distance_from_bottom=8, direction="front" - ) - call = self.mock_backend.pick_up_at_location.call_args - _assert_location(self, call, 165, 145, 58) - # "front" = -Y in deck frame = 270° under the +X-is-zero convention. - self.assertAlmostEqual(call.kwargs["direction"], 270.0) - # "front" grips along the X axis → X width = 120 - self.assertAlmostEqual(call.kwargs["resource_width"], 120) - - async def test_pick_up_right(self): - await self.arm.pick_up_resource( - self.plate, pickup_distance_from_bottom=8, direction="right" - ) - call = self.mock_backend.pick_up_at_location.call_args - # "right" = +X = 0° under the +X-is-zero convention. - self.assertAlmostEqual(call.kwargs["direction"], 0.0) - # "right" grips along the Y axis → Y width = 80 - self.assertAlmostEqual(call.kwargs["resource_width"], 80) - - async def test_drop_at_location(self): - location = Coordinate(x=100, y=200, z=300) - await self.arm.pick_up_at_location(location, resource_width=80.0, direction=0.0) - await self.arm.drop_at_location(location, direction=180.0) - self.mock_backend.drop_at_location.assert_called_once_with( - location=location, direction=180.0, resource_width=80.0, backend_params=None - ) - - async def test_move_to_location(self): - location = Coordinate(x=100, y=200, z=300) - await self.arm.move_to_location(location, direction=90.0) - self.mock_backend.move_to_location.assert_called_once_with( - location=location, direction=90.0, backend_params=None - ) - - async def test_move_plate(self): - """Pick from site_a, drop at site_b.""" - await self.arm.pick_up_resource( - self.plate, pickup_distance_from_bottom=8, direction="front" - ) - await self.arm.drop_resource(self.site_b, direction="front") - drop_call = self.mock_backend.drop_at_location.call_args - _assert_location(self, drop_call, 160, 340, 58) - self.assertEqual(self.plate.parent.name, "site_b") diff --git a/pylabrobot/arms/articulated_arm.py b/pylabrobot/arms/articulated_arm.py deleted file mode 100644 index 2336046e98e..00000000000 --- a/pylabrobot/arms/articulated_arm.py +++ /dev/null @@ -1,10 +0,0 @@ -import warnings - -warnings.warn( - "Importing from pylabrobot.arms.articulated_arm is deprecated. " - "Use pylabrobot.capabilities.arms.articulated_arm instead.", - DeprecationWarning, - stacklevel=2, -) - -from pylabrobot.capabilities.arms.articulated_arm import * # noqa: F401,F403,E402 diff --git a/pylabrobot/arms/backend.py b/pylabrobot/arms/backend.py deleted file mode 100644 index 204aca34066..00000000000 --- a/pylabrobot/arms/backend.py +++ /dev/null @@ -1,10 +0,0 @@ -import warnings - -warnings.warn( - "Importing from pylabrobot.arms.backend is deprecated. " - "Use pylabrobot.capabilities.arms.backend instead.", - DeprecationWarning, - stacklevel=2, -) - -from pylabrobot.capabilities.arms.backend import * # noqa: F401,F403,E402 diff --git a/pylabrobot/arms/orientable_arm.py b/pylabrobot/arms/orientable_arm.py deleted file mode 100644 index cc0596a6040..00000000000 --- a/pylabrobot/arms/orientable_arm.py +++ /dev/null @@ -1,10 +0,0 @@ -import warnings - -warnings.warn( - "Importing from pylabrobot.arms.orientable_arm is deprecated. " - "Use pylabrobot.capabilities.arms.orientable_arm instead.", - DeprecationWarning, - stacklevel=2, -) - -from pylabrobot.capabilities.arms.orientable_arm import * # noqa: F401,F403,E402 diff --git a/pylabrobot/arms/standard.py b/pylabrobot/arms/standard.py deleted file mode 100644 index 2f653072394..00000000000 --- a/pylabrobot/arms/standard.py +++ /dev/null @@ -1,10 +0,0 @@ -import warnings - -warnings.warn( - "Importing from pylabrobot.arms.standard is deprecated. " - "Use pylabrobot.capabilities.arms.standard instead.", - DeprecationWarning, - stacklevel=2, -) - -from pylabrobot.capabilities.arms.standard import * # noqa: F401,F403,E402 diff --git a/pylabrobot/brooks/pf400_test.ipynb b/pylabrobot/brooks/pf400_test.ipynb index 037b77b8efb..a0a885748cf 100644 --- a/pylabrobot/brooks/pf400_test.ipynb +++ b/pylabrobot/brooks/pf400_test.ipynb @@ -3,32 +3,14 @@ { "cell_type": "markdown", "metadata": {}, - "source": [ - "# PreciseFlex 400 — Teaching & Freedrive Notebook\n", - "\n", - "Uses the `JointArm` frontend with a `PreciseFlex400Backend`." - ] + "source": "# PreciseFlex 400 — Teaching & Freedrive Notebook\n\nUses the `OrientableGripperArm` frontend with a `PreciseFlex400Backend`." }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "import json\n", - "from pathlib import Path\n", - "\n", - "from pylabrobot.arms.joint_arm import JointArm\n", - "from pylabrobot.brooks.precise_flex import (\n", - " ElbowOrientation,\n", - " PFAxis,\n", - " PreciseFlex400Backend,\n", - " PreciseFlexBackend,\n", - ")\n", - "from pylabrobot.resources import Coordinate, Resource, Rotation\n", - "\n", - "POSITIONS_FILE = Path(\"positions.json\")" - ] + "source": "import json\nfrom pathlib import Path\n\nfrom pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm\nfrom pylabrobot.brooks.precise_flex import (\n ElbowOrientation,\n PFAxis,\n PreciseFlex400Backend,\n PreciseFlexBackend,\n)\nfrom pylabrobot.resources import Coordinate, Resource, Rotation\n\nPOSITIONS_FILE = Path(\"positions.json\")" }, { "cell_type": "code", @@ -88,7 +70,7 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": "backend = PreciseFlex400Backend(host=\"192.168.0.1\")\nreference = Resource(\"workcell\", size_x=2000, size_y=2000, size_z=0)\narm = JointArm(backend=backend, reference_resource=reference)\n\nawait arm.setup()\nprint(f\"Version: {await backend.request_version()}\")" + "source": "backend = PreciseFlex400Backend(host=\"192.168.0.1\")\nreference = Resource(\"workcell\", size_x=2000, size_y=2000, size_z=0)\narm = OrientableGripperArm(backend=backend, reference_resource=reference)\n\nawait arm.setup()\nprint(f\"Version: {await backend.request_version()}\")" }, { "cell_type": "markdown", @@ -279,20 +261,14 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "await arm.open_gripper(gripper_width=80.0)\n", - "print(f\"Gripper closed: {await arm.is_gripper_closed()}\")" - ] + "source": "await arm.move_gripper(width=80.0, force_sensing=False)\nprint(f\"Gripper closed: {await arm.is_gripper_closed()}\")" }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "await arm.close_gripper(gripper_width=10.0)\n", - "print(f\"Gripper closed: {await arm.is_gripper_closed()}\")" - ] + "source": "await arm.move_gripper(width=10.0, force_sensing=True)\nprint(f\"Gripper closed: {await arm.is_gripper_closed()}\")" }, { "cell_type": "markdown", diff --git a/pylabrobot/brooks/precise_flex.py b/pylabrobot/brooks/precise_flex.py index 3a918f82b95..d36a59ada05 100644 --- a/pylabrobot/brooks/precise_flex.py +++ b/pylabrobot/brooks/precise_flex.py @@ -14,7 +14,7 @@ HasJoints, OrientableGripperArmBackend, ) -from pylabrobot.capabilities.arms.orientable_arm import OrientableArm +from pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm from pylabrobot.capabilities.arms.standard import CartesianPose, JointPose from pylabrobot.capabilities.capability import BackendParams from pylabrobot.device import Device, Driver @@ -335,6 +335,7 @@ def __init__( driver: PreciseFlexDriver, gripper_length: float, gripper_z_offset: float, + closed_gripper_position: float, is_dual_gripper: bool = False, has_rail: bool = False, ) -> None: @@ -346,6 +347,11 @@ def __init__( gripper_z_offset: vertical offset in mm from the wrist plate to the tool tip. Depends on the mounted gripper; the concrete Device wrapper supplies a model-appropriate default. + closed_gripper_position: firmware-unit value (passed to ``GripClosePos`` / + ``GripOpenPos``) at which the jaws are at :attr:`min_gripper_width`. + Depends on the mounted gripper. The conversion mm → firmware units is + linear with slope 1: ``units = closed_gripper_position + (width_mm - + min_gripper_width)``. """ super().__init__() self.driver = driver @@ -356,6 +362,7 @@ def __init__( self.horizontal_compliance_torque: int = 0 self._has_rail = has_rail self._is_dual_gripper = is_dual_gripper + self.closed_gripper_position = closed_gripper_position self._kinematics_params = kinematics.PF400Params( gripper_length=gripper_length, gripper_z_offset=gripper_z_offset ) @@ -411,21 +418,43 @@ async def _request_speed(self) -> float: """Get the current speed percentage of the arm's movement.""" return await self.request_profile_speed(self.profile_index) - async def open_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None - ): - """Open the gripper to the specified width.""" - logger.info("[PreciseFlex %s] open_gripper: width_mm=%s", self.driver.io._host, gripper_width) - await self._set_grip_open_pos(gripper_width) - await self.driver.send_command("gripper 1") + # Physical jaw range for the PF400 servoed gripper. + min_gripper_width: float = 60.0 + max_gripper_width: float = 145.0 + + def _mm_to_firmware_units(self, width_mm: float) -> float: + """Convert a jaw width (mm) to the firmware's native position unit. + + Anchored at :attr:`closed_gripper_position`, which is the firmware value + when the jaws are at :attr:`min_gripper_width`. Slope is 1 (1 mm = 1 unit). + """ + return self.closed_gripper_position + (width_mm - self.min_gripper_width) - async def close_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None + async def move_gripper( + self, + width: float, + force_sensing: bool = False, + backend_params: Optional[BackendParams] = None, ): - """Close the gripper to the specified width.""" - logger.info("[PreciseFlex %s] close_gripper: width_mm=%s", self.driver.io._host, gripper_width) - await self._set_grip_close_pos(gripper_width) - await self.driver.send_command("gripper 2") + """Move the PreciseFlex gripper jaws. + + ``force_sensing=False`` drives to the open position (``gripper 1``); + ``force_sensing=True`` drives to the close position with force feedback + (``gripper 2``), which may stop short of ``width`` on contact. + """ + logger.info( + "[PreciseFlex %s] move_gripper: width_mm=%s force_sensing=%s", + self.driver.io._host, + width, + force_sensing, + ) + units = self._mm_to_firmware_units(width) + if force_sensing: + await self._set_grip_close_pos(units) + await self.driver.send_command("gripper 2") + else: + await self._set_grip_open_pos(units) + await self.driver.send_command("gripper 1") async def halt(self, backend_params: Optional[BackendParams] = None): """Stops the current robot immediately but leaves power on.""" @@ -1713,6 +1742,7 @@ class PreciseFlex400(Device): def __init__( self, host: str, + closed_gripper_position: float, port: int = 10100, has_rail: bool = False, timeout: int = 20, @@ -1721,6 +1751,9 @@ def __init__( ) -> None: """ Args: + closed_gripper_position: firmware-unit value at which the jaws are at the + backend's :attr:`~PreciseFlexArmBackend.min_gripper_width`. Depends on + the specific gripper mounted; calibrate before first use. gripper_length: wrist-axis → TCP distance in mm. Defaults to 162 mm, which matches the stock single gripper on the PF400. gripper_z_offset: vertical offset in mm from the wrist plate to the tool tip. @@ -1734,9 +1767,10 @@ def __init__( has_rail=has_rail, gripper_length=gripper_length, gripper_z_offset=gripper_z_offset, + closed_gripper_position=closed_gripper_position, ) self.reference = Resource(name="PreciseFlex400", size_x=200, size_y=200, size_z=200) - self.arm = OrientableArm(backend=backend, reference_resource=self.reference) + self.arm = OrientableGripperArm(backend=backend, reference_resource=self.reference) self._capabilities = [self.arm] @@ -1748,6 +1782,7 @@ def __init__( driver: PreciseFlexDriver, gripper_length: float, gripper_z_offset: float, + closed_gripper_position: float, has_rail: bool = False, ) -> None: super().__init__( @@ -1755,4 +1790,5 @@ def __init__( has_rail=has_rail, gripper_length=gripper_length, gripper_z_offset=gripper_z_offset, + closed_gripper_position=closed_gripper_position, ) diff --git a/pylabrobot/brooks/precise_flex_tests.py b/pylabrobot/brooks/precise_flex_tests.py new file mode 100644 index 00000000000..73fa9496494 --- /dev/null +++ b/pylabrobot/brooks/precise_flex_tests.py @@ -0,0 +1,75 @@ +import unittest +from typing import Tuple +from unittest.mock import AsyncMock, MagicMock + +from pylabrobot.brooks.precise_flex import PreciseFlex400Backend + + +def _make_backend( + closed_gripper_position: float = 500.0, +) -> Tuple[PreciseFlex400Backend, MagicMock]: + driver = MagicMock() + driver.send_command = AsyncMock(return_value="") + driver.io._host = "localhost" + backend = PreciseFlex400Backend( + driver=driver, + gripper_length=162.0, + gripper_z_offset=0.0, + closed_gripper_position=closed_gripper_position, + ) + return backend, driver + + +class TestPreciseFlex400Gripper(unittest.IsolatedAsyncioTestCase): + def setUp(self): + # closed_gripper_position=500 ⇒ min_gripper_width(60mm) maps to 500 units. + self.backend, self.driver = _make_backend(closed_gripper_position=500.0) + + def _sent_commands(self) -> list[str]: + return [c.args[0] for c in self.driver.send_command.call_args_list] + + async def test_move_gripper_force_sensing_false_opens_with_position(self): + # 80 mm ⇒ 500 + (80 - 60) = 520 firmware units. + await self.backend.move_gripper(width=80.0, force_sensing=False) + self.assertEqual(self._sent_commands(), ["GripOpenPos 520.0", "gripper 1"]) + + async def test_move_gripper_force_sensing_true_closes_with_position(self): + # 60 mm (the closed reference) ⇒ exactly closed_gripper_position. + await self.backend.move_gripper(width=60.0, force_sensing=True) + self.assertEqual(self._sent_commands(), ["GripClosePos 500.0", "gripper 2"]) + + async def test_move_gripper_position_command_precedes_move(self): + await self.backend.move_gripper(width=120.0, force_sensing=False) + commands = self._sent_commands() + self.assertLess( + commands.index("GripOpenPos 560.0"), + commands.index("gripper 1"), + "Position must be set before the gripper move command fires.", + ) + + async def test_force_sensing_branches_use_different_firmware_commands(self): + await self.backend.move_gripper(width=90.0, force_sensing=False) + await self.backend.move_gripper(width=90.0, force_sensing=True) + commands = self._sent_commands() + self.assertIn("gripper 1", commands) + self.assertIn("gripper 2", commands) + self.assertIn("GripOpenPos 530.0", commands) + self.assertIn("GripClosePos 530.0", commands) + + async def test_min_max_gripper_width_advertised(self): + self.assertEqual(self.backend.min_gripper_width, 60.0) + self.assertEqual(self.backend.max_gripper_width, 145.0) + + async def test_closed_gripper_position_shifts_units(self): + # Different anchor ⇒ same width yields a different firmware-unit target. + backend, driver = _make_backend(closed_gripper_position=1000.0) + await backend.move_gripper(width=80.0, force_sensing=False) + commands = [c.args[0] for c in driver.send_command.call_args_list] + # 80 mm ⇒ 1000 + (80 - 60) = 1020 units. + self.assertEqual(commands, ["GripOpenPos 1020.0", "gripper 1"]) + + def test_mm_to_firmware_units_helper(self): + # Direct check of the linear mapping. + self.assertEqual(self.backend._mm_to_firmware_units(60.0), 500.0) + self.assertEqual(self.backend._mm_to_firmware_units(145.0), 585.0) + self.assertEqual(self.backend._mm_to_firmware_units(100.0), 540.0) diff --git a/pylabrobot/capabilities/arms/architecture.md b/pylabrobot/capabilities/arms/architecture.md index ddae7dfd315..de03f85373b 100644 --- a/pylabrobot/capabilities/arms/architecture.md +++ b/pylabrobot/capabilities/arms/architecture.md @@ -30,17 +30,23 @@ must interpret that float under the convention above. ``` _BaseArm(Capability) - │ halt(), park(), get_gripper_location() + │ halt(), park(), request_gripper_pose() │ resource tracking (pick_up/drop state) │ - └── GripperArm - │ open/close_gripper, is_gripper_closed - │ pick_up/drop/move at location - │ pick_up_resource(), drop_resource(), move_resource() (convenience) + └── GripperArm (abstract base for any arm with a gripper) + │ move_gripper, open/close_gripper, is_gripper_closed │ - └── OrientableArm - Arm with rotation. E.g. Hamilton iSWAP, PreciseFlex. - pick_up/drop/move with direction parameter + ├── FixedAxisGripperArm + │ Fixed grip axis (x or y). E.g. Hamilton core grippers. + │ pick_up/drop/move at location + │ + ├── OrientableGripperArm + │ Arm with rotation. E.g. Hamilton iSWAP, PreciseFlex. + │ pick_up/drop/move with direction parameter + │ + └── ArticulatedGripperArm + Arm with full 3D rotation. E.g. UFACTORY xArm 6. + pick_up/drop/move with Rotation parameter ``` Frontend mirrors backend hierarchy exactly. @@ -50,10 +56,10 @@ Joint-space methods are backend-only (robot-specific), accessed via `arm.backend ``` _BaseArmBackend(CapabilityBackend) - │ halt(), park(), get_gripper_location() + │ halt(), park(), request_gripper_pose() │ ├── GripperArmBackend - │ open/close_gripper, is_gripper_closed + │ move_gripper, is_gripper_closed, min/max_gripper_width │ pick_up/drop/move at location (no rotation) │ ├── OrientableGripperArmBackend @@ -72,9 +78,9 @@ _BaseArmBackend(CapabilityBackend) | Device | Driver | Arm Backend | Frontend | |--------|--------|-------------|----------| -| Hamilton STAR (iSWAP) | STARDriver (shared) | `iSWAP(OrientableGripperArmBackend)` | `OrientableArm` | -| Hamilton STAR (core) | STARDriver (shared) | `CoreGripper(GripperArmBackend)` | `Arm` | -| PreciseFlex 400 | `PreciseFlexDriver` | `PreciseFlexArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive)` | `OrientableArm` | +| Hamilton STAR (iSWAP) | STARDriver (shared) | `iSWAP(OrientableGripperArmBackend)` | `OrientableGripperArm` | +| Hamilton STAR (core) | STARDriver (shared) | `CoreGripper(GripperArmBackend)` | `FixedAxisGripperArm` | +| PreciseFlex 400 | `PreciseFlexDriver` | `PreciseFlexArmBackend(OrientableGripperArmBackend, HasJoints, CanFreedrive)` | `OrientableGripperArm` | ## Usage @@ -85,8 +91,8 @@ class STAR(Device): def __init__(self, ...): driver = STARDriver(...) super().__init__(driver=driver) - self.iswap = OrientableArm(backend=iSWAP(driver), reference_resource=deck) - self.core_gripper = GripperArm(backend=CoreGripper(driver), reference_resource=deck) + self.iswap = OrientableGripperArm(backend=iSWAP(driver), reference_resource=deck) + self.core_gripper = FixedAxisGripperArm(backend=CoreGripper(driver), reference_resource=deck) self._capabilities = [self.iswap, self.core_gripper] ``` @@ -105,7 +111,7 @@ class PreciseFlex400(Device): gripper_length=gripper_length, gripper_z_offset=gripper_z_offset, ) - self.arm = OrientableArm(backend=backend, reference_resource=self.reference) + self.arm = OrientableGripperArm(backend=backend, reference_resource=self.reference) self._capabilities = [self.arm] # Joint methods accessed via backend (robot-specific): diff --git a/pylabrobot/capabilities/arms/arm.py b/pylabrobot/capabilities/arms/arm.py index 9d644a8e229..a5f8c903868 100644 --- a/pylabrobot/capabilities/arms/arm.py +++ b/pylabrobot/capabilities/arms/arm.py @@ -1,8 +1,9 @@ import logging +from abc import abstractmethod from dataclasses import dataclass -from typing import List, Literal, Optional, Tuple, Union +from typing import List, Literal, Optional, Tuple, Union, cast -from pylabrobot.capabilities.arms.backend import GripperArmBackend, _BaseArmBackend +from pylabrobot.capabilities.arms.backend import CanGrip, GripperArmBackend, _BaseArmBackend from pylabrobot.capabilities.arms.standard import CartesianPose, GripperDirection from pylabrobot.capabilities.capability import BackendParams, Capability from pylabrobot.legacy.tilting.tilter import Tilter @@ -297,7 +298,112 @@ def _finalize_drop( class GripperArm(_BaseArm): - """A gripper arm without rotation capability. E.g. Hamilton core grippers.""" + """Abstract base for arms with a gripper. + + Owns the gripper-width controls (:meth:`move_gripper`, :meth:`open_gripper`, + :meth:`close_gripper`, :meth:`is_gripper_closed`) shared by all gripper + arms. + + Do not instantiate this class directly. Pick/drop/move signatures differ + between fixed-axis, orientable, and articulated arms, so subclass one of: + + - :class:`FixedAxisGripperArm` — grips along a single deck-fixed axis + (e.g. Hamilton core grippers). + - :class:`OrientableGripperArm` — grip direction is a yaw angle + (e.g. Hamilton iSWAP, PreciseFlex). + - :class:`ArticulatedGripperArm` — full 3D rotation + (e.g. UFACTORY xArm 6). + """ + + async def move_gripper( + self, + width: float, + force_sensing: bool = False, + backend_params: Optional[BackendParams] = None, + ) -> None: + """Move the gripper jaws. + + Args: + width: Target jaw width in mm. Must lie within the backend's advertised + ``[min_gripper_width, max_gripper_width]`` range; either bound declared + as ``None`` is treated as unbounded on that side. + force_sensing: If ``True``, close toward ``width`` with force feedback and + stop on contact (final width may be larger than ``width``). If + ``False``, drive to exactly ``width`` without feedback. + backend_params: Optional backend-specific parameters. + + Raises: + ValueError: If ``width`` falls outside the advertised range. + """ + backend = cast(CanGrip, self.backend) + min_w = backend.min_gripper_width + max_w = backend.max_gripper_width + if min_w is not None and width < min_w: + raise ValueError( + f"width {width} mm is below {type(self.backend).__name__}.min_gripper_width ({min_w} mm)." + ) + if max_w is not None and width > max_w: + raise ValueError( + f"width {width} mm is above {type(self.backend).__name__}.max_gripper_width ({max_w} mm)." + ) + return await backend.move_gripper( + width=width, force_sensing=force_sensing, backend_params=backend_params + ) + + async def open_gripper(self, backend_params: Optional[BackendParams] = None) -> None: + """Drive the gripper to its full open width. + + Calls :meth:`move_gripper` with ``width=max_gripper_width`` and + ``force_sensing=False``. Raises :class:`NotImplementedError` if the backend + does not declare ``max_gripper_width``. + """ + width = self.backend.max_gripper_width + if width is None: + raise NotImplementedError( + f"{type(self.backend).__name__} does not support open_gripper (max_gripper_width is None)." + ) + return await self.backend.move_gripper( + width=width, force_sensing=False, backend_params=backend_params + ) + + async def close_gripper(self, backend_params: Optional[BackendParams] = None) -> None: + """Drive the gripper closed with force feedback. + + Calls :meth:`move_gripper` with ``width=min_gripper_width`` and + ``force_sensing=True``. Raises :class:`NotImplementedError` if the backend + does not declare ``min_gripper_width``. + """ + width = self.backend.min_gripper_width + if width is None: + raise NotImplementedError( + f"{type(self.backend).__name__} does not support close_gripper (min_gripper_width is None)." + ) + return await self.backend.move_gripper( + width=width, force_sensing=True, backend_params=backend_params + ) + + async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool: + """Return whether the gripper is currently in the closed state.""" + return await self.backend.is_gripper_closed(backend_params=backend_params) + + @abstractmethod + async def pick_up_resource(self, resource: Resource, *args, **kwargs) -> None: + """Pick up a resource. Concrete subclasses define the full signature.""" + + @abstractmethod + async def drop_resource(self, destination, *args, **kwargs) -> None: + """Drop the held resource at a destination. Subclasses define the signature.""" + + @abstractmethod + async def move_resource(self, resource: Resource, to, *args, **kwargs) -> None: + """Pick up, optionally pass through waypoints, and drop. Subclass-defined signature.""" + + +class FixedAxisGripperArm(GripperArm): + """A gripper arm without rotation capability. E.g. Hamilton core grippers. + + Grips along a single deck-fixed axis chosen at construction time. + """ def __init__( self, @@ -309,23 +415,6 @@ def __init__( self.backend: GripperArmBackend = backend self._grip_axis = grip_axis - async def open_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None - ) -> None: - return await self.backend.open_gripper( - gripper_width=gripper_width, backend_params=backend_params - ) - - async def close_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None - ) -> None: - return await self.backend.close_gripper( - gripper_width=gripper_width, backend_params=backend_params - ) - - async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool: - return await self.backend.is_gripper_closed(backend_params=backend_params) - def _resource_width(self, resource: Resource) -> float: if self._grip_axis == "y": return resource.get_absolute_size_y() diff --git a/pylabrobot/capabilities/arms/arm_tests.py b/pylabrobot/capabilities/arms/arm_tests.py index b7a9aa622a5..ccc6b773a07 100644 --- a/pylabrobot/capabilities/arms/arm_tests.py +++ b/pylabrobot/capabilities/arms/arm_tests.py @@ -1,12 +1,12 @@ import unittest from unittest.mock import AsyncMock, MagicMock -from pylabrobot.capabilities.arms.arm import GripperArm +from pylabrobot.capabilities.arms.arm import FixedAxisGripperArm, GripperArm from pylabrobot.capabilities.arms.backend import ( GripperArmBackend, OrientableGripperArmBackend, ) -from pylabrobot.capabilities.arms.orientable_arm import OrientableArm +from pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm from pylabrobot.resources import Coordinate, Resource, ResourceHolder @@ -48,16 +48,17 @@ async def asyncSetUp(self): "pick_up_at_location", "drop_at_location", "move_to_location", - "open_gripper", - "close_gripper", + "move_gripper", "is_gripper_closed", "halt", "park", ]: setattr(self.mock_backend, method_name, AsyncMock()) + self.mock_backend.min_gripper_width = 50.0 + self.mock_backend.max_gripper_width = 145.0 self.deck, self.site_a, self.site_b, self.plate = _make_deck_with_sites() - self.arm = GripperArm(backend=self.mock_backend, reference_resource=self.deck) + self.arm = FixedAxisGripperArm(backend=self.mock_backend, reference_resource=self.deck) async def test_pick_up_resource(self): # plate at site_a(100,100,50) + child_loc(5,5,0), center_xy=(60,40), size_z=10 @@ -99,9 +100,54 @@ async def test_move_to_location(self): location=location, backend_params=None ) + async def test_move_gripper(self): + await self.arm.move_gripper(width=80.0, force_sensing=True) + self.mock_backend.move_gripper.assert_called_once_with( + width=80.0, force_sensing=True, backend_params=None + ) + + async def test_move_gripper_below_min_raises(self): + with self.assertRaises(ValueError): + await self.arm.move_gripper(width=10.0) + self.mock_backend.move_gripper.assert_not_called() + + async def test_move_gripper_above_max_raises(self): + with self.assertRaises(ValueError): + await self.arm.move_gripper(width=200.0) + self.mock_backend.move_gripper.assert_not_called() + + async def test_move_gripper_at_min_and_max_allowed(self): + await self.arm.move_gripper(width=50.0) + await self.arm.move_gripper(width=145.0) + self.assertEqual(self.mock_backend.move_gripper.call_count, 2) + + async def test_move_gripper_skips_check_when_bound_is_none(self): + self.mock_backend.max_gripper_width = None + # No upper bound declared → 999 mm passes through. + await self.arm.move_gripper(width=999.0) + self.mock_backend.move_gripper.assert_called_once() + async def test_open_gripper(self): - await self.arm.open_gripper(gripper_width=50.0) - self.mock_backend.open_gripper.assert_called_once_with(gripper_width=50.0, backend_params=None) + await self.arm.open_gripper() + self.mock_backend.move_gripper.assert_called_once_with( + width=145.0, force_sensing=False, backend_params=None + ) + + async def test_close_gripper(self): + await self.arm.close_gripper() + self.mock_backend.move_gripper.assert_called_once_with( + width=50.0, force_sensing=True, backend_params=None + ) + + async def test_open_gripper_unsupported(self): + self.mock_backend.max_gripper_width = None + with self.assertRaises(NotImplementedError): + await self.arm.open_gripper() + + async def test_close_gripper_unsupported(self): + self.mock_backend.min_gripper_width = None + with self.assertRaises(NotImplementedError): + await self.arm.close_gripper() async def test_halt(self): await self.arm.halt() @@ -113,7 +159,9 @@ async def test_park(self): async def test_grip_axis_y(self): """With grip_axis='y', resource_width should be the Y size.""" - arm_y = GripperArm(backend=self.mock_backend, reference_resource=self.deck, grip_axis="y") + arm_y = FixedAxisGripperArm( + backend=self.mock_backend, reference_resource=self.deck, grip_axis="y" + ) await arm_y.pick_up_resource(self.plate, pickup_distance_from_bottom=8) call = self.mock_backend.pick_up_at_location.call_args # plate size_y=80 @@ -121,7 +169,7 @@ async def test_grip_axis_y(self): class TestOrientableArm(unittest.IsolatedAsyncioTestCase): - """Test OrientableArm coordinate computation with fictional resources.""" + """Test OrientableGripperArm coordinate computation with fictional resources.""" async def asyncSetUp(self): self.mock_backend = MagicMock(spec=OrientableGripperArmBackend) @@ -133,12 +181,10 @@ async def asyncSetUp(self): setattr(self.mock_backend, method_name, AsyncMock()) self.deck, self.site_a, self.site_b, self.plate = _make_deck_with_sites() - self.arm = OrientableArm(backend=self.mock_backend, reference_resource=self.deck) + self.arm = OrientableGripperArm(backend=self.mock_backend, reference_resource=self.deck) async def test_pick_up_front(self): - await self.arm.pick_up_resource( - self.plate, pickup_distance_from_bottom=8, direction="front" - ) + await self.arm.pick_up_resource(self.plate, pickup_distance_from_bottom=8, direction="front") call = self.mock_backend.pick_up_at_location.call_args _assert_location(self, call, 165, 145, 58) # "front" = -Y in deck frame = 270° under the +X-is-zero convention. @@ -147,9 +193,7 @@ async def test_pick_up_front(self): self.assertAlmostEqual(call.kwargs["resource_width"], 120) async def test_pick_up_right(self): - await self.arm.pick_up_resource( - self.plate, pickup_distance_from_bottom=8, direction="right" - ) + await self.arm.pick_up_resource(self.plate, pickup_distance_from_bottom=8, direction="right") call = self.mock_backend.pick_up_at_location.call_args # "right" = +X = 0° under the +X-is-zero convention. self.assertAlmostEqual(call.kwargs["direction"], 0.0) @@ -173,10 +217,16 @@ async def test_move_to_location(self): async def test_move_plate(self): """Pick from site_a, drop at site_b.""" - await self.arm.pick_up_resource( - self.plate, pickup_distance_from_bottom=8, direction="front" - ) + await self.arm.pick_up_resource(self.plate, pickup_distance_from_bottom=8, direction="front") await self.arm.drop_resource(self.site_b, direction="front") drop_call = self.mock_backend.drop_at_location.call_args _assert_location(self, drop_call, 160, 340, 58) self.assertEqual(self.plate.parent.name, "site_b") + + +class TestGripperArmAbstract(unittest.TestCase): + def test_cannot_instantiate_abstract_base(self): + with self.assertRaises(TypeError): + GripperArm( + backend=MagicMock(spec=GripperArmBackend), reference_resource=Resource("x", 1, 1, 1) + ) # type: ignore[abstract] diff --git a/pylabrobot/capabilities/arms/articulated_arm.py b/pylabrobot/capabilities/arms/articulated_arm.py index 752c31bb8eb..863c9d0091f 100644 --- a/pylabrobot/capabilities/arms/articulated_arm.py +++ b/pylabrobot/capabilities/arms/articulated_arm.py @@ -1,32 +1,19 @@ from typing import List, Optional, Union -from pylabrobot.capabilities.arms.arm import _BaseArm, _PickedUpState +from pylabrobot.capabilities.arms.arm import GripperArm, _PickedUpState from pylabrobot.capabilities.arms.backend import ArticulatedGripperArmBackend from pylabrobot.capabilities.capability import BackendParams from pylabrobot.resources import Coordinate, Resource, ResourceHolder, ResourceStack from pylabrobot.resources.rotation import Rotation -class ArticulatedArm(_BaseArm): - """An arm with full 3D rotation capability. E.g. a 6-axis robot arm.""" +class ArticulatedGripperArm(GripperArm): + """A gripper arm with full 3D rotation capability. E.g. a 6-axis robot arm.""" def __init__(self, backend: ArticulatedGripperArmBackend, reference_resource: Resource): - super().__init__(backend=backend, reference_resource=reference_resource) + super().__init__(backend=backend, reference_resource=reference_resource) # type: ignore[arg-type] self.backend: ArticulatedGripperArmBackend = backend # type: ignore[assignment] - async def open_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None - ) -> None: - await self.backend.open_gripper(gripper_width=gripper_width, backend_params=backend_params) - - async def close_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None - ) -> None: - await self.backend.close_gripper(gripper_width=gripper_width, backend_params=backend_params) - - async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool: - return await self.backend.is_gripper_closed(backend_params=backend_params) - @staticmethod def _resource_width_for_rotation(resource: Resource, rotation: Rotation) -> float: if rotation.z % 180 == 0: diff --git a/pylabrobot/capabilities/arms/backend.py b/pylabrobot/capabilities/arms/backend.py index c90bd18f363..bc64c8e5ed1 100644 --- a/pylabrobot/capabilities/arms/backend.py +++ b/pylabrobot/capabilities/arms/backend.py @@ -14,8 +14,8 @@ # - is_holding_resource # CanGrip -# - open_gripper -# - close_gripper +# - min_gripper_width, max_gripper_width +# - move_gripper(width, force_sensing) # - is_gripper_closed # CanSuction @@ -94,19 +94,55 @@ async def request_joint_position( class CanGrip(metaclass=ABCMeta): - """Mixin for arms that have a gripper.""" + """Mixin for arms that have a gripper. + All widths in this interface are in **millimeters**. Backends that drive + their hardware in other units (encoder counts, SDK steps, etc.) must convert + from mm internally. + + Gripper actuation has two fundamental modes, exposed through a single + :meth:`move_gripper` call: + + - **Positional** (``force_sensing=False``): move the jaws to ``width`` mm + without force feedback. The jaws reach exactly that width. + - **Grip** (``force_sensing=True``): close toward ``width`` mm but stop on + contact. The final width may be larger than the target. + + Backends must also declare the hardware limits of the gripper via + :attr:`min_gripper_width` and :attr:`max_gripper_width`. + """ + + @property @abstractmethod - async def open_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None - ) -> None: - """Open the gripper to the specified width.""" + def min_gripper_width(self) -> Optional[float]: + """Smallest jaw width in mm, or ``None`` if the gripper cannot be commanded + closed via :meth:`move_gripper` (e.g. closing happens only as part of + pick-up).""" + + @property + @abstractmethod + def max_gripper_width(self) -> Optional[float]: + """Largest jaw width in mm, or ``None`` if the gripper cannot be commanded + open via :meth:`move_gripper`.""" @abstractmethod - async def close_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None + async def move_gripper( + self, + width: float, + force_sensing: bool = False, + backend_params: Optional[BackendParams] = None, ) -> None: - """Close the gripper to the specified width.""" + """Move the gripper jaws to ``width`` mm. + + Args: + width: Target jaw width in mm. + force_sensing: If ``True``, close with force feedback and stop on contact. + If ``False``, drive the jaws to ``width`` without sensing. + + Backends that do not implement force feedback should either raise + :class:`NotImplementedError` when ``force_sensing=True`` is requested or + document that the flag has no effect on their hardware. + """ @abstractmethod async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool: diff --git a/pylabrobot/capabilities/arms/orientable_arm.py b/pylabrobot/capabilities/arms/orientable_arm.py index 831459ba6f1..0b0888b8d74 100644 --- a/pylabrobot/capabilities/arms/orientable_arm.py +++ b/pylabrobot/capabilities/arms/orientable_arm.py @@ -1,6 +1,6 @@ from typing import Dict, List, Optional, Union -from pylabrobot.capabilities.arms.arm import GripperOrientation, _BaseArm, _PickedUpState +from pylabrobot.capabilities.arms.arm import GripperArm, GripperOrientation, _PickedUpState from pylabrobot.capabilities.arms.backend import OrientableGripperArmBackend from pylabrobot.capabilities.arms.standard import ( _GRIPPER_DIRECTION_VALUES, @@ -34,26 +34,13 @@ def _resolve_direction(direction: GripperOrientation) -> float: return direction -class OrientableArm(_BaseArm): - """An arm with rotation capability. E.g. Hamilton iSWAP.""" +class OrientableGripperArm(GripperArm): + """A gripper arm with rotation capability. E.g. Hamilton iSWAP.""" def __init__(self, backend: OrientableGripperArmBackend, reference_resource: Resource): - super().__init__(backend=backend, reference_resource=reference_resource) + super().__init__(backend=backend, reference_resource=reference_resource) # type: ignore[arg-type] self.backend: OrientableGripperArmBackend = backend # type: ignore[assignment] - async def open_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None - ) -> None: - await self.backend.open_gripper(gripper_width=gripper_width, backend_params=backend_params) - - async def close_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None - ) -> None: - await self.backend.close_gripper(gripper_width=gripper_width, backend_params=backend_params) - - async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool: - return await self.backend.is_gripper_closed(backend_params=backend_params) - @staticmethod def _resource_width_for_direction(resource: Resource, direction: float) -> float: # Front-finger axis is `direction`; jaws come together perpendicular to @@ -98,10 +85,7 @@ async def pick_up_resource( ) dir_degrees = _resolve_direction(direction) resource_width = self._resource_width_for_direction(resource, dir_degrees) - # if gripper: await self.pick_up_at_location(location, resource_width, dir_degrees, backend_params) - # if suction: - # TODO: self._picked_up = _PickedUpState( resource=resource, offset=offset, diff --git a/pylabrobot/hamilton/liquid_handlers/star/core.py b/pylabrobot/hamilton/liquid_handlers/star/core.py index 52a926776ce..39d9eba4df6 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/core.py +++ b/pylabrobot/hamilton/liquid_handlers/star/core.py @@ -211,18 +211,30 @@ async def move_to_location( th=f"{round(backend_params.minimum_traverse_height * 10):04}", ) - async def open_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None - ) -> None: - """Open the CoRe gripper.""" - await self.driver.send_command(module="C0", command="ZO") + # ``min`` is the fully-closed jaw width. ``max`` is None because the CoRe + # gripper has no commandable "open" width: the firmware ZO command opens the + # jaws fully and ``width`` is therefore ignored. + min_gripper_width: float = 9.0 + max_gripper_width: Optional[float] = None - async def close_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None + async def move_gripper( + self, + width: float, + force_sensing: bool = False, + backend_params: Optional[BackendParams] = None, ) -> None: - raise NotImplementedError( - "CoreGripper does not support close_gripper directly. Use pick_up_at_location instead." - ) + """Open the CoRe gripper. + + Sends the firmware ZO command, which opens the jaws fully; ``width`` has + no effect. Force-sensing closes happen as part of :meth:`pick_up_at_location` + and are not exposed here. + """ + if force_sensing: + raise NotImplementedError( + "CoreGripper does not support force-sensing closes directly. " + "Use pick_up_at_location instead." + ) + await self.driver.send_command(module="C0", command="ZO") async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool: raise NotImplementedError("CoreGripper does not support is_gripper_closed") diff --git a/pylabrobot/hamilton/liquid_handlers/star/iswap.py b/pylabrobot/hamilton/liquid_handlers/star/iswap.py index 517d4c314f2..3a70383a537 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/iswap.py +++ b/pylabrobot/hamilton/liquid_handlers/star/iswap.py @@ -588,23 +588,14 @@ async def park(self, backend_params: Optional[BackendParams] = None) -> None: ) self._parked = True - async def open_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None - ) -> None: - """Open the iSWAP gripper. - - Args: - gripper_width: Open position [mm]. - backend_params: Unused, reserved for future use. - """ - if not 0 <= gripper_width <= 999.9: - raise ValueError("gripper_width must be between 0 and 999.9") - - await self.driver.send_command(module="C0", command="GF", go=f"{round(gripper_width * 10):04}") + # Physical jaw range for the iSWAP gripper. The firmware accepts up to 999.9 mm, + # but the mechanism cannot actually reach those extremes. + min_gripper_width: float = 50.0 + max_gripper_width: float = 145.0 @dataclass - class CloseGripperParams(BackendParams): - """Parameters for closing the iSWAP gripper. + class GripParams(BackendParams): + """Parameters for a force-sensing close on the iSWAP gripper. The gripper should be at position plate_width + plate_width_tolerance + 2.0 mm before sending this command. @@ -619,22 +610,37 @@ class CloseGripperParams(BackendParams): grip_strength: int = 5 plate_width_tolerance: float = 2.0 - async def close_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None + async def move_gripper( + self, + width: float, + force_sensing: bool = False, + backend_params: Optional[BackendParams] = None, ) -> None: - """Close the iSWAP gripper. + """Move the iSWAP gripper jaws. Args: - gripper_width: Plate width [mm]. - backend_params: iSWAP.CloseGripperParams with grip_strength and plate_width_tolerance. + width: Target jaw width [mm]. Must be between 0 and 999.9. + force_sensing: If True, close with force feedback (C0 GC) and stop on + contact. If False, drive the jaws to ``width`` without sensing (C0 GF). + backend_params: iSWAP.GripParams. Only valid when ``force_sensing=True``; + passing it with ``force_sensing=False`` raises ``ValueError``. """ - if not isinstance(backend_params, iSWAPBackend.CloseGripperParams): - backend_params = iSWAPBackend.CloseGripperParams() - + if not 0 <= width <= 999.9: + raise ValueError("width must be between 0 and 999.9") + + if not force_sensing: + if backend_params is not None: + raise ValueError( + "GripParams is only valid with force_sensing=True. " + "Drop backend_params or set force_sensing=True." + ) + await self.driver.send_command(module="C0", command="GF", go=f"{round(width * 10):04}") + return + + if not isinstance(backend_params, iSWAPBackend.GripParams): + backend_params = iSWAPBackend.GripParams() if not 0 <= backend_params.grip_strength <= 9: raise ValueError("grip_strength must be between 0 and 9") - if not 0 <= gripper_width <= 999.9: - raise ValueError("gripper_width must be between 0 and 999.9") if not 0.5 <= backend_params.plate_width_tolerance <= 9.9: raise ValueError("plate_width_tolerance must be between 0.5 and 9.9") @@ -642,7 +648,7 @@ async def close_gripper( module="C0", command="GC", gw=backend_params.grip_strength, - gb=f"{round(gripper_width * 10):04}", + gb=f"{round(width * 10):04}", gt=f"{round(backend_params.plate_width_tolerance * 10):02}", ) diff --git a/pylabrobot/hamilton/liquid_handlers/star/misc/architecture.md b/pylabrobot/hamilton/liquid_handlers/star/misc/architecture.md index 375a8cf4405..ee01d1fbd2e 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/misc/architecture.md +++ b/pylabrobot/hamilton/liquid_handlers/star/misc/architecture.md @@ -30,7 +30,7 @@ STAR (Device) — only exposes Capabilities │ pip: PIP ──────────► pip backend (above) head96: Head96 ────► head96 backend (above) - iswap: OrientableArm ► iswap backend (above) + iswap: OrientableGripperArm ► iswap backend (above) ``` The STAR device only exposes Capabilities (PIP, Head96, iSWAP). Subsystems (autoload, x-arms, cover, wash station) and generic driver methods live on `star._driver`. @@ -231,7 +231,7 @@ await star.setup() ├─ Wire capability frontends to backends (on STAR device) │ self.pip = PIP(backend=driver.pip) │ self.head96 = Head96(backend=driver.head96) # if installed - │ self.iswap = OrientableArm(backend=driver.iswap) # if installed + │ self.iswap = OrientableGripperArm(backend=driver.iswap) # if installed │ └─ Call _on_setup() for each Capability ``` diff --git a/pylabrobot/hamilton/liquid_handlers/star/misc/core_test.ipynb b/pylabrobot/hamilton/liquid_handlers/star/misc/core_test.ipynb index 5140927f426..d62ce6b96d9 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/misc/core_test.ipynb +++ b/pylabrobot/hamilton/liquid_handlers/star/misc/core_test.ipynb @@ -27,7 +27,7 @@ "metadata": {}, "outputs": [], "source": [ - "from pylabrobot.arms.arm import Arm\n", + "from pylabrobot.capabilities.arms.arm import Arm\n", "from pylabrobot.hamilton.liquid_handlers.star.core import CoreGripper\n", "from pylabrobot.legacy.liquid_handling.backends.hamilton.STAR_backend import STARBackend\n", "from pylabrobot.resources.corning.plates import Cor_96_wellplate_360ul_Fb\n", diff --git a/pylabrobot/hamilton/liquid_handlers/star/misc/demo.ipynb b/pylabrobot/hamilton/liquid_handlers/star/misc/demo.ipynb index f8a4ee0c43e..58b7b276788 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/misc/demo.ipynb +++ b/pylabrobot/hamilton/liquid_handlers/star/misc/demo.ipynb @@ -363,7 +363,7 @@ "text/plain": [ "[,\n", " ,\n", - " ]" + " ]" ] }, "execution_count": 24, @@ -447,7 +447,7 @@ "source": [ "### iSWAP: plate gripper arm\n", "\n", - "The iSWAP backend is already implemented. The `OrientableArm` frontend provides `pick_up_resource` / `drop_resource` / `move_resource`." + "The iSWAP backend is already implemented. The `OrientableGripperArm` frontend provides `pick_up_resource` / `drop_resource` / `move_resource`." ] }, { @@ -484,7 +484,7 @@ "id": "6e2ed94e", "metadata": {}, "source": [ - "Moving using a function that takes any `OrientableArm`:" + "Moving using a function that takes any `OrientableGripperArm`:" ] }, { @@ -493,7 +493,7 @@ "id": "d5b972f7", "metadata": {}, "outputs": [], - "source": "from pylabrobot.arms.orientable_arm import OrientableArm\n\n\nasync def move_resource(resource, destination, arm: OrientableArm):\n await arm.move_resource(resource, destination, pickup_distance_from_bottom=13.2)\n\n\nawait move_resource(plate2, plt_car2[0], arm=star.iswap)\n# await move_resource(plate2, plt_car2[0], arm=pf400.arm)" + "source": "from pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm\n\n\nasync def move_resource(resource, destination, arm: OrientableGripperArm):\n await arm.move_resource(resource, destination, pickup_distance_from_bottom=13.2)\n\n\nawait move_resource(plate2, plt_car2[0], arm=star.iswap)\n# await move_resource(plate2, plt_car2[0], arm=pf400.arm)" }, { "cell_type": "markdown", @@ -515,7 +515,7 @@ "text": [ "C0PGid0015th2800\n", "C0ZTid0016xs13375xd0ya1250yb1070pa07pb08tp2350tz2250th2800tt14\n", - "arm type: \n", + "arm type: \n", "C0ZPid0017xs04829xd0yj1142yv0050zj1841zy0500yo0885yg0825yw15th2800te2800\n", "C0ZRid0018xs04829xd0yj3062zj1841zi000zy0500yo0885th2800te2800\n", "C0ZSid0019xs13375xd0ya1250yb1070tp2150tz2050th2800te2800\n" diff --git a/pylabrobot/hamilton/liquid_handlers/star/misc/iswap_test.ipynb b/pylabrobot/hamilton/liquid_handlers/star/misc/iswap_test.ipynb index 67a45fdd1c6..77b082c5787 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/misc/iswap_test.ipynb +++ b/pylabrobot/hamilton/liquid_handlers/star/misc/iswap_test.ipynb @@ -4,9 +4,9 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "# iSWAP + OrientableArm: Simple Resource Move Test\n", + "# iSWAP + OrientableGripperArm: Simple Resource Move Test\n", "\n", - "Tests the `iSWAP` backend through `OrientableArm` with real PLR resources and the real STAR firmware interface." + "Tests the `iSWAP` backend through `OrientableGripperArm` with real PLR resources and the real STAR firmware interface." ] }, { @@ -24,7 +24,7 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": "from pylabrobot.arms.orientable_arm import OrientableArm\nfrom pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAP\nfrom pylabrobot.legacy.liquid_handling.backends.hamilton.STAR_backend import STARBackend\nfrom pylabrobot.resources.corning.plates import Cor_96_wellplate_360ul_Fb\nfrom pylabrobot.resources.hamilton.hamilton_decks import STARLetDeck\nfrom pylabrobot.resources.hamilton.plate_carriers import PLT_CAR_L5AC_A00" + "source": "from pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm\nfrom pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAP\nfrom pylabrobot.legacy.liquid_handling.backends.hamilton.STAR_backend import STARBackend\nfrom pylabrobot.resources.corning.plates import Cor_96_wellplate_360ul_Fb\nfrom pylabrobot.resources.hamilton.hamilton_decks import STARLetDeck\nfrom pylabrobot.resources.hamilton.plate_carriers import PLT_CAR_L5AC_A00" }, { "cell_type": "markdown", @@ -132,7 +132,7 @@ "cmd H0RFid0025\n", "cmd H0QUid0026\n", "cmd H0QGid0027\n", - "OrientableArm ready, iSWAP parked: False\n" + "OrientableGripperArm ready, iSWAP parked: False\n" ] } ], @@ -143,8 +143,8 @@ "\n", "iswap_backend = iSWAP(interface=star_backend)\n", "\n", - "arm = OrientableArm(backend=iswap_backend, reference_resource=deck)\n", - "print(f\"OrientableArm ready, iSWAP parked: {iswap_backend.parked}\")" + "arm = OrientableGripperArm(backend=iswap_backend, reference_resource=deck)\n", + "print(f\"OrientableGripperArm ready, iSWAP parked: {iswap_backend.parked}\")" ] }, { diff --git a/pylabrobot/hamilton/liquid_handlers/star/star.py b/pylabrobot/hamilton/liquid_handlers/star/star.py index f7c8de6b92d..ba21452b61f 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/star.py +++ b/pylabrobot/hamilton/liquid_handlers/star/star.py @@ -4,8 +4,8 @@ from contextlib import asynccontextmanager from typing import AsyncIterator, Optional -from pylabrobot.capabilities.arms.arm import GripperArm -from pylabrobot.capabilities.arms.orientable_arm import OrientableArm +from pylabrobot.capabilities.arms.arm import FixedAxisGripperArm +from pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm from pylabrobot.capabilities.capability import BackendParams from pylabrobot.capabilities.liquid_handling.head96 import Head96 from pylabrobot.capabilities.liquid_handling.pip import PIP @@ -33,7 +33,7 @@ def __init__(self, deck: HamiltonDeck, chatterbox: bool = False): self.deck = deck self.pip: PIP # set in setup() self.head96: Optional[Head96] = None # set in setup() if installed - self.iswap: Optional[OrientableArm] = None # set in setup() if installed + self.iswap: Optional[OrientableGripperArm] = None # set in setup() if installed async def setup(self, backend_params: Optional[BackendParams] = None): await self.driver.setup(backend_params=backend_params) @@ -49,7 +49,7 @@ async def setup(self, backend_params: Optional[BackendParams] = None): # iSWAP only if installed. if self.driver.iswap is not None: - self.iswap = OrientableArm(backend=self.driver.iswap, reference_resource=self.deck) + self.iswap = OrientableGripperArm(backend=self.driver.iswap, reference_resource=self.deck) self._capabilities.append(self.iswap) # Matches legacy: autoload runs in parallel with arm modules. @@ -85,7 +85,7 @@ async def core_grippers( front_offset: Coordinate = Coordinate.zero(), back_offset: Coordinate = Coordinate.zero(), traversal_height: float = 280.0, - ) -> AsyncIterator[GripperArm]: + ) -> AsyncIterator[FixedAxisGripperArm]: """Context manager that picks up CoRe gripper tools on enter and returns them on exit. Usage:: @@ -121,7 +121,7 @@ async def core_grippers( ) backend = CoreGripper(driver=self.driver) - arm = GripperArm(backend=backend, reference_resource=self.deck, grip_axis="y") + arm = FixedAxisGripperArm(backend=backend, reference_resource=self.deck, grip_axis="y") try: yield arm diff --git a/pylabrobot/hamilton/liquid_handlers/star/tests/core_tests.py b/pylabrobot/hamilton/liquid_handlers/star/tests/core_tests.py index fd386a489a2..7f9a4b4b525 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/tests/core_tests.py +++ b/pylabrobot/hamilton/liquid_handlers/star/tests/core_tests.py @@ -156,11 +156,15 @@ async def test_move_to_location_custom_params(self): th="3500", ) - async def test_open_gripper(self): - """ZO command.""" - await self.core.open_gripper(gripper_width=0) + async def test_move_gripper_position(self): + """ZO command. CoreGripper ignores ``width`` and just opens fully.""" + await self.core.move_gripper(width=0, force_sensing=False) self.mock_driver.send_command.assert_called_once_with( module="C0", command="ZO", ) + + async def test_move_gripper_force_sensing_not_supported(self): + with self.assertRaises(NotImplementedError): + await self.core.move_gripper(width=80, force_sensing=True) diff --git a/pylabrobot/hamilton/liquid_handlers/star/tests/iswap_tests.py b/pylabrobot/hamilton/liquid_handlers/star/tests/iswap_tests.py index d23572a1def..7b7674f15b2 100644 --- a/pylabrobot/hamilton/liquid_handlers/star/tests/iswap_tests.py +++ b/pylabrobot/hamilton/liquid_handlers/star/tests/iswap_tests.py @@ -143,8 +143,8 @@ async def test_park_custom_height(self): th=2000, ) - async def test_open_gripper(self): - await self.iswap.open_gripper(gripper_width=130.8) + async def test_move_gripper_position(self): + await self.iswap.move_gripper(width=130.8, force_sensing=False) self.mock_driver.send_command.assert_called_once_with( module="C0", @@ -152,10 +152,11 @@ async def test_open_gripper(self): go="1308", ) - async def test_close_gripper(self): - await self.iswap.close_gripper( - gripper_width=86.0, - backend_params=iSWAPBackend.CloseGripperParams(grip_strength=5, plate_width_tolerance=2.0), + async def test_move_gripper_grip(self): + await self.iswap.move_gripper( + width=86.0, + force_sensing=True, + backend_params=iSWAPBackend.GripParams(grip_strength=5, plate_width_tolerance=2.0), ) self.mock_driver.send_command.assert_called_once_with( @@ -166,6 +167,15 @@ async def test_close_gripper(self): gt="20", ) + async def test_move_gripper_rejects_params_without_force_sensing(self): + with self.assertRaises(ValueError): + await self.iswap.move_gripper( + width=130.0, + force_sensing=False, + backend_params=iSWAPBackend.GripParams(grip_strength=5), + ) + self.mock_driver.send_command.assert_not_called() + async def test_is_gripper_closed(self): self.mock_driver.send_command.return_value = {"ph": 1} result = await self.iswap.is_gripper_closed() diff --git a/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py b/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py index d04ca23deda..97dfef322aa 100644 --- a/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py +++ b/pylabrobot/legacy/liquid_handling/backends/hamilton/STAR_backend.py @@ -6929,7 +6929,7 @@ async def iswap_open_gripper(self, open_position: Optional[float] = None): assert 0 <= open_position <= 999.9, "open_position must be between 0 and 999.9" - return await self._iswap.open_gripper(gripper_width=open_position) + return await self._iswap.move_gripper(width=open_position, force_sensing=False) async def iswap_close_gripper( self, @@ -6956,9 +6956,10 @@ async def iswap_close_gripper( from pylabrobot.hamilton.liquid_handlers.star.iswap import iSWAPBackend - return await self._iswap.close_gripper( - gripper_width=plate_width, - backend_params=iSWAPBackend.CloseGripperParams( + return await self._iswap.move_gripper( + width=plate_width, + force_sensing=True, + backend_params=iSWAPBackend.GripParams( grip_strength=grip_strength, plate_width_tolerance=plate_width_tolerance, ), diff --git a/pylabrobot/legacy/liquid_handling/liquid_handler.py b/pylabrobot/legacy/liquid_handling/liquid_handler.py index 64cc7845aea..581138e70d9 100644 --- a/pylabrobot/legacy/liquid_handling/liquid_handler.py +++ b/pylabrobot/legacy/liquid_handling/liquid_handler.py @@ -21,7 +21,7 @@ ) from pylabrobot.capabilities.arms.backend import OrientableGripperArmBackend -from pylabrobot.capabilities.arms.orientable_arm import OrientableArm +from pylabrobot.capabilities.arms.orientable_arm import OrientableGripperArm from pylabrobot.capabilities.arms.standard import GripperDirection as _NewGripperDirection from pylabrobot.capabilities.arms.standard import CartesianPose from pylabrobot.capabilities.liquid_handling.head96 import Head96 @@ -397,7 +397,7 @@ def __init__( # New capability instances — created during setup() self._lh_cap: Optional[PIP] = None self._head96_cap: Optional[Head96] = None - self._arm_cap: Optional[OrientableArm] = None + self._arm_cap: Optional[OrientableGripperArm] = None # Default offset applied to all 96-head operations. Any offset passed to a 96-head method is # added to this value. @@ -451,7 +451,9 @@ async def setup(self, **backend_kwargs): # Create arm capability with adapter backend if self.backend.num_arms > 0: - self._arm_cap = OrientableArm(backend=_ArmAdapter(self.backend), reference_resource=self.deck) + self._arm_cap = OrientableGripperArm( + backend=_ArmAdapter(self.backend), reference_resource=self.deck + ) await self._arm_cap._on_setup() def serialize_state(self) -> Dict[str, Any]: diff --git a/pylabrobot/ufactory/xarm6/backend.py b/pylabrobot/ufactory/xarm6/backend.py index c89d52e5388..92079e6a68c 100644 --- a/pylabrobot/ufactory/xarm6/backend.py +++ b/pylabrobot/ufactory/xarm6/backend.py @@ -123,17 +123,26 @@ async def _set_gripper_units(self, units: int) -> None: # -- CanGrip --------------------------------------------------------------- - async def open_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None - ) -> None: - """Open the bio-gripper to the specified width (mm).""" - await self._set_gripper_units(self._mm_to_gripper_units(gripper_width)) + @property + def min_gripper_width(self) -> float: + return self.gripper_min_mm + + @property + def max_gripper_width(self) -> float: + return self.gripper_max_mm - async def close_gripper( - self, gripper_width: float, backend_params: Optional[BackendParams] = None + async def move_gripper( + self, + width: float, + force_sensing: bool = False, + backend_params: Optional[BackendParams] = None, ) -> None: - """Close the bio-gripper to the specified width (mm).""" - await self._set_gripper_units(self._mm_to_gripper_units(gripper_width)) + """Move the bio-gripper jaws to ``width`` mm. + + The xArm bio-gripper is position-controlled; ``force_sensing`` has no + effect on the SDK call. + """ + await self._set_gripper_units(self._mm_to_gripper_units(width)) async def is_gripper_closed(self, backend_params: Optional[BackendParams] = None) -> bool: """Return True if the gripper width is at or below ``closed_threshold_mm``.""" @@ -209,7 +218,7 @@ async def pick_up_at_location( ) -> None: """Move to ``location`` and close the gripper to ``resource_width``.""" await self.move_to_location(location, rotation, backend_params=backend_params) - await self.close_gripper(resource_width) + await self.move_gripper(width=resource_width, force_sensing=True) async def drop_at_location( self, @@ -262,7 +271,7 @@ async def pick_up_at_joint_position( ) -> None: """Move to the joint target and close the gripper to ``resource_width``.""" await self.move_to_joint_position(position, backend_params=backend_params) - await self.close_gripper(resource_width) + await self.move_gripper(width=resource_width, force_sensing=True) async def drop_at_joint_position( self, @@ -283,8 +292,8 @@ async def start_freedrive_mode( ) -> None: """Enter freedrive (manual teaching) mode. - The xArm SDK only supports freeing all axes at once, so ``free_axes`` is - accepted for interface compatibility but ignored. + The xArm SDK only supports freeing all axes at once, so ``free_axes`` has + no effect. """ await self._driver._call_sdk(self._driver._arm.set_mode, 2, op="set_mode") await self._driver._call_sdk(self._driver._arm.set_state, 0, op="set_state") diff --git a/pylabrobot/ufactory/xarm6/backend_tests.py b/pylabrobot/ufactory/xarm6/backend_tests.py index 058f0b9b6bc..20db6cc0b57 100644 --- a/pylabrobot/ufactory/xarm6/backend_tests.py +++ b/pylabrobot/ufactory/xarm6/backend_tests.py @@ -35,31 +35,33 @@ def _sdk_calls_for(self, func) -> list: # -- Gripper --------------------------------------------------------------- - async def test_open_gripper_mm_to_units(self): - await self.backend.open_gripper(gripper_width=85) + async def test_move_gripper_mm_to_units(self): + # Default range is 71..150 mm mapped to 0..850 units, so 85 mm → ~151 units. + await self.backend.move_gripper(width=85, force_sensing=False) calls = self._sdk_calls_for(self.driver._arm.set_gripper_position) self.assertEqual(len(calls), 1) - self.assertEqual(calls[0].args[1], 850) + self.assertEqual(calls[0].args[1], 151) self.assertEqual(calls[0].kwargs["wait"], True) self.assertEqual(calls[0].kwargs["speed"], 0) - async def test_open_gripper_half(self): - await self.backend.open_gripper(gripper_width=42.5) + async def test_move_gripper_midpoint(self): + # Midpoint of the 71..150 range is 110.5 mm → 425 units. + await self.backend.move_gripper(width=110.5, force_sensing=False) calls = self._sdk_calls_for(self.driver._arm.set_gripper_position) self.assertEqual(calls[0].args[1], 425) - async def test_close_gripper(self): - await self.backend.close_gripper(gripper_width=0) + async def test_move_gripper_force_sensing(self): + await self.backend.move_gripper(width=71, force_sensing=True) calls = self._sdk_calls_for(self.driver._arm.set_gripper_position) self.assertEqual(calls[0].args[1], 0) - async def test_open_gripper_clamped_high(self): - await self.backend.open_gripper(gripper_width=200) + async def test_move_gripper_clamped_high(self): + await self.backend.move_gripper(width=200, force_sensing=False) calls = self._sdk_calls_for(self.driver._arm.set_gripper_position) self.assertEqual(calls[0].args[1], 850) - async def test_open_gripper_clamped_low(self): - await self.backend.open_gripper(gripper_width=-5) + async def test_move_gripper_clamped_low(self): + await self.backend.move_gripper(width=-5, force_sensing=False) calls = self._sdk_calls_for(self.driver._arm.set_gripper_position) self.assertEqual(calls[0].args[1], 0) @@ -150,7 +152,8 @@ async def test_pick_up_at_location_move_then_close(self): grip_calls = self._sdk_calls_for(self.driver._arm.set_gripper_position) self.assertEqual(len(grip_calls), 1) - self.assertEqual(grip_calls[0].args[1], 800) # 80 mm → 800 units + # 80 mm in 71..150 mm range → (80-71)/(150-71) * 850 ≈ 97 units. + self.assertEqual(grip_calls[0].args[1], 97) async def test_drop_at_location_move_then_open_max(self): loc = Coordinate(x=300, y=100, z=50) @@ -195,7 +198,7 @@ async def test_pick_up_at_joint_position(self): self.assertEqual(len(self._sdk_calls_for(self.driver._arm.set_servo_angle)), 1) grip_calls = self._sdk_calls_for(self.driver._arm.set_gripper_position) self.assertEqual(len(grip_calls), 1) - self.assertEqual(grip_calls[0].args[1], 800) + self.assertEqual(grip_calls[0].args[1], 97) async def test_drop_at_joint_position(self): await self.backend.drop_at_joint_position({1: 0, 2: 0}, resource_width=80) @@ -222,8 +225,9 @@ async def test_stop_freedrive_mode(self): # -- Custom configuration -------------------------------------------------- - async def test_custom_mm_per_gripper_unit(self): - backend = XArm6ArmBackend(driver=self.driver, mm_per_gripper_unit=0.2) - await backend.open_gripper(gripper_width=85) + async def test_custom_gripper_range(self): + backend = XArm6ArmBackend(driver=self.driver, gripper_min_mm=50.0, gripper_max_mm=100.0) + await backend.move_gripper(width=75.0, force_sensing=False) calls = self._sdk_calls_for(self.driver._arm.set_gripper_position) + # Midpoint of 50..100 mm range → 0.5 * 850 = 425 units. self.assertEqual(calls[0].args[1], 425) diff --git a/pylabrobot/ufactory/xarm6/xarm6.py b/pylabrobot/ufactory/xarm6/xarm6.py index ce0fa858967..7c16a4d0a65 100644 --- a/pylabrobot/ufactory/xarm6/xarm6.py +++ b/pylabrobot/ufactory/xarm6/xarm6.py @@ -1,4 +1,4 @@ -from pylabrobot.capabilities.arms.articulated_arm import ArticulatedArm +from pylabrobot.capabilities.arms.articulated_arm import ArticulatedGripperArm from pylabrobot.device import Device from pylabrobot.resources import Resource from pylabrobot.ufactory.xarm6.backend import XArm6ArmBackend @@ -9,7 +9,7 @@ class XArm6(Device): """UFACTORY xArm 6 robotic arm with bio-gripper. Composes an :class:`XArm6Driver`, a default :class:`XArm6ArmBackend`, and an - :class:`ArticulatedArm` frontend. The arm capability is exposed as + :class:`ArticulatedGripperArm` frontend. The arm capability is exposed as ``self.arm``; joint-space and freedrive operations live on ``self.arm.backend``. @@ -23,5 +23,5 @@ def __init__(self, driver: XArm6Driver) -> None: self.driver: XArm6Driver = driver backend = XArm6ArmBackend(driver=driver) self.reference = Resource(name="XArm6", size_x=200, size_y=200, size_z=200) - self.arm = ArticulatedArm(backend=backend, reference_resource=self.reference) + self.arm = ArticulatedGripperArm(backend=backend, reference_resource=self.reference) self._capabilities = [self.arm] From dd6434b7d5949f02faeea8065ede1562c1a3b8b5 Mon Sep 17 00:00:00 2001 From: Rick Wierenga Date: Sun, 17 May 2026 16:43:41 -0700 Subject: [PATCH 93/93] KX2: update park pose (SHOULDER=2, Z=750, ELBOW=1, WRIST=356) 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 --- pylabrobot/paa/kx2/arm_backend.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pylabrobot/paa/kx2/arm_backend.py b/pylabrobot/paa/kx2/arm_backend.py index a0f03087717..5bec4e58b6b 100644 --- a/pylabrobot/paa/kx2/arm_backend.py +++ b/pylabrobot/paa/kx2/arm_backend.py @@ -1173,7 +1173,7 @@ async def halt(self, backend_params: Optional[BackendParams] = None) -> None: # Park pose (centered, well inside workspace) and motion caps. _PARK_JOINTS: Dict[Axis, float] = { - Axis.SHOULDER: 0.0, Axis.Z: 300.0, Axis.ELBOW: 250.0, Axis.WRIST: 0.0, + Axis.SHOULDER: 2.0, Axis.Z: 750.0, Axis.ELBOW: 1.0, Axis.WRIST: 356.0, } _PARK_SPEED: float = 80.0 _PARK_ACCEL: float = 400.0