Skip to content

Fix SpatialForce/SpatialMomentum SE(3) transform to use the coadjoint (Ad^-T) #207

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
petercorke merged 6 commits into from
Sep 16, 2026
Merged

Fix SpatialForce/SpatialMomentum SE(3) transform to use the coadjoint (Ad^-T) #207

Changes from 1 commit
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
a319c45
Fix SpatialForce/SpatialMomentum SE(3) transform to use the coadjoint
youdie006 Aug 20, 2026
67f5413
Address review: refresh stale docstrings and take the cheaper coadjoi…
youdie006 Aug 25, 2026
444e126
Merge branch 'master' into fix/169-spatial-force-coadjoint
youdie006 Aug 31, 2026
f4ef2ba
Merge branch 'master' into fix/169-spatial-force-coadjoint
youdie006 Sep 1, 2026
97e2798
Merge branch 'master' into fix/169-spatial-force-coadjoint
youdie006 Sep 14, 2026
a3e9f7c
Merge branch 'master' into fix/169-spatial-force-coadjoint
petercorke Sep 16, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Address review: refresh stale docstrings and take the cheaper coadjoi… 
…nt path

The class and module tables still described the SpatialForce/SpatialMomentum
product as an "adjoint transpose product", which the previous commit made
inaccurate; they now say coadjoint, and the __rmul__ docstring names it as the
inverse-transpose of the adjoint.

Ad(X)^-1 == Ad(X^-1), so the coadjoint is now built from the adjoint of the
inverse pose rather than by inverting the 6x6 adjoint. Measured identical to
the previous expression to 1.3e-15 over 500 random SE3.Rand() and 200 Twist3
samples, and modestly faster.
  • Loading branch information
@youdie006
youdie006 committed Aug 25, 2026
commit 67f54139fffbaba2d5f94125cf5f861ba3472d73
24 changes: 14 additions & 10 deletions spatialmath/spatialvector.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,8 @@ class SpatialVector(BasePoseList):
=================== ==================== =================== =========================
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
SE3, Twist3 SpatialMomentum SpatialMomentum coadjoint product
SE3, Twist3 SpatialForce SpatialForce coadjoint product
SpatialAcceleration SpatialInertia SpatialForce matrix-vector product**
SpatialVelocity SpatialInertia SpatialMomentum matrix-vector product**
=================== ==================== =================== =========================
Expand Down Expand Up @@ -268,8 +268,9 @@ def __rmul__(

``X * S`` transforms the spatial vector ``S`` by the relative pose ``X``
which may be either an ``SE3`` or ``Twist3`` instance. The spatial
vector is premultiplied by the adjoint of ``X`` or adjoint transpose
of ``X`` depending on the SpatialVector subclass of ``S``.
vector is premultiplied by the adjoint of ``X``, or by the coadjoint
(the inverse-transpose of the adjoint) of ``X``, depending on the
SpatialVector subclass of ``S``.

=========== ==================== =================== =========================
Multiplicands Product
Expand All @@ -278,19 +279,20 @@ def __rmul__(
=========== ==================== =================== =========================
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
SE3, Twist3 SpatialMomentum SpatialMomentum coadjoint product
SE3, Twist3 SpatialForce SpatialForce coadjoint product
=========== ==================== =================== =========================
"""
if isinstance(left, (SE3, Twist3)):
X = left.Ad()
if isinstance(right, SpatialM6):
return right.__class__(X @ right.A)
return right.__class__(left.Ad() @ right.A)
else:
# 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)
# Ad(X)^-1 == Ad(X^-1), so take the adjoint of the inverse pose
# rather than inverting the 6x6 adjoint.
return right.__class__(left.inv().Ad().T @ right.A)
else:
raise TypeError("left operand of * must be SE3 or Twist3")

Expand Down Expand Up @@ -492,7 +494,9 @@ def __rmul__(
# Twist * SpatialForce -> SpatialForce
# 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)
# Ad(X)^-1 == Ad(X^-1), so take the adjoint of the inverse pose
# rather than inverting the 6x6 adjoint.
return SpatialForce(left.inv().Ad().T @ right.A)


# ------------------------------------------------------------------------- #
Expand Down