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:
@@ -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']})"
|
||||
Reference in New Issue
Block a user