Skip to content

fix(rne): attribute a sandwiched static link's mass to its preceding joint, not the following one - #693

Merged
petercorke merged 1 commit into
mainfrom
fix/rne-sandwiched-static-link-mass
Sep 19, 2026
Merged

petercorke merged 1 commit into
mainfrom
fix/rne-sandwiched-static-link-mass

Conversation

@petercorke

Copy link
Copy Markdown
Owner

Problem

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 (joint_A -> static -> joint_B):
it's rigidly welded to joint_A's output, not joint_B's, so its mass was
being 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 the
separate 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 link
does 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 .parent chain up to
its nearest joint ancestor (correct under branching too, not just a flat
list scan). This also makes group[0] unconditionally the joint, which
simplifies 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-term
assertions 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 the
exact scenario from #483.

Fixes #483.

Verification

  • Red/green: reverted just the Robot.py fix (keeping the corrected
    tests) 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.
  • Full suite: 744 passed, 0 failed, 82 skipped
    (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 regression
    tests) still pass -- confirms the restructuring doesn't reintroduce that
    bug.

🤖 Generated with Claude Code

…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>
@petercorke
petercorke merged commit bebede0 into main Sep 19, 2026
21 checks passed
@petercorke
petercorke deleted the fix/rne-sandwiched-static-link-mass branch September 19, 2026 12:32
@github-actions github-actions Bot mentioned this pull request Sep 19, 2026
@codecov

codecov Bot commented Sep 19, 2026

Copy link
Copy Markdown

Codecov Report

❌ Patch coverage is 0% with 15 lines in your changes missing coverage. Please review.
✅ Project coverage is 0.00%. Comparing base (a0ef01c) to head (e4fb84a).
⚠️ Report is 1 commits behind head on main.

Files with missing lines Patch % Lines
src/roboticstoolbox/robot/Robot.py 0.00% 15 Missing ⚠️
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.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

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>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

Proposed fixes for RNE with fixed joints give incorrect results

1 participant