fix jogging stepping by holding one velocity instead of re-arming
`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.
This commit is contained in:
@@ -60,9 +60,10 @@ class MotionConfig:
|
||||
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.
|
||||
# How long a jog stream may go quiet before this node stops the arm. The
|
||||
# velocity is set on the controller once and held there indefinitely — see
|
||||
# `ULite6.set_cartesian_velocity` — so nothing expires it but us: this is
|
||||
# the deadman behind a sender that dies mid-jog, enforced by `_expire_jog`.
|
||||
velocity_command_ttl_s: float = 0.5
|
||||
|
||||
@classmethod
|
||||
|
||||
+12
-4
@@ -575,16 +575,24 @@ class Control:
|
||||
# --- the reporter thread ---
|
||||
|
||||
def _expire_jog(self) -> None:
|
||||
"""Stop calling the arm jogging once the controller has expired the command.
|
||||
"""Stop the arm once the sender driving it has gone quiet.
|
||||
|
||||
A jog lives on the controller's own TTL, so a sender that goes quiet
|
||||
leaves an arm that stopped moving a moment later. Nothing tells us; the
|
||||
clock does.
|
||||
**This is the deadman, not bookkeeping.** The velocity is set on the
|
||||
controller once and held there indefinitely — see
|
||||
:meth:`ULite6.set_cartesian_velocity` for why it cannot be given a
|
||||
duration — so a sender that dies mid-jog leaves an arm that travels
|
||||
until something says otherwise. Nothing tells us; the clock does, and
|
||||
this is what says otherwise.
|
||||
|
||||
The stop goes through the job queue rather than the arm directly: this
|
||||
runs on the reporter thread, and the control thread is the only one that
|
||||
may touch the driver.
|
||||
"""
|
||||
with self._lock:
|
||||
if not self._jogging or time.monotonic() - self._last_jog < self.config.motion.velocity_command_ttl_s:
|
||||
return
|
||||
self._jogging = False
|
||||
self._jobs.put(Jog(velocity=[0.0] * 6))
|
||||
self._idle_unless_busy()
|
||||
|
||||
def _run_reporter(self) -> None:
|
||||
|
||||
+30
-5
@@ -61,6 +61,10 @@ class ULite6:
|
||||
# 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 ---
|
||||
@@ -500,10 +504,24 @@ class ULite6:
|
||||
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.
|
||||
``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):
|
||||
@@ -511,12 +529,19 @@ class ULite6:
|
||||
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=self.motion.velocity_command_ttl_s,
|
||||
duration=0,
|
||||
)
|
||||
self._commanded = list(velocity)
|
||||
self._check_motion_code(code, "set_cartesian_velocity")
|
||||
return
|
||||
if self._velocity_mode:
|
||||
|
||||
Reference in New Issue
Block a user