cristhian
a95d804f0b
`set_cartesian_velocity` passed `duration=velocity_command_ttl_s` on every message, so each one re-armed a timed segment and the controller re-planned the same motion. On a real Lite6 that is not smooth travel: the arm moves in steps. Reducing how often the velocity is sent does not help — re-arming four times a second still steps — so it now uses `duration=0`, the SDK's "always effective, will not stop automatically", and skips a velocity that has not changed. One command stands until a different one replaces it. That removes the controller-side expiry, which `_expire_jog` was relying on: it flipped `_jogging` and reported `ready` without commanding anything, because the controller's own timer had already stopped the arm. With the velocity held indefinitely that would leave an arm travelling while the node reported idle, so `_expire_jog` now queues the stop itself. It goes through the job queue rather than the driver, since it runs on the reporter thread and the control thread owns the driver. `VELOCITY_COMMAND_TTL_S` is unchanged in value and meaning to a caller, but it is now enforced here rather than by the controller. Both halves are one commit on purpose: `duration=0` without the stop in `_expire_jog` is an arm that never stops when its sender dies. Adds tests/test_driver.py — the driver had no direct coverage, because FakeArm replaces it everywhere else, which is why this was invisible to the suite.
552 lines
24 KiB
Python
552 lines
24 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
|
|
# The velocity currently standing on the controller. A velocity is a
|
|
# set-point it holds, so re-sending an unchanged one is not a no-op: see
|
|
# `set_cartesian_velocity`.
|
|
self._commanded: list | None = None
|
|
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
|
|
controller holds it until a different one arrives or an all-zero vector
|
|
stops it and leaves velocity mode.
|
|
|
|
**The velocity is set once and then left alone.** ``duration=0`` is the
|
|
SDK's "always effective, will not stop automatically", and an unchanged
|
|
velocity is not sent again at all. Both halves matter: this used to pass
|
|
``duration=velocity_command_ttl_s`` on every message, which re-armed a
|
|
timed segment several times a second and made the controller re-plan the
|
|
same motion over and over. On a real arm that is not smooth travel — an
|
|
operator feels it as the arm moving in steps. Re-arming even four times a
|
|
second is enough to produce it, so the answer is not to re-arm less
|
|
often but not to re-arm at all.
|
|
|
|
Dropping ``duration`` means the controller no longer stops the arm by
|
|
itself, so the expiry that backs a sender going quiet has to be enforced
|
|
here instead: :meth:`Control._expire_jog` commands the stop that the
|
|
controller's timer used to.
|
|
"""
|
|
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
|
|
# Every route out of velocity mode comes back through here, so
|
|
# this is the one place the standing velocity has to be
|
|
# forgotten — re-entering the mode must always re-send.
|
|
self._commanded = None
|
|
if velocity == self._commanded:
|
|
return
|
|
code = self.arm.vc_set_cartesian_velocity(
|
|
velocity,
|
|
is_radian=False,
|
|
is_tool_coord=False,
|
|
duration=0,
|
|
)
|
|
self._commanded = list(velocity)
|
|
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
|