Skip to content

Commit 13cef32

Browse files
petercorkeclaude
andauthored
fix: Twist2.ad raised ValueError for every twist; add Twist2.Ad and SE2.Ad (#259)
## Problem `Twist2.ad` raises `ValueError: argument must be a 1- or 3-vector` for every twist, and always has. It is a copy of `Twist3.ad` that was never adapted to 2D: it calls `skew()` on the scalar `w` and builds 6x6 blocks, where the 2D result is 3x3. Nothing in the repo calls it and no test covered it. It also differs from its 3D sibling in other ways: - It is a `@property`, while `Twist3.ad` is a method and both docstrings say `S.ad()`. - Its docstring example calls `S.unit()`, and its see-also points at `SE3.Ad`. - `Twist2` has no `Ad()`, and neither does `SE2`. `tr2adjoint2` exists, but only as a base function, so the `ad`/`Ad` pair that `Twist3` and `SE3` have was half missing in 2D. ## Changes - **`Twist2.ad()`** is now a method returning the 3x3 matrix of the Lie bracket, `[[0, -ω, v_y], [ω, 0, -v_x], [0, 0, 0]]`. For a sequence it returns a list, as `skewa()` does. - **`Twist2.Ad()`** is new and mirrors `Twist3.Ad()`: the adjoint of the equivalent SE(2) pose. - **`SE2.Ad()`** is new and mirrors `SE3.Ad()`. - **`tr2adjoint2` docstring:** its text named `tr2jac2` and said 6x6. Now it names `tr2adjoint2` and gives 3x3 (1x1 for SO(2)). ## Verification The new `tests/test_twist2_adjoint.py` checks against independent oracles, not against the implementation: - `ad(S) @ T` equals the commutator of the augmented skew matrices, `vee([S, T])`. - Antisymmetry: `[S, S] = 0` and `ad(S) T = -ad(T) S`. - `expm(ad(S))` equals `Ad(exp(S))`. - `SE2.Ad() @ S` equals `vee(T [S] T^-1)`. - `Ad(T1 T2) = Ad(T1) Ad(T2)`, and `Ad` of the identity is `I`. - Sequence handling. All six fail on `master` and pass here. Full suite: 386 passed, 3 skipped. Black is clean. The branch merges cleanly with #225, #248, #249, #258 and the integration hub #246. ## Not changed - `tr2adjoint` and `tr2adjoint2` both have a `.. math:` directive with a single colon (so it renders as a comment) and unbalanced braces in the formula. I left those alone because the 3D function shares them and fixing the math needs a check of the Sphinx macros. - `Twist3.ad()` silently uses the first element of a sequence. `Twist2.ad()` returns a list, so the two now differ for sequences. I'd harmonize them separately. 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-authored-by: Claude Sonnet 5.5 <noreply@anthropic.com>
1 parent 9b83ba0 commit 13cef32

4 files changed

Lines changed: 151 additions & 16 deletions

File tree

‎spatialmath/base/transforms2d.py‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -796,9 +796,9 @@ def tr2adjoint2(T):
796796
797797
where :math:`\mat{T} \in \SE2`.
798798
799-
``tr2jac2(T)`` is an adjoint matrix (6x6) that maps spatial velocity or
800-
differential motion between frame {B} to frame {A} which are attached to the
801-
same moving body. The pose of {B} relative to {A} is represented by the
799+
``tr2adjoint2(T)`` is an adjoint matrix (3x3 for SE(2), 1x1 for SO(2)) that
800+
maps spatial velocity or differential motion between frame {B} to frame {A}
801+
which are attached to the same moving body. The pose of {B} relative to {A} is represented by the
802802
homogeneous transform T = :math:`{}^A {\bf T}_B`.
803803
804804
.. runblock:: pycon

‎spatialmath/pose2d.py‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -609,6 +609,32 @@ def inv(self):
609609
else:
610610
return SE2([smb.rt2tr(x.R.T, -x.R.T @ x.t) for x in self], check=False)
611611

612+
def Ad(self):
613+
r"""
614+
Adjoint of SE(2)
615+
616+
:return: adjoint matrix
617+
:rtype: ndarray(3,3)
618+
619+
``SE2.Ad`` is the 3x3 adjoint matrix
620+
621+
If the twist or spatial velocity :math:`\nu = (v_x, v_y, \omega)^T`
622+
and the SE(2) represents the pose of {B} relative to {A},
623+
ie. :math:`{}^A {\bf T}_B`, and the adjoint is :math:`\mathbf{A}` then
624+
:math:`{}^{A}\!\nu = \mathbf{A} {}^{B}\!\nu`.
625+
626+
Example:
627+
628+
.. runblock:: pycon
629+
630+
>>> from spatialmath import SE2
631+
>>> SE2(1, 2, 0.3).Ad()
632+
633+
:seealso: :meth:`spatialmath.pose3d.SE3.Ad`, :meth:`spatialmath.twist.Twist2.ad`,
634+
:func:`~spatialmath.base.transforms2d.tr2adjoint2`
635+
"""
636+
return smb.tr2adjoint2(self.A)
637+
612638
def SE3(self, z=0):
613639
"""
614640
Create SE(3) from SE(2)

‎spatialmath/twist.py‎

Lines changed: 56 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1641,13 +1641,26 @@ def unit(self):
16411641
# general twist: normalize so |w| == 1 (w is a scalar for Twist2)
16421642
return Twist2(self.S / abs(self.w))
16431643

1644-
@property
16451644
def ad(self):
1646-
"""
1647-
Twist2.ad Logarithm of adjoint
1645+
r"""
1646+
Logarithm of adjoint of 2D twist
1647+
1648+
:return: logarithm of adjoint matrix
1649+
:rtype: ndarray(3,3), or list of ndarray if the twist is a sequence
1650+
1651+
``S.ad()`` is the 3x3 matrix representation of the Lie bracket with
1652+
the twist ``S``, such that for twists ``S`` and ``T``
16481653
1649-
- ``S.ad()`` is the logarithm of the adjoint matrix of the corresponding
1650-
homogeneous transformation.
1654+
.. math::
1655+
1656+
\mathrm{ad}(S) \vec{T} = \vee \left( [S] [T] - [T] [S] \right)
1657+
1658+
where :math:`[S]` is the augmented skew-symmetric matrix of the twist.
1659+
For :math:`S = (v_x, v_y, \omega)` it is
1660+
1661+
.. math::
1662+
1663+
\begin{pmatrix} 0 & -\omega & v_y \\ \omega & 0 & -v_x \\ 0 & 0 & 0 \end{pmatrix}
16511664
16521665
Example:
16531666
@@ -1656,16 +1669,46 @@ def ad(self):
16561669
>>> from spatialmath import SE2, Twist2
16571670
>>> T = SE2(1, 2, 0.3)
16581671
>>> S = Twist2(T)
1659-
>>> S.unit()
1672+
>>> S.ad()
1673+
1674+
.. note:: An alternative approach to computing the adjoint is to
1675+
exponentiate this 3x3 matrix.
16601676
1661-
:seealso: SE3.Ad.
1677+
:seealso: :func:`Twist2.Ad`
16621678
"""
1663-
return np.array(
1664-
[
1665-
[smb.skew(self.w), smb.skew(self.v)],
1666-
[np.zeros((3, 3)), smb.skew(self.w)],
1667-
]
1668-
)
1679+
if len(self) == 1:
1680+
v, w = self.v, self.w
1681+
return np.array([[0, -w, v[1]], [w, 0, -v[0]], [0, 0, 0]])
1682+
else:
1683+
return [x.ad() for x in self]
1684+
1685+
def Ad(self):
1686+
"""
1687+
Adjoint of 2D twist
1688+
1689+
:return: adjoint matrix
1690+
:rtype: ndarray(3,3)
1691+
1692+
``S.Ad()`` is the 3x3 adjoint matrix of the corresponding
1693+
homogeneous transformation.
1694+
1695+
For a twist representing motion from frame {B} to {A}, the adjoint will
1696+
transform a twist relative to frame {A} to one relative to frame {B}.
1697+
1698+
Example:
1699+
1700+
.. runblock:: pycon
1701+
1702+
>>> from spatialmath import Twist2
1703+
>>> S = Twist2([1, 2, 0.3])
1704+
>>> S.Ad()
1705+
1706+
.. note:: This method computes the equivalent SE(2) matrix, then the adjoint
1707+
of that.
1708+
1709+
:seealso: :func:`Twist2.ad`, :func:`Twist2.SE2`, :func:`Twist2.exp`
1710+
"""
1711+
return self.SE2().Ad()
16691712

16701713
@classmethod
16711714
def Tx(cls, x):

‎tests/test_twist2_adjoint.py‎

Lines changed: 66 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
import numpy as np
2+
import numpy.testing as nt
3+
import pytest
4+
from scipy.linalg import expm
5+
6+
from spatialmath import SE2, Twist2
7+
from spatialmath.base import skewa, vexa
8+
9+
10+
def _bracket(a, b):
11+
"""Lie bracket of two se(2) vectors, via their matrix commutator."""
12+
A, B = skewa(a), skewa(b)
13+
return vexa(A @ B - B @ A)
14+
15+
16+
@pytest.fixture
17+
def twists():
18+
rng = np.random.default_rng(7)
19+
return rng.normal(size=3), rng.normal(size=3)
20+
21+
22+
def test_ad_is_lie_bracket(twists):
23+
"""ad(S) @ T is the Lie bracket [S, T]."""
24+
a, b = twists
25+
ad = Twist2(a).ad()
26+
assert ad.shape == (3, 3)
27+
nt.assert_allclose(ad @ b, _bracket(a, b), atol=1e-14)
28+
29+
30+
def test_ad_antisymmetry(twists):
31+
"""[S, S] = 0, and ad(S) T = -ad(T) S."""
32+
a, b = twists
33+
nt.assert_allclose(Twist2(a).ad() @ a, 0, atol=1e-14)
34+
nt.assert_allclose(Twist2(a).ad() @ b, -Twist2(b).ad() @ a, atol=1e-14)
35+
36+
37+
def test_exp_of_ad_is_Ad(twists):
38+
"""The adjoint of exp(S) is the exponential of ad(S)."""
39+
a, _ = twists
40+
S = Twist2(a)
41+
nt.assert_allclose(S.Ad(), expm(S.ad()), atol=1e-12)
42+
nt.assert_allclose(S.Ad(), S.SE2().Ad(), atol=1e-14)
43+
44+
45+
def test_SE2_Ad_maps_twists(twists):
46+
"""Ad(T) S is vee(T [S] T^-1)."""
47+
a, _ = twists
48+
T = SE2(1.5, -0.7, 0.9)
49+
expected = vexa(T.A @ skewa(a) @ T.inv().A)
50+
nt.assert_allclose(T.Ad() @ a, expected, atol=1e-14)
51+
52+
53+
def test_SE2_Ad_identity_and_composition():
54+
"""Ad is a homomorphism: Ad(T1 T2) = Ad(T1) Ad(T2)."""
55+
nt.assert_allclose(SE2().Ad(), np.eye(3), atol=1e-14)
56+
T1, T2 = SE2(1, 2, 0.3), SE2(-0.5, 0.25, -1.1)
57+
nt.assert_allclose((T1 * T2).Ad(), T1.Ad() @ T2.Ad(), atol=1e-14)
58+
59+
60+
def test_ad_sequence():
61+
"""A sequence of twists gives a list of matrices, as for skewa()."""
62+
seq = Twist2([np.array([1.0, 2.0, 0.3]), np.array([0.0, 1.0, -0.4])])
63+
ads = seq.ad()
64+
assert isinstance(ads, list) and len(ads) == 2
65+
for m, t in zip(ads, seq):
66+
nt.assert_allclose(m, t.ad())

0 commit comments

Comments
 (0)