diff --git a/dora_ulite6/config.py b/dora_ulite6/config.py new file mode 100644 index 0000000..c7da41b --- /dev/null +++ b/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(), + ) diff --git a/dora_ulite6/controller.py b/dora_ulite6/controller.py new file mode 100644 index 0000000..ffb703b --- /dev/null +++ b/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']})" diff --git a/dora_ulite6/driver.py b/dora_ulite6/driver.py new file mode 100644 index 0000000..a5bcded --- /dev/null +++ b/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 diff --git a/dora_ulite6/effectors.py b/dora_ulite6/effectors.py new file mode 100644 index 0000000..2e6267d --- /dev/null +++ b/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 diff --git a/dora_ulite6/errors.py b/dora_ulite6/errors.py new file mode 100644 index 0000000..6c1448e --- /dev/null +++ b/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.""" diff --git a/dora_ulite6/log.py b/dora_ulite6/log.py new file mode 100644 index 0000000..41b5c0f --- /dev/null +++ b/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) diff --git a/dora_ulite6/pose.py b/dora_ulite6/pose.py new file mode 100644 index 0000000..c9c7276 --- /dev/null +++ b/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) diff --git a/tests/test_config.py b/tests/test_config.py new file mode 100644 index 0000000..0b63347 --- /dev/null +++ b/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()) diff --git a/tests/test_pose.py b/tests/test_pose.py new file mode 100644 index 0000000..7cc1c83 --- /dev/null +++ b/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)