add Python code for log of SO(2) and SE(2) rather than use scipy

Author: petercorke

spatialmath/base/transforms2d.py

@@ -374,31 +374,42 @@ def trlog2(T, check=True, twist=False):
         >>> trlog2(rot2(0.3))
         >>> trlog2(rot2(0.3), twist=True)
 
-    :seealso: :func:`~trexp`, :func:`~spatialmath.base.transformsNd.vex`,
-              :func:`~spatialmath.base.transformsNd.vexa`
+    :seealso: :func:`~trexp`, :func:`~spatialmath.smb.transformsNd.vex`,
+              :func:`~spatialmath.smb.transformsNd.vexa`
     """
 
     if ishom2(T, check=check):
         # SE(2) matrix
 
-        if base.iseye(T):
+        if smb.iseye(T):
             # is identity matrix
             if twist:
                 return np.zeros((3,))
             else:
                 return np.zeros((3, 3))
         else:
+            st = T[1,0]
+            ct = T[0,0]
+            theta = math.atan(st / ct)
+
+            V = np.array([[st, -(1-ct)], [1-ct, st]])
+            tr = (np.linalg.inv(V) @ T[:2, 2]) * theta
+            print(tr)
             if twist:
-                return base.vexa(scipy.linalg.logm(T))
+                return np.array([tr, theta])
             else:
-                return scipy.linalg.logm(T)
+                return np.block([
+                    [smb.skew(theta), tr[:, np.newaxis]], 
+                    [np.zeros((1,3))]
+                ])
 
     elif isrot2(T, check=check):
         # SO(2) rotation matrix
+        theta = math.atan(T[1,0] / T[0,0])
         if twist:
-            return base.vex(scipy.linalg.logm(T))
+            return theta
         else:
-            return scipy.linalg.logm(T)
+            return smb.skew(theta)
     else:
         raise ValueError("Expect SO(2) or SE(2) matrix")
 

tests/base/test_transforms2d.py

@@ -15,7 +15,7 @@
 from scipy.linalg import logm, expm
 
 from spatialmath.base.transforms2d import *
-from spatialmath.base.transformsNd import isR, t2r, r2t, rt2tr
+from spatialmath.base.transformsNd import isR, t2r, r2t, rt2tr, skew
 
 import matplotlib.pyplot as plt
 
@@ -66,6 +66,20 @@ def test_Rt(self):
         nt.assert_array_almost_equal(transl2(T), np.array(t))
         # TODO
 
+    def test_trlog2(self):
+        R = rot2(0.5)
+        nt.assert_array_almost_equal(trlog2(R), skew(0.5))
+
+        T = transl2(1, 2) @ trot2(0.5)
+        nt.assert_array_almost_equal(logm(T), trlog2(T))
+
+    def test_trexp2(self):
+        R = trexp2(skew(0.5))
+        nt.assert_array_almost_equal(R, rot2(0.5))
+
+        T = transl2(1, 2) @ trot2(0.5)
+        nt.assert_array_almost_equal(trexp2(logm(T)), T)
+
     def test_transl2(self):
         nt.assert_array_almost_equal(
             transl2(1, 2), np.array([[1, 0, 1], [0, 1, 2], [0, 0, 1]])