dora_ulite6/config.py

@@ -0,0 +1,130 @@
+"""Everything the node reads from the environment, parsed once at startup.
+
+Parsing happens here rather than where each value is used, so an unusable
+setting stops the node before it joins the dataflow instead of surfacing on the
+first motion — the same reason the ZED node parses its depth mode up front.
+"""
+
+import os
+from dataclasses import dataclass, field
+
+from dora_ulite6.effectors import EndEffector, get_effector
+from dora_ulite6.pose import RobotPose
+
+INIT_MODES = ("none", "home", "pose")
+
+
+def env_str(name: str, default: str) -> str:
+    """Return the variable, or ``default`` when it is unset."""
+    return os.getenv(name, default)
+
+
+def env_float(name: str, default: float) -> float:
+    """Return the variable as a float, naming it if it is not one."""
+    value = os.getenv(name)
+    if value is None or not value.strip():
+        return default
+    try:
+        return float(value)
+    except ValueError:
+        raise ValueError(f"{name} must be a number, not {value!r}") from None
+
+
+def env_int(name: str, default: int) -> int:
+    """Return the variable as an integer, naming it if it is not one."""
+    value = os.getenv(name)
+    if value is None or not value.strip():
+        return default
+    try:
+        return int(value)
+    except ValueError:
+        raise ValueError(f"{name} must be an integer, not {value!r}") from None
+
+
+@dataclass
+class MotionConfig:
+    """Driver-level motion limits and timeouts.
+
+    The arrival tolerances are wide on purpose: whether a motion happened is
+    decided by the controller's error, and these only catch an arm that never
+    left where it stood. Tightening them starts refusing moves that worked.
+    """
+
+    arrival_tolerance_mm: float = 25.0
+    arrival_tolerance_deg: float = 15.0
+    arrival_timeout_s: float = 2.0
+    # How clear of its own bound a joint solution must stay for a pose to be
+    # commanded, and how many points along the straight line are checked.
+    joint_limit_margin_deg: float = 1.0
+    path_samples: int = 4
+    ready_timeout_s: float = 3.0
+    home_timeout_s: float = 20.0
+    tool_settle_s: float = 2.0
+    # The controller's own expiry for one jog command: the arm keeps moving at
+    # the commanded velocity until this runs out, so a sender that stops sending
+    # stops the arm.
+    velocity_command_ttl_s: float = 0.5
+
+    @classmethod
+    def from_env(cls) -> "MotionConfig":
+        """Read the motion limits and timeouts from the environment."""
+        return cls(
+            arrival_tolerance_mm=env_float("ARRIVAL_TOLERANCE_MM", 25.0),
+            arrival_tolerance_deg=env_float("ARRIVAL_TOLERANCE_DEG", 15.0),
+            arrival_timeout_s=env_float("ARRIVAL_TIMEOUT_S", 2.0),
+            joint_limit_margin_deg=env_float("JOINT_LIMIT_MARGIN_DEG", 1.0),
+            path_samples=env_int("PATH_SAMPLES", 4),
+            ready_timeout_s=env_float("READY_TIMEOUT_S", 3.0),
+            home_timeout_s=env_float("HOME_TIMEOUT_S", 20.0),
+            tool_settle_s=env_float("TOOL_SETTLE_S", 2.0),
+            velocity_command_ttl_s=env_float("VELOCITY_COMMAND_TTL_S", 0.5),
+        )
+
+
+@dataclass
+class Config:
+    """The node's settings: where the arm is, what it carries, and how it starts."""
+
+    robot_ip: str = "192.168.1.192"
+    effector: EndEffector = field(default_factory=lambda: get_effector("none"))
+    # The mounted tool's own reach past the flange, plus TOOL_OFFSET_MM.
+    tool_offset_mm: float = 0.0
+    default_speed: float = 30.0
+    default_joint_speed: float = 20.0
+    # How often the control thread re-reads the arm. The dataflow tick only
+    # decides how often the newest reading is published.
+    telemetry_interval_s: float = 0.1
+    # How long to wait before trying an unreachable arm again.
+    reconnect_interval_s: float = 5.0
+    init_mode: str = "none"
+    init_pose: RobotPose = field(default_factory=lambda: RobotPose(300.0, 0.0, 250.0))
+    init_speed: float = 50.0
+    motion: MotionConfig = field(default_factory=MotionConfig)
+
+    @classmethod
+    def from_env(cls) -> "Config":
+        """Read every setting from the environment, refusing one the node cannot use."""
+        effector = get_effector(env_str("TOOL_TYPE", "none").lower())
+        init_mode = env_str("INIT_MODE", "none").lower()
+        if init_mode not in INIT_MODES:
+            raise ValueError(f"INIT_MODE must be one of {', '.join(INIT_MODES)}, not {init_mode!r}")
+        return cls(
+            robot_ip=env_str("ROBOT_IP", "192.168.1.192"),
+            effector=effector,
+            tool_offset_mm=effector.offset_mm + env_float("TOOL_OFFSET_MM", 0.0),
+            default_speed=env_float("DEFAULT_SPEED", 30.0),
+            default_joint_speed=env_float("DEFAULT_JOINT_SPEED", 20.0),
+            telemetry_interval_s=env_int("TELEMETRY_INTERVAL_MS", 100) / 1000.0,
+            reconnect_interval_s=env_float("RECONNECT_INTERVAL_S", 5.0),
+            init_mode=init_mode,
+            init_pose=RobotPose(
+                x=env_float("INIT_X", 300.0),
+                y=env_float("INIT_Y", 0.0),
+                z=env_float("INIT_Z", 250.0),
+                roll=env_float("INIT_ROLL", 180.0),
+                pitch=env_float("INIT_PITCH", 0.0),
+                yaw=env_float("INIT_YAW", 0.0),
+            ),
+            init_speed=env_float("INIT_SPEED", 50.0),
+            motion=MotionConfig.from_env(),
+        )

