fix code for .ad method

petercorke · petercorke · commit d7a60a604a9b · 2020-09-07T18:08:07.000+10:00
diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py
@@ -260,8 +260,10 @@ def Ad(self):
         :seealso: Twist.ad.
 
         """
-
-        return self.R
+        return np.array([
+            [self.R, tr.skew(self.t) @ self.R],
+            [np.zeros((3, 3)), self.R]
+            ])
 # ------------------------------------------------------------------------ #
 
     @staticmethod
