fix: preserve small rotations in quaternion logarithms

spatialmath/quaternion.py

@@ -381,6 +381,9 @@ def log(self) -> Quaternion:
 
              \ln \| q \|,  \langle \frac{\vec{v}}{\| \vec{v} \|} \cos^{-1} \frac{s}{\| q \|} \rangle
 
+        The implementation uses the equivalent :math:`\operatorname{atan2}(\|\vec{v}\|, s)`
+        to avoid the loss of precision in ``acos`` for small rotation angles.
+
         For a ``UnitQuaternion`` the logarithm is a pure quaternion whose vector
         part :math:`\vec{v}` and :math:`\vec{v}/2` is a Euler vector: parallel
         to the axis of rotation and whose norm is the magnitude of rotation.
@@ -403,20 +406,17 @@ def log(self) -> Quaternion:
         norm = self.norm()
         s = np.log(norm)
         if len(self) == 1:
-            if smb.iszerovec(self._A[1:4]):
+            n = math.hypot(*self._A[1:4])
+            if n == 0:
                 v = np.zeros((3,))
             else:
-                v = math.acos(np.clip(self._A[0] / norm, -1, 1)) * smb.unitvec(
-                    self._A[1:4]
-                )
+                v = math.atan2(n, self._A[0]) * self._A[1:4] / n
             return Quaternion(s=s, v=v)
         else:
-            v = [
-                np.zeros((3,))
-                if smb.iszerovec(A[1:4])
-                else math.acos(np.clip(A[0] / n, -1, 1)) * smb.unitvec(A[1:4])
-                for A, n in zip(self._A, norm)
-            ]
+            v = []
+            for A in self._A:
+                n = math.hypot(*A[1:4])
+                v.append(np.zeros((3,)) if n == 0 else math.atan2(n, A[0]) * A[1:4] / n)
             return Quaternion(s=s, v=np.array(v))
 
     def exp(self, tol: float = 20) -> Quaternion:

tests/test_quaternion.py

@@ -943,6 +943,60 @@ def test_log(self):
         self.assertIsInstance(qq, UnitQuaternion)
         nt.assert_array_almost_equal(qq.vec, np.r_[1, 0, 0, 0])
 
+    def test_log_small_rotation(self):
+        axis = np.array([1.0, -2.0, 3.0]) / math.sqrt(14)
+        for angle in (1e-12, -1e-9, 1e-7, 1e-4, 0.3):
+            for scale in (1.0, 3.0, 1e-6):
+                with self.subTest(angle=angle, scale=scale):
+                    values = scale * np.r_[cos(angle / 2), sin(angle / 2) * axis]
+                    q = UnitQuaternion(values) if scale == 1 else Quaternion(values)
+                    original = q.vec.copy()
+                    qlog = q.log()
+                    nt.assert_allclose(qlog.v, angle / 2 * axis, rtol=1e-13, atol=0)
+                    self.assertAlmostEqual(qlog.s, math.log(scale))
+                    nt.assert_allclose(qlog.exp().vec, q.vec, rtol=1e-13, atol=0)
+                    nt.assert_array_equal(q.vec, original)
+
+    def test_log_small_rotation_sequence(self):
+        angles = np.array([1e-12, -1e-9, 1e-7, 0.3])
+        axes = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 0, 0]])
+        values = np.column_stack(
+            (np.cos(angles / 2), np.sin(angles[:, None] / 2) * axes)
+        )
+        expected = angles[:, None] / 2 * axes
+        for scales in (
+            np.ones(4),
+            np.array([2.0, 3.0, 4.0, 5.0]),
+            1e-6 * np.ones(4),
+        ):
+            with self.subTest(scales=scales):
+                scaled = scales[:, None] * values
+                q = (
+                    UnitQuaternion(scaled)
+                    if np.all(scales == 1)
+                    else Quaternion(list(scaled))
+                )
+                original = q.vec.copy()
+                qlog = q.log()
+                self.assertEqual(len(qlog), len(q))
+                nt.assert_allclose(qlog.v, expected, rtol=1e-13, atol=0)
+                nt.assert_allclose(qlog.s, np.log(scales), atol=1e-15)
+                for i in range(len(q)):
+                    nt.assert_allclose(qlog[i].exp().vec, scaled[i], rtol=1e-13, atol=0)
+                    nt.assert_allclose(qlog[i].vec, q[i].log().vec, rtol=1e-13, atol=0)
+                nt.assert_array_equal(q.vec, original)
+
+    def test_log_near_negative_real(self):
+        # Keep the principal quaternion branch near pi, rather than folding
+        # a negative scalar part onto the small-angle branch.
+        angle = 1e-7
+        q = Quaternion([-cos(angle), sin(angle), 0, 0])
+        expected = np.r_[0, pi - angle, 0, 0]
+        nt.assert_allclose(q.log().vec, expected, rtol=0, atol=1e-15)
+        nt.assert_allclose(
+            Quaternion([q, q]).log().vec, [expected, expected], rtol=0, atol=1e-15
+        )
+
     def test_concat(self):
         u = Quaternion()
         uu = Quaternion([u, u, u, u])