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.

Author: binggao1230

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
 
 

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:
         """

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_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()