Repository navigation
fix: judge arrival from the frame-to-frame position delta - #22
Merged
Merged
Conversation
_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.
|
🎉 This PR is included in version 1.1.1 🎉 The release is available on GitHub release Your semantic-release bot 📦🚀 |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
_arriveandcart._wait_settledboth required the reporteddqto stay underdq_tolforarrive_framesconsecutive frames. On real hardware that never happens atsome poses, so arrival waits ran the full
move_timeoutand raisedMotionTimeoutErrorwith the arm sitting on the target.
Observed with the joints provably frozen — 800 consecutive state frames with
bit-identical
q, position drift0.00000 rad— while one joint's reporteddqswungbetween 0.007 and 0.256 rad/s, over
dq_tol=0.1on 66% of frames:arrive_framesis 3, so the wait never completes. The same failure surfaced once intests/test_live.py::test_live_movej_tcp_ikas a 15 s timeout, and crashed three testscripts during this round.
dqis not a firmware computation:control_loop.c:512forwards the motor's ownvelocity report unfiltered (
st->dq = p->vel * direction), and a DM4310 reports it as a12-bit field, so the observed
0.01465 rad/squantum is60/4096— the motor'sresolution, not a defect here. The position stream is clean over the same window, so
both sites now compare consecutive frames instead.
Changes
_protocol.py— addSTATUS_HZ = 100.0, the state frame rate, for the conversionbelow (measured
get_state().hz = 99.99).arm.py—_arrive's stillness criterion reads the frame-to-frame position deltainstead of the reported
dq. Needs the previous frame, so the first frame registersonly; that costs one extra frame (~10 ms).
cart.py—_wait_settleduses the same criterion. Both docstrings already requiredthese two sites to be kept identical, so they move together.
dq_tol / STATUS_HZ=0.1 / 100= 0.001 rad/frame, which is theold
dqlimit 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_qasserted that a highdqmust block arrival. That is the premise the hardware measurement contradicts, soit is rewritten as
test_a_reported_dq_over_tolerance_does_not_block_settlingandasserts the opposite.
test_settle_err_rad_is_the_jitter_of_the_settling_framesused a 0.02 rad/framejitter — 2 rad/s, which should not count as settled. It now uses a jitter inside the
new threshold.
Testing
Both directions of the new criterion were checked against the arm:
movejin the live suite arrives)Known gap, stated plainly: the original fault (noisy
dqduring amovej) did notreproduce 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