Development branch, release 3.12.0

.github/workflows/python-package.yml

@@ -14,7 +14,7 @@ jobs:
     runs-on: ubuntu-latest
     strategy:
       matrix:
-        python-version: ["3.7", "3.8", "3.9", "3.10", "3.11", "3.12", "3.13"]
+        python-version: ["3.8", "3.9", "3.10", "3.11", "3.12", "3.13"]
 
     steps:
     - uses: actions/checkout@v2

doc/source/api.rst

@@ -414,6 +414,7 @@ Dual Quaternion
    ~check_dual_quaternion
 
    ~dual_quaternion_requires_renormalization
+   ~norm_dual_quaternion
 
    ~assert_unit_dual_quaternion
    ~assert_unit_dual_quaternion_equal

pytransform3d/test/test_transformations.py

@@ -603,24 +603,45 @@ def test_check_dual_quaternion():
     dq4 = pt.check_dual_quaternion(dq3, unit=True)
     assert not pt.dual_quaternion_requires_renormalization(dq4)
 
+    dq5 = np.array([
+        0.94508498, 0.0617101, -0.06483886, 0.31432811,
+        -0.07743254, 0.04985168, -0.26119618, 0.1691491])
+    dq5_not_orthogonal = np.copy(dq5)
+    dq5_not_orthogonal[4:] = np.round(dq5_not_orthogonal[4:], 1)
+    assert pt.dual_quaternion_requires_renormalization(dq5_not_orthogonal)
+
 
 def test_normalize_dual_quaternion():
     dq = [1, 0, 0, 0, 0, 0, 0, 0]
     dq_norm = pt.check_dual_quaternion(dq)
     pt.assert_unit_dual_quaternion(dq_norm)
     assert_array_almost_equal(dq, dq_norm)
+    assert_array_almost_equal(dq, pt.norm_dual_quaternion(dq))
 
     dq = [0, 0, 0, 0, 0, 0, 0, 0]
     dq_norm = pt.check_dual_quaternion(dq)
     pt.assert_unit_dual_quaternion(dq_norm)
     assert_array_almost_equal([1, 0, 0, 0, 0, 0, 0, 0], dq_norm)
+    assert_array_almost_equal(dq_norm, pt.norm_dual_quaternion(dq))
 
     rng = np.random.default_rng(999)
-    for _ in range(5):
+    for _ in range(5):  # norm != 1
         A2B = pt.random_transform(rng)
         dq = rng.standard_normal() * pt.dual_quaternion_from_transform(A2B)
         dq_norm = pt.check_dual_quaternion(dq)
         pt.assert_unit_dual_quaternion(dq_norm)
+        assert_array_almost_equal(dq_norm, pt.norm_dual_quaternion(dq))
+
+    for _ in range(5):  # real and dual quaternion are not orthogonal
+        A2B = pt.random_transform(rng)
+        dq = pt.dual_quaternion_from_transform(A2B)
+        dq_roundoff_error = np.copy(dq)
+        dq_roundoff_error[4:] = np.round(dq_roundoff_error[4:], 3)
+        assert pt.dual_quaternion_requires_renormalization(dq_roundoff_error)
+        dq_norm = pt.norm_dual_quaternion(dq_roundoff_error)
+        pt.assert_unit_dual_quaternion(dq_norm)
+        assert not pt.dual_quaternion_requires_renormalization(dq_norm)
+        assert_array_almost_equal(dq, dq_norm, decimal=3)
 
 
 def test_conversions_between_dual_quternion_and_transform():

pytransform3d/transformations/__init__.py

@@ -31,8 +31,9 @@
     vectors_to_directions, transform)
 from ._pq_operations import pq_slerp
 from ._dual_quaternion_operations import (
-    dual_quaternion_double, dq_q_conj, dq_conj, concatenate_dual_quaternions,
-    dual_quaternion_sclerp, dual_quaternion_power, dq_prod_vector)
+    norm_dual_quaternion, dual_quaternion_double, dq_q_conj, dq_conj,
+    concatenate_dual_quaternions, dual_quaternion_sclerp,
+    dual_quaternion_power, dq_prod_vector)
 from ._random import (
     random_transform, random_screw_axis, random_exponential_coordinates)
 from ._plot import plot_transform, plot_screw