dora_ulite6/controller.py

@@ -0,0 +1,71 @@
+"""Facts about the xArm controller: its modes, its codes, and the pose maths.
+
+Nothing tunable lives here — the limits and timeouts are configuration, read
+from the environment in :mod:`dora_ulite6.config`. These are the constants the
+SDK itself defines and the helpers that read its answers.
+"""
+
+from xarm.core.config.x_code import ControllerErrorCodeMap
+from xarm.x3.code import APIState
+
+MODE_POSITION = 0
+MODE_TEACH = 2
+MODE_CARTESIAN_VELOCITY = 5
+
+# Controller states as the arm reports them. From 4 on the arm is stopped and
+# refuses to move until enabled again — where the emergency stop leaves it.
+STATE_STOPPED = 4
+
+# Return codes meaning the command never ran because the arm was not usable. The
+# SDK returns them instead of raising, so a driver that only logged them would
+# have every caller believe a motion happened that never did.
+UNAVAILABLE_CODES = frozenset(
+    {APIState.NOT_CONNECTED, APIState.NOT_READY, APIState.HAS_ERROR, APIState.STATE_NOT_READY}
+)
+
+# The one controller error a motion of ours can clear: C23, joints angle exceed
+# limit, whose documented remedy is to send the arm to its initial position.
+# Every other error is answered by a person — releasing a button, dragging the
+# arm out of a collision — and homing one would move an arm somebody stopped.
+HOMEABLE_ERROR = 23
+
+# How often to re-ask the controller while waiting. Cadence, not a limit.
+ARRIVAL_POLL = 0.1
+READY_POLL = 0.2
+
+
+def format_pose(pose) -> str:
+    """Render a pose for a log line or an error message."""
+    x, y, z, roll, pitch, yaw = pose
+    return f"({x:.0f}, {y:.0f}, {z:.0f} mm; {roll:.0f}, {pitch:.0f}, {yaw:.0f}°)"
+
+
+def shortest_turn(start: float, target: float) -> float:
+    """Return the signed degrees from ``start`` to ``target``, the short way round.
+
+    Euler angles wrap: an arm reporting roll −178 and asked for roll 180 has two
+    degrees to turn, but subtracting gives 358 — a sweep through roll 0 with the
+    tool pointing sideways, which no arm passes through.
+    """
+    return (target - start + 180) % 360 - 180
+
+
+def interpolate(start: list, target: list, fraction: float) -> list:
+    """Return the pose ``fraction`` of the way from ``start`` to ``target``.
+
+    Position moves in a straight line and each rotation turns the short way
+    round, which is what the controller does along a linear move.
+    """
+    pose = [a + (b - a) * fraction for a, b in zip(start[:3], target[:3], strict=True)]
+    pose += [a + shortest_turn(a, b) * fraction for a, b in zip(start[3:], target[3:], strict=True)]
+    return pose
+
+
+def describe_controller_error(code: int) -> str:
+    """Name a controller error, so a log reader does not have to look the number up."""
+    if not code:
+        return "no controller error"
+    entry = ControllerErrorCodeMap.get(code)
+    if entry is None:
+        return f"controller error {code}"
+    return f"controller error {code} ({entry['en']['title']})"

dora_ulite6/driver.py

