diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index 364a5ea8..1f622200 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/baseposematrix.py b/spatialmath/baseposematrix.py index 87071df3..0215bd8c 100644 --- a/spatialmath/baseposematrix.py +++ b/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) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 2994b6e6..7ec19064 100644 --- a/spatialmath/quaternion.py +++ b/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: """ 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_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) 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()