fix: Twist2.ad raised ValueError for every twist; add Twist2.Ad and SE2.Ad

spatialmath/base/transforms2d.py

@@ -796,9 +796,9 @@ def tr2adjoint2(T):
 
     where :math:`\mat{T} \in \SE2`.
 
-    ``tr2jac2(T)`` is an adjoint matrix (6x6) that maps spatial velocity or
-    differential motion between frame {B} to frame {A} which are attached to the
-    same moving body.  The pose of {B} relative to {A} is represented by the
+    ``tr2adjoint2(T)`` is an adjoint matrix (3x3 for SE(2), 1x1 for SO(2)) that
+    maps spatial velocity or differential motion between frame {B} to frame {A}
+    which are attached to the same moving body.  The pose of {B} relative to {A} is represented by the
     homogeneous transform T = :math:`{}^A {\bf T}_B`.
 
     .. runblock:: pycon

spatialmath/pose2d.py

@@ -609,6 +609,32 @@ def inv(self):
         else:
             return SE2([smb.rt2tr(x.R.T, -x.R.T @ x.t) for x in self], check=False)
 
+    def Ad(self):
+        r"""
+        Adjoint of SE(2)
+
+        :return: adjoint matrix
+        :rtype: ndarray(3,3)
+
+        ``SE2.Ad`` is the 3x3 adjoint matrix
+
+        If the twist or spatial velocity :math:`\nu = (v_x, v_y, \omega)^T`
+        and the SE(2) represents the pose of {B} relative to {A},
+        ie. :math:`{}^A {\bf T}_B`, and the adjoint is :math:`\mathbf{A}` then
+        :math:`{}^{A}\!\nu = \mathbf{A} {}^{B}\!\nu`.
+
+        Example:
+
+        .. runblock:: pycon
+
+            >>> from spatialmath import SE2
+            >>> SE2(1, 2, 0.3).Ad()
+
+        :seealso: :meth:`spatialmath.pose3d.SE3.Ad`, :meth:`spatialmath.twist.Twist2.ad`,
+            :func:`~spatialmath.base.transforms2d.tr2adjoint2`
+        """
+        return smb.tr2adjoint2(self.A)
+
     def SE3(self, z=0):
         """
         Create SE(3) from SE(2)

spatialmath/twist.py

@@ -1633,13 +1633,26 @@ def unit(self):
             # prismatic twist
             return Twist2(smb.unitvec(self.v), [0, 0, 0])
 
-    @property
     def ad(self):
-        """
-        Twist2.ad Logarithm of adjoint
+        r"""
+        Logarithm of adjoint of 2D twist
+
+        :return: logarithm of adjoint matrix
+        :rtype: ndarray(3,3), or list of ndarray if the twist is a sequence
+
+        ``S.ad()`` is the 3x3 matrix representation of the Lie bracket with
+        the twist ``S``, such that for twists ``S`` and ``T``
 
-        - ``S.ad()`` is the logarithm of the adjoint matrix of the corresponding
-          homogeneous transformation.
+        .. math::
+
+            \mathrm{ad}(S) \vec{T} = \vee \left( [S] [T] - [T] [S] \right)
+
+        where :math:`[S]` is the augmented skew-symmetric matrix of the twist.
+        For :math:`S = (v_x, v_y, \omega)` it is
+
+        .. math::
+
+            \begin{pmatrix} 0 & -\omega & v_y \\ \omega & 0 & -v_x \\ 0 & 0 & 0 \end{pmatrix}
 
         Example:
 
@@ -1648,16 +1661,46 @@ def ad(self):
             >>> from spatialmath import SE2, Twist2
             >>> T = SE2(1, 2, 0.3)
             >>> S = Twist2(T)
-            >>> S.unit()
+            >>> S.ad()
+
+        .. note:: An alternative approach to computing the adjoint is to
+            exponentiate this 3x3 matrix.
 
-        :seealso: SE3.Ad.
+        :seealso: :func:`Twist2.Ad`
         """
