Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 10 additions & 5 deletions spatialmath/DualQuaternion.py
Original file line number Diff line number Diff line change
Expand Up @@ -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).

Expand All @@ -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"""
Expand Down
34 changes: 34 additions & 0 deletions tests/test_dualquaternion_norm.py
Original file line number Diff line number Diff line change
@@ -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))