@@ -74,7 +75,7 @@
     "vectors_to_directions", "transform",
     "random_transform", "random_screw_axis", "random_exponential_coordinates",
     "pq_slerp",
-    "dual_quaternion_double", "dq_q_conj", "dq_conj",
+    "norm_dual_quaternion", "dual_quaternion_double", "dq_q_conj", "dq_conj",
     "concatenate_dual_quaternions", "dual_quaternion_sclerp",
     "dual_quaternion_power", "dq_prod_vector",
     "plot_transform", "plot_screw",

pytransform3d/transformations/_dual_quaternion_operations.py

@@ -6,6 +6,60 @@
 from ..rotations import concatenate_quaternions
 
 
+def norm_dual_quaternion(dq):
+    """Normalize unit dual quaternion.
+
+    A unit dual quaternion has a real quaternion with unit norm and an
+    orthogonal real part. Both properties are enforced by multiplying a
+    normalization factor [1]_. This is not always necessary. It is often
+    sufficient to only enforce the unit norm property of the real quaternion.
+    This can also be done with :func:`check_dual_quaternion`.
+
+    Parameters
+    ----------
+    dq : array-like, shape (8,)
+        Dual quaternion to represent transform:
+        (pw, px, py, pz, qw, qx, qy, qz)
+
+    Returns
+    -------
+    dq : array, shape (8,)
+        Unit dual quaternion to represent transform with orthogonal real and
+        dual quaternion.
+
+    See Also
+    --------
+    check_dual_quaternion
+        Input validation of dual quaternion representation. Has an option to
+        normalize the dual quaternion.
+
+    dual_quaternion_requires_renormalization
+        Check if normalization is required.
+
+    References
+    ----------
+    .. [1] enki (2023). Properly normalizing a dual quaternion.
+       https://stackoverflow.com/a/76313524
+    """
+    dq = check_dual_quaternion(dq, unit=False)
+    dq_prod = concatenate_dual_quaternions(dq, dq_q_conj(dq), unit=False)
+
+    prod_real = dq_prod[:4]
+    prod_dual = dq_prod[4:]
+
+    prod_real_norm = np.linalg.norm(prod_real)
+    if prod_real_norm == 0.0:
+        return np.r_[1, 0, 0, 0, dq[4:]]
+    real_inv_sqrt = 1.0 / prod_real_norm
+    dual_inv_sqrt = -0.5 * prod_dual * real_inv_sqrt ** 3
+
+    real = real_inv_sqrt * dq[:4]
+    dual = real_inv_sqrt * dq[4:] + concatenate_quaternions(
+        dual_inv_sqrt, dq[:4])
+
+    return np.hstack((real, dual))
+
+
 def dual_quaternion_double(dq):
     r"""Create another dual quaternion that represents the same transformation.
 
@@ -93,7 +147,7 @@ def dq_q_conj(dq):
     return np.r_[dq[0], -dq[1:4], dq[4], -dq[5:]]
 
 
-def concatenate_dual_quaternions(dq1, dq2):
+def concatenate_dual_quaternions(dq1, dq2, unit=True):
     r"""Concatenate dual quaternions.
 
     We concatenate two dual quaternions by dual quaternion multiplication
@@ -122,6 +176,12 @@ def concatenate_dual_quaternions(dq1, dq2):
         Dual quaternion to represent transform:
         (pw, px, py, pz, qw, qx, qy, qz)
 
+    unit : bool, optional (default: True)
+        Normalize the dual quaternion so that it is a unit dual quaternion.
+        A unit dual quaternion has the properties
+        :math:`p_w^2 + p_x^2 + p_y^2 + p_z^2 = 1` and
+        :math:`p_w q_w + p_x q_x + p_y q_y + p_z q_z = 0`.
+
     Returns
     -------
     dq3 : array, shape (8,)
