liria/dora-ulite6
4
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Projects Releases 5 Wiki Activity

fix zero-velocity jog reporting a running motion as idle

Browse Source 
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:
cristhian
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
Download Patch File Download Diff File Expand all files Collapse all files
dora_ulite6/control.py
+25 -6
View File
@@ -232,10 +232,13 @@ class Control:
self._preempt_motions("jog")
self._set_state(MOVING)
with self._lock:
self._jogging = moving
was_jogging, self._jogging = self._jogging, moving
self._last_jog = time.monotonic()
if not moving:
self._set_state(READY)
# Only a jog that was in fact jogging ends one. A zero velocity from a
# 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))
def start_motion(self, call: ServiceCall) -> None:
@@ -403,8 +406,7 @@ class Control:
self._finish_motion(call, PREEMPTED, False, f"preempted by {preempted_by}")
else:
self._finish_motion(call, DONE if ok else FAILED, ok, fields.get("message", ""), fields)
if self._state == MOVING:
self._set_state(READY)
self._idle_unless_busy()
def _leave_jog(self) -> bool:
"""Clear the jogging flag, reporting whether the arm was in fact jogging."""
@@ -515,6 +517,23 @@ class Control:
self._state_changes.append(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 ---
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:
return
self._jogging = False
self._set_state(READY)
self._idle_unless_busy()
def _run_reporter(self) -> None:
"""Publish telemetry, whatever the control thread is doing.