@@ -0,0 +1,526 @@
+"""The UFactory Lite6 driver, over the vendor xArm SDK.
+
+Ported from the littlehand robot layer, which was written against this same
+controller. The point of all of it is that a command either moved the arm or
+raised: the SDK returns a code for a motion it quietly refused, and a driver that
+only logged that code would have every caller believe in a motion that never
+happened.
+
+**Every method here blocks**, some of them for as long as the arm takes to move,
+and on a disconnected controller some block unboundedly. Nothing but
+:mod:`dora_ulite6.control` may call into this class — see the note there.
+"""
+
+import time
+
+from xarm.wrapper import XArmAPI
+from xarm.x3.code import APIState
+
+from dora_ulite6.config import Config
+from dora_ulite6.controller import (
+    ARRIVAL_POLL,
+    HOMEABLE_ERROR,
+    MODE_CARTESIAN_VELOCITY,
+    MODE_POSITION,
+    MODE_TEACH,
+    READY_POLL,
+    STATE_STOPPED,
+    UNAVAILABLE_CODES,
+    describe_controller_error,
+    format_pose,
+    interpolate,
+    shortest_turn,
+)
+from dora_ulite6.errors import (
+    ArmNeedsHomingError,
+    MotionInterruptedError,
+    RobotUnavailableError,
+    UnreachablePoseError,
+)
+from dora_ulite6.log import log
+from dora_ulite6.pose import RobotPose
+
+
+class ULite6:
+    """A UFactory Lite6 arm and the tool bolted to it."""
+
+    def __init__(self, config: Config):
+        self.config = config
+        self.motion = config.motion
+        self.ip = config.robot_ip
+        self.effector = config.effector
+        self.effector.verify(self)
+        # Whether the arm is holding something with whatever tool it carries.
+        self.tool_engaged = False
+        self.freedrive_on = False
+        # Built without opening, so every connection goes through connect() and
+        # an arm that was absent at boot is no different from one unplugged later.
+        # enable_report is the SDK's default, but read_report's whole promise —
+        # that it costs no round trip and cannot block — rests on it, so it is
+        # stated here rather than inherited. Without it `arm.position` quietly
+        # falls back to a blocking get_position().
+        self.arm = XArmAPI(self.ip, do_not_open=True, enable_report=True)
+        self._velocity_mode = False
+        self.connect()
+
+    # --- connection ---
+
+    def connect(self) -> bool:
+        """Open (or reopen) the connection and enable motion.
+
+        Never raises: an unreachable arm is a state to report and retry, not an
+        error that stops the node from starting. The handle is reused rather than
+        rebuilt, which is how the SDK reconnects.
+        """
+        try:
+            self.arm.connect()
+            self.arm.motion_enable(enable=True)
+            self.arm.set_mode(MODE_POSITION)
+            self.arm.set_state(0)
+            self._velocity_mode = False
+            log(f"connected to the robot at {self.ip}")
+            return True
+        except Exception as error:  # noqa: BLE001 — the SDK raises anything; an absent arm is a state, not a crash
+            log(f"failed to connect to the robot at {self.ip}: {error}")
+            return False
+
+    def disconnect(self) -> None:
+        """Close the connection."""
+        self.arm.disconnect()
+        log("robot disconnected")
+
+    def is_connected(self) -> bool:
+        """Return whether the arm controller is reachable."""
+        return bool(self.arm.connected)
+
+    def is_ready(self) -> bool:
+        """Return whether the arm would accept a motion right now.
+
+        Read from the report stream, which is what makes it cheap enough for
+        telemetry: a pressed emergency stop leaves the controller answering every
+        query while refusing every move.
+        """
+        return bool(self.arm.connected and self.arm.state < STATE_STOPPED and self.arm.error_code == 0)
+
+    def _require_connection(self) -> None:
+        """Raise unless the controller link is up; every command needs one."""
+        if not self.arm.connected:
+            raise RobotUnavailableError(f"robot at {self.ip} is not connected")
+
+    # --- reads ---
+
+    def get_current_position(self) -> RobotPose:
+        """Return the current flange pose."""
+        self._require_connection()
+        code, values = self.arm.get_position()
+        if code != 0 or len(values) < 6:
+            raise RobotUnavailableError(f"could not read the arm pose (code {code})")
+        return RobotPose.from_sequence(values)
+
+    def get_joint_angles(self) -> list:
+        """Return the current joint angles in degrees."""
+        self._require_connection()
+        code, angles = self.arm.get_servo_angle()
+        if code != 0 or not len(angles):
+            raise RobotUnavailableError(f"could not read the joint angles (code {code})")
+        return list(angles)
+
+    def read_report(self) -> tuple[list, RobotPose] | None:
+        """Return ``(joints, flange pose)`` from the controller's report stream, or None.
+
+        The one read that works **while a motion blocks**, and the only one the
+        reporter thread is allowed to make. The SDK keeps these values fresh from
+        a background stream of the controller's own, so this costs no round trip
+        and cannot block behind the move in flight — `get_position()` would sit
+        on the same socket the motion is using.
+
+        The cost is that it is a cache: it says what the arm last reported, with
+        no error code to say whether the report is current. That is the right
+        trade for the telemetry a caller follows a motion by, and the wrong one for
+        deciding whether a motion happened, which is why the driver's own checks
+        keep asking over the wire.
+
+        Returns None when the link is down, rather than handing back the last
+        pose the arm held before the cable was pulled.
+        """
+        if not self.arm.connected:
+            return None
+        position = self.arm.position
+        angles = self.arm.angles
+        if not position or len(position) < 6 or not angles:
+            return None
+        return list(angles), RobotPose.from_sequence(position)
+
+    def _live_error_code(self) -> int:
+        """Read the controller error over the wire, not from the report cache.
+
+        ``arm.error_code`` is whatever the report stream last pushed, which is
+        stale exactly when it matters: right after an abort, and right after
+        clearing one.
+        """
+        _, err_warn = self.arm.get_err_warn_code()
+        return err_warn[0]
+
+    # --- motion ---
+
+    def move_to(self, pose: RobotPose, speed: float | None = None) -> None:
+        """Move the flange to ``pose``, refusing a move it cannot make without commanding it."""
+        self._require_connection()
+        target = pose.as_list()
+        self._check_path_reachable(target, "move_to")
+        self.arm.set_mode(MODE_POSITION)
+        self.arm.set_state(0)
+        code = self.arm.set_position(
+            pose.x,
+            pose.y,
+            pose.z,
+            roll=pose.roll,
+            pitch=pose.pitch,
+            yaw=pose.yaw,
+            speed=speed if speed is not None else self.config.default_speed,
+            wait=True,
+        )
+        self._check_motion_code(code, "move_to", pose=target)
+        self._check_arrived(target, "move_to")
+        log(f"moved to {format_pose(target)}")
+
+    def set_joint_angles(self, angles: list, speed: float | None = None) -> None:
+        """Move the joints to ``angles`` in degrees."""
+        self._require_connection()
+        self.arm.set_mode(MODE_POSITION)
+        self.arm.set_state(0)
+        code = self.arm.set_servo_angle(
+            angle=angles,
+            speed=speed if speed is not None else self.config.default_joint_speed,
+            wait=True,
+        )
+        self._check_motion_code(code, "set_joint_angles")
+        log(f"joints moved to {angles}")
+
+    def go_home(self) -> None:
+        """Send the arm to its joint-space home."""
+        self._require_connection()
+        self.arm.set_mode(MODE_POSITION)
+        self.arm.set_state(0)
+        self._go_home_blocking()
+        log("moved to home")
+
+    def _go_home_blocking(self) -> None:
+        """Command the joint-space homing and confirm the arm really made it.
+
+        ``XArmAPI.reset`` looks like it does this, but it throws ``move_gohome``'s
+        return code away: ask it to home an arm the controller refuses to move and
+        it reports nothing and leaves the arm where it was.
+        """
+        code = self.arm.move_gohome(wait=True, timeout=self.motion.home_timeout_s)
+        self._check_motion_code(code, "move_gohome")
+        self._check_at_home()
+
+    def _check_at_home(self) -> None:
+        """Raise unless the arm really is at the joint zero it was sent to.
+
+        Homing is waited on by the same ``wait_move`` that calls a refused motion
+        finished, so it can report success for an arm that never moved.
+        """
+        try:
+            angles = self.get_joint_angles()
+        except RobotUnavailableError as error:
+            log(f"could not confirm the arm reached home ({error}); assuming it did")
+            return
+        off_by = max(abs(angle) for angle in angles)
+        if off_by > self.motion.arrival_tolerance_deg:
+            raise RobotUnavailableError(f"homing did not move the arm; it is still {off_by:.0f}° from home")
+
+    def home_and_clear(self) -> None:
+        """Clear the latched error and drive the arm home in joint space; this **moves the arm**.
+
+        The answer to :class:`ArmNeedsHomingError`, and only ever run after
+        whoever asked for it has warned the room.
+
+        Order matters: the controller refuses a motion while an error is latched,
+        and a latched error is what this is here to escape.
+        """
+        self.clear()
+        self.go_home()
+
+    def stop(self) -> None:
+        """Abort the current motion immediately (software stop, state 4).
+
+        The one method safe to call from another thread while a blocking move is
+        in flight — that is the whole point of it — so the disconnected case
+        returns rather than sitting for seconds retrying down a dead socket. The
+        interrupted move raises :class:`MotionInterruptedError` on its own thread.
+        """
+        if not self.arm.connected:
+            log("stop requested while disconnected")
+            return
+        self.arm.emergency_stop()
+        self._velocity_mode = False
+        log("motion stopped")
+
+    # --- arrival and reachability ---
+
+    def _check_arrived(self, target: list, operation: str) -> None:
+        """Raise unless the arm actually made the move it was commanded.
+
+        A return code of 0 does not mean it arrived: ``wait_move`` calls a motion
+        finished once the arm has been idle for a few polls, so a command the
+        controller quietly refused comes back looking like one that ran. The
+        controller's error is the half that decides; the pose is a coarse
+        backstop for a refusal that latched nothing.
+        """
+        error = self._live_error_code()
+        if error:
+            raise RobotUnavailableError(
+                f"{operation} to {format_pose(target)} left the arm in {describe_controller_error(error)}"
+            )
+        deadline = time.monotonic() + self.motion.arrival_timeout_s
+        while True:
+            code, current = self.arm.get_position()
+            if code != 0 or not len(current):
+                log(f"{operation}: could not confirm arrival (code {code}); assuming it landed")
+                return
+            off_by = max(abs(a - b) for a, b in zip(current[:3], target[:3], strict=True))
+            turned = max(abs(shortest_turn(a, b)) for a, b in zip(current[3:], target[3:], strict=True))
+            if off_by <= self.motion.arrival_tolerance_mm and turned <= self.motion.arrival_tolerance_deg:
+                return
+            if time.monotonic() >= deadline:
+                raise MotionInterruptedError(
+                    f"{operation} to {format_pose(target)} never arrived; the arm is at {format_pose(current)} "
+                    f"({describe_controller_error(self._live_error_code())})"
+                )
+            time.sleep(ARRIVAL_POLL)
+
+    def _check_path_reachable(self, target: list, operation: str) -> None:
+        """Refuse a move whose straight-line path leaves the joint envelope.
+
+        The controller aborts partway through a line it cannot follow, with a
+        latched error that then refuses every later command. Asking it to solve
+        the path first turns that into a refusal nobody has to recover from.
+        Sampling matters as much as the target: a line from a turned wrist to a
+        downward one passes through configurations neither end visits.
+        """
+        *waypoints, destination = self._path_to(target)
+        for pose in waypoints:
+            self._check_pose_holdable(pose, target, operation, margin=False)
+        self._check_pose_holdable(destination, target, operation, margin=True)
+
+    def _path_to(self, target: list) -> list:
+        """Return the poses to check on the way to ``target``, ``target`` last.
+
+        A pose read the controller could not answer leaves nothing to interpolate
+        from, so the path collapses to the target alone rather than being sampled
+        from a stale start, which would refuse perfectly good moves.
+        """
+        code, current = self.arm.get_position()
+        if code != 0 or not len(current):
+            log(f"could not read the pose to plan from (code {code}); checking the target only")
+            return [target]
+        samples = self.motion.path_samples
+        steps = [interpolate(current, target, step / samples) for step in range(1, samples)]
+        return steps + [target]
+
+    def _check_pose_holdable(self, pose: list, target: list, operation: str, margin: bool) -> None:
+        """Raise unless the arm has a joint solution for ``pose``, clear of its limits.
+
+        ``margin`` asks the dearer question — whether the solution merely fits —
+        and is worth its round trips only where the arm comes to rest.
+        """
+        code, angles = self.arm.get_inverse_kinematics(pose)
+        if code != 0 or not len(angles):
+            raise UnreachablePoseError(
+                f"{operation} to {format_pose(target)}: no joint solution for {format_pose(pose)} (code {code})"
+            )
+        code, limited = self.arm.is_joint_limit(angles)
+        # A check the controller could not answer is not a refusal: failing shut
+        # would ground the arm on any transient, and it still guards the motion.
+        if code == 0 and limited:
+            raise UnreachablePoseError(
+                f"{operation} to {format_pose(target)}: {format_pose(pose)} needs joint angles out of range"
+            )
+        near = self._joints_near_limit(angles) if margin else []
+        if near:
+            joints = ", ".join(f"J{index}" for index in near)
+            raise UnreachablePoseError(
+                f"{operation} to {format_pose(target)}: {format_pose(pose)} leaves "
+                f"{joints} within {self.motion.joint_limit_margin_deg}° of a limit"
+            )
+
+    def _joints_near_limit(self, angles) -> list:
+        """Return the 1-based joints whose solution sits within the margin of a bound.
+
+        Asked of the controller rather than read off a table: the SDK exposes only
+        the reduced-mode bounds, and the real ones differ per arm model.
+        """
+        margin = self.motion.joint_limit_margin_deg
+        near = []
+        for index in range(len(angles)):
+            for delta in (margin, -margin):
+                probe = list(angles)
+                probe[index] += delta
+                code, limited = self.arm.is_joint_limit(probe)
+                if code == 0 and limited:
+                    near.append(index + 1)
+                    break
+        return near
+
+    # --- failure handling ---
+
+    def _check_motion_code(self, code: int, operation: str, pose: list | None = None) -> None:
+        """Raise on a motion that was aborted or never ran, log anything else."""
+        if code == APIState.EMERGENCY_STOP:
+            # The SDK returns this for any aborted motion, a stop of ours and a
+            # controller fault alike; only the live error code tells them apart.
+            where = f" to {format_pose(pose)}" if pose else ""
+            error = self._live_error_code()
+            if error:
+                raise self._motion_fault(f"{operation}{where} aborted by {describe_controller_error(error)}", error)
+            raise MotionInterruptedError(f"{operation}{where} aborted by stop")
+        self._check_command_code(code, operation, pose)
+
+    def _check_command_code(self, code: int, operation: str, pose: list | None = None) -> None:
+        """Raise on a command that never ran, log anything else.
+
+        For commands that are not motions — switching the vacuum, say. Nothing
+        here can raise :class:`MotionInterruptedError`, because there was no
+        motion to interrupt; a refusal means the arm was not usable.
+        """
+        if code == 0:
+            return
+        where = f" to {format_pose(pose)}" if pose else ""
+        if code in UNAVAILABLE_CODES:
+            error = self._live_error_code()
+            raise self._motion_fault(
+                f"{operation}{where} was refused (code {code}, {describe_controller_error(error)}): "
+                "the arm is disconnected, stopped or not enabled",
+                error,
+            )
+        log(f"{operation} returned error code {code}")
+
+    def _motion_fault(self, detail: str, error: int) -> RobotUnavailableError:
+        """Pick the failure that names what will actually get the arm moving again.
+
+        A joint past its limit clears whenever asked and comes straight back on
+        the next motion, so "clear it and try again" never converges; it needs a
+        joint-space move home, and that has to be announced first.
+        """
+        if error == HOMEABLE_ERROR:
+            return ArmNeedsHomingError(detail)
+        return RobotUnavailableError(detail)
+
+    def recover(self) -> None:
+        """Reconnect if the link dropped, clear errors, and re-enable motion. Never moves the arm.
+
+        Clearing is unconditional, because the cached error codes come from the
+        report stream, which is stale exactly in the case that dropped the link.
+        A joint past its own limit is the one thing clearing cannot fix — that
+        raises :class:`ArmNeedsHomingError`, whose answer is
+        :meth:`home_and_clear`.
+        """
+        self._velocity_mode = False
+        if not self.arm.connected and not self.connect():
+            raise RobotUnavailableError(f"robot at {self.ip} is not reachable")
+        self.clear()
+        try:
+            self._wait_until_ready()
+        except RobotUnavailableError as error:
+            if self._live_error_code() == HOMEABLE_ERROR:
+                raise ArmNeedsHomingError(f"{error}; only homing the arm will clear it") from error
+            raise
+        log("recovered and motion re-enabled")
+
+    def clear(self) -> None:
+        """Clear any latched error and warning, and put the arm back in position mode."""
+        self._require_connection()
+        self._velocity_mode = False
+        self.arm.clean_error()
+        self.arm.clean_warn()
+        self.arm.motion_enable(enable=True)
+        self.arm.set_mode(MODE_POSITION)
+        self.arm.set_state(0)
+
+    def _wait_until_ready(self) -> None:
+        """Block until the controller reports a state it can move from.
+
+        Both the state and the error are asked over the wire: after a reconnect
+        the report stream may have delivered nothing yet, and after a clear it
+        still carries the error that was just cleared.
+        """
+        deadline = time.monotonic() + self.motion.ready_timeout_s
+        while True:
+            code, state = self.arm.get_state()
+            error = self._live_error_code()
+            if code == 0 and state < STATE_STOPPED and error == 0:
+                return
+            if time.monotonic() >= deadline:
+                raise RobotUnavailableError(
+                    f"robot did not become ready (state {state}, {describe_controller_error(error)}): "
+                    "is the emergency stop still pressed?"
+                )
+            time.sleep(READY_POLL)
+
+    # --- the tool ---
+
+    def engage_tool(self) -> None:
+        """Take hold with whatever tool is mounted."""
+        self.effector.attach(self)
+        self.tool_engaged = True
+
+    def release_tool(self) -> None:
+        """Let go."""
+        self.effector.detach(self)
+        self.tool_engaged = False
+
+    def set_vacuum(self, on: bool) -> None:
+        """Switch the vacuum gripper and wait for it to build or release."""
+        self._require_connection()
+        code = self.arm.set_vacuum_gripper(on, wait=False)
+        self._check_command_code(code, "set_vacuum")
+        time.sleep(self.motion.tool_settle_s)
+        log(f"vacuum set to {on}")
+
+    def set_gripper(self, closed: bool) -> None:
+        """Close or open the Lite6 two-finger gripper and wait for the fingers to settle."""
+        self._require_connection()
+        code = self.arm.close_lite6_gripper() if closed else self.arm.open_lite6_gripper()
+        self._check_command_code(code, "set_gripper")
+        time.sleep(self.motion.tool_settle_s)
+        log(f"gripper {'closed' if closed else 'opened'}")
+
+    # --- control modes ---
+
+    def set_freedrive(self, enabled: bool) -> None:
+        """Enter or leave manual teach mode, so the arm can be moved by hand."""
+        self._require_connection()
+        self.arm.set_mode(MODE_TEACH if enabled else MODE_POSITION)
+        self.arm.set_state(0)
+        self.freedrive_on = enabled
+        log(f"freedrive {'enabled' if enabled else 'disabled'}")
+
+    def set_cartesian_velocity(self, velocity: list) -> None:
+        """Drive the arm at a constant base-frame velocity (xArm mode 5) and return.
+
+        ``velocity`` is ``[vx, vy, vz, wx, wy, wz]`` in mm/s and deg/s. The arm
+        keeps moving until the controller's own TTL expires or a new velocity
+        arrives, so a sender must keep sending; an all-zero vector stops it and
+        leaves velocity mode.
+        """
+        self._require_connection()
+        if any(velocity):
+            if not self._velocity_mode:
+                self.arm.set_mode(MODE_CARTESIAN_VELOCITY)
+                self.arm.set_state(0)
+                self._velocity_mode = True
+            code = self.arm.vc_set_cartesian_velocity(
+                velocity,
+                is_radian=False,
+                is_tool_coord=False,
+                duration=self.motion.velocity_command_ttl_s,
+            )
+            self._check_motion_code(code, "set_cartesian_velocity")
+            return
+        if self._velocity_mode:
+            self.arm.vc_set_cartesian_velocity([0.0] * 6, is_radian=False)
+            self.arm.set_mode(MODE_POSITION)
+            self.arm.set_state(0)
+            self._velocity_mode = False

