Fix slerp at coincident/antipodal endpoints, and SO2/SE2.interp1()

spatialmath/base/quaternions.py

@@ -771,6 +771,47 @@ def r2q(
 #         return np.r_[qs, (math.sqrt(1.0 - qs**2) / nm) * kv]
 
 
+def _qslerp_prepare(
+    q0: ArrayLike4,
+    q1: ArrayLike4,
+    shortest: Optional[bool] = False,
+) -> tuple[
+    UnitQuaternionArray,
+    UnitQuaternionArray,
+    UnitQuaternionArray,
+    float,
+    float,
+]:
+    """Compute the loop-invariant slerp terms for two unit quaternions.
+
+    The original ``q0`` endpoint is returned separately from the sign-adjusted
+    value used by shortest-path interpolation, preserving the exact value at
+    ``s=0``.
+    """
+    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
+    return q0_endpoint, q0, q1, sin_theta, theta
+
+
 def qslerp(
     q0: ArrayLike4,
     q1: ArrayLike4,
@@ -789,7 +830,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,37 +855,27 @@ 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.
 
     """
     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)
-
+    q0_endpoint, q0, q1, sin_theta, theta = _qslerp_prepare(q0, q1, shortest=shortest)
     if s == 0:
-        return q0
+        return q0_endpoint
     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()
-    theta = math.acos(dotprod)  # theta is the angle between rotation vectors
-    if abs(theta) > tol * _eps:
+    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
 
 

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)

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_prepare
 from spatialmath.pose3d import SO3, SE3
 from spatialmath.baseposelist import BasePoseList
 from spatialmath.base.types import *
@@ -1949,32 +1950,23 @@ 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
 
+        q0_endpoint, q0, q1, sin_theta, theta = _qslerp_prepare(
+            self.vec, end.vec, shortest=shortest
+        )
         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)
+            if sk == 0:
+                out = q0_endpoint
+            elif sk == 1:
+                out = q1
+            elif sin_theta > 20 * _eps:
+                s0 = math.sin((1 - sk) * theta)
+                s1 = math.sin(sk * theta)
+                out = ((q0 * s0) + (q1 * s1)) / sin_theta
+            else:
+                out = q0
             qi.append(out)
-
         return UnitQuaternion(qi)
 
     def interp1(self, s: float = 0, shortest: Optional[bool] = False) -> UnitQuaternion:
@@ -2022,31 +2014,22 @@ 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
-
+        q0_endpoint, q0, q1, sin_theta, theta = _qslerp_prepare(
+            smb.qeye(), self.vec, shortest=shortest
+        )
         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)
+            if sk == 0:
+                out = q0_endpoint
+            elif sk == 1:
+                out = q1
+            elif sin_theta > 20 * _eps:
+                s0 = math.sin((1 - sk) * theta)
+                s1 = math.sin(sk * theta)
+                out = ((q0 * s0) + (q1 * s1)) / sin_theta
+            else:
+                out = q0
             qi.append(out)
-
         return UnitQuaternion(qi)
 
     def increment(self, w: ArrayLike3, normalize: Optional[bool] = False) -> None:

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
 

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)
 

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 *
@@ -701,6 +702,46 @@ 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_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()