Skip to content

Commit 3f72042

Browse files
authored
fix: broken docstring examples in Twist2/Twist3/DualQuaternion (and their shared root cause) (#225)
## Summary Found via the sphinx-pyrunblock rollout (#221): several `.. runblock::` docstring examples were silently baking broken tracebacks into the published docs instead of demonstrating real output. **Twist2.unit() / Twist3.unit()** -- branches were swapped, `self.w` had a `S.w` typo, and the "prismatic" branch passed a wrong-shape zero argument for `Twist2` (3-element list where a scalar is required). **Twist2.pole** -- the docstring's own example called `S.pole()`, but `pole` is a `@property`, not a method. **DualQuaternion: a shared root cause across three methods** -- fixing `SE3()`'s docstring (called `d.T` instead of the actual `d.SE3()` method) surfaced that `UnitQuaternion.conj()` silently re-canonicalized its result's sign, which is correct for *constructing* a UnitQuaternion (q and -q are the same rotation) but wrong for conjugation, an algebraic operation that must satisfy `q*conj(q) == 1`. Fixed `Quaternion.conj()` itself rather than routing around it at each call site. This one fix resolved three separate, previously-untested bugs: - `SE3()`: silently returned a transform with negated translation for any rotation with negative quaternion scalar part. - `norm()`: crashed outright (`math domain error`) for the same reason -- also added a small floating-point clamp for the mathematically-non-negative-but-can-round-negative norm-squared terms. - Vector transformation (`dq * v`): unrelated to the sign bug -- affected *every* case regardless of sign. The textbook `q*P*conj(q)` sandwich product's translation terms cancel to exactly zero under this class's own dual-part embedding convention (`dual = 0.5*Pure(t)*real`). Reused the already-correct `SE3()` extraction instead of hand-deriving a second, convention-specific sandwich formula. ## Test coverage All of the above had zero prior test coverage -- existing DualQuaternion tests used `SE3.Rx(pi/4)` (no translation, positive quaternion scalar part), which can't exercise any of these bugs. Added regression tests for `conj()`'s sign behaviour, both branches of `Twist2`/`Twist3.unit()`, `Twist2`/`Twist3.pole`, and `DualQuaternion`'s `SE3()`/`norm()`/vector-transform. ## Test plan - [x] Full suite: 334 passed (up from 326), 0 regressions - [x] Real sphinx-build verification (isolated venv, before/after this branch): RUNBLOCK-ERROR count 6 -> 0, warning count unchanged at 14 (confirmed stable across repeat builds of both branches, ruling out build-to-build nondeterminism as an explanation either way)
1 parent 8369197 commit 3f72042

6 files changed

Lines changed: 212 additions & 21 deletions

File tree

‎spatialmath/DualQuaternion.py‎

Lines changed: 18 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -117,7 +117,11 @@ def norm(self) -> Tuple[float, float]:
117117
"""
118118
a = self.real * self.real.conj()
119119
b = self.real * self.dual.conj() + self.dual * self.real.conj()
120-
return (base.sqrt(a.s), base.sqrt(b.s))
120+
# a.s/b.s are mathematically guaranteed non-negative (they're the
121+
# scalar part of q*conj(q)-like products), but floating-point
122+
# rounding can leave a value like -1e-17 instead of exactly 0,
123+
# which sqrt() rejects outright. Clamp away that noise.
124+
return (base.sqrt(max(0.0, a.s)), base.sqrt(max(0.0, b.s)))
121125

122126
def conj(self) -> Self:
123127
r"""
@@ -208,8 +212,18 @@ def __mul__(left, right: Self) -> Self: # pylint: disable=no-self-argument
208212
return DualQuaternion(real, dual)
209213
elif isinstance(left, UnitDualQuaternion) and base.isvector(right, 3):
210214
v = base.getvector(right, 3)
211-
vp = left * DualQuaternion.Pure(v) * left.conj()
212-
return vp.dual.v
215+
# NB: not the textbook q*P*conj(q) sandwich product. With this
216+
# class's own dual-part embedding convention (__init__ builds
217+
# dual = 0.5*Pure(t)*real, translation quaternion on the left),
218+
# that sandwich's translation terms cancel exactly to zero:
219+
# qr*conj(qd) + qd*conj(qr) == 0 for this embedding, leaving
220+
# only the rotated point with no translation applied at all.
221+
# SE3() already correctly extracts (R, t) from this same
222+
# embedding (see its own derivation), so reuse it here rather
223+
# than hand-deriving a second, convention-specific formula.
224+
# Flatten to match this method's original flat-vector return
225+
# convention (SE3.__mul__ returns a (3,1) column instead).
226+
return (left.SE3() * v).flatten()
213227

214228
def matrix(self) -> R8x8:
215229
"""
@@ -343,7 +357,7 @@ def SE3(self) -> SE3:
343357
>>> print(T)
344358
>>> d = UnitDualQuaternion(T)
345359
>>> print(d)
346-
>>> print(d.T)
360+
>>> print(d.SE3())
347361
"""
348362
R = base.q2r(self.real.A)
349363
t = 2 * self.dual * self.real.conj()

‎spatialmath/quaternion.py‎

Lines changed: 49 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -306,17 +306,63 @@ def conj(self) -> Quaternion:
306306
``q.conj()`` is the quaternion ``q`` with the vector part negated, ie.
307307
:math:`q = s \langle -v_x, -v_y, -v_z \rangle`
308308
309+
.. note:: For a ``UnitQuaternion`` this deliberately does **not**
310+
canonicalize the result's scalar part to be non-negative, unlike
311+
normal ``UnitQuaternion`` construction. ``UnitQuaternion``
312+
construction canonicalizes because :math:`q` and :math:`-q`
313+
represent the same rotation, which is the right behaviour when
314+
building a unit quaternion from arbitrary data. But conjugation
315+
is an algebraic operation, not a re-representation of a
316+
rotation: it must satisfy :math:`q \cdot \bar{q} = 1` for any
317+
downstream algebra to be correct (e.g. dual-quaternion
318+
translation extraction, which relies on exactly this identity).
319+
Re-canonicalizing the conjugate would silently return
320+
:math:`-\bar{q}` whenever ``q`` has negative scalar part,
321+
breaking that identity. See the example below: the scalar part
322+
of the result matches the input's sign, it is not forced
323+
positive.
324+
325+
.. versionchanged:: 1.1.18
326+
Fixes a bug present in 1.1.17 and earlier: ``UnitQuaternion.conj()``
327+
re-canonicalized its result's sign, silently returning
328+
:math:`-\bar{q}` instead of the true conjugate whenever ``q`` had
329+
negative scalar part. This broke any algebra relying on
330+
:math:`q \cdot \bar{q} = 1`, including :class:`DualQuaternion`
331+
translation extraction (``SE3()``), ``norm()`` (which could raise
332+
``ValueError`` from taking the square root of a small negative
333+
float), and point transformation via ``dq * v``.
334+
309335
Example:
310336
311337
.. runblock:: pycon
312338
313-
>>> from spatialmath import Quaternion
339+
>>> from spatialmath import Quaternion, UnitQuaternion
340+
>>> import numpy as np
314341
>>> print(Quaternion.Pure([1,2,3]).conj())
342+
>>> q = UnitQuaternion(np.array([[-0.5, 0.5, 0.5, 0.5]]), norm=False)
343+
>>> print(q)
344+
>>> print(q.conj())
315345
316346
:seealso: :func:`~spatialmath.base.quaternions.qconj`
317347
"""
318-
319-
return self.__class__([smb.qconj(q._A) for q in self])
348+
# NB: iterate self.data directly, not `for q in self` -- indexing
349+
# a UnitQuaternion (which iteration uses under the hood) goes
350+
# through BasePoseList.__getitem__, which reconstructs each
351+
# element via self.__class__(self.data[i], check=False) with no
352+
# norm=False override, silently re-canonicalizing sign on every
353+
# single access. Working from self.data sidesteps that entirely.
354+
if isinstance(self, UnitQuaternion):
355+
# Pass a 2D (N,4) array with norm=False so construction stores
356+
# the conjugated array as-is, bypassing qunit()'s scalar-sign
357+
# canonicalization -- see the note above for why that
358+
# canonicalization must not apply here. A 1D (4,) array would
359+
# instead be caught by the generic arghandler() path first,
360+
# which normalizes/canonicalizes unconditionally regardless of
361+
# norm -- the 2D-array path is what actually honours norm=False.
362+
return self.__class__(
363+
np.array([smb.qconj(d) for d in self.data]), norm=False
364+
)
365+
return self.__class__([smb.qconj(d) for d in self.data])
320366

321367
def norm(self) -> float:
322368
r"""

‎spatialmath/twist.py‎

Lines changed: 14 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -813,7 +813,7 @@ def unit(self):
813813
"""
814814
Unit twist
815815
816-
- ``S.unit()`` is a Twist2 objec3 representing a unit twist aligned with the
816+
- ``S.unit()`` is a Twist3 object representing a unit twist aligned with the
817817
Twist ``S``.
818818
819819
Example:
@@ -825,12 +825,12 @@ def unit(self):
825825
>>> S = Twist3(T)
826826
>>> S.unit()
827827
"""
828-
if smb.iszerovec(self.w):
829-
# rotational twist
830-
return Twist3(self.S / smb.norm(S.w))
831-
else:
832-
# prismatic twist
828+
if self.isprismatic:
829+
# prismatic twist (zero rotation): normalize the direction vector
833830
return Twist3(smb.unitvec(self.v), [0, 0, 0])
831+
else:
832+
# general twist: normalize so |w| == 1
833+
return Twist3(self.S / smb.norm(self.w))
834834

835835
def ad(self):
836836
"""
@@ -974,7 +974,7 @@ def pole(self):
974974
:return: the pole of the twist
975975
:rtype: ndarray(3)
976976
977-
``X.pole()`` is a point on the twist axis. For a pure translation
977+
``X.pole`` is a point on the twist axis. For a pure translation
978978
this point is at infinity.
979979
980980
Example:
@@ -1487,7 +1487,7 @@ def pole(self):
14871487
:return: the pole of the twist
14881488
:rtype: ndarray(2)
14891489
1490-
``X.pole()`` is a point on the twist axis. For a pure translation
1490+
``X.pole`` is a point on the twist axis. For a pure translation
14911491
this point is at infinity.
14921492
14931493
Example:
@@ -1497,7 +1497,7 @@ def pole(self):
14971497
>>> from spatialmath import SE2, Twist2
14981498
>>> T = SE2(1, 2, 0.3)
14991499
>>> S = Twist2(T)
1500-
>>> S.pole()
1500+
>>> S.pole
15011501
15021502
"""
15031503
p = np.cross(np.r_[0, 0, self.w], np.r_[self.v, 0]) / self.theta
@@ -1626,12 +1626,12 @@ def unit(self):
16261626
>>> S = Twist2(T)
16271627
>>> S.unit()
16281628
"""
1629-
if smb.iszerovec(self.w):
1630-
# rotational twist
1631-
return Twist2(self.S / smb.norm(S.w))
1629+
if self.isprismatic:
1630+
# prismatic twist (zero rotation): normalize the direction vector
1631+
return Twist2(smb.unitvec(self.v), 0)
16321632
else:
1633-
# prismatic twist
1634-
return Twist2(smb.unitvec(self.v), [0, 0, 0])
1633+
# general twist: normalize so |w| == 1 (w is a scalar for Twist2)
1634+
return Twist2(self.S / abs(self.w))
16351635

16361636
@property
16371637
def ad(self):

‎tests/test_dualquaternion.py‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -84,11 +84,36 @@ def test_init(self):
8484
dq = UnitDualQuaternion(T)
8585
nt.assert_array_almost_equal(dq.SE3().A, T.A)
8686

87+
def test_init_negative_scalar(self):
88+
# Rx(pi/4) above has no translation and a positive quaternion
89+
# scalar part, so it can't exercise either bug that used to live
90+
# here: SE3() used self.real.conj(), which silently returned
91+
# -conj(real) whenever real had negative scalar part (flipping
92+
# the sign of the recovered translation). This seed's first draw
93+
# is confirmed to produce a UnitQuaternion with negative scalar
94+
# part, so it's kept as a fixed regression case rather than
95+
# relying on randomness at test time.
96+
np.random.seed(0)
97+
T = SE3.Rand()
98+
dq = UnitDualQuaternion(T)
99+
self.assertLess(dq.real.A[0], 0)
100+
nt.assert_array_almost_equal(dq.SE3().A, T.A)
101+
87102
def test_norm(self):
88103
T = SE3.Rx(pi / 4)
89104
dq = UnitDualQuaternion(T)
90105
nt.assert_array_almost_equal(dq.norm(), (1, 0))
91106

107+
def test_norm_negative_scalar(self):
108+
# see test_init_negative_scalar: norm() used the same broken
109+
# conj() and would crash with "math domain error" (sqrt of a
110+
# small negative float) for this case before the fix.
111+
np.random.seed(0)
112+
T = SE3.Rand()
113+
dq = UnitDualQuaternion(T)
114+
self.assertLess(dq.real.A[0], 0)
115+
nt.assert_array_almost_equal(dq.norm(), (1, 0))
116+
92117
def test_multiply(self):
93118
T1 = SE3.Rx(pi / 4)
94119
T2 = SE3.Rz(-pi / 3)
@@ -101,6 +126,28 @@ def test_multiply(self):
101126
d = d1 * d2
102127
nt.assert_array_almost_equal(d.SE3().A, T.A)
103128

129+
def test_vector_transform(self):
130+
# previously untested and broken: the q*P*conj(q) sandwich
131+
# product's translation terms cancel exactly to zero under this
132+
# class's own dual-part embedding convention (dual =
133+
# 0.5*Pure(t)*real), so the old code silently applied only the
134+
# rotation and dropped the translation entirely.
135+
T = SE3(1, 2, 3) * SE3.Rx(0.3)
136+
dq = UnitDualQuaternion(T)
137+
v = np.array([4.0, 5.0, 6.0])
138+
vp = dq * v
139+
expected = (T * v).flatten()
140+
nt.assert_array_almost_equal(vp, expected)
141+
142+
# also check a second, independent transform for good measure
143+
np.random.seed(2)
144+
SE3.Rand()
145+
T = SE3.Rand()
146+
dq = UnitDualQuaternion(T)
147+
vp = dq * v
148+
expected = (T * v).flatten()
149+
nt.assert_array_almost_equal(vp, expected)
150+
104151

105152
# ---------------------------------------------------------------------------------------#
106153
if __name__ == "__main__": # pragma: no cover

‎tests/test_quaternion.py‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -380,6 +380,42 @@ def test_canonic(self):
380380
R = rotz(-pi)
381381
qcompare(UnitQuaternion(R), np.r_[cos(pi / 2), sin(pi / 2) * np.r_[0, 0, 1]])
382382

383+
def test_conj(self):
384+
# plain Quaternion: conj negates the vector part only
385+
q = Quaternion([1, 2, 3, 4])
386+
qcompare(q.conj(), [1, -2, -3, -4])
387+
self.assertIsInstance(q.conj(), Quaternion)
388+
389+
# UnitQuaternion, positive scalar part: matches plain conjugate
390+
u = UnitQuaternion(rotx(0.3))
391+
self.assertGreater(u.A[0], 0)
392+
qcompare(u.conj(), qconj(u.A))
393+
self.assertIsInstance(u.conj(), UnitQuaternion)
394+
395+
# UnitQuaternion, negative scalar part (reachable via norm=False,
396+
# e.g. a >180 deg rotation before its own construction-time
397+
# canonicalization -- constructed directly here to isolate conj()):
398+
# conj() must NOT re-canonicalize the result's sign to be
399+
# positive, unlike normal UnitQuaternion construction. If it did,
400+
# this would silently return -conj(u) instead of the true
401+
# conjugate, breaking the identity u * conj(u) == 1 that
402+
# downstream algebra (e.g. DualQuaternion) depends on.
403+
u = UnitQuaternion(np.array([[-0.5, 0.5, 0.5, 0.5]]), norm=False)
404+
self.assertLess(u.A[0], 0)
405+
qcompare(u.conj(), [-0.5, -0.5, -0.5, -0.5])
406+
self.assertIsInstance(u.conj(), UnitQuaternion)
407+
408+
# the algebraic identity that must hold regardless of sign
409+
qcompare(u * u.conj(), [1, 0, 0, 0])
410+
411+
# multi-valued UnitQuaternion, mixed signs
412+
us = UnitQuaternion(
413+
np.array([[-0.5, 0.5, 0.5, 0.5], [0.5, -0.5, -0.5, -0.5]]), norm=False
414+
)
415+
conjs = us.conj()
416+
qcompare(conjs.data[0], [-0.5, -0.5, -0.5, -0.5])
417+
qcompare(conjs.data[1], [0.5, 0.5, 0.5, 0.5])
418+
383419
def test_convert(self):
384420
# test conversion from rotn matrix to u.quaternion and back
385421
R = rotx(0)

‎tests/test_twist.py‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -213,6 +213,28 @@ def test_prod(self):
213213
x = Twist3([x1, x2])
214214
array_compare(x.prod().SE3(), T1 * T2)
215215

216+
def test_unit(self):
217+
# general (rotational) twist: normalized so |w| == 1
218+
T = SE3(1, 2, 3) * SE3.Rx(0.3)
219+
S = Twist3(T)
220+
u = S.unit()
221+
self.assertAlmostEqual(np.linalg.norm(u.w), 1.0)
222+
nt.assert_array_almost_equal(u.S, S.S / np.linalg.norm(S.w))
223+
224+
# prismatic twist (zero rotation): normalized direction vector,
225+
# previously untested -- this branch raised ValueError before the
226+
# fix (wrong-shape zero argument, S.w typo)
227+
S = Twist3(np.r_[3, 4, 0, 0, 0, 0])
228+
u = S.unit()
229+
self.assertAlmostEqual(np.linalg.norm(u.v), 1.0)
230+
nt.assert_array_almost_equal(u.w, [0, 0, 0])
231+
232+
def test_pole(self):
233+
T = SE3(1, 2, 3) * SE3.Rx(0.3)
234+
S = Twist3(T)
235+
p = S.pole
236+
self.assertEqual(len(p), 3)
237+
216238

217239
class Twist2dTest(unittest.TestCase):
218240
def test_constructor(self):
@@ -375,6 +397,32 @@ def test_prod(self):
375397
x = Twist2([x1, x2])
376398
array_compare(x.prod().SE2(), T1 * T2)
377399

400+
def test_unit(self):
401+
# general (rotational) twist: normalized so |w| == 1. Previously
402+
# broken: branches were swapped (this case fell into the "zero
403+
# rotation" branch and tried to construct Twist2 with a 3-element
404+
# zero argument instead of scalar 0, raising ValueError).
405+
T = SE2(1, 2, 0.3)
406+
S = Twist2(T)
407+
u = S.unit()
408+
self.assertAlmostEqual(abs(u.w), 1.0)
409+
nt.assert_array_almost_equal(u.S, S.S / abs(S.w))
410+
411+
# prismatic twist (zero rotation): normalized direction vector
412+
S = Twist2([3, 4], 0)
413+
u = S.unit()
414+
self.assertAlmostEqual(np.linalg.norm(u.v), 1.0)
415+
self.assertEqual(u.w, 0)
416+
417+
def test_pole(self):
418+
# previously broken: docstring example called S.pole() but pole
419+
# is a @property, not a method -- TypeError: 'numpy.ndarray'
420+
# object is not callable
421+
T = SE2(1, 2, 0.3)
422+
S = Twist2(T)
423+
p = S.pole
424+
self.assertEqual(len(p), 2)
425+
378426

379427
# ---------------------------------------------------------------------------------------#
380428
if __name__ == "__main__":

0 commit comments

Comments
 (0)