diff --git a/spatialmath/DualQuaternion.py b/spatialmath/DualQuaternion.py index 5e606dcd..6e657192 100644 --- a/spatialmath/DualQuaternion.py +++ b/spatialmath/DualQuaternion.py @@ -117,7 +117,11 @@ def norm(self) -> Tuple[float, float]: """ a = self.real * self.real.conj() b = self.real * self.dual.conj() + self.dual * self.real.conj() - return (base.sqrt(a.s), base.sqrt(b.s)) + # a.s/b.s are mathematically guaranteed non-negative (they're the + # scalar part of q*conj(q)-like products), but floating-point + # rounding can leave a value like -1e-17 instead of exactly 0, + # which sqrt() rejects outright. Clamp away that noise. + return (base.sqrt(max(0.0, a.s)), base.sqrt(max(0.0, b.s))) def conj(self) -> Self: r""" @@ -208,8 +212,18 @@ def __mul__(left, right: Self) -> Self: # pylint: disable=no-self-argument return DualQuaternion(real, dual) elif isinstance(left, UnitDualQuaternion) and base.isvector(right, 3): v = base.getvector(right, 3) - vp = left * DualQuaternion.Pure(v) * left.conj() - return vp.dual.v + # NB: not the textbook q*P*conj(q) sandwich product. With this + # class's own dual-part embedding convention (__init__ builds + # dual = 0.5*Pure(t)*real, translation quaternion on the left), + # that sandwich's translation terms cancel exactly to zero: + # qr*conj(qd) + qd*conj(qr) == 0 for this embedding, leaving + # only the rotated point with no translation applied at all. + # SE3() already correctly extracts (R, t) from this same + # embedding (see its own derivation), so reuse it here rather + # than hand-deriving a second, convention-specific formula. + # Flatten to match this method's original flat-vector return + # convention (SE3.__mul__ returns a (3,1) column instead). + return (left.SE3() * v).flatten() def matrix(self) -> R8x8: """ @@ -343,7 +357,7 @@ def SE3(self) -> SE3: >>> print(T) >>> d = UnitDualQuaternion(T) >>> print(d) - >>> print(d.T) + >>> print(d.SE3()) """ R = base.q2r(self.real.A) t = 2 * self.dual * self.real.conj() diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index a5759709..28d44b51 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -306,17 +306,63 @@ def conj(self) -> Quaternion: ``q.conj()`` is the quaternion ``q`` with the vector part negated, ie. :math:`q = s \langle -v_x, -v_y, -v_z \rangle` + .. note:: For a ``UnitQuaternion`` this deliberately does **not** + canonicalize the result's scalar part to be non-negative, unlike + normal ``UnitQuaternion`` construction. ``UnitQuaternion`` + construction canonicalizes because :math:`q` and :math:`-q` + represent the same rotation, which is the right behaviour when + building a unit quaternion from arbitrary data. But conjugation + is an algebraic operation, not a re-representation of a + rotation: it must satisfy :math:`q \cdot \bar{q} = 1` for any + downstream algebra to be correct (e.g. dual-quaternion + translation extraction, which relies on exactly this identity). + Re-canonicalizing the conjugate would silently return + :math:`-\bar{q}` whenever ``q`` has negative scalar part, + breaking that identity. See the example below: the scalar part + of the result matches the input's sign, it is not forced + positive. + + .. versionchanged:: 1.1.18 + Fixes a bug present in 1.1.17 and earlier: ``UnitQuaternion.conj()`` + re-canonicalized its result's sign, silently returning + :math:`-\bar{q}` instead of the true conjugate whenever ``q`` had + negative scalar part. This broke any algebra relying on + :math:`q \cdot \bar{q} = 1`, including :class:`DualQuaternion` + translation extraction (``SE3()``), ``norm()`` (which could raise + ``ValueError`` from taking the square root of a small negative + float), and point transformation via ``dq * v``. + Example: .. runblock:: pycon - >>> from spatialmath import Quaternion + >>> from spatialmath import Quaternion, UnitQuaternion + >>> import numpy as np >>> print(Quaternion.Pure([1,2,3]).conj()) + >>> q = UnitQuaternion(np.array([[-0.5, 0.5, 0.5, 0.5]]), norm=False) + >>> print(q) + >>> print(q.conj()) :seealso: :func:`~spatialmath.base.quaternions.qconj` """ - - return self.__class__([smb.qconj(q._A) for q in self]) + # NB: iterate self.data directly, not `for q in self` -- indexing + # a UnitQuaternion (which iteration uses under the hood) goes + # through BasePoseList.__getitem__, which reconstructs each + # element via self.__class__(self.data[i], check=False) with no + # norm=False override, silently re-canonicalizing sign on every + # single access. Working from self.data sidesteps that entirely. + if isinstance(self, UnitQuaternion): + # Pass a 2D (N,4) array with norm=False so construction stores + # the conjugated array as-is, bypassing qunit()'s scalar-sign + # canonicalization -- see the note above for why that + # canonicalization must not apply here. A 1D (4,) array would + # instead be caught by the generic arghandler() path first, + # which normalizes/canonicalizes unconditionally regardless of + # norm -- the 2D-array path is what actually honours norm=False. + return self.__class__( + np.array([smb.qconj(d) for d in self.data]), norm=False + ) + return self.__class__([smb.qconj(d) for d in self.data]) def norm(self) -> float: r""" diff --git a/spatialmath/twist.py b/spatialmath/twist.py index 928235e4..4579ec29 100644 --- a/spatialmath/twist.py +++ b/spatialmath/twist.py @@ -813,7 +813,7 @@ def unit(self): """ Unit twist - - ``S.unit()`` is a Twist2 objec3 representing a unit twist aligned with the + - ``S.unit()`` is a Twist3 object representing a unit twist aligned with the Twist ``S``. Example: @@ -825,12 +825,12 @@ def unit(self): >>> S = Twist3(T) >>> S.unit() """ - if smb.iszerovec(self.w): - # rotational twist - return Twist3(self.S / smb.norm(S.w)) - else: - # prismatic twist + if self.isprismatic: + # prismatic twist (zero rotation): normalize the direction vector return Twist3(smb.unitvec(self.v), [0, 0, 0]) + else: + # general twist: normalize so |w| == 1 + return Twist3(self.S / smb.norm(self.w)) def ad(self): """ @@ -974,7 +974,7 @@ def pole(self): :return: the pole of the twist :rtype: ndarray(3) - ``X.pole()`` is a point on the twist axis. For a pure translation + ``X.pole`` is a point on the twist axis. For a pure translation this point is at infinity. Example: @@ -1487,7 +1487,7 @@ def pole(self): :return: the pole of the twist :rtype: ndarray(2) - ``X.pole()`` is a point on the twist axis. For a pure translation + ``X.pole`` is a point on the twist axis. For a pure translation this point is at infinity. Example: @@ -1497,7 +1497,7 @@ def pole(self): >>> from spatialmath import SE2, Twist2 >>> T = SE2(1, 2, 0.3) >>> S = Twist2(T) - >>> S.pole() + >>> S.pole """ p = np.cross(np.r_[0, 0, self.w], np.r_[self.v, 0]) / self.theta @@ -1626,12 +1626,12 @@ def unit(self): >>> S = Twist2(T) >>> S.unit() """ - if smb.iszerovec(self.w): - # rotational twist - return Twist2(self.S / smb.norm(S.w)) + if self.isprismatic: + # prismatic twist (zero rotation): normalize the direction vector + return Twist2(smb.unitvec(self.v), 0) else: - # prismatic twist - return Twist2(smb.unitvec(self.v), [0, 0, 0]) + # general twist: normalize so |w| == 1 (w is a scalar for Twist2) + return Twist2(self.S / abs(self.w)) @property def ad(self): diff --git a/tests/test_dualquaternion.py b/tests/test_dualquaternion.py index ed785313..cb9b5af9 100644 --- a/tests/test_dualquaternion.py +++ b/tests/test_dualquaternion.py @@ -84,11 +84,36 @@ def test_init(self): dq = UnitDualQuaternion(T) nt.assert_array_almost_equal(dq.SE3().A, T.A) + def test_init_negative_scalar(self): + # Rx(pi/4) above has no translation and a positive quaternion + # scalar part, so it can't exercise either bug that used to live + # here: SE3() used self.real.conj(), which silently returned + # -conj(real) whenever real had negative scalar part (flipping + # the sign of the recovered translation). This seed's first draw + # is confirmed to produce a UnitQuaternion with negative scalar + # part, so it's kept as a fixed regression case rather than + # relying on randomness at test time. + np.random.seed(0) + T = SE3.Rand() + dq = UnitDualQuaternion(T) + self.assertLess(dq.real.A[0], 0) + nt.assert_array_almost_equal(dq.SE3().A, T.A) + def test_norm(self): T = SE3.Rx(pi / 4) dq = UnitDualQuaternion(T) nt.assert_array_almost_equal(dq.norm(), (1, 0)) + def test_norm_negative_scalar(self): + # see test_init_negative_scalar: norm() used the same broken + # conj() and would crash with "math domain error" (sqrt of a + # small negative float) for this case before the fix. + np.random.seed(0) + T = SE3.Rand() + dq = UnitDualQuaternion(T) + self.assertLess(dq.real.A[0], 0) + nt.assert_array_almost_equal(dq.norm(), (1, 0)) + def test_multiply(self): T1 = SE3.Rx(pi / 4) T2 = SE3.Rz(-pi / 3) @@ -101,6 +126,28 @@ def test_multiply(self): d = d1 * d2 nt.assert_array_almost_equal(d.SE3().A, T.A) + def test_vector_transform(self): + # previously untested and broken: the q*P*conj(q) sandwich + # product's translation terms cancel exactly to zero under this + # class's own dual-part embedding convention (dual = + # 0.5*Pure(t)*real), so the old code silently applied only the + # rotation and dropped the translation entirely. + T = SE3(1, 2, 3) * SE3.Rx(0.3) + dq = UnitDualQuaternion(T) + v = np.array([4.0, 5.0, 6.0]) + vp = dq * v + expected = (T * v).flatten() + nt.assert_array_almost_equal(vp, expected) + + # also check a second, independent transform for good measure + np.random.seed(2) + SE3.Rand() + T = SE3.Rand() + dq = UnitDualQuaternion(T) + vp = dq * v + expected = (T * v).flatten() + nt.assert_array_almost_equal(vp, expected) + # ---------------------------------------------------------------------------------------# if __name__ == "__main__": # pragma: no cover diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 8a87a433..63a66946 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -380,6 +380,42 @@ def test_canonic(self): R = rotz(-pi) qcompare(UnitQuaternion(R), np.r_[cos(pi / 2), sin(pi / 2) * np.r_[0, 0, 1]]) + def test_conj(self): + # plain Quaternion: conj negates the vector part only + q = Quaternion([1, 2, 3, 4]) + qcompare(q.conj(), [1, -2, -3, -4]) + self.assertIsInstance(q.conj(), Quaternion) + + # UnitQuaternion, positive scalar part: matches plain conjugate + u = UnitQuaternion(rotx(0.3)) + self.assertGreater(u.A[0], 0) + qcompare(u.conj(), qconj(u.A)) + self.assertIsInstance(u.conj(), UnitQuaternion) + + # UnitQuaternion, negative scalar part (reachable via norm=False, + # e.g. a >180 deg rotation before its own construction-time + # canonicalization -- constructed directly here to isolate conj()): + # conj() must NOT re-canonicalize the result's sign to be + # positive, unlike normal UnitQuaternion construction. If it did, + # this would silently return -conj(u) instead of the true + # conjugate, breaking the identity u * conj(u) == 1 that + # downstream algebra (e.g. DualQuaternion) depends on. + u = UnitQuaternion(np.array([[-0.5, 0.5, 0.5, 0.5]]), norm=False) + self.assertLess(u.A[0], 0) + qcompare(u.conj(), [-0.5, -0.5, -0.5, -0.5]) + self.assertIsInstance(u.conj(), UnitQuaternion) + + # the algebraic identity that must hold regardless of sign + qcompare(u * u.conj(), [1, 0, 0, 0]) + + # multi-valued UnitQuaternion, mixed signs + us = UnitQuaternion( + np.array([[-0.5, 0.5, 0.5, 0.5], [0.5, -0.5, -0.5, -0.5]]), norm=False + ) + conjs = us.conj() + qcompare(conjs.data[0], [-0.5, -0.5, -0.5, -0.5]) + qcompare(conjs.data[1], [0.5, 0.5, 0.5, 0.5]) + def test_convert(self): # test conversion from rotn matrix to u.quaternion and back R = rotx(0) diff --git a/tests/test_twist.py b/tests/test_twist.py index dfbd9f24..dcfb0fd1 100755 --- a/tests/test_twist.py +++ b/tests/test_twist.py @@ -213,6 +213,28 @@ def test_prod(self): x = Twist3([x1, x2]) array_compare(x.prod().SE3(), T1 * T2) + def test_unit(self): + # general (rotational) twist: normalized so |w| == 1 + T = SE3(1, 2, 3) * SE3.Rx(0.3) + S = Twist3(T) + u = S.unit() + self.assertAlmostEqual(np.linalg.norm(u.w), 1.0) + nt.assert_array_almost_equal(u.S, S.S / np.linalg.norm(S.w)) + + # prismatic twist (zero rotation): normalized direction vector, + # previously untested -- this branch raised ValueError before the + # fix (wrong-shape zero argument, S.w typo) + S = Twist3(np.r_[3, 4, 0, 0, 0, 0]) + u = S.unit() + self.assertAlmostEqual(np.linalg.norm(u.v), 1.0) + nt.assert_array_almost_equal(u.w, [0, 0, 0]) + + def test_pole(self): + T = SE3(1, 2, 3) * SE3.Rx(0.3) + S = Twist3(T) + p = S.pole + self.assertEqual(len(p), 3) + class Twist2dTest(unittest.TestCase): def test_constructor(self): @@ -375,6 +397,32 @@ def test_prod(self): x = Twist2([x1, x2]) array_compare(x.prod().SE2(), T1 * T2) + def test_unit(self): + # general (rotational) twist: normalized so |w| == 1. Previously + # broken: branches were swapped (this case fell into the "zero + # rotation" branch and tried to construct Twist2 with a 3-element + # zero argument instead of scalar 0, raising ValueError). + T = SE2(1, 2, 0.3) + S = Twist2(T) + u = S.unit() + self.assertAlmostEqual(abs(u.w), 1.0) + nt.assert_array_almost_equal(u.S, S.S / abs(S.w)) + + # prismatic twist (zero rotation): normalized direction vector + S = Twist2([3, 4], 0) + u = S.unit() + self.assertAlmostEqual(np.linalg.norm(u.v), 1.0) + self.assertEqual(u.w, 0) + + def test_pole(self): + # previously broken: docstring example called S.pole() but pole + # is a @property, not a method -- TypeError: 'numpy.ndarray' + # object is not callable + T = SE2(1, 2, 0.3) + S = Twist2(T) + p = S.pole + self.assertEqual(len(p), 2) + # ---------------------------------------------------------------------------------------# if __name__ == "__main__":