add xArm driver layer ported from littlehand

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.
This commit is contained in:
2026-09-12 11:46:54 -03:00
parent 69ba84e406
commit eccd9e3c24
9 changed files with 1113 additions and 0 deletions
+71
View File
@@ -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']})"