diff --git a/doc/source/api.rst b/doc/source/api.rst index 6b9774bc..3a5bc8a8 100644 --- a/doc/source/api.rst +++ b/doc/source/api.rst @@ -196,6 +196,7 @@ Singularities and Ambiguities ~euler_near_gimbal_lock ~compact_axis_angle_near_pi + ~mrp_near_singularity Random Sampling --------------- diff --git a/pytransform3d/rotations/__init__.py b/pytransform3d/rotations/__init__.py index c91e9d53..c2861ab5 100644 --- a/pytransform3d/rotations/__init__.py +++ b/pytransform3d/rotations/__init__.py @@ -91,7 +91,7 @@ quaternion_double, quaternion_integrate, quaternion_gradient, concatenate_quaternions, q_conj, q_prod_vector, quaternion_diff, quaternion_dist, quaternion_from_euler) -from ._mrp import concatenate_mrp +from ._mrp import mrp_near_singularity, concatenate_mrp from ._slerp import (slerp_weights, pick_closest_quaternion, quaternion_slerp, axis_angle_slerp, rotor_slerp) from ._testing import ( @@ -217,6 +217,7 @@ "euler_from_quaternion", "quaternion_from_angle", "quaternion_from_euler", + "mrp_near_singularity", "concatenate_mrp", "cross_product_matrix", "mrp_from_quaternion", diff --git a/pytransform3d/rotations/_mrp.py b/pytransform3d/rotations/_mrp.py index edc66ea8..f70cc982 100644 --- a/pytransform3d/rotations/_mrp.py +++ b/pytransform3d/rotations/_mrp.py @@ -1,6 +1,32 @@ """Modified Rodrigues parameters.""" import numpy as np from ._utils import check_mrp +from ._constants import two_pi + + +def mrp_near_singularity(mrp, tolerance=1e-6): + """Check if modified Rodrigues parameters are close to singularity. + + MRPs have a singularity at 2 * pi, i.e., the norm approaches infinity as + the angle approaches 2 * pi. + + Parameters + ---------- + mrp : array-like, shape (3,) + Modified Rodrigues parameters. + + tolerance : float, optional (default: 1e-6) + Tolerance for check. + + Returns + ------- + near_singularity : bool + MRPs are near singularity. + """ + check_mrp(mrp) + mrp_norm = np.linalg.norm(mrp) + angle = np.arctan(mrp_norm) * 4.0 + return abs(angle - two_pi) < tolerance def concatenate_mrp(mrp1, mrp2): diff --git a/pytransform3d/rotations/_mrp.pyi b/pytransform3d/rotations/_mrp.pyi index ff1490f9..2421ef38 100644 --- a/pytransform3d/rotations/_mrp.pyi +++ b/pytransform3d/rotations/_mrp.pyi @@ -2,4 +2,7 @@ import numpy as np import numpy.typing as npt +def mrp_near_singularity(mrp: npt.ArrayLike, tolerance: float = ...) -> bool: ... + + def concatenate_mrp(mrp1: npt.ArrayLike, mrp2: npt.ArrayLike) -> np.ndarray: ... diff --git a/pytransform3d/test/test_rotations.py b/pytransform3d/test/test_rotations.py index 00b6c148..f8bb6e7b 100644 --- a/pytransform3d/test/test_rotations.py +++ b/pytransform3d/test/test_rotations.py @@ -2350,6 +2350,15 @@ def test_norm_angle_precision(): assert_array_equal(pr.norm_angle(a_epsneg), a_epsneg) +def test_mrp_near_singularity(): + axis = np.array([1.0, 0.0, 0.0]) + assert pr.mrp_near_singularity(np.tan(2.0 * np.pi / 4.0) * axis) + assert pr.mrp_near_singularity(np.tan(2.0 * np.pi / 4.0 - 1e-7) * axis) + assert pr.mrp_near_singularity(np.tan(2.0 * np.pi / 4.0 + 1e-7) * axis) + assert not pr.mrp_near_singularity(np.tan(np.pi / 4.0) * axis) + assert not pr.mrp_near_singularity(np.tan(0.0 / 4.0) * axis) + + def test_concatenate_mrp(): rng = np.random.default_rng(283) for _ in range(5):