Provide functions to write numerically stable code and avoid pitfalls

doc/source/index.rst

@@ -40,6 +40,8 @@ pytransform3d offers...
   translation / position
 * conversions between those representations
 * clear documentation of conventions
+* functions to check for common pitfalls (e.g., singularities, ambiguities,
+  discontinuities)
 * tight coupling with matplotlib to quickly visualize (or animate)
   transformations
 * the TransformManager which organizes complex chains of transformations

doc/source/user_guide/euler_angles.rst

@@ -71,7 +71,8 @@ and Cardan angles are usually restricted to
 
     -\pi \leq \alpha < \pi, \qquad -\frac{\pi}{2} \leq \beta \leq \frac{\pi}{2}, \qquad -\pi \leq \gamma < \pi
 
-to make these representations unique.
+to make these representations unique (using
+:func:`~pytransform3d.rotations.norm_euler`).
 
 An alternative convention limits the range of :math:`\alpha` and :math:`\gamma`
 to :math:`\left[0, 2 \pi\right)`.
@@ -89,7 +90,8 @@ to an interval of length :math:`2\pi`: either all pairs of angles that
 satisfy :math:`\alpha + \gamma = constant` or all pairs of angles
 that satisfy :math:`\alpha - \gamma = constant`. When we reconstruct
 Euler angles from a rotation matrix, we set one of these angles to 0 to
-determine the other.
+determine the other. We can check if Euler angles are close to gimbal lock
+with :func:`~pytransform3d.rotations.euler_near_gimbal_lock`.
 
 -----------
 Other Names
@@ -100,79 +102,6 @@ xyz Cardan angles can also be called roll, pitch, and yaw (or sometimes
 the intrinsic convention is used here as well). Roll is a rotation about
 x, pitch is a rotation about y and yaw is a rotation about z.
 
----------------------------------------------------
-API: Rotation Matrix / Quaternion from Euler Angles
----------------------------------------------------
-
-.. currentmodule:: pytransform3d.rotations
-
-.. autosummary::
-   :toctree: _apidoc/
-   :template: function.rst
-
-   ~quaternion_from_euler
-   ~matrix_from_euler
-   ~active_matrix_from_intrinsic_euler_xzx
-   ~active_matrix_from_extrinsic_euler_xzx
-   ~active_matrix_from_intrinsic_euler_xyx
-   ~active_matrix_from_extrinsic_euler_xyx
-   ~active_matrix_from_intrinsic_euler_yxy
-   ~active_matrix_from_extrinsic_euler_yxy
-   ~active_matrix_from_intrinsic_euler_yzy
-   ~active_matrix_from_extrinsic_euler_yzy
-   ~active_matrix_from_intrinsic_euler_zyz
-   ~active_matrix_from_extrinsic_euler_zyz
-   ~active_matrix_from_intrinsic_euler_zxz
-   ~active_matrix_from_extrinsic_euler_zxz
-   ~active_matrix_from_intrinsic_euler_xzy
-   ~active_matrix_from_extrinsic_euler_xzy
-   ~active_matrix_from_intrinsic_euler_xyz
-   ~active_matrix_from_extrinsic_euler_xyz
-   ~active_matrix_from_intrinsic_euler_yxz
-   ~active_matrix_from_extrinsic_euler_yxz
-   ~active_matrix_from_intrinsic_euler_yzx
-   ~active_matrix_from_extrinsic_euler_yzx
-   ~active_matrix_from_intrinsic_euler_zyx
-   ~active_matrix_from_extrinsic_euler_zyx
-   ~active_matrix_from_intrinsic_euler_zxy
-   ~active_matrix_from_extrinsic_euler_zxy
-   ~active_matrix_from_extrinsic_roll_pitch_yaw
-
----------------------------------------------------
-API: Euler Angles from Rotation Matrix / Quaternion
----------------------------------------------------
-
-.. autosummary::
-   :toctree: _apidoc/
-   :template: function.rst
-
-   ~euler_from_quaternion
-   ~euler_from_matrix
-   ~intrinsic_euler_xzx_from_active_matrix
-   ~extrinsic_euler_xzx_from_active_matrix
-   ~intrinsic_euler_xyx_from_active_matrix
-   ~extrinsic_euler_xyx_from_active_matrix
-   ~intrinsic_euler_yxy_from_active_matrix
-   ~extrinsic_euler_yxy_from_active_matrix
-   ~intrinsic_euler_yzy_from_active_matrix
-   ~extrinsic_euler_yzy_from_active_matrix
-   ~intrinsic_euler_zyz_from_active_matrix
-   ~extrinsic_euler_zyz_from_active_matrix
-   ~intrinsic_euler_zxz_from_active_matrix
-   ~extrinsic_euler_zxz_from_active_matrix
-   ~intrinsic_euler_xzy_from_active_matrix
-   ~extrinsic_euler_xzy_from_active_matrix
-   ~intrinsic_euler_xyz_from_active_matrix
-   ~extrinsic_euler_xyz_from_active_matrix
-   ~intrinsic_euler_yxz_from_active_matrix
-   ~extrinsic_euler_yxz_from_active_matrix
-   ~intrinsic_euler_yzx_from_active_matrix
-   ~extrinsic_euler_yzx_from_active_matrix
-   ~intrinsic_euler_zyx_from_active_matrix
-   ~extrinsic_euler_zyx_from_active_matrix
-   ~intrinsic_euler_zxy_from_active_matrix
-   ~extrinsic_euler_zxy_from_active_matrix
-
 ----------
 References
 ----------

