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.
72 lines
2.9 KiB
Python
72 lines
2.9 KiB
Python
"""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']})"
|