dora_ulite6/effectors.py

@@ -0,0 +1,99 @@
+"""End effectors: what the arm carries at its flange, and what that lets it do.
+
+"Grasp" is the same symbolic command whatever is bolted on; what differs is how
+it is realized (suction on, fingers closed). That difference lives here, so the
+event loop never names a tool.
+
+Each effector declares the driver method that switches it and how far past the
+flange its tip sits, which is what `tcp_pose` is offset by. Adding a tool is a
+method on the driver and a class here.
+"""
+
+from abc import ABC
+from typing import ClassVar
+
+from dora_ulite6.errors import EffectorUnsupportedError
+
+
+class EndEffector(ABC):
+    """Base class for whatever is mounted at the flange."""
+
+    # The driver method that switches this tool on and off; None for a bare
+    # flange, which asks nothing of the arm.
+    drives: ClassVar[str | None]
+    # Distance from the flange to the tool tip along the flange Z axis, in mm.
+    offset_mm: ClassVar[float]
+
+    def attach(self, robot) -> None:
+        """Take hold."""
+        self._switch(robot, True)
+
+    def detach(self, robot) -> None:
+        """Let go."""
+        self._switch(robot, False)
+
+    def _switch(self, robot, engaged: bool) -> None:
+        """Call the driver method this effector drives.
+
+        Using the declared name rather than a literal is what keeps ``drives``
+        and the call from drifting apart.
+        """
+        getattr(robot, self.drives)(engaged)
+
+    def verify(self, robot) -> None:
+        """Raise unless this arm has the method this effector switches.
+
+        Checked when the arm is built rather than at first use: a configured
+        effector the driver has no method for is a setup mistake, and it should
+        stop the node joining the dataflow rather than surface mid-motion.
+        """
+        if self.drives and not hasattr(robot, self.drives):
+            raise EffectorUnsupportedError(
+                f"{type(self).__name__} needs a driver with {self.drives}(); "
+                f"{type(robot).__name__} has none"
+            )
+
+
+class VacuumEffector(EndEffector):
+    """A suction cup: it comes down onto an object's top face and holds it by vacuum."""
+
+    drives = "set_vacuum"
+    offset_mm = 61.0
+
+
+class GripperEffector(EndEffector):
+    """The Lite6 two-finger gripper."""
+
+    drives = "set_gripper"
+    offset_mm = 65.0
+
+
+class NoEffector(EndEffector):
+    """A bare flange: the arm can reach and point, but cannot hold anything."""
+
+    drives = None
+    offset_mm = 0.0
+
+    def attach(self, robot) -> None:
+        """Refuse: there is nothing mounted to grasp with."""
+        raise EffectorUnsupportedError("this arm has no effector to grasp with")
+
+    def detach(self, robot) -> None:
+        """Refuse: there is nothing mounted to release with."""
+        raise EffectorUnsupportedError("this arm has no effector to release with")
+
+
+EFFECTORS: dict[str, type[EndEffector]] = {
+    "vacuum": VacuumEffector,
+    "gripper": GripperEffector,
+    "none": NoEffector,
+}
+
+
+def get_effector(name: str) -> EndEffector:
+    """Return the effector registered under ``name``, or raise listing which exist."""
+    try:
+        return EFFECTORS[name]()
+    except KeyError:
+        available = ", ".join(sorted(EFFECTORS))
+        raise ValueError(f"unknown effector {name!r}; available: {available}") from None