doc/source/user_guide/introduction.rst

@@ -149,7 +149,7 @@ representations on the following pages.
 | :math:`(\pmb{p}, \pmb{q})`             |                     |                  |               |
 +----------------------------------------+---------------------+------------------+---------------+
 | Dual quaternion                        | (8,)                | X                | X             |
-| :math:`\pmb{p} + \epsilon\pmb{q}`      |                     |                  |               |
+| :math:`\boldsymbol{\sigma}`            |                     |                  |               |
 +----------------------------------------+---------------------+------------------+---------------+
 
 ----------

doc/source/user_guide/transformations.rst

@@ -59,7 +59,7 @@ another representation. The following table is an overview.
 | :math:`(\pmb{p}, \pmb{q})`             |            |                          |               |               |                 |
 +----------------------------------------+------------+--------------------------+---------------+---------------+-----------------+
 | Dual quaternion                        | Quaternion | Yes                      | Yes           | ScLERP        | Required        |
-| :math:`\pmb{p} + \epsilon \pmb{q}`     | Conjugate  |                          |               |               |                 |
+| :math:`\boldsymbol{\sigma}`            | Conjugate  |                          |               |               |                 |
 +----------------------------------------+------------+--------------------------+---------------+---------------+-----------------+
 
 ---------------------
@@ -323,7 +323,8 @@ A dual quaternion consists of a real quaternion and a dual quaternion:
 
 .. math::
 
-    \boldsymbol{p} + \epsilon \boldsymbol{q} = p_w + p_x i + p_y j + p_z k + \epsilon (q_w + q_x i + q_y j + q_z k),
+    \boldsymbol{\sigma} = \boldsymbol{p} + \epsilon \boldsymbol{q}
+    = p_w + p_x i + p_y j + p_z k + \epsilon (q_w + q_x i + q_y j + q_z k),
 
 where :math:`\epsilon^2 = 0` and :math:`\epsilon \neq 0`.
 We use unit dual quaternions to represent
