Skip to content

fix: judge arrival from the frame-to-frame position delta - #22

Merged
X-F-R merged 1 commit into
mainfrom
fix/arrival-criterion-position-based
Oct 10, 2026
Merged

X-F-R merged 1 commit into
mainfrom
fix/arrival-criterion-position-based

Conversation

@X-F-R

@X-F-R X-F-R commented Oct 10, 2026

Copy link
Copy Markdown
Contributor

Summary

_arrive and cart._wait_settled both required the reported dq to stay under
dq_tol for arrive_frames consecutive frames. On real hardware that never happens at
some poses, so arrival waits ran the full move_timeout and raised MotionTimeoutError
with the arm sitting on the target.

Observed with the joints provably frozen — 800 consecutive state frames with
bit-identical q, position drift 0.00000 rad — while one joint's reported dq swung
between 0.007 and 0.256 rad/s, over dq_tol=0.1 on 66% of frames:

t(s)     seq  max|q-tgt|   max|dq|  pos_ok  vel_ok  n_ok
0.20      21     0.00265   0.19780    True   False     0
0.22      23     0.00265   0.00733    True    True     1
0.24      25     0.00265   0.21245    True   False     0     <- never reaches 3 in a row
0.26      27     0.00265   0.19780    True   False     0
0.28      29     0.00265   0.00733    True    True     1

arrive_frames is 3, so the wait never completes. The same failure surfaced once in
tests/test_live.py::test_live_movej_tcp_ik as a 15 s timeout, and crashed three test
scripts during this round.

dq is not a firmware computation: control_loop.c:512 forwards the motor's own
velocity report unfiltered (st->dq = p->vel * direction), and a DM4310 reports it as a
12-bit field, so the observed 0.01465 rad/s quantum is 60/4096 — the motor's
resolution, not a defect here. The position stream is clean over the same window, so
both sites now compare consecutive frames instead.

Changes

  • _protocol.py — add STATUS_HZ = 100.0, the state frame rate, for the conversion
    below (measured get_state().hz = 99.99).
  • arm.py — _arrive's stillness criterion reads the frame-to-frame position delta
    instead of the reported dq. Needs the previous frame, so the first frame registers
    only; that costs one extra frame (~10 ms).
  • cart.py — _wait_settled uses the same criterion. Both docstrings already required
    these two sites to be kept identical, so they move together.
  • The threshold is dq_tol / STATUS_HZ = 0.1 / 100 = 0.001 rad/frame, which is the
    old dq limit expressed as a displacement per frame, so the strictness is unchanged.
    The position quantum (0.383 mrad/frame) resolves this comfortably.
  • tests/test_cart_wait.py — two tests pinned the old criterion:
    • test_bit10_falling_to_zero_is_not_enough_without_a_static_q asserted that a high
      dq must block arrival. That is the premise the hardware measurement contradicts, so
      it is rewritten as test_a_reported_dq_over_tolerance_does_not_block_settling and
      asserts the opposite.
    • test_settle_err_rad_is_the_jitter_of_the_settling_frames used a 0.02 rad/frame
      jitter — 2 rad/s, which should not count as settled. It now uses a jitter inside the
      new threshold.

Testing

$ python3 -m pytest -q
657 passed, 2 skipped

$ LITEARM_LIVE=1 python3 -m pytest -q
659 passed

Both directions of the new criterion were checked against the arm:

  • positive — 800 stationary frames, zero drift: judged settled
  • negative — a moving arm: not judged settled (every movej in the live suite arrives)

Known gap, stated plainly: the original fault (noisy dq during a movej) did not
reproduce again afterwards — 300 s / 30024 frames of watching at the pose that had shown
it came back clean after a board reset — so that exact end-to-end combination could not
be re-verified. The two directions above and the rewritten offline tests are what the
change rests on.

Issues

Closes #20

_arrive and cart._wait_settled both required the reported dq to stay under
dq_tol for arrive_frames consecutive frames. On real hardware that never
happens at some poses: with the joints provably frozen -- 800 frames of
bit-identical q, zero drift -- one joint's reported dq still swung between
0.007 and 0.256 rad/s, over dq_tol on 66% of frames. Arrival waits then ran
the full move_timeout and raised MotionTimeoutError with the arm sitting on
the target (0.0026 rad off, well inside q_tol).

dq is not a firmware computation: control_loop.c:512 forwards the motor's
own velocity report unfiltered, and a DM4310 reports it as a 12-bit field,
so the 0.01465 rad/s quantum is the motor's resolution rather than a defect
here. The position stream is clean, so both sites now compare consecutive
frames instead. The threshold is dq_tol / STATUS_HZ -- the same limit
expressed as a displacement per frame -- so the strictness is unchanged.

Two tests pinned the old criterion. One asserted that a high dq must block
arrival; that is the premise hardware contradicts, so it is rewritten to
assert the opposite. The other used a 0.02 rad/frame jitter, which is
2 rad/s and should not count as settled; it now uses a jitter inside the
new threshold.
@X-F-R
X-F-R merged commit 4384526 into main Oct 10, 2026
2 checks passed
@X-F-R
X-F-R deleted the fix/arrival-criterion-position-based branch October 10, 2026 09:57
@github-actions

Copy link
Copy Markdown

🎉 This PR is included in version 1.1.1 🎉

The release is available on GitHub release

Your semantic-release bot 📦🚀

@github-actions github-actions Bot added the released Shipped in a release label Oct 10, 2026
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

released Shipped in a release

Projects

None yet

Development

Successfully merging this pull request may close these issues.

Judge arrival from the position stream, not the reported dq

2 participants