Merge pull request bdaiinstitute#86 from bdaiinstitute/bokorn-bdaii/fix_log_domain

myeatman-bdai · web-flow · commit a07e74dadd09 · 2023-10-30T11:01:44.000-04:00
Fixed domain errors in acos near identity
diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py
@@ -1376,7 +1376,9 @@ def trlog(
                 return skew(w * theta)
         else:
             # general case
-            theta = math.acos((np.trace(R) - 1) / 2)
+            tr = (np.trace(R) - 1) / 2
+            # min for inaccuracies near identity yielding trace > 3
+            theta = math.acos(min(tr, 1.0))
             st = math.sin(theta)
             if st == 0:
                 if twist:
diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py
@@ -701,6 +701,18 @@ def test_functions_vect(self):
         # .T
         pass
 
+    def test_functions_lie(self):
+        R = SO3.EulerVec([0.42, 0.73, -1.17])
+
+        # Check log and exponential map
+        nt.assert_equal(R, SO3.Exp(R.log()))
+        np.testing.assert_equal((R.inv() * R).log(), np.zeros([3, 3]))
+
+        # Check euler vector map
+        nt.assert_equal(R, SO3.EulerVec(R.eulervec()))
+        np.testing.assert_equal((R.inv() * R).eulervec(), np.zeros(3))
+
+
 # ============================== SE3 =====================================#
 
 class TestSE3(unittest.TestCase):
