Preserve scaled quaternion rotations in logarithms

Author: dddd-jh

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,21 +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:
-                # atan2 retains small angles when s / norm rounds to one.
-                v = math.atan2(smb.norm(self._A[1:4]), self._A[0]) * 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.atan2(smb.norm(A[1:4]), A[0]) * smb.unitvec(A[1:4])
-                for A in self._A
-            ]
+            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

@@ -946,7 +946,7 @@ def test_log(self):
     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):
+            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)
@@ -964,7 +964,11 @@ def test_log_small_rotation_sequence(self):
             (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])):
+        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 = (