dora_ulite6/errors.py

@@ -0,0 +1,43 @@
+"""Everything the robot layer raises.
+
+The distinctions matter: they say whether the arm moved, whether anything can be
+done about it, and whether the answer needs a human first. Taken from the
+littlehand robot layer, where they were arrived at against this same controller.
+"""
+
+
+class MotionInterruptedError(Exception):
+    """The motion was aborted in flight, by :meth:`ULite6.stop` or a controller fault."""
+
+
+class RobotUnavailableError(Exception):
+    """The command never ran because the arm was not usable.
+
+    A pulled cable, a pressed emergency stop, a latched controller error: what a
+    caller must do is the same, and it is never to carry on as if the arm moved.
+    Recoverable by :meth:`ULite6.recover`.
+    """
+
+
+class UnreachablePoseError(Exception):
+    """The pose was never commanded because the arm cannot hold it.
+
+    Deliberately not a :class:`RobotUnavailableError`: the arm is fine and the
+    target is not, so a retry would only ask the same impossible thing and latch
+    a joint limit error on the controller.
+    """
+
+
+class ArmNeedsHomingError(RobotUnavailableError):
+    """Only a motion of ours can clear this error, so the caller must warn first.
+
+    A joint past its own limit stays past it however often the error is cleared.
+    Narrow on purpose: an arm that is stopped rather than stuck — emergency stop
+    held, collision to be dragged out of by hand — stays a plain
+    :class:`RobotUnavailableError`, because homing it would move an arm somebody
+    stopped deliberately.
+    """
+
+
+class EffectorUnsupportedError(Exception):
+    """The configured effector is not one this arm can drive."""

