diff --git a/spatialmath/DualQuaternion.py b/spatialmath/DualQuaternion.py index 6e657192..03e90ce6 100644 --- a/spatialmath/DualQuaternion.py +++ b/spatialmath/DualQuaternion.py @@ -104,6 +104,10 @@ def norm(self) -> Tuple[float, float]: :return: Norm as a dual number :rtype: 2-tuple + For a nonzero real part, the norm of ``p + epsilon*q`` is the dual + number ``(||p||, dot(p, q) / ||p||)``. Its dual coefficient is signed. + The existing convention for a zero real part is ``(0, 0)``. + The norm of a ``UnitDualQuaternion`` is unity, represented by the dual number (1,0). @@ -117,11 +121,12 @@ def norm(self) -> Tuple[float, float]: """ a = self.real * self.real.conj() b = self.real * self.dual.conj() + self.dual * self.real.conj() - # 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))) + # sqrt(a + epsilon*b) = sqrt(a) + epsilon*b/(2*sqrt(a)). + # The dual coefficient can have either sign, unlike the real norm. + real_norm = base.sqrt(max(0.0, a.s)) + if real_norm == 0: + return (0.0, 0.0) # retain the existing zero-real convention + return (real_norm, b.s / (2 * real_norm)) def conj(self) -> Self: r""" diff --git a/tests/test_dualquaternion_norm.py b/tests/test_dualquaternion_norm.py new file mode 100644 index 00000000..fefeb5bb --- /dev/null +++ b/tests/test_dualquaternion_norm.py @@ -0,0 +1,34 @@ +import numpy as np +import numpy.testing as nt +import pytest + +from spatialmath import DualQuaternion, Quaternion, SE3, UnitDualQuaternion + + +@pytest.mark.parametrize("real", [2.0, -2.0, 0.25]) +@pytest.mark.parametrize("dual", [3.0, -3.0, 0.0]) +def test_scalar_dual_quaternion_norm(real, dual): + dq = DualQuaternion(Quaternion([real, 0, 0, 0]), Quaternion([dual, 0, 0, 0])) + nt.assert_allclose(dq.norm(), (abs(real), np.sign(real) * dual)) + + +@pytest.mark.parametrize("seed", range(10)) +def test_norm_squares_to_product_with_conjugate(seed): + rng = np.random.default_rng(seed) + dq = DualQuaternion(Quaternion(rng.normal(size=4)), Quaternion(rng.normal(size=4))) + real, dual = dq.norm() + squared = dq * dq.conj() + nt.assert_allclose( + [real**2, 2 * real * dual], [squared.real.s, squared.dual.s], atol=1e-14 + ) + + +@pytest.mark.parametrize("angle", [0.0, 0.3, -2.0]) +def test_unit_norm_with_translation(angle): + dq = UnitDualQuaternion(SE3(1, -2, 3) * SE3.Rx(angle)) + nt.assert_allclose(dq.norm(), (1, 0), atol=1e-14) + + +def test_zero_real_part_keeps_existing_zero_norm(): + dq = DualQuaternion(Quaternion([0, 0, 0, 0]), Quaternion([1, 2, 3, 4])) + nt.assert_allclose(dq.norm(), (0, 0))