diff --git a/spatialmath/twist.py b/spatialmath/twist.py index dcefa840..ea01aa0f 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -887,12 +887,13 @@ def Ad(self): >>> S = Twist3.Rx(0.3) >>> S.Ad() - .. note:: This method computes the equivalent SE(3) matrix, then the adjoint - of that. + .. note:: Equivalent to, but faster than, ``self.SE3().Ad()`` -- computes + the adjoint directly from the twist's exponential without + constructing an intermediate ``SE3`` instance. :seealso: :func:`Twist3.ad`, :func:`Twist3.SE3`, :func:`Twist3.exp` """ - return self.SE3().Ad() + return smb.tr2adjoint(smb.trexp(self.S, check=False)) def skewa(self): """ diff --git a/tests/test_twist.py b/tests/test_twist.py index 70f237a8..75f9549c 100755 --- a/tests/test_twist.py +++ b/tests/test_twist.py @@ -179,6 +179,32 @@ def test_exp(self): tw = Twist3.UnitRevolute([0, 0, 1], [0, 0, 0]) array_compare(tw.exp(pi / 2), SE3.Rz(pi / 2)) + def test_Ad(self): + # pure rotation: Ad is block-diagonal [[R,0],[0,R]], no translation + # coupling -- hand-verified ground truth, not derived from Ad() itself. + R = SE3.Rx(0.4).R + S = Twist3(SE3.Rx(0.4)) + expected = np.zeros((6, 6)) + expected[:3, :3] = R + expected[3:, 3:] = R + nt.assert_almost_equal(S.Ad(), expected) + + # pure translation: Ad couples translation into the top-right block + # via skew(t), identity rotation blocks -- also hand-verified. + t = np.r_[1, 2, 3] + S = Twist3(SE3(t)) + expected = np.eye(6) + expected[:3, 3:] = skew(t) + nt.assert_almost_equal(S.Ad(), expected) + + # general case: cross-check against SE3.Ad(), computed from the same + # twist's own exponential, for several random transforms. + for _ in range(20): + T = SE3.Rand() + S = Twist3(T) + nt.assert_almost_equal(S.Ad(), S.SE3().Ad()) + nt.assert_almost_equal(S.Ad(), T.Ad()) + def test_arith(self): # check overloaded *