Add mrp_near_singularity

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
 ---------------

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",

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):

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: ...

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):