Bounds on acos usages; fix a couple of links in readme (rai-opensource#108)

jcao-bdai · web-flow · commit 423b781794ba · 2023-12-05T15:49:45.000-05:00
diff --git a/README.md b/README.md
@@ -277,8 +277,8 @@ t =        1,        2,        3; rpy/zyx =       30,        0,        0 deg
 
 For more detail checkout the shipped Python notebooks:
 
-* [gentle introduction](https://github.com/bdaiinstitute/spatialmath-python/blob/master/spatialmath/gentle-introduction.ipynb)
-* [deeper introduction](https://github.com/bdaiinstitute/spatialmath-python/blob/master/spatialmath/introduction.ipynb)
+* [gentle introduction](https://github.com/bdaiinstitute/spatialmath-python/blob/master/notebooks/gentle-introduction.ipynb)
+* [deeper introduction](https://github.com/bdaiinstitute/spatialmath-python/blob/master/notebooks/introduction.ipynb)
 
 
 You can browse it statically through the links above, or clone the toolbox and run them interactively using [Jupyter](https://jupyter.org) or [JupyterLab](https://jupyter.org).
diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py
@@ -393,7 +393,7 @@ def log(self) -> Quaternion:
         """
         norm = self.norm()
         s = math.log(norm)
-        v = math.acos(self.s / norm) * smb.unitvec(self.v)
+        v = math.acos(np.clip(self.s / norm, -1, 1)) * smb.unitvec(self.v)
         return Quaternion(s=s, v=v)
 
     def exp(self, tol: float = 20) -> Quaternion:
@@ -2242,9 +2242,9 @@ def angdist(self, other: UnitQuaternion, metric: Optional[int] = 3) -> float:
         if metric == 0:
             measure = lambda p, q: 1 - abs(np.dot(p, q))
         elif metric == 1:
-            measure = lambda p, q: math.acos(abs(np.dot(p, q)))
+            measure = lambda p, q: math.acos(min(1.0, abs(np.dot(p, q))))
         elif metric == 2:
-            measure = lambda p, q: math.acos(abs(np.dot(p, q)))
+            measure = lambda p, q: math.acos(min(1.0, abs(np.dot(p, q))))
         elif metric == 3:
 
             def metric3(p, q):
@@ -2257,7 +2257,7 @@ def metric3(p, q):
 
             measure = metric3
         elif metric == 4:
-            measure = lambda p, q: math.acos(2 * np.dot(p, q) ** 2 - 1)
+            measure = lambda p, q: math.acos(min(1.0, 2 * np.dot(p, q) ** 2 - 1))
 
         ad = self.binop(other, measure)
         if len(ad) == 1:
diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py
@@ -497,8 +497,16 @@ def test_divide(self):
 
     def test_angle(self):
         # angle between quaternions
-        # pure
-        v = [5, 6, 7]
+        uq1 = UnitQuaternion.Rx(0.1)
+        uq2 = UnitQuaternion.Ry(0.1)
+        for metric in range(5):
+            self.assertEqual(uq1.angdist(other=uq1, metric=metric), 0.0)
+            self.assertEqual(uq2.angdist(other=uq2, metric=metric), 0.0)
+            self.assertEqual(
+                uq1.angdist(other=uq2, metric=metric),
+                uq2.angdist(other=uq1, metric=metric),
+            )
+            self.assertTrue(uq1.angdist(other=uq2, metric=metric) > 0)
 
     def test_conversions(self):
         # , 3 angle
@@ -793,8 +801,15 @@ def log_test_exp(self):
         nt.assert_array_almost_equal(exp(log(q1)), q1)
         nt.assert_array_almost_equal(exp(log(q2)), q2)
 
-        # nt.assert_array_almost_equal(log(exp(q1)), q1)
-        # nt.assert_array_almost_equal(log(exp(q2)), q2)
+    def test_log(self):
+        q1 = Quaternion([4, 3, 2, 1])
+        q2 = Quaternion([-1, 2, -3, 4])
+
+        self.assertTrue(isscalar(q1.log().s))
+        self.assertTrue(isvector(q1.log().v, 3))
+
+        nt.assert_array_almost_equal(q1.log().exp(), q1)
+        nt.assert_array_almost_equal(q2.log().exp(), q2)
 
     def test_concat(self):
         u = Quaternion()
