diff --git a/doc/source/api.rst b/doc/source/api.rst index b8050c92..08281eb2 100644 --- a/doc/source/api.rst +++ b/doc/source/api.rst @@ -361,6 +361,7 @@ Dual Quaternion :template: function.rst ~check_dual_quaternion + ~dual_quaternion_requires_renormalization ~dq_conj ~dq_q_conj ~concatenate_dual_quaternions diff --git a/pytransform3d/test/test_transformations.py b/pytransform3d/test/test_transformations.py index fc307dc2..651a7d89 100644 --- a/pytransform3d/test/test_transformations.py +++ b/pytransform3d/test/test_transformations.py @@ -599,6 +599,9 @@ def test_check_dual_quaternion(): dq3 = pt.check_dual_quaternion([0] * 8, unit=False) assert dq3.shape[0] == 8 + assert pt.dual_quaternion_requires_renormalization(dq3) + dq4 = pt.check_dual_quaternion(dq3, unit=True) + assert not pt.dual_quaternion_requires_renormalization(dq4) def test_normalize_dual_quaternion(): diff --git a/pytransform3d/transformations/__init__.py b/pytransform3d/transformations/__init__.py index 564a6500..7f0954ea 100644 --- a/pytransform3d/transformations/__init__.py +++ b/pytransform3d/transformations/__init__.py @@ -5,7 +5,8 @@ from ._utils import ( transform_requires_renormalization, check_transform, check_pq, check_screw_parameters, check_screw_axis, check_exponential_coordinates, - check_screw_matrix, check_transform_log, check_dual_quaternion) + check_screw_matrix, check_transform_log, + dual_quaternion_requires_renormalization, check_dual_quaternion) from ._conversions import ( transform_from, rotate_transform, translate_transform, pq_from_transform, transform_from_pq, @@ -48,7 +49,8 @@ "transform_requires_renormalization", "check_transform", "check_pq", "check_screw_parameters", "check_screw_axis", "check_exponential_coordinates", "check_screw_matrix", - "check_transform_log", "check_dual_quaternion", + "check_transform_log", "dual_quaternion_requires_renormalization", + "check_dual_quaternion", "transform_from", "rotate_transform", "translate_transform", "pq_from_transform", "transform_from_pq", "transform_from_transform_log", "transform_log_from_transform", diff --git a/pytransform3d/transformations/_utils.py b/pytransform3d/transformations/_utils.py index 99383841..b89a1ed2 100644 --- a/pytransform3d/transformations/_utils.py +++ b/pytransform3d/transformations/_utils.py @@ -373,6 +373,51 @@ def check_transform_log(transform_log, tolerance=1e-6, strict_check=True): return transform_log +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, + + .. math:: + + ||\boldsymbol{p} + \epsilon \boldsymbol{q}|| + = + \sqrt{(\boldsymbol{p} + \epsilon \boldsymbol{q}) \cdot + (\boldsymbol{p} + \epsilon \boldsymbol{q})} + = + \sqrt{\boldsymbol{p}\cdot\boldsymbol{p} + + 2\epsilon \boldsymbol{p}\cdot\boldsymbol{q} + + \epsilon^2 \boldsymbol{q}\cdot\boldsymbol{q}} + = + \sqrt{\boldsymbol{p}\cdot\boldsymbol{p}}, + + i.e., the norm only depends on the real quaternion. + + Parameters + ---------- + dq : array-like, shape (8,) + Dual quaternion to represent transform: + (pw, px, py, pz, qw, qx, qy, qz) + + tolerance : float, optional (default: 1e-6) + Tolerance for check. + + Returns + ------- + required : bool + Indicates if renormalization is required. + + See Also + -------- + check_dual_quaternion + Input validation of dual quaternion representation. Has an option to + normalize the dual quaternion. + """ + return abs(np.linalg.norm(dq[:4]) - 1.0) > tolerance + + def check_dual_quaternion(dq, unit=True): """Input validation of dual quaternion representation. @@ -418,7 +463,8 @@ def check_dual_quaternion(dq, unit=True): "array-like object with shape %s" % (dq.shape,)) if unit: # Norm of a dual quaternion only depends on the real part because - # the dual part vanishes with epsilon ** 2 = 0. + # the dual part vanishes with (1) epsilon ** 2 = 0 and (2) the real + # and dual part being orthogonal, i.e., their product is 0. real_norm = np.linalg.norm(dq[:4]) if real_norm == 0.0: return np.r_[1, 0, 0, 0, dq[4:]] diff --git a/pytransform3d/transformations/_utils.pyi b/pytransform3d/transformations/_utils.pyi index 4b5c555f..cd90a60d 100644 --- a/pytransform3d/transformations/_utils.pyi +++ b/pytransform3d/transformations/_utils.pyi @@ -33,4 +33,7 @@ def check_transform_log(transform_log: npt.ArrayLike, tolerance: float = ..., strict_check: bool = ...) -> np.ndarray: ... +def dual_quaternion_requires_renormalization(dq: npt.ArrayLike, tolerance: float = ...) -> bool: ... + + def check_dual_quaternion(dq: npt.ArrayLike, unit: bool = ...) -> np.ndarray: ...