dora_ulite6/log.py

@@ -0,0 +1,13 @@
+"""Timestamped stderr logging.
+
+stderr rather than stdout: dora reads a dynamic node's stdout, and a print that
+lands there is noise in the wrong stream.
+"""
+
+import sys
+import time
+
+
+def log(message: str) -> None:
+    """Print a timestamped line for the node."""
+    print(f"[ulite6 {time.strftime('%H:%M:%S')}] {message}", file=sys.stderr, flush=True)

dora_ulite6/pose.py

@@ -0,0 +1,50 @@
+"""The pose that crosses the driver boundary, and the tool-tip offset applied to it."""
+
+from dataclasses import dataclass
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+@dataclass
+class RobotPose:
+    """A robot pose: position in mm, orientation in degrees, both in the base frame.
+
+    The controller reports and accepts the flange pose. ``tcp_pose`` on the
+    dataflow is this pose pushed out to the mounted tool's tip by
+    :func:`with_tool_offset`.
+    """
+
+    x: float
+    y: float
+    z: float
+    roll: float = 180.0
+    pitch: float = 0.0
+    yaw: float = 0.0
+
+    @classmethod
+    def from_sequence(cls, values) -> "RobotPose":
+        """Build a pose from ``[x, y, z, roll, pitch, yaw]``; a position alone is enough."""
+        return cls(**dict(zip(("x", "y", "z", "roll", "pitch", "yaw"), values, strict=False)))
+
+    def as_list(self) -> list[float]:
+        """Return the pose as ``[x, y, z, roll, pitch, yaw]``, the order the SDK uses."""
+        return [self.x, self.y, self.z, self.roll, self.pitch, self.yaw]
+
+
+def rotation_matrix(pose: RobotPose) -> np.ndarray:
+    """Return the 3x3 base-frame rotation of ``pose``."""
+    return Rotation.from_euler("xyz", [pose.roll, pose.pitch, pose.yaw], degrees=True).as_matrix()
+
+
+def with_tool_offset(pose: RobotPose, offset_mm: float) -> RobotPose:
+    """Return ``pose`` moved ``offset_mm`` along its own Z axis.
+
+    The offset is along the *tool* Z, not the base Z, so a tilted flange puts the
+    tip where the tool actually reaches rather than straight down from it.
+    Orientation is unchanged: sliding along the axis does not turn anything.
+    """
+    if offset_mm == 0.0:
+        return pose
+    tip = np.array([pose.x, pose.y, pose.z]) + rotation_matrix(pose) @ np.array([0.0, 0.0, offset_mm])
+    return RobotPose(tip[0], tip[1], tip[2], pose.roll, pose.pitch, pose.yaw)

