fix zero-velocity jog reporting a running motion as idle

start_jog restored READY for any zero velocity, without checking whether a
motion owned the arm. A teleop source streaming zero-velocity ticks during a
move published moving -> ready mid-motion, and the closing guard in _run_motion
then saw READY rather than MOVING and never republished, so a caller watching
robot_state concluded the motion had finished early.

Route every "the arm has stopped" transition through _idle_unless_busy, which
reports READY only when no motion is running, no jog is driving the arm, and the
state has not already left MOVING for unavailable or recovering.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
2026-09-15 12:32:17 -05:00
co-authored by Claude Opus 5
parent 412aeac955
commit 608e22078e
2 changed files with 48 additions and 6 deletions
+25 -6
View File
@@ -232,10 +232,13 @@ class Control:
self._preempt_motions("jog") self._preempt_motions("jog")
self._set_state(MOVING) self._set_state(MOVING)
with self._lock: with self._lock:
self._jogging = moving was_jogging, self._jogging = self._jogging, moving
self._last_jog = time.monotonic() self._last_jog = time.monotonic()
if not moving: # Only a jog that was in fact jogging ends one. A zero velocity from a
self._set_state(READY) # teleop source that is merely idling says nothing about a motion that
# owns the arm, and must not answer for it.
if not moving and was_jogging:
self._idle_unless_busy()
self._jobs.put(Jog(velocity=velocity)) self._jobs.put(Jog(velocity=velocity))
def start_motion(self, call: ServiceCall) -> None: def start_motion(self, call: ServiceCall) -> None:
@@ -403,8 +406,7 @@ class Control:
self._finish_motion(call, PREEMPTED, False, f"preempted by {preempted_by}") self._finish_motion(call, PREEMPTED, False, f"preempted by {preempted_by}")
else: else:
self._finish_motion(call, DONE if ok else FAILED, ok, fields.get("message", ""), fields) self._finish_motion(call, DONE if ok else FAILED, ok, fields.get("message", ""), fields)
if self._state == MOVING: self._idle_unless_busy()
self._set_state(READY)
def _leave_jog(self) -> bool: def _leave_jog(self) -> bool:
"""Clear the jogging flag, reporting whether the arm was in fact jogging.""" """Clear the jogging flag, reporting whether the arm was in fact jogging."""
@@ -515,6 +517,23 @@ class Control:
self._state_changes.append(state) self._state_changes.append(state)
log(f"state: {state}") log(f"state: {state}")
def _idle_unless_busy(self) -> None:
"""Report the arm idle, but only when nothing is still driving it.
One thing ending is not the arm stopping. A motion may still be winding
down inside the SDK after the jog that preempted it went quiet; a jog may
have taken the arm over from the motion that just finished. Either way
`ready` published over it is the false idle the state exists to rule out.
The state also stops being ours to set once it has left `moving` — that
is `unavailable` or `recovering` reporting something this does not know
about, and it must not be overwritten with `ready`.
"""
with self._lock:
if self._running_motion is not None or self._jogging or self._state != MOVING:
return
self._set_state(READY)
# --- the reporter thread --- # --- the reporter thread ---
def _expire_jog(self) -> None: def _expire_jog(self) -> None:
@@ -528,7 +547,7 @@ class Control:
if not self._jogging or time.monotonic() - self._last_jog < self.config.motion.velocity_command_ttl_s: if not self._jogging or time.monotonic() - self._last_jog < self.config.motion.velocity_command_ttl_s:
return return
self._jogging = False self._jogging = False
self._set_state(READY) self._idle_unless_busy()
def _run_reporter(self) -> None: def _run_reporter(self) -> None:
"""Publish telemetry, whatever the control thread is doing. """Publish telemetry, whatever the control thread is doing.
+23
View File
@@ -130,6 +130,29 @@ def test_a_zero_velocity_jog_puts_the_arm_back_to_ready(monkeypatch):
control.shutdown() control.shutdown()
def test_a_zero_velocity_jog_does_not_make_a_running_motion_look_idle(monkeypatch):
# A teleop source streaming zero velocity says nothing about a motion that
# owns the arm, and must not answer for it: an arm that is travelling must
# never look idle, and a `ready` here would also stop the motion's own end
# from ever being published.
control = build(monkeypatch)
control.arms[0].block = threading.Event()
try:
wait_for(lambda: READY in control.take_state_changes())
control.submit(ServiceCall(request_id="m1", row={"service": "home"}))
wait_for(lambda: MOVING in control.take_state_changes())
for _ in range(5):
control.start_jog([0.0] * 6)
assert control.take_state_changes() == []
control.arms[0].block.set()
wait_for(lambda: READY in control.take_state_changes())
finally:
control.shutdown()
def test_a_sender_that_goes_quiet_stops_being_called_jogging(monkeypatch): 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 # The controller expires the command on its own; nothing tells us, so the
# clock has to. # clock has to.