Merge PR #211: perf: compute Twist3.Ad() directly instead of via a throwaway SE3

Author: taughz

spatialmath/twist.py

@@ -903,12 +903,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):
         """

tests/test_twist.py

@@ -190,6 +190,32 @@ def test_pitch(self):
         tw = Twist3.UnitRevolute([0, 0, 1], [0, 0, 0], pitch=3)
         self.assertAlmostEqual(tw.pitch, 3.0)
 
+    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 *