From a95d804f0b0fbacaebe15f7e3e25bbc6a5080d87 Mon Sep 17 00:00:00 2001 From: Cristhian Date: Wed, 16 Sep 2026 14:27:46 -0300 Subject: [PATCH] fix jogging stepping by holding one velocity instead of re-arming MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit `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. --- AGENTS.md | 11 ++++- README.md | 22 ++++++--- dora-node.yml | 12 +++-- dora_ulite6/config.py | 7 +-- dora_ulite6/control.py | 16 +++++-- dora_ulite6/driver.py | 35 ++++++++++++-- tests/test_driver.py | 104 ++++++++++++++++++++++++++++++++++++++++ tests/test_lifecycle.py | 19 +++++++- 8 files changed, 201 insertions(+), 25 deletions(-) create mode 100644 tests/test_driver.py diff --git a/AGENTS.md b/AGENTS.md index 28a3f19..233c8df 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -60,7 +60,7 @@ Everything is configured through environment variables — see README.md for the ## Test ```bash -uv run pytest # 86 tests, no robot and no dataflow needed +uv run pytest # 92 tests, no robot and no dataflow needed uv run ruff check . ``` @@ -183,6 +183,15 @@ A typed optional input therefore forces the example dataflow to wire it. `reques ## Rules - Do not mention AI tools in commit messages or documentation. +- **A velocity is set on the controller once and held.** `set_cartesian_velocity` passes no + duration and skips an unchanged velocity, because re-arming makes the controller re-plan + the same motion and the arm steps instead of travelling — four times a second is enough + to feel it. Do not reintroduce a per-message `duration`, and do not "simplify" the + unchanged-velocity check away. +- **`_expire_jog` is the deadman, not bookkeeping.** Nothing else stops an arm whose sender + died mid-jog: the controller holds the velocity indefinitely. It must command the stop, + not merely report `ready` — an arm travelling while the node says idle is worse than no + expiry at all. - This controls a real robot. Test changes carefully and preserve the safety checks: the reachability check before a move, the arrival confirmation after it, and the live error reads that make both of them mean anything. diff --git a/README.md b/README.md index 77242d4..c4f5a3e 100644 --- a/README.md +++ b/README.md @@ -123,8 +123,7 @@ you see `done`. Sending them all at once does not queue them; the last one wins. ## Jogging -`jog` is a topic, not a service: a velocity command expires on the controller's own TTL, -so a sender keeps sending and nobody waits for an answer. +`jog` is a topic, not a service: a sender keeps sending and nobody waits for an answer. ```python node.send_output("jog", pa.array([{"velocity": [20.0, 0, 0, 0, 0, 0]}])) @@ -132,9 +131,18 @@ node.send_output("jog", pa.array([{"velocity": [20.0, 0, 0, 0, 0, 0]}])) It still drives the arm, so it behaves like a motion where that matters: the first non-zero jog preempts a motion in play and `robot_state` turns to `moving`. A zero -velocity stops it, and so does simply going quiet — the node returns to `ready` once -`VELOCITY_COMMAND_TTL_S` has passed, because that is when the controller has expired the -command. +velocity stops it, and so does simply going quiet — the node stops the arm and returns to +`ready` once `VELOCITY_COMMAND_TTL_S` has passed. + +**The velocity is set once and held.** The controller is given a velocity with no +duration, so it keeps it until a different one arrives: re-sending an unchanged velocity +does not reach the arm at all. Sending it repeatedly re-armed a timed segment and made the +controller re-plan the same motion, which on a real arm is felt as stepping rather than +travel — even at four messages a second. + +That also means **the controller no longer stops the arm on its own**, so +`VELOCITY_COMMAND_TTL_S` is enforced here instead. It is a real deadman now rather than a +description of one: a sender that dies mid-jog is what it exists for. ## Telemetry @@ -239,13 +247,13 @@ left where it stood. | `READY_TIMEOUT_S` | `3.0` | How long `recover` waits for a movable state | | `HOME_TIMEOUT_S` | `20.0` | How long homing may take | | `TOOL_SETTLE_S` | `2.0` | How long to wait for suction to build or fingers to close | -| `VELOCITY_COMMAND_TTL_S` | `0.5` | The controller's own expiry for one `jog` command | +| `VELOCITY_COMMAND_TTL_S` | `0.5` | How long a `jog` stream may go quiet before this node stops the arm | ## Development ```bash uv sync --group dev -uv run pytest # 86 tests, no robot needed +uv run pytest # 92 tests, no robot needed uv run ruff check . ``` diff --git a/dora-node.yml b/dora-node.yml index 4eaf428..3fe4675 100644 --- a/dora-node.yml +++ b/dora-node.yml @@ -49,9 +49,15 @@ inputs: description: >- A continuous velocity command: a one-row struct {velocity} of 6 numbers [vx, vy, vz, wx, wy, wz] in mm/s and deg/s. A topic rather than a service - because it expires on the controller's own TTL and a sender must keep - sending; nothing is published in answer. It drives the arm, so the first - non-zero one preempts a motion in play and robot_state turns to moving. + because a sender keeps sending and nothing is published in answer. It + drives the arm, so the first non-zero one preempts a motion in play and + robot_state turns to moving. + + The velocity is set on the controller once and held: an unchanged one is + not sent to the arm again, because re-arming it makes the controller + re-plan the same motion and the arm step rather than travel. A sender that + goes quiet for longer than VELOCITY_COMMAND_TTL_S is stopped by this node, + which is the deadman behind a sender that dies mid-jog. outputs: response: diff --git a/dora_ulite6/config.py b/dora_ulite6/config.py index c7da41b..75689a9 100644 --- a/dora_ulite6/config.py +++ b/dora_ulite6/config.py @@ -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 diff --git a/dora_ulite6/control.py b/dora_ulite6/control.py index f53dc7f..c6e19a3 100644 --- a/dora_ulite6/control.py +++ b/dora_ulite6/control.py @@ -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: diff --git a/dora_ulite6/driver.py b/dora_ulite6/driver.py index a5bcded..e89c1f1 100644 --- a/dora_ulite6/driver.py +++ b/dora_ulite6/driver.py @@ -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: diff --git a/tests/test_driver.py b/tests/test_driver.py new file mode 100644 index 0000000..20764b1 --- /dev/null +++ b/tests/test_driver.py @@ -0,0 +1,104 @@ +"""The driver's half of velocity control: what actually reaches the controller. + +`FakeArm` stands in for this whole class everywhere else, so nothing else in the +suite can see what the driver says to the SDK. That is the one thing worth +pinning here: a velocity is a set-point the controller holds, and how often it is +re-sent is the difference between an arm that travels and an arm that steps. +""" + +import pytest + +from dora_ulite6.config import Config +from dora_ulite6.driver import ULite6 + +MOVE = [20.0, 0.0, 0.0, 0.0, 0.0, 0.0] +BACK = [-20.0, 0.0, 0.0, 0.0, 0.0, 0.0] +STILL = [0.0] * 6 + + +class FakeSDK: + """Just enough XArmAPI to build a driver and watch it command a velocity.""" + + def __init__(self, *args, **kwargs): + self.connected = True + self.mode = None + self.velocities = [] + + def connect(self): + self.connected = True + + def disconnect(self): + self.connected = False + + def motion_enable(self, enable=True): + pass + + def set_mode(self, mode): + self.mode = mode + + def set_state(self, state): + pass + + def vc_set_cartesian_velocity(self, speeds, is_radian=None, is_tool_coord=False, duration=-1): + self.velocities.append((list(speeds), duration)) + return 0 + + +@pytest.fixture +def robot(monkeypatch): + """Return a driver over a fake SDK, with the calls it made on `arm`.""" + monkeypatch.setattr("dora_ulite6.driver.XArmAPI", FakeSDK) + return ULite6(Config()) + + +def commanded(robot): + """Return every velocity the driver put on the controller.""" + return [speeds for speeds, _ in robot.arm.velocities] + + +def test_a_velocity_is_never_given_a_duration(robot): + # `duration` makes the controller stop the arm on its own, which means every + # message re-arms a timed segment and the controller re-plans the same + # motion. The SDK reads 0 as "always effective, will not stop + # automatically", which is what lets one command stand. + robot.set_cartesian_velocity(MOVE) + + assert [duration for _, duration in robot.arm.velocities] == [0] + + +def test_a_held_velocity_reaches_the_controller_once(robot): + # The arm holds it, so saying it again says nothing new — and saying it + # again several times a second is what an operator feels as stepping. + for _ in range(5): + robot.set_cartesian_velocity(MOVE) + + assert commanded(robot) == [MOVE] + + +def test_a_changed_velocity_reaches_the_controller(robot): + # Sending once is worth nothing if a new direction is taken for the old one. + robot.set_cartesian_velocity(MOVE) + robot.set_cartesian_velocity(BACK) + + assert commanded(robot) == [MOVE, BACK] + + +def test_a_zero_velocity_stops_the_arm_and_leaves_velocity_mode(robot): + robot.set_cartesian_velocity(MOVE) + + robot.set_cartesian_velocity(STILL) + + assert commanded(robot) == [MOVE, STILL] + assert robot._velocity_mode is False + + +def test_the_same_velocity_is_sent_again_after_leaving_velocity_mode(robot): + # Leaving the mode clears what the controller was holding, so the velocity + # it had before means nothing on the way back in. Remembering it across the + # gap would leave the arm still while the operator held a direction. + robot.set_cartesian_velocity(MOVE) + robot.set_cartesian_velocity(STILL) + + robot.set_cartesian_velocity(MOVE) + + assert commanded(robot) == [MOVE, STILL, MOVE] diff --git a/tests/test_lifecycle.py b/tests/test_lifecycle.py index 5e50c83..e38c7ba 100644 --- a/tests/test_lifecycle.py +++ b/tests/test_lifecycle.py @@ -205,8 +205,7 @@ def test_a_zero_velocity_jog_does_not_make_a_running_motion_look_idle(monkeypatc def test_a_sender_that_goes_quiet_stops_being_called_jogging(monkeypatch): - # The controller expires the command on its own; nothing tells us, so the - # clock has to. + # Nothing tells us a sender died, so the clock has to. control = build(monkeypatch, motion=MotionConfig(velocity_command_ttl_s=0.02)) try: wait_for(lambda: READY in control.take_state_changes()) @@ -218,6 +217,22 @@ def test_a_sender_that_goes_quiet_stops_being_called_jogging(monkeypatch): control.shutdown() +def test_a_sender_that_goes_quiet_actually_stops_the_arm(monkeypatch): + # The velocity is held by the controller indefinitely, so the expiry is the + # deadman itself and not a note about one. Reporting `ready` without + # commanding the stop would leave the arm travelling while the node said it + # was idle, which is worse than not expiring it at all. + control = build(monkeypatch, motion=MotionConfig(velocity_command_ttl_s=0.02)) + try: + wait_for(lambda: READY in control.take_state_changes()) + control.start_jog([20.0, 0, 0, 0, 0, 0]) + wait_for(lambda: ("jog", [20.0, 0, 0, 0, 0, 0]) in control.arms[0].calls) + + wait_for(lambda: ("jog", [0.0] * 6) in control.arms[0].calls) + finally: + control.shutdown() + + def test_a_jog_preempts_a_motion_that_is_running(monkeypatch): control = build(monkeypatch) control.arms[0].block = threading.Event()