Repository navigation
kinematics: measure ToF hit range from the trunk axis - #441
Open
Amidwestnoob wants to merge 2 commits into
Open
Amidwestnoob wants to merge 2 commits into
Amidwestnoob wants to merge 2 commits into
Conversation
Zone::Hit documents range as the horizontal distance from the trunk origin's vertical axis, but project() computed it from the sensor, so it came out short by the sensor's forward offset (~8 cm with a level head). Compute it from the gravity-levelled point instead. The short-range noise band stays judged from the sensor, since the crosstalk is the sensor's own. Regression tests check range against the levelled point's horizontal norm for a level head, a walking head pose and a leaned trunk. Fixes pollen-robotics#430
Keep the trunk-axis geometry fix and replace its permissive sanity window with a mount-specific range check. Exercise all zones under head yaw/roll and trunk tilt using distance to the gravity line, independent of the levelling quaternion. Pin the sensor-relative noise boundary and untrusted-gravity fallback. Assisted-by: GPT-6 Astra
This branch has not been deployed
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.
Fixes #430.
Zone::Hit::rangenow measures horizontal distance from the trunk origin's vertical axis, as documented. It uses the horizontal norm of the gravity-levelled hit point, including the sensor's offset from the trunk. Points remain in the trunk frame, and the short-range noise filter still measures horizontal distance from the sensor; floor classification is unchanged.Regression tests cover level and walking head poses, trunk tilt, head yaw/roll, every grid zone, and the sensor-relative noise threshold. An independent distance-to-gravity-axis oracle checks the range without reusing the levelling rotation. The one-metre sanity check now rejects the old sensor-relative result.
Validation on Rust 1.89.0:
cargo test -p kinematics --locked: 37 passed, two existing performance probes ignored.cargo fmt -p kinematics -- --checkandcargo clippy -p kinematics --all-targets --locked -- -D warnings: passed.The existing MuJoCo FK fixture comparison passed. Live stand/drive simulation was not rerun because the simulator checkout and Python environment were unavailable; no hardware testing was performed. Full workspace CI remains to run on this PR.