From 223f4828579cab217e4ed69289f49d96fee57599 Mon Sep 17 00:00:00 2001 From: Alexander Fabisch Date: Sat, 19 Oct 2024 23:42:17 +0200 Subject: [PATCH] Add pr.euler_near_gimbal_lock --- doc/source/api.rst | 9 +++++++ pytransform3d/rotations/__init__.py | 4 ++- pytransform3d/rotations/_conversions.py | 34 ++++++++++++++++++++++++ pytransform3d/rotations/_conversions.pyi | 4 +++ pytransform3d/test/test_rotations.py | 17 ++++++++++++ 5 files changed, 67 insertions(+), 1 deletion(-) diff --git a/doc/source/api.rst b/doc/source/api.rst index 9213683f5..afa40ef3f 100644 --- a/doc/source/api.rst +++ b/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 --------------- diff --git a/pytransform3d/rotations/__init__.py b/pytransform3d/rotations/__init__.py index 6a882c2df..bfb7b10ca 100644 --- a/pytransform3d/rotations/__init__.py +++ b/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", diff --git a/pytransform3d/rotations/_conversions.py b/pytransform3d/rotations/_conversions.py index 3f83cae07..5980dffb5 100644 --- a/pytransform3d/rotations/_conversions.py +++ b/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. diff --git a/pytransform3d/rotations/_conversions.pyi b/pytransform3d/rotations/_conversions.pyi index e88a71985..05430c9c2 100644 --- a/pytransform3d/rotations/_conversions.pyi +++ b/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: ... diff --git a/pytransform3d/test/test_rotations.py b/pytransform3d/test/test_rotations.py index e72fea139..812022f07 100644 --- a/pytransform3d/test/test_rotations.py +++ b/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)