@@ -345,9 +346,9 @@ interpolation between two dual quaternions is possible (with
 
 .. warning::
 
-    The unit dual quaternions :math:`\boldsymbol{p} + \epsilon \boldsymbol{q}`
-    and :math:`-\boldsymbol{p} - \epsilon \boldsymbol{q}` represent exactly
-    the same transformation.
+    The unit dual quaternions
+    :math:`\boldsymbol{\sigma} = \boldsymbol{p} + \epsilon \boldsymbol{q}` and
+    :math:`-\boldsymbol{\sigma}` represent exactly the same transformation.
 
 The reason for this ambiguity is that the real quaternion
 :math:`\boldsymbol{p}` represents the orientation component, the dual

pytransform3d/rotations/__init__.py

@@ -7,7 +7,9 @@
 from ._utils import (
     norm_angle, norm_vector, angle_between_vectors, perpendicular_to_vector,
     vector_projection, perpendicular_to_vectors,
-    norm_axis_angle, norm_compact_axis_angle, norm_matrix,
+    norm_axis_angle, compact_axis_angle_near_pi, norm_compact_axis_angle,
+    matrix_requires_renormalization, norm_matrix,
+    quaternion_requires_renormalization,
     plane_basis_from_normal,
     check_skew_symmetric_matrix, check_matrix, check_quaternion,
     check_quaternions, check_axis_angle, check_compact_axis_angle,
@@ -27,7 +29,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, 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,
@@ -83,16 +86,19 @@
     quaternion_from_angle,
     cross_product_matrix,
     mrp_from_quaternion,
-    quaternion_from_mrp)
-from ._quaternion_operations import (
-    quaternion_integrate, quaternion_gradient, concatenate_quaternions, q_conj,
-    q_prod_vector, quaternion_diff, quaternion_dist, quaternion_from_euler)
-from ._mrp import concatenate_mrp
+    quaternion_from_mrp,
+    mrp_from_axis_angle,
+    axis_angle_from_mrp)
+from ._quaternions import (
+    quaternion_double, quaternion_integrate, quaternion_gradient,
+    concatenate_quaternions, q_conj, q_prod_vector, quaternion_diff,
+    quaternion_dist, quaternion_from_euler)
+from ._mrp import mrp_near_singularity, norm_mrp, mrp_double, concatenate_mrp
 from ._slerp import (slerp_weights, pick_closest_quaternion, quaternion_slerp,
                      axis_angle_slerp, rotor_slerp)
 from ._testing import (
-    assert_quaternion_equal, assert_axis_angle_equal,
-    assert_compact_axis_angle_equal, assert_rotation_matrix)
+    assert_euler_equal, assert_quaternion_equal, assert_axis_angle_equal,
+    assert_compact_axis_angle_equal, assert_rotation_matrix, assert_mrp_equal)
 from ._plot import plot_basis, plot_axis_angle, plot_bivector
 from ._rotors import (
     wedge, geometric_product, rotor_apply, rotor_reverse, concatenate_rotors,
@@ -121,8 +127,11 @@
     "vector_projection",
     "perpendicular_to_vectors",
     "norm_axis_angle",
+    "compact_axis_angle_near_pi",
     "norm_compact_axis_angle",
+    "matrix_requires_renormalization",
     "norm_matrix",
+    "quaternion_requires_renormalization",
     "random_vector",
     "random_axis_angle",
     "random_compact_axis_angle",
@@ -153,6 +162,8 @@
     "matrix_from_axis_angle",
     "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",
@@ -208,10 +219,16 @@
     "euler_from_quaternion",
     "quaternion_from_angle",
     "quaternion_from_euler",
+    "mrp_near_singularity",
+    "norm_mrp",
+    "mrp_double",
     "concatenate_mrp",
     "cross_product_matrix",
     "mrp_from_quaternion",
     "quaternion_from_mrp",
+    "mrp_from_axis_angle",
+    "axis_angle_from_mrp",
+    "quaternion_double",
     "quaternion_integrate",
     "quaternion_gradient",
     "concatenate_quaternions",
@@ -223,10 +240,12 @@
     "pick_closest_quaternion",
     "quaternion_slerp",
     "axis_angle_slerp",
+    "assert_euler_equal",
     "assert_quaternion_equal",
     "assert_axis_angle_equal",
     "assert_compact_axis_angle_equal",
     "assert_rotation_matrix",
+    "assert_mrp_equal",
     "plot_basis",
     "plot_axis_angle",
     "wedge",

pytransform3d/rotations/_constants.py

@@ -17,4 +17,4 @@
 eps = 1e-7
 
 two_pi = 2.0 * np.pi
-
+half_pi = 0.5 * np.pi