From 2e48076f11fd28b22cf7f9d86dbac313437b6751 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 15 Oct 2026 10:39:16 +1000 Subject: [PATCH] fix: Twist2.ad raised ValueError for every twist; add Twist2.Ad and SE2.Ad Twist2.ad was a copy of Twist3.ad that was never adapted to 2D: it called skew() on the scalar w and built 6x6 blocks, so it raised ValueError for any Twist2. It was also a property while Twist3.ad is a method, and its docstring example called unit(). Rewrite it as a method returning the 3x3 matrix of the Lie bracket, [[0, -w, vy], [w, 0, -vx], [0, 0, 0]] and return a list for a sequence, as skewa() does. Add Twist2.Ad() and SE2.Ad() (tr2adjoint2 existed but only as a base function) to match Twist3.Ad and SE3.Ad. Also correct the 2D-specific text in tr2adjoint2's docstring, which named tr2jac2 and said 6x6. Co-Authored-By: Claude Sonnet 5.5 --- spatialmath/base/transforms2d.py | 6 +-- spatialmath/pose2d.py | 26 ++++++++++++ spatialmath/twist.py | 69 ++++++++++++++++++++++++++------ tests/test_twist2_adjoint.py | 66 ++++++++++++++++++++++++++++++ 4 files changed, 151 insertions(+), 16 deletions(-) create mode 100644 tests/test_twist2_adjoint.py diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index 6aad235c..f431df2e 100644 --- a/spatialmath/base/transforms2d.py +++ b/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 diff --git a/spatialmath/pose2d.py b/spatialmath/pose2d.py index 62ac8510..f919ddff 100644 --- a/spatialmath/pose2d.py +++ b/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) diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 928235e4..d3433869 100644 --- a/spatialmath/twist.py +++ b/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): diff --git a/tests/test_twist2_adjoint.py b/tests/test_twist2_adjoint.py new file mode 100644 index 00000000..23413c44 --- /dev/null +++ b/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())