[youdie006]

Fixes #169.

Problem

A spatial force/wrench is dual to spatial motion, so under an SE(3) transform it must map by the coadjoint Ad^-T (inverse-transpose of the adjoint), not the adjoint transpose Ad^T. Because the SE(3) adjoint is non-orthogonal whenever the translation is nonzero, Ad^T != Ad^-T, so SpatialForce and SpatialMomentum were transformed incorrectly for any pose with translation (correct only for a pure rotation, where Ad is orthogonal). Velocity/acceleration (motion), which correctly use Ad, were fine.

Repro / self-oracle

Instantaneous power f . v is frame-invariant, so it must be preserved by the transform:

from spatialmath import SE3, SpatialVelocity, SpatialForce
import numpy as np

T = SE3(1, 0, 0)                       # pure +x translation
v = SpatialVelocity([0, 0, 0, 0, 1, 0])
f = SpatialForce([0, 0, 1, 0, 0, 0])

v2, f2 = T * v, T * f
print(f.A @ v.A, f2.A @ v2.A)          # should be equal
# before: 0.0 2.0   <- power not invariant (bug)
# after:  0.0 0.0   <- invariant

The transformed wrench's moment component came out with the wrong sign; f2 should be [0, 0, 1, 0, -1, 0] but was [0, 0, 1, 0, 1, 0].

Fix

Use np.linalg.inv(Ad).T (the coadjoint) in SpatialForce.__rmul__ and in the force/momentum branch of the general SpatialVector.__rmul__ (which SpatialMomentum uses). The motion branch is unchanged.

Test

The test_products stub in tests/test_spatialvector.py was empty, so these transforms were untested. Adds test_spatial_transform_duality asserting the power invariance (f2 . v2 == f . v, convention-independent) plus the explicit transformed wrench and momentum. Red before the fix (power 2 != 0), green after; the existing motion tests are unaffected.

Reported by @Yuxiang-Ma, who cited Featherstone's Rigid Body Dynamics Algorithms.

---

This change was prepared with AI assistance and reviewed by me before submission.

[codecov-commenter]

⚠️ Please install the to ensure uploads and comments are reliably processed by Codecov.

Codecov Report

✅ All modified and coverable lines are covered by tests.

📢 Thoughts on this report? Let us know!

[petercorke]

A modest (8%) speed increase if you compute adjoint of inv(T).

[petercorke]

Thanks for picking this up. Clear description, and a clear fix. 2 minor things which you might want to tweak:

• fix the table lines 274-293 to match your change
• consider the speedup in the code comment

[petercorke]

Nice catch, and a well-constructed fix. Verified the physics independently rather than just checking your one case: instantaneous power f·v must be frame-invariant, so I ran it across 500 random SE3.Rand() transforms (not just the pure-translation case above) — before this fix that invariant is violated, after it the worst error over all 500 is 8.9e-16, i.e. exact to machine precision. Full test suite passes too (344 tests, no regressions).

Good instinct using the power invariant as the test oracle rather than a hand-computed expected value — that's the right way to pin this down, and citing Featherstone directly makes the reasoning easy to check. Appreciate the disclosure on AI assistance + your own review too.

Left two non-blocking notes inline — a docs accuracy nit (the class docstring's own table still says "adjoint transpose product" for Force/Momentum, which is now stale) and an optional computation-path suggestion for the coadjoint. Neither affects correctness of what's here.

Approving — this is good to merge as-is; the inline notes are take-it-or-leave-it.

[youdie006]

Thanks for verifying it independently across 500 random poses rather than just re-running my one case — the frame-invariance check over the full SE(3) group is a stronger statement than anything I put in the PR, and it is good to see the residual land at machine precision.

I have taken both inline notes.

The docstring nit was a real hole and I am glad you caught it: the stale wording was in three places, not one. Both the module-level and class-level multiplicand tables listed the Force/Momentum rows as "adjoint transpose product", and the __rmul__ prose said the vector is premultiplied by "the adjoint of X or adjoint transpose of X". All three now say coadjoint, with the __rmul__ prose naming it as the inverse-transpose of the adjoint so the reader does not have to infer it.

On the computation path, you were right and my first measurement was not. A crude timing loop said your suggestion was slower; a proper timeit (5000 iterations, best of 7) says it is faster, in line with your 8 percent:

inv(T.Ad()).T                      21.66 us/call   1.00x
T.inv().Ad().T                     20.72 us/call   1.05x
tr2adjoint(trinv(T.A)).T           17.14 us/call   1.26x

I took left.inv().Ad().T. The third row goes further by skipping the SE3 construction entirely, but it reaches into spatialmath.base from a class that otherwise stays on the public API, and I did not think the extra 20 percent was worth that. Mentioning it in case you disagree.

Before switching I confirmed the two expressions agree: max absolute difference 1.3e-15 over 500 SE3.Rand() and 1.8e-15 over 200 Twist3, i.e. identical to floating point. Ad(X)^-1 == Ad(X^-1) is the reason, and I put that in the comment so the next reader does not have to rediscover it.

Re-ran tests/test_spatialvector.py, test_twist.py, test_pose3d.py and test_baseposelist.py after the change: 79 passed. I did not re-run the whole 344 locally, since the symbolic modules are slow here and CI covers them.

Disclosure: I use AI assistance in my work, and I review and verify everything before it goes out. The measurements above are my own.

[petercorke]

Thank you for your interest in SMTB, and for this fix! Confirmed and merged -- the coadjoint transform for SpatialForce/SpatialMomentum is corrected. Appreciate the contribution.