Add pr.euler_near_gimbal_lock

doc/source/api.rst

@@ -184,6 +184,15 @@ Normalization
    ~norm_axis_angle
    ~norm_compact_axis_angle
 
+Singularities and Ambiguities
+-----------------------------
+
+.. autosummary::
+   :toctree: _apidoc/
+   :template: function.rst
+
+   ~euler_near_gimbal_lock
+
 Random Sampling
 ---------------
 

pytransform3d/rotations/__init__.py

@@ -28,7 +28,8 @@
     compact_axis_angle_from_matrix,
     matrix_from_quaternion, matrix_from_compact_axis_angle,
     matrix_from_axis_angle, matrix_from_two_vectors,
-    active_matrix_from_angle, norm_euler, matrix_from_euler,
+    active_matrix_from_angle, norm_euler, euler_near_gimbal_lock,
+    matrix_from_euler,
     active_matrix_from_extrinsic_euler_xyx,
     active_matrix_from_intrinsic_euler_xyx,
     active_matrix_from_extrinsic_euler_xyz,
@@ -156,6 +157,7 @@
     "matrix_from_two_vectors",
     "active_matrix_from_angle",
     "norm_euler",
+    "euler_near_gimbal_lock",
     "matrix_from_euler",
     "active_matrix_from_extrinsic_euler_xyx",
     "active_matrix_from_intrinsic_euler_xyx",

pytransform3d/rotations/_conversions.py

@@ -923,6 +923,40 @@ def norm_euler(e, i, j, k):
     return norm_angle([alpha, beta, gamma])
 
 
+def euler_near_gimbal_lock(e, i, j, k, tolerance=1e-6):
+    """Check if Euler angles are close to gimbal lock.
+
+    Parameters
+    ----------
+    e : array-like, shape (3,)
+        Rotation angles in radians about the axes i, j, k in this order.
+
+    i : int from [0, 1, 2]
+        The first rotation axis (0: x, 1: y, 2: z)
+
+    j : int from [0, 1, 2]
+        The second rotation axis (0: x, 1: y, 2: z)
+
+    k : int from [0, 1, 2]
+        The third rotation axis (0: x, 1: y, 2: z)
+
+    tolerance : float
+        Tolerance for the comparison.
+
+    Returns
+    -------
+    near_gimbal_lock : bool
+        Indicates if the Euler angles are near the gimbal lock singularity.
+    """
+    e = norm_euler(e, i, j, k)
+    beta = e[1]
+    proper_euler = i == k
+    if proper_euler:
+        return abs(beta) < tolerance or abs(beta - np.pi) < tolerance
+    else:
+        return abs(abs(beta) - half_pi) < tolerance
+
+
 def matrix_from_euler(e, i, j, k, extrinsic):
     """General method to compute active rotation matrix from any Euler angles.
 

pytransform3d/rotations/_conversions.pyi

@@ -104,6 +104,10 @@ def check_axis_index(name: str, i: int): ...
 def norm_euler(e: npt.ArrayLike, i: int, j: int, k: int) -> np.ndarray: ...
 
 
+def euler_near_gimbal_lock(
+        e: npt.ArrayLike, i: int, j: int, k: int, tolerance: float = ...) -> bool: ...
+
+
 def matrix_from_euler(
         e: npt.ArrayLike, i: int, j: int, k: int, extrinsic: bool) -> np.ndarray: ...
 

pytransform3d/test/test_rotations.py

@@ -2180,6 +2180,23 @@ def test_norm_euler():
             assert -np.pi <= e_norm[2] <= np.pi
 
 
+def test_euler_near_gimbal_lock():
+    assert pr.euler_near_gimbal_lock([0, 0, 0], 1, 2, 1)
+    assert pr.euler_near_gimbal_lock([0, -1e-7, 0], 1, 2, 1)
+    assert pr.euler_near_gimbal_lock([0, 1e-7, 0], 1, 2, 1)
+    assert pr.euler_near_gimbal_lock([0, np.pi, 0], 1, 2, 1)
+    assert pr.euler_near_gimbal_lock([0, np.pi - 1e-7, 0], 1, 2, 1)
+    assert pr.euler_near_gimbal_lock([0, np.pi + 1e-7, 0], 1, 2, 1)
+    assert not pr.euler_near_gimbal_lock([0, 0.5, 0], 1, 2, 1)
+    assert pr.euler_near_gimbal_lock([0, 0.5 * np.pi, 0], 0, 1, 2)
+    assert pr.euler_near_gimbal_lock([0, 0.5 * np.pi - 1e-7, 0], 0, 1, 2)
+    assert pr.euler_near_gimbal_lock([0, 0.5 * np.pi + 1e-7, 0], 0, 1, 2)
+    assert pr.euler_near_gimbal_lock([0, -0.5 * np.pi, 0], 0, 1, 2)
+    assert pr.euler_near_gimbal_lock([0, -0.5 * np.pi - 1e-7, 0], 0, 1, 2)
+    assert pr.euler_near_gimbal_lock([0, -0.5 * np.pi + 1e-7, 0], 0, 1, 2)
+    assert not pr.euler_near_gimbal_lock([0, 0, 0], 0, 1, 2)
+
+
 def test_general_matrix_euler_conversions():
     """General conversion algorithms between matrix and Euler angles."""
     rng = np.random.default_rng(22)