cristhian
eccd9e3c24
Brings across the robot layer from monolitic-littlehand-python, which was written against this same controller. The point of it is that a command either moved the arm or raised: the SDK returns a code for a motion the controller quietly refused, wait_move calls that refusal finished, and arm.error_code is the report stream's stale copy. So errors are read live, arrival is confirmed rather than assumed, and a move whose straight-line path leaves the joint envelope is refused before it is commanded. Failures are typed, because the distinctions decide the response: an unreachable pose is not an unavailable arm, and a joint past its limit needs a motion nobody should start without warning the room. read_report is the deliberate exception to all of it: a report-cache read that costs no round trip and cannot block, which is the only read that works while a motion is in flight.
527 lines
22 KiB
Python
527 lines
22 KiB
Python
"""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
|