fix: broken docstring examples in Twist2/Twist3/DualQuaternion (and their shared root cause)

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

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

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

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

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)

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