-        return np.array(
-            [
-                [smb.skew(self.w), smb.skew(self.v)],
-                [np.zeros((3, 3)), smb.skew(self.w)],
-            ]
-        )
+        if len(self) == 1:
+            v, w = self.v, self.w
+            return np.array([[0, -w, v[1]], [w, 0, -v[0]], [0, 0, 0]])
+        else:
+            return [x.ad() for x in self]
+
+    def Ad(self):
+        """
+        Adjoint of 2D twist
+
+        :return: adjoint matrix
+        :rtype: ndarray(3,3)
+
+        ``S.Ad()`` is the 3x3 adjoint matrix of the corresponding
+        homogeneous transformation.
+
+        For a twist representing motion from frame {B} to {A}, the adjoint will
+        transform a twist relative to frame {A} to one relative to frame {B}.
+
+        Example:
+
+        .. runblock:: pycon
+
+            >>> from spatialmath import Twist2
+            >>> S = Twist2([1, 2, 0.3])
+            >>> S.Ad()
+
+        .. note:: This method computes the equivalent SE(2) matrix, then the adjoint
+            of that.
+
+        :seealso: :func:`Twist2.ad`, :func:`Twist2.SE2`, :func:`Twist2.exp`
+        """
+        return self.SE2().Ad()
 
     @classmethod
     def Tx(cls, x):

tests/test_twist2_adjoint.py

@@ -0,0 +1,66 @@
+import numpy as np
+import numpy.testing as nt
+import pytest
+from scipy.linalg import expm
+
+from spatialmath import SE2, Twist2
+from spatialmath.base import skewa, vexa
+
+
+def _bracket(a, b):
+    """Lie bracket of two se(2) vectors, via their matrix commutator."""
+    A, B = skewa(a), skewa(b)
+    return vexa(A @ B - B @ A)
+
+
+@pytest.fixture
+def twists():
+    rng = np.random.default_rng(7)
+    return rng.normal(size=3), rng.normal(size=3)
+
+
+def test_ad_is_lie_bracket(twists):
+    """ad(S) @ T is the Lie bracket [S, T]."""
+    a, b = twists
+    ad = Twist2(a).ad()
+    assert ad.shape == (3, 3)
+    nt.assert_allclose(ad @ b, _bracket(a, b), atol=1e-14)
+
+
+def test_ad_antisymmetry(twists):
+    """[S, S] = 0, and ad(S) T = -ad(T) S."""
+    a, b = twists
+    nt.assert_allclose(Twist2(a).ad() @ a, 0, atol=1e-14)
+    nt.assert_allclose(Twist2(a).ad() @ b, -Twist2(b).ad() @ a, atol=1e-14)
+
+
+def test_exp_of_ad_is_Ad(twists):
+    """The adjoint of exp(S) is the exponential of ad(S)."""
+    a, _ = twists
+    S = Twist2(a)
+    nt.assert_allclose(S.Ad(), expm(S.ad()), atol=1e-12)
+    nt.assert_allclose(S.Ad(), S.SE2().Ad(), atol=1e-14)
+
+
+def test_SE2_Ad_maps_twists(twists):
+    """Ad(T) S is vee(T [S] T^-1)."""
+    a, _ = twists
+    T = SE2(1.5, -0.7, 0.9)
+    expected = vexa(T.A @ skewa(a) @ T.inv().A)
+    nt.assert_allclose(T.Ad() @ a, expected, atol=1e-14)
+
+
+def test_SE2_Ad_identity_and_composition():
+    """Ad is a homomorphism: Ad(T1 T2) = Ad(T1) Ad(T2)."""
+    nt.assert_allclose(SE2().Ad(), np.eye(3), atol=1e-14)
+    T1, T2 = SE2(1, 2, 0.3), SE2(-0.5, 0.25, -1.1)
+    nt.assert_allclose((T1 * T2).Ad(), T1.Ad() @ T2.Ad(), atol=1e-14)
+
+
+def test_ad_sequence():
+    """A sequence of twists gives a list of matrices, as for skewa()."""
+    seq = Twist2([np.array([1.0, 2.0, 0.3]), np.array([0.0, 1.0, -0.4])])
+    ads = seq.ad()
+    assert isinstance(ads, list) and len(ads) == 2
+    for m, t in zip(ads, seq):
+        nt.assert_allclose(m, t.ad())