Fix SpatialForce/SpatialMomentum SE(3) transform to use the coadjoint

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/SpatialMomentum were
transformed incorrectly for any pose with translation (correct only for a pure
rotation, where Ad is orthogonal).

For example T = SE3(1, 0, 0) acting on a unit z-force and a y-angular-velocity
breaks the invariance of instantaneous power (f . v), which must be
frame-independent: the current code gives 0 in one frame and 2 in the other.

Use np.linalg.inv(Ad).T in SpatialForce.__rmul__ and in the force/momentum
branch of the general SpatialVector.__rmul__. Motion (velocity/acceleration),
which correctly uses Ad, is unchanged.

Add a regression test to the previously-empty test_products stub asserting the
power invariance and the explicit transformed wrench/momentum.

Fixes #169.

Author: youdie006

spatialmath/spatialvector.py

@@ -287,7 +287,10 @@ def __rmul__(
             if isinstance(right, SpatialM6):
                 return right.__class__(X @ right.A)
             else:
-                return right.__class__(X.T @ right.A)
+                # Force/momentum are dual to motion and transform by the
+                # coadjoint (inverse-transpose of the adjoint), not the
+                # adjoint transpose. These are only equal for a pure rotation.
+                return right.__class__(np.linalg.inv(X).T @ right.A)
         else:
             raise TypeError("left operand of * must be SE3 or Twist3")
 
@@ -487,7 +490,9 @@ def __rmul__(
         right, left
     ):  # lgtm[py/not-named-self] pylint: disable=no-self-argument
         # Twist * SpatialForce -> SpatialForce
-        return SpatialForce(left.Ad().T @ right.A)
+        # A force is dual to motion, so it transforms by the coadjoint
+        # (inverse-transpose of the adjoint), not the adjoint transpose.
+        return SpatialForce(np.linalg.inv(left.Ad()).T @ right.A)
 
 
 # ------------------------------------------------------------------------- #

tests/test_spatialvector.py

@@ -228,6 +228,29 @@ def test_products(self):
         # twist x v, twist x a, twist x F
         pass
 
+    def test_spatial_transform_duality(self):
+        # A pose transform acts on motion (velocity/acceleration) by the
+        # adjoint Ad, and on force/momentum by the coadjoint Ad^-T, because
+        # force space is dual to motion space (Featherstone, "Rigid Body
+        # Dynamics Algorithms"). With a nonzero translation the SE(3) adjoint
+        # is not orthogonal, so Ad^-T differs from the adjoint transpose Ad^T.
+        T = SE3(1, 0, 0)
+        # ordering is [v(0:3); omega(3:6)] (see SE3.Ad docstring)
+        v = SpatialVelocity([0, 0, 0, 0, 1, 0])
+        f = SpatialForce([0, 0, 1, 0, 0, 0])
+        v2, f2 = T * v, T * f
+
+        # Instantaneous power f . v is frame-invariant; this is the self-oracle
+        # and is independent of the [v; omega] vs [omega; v] convention.
+        nt.assert_almost_equal(f2.A @ v2.A, f.A @ v.A)
+
+        # Explicit transformed wrench: the moment component flips sign.
+        nt.assert_almost_equal(f2.A, np.r_[0, 0, 1, 0, -1, 0])
+
+        # SpatialMomentum exercises the general SpatialVector.__rmul__ path.
+        m2 = T * SpatialMomentum([0, 0, 1, 0, 0, 0])
+        nt.assert_almost_equal(m2.A, np.r_[0, 0, 1, 0, -1, 0])
+
 
 # ---------------------------------------------------------------------------------------#
 if __name__ == "__main__":