tests/test_config.py

@@ -0,0 +1,110 @@
+"""Reading the environment, and refusing a setting the node cannot use."""
+
+import pytest
+
+from dora_ulite6.config import Config, env_float, env_int
+from dora_ulite6.effectors import (
+    EFFECTORS,
+    GripperEffector,
+    NoEffector,
+    VacuumEffector,
+    get_effector,
+)
+from dora_ulite6.errors import EffectorUnsupportedError
+
+
+@pytest.fixture(autouse=True)
+def clean_env(monkeypatch):
+    """Run every test against a bare environment, whatever the shell set."""
+    for name in ("ROBOT_IP", "TOOL_TYPE", "TOOL_OFFSET_MM", "INIT_MODE", "INIT_Z", "DEFAULT_SPEED"):
+        monkeypatch.delenv(name, raising=False)
+
+
+def test_the_defaults_need_no_environment():
+    config = Config.from_env()
+
+    assert config.robot_ip == "192.168.1.192"
+    assert isinstance(config.effector, NoEffector)
+    assert config.tool_offset_mm == 0.0
+    assert config.init_mode == "none"
+
+
+def test_the_tool_offset_is_the_tools_own_plus_the_extra(monkeypatch):
+    monkeypatch.setenv("TOOL_TYPE", "vacuum")
+    monkeypatch.setenv("TOOL_OFFSET_MM", "4.5")
+
+    config = Config.from_env()
+
+    assert isinstance(config.effector, VacuumEffector)
+    assert config.tool_offset_mm == pytest.approx(VacuumEffector.offset_mm + 4.5)
+
+
+def test_an_unknown_tool_is_refused_at_startup(monkeypatch):
+    monkeypatch.setenv("TOOL_TYPE", "magnet")
+
+    with pytest.raises(ValueError, match="vacuum"):
+        Config.from_env()
+
+
+def test_an_unknown_init_mode_is_refused_at_startup(monkeypatch):
+    # Caught here rather than at the first motion, so the node never joins the
+    # dataflow with a startup move nobody can name.
+    monkeypatch.setenv("INIT_MODE", "somewhere")
+
+    with pytest.raises(ValueError, match="INIT_MODE"):
+        Config.from_env()
+
+
+def test_init_pose_components_are_read(monkeypatch):
+    monkeypatch.setenv("INIT_MODE", "pose")
+    monkeypatch.setenv("INIT_Z", "180")
+
+    assert Config.from_env().init_pose.z == 180.0
+
+
+@pytest.mark.parametrize("value", ["", "   "])
+def test_a_blank_variable_falls_back_to_the_default(monkeypatch, value):
+    monkeypatch.setenv("DEFAULT_SPEED", value)
+
+    assert env_float("DEFAULT_SPEED", 30.0) == 30.0
+
+
+def test_a_number_that_is_not_one_names_itself(monkeypatch):
+    monkeypatch.setenv("PATH_SAMPLES", "four")
+
+    with pytest.raises(ValueError, match="PATH_SAMPLES"):
+        env_int("PATH_SAMPLES", 4)
+
+
+def test_a_bare_flange_refuses_to_grasp():
+    with pytest.raises(EffectorUnsupportedError):
+        NoEffector().attach(object())
+
+
+def test_an_effector_is_refused_by_a_driver_that_cannot_switch_it():
+    class ArmWithNoTools:
+        pass
+
+    with pytest.raises(EffectorUnsupportedError, match="set_vacuum"):
+        VacuumEffector().verify(ArmWithNoTools())
+
+
+def test_each_effector_switches_the_driver_method_it_declares():
+    switched = []
+
+    class Arm:
+        def set_gripper(self, closed):
+            switched.append(closed)
+
+    arm = Arm()
+    effector = get_effector("gripper")
+    effector.verify(arm)
+    effector.attach(arm)
+    effector.detach(arm)
+
+    assert isinstance(effector, GripperEffector)
+    assert switched == [True, False]
+
+
+def test_every_registered_effector_declares_a_reach():
+    assert all(effector.offset_mm >= 0 for effector in EFFECTORS.values())

