fix(rne): attribute a sandwiched static link's mass to its preceding joint, not the following one - #693
Merged
Conversation
…joint, not the following one Robot.rne()'s link-grouping attached a static (fixed) link to the first joint encountered scanning *forward* through the link list -- correct for a trailing run of static links after the last joint (#636), but wrong for a static link sandwiched between two joints: it's rigidly welded to the *preceding* joint's output, not the following one, so its mass was being misattributed to the wrong joint's torque. Restructures the grouping to walk each static link's .parent chain up to its nearest joint ancestor (correct under branching too, not just a flat list scan), which also lets group[0] be unconditionally the joint -- simplifying the forward/backward recursion's joint lookups and the kinematic-transform construction that #636's fix had to special-case. test_invdyn_static2's previous expected values were themselves wrong, copied from test_invdyn's different topology (where the second link *does* rotate with the second joint) rather than derived for its own (where the fixed link's position is independent of the second joint's angle, so that joint's torque, Coriolis terms, and inertia-matrix row/column must all be exactly zero) -- so the bug was being actively protected by a passing test rather than caught by one. Rederived and verified against an independent virtual-work (finite-difference of CoM height vs. q) ground truth, not just against rne() itself. Also adds a dedicated regression test (test_invdyn_sandwiched_static_link, +inertia variant) isolating the exact scenario from #483. Fixes #483. Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
Merged
Codecov Report❌ Patch coverage is
Additional details and impacted files@@ Coverage Diff @@
## main #693 +/- ##
=====================================
Coverage 0.00% 0.00%
=====================================
Files 143 143
Lines 14103 14091 -12
=====================================
+ Misses 14103 14091 -12 ☔ View full report in Codecov by Harness. 🚀 New features to boost your workflow:
|
petercorke
added a commit
that referenced
this pull request
Sep 19, 2026
…ed static link (#696) Follow-up to #693: a static link with no joint ancestor at all (rigidly mounted directly on the immovable base, e.g. URDF Panda's panda_link0) is correctly dropped from link_groups -- it contributes no joint torque. But the joint immediately downstream of it still needs to resolve its own *kinematic* parent group for velocity/acceleration propagation, and doing that via a raw list-membership search over link_groups broke once that static link stopped being a member of any group: IndexError on any robot with the same shape as the default Panda(), caught while smoke- testing the previous fix against real models rather than just synthetic test topologies. Reuses group_of_link_idx (already built once for #693's grouping) to resolve a joint's nearest joint ancestor directly, treating "no entry" (no joint ancestor upstream) the same as .parent being None outright, rather than crashing. Co-authored-by: Claude Sonnet 5 <noreply@anthropic.com>
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.
Problem
Robot.rne()'s link-grouping attached a static (fixed) link to the firstjoint encountered scanning forward through the link list. Correct for a
trailing run of static links after the last joint (#636), but wrong for a
static link sandwiched between two joints (
joint_A -> static -> joint_B):it's rigidly welded to
joint_A's output, notjoint_B's, so its mass wasbeing misattributed to the wrong joint's torque.
Found while reviewing old third-party issues -- #483 proposed a fix for
exactly this, filed against the pre-#636 code. Verified the underlying
problem is still real on current
main(after both #636/#684 and theseparate force-propagation-direction fix in #691) using an independent
virtual-work (finite-difference of CoM height vs. q) ground truth, not just
against
rne()itself.test_invdyn_static2's previous expected values were themselves wrong --copied from
test_invdyn's different topology (where the second linkdoes rotate with the second joint) rather than derived for its own. So
this bug was being actively protected by a passing test, not caught by one.
Fix
Restructures the grouping to walk each static link's
.parentchain up toits nearest joint ancestor (correct under branching too, not just a flat
list scan). This also makes
group[0]unconditionally the joint, whichsimplifies the forward/backward recursion's joint lookups and the
kinematic-transform construction that #636's fix had to special-case for a
joint that "might not be the last link in the group."
Rederives
test_invdyn_static2's gravity/velocity/inertial-termassertions for its actual topology (joint2's torque, Coriolis terms, and
inertia-matrix row/column must all be exactly zero -- nothing with mass is
ever downstream of it), and adds a dedicated regression test
(
test_invdyn_sandwiched_static_link, +inertia variant) isolating theexact scenario from #483.
Fixes #483.
Verification
Robot.pyfix (keeping the correctedtests) and confirmed all 3 new/updated tests fail against the old code,
with exactly the misattributed-torque symptom; restored the fix and
confirmed they pass.
(
pytest tests/ --ignore=tests/test_blocks.py)test_invdyn_trailing_static_link/_with_inertia(RNE/inertia() silently wrong (not crashing) for URDF robots with fixed/structural joints #636's own regressiontests) still pass -- confirms the restructuring doesn't reintroduce that
bug.
🤖 Generated with Claude Code