fix(rne): propagate backward-pass force through the correct transform direction - #691
Merged
Merged
Conversation
… direction Robot.rne()'s backward recursion propagated force through Xup[j], the child<-parent motion transform, but force needs to go the other way (child->parent) and so needs the other direction's transform too. This was silently masked by a bug in spatialmath-python's SE3 * SpatialForce (adjoint transpose instead of the coadjoint, fixed in spatialmath-python 1.1.18 / rai-opensource/spatialmath-python#207) that happened to cancel it out for this specific case. With that upstream bug fixed, the direction mismatch here was exposed: URDF/ETS-based robots' rne() gave wrong torques for any joint with a translational offset from its parent (i.e. virtually all real robots), while DHRobot's compiled-C++ rne() path (which never touches spatialmath's SE3/SpatialForce classes) was unaffected throughout. Verified against the Puma560 DHRobot reference torques, the Spong textbook 2-link closed-form (test_invdyn), and the symbolic RNE path (test_symdyn, whose expected string is regenerated here -- Q1 is unchanged, Q0 differs only by an unsimplified sin^2+cos^2 term introduced by the added .inv()). Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
Codecov Report❌ Patch coverage is
Additional details and impacted files@@ Coverage Diff @@
## main #691 +/- ##
=====================================
Coverage 0.00% 0.00%
=====================================
Files 143 143
Lines 14082 14082
=====================================
Misses 14082 14082 ☔ View full report in Codecov by Harness. 🚀 New features to boost your workflow:
|
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 backward recursion (Robot.py) propagated force throughXup[j], which is the child<-parent motion transform (used in the forwardpass:
v_child = Xup[j] * v_parent). But force needs to propagate theother direction, child->parent, and needs the transform in that direction
instead.
This was masked by a bug in
spatialmath-python'sSE3 * SpatialForce,which used the adjoint transpose instead of the coadjoint (fixed upstream in
spatialmath-python 1.1.18) —
the old, incorrect formula happened to cancel out RTB's own direction
mismatch for this specific operation. With the upstream fix, that masking
went away:
Robot.rne()(used by all URDF/ETS-based robots) startedreturning wrong torques for any joint with a translational offset from its
parent — i.e. virtually every real robot.
DHRobot.rne()'s compiled C++path never touches spatialmath's
SE3/SpatialForceclasses, so it wasunaffected throughout — which is how this got isolated.
Fix
Propagate force through the un-inverted transform,
Xup[j].inv()(i.e. theparent<-child direction), matching the direction spatialmath's corrected
SE3 * SpatialForcenow expects.Verification
DHRobot/ Puma560 reference torques (compiled C++, independent ofspatialmath's Python classes) — unaffected throughout, used as ground
truth
test_invdyn— Spong textbook 2-link closed-form — now passes underspatialmath-python 1.1.18 (previously failed)
test_symdyn— symbolic RNE — expected string regenerated;Q[1]isbyte-identical to before,
Q[0]differs only by an unsimplifiedsin(q1)**2 + cos(q1)**2term introduced by the added.inv()(confirmed algebraically/numerically equivalent to the old expression)
test_fknm_fallback.py— now pass(
pytest tests/ --ignore=tests/test_blocks.py, spatialmath-python 1.1.18)Also bumps
pyproject.toml'sspatialmath-pythonfloor to>=1.1.18,since that's the first release where
SE3 * SpatialForceis correct.Test plan
🤖 Generated with Claude Code