Add assert_mrp_equal

doc/source/api.rst

@@ -153,6 +153,7 @@ Modified Rodrigues Parameters
    ~concatenate_mrp
    ~mrp_from_axis_angle
    ~mrp_from_quaternion
+   ~assert_mrp_equal
 
 Jacobians
 ---------

pytransform3d/rotations/__init__.py

@@ -98,7 +98,7 @@
                      axis_angle_slerp, rotor_slerp)
 from ._testing import (
     assert_euler_equal, assert_quaternion_equal, assert_axis_angle_equal,
-    assert_compact_axis_angle_equal, assert_rotation_matrix)
+    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,
@@ -245,6 +245,7 @@
     "assert_axis_angle_equal",
     "assert_compact_axis_angle_equal",
     "assert_rotation_matrix",
+    "assert_mrp_equal",
     "plot_basis",
     "plot_axis_angle",
     "wedge",

pytransform3d/rotations/_testing.py

@@ -3,6 +3,7 @@
 from numpy.testing import assert_array_almost_equal
 from ._utils import norm_axis_angle, norm_compact_axis_angle
 from ._conversions import norm_euler
+from ._mrp import mrp_double
 
 
 def assert_euler_equal(e1, e2, i, j, k, *args, **kwargs):
@@ -159,3 +160,32 @@ def assert_rotation_matrix(R, *args, **kwargs):
     """
     assert_array_almost_equal(np.dot(R, R.T), np.eye(3), *args, **kwargs)
     assert_array_almost_equal(np.linalg.det(R), 1.0, *args, **kwargs)
+
+
+def assert_mrp_equal(mrp1, mrp2, *args, **kwargs):
+    """Raise an assertion if two MRPs are not approximately equal.
+
+    There are two MRPs that represent the same orientation (double cover). See
+    numpy.testing.assert_array_almost_equal for a more detailed documentation
+    of the other parameters.
+
+    Parameters
+    ----------
+    mrp1 : array-like, shape (3,)
+        Modified Rodrigues parameters.
+
+    mrp1 : array-like, shape (3,)
+        Modified Rodrigues parameters.
+
+    args : tuple
+        Positional arguments that will be passed to
+        `assert_array_almost_equal`
+
+    kwargs : dict
+        Positional arguments that will be passed to
+        `assert_array_almost_equal`
+    """
+    try:
+        assert_array_almost_equal(mrp1, mrp2, *args, **kwargs)
+    except AssertionError:
+        assert_array_almost_equal(mrp1, mrp_double(mrp2), *args, **kwargs)

pytransform3d/rotations/_testing.pyi

@@ -15,3 +15,6 @@ def assert_quaternion_equal(q1: npt.ArrayLike, q2: npt.ArrayLike, *args, **kwarg
 
 
 def assert_rotation_matrix(R: npt.ArrayLike, *args, **kwargs): ...
+
+
+def assert_mrp_equal(mrp1: npt.ArrayLike, mrp2: npt.ArrayLike, *args, **kwargs): ...

pytransform3d/test/test_rotations.py

@@ -2432,6 +2432,7 @@ def test_mrp_double():
     mrp_double = pr.mrp_double(mrp)
     q = pr.quaternion_from_mrp(mrp)
     q_double = pr.quaternion_from_mrp(mrp_double)
+    pr.assert_mrp_equal(mrp, mrp_double)
     assert not np.allclose(mrp, mrp_double)
     pr.assert_quaternion_equal(q, q_double)
     assert not np.allclose(q, q_double)