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