@@ -133,8 +193,8 @@ def concatenate_dual_quaternions(dq1, dq2):
     pytransform3d.rotations.concatenate_quaternions
         Quaternion multiplication.
     """
-    dq1 = check_dual_quaternion(dq1)
-    dq2 = check_dual_quaternion(dq2)
+    dq1 = check_dual_quaternion(dq1, unit=unit)
+    dq2 = check_dual_quaternion(dq2, unit=unit)
     real = concatenate_quaternions(dq1[:4], dq2[:4])
     dual = (concatenate_quaternions(dq1[:4], dq2[4:]) +
             concatenate_quaternions(dq1[4:], dq2[:4]))

pytransform3d/transformations/_dual_quaternion_operations.pyi

@@ -2,6 +2,9 @@ import numpy as np
 import numpy.typing as npt
 
 
+def norm_dual_quaternion(dq: npt.ArrayLike) -> np.ndarray: ...
+
+
 def dual_quaternion_double(dq: npt.ArrayLike) -> np.ndarray: ...
 
 
@@ -12,7 +15,8 @@ def dq_q_conj(dq: npt.ArrayLike) -> np.ndarray: ...
 
 
 def concatenate_dual_quaternions(
-        dq1: npt.ArrayLike, dq2: npt.ArrayLike) -> np.ndarray: ...
+        dq1: npt.ArrayLike, dq2: npt.ArrayLike,
+        unit: bool = ...) -> np.ndarray: ...
 
 
 def dq_prod_vector(dq: npt.ArrayLike, v: npt.ArrayLike) -> np.ndarray: ...

pytransform3d/transformations/_testing.py

@@ -80,6 +80,19 @@ def assert_unit_dual_quaternion(dq, *args, **kwargs):
     kwargs : dict
         Positional arguments that will be passed to
         `assert_array_almost_equal`
+
+    See Also
+    --------
+    check_dual_quaternion
+        Input validation of dual quaternion representation. Has an option to
+        normalize the dual quaternion.
+
+    dual_quaternion_requires_renormalization
+        Check if normalization is required.
+
+    norm_dual_quaternion
+        Normalization that enforces unit norm and orthogonality of the real and
+        dual quaternion.
     """
     real = dq[:4]
     dual = dq[4:]

pytransform3d/transformations/_utils.py

@@ -377,8 +377,8 @@ def dual_quaternion_requires_renormalization(dq, tolerance=1e-6):
     r"""Check if dual quaternion requires renormalization.
 
     Dual quaternions that represent transformations in 3D should have unit
-    norm. Since the real and the dual quaternion are orthogonal, their
-    product is 0. In addition, :math:`\epsilon^2 = 0`. Hence,
+    norm. Since the real and the dual quaternion are orthogonal, their dot
+    product should be 0. In addition, :math:`\epsilon^2 = 0`. Hence,
 
     .. math::
 
@@ -395,6 +395,9 @@ def dual_quaternion_requires_renormalization(dq, tolerance=1e-6):
 
     i.e., the norm only depends on the real quaternion.
 
+    This function checks unit norm and orthogonality of the real and dual
+    part.
+
     Parameters
     ----------
     dq : array-like, shape (8,)
@@ -414,8 +417,19 @@ def dual_quaternion_requires_renormalization(dq, tolerance=1e-6):
     check_dual_quaternion
         Input validation of dual quaternion representation. Has an option to
         normalize the dual quaternion.
+
+    norm_dual_quaternion
+        Normalization that enforces unit norm and orthogonality of the real and
+        dual quaternion.
+
+    assert_unit_dual_quaternion
+        Checks unit norm and orthogonality of real and dual quaternion.
     """
-    return abs(np.linalg.norm(dq[:4]) - 1.0) > tolerance
+    real = dq[:4]
+    dual = dq[4:]
+    real_norm = np.linalg.norm(real)
+    real_dual_dot = np.dot(real, dual)
+    return abs(real_norm - 1.0) > tolerance or abs(real_dual_dot) > tolerance
 
 
 def check_dual_quaternion(dq, unit=True):
@@ -456,6 +470,12 @@ def check_dual_quaternion(dq, unit=True):
     ------
     ValueError
         If input is invalid
+
+    See Also
+    --------
+    norm_dual_quaternion
+        Normalization that enforces unit norm and orthogonality of the real and
+        dual quaternion.
     """
     dq = np.asarray(dq, dtype=np.float64)
     if dq.ndim != 1 or dq.shape[0] != 8: