From 4d4196db632621ddbc814304085311d8c1c7eb08 Mon Sep 17 00:00:00 2001 From: Vincent Gao Date: Sun, 26 Jul 2026 17:03:40 +0200 Subject: [PATCH 1/3] Fix quaternion slerp when the endpoints are the same rotation The slerp weights sin((1-s)t)/sin(t) and sin(s.t)/sin(t) are singular wherever sin(t) vanishes, ie. at t=0 (coincident endpoints) and t=pi (antipodal endpoints, which are the same rotation under the double cover). qslerp() guarded only t=0; UnitQuaternion.interp() and .interp1() re-derived the weights inline and guarded neither, so they raised on valid input: q = UnitQuaternion.Rx(0.3) q.interp(q, 0.5) # ZeroDivisionError UnitQuaternion().interp1(0.5) # ZeroDivisionError UnitQuaternion.Rx(pi).interp(UnitQuaternion.Rx(-pi), 0.5, shortest=True) # ZeroDivisionError UnitQuaternion.Rx(pi).interp(UnitQuaternion.Rx(-pi), 5) # TypeError, non-unit result qslerp() itself returned non-unit quaternions near t=pi: qslerp(q, -q, 0.5) gave [0 0 0 0], and for endpoints 1e-9 from antipodal the norm reached 5.8e6. Root of that: acos(dotprod) loses the small angle to rounding at both ends, so sin(acos(dotprod)) is a bad denominator. Take sin(t) directly as the length of the component of q1 orthogonal to q0, which keeps full relative precision, and get t from atan2. The only remaining degenerate case is sin(t) == 0, where q0 and q1 are the same rotation and so is every interpolate. Measured against a 50-digit q0*exp(s*log(q0^-1.q1)) reference over the whole range of t and s, worst error for t within 1e-6 of pi drops from 4.4e-5 to 2.7e-10 rad, and the largest deviation from unit norm anywhere from 8.2e6 to 2.8e-4. Ordinary angles are unchanged to within 1 ulp, and the whole surface still agrees with scipy's Slerp to 1.4e-15 rad over 10000 random pairs. The two UnitQuaternion methods now call qslerp(), which their own :seealso: already pointed at, so the formula lives in one place. --- spatialmath/base/quaternions.py | 20 +++++++++--- spatialmath/quaternion.py | 58 ++++----------------------------- tests/base/test_quaternions.py | 28 ++++++++++++++++ tests/test_quaternion.py | 31 ++++++++++++++++++ 4 files changed, 80 insertions(+), 57 deletions(-) diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index 24a9cadd..63cbf599 100755 --- a/spatialmath/base/quaternions.py +++ b/spatialmath/base/quaternions.py @@ -789,7 +789,7 @@ def qslerp( :type s: float :arg shortest: choose shortest distance [default False] :type shortest: bool - :param tol: Tolerance when checking for identical quaternions, in multiples of eps, defaults to 20 + :param tol: Tolerance when checking for coincident quaternions, in multiples of eps, defaults to 20 :type tol: float, optional :return: interpolated unit-quaternion :rtype: ndarray(4) @@ -814,6 +814,9 @@ def qslerp( >>> qprint(qslerp(q0, q1, 1)) # this is q1 >>> qprint(qslerp(q0, q1, 0.5)) # this is in "half way" between + .. note:: If ``q0`` and ``q1`` are the same rotation, ie. their dot product is + :math:`\\pm 1`, the interpolate is that rotation for all ``s``. + .. warning:: There is no check that the passed values are unit-quaternions. """ @@ -838,13 +841,20 @@ def qslerp( dotprod = -dotprod # pylint: disable=invalid-unary-operand-type dotprod = np.clip(dotprod, -1, 1) # Clip within domain of acos() - theta = math.acos(dotprod) # theta is the angle between rotation vectors - if abs(theta) > tol * _eps: + + # sin(theta) is the length of the component of q1 orthogonal to q0. Computing + # it this way keeps full relative precision as theta approaches 0 or pi, where + # sin(acos(dotprod)) does not: acos loses the small angle to rounding. + sin_theta = float(np.linalg.norm(q1 - dotprod * q0)) + theta = math.atan2(sin_theta, dotprod) # theta is the angle between q0 and q1 + + if sin_theta > tol * _eps: s0 = math.sin((1 - s) * theta) s1 = math.sin(s * theta) - return ((q0 * s0) + (q1 * s1)) / math.sin(theta) + return ((q0 * s0) + (q1 * s1)) / sin_theta else: - # quaternions are identical + # theta is 0 or pi: q0 and q1 are the same rotation, so is every + # interpolate between them return q0 diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 54ea1362..bca16d34 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -1949,33 +1949,10 @@ def interp( # 2 quaternion form if not isinstance(end, UnitQuaternion): raise TypeError("end argument must be a UnitQuaternion") - q1 = self.vec - q2 = end.vec - dot = smb.qinner(q1, q2) - # If the dot product is negative, the quaternions - # have opposite handed-ness and slerp won't take - # the shorter path. Fix by reversing one quaternion. - if shortest: - if dot < 0: - q1 = -q1 - dot = -dot - - # shouldn't be needed by handle numerical errors: -eps, 1+eps cases - dot = np.clip(dot, -1, 1) # Clip within domain of acos() - - theta_0 = math.acos(dot) # theta_0 = angle between input vectors - - qi = [] - for sk in s: - theta = theta_0 * sk # theta = angle between v0 and result - - s1 = float(math.cos(theta) - dot * math.sin(theta) / math.sin(theta_0)) - s2 = math.sin(theta) / math.sin(theta_0) - out = (q1 * s1) + (q2 * s2) - qi.append(out) - - return UnitQuaternion(qi) + return UnitQuaternion( + [smb.qslerp(self.vec, end.vec, sk, shortest=shortest) for sk in s] + ) def interp1(self, s: float = 0, shortest: Optional[bool] = False) -> UnitQuaternion: """ @@ -2022,32 +1999,9 @@ def interp1(self, s: float = 0, shortest: Optional[bool] = False) -> UnitQuatern s = smb.getvector(s) s = np.clip(s, 0, 1) # enforce valid values - q = self.vec - dot = q[0] # s - - # If the dot product is negative, the quaternions - # have opposite handed-ness and slerp won't take - # the shorter path. Fix by reversing one quaternion. - if shortest: - if dot < 0: - q = -q - dot = -dot - - # shouldn't be needed by handle numerical errors: -eps, 1+eps cases - dot = np.clip(dot, -1, 1) # Clip within domain of acos() - - theta_0 = math.acos(dot) # theta_0 = angle between input vectors - - qi = [] - for sk in s: - theta = theta_0 * sk # theta = angle between v0 and result - - s1 = float(math.cos(theta) - dot * math.sin(theta) / math.sin(theta_0)) - s2 = math.sin(theta) / math.sin(theta_0) - out = np.r_[s1, 0, 0, 0] + (q * s2) - qi.append(out) - - return UnitQuaternion(qi) + return UnitQuaternion( + [smb.qslerp(smb.qeye(), self.vec, sk, shortest=shortest) for sk in s] + ) def increment(self, w: ArrayLike3, normalize: Optional[bool] = False) -> None: """ diff --git a/tests/base/test_quaternions.py b/tests/base/test_quaternions.py index f5859b54..c1b436a9 100644 --- a/tests/base/test_quaternions.py +++ b/tests/base/test_quaternions.py @@ -178,6 +178,34 @@ def test_slerp(self): qslerp(r2q(tr.roty(0.3)), r2q(tr.roty(0.5)), 0.5), r2q(tr.roty(0.4)) ) + def test_slerp_same_rotation(self): + # coincident (dot = +1) and antipodal (dot = -1) endpoints are the same + # rotation, so every interpolate is that rotation + q = r2q(tr.rotx(0.3)) + for s in (0, 0.25, 0.5, 1): + for shortest in (False, True): + nt.assert_array_almost_equal(qslerp(q, q, s, shortest=shortest), q) + qi = qslerp(q, -q, s, shortest=shortest) + self.assertAlmostEqual(np.linalg.norm(qi), 1) + nt.assert_array_almost_equal(q2r(qi), tr.rotx(0.3)) + + def test_slerp_near_pi(self): + # the slerp weights are sin(...)/sin(theta), singular at theta = 0 and pi. + # Check against the closed form cos(s.theta) q0 + sin(s.theta) v, where v is + # the unit quaternion orthogonal to q0 in the plane of the great circle. + q0 = r2q(tr.rpy2r(0.2, 0.3, 0.4)) + v = np.r_[0, 0, 1, 0] - np.dot(np.r_[0, 0, 1, 0], q0) * q0 + v = v / np.linalg.norm(v) + + for theta in (1e-6, 1e-3, 0.5, 1.5, math.pi - 1e-3, math.pi - 1e-6): + q1 = math.cos(theta) * q0 + math.sin(theta) * v + for s in (0.25, 0.5, 0.75): + qi = qslerp(q0, q1, s) + nt.assert_array_almost_equal( + qi, math.cos(s * theta) * q0 + math.sin(s * theta) * v + ) + self.assertAlmostEqual(np.linalg.norm(qi), 1) + def test_rotx(self): pass diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 75d31b7c..904dd24d 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -701,6 +701,37 @@ def test_interp(self): # qcompare( qq(6), UnitQuaternion.Rx(pi) ) # TODO interp + def test_interp_same_rotation(self): + # endpoints that are the same rotation make the slerp weights singular + q = UnitQuaternion.Rx(0.3) + for s in (0, 0.4, 1): + for shortest in (False, True): + qcompare(q.interp(q, s, shortest=shortest), q) + qcompare( + q.interp(UnitQuaternion.Rx(0.3 + 1e-9), s, shortest=shortest), q + ) + qq = q.interp(q, 5) + self.assertEqual(len(qq), 5) + qcompare(qq[3], q) + + u = UnitQuaternion() + for s in (0, 0.4, 1): + qcompare(u.interp1(s), u) + qcompare(UnitQuaternion.Rx(1e-9).interp1(s), u) + self.assertEqual(len(u.interp1(5)), 5) + + # Rx(pi) and Rx(-pi) are the same rotation, with a dot product of -1 + p = UnitQuaternion.Rx(pi) + m = UnitQuaternion.Rx(-pi) + self.assertAlmostEqual(np.dot(p.vec, m.vec), -1) + for shortest in (False, True): + for s in (0, 0.4, 1): + qi = p.interp(m, s, shortest=shortest) + self.assertAlmostEqual(np.linalg.norm(qi.vec), 1) + nt.assert_array_almost_equal(qi.R, p.R) + for qi in p.interp(m, 5): + nt.assert_array_almost_equal(qi.R, p.R) + def test_increment(self): q = UnitQuaternion() From 1e0643d85ef743d83438ce6e4b859dffce93a79d Mon Sep 17 00:00:00 2001 From: Vincent Gao Date: Sun, 26 Jul 2026 17:03:52 +0200 Subject: [PATCH 2/3] Fix SO2/SE2.interp1(), which raised NameError for every input The fix for #33 dropped the `start` local from interp1() but only replaced its two uses in the N == 3 branch, so the SO(2)/SE(2) branch has referenced an undefined name ever since: SE2(1, 2, 0.3).interp1(0.5) # NameError: name 'start' is not defined #33 did report it for both SE2 and SE3. Pass None like the N == 3 branch does; trinterp2() already treats a None start as the identity. interp1() had no test coverage for either dimension, hence the four years. --- spatialmath/baseposematrix.py | 4 ++-- tests/test_pose2d.py | 17 +++++++++++++++++ 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/spatialmath/baseposematrix.py b/spatialmath/baseposematrix.py index c798c817..7c3b3192 100644 --- a/spatialmath/baseposematrix.py +++ b/spatialmath/baseposematrix.py @@ -522,10 +522,10 @@ def interp1(self, s: float = None) -> Self: # SO(2) or SE(2) if len(s) > 1: assert len(self) == 1, "if len(s) > 1, len(X) must == 1" - return self.__class__([smb.trinterp2(start, self.A, s=_s) for _s in s]) + return self.__class__([smb.trinterp2(None, self.A, s=_s) for _s in s]) else: return self.__class__( - [smb.trinterp2(start, x, s=s[0]) for x in self.data] + [smb.trinterp2(None, x, s=s[0]) for x in self.data] ) elif self.N == 3: # SO(3) or SE(3) diff --git a/tests/test_pose2d.py b/tests/test_pose2d.py index d6d96813..fc3734d6 100755 --- a/tests/test_pose2d.py +++ b/tests/test_pose2d.py @@ -466,6 +466,23 @@ def test_interp(self): array_compare(T1.interp(T2, s=0.5, shortest=False), SE2(0, 0, 0.05)) array_compare(T1.interp(T2, s=0.5, shortest=True), SE2(0, 0, -math.pi + 0.05)) + def test_interp1(self): + # interpolate from the identity pose + TT = SE2(2, -4, 0.6) + array_compare(TT.interp1(0), SE2()) + array_compare(TT.interp1(1), TT) + array_compare(TT.interp1(0.5), SE2(1, -2, 0.3)) + + z = TT.interp1([0, 0.5, 1]) + self.assertEqual(len(z), 3) + array_compare(z[2], TT) + + R = SO2(0.6) + array_compare(R.interp1(0), SO2()) + array_compare(R.interp1(1), R) + array_compare(R.interp1(0.5), SO2(0.3)) + self.assertEqual(len(SE2([TT, TT]).interp1(0.5)), 2) + def test_miscellany(self): TT = SE2(1, 2, 0.3) From 75856e85f2171014143c75806371be9547e3686a Mon Sep 17 00:00:00 2001 From: Vincent Gao Date: Thu, 20 Aug 2026 10:51:35 +0200 Subject: [PATCH 3/3] Hoist loop-invariant slerp setup Prepare the validated quaternion pair and stable angle terms once per interpolation batch, then reuse them for each sample. This removes the scale-dependent regression introduced when interp() and interp1() were routed through qslerp(), while keeping the public scalar qslerp behavior byte-equivalent. --- spatialmath/base/quaternions.py | 84 +++++++++++++++++++-------------- spatialmath/quaternion.py | 11 ++--- tests/test_quaternion.py | 10 ++++ 3 files changed, 64 insertions(+), 41 deletions(-) diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index 63cbf599..b54d16dc 100755 --- a/spatialmath/base/quaternions.py +++ b/spatialmath/base/quaternions.py @@ -17,7 +17,7 @@ from spatialmath.base.argcheck import getunit from spatialmath.base.types import * import scipy.interpolate as interpolate -from typing import Optional +from typing import Callable, Optional from functools import lru_cache import warnings @@ -771,6 +771,53 @@ def r2q( # return np.r_[qs, (math.sqrt(1.0 - qs**2) / nm) * kv] +def _qslerp( + q0: ArrayLike4, + q1: ArrayLike4, + shortest: Optional[bool] = False, + tol: float = 20, +) -> Callable[[float], UnitQuaternionArray]: + """Prepare an interpolator for a pair of unit quaternions.""" + q0 = smb.getvector(q0, 4) + q1 = smb.getvector(q1, 4) + q0_endpoint = q0 + + dotprod = np.dot(q0, q1) + + # If the dot product is negative, the quaternions + # have opposite handed-ness and slerp won't take + # the shorter path. Fix by reversing one quaternion. + if shortest: + if dotprod < 0: + q0 = -q0 # pylint: disable=invalid-unary-operand-type + dotprod = -dotprod # pylint: disable=invalid-unary-operand-type + + dotprod = np.clip(dotprod, -1, 1) + + # sin(theta) is the length of the component of q1 orthogonal to q0. Computing + # it this way keeps full relative precision as theta approaches 0 or pi, where + # sin(acos(dotprod)) does not: acos loses the small angle to rounding. + sin_theta = float(np.linalg.norm(q1 - dotprod * q0)) + theta = math.atan2(sin_theta, dotprod) # theta is the angle between q0 and q1 + + def interpolate(s: float) -> UnitQuaternionArray: + if s == 0: + return q0_endpoint + elif s == 1: + return q1 + + if sin_theta > tol * _eps: + s0 = math.sin((1 - s) * theta) + s1 = math.sin(s * theta) + return ((q0 * s0) + (q1 * s1)) / sin_theta + else: + # theta is 0 or pi: q0 and q1 are the same rotation, so is every + # interpolate between them + return q0 + + return interpolate + + def qslerp( q0: ArrayLike4, q1: ArrayLike4, @@ -822,40 +869,7 @@ def qslerp( """ if not 0 <= s <= 1: raise ValueError("s must be in the interval [0,1]") - q0 = smb.getvector(q0, 4) - q1 = smb.getvector(q1, 4) - - if s == 0: - return q0 - elif s == 1: - return q1 - - dotprod = np.dot(q0, q1) - - # If the dot product is negative, the quaternions - # have opposite handed-ness and slerp won't take - # the shorter path. Fix by reversing one quaternion. - if shortest: - if dotprod < 0: - q0 = -q0 # pylint: disable=invalid-unary-operand-type - dotprod = -dotprod # pylint: disable=invalid-unary-operand-type - - dotprod = np.clip(dotprod, -1, 1) # Clip within domain of acos() - - # sin(theta) is the length of the component of q1 orthogonal to q0. Computing - # it this way keeps full relative precision as theta approaches 0 or pi, where - # sin(acos(dotprod)) does not: acos loses the small angle to rounding. - sin_theta = float(np.linalg.norm(q1 - dotprod * q0)) - theta = math.atan2(sin_theta, dotprod) # theta is the angle between q0 and q1 - - if sin_theta > tol * _eps: - s0 = math.sin((1 - s) * theta) - s1 = math.sin(s * theta) - return ((q0 * s0) + (q1 * s1)) / sin_theta - else: - # theta is 0 or pi: q0 and q1 are the same rotation, so is every - # interpolate between them - return q0 + return _qslerp(q0, q1, shortest=shortest, tol=tol)(s) def _compute_cdf_sin_squared(theta: float): diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index bca16d34..f89a4cc2 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -19,6 +19,7 @@ import numpy as np from typing import Any import spatialmath.base as smb +from spatialmath.base.quaternions import _qslerp from spatialmath.pose3d import SO3, SE3 from spatialmath.baseposelist import BasePoseList from spatialmath.base.types import * @@ -1950,9 +1951,8 @@ def interp( if not isinstance(end, UnitQuaternion): raise TypeError("end argument must be a UnitQuaternion") - return UnitQuaternion( - [smb.qslerp(self.vec, end.vec, sk, shortest=shortest) for sk in s] - ) + interpolate = _qslerp(self.vec, end.vec, shortest=shortest) + return UnitQuaternion([interpolate(sk) for sk in s]) def interp1(self, s: float = 0, shortest: Optional[bool] = False) -> UnitQuaternion: """ @@ -1999,9 +1999,8 @@ def interp1(self, s: float = 0, shortest: Optional[bool] = False) -> UnitQuatern s = smb.getvector(s) s = np.clip(s, 0, 1) # enforce valid values - return UnitQuaternion( - [smb.qslerp(smb.qeye(), self.vec, sk, shortest=shortest) for sk in s] - ) + interpolate = _qslerp(smb.qeye(), self.vec, shortest=shortest) + return UnitQuaternion([interpolate(sk) for sk in s]) def increment(self, w: ArrayLike3, normalize: Optional[bool] = False) -> None: """ diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 904dd24d..70613c90 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -2,6 +2,7 @@ from math import pi import numpy.testing as nt import unittest +from unittest.mock import patch from spatialmath import * from spatialmath.base import * @@ -732,6 +733,15 @@ def test_interp_same_rotation(self): for qi in p.interp(m, 5): nt.assert_array_almost_equal(qi.R, p.R) + def test_interp_prepares_slerp_once(self): + q0 = UnitQuaternion.RPY([0.2, 0.3, 0.4]) + q1 = UnitQuaternion.RPY([-0.3, 0.1, 0.2]) + + for interpolate in (lambda: q0.interp1(5), lambda: q0.interp(q1, 5)): + with patch("spatialmath.base.quaternions.np.dot", wraps=np.dot) as dot: + self.assertEqual(len(interpolate()), 5) + self.assertEqual(dot.call_count, 1) + def test_increment(self): q = UnitQuaternion()