From 3580f02d391e4feb78c801d7797eb091f1029463 Mon Sep 17 00:00:00 2001 From: Jinhang Dong <186017830+dddd-jh@users.noreply.github.com> Date: Tue, 6 Oct 2026 14:58:32 +0800 Subject: [PATCH] fix: preserve small rotations in quaternion logarithms --- spatialmath/quaternion.py | 7 +++--- tests/test_quaternion.py | 50 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 54 insertions(+), 3 deletions(-) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index a5759709..cfc769f0 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -406,7 +406,8 @@ def log(self) -> Quaternion: if smb.iszerovec(self._A[1:4]): v = np.zeros((3,)) else: - v = math.acos(np.clip(self._A[0] / norm, -1, 1)) * smb.unitvec( + # 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] ) return Quaternion(s=s, v=v) @@ -414,8 +415,8 @@ def log(self) -> Quaternion: 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) + else math.atan2(smb.norm(A[1:4]), A[0]) * smb.unitvec(A[1:4]) + for A in self._A ] return Quaternion(s=s, v=np.array(v)) diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 8a87a433..c55398b9 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -943,6 +943,56 @@ 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): + 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])): + 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])