tests/test_pose.py

@@ -0,0 +1,71 @@
+"""Pose maths and the tool-tip offset. No robot needed."""
+
+import numpy as np
+import pytest
+
+from dora_ulite6.controller import interpolate, shortest_turn
+from dora_ulite6.pose import RobotPose, with_tool_offset
+
+
+def test_from_sequence_accepts_a_position_alone():
+    pose = RobotPose.from_sequence([10, 20, 30])
+
+    assert pose.as_list() == [10, 20, 30, 180.0, 0.0, 0.0]
+
+
+def test_from_sequence_round_trips_a_full_pose():
+    values = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]
+
+    assert RobotPose.from_sequence(values).as_list() == values
+
+
+@pytest.mark.parametrize(
+    ("start", "target", "expected"),
+    [(-178.0, 180.0, -2.0), (180.0, -178.0, 2.0), (0.0, 90.0, 90.0), (0.0, 270.0, -90.0)],
+)
+def test_shortest_turn_never_takes_the_long_way(start, target, expected):
+    assert shortest_turn(start, target) == pytest.approx(expected)
+
+
+def test_interpolate_turns_the_short_way_across_the_wrap():
+    halfway = interpolate([0, 0, 0, -178, 0, 0], [0, 0, 0, 180, 0, 0], 0.5)
+
+    assert halfway[3] == pytest.approx(-179.0)
+
+
+def test_interpolate_walks_the_straight_line_in_position():
+    halfway = interpolate([0, 0, 0, 180, 0, 0], [100, 200, 300, 180, 0, 0], 0.5)
+
+    assert halfway[:3] == pytest.approx([50, 100, 150])
+
+
+def test_tool_down_offset_lowers_the_tip():
+    tip = with_tool_offset(RobotPose(300, 0, 250, roll=180, pitch=0, yaw=0), 61.0)
+
+    assert tip.z == pytest.approx(250 - 61.0)
+    assert (tip.x, tip.y) == pytest.approx((300, 0))
+
+
+def test_offset_follows_the_tool_axis_not_the_base_axis():
+    # Pitched 90° from tool-down, the tool points along +x rather than -z.
+    tip = with_tool_offset(RobotPose(300, 0, 250, roll=0, pitch=90, yaw=0), 61.0)
+
+    assert tip.x == pytest.approx(300 + 61.0)
+    assert tip.z == pytest.approx(250)
+
+
+def test_a_zero_offset_returns_the_pose_untouched():
+    pose = RobotPose(1, 2, 3, 4, 5, 6)
+
+    assert with_tool_offset(pose, 0.0) is pose
+
+
+def test_the_offset_inverts_exactly():
+    # move_to relies on this: it takes the offset back off a tip pose to get the
+    # flange pose the controller wants.
+    pose = RobotPose(123, -45, 210, roll=170, pitch=12, yaw=-30)
+
+    there = with_tool_offset(pose, 61.0)
+    back = with_tool_offset(there, -61.0)
+
+    np.testing.assert_allclose(back.as_list(), pose.as_list(), atol=1e-9)