SE(3) transformations for SpatialForce and SpatialMomentum are wrong

Open Yuxiang-Ma opened this issue 10 months ago • 0 comments

Among SpatialVector subclasses, some multiplication are defined as:

Certain subtypes can be multiplied

===================   ====================  ===================  =========================
            Multiplicands                   Product
------------------------------------------  ----------------------------------------------
left                  right                 type                 operation
===================   ====================  ===================  =========================
SE3, Twist3           SpatialVelocity       SpatialVelocity      adjoint product
SE3, Twist3           SpatialAcceleration   SpatialAcceleration  adjoint product
SE3, Twist3           SpatialMomentum       SpatialMomentum      adjoint transpose product
SE3, Twist3           SpatialForce          SpatialForce         adjoint transpose product
SpatialAcceleration   SpatialInertia        SpatialForce         matrix-vector product**
SpatialVelocity       SpatialInertia        SpatialMomentum      matrix-vector product**
===================   ====================  ===================  =========================

For SpatialMomemtum and SpatialForce, the operation should be adjoint transpose invese product because of the duality of motion space and force space. As described in Featherstone's Rigid Body Dynamics Algorithms:

Mar 06 '25 20:03 Yuxiang-Ma