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

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/baseposematrix.py

@@ -512,10 +512,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

@@ -1946,33 +1946,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:
         """
@@ -2019,32 +1996,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_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

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