Add mrp_double

doc/source/api.rst

@@ -149,6 +149,7 @@ Modified Rodrigues Parameters
    ~check_mrp
    ~mrp_near_singularity
    ~norm_mrp
+   ~mrp_double
    ~concatenate_mrp
    ~mrp_from_axis_angle
    ~mrp_from_quaternion

pytransform3d/rotations/__init__.py

@@ -93,7 +93,7 @@
     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, concatenate_mrp
+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 (
@@ -221,6 +221,7 @@
     "quaternion_from_euler",
     "mrp_near_singularity",
     "norm_mrp",
+    "mrp_double",
     "concatenate_mrp",
     "cross_product_matrix",
     "mrp_from_quaternion",

pytransform3d/rotations/_mrp.py

@@ -52,6 +52,35 @@ def mrp_near_singularity(mrp, tolerance=1e-6):
     return abs(angle - two_pi) < tolerance
 
 
+def mrp_double(mrp):
+    r"""Other modified Rodrigues parameters representing the same orientation.
+
+    MRPs have two representations for the same rotation:
+    :math:`\boldsymbol{\psi}` and :math:`-\frac{1}{||\boldsymbol{\psi}||^2}
+    \boldsymbol{\psi}` represent the same rotation and correspond to two
+    antipodal unit quaternions [1]_.
+
+    Parameters
+    ----------
+    mrp : array-like, shape (3,)
+        Modified Rodrigues parameters.
+
+    Returns
+    -------
+    mrp_double : array, shape (3,)
+        Different modified Rodrigues parameters that represent the same
+        orientation.
+
+    References
+    ----------
+    .. [1] Shuster, M. D. (1993). A Survey of Attitude Representations.
+       Journal of the Astronautical Sciences, 41, 439-517.
+       http://malcolmdshuster.com/Pub_1993h_J_Repsurv_scan.pdf
+    """
+    mrp = check_mrp(mrp)
+    return mrp / -np.dot(mrp, mrp)
+
+
 def concatenate_mrp(mrp1, mrp2):
     r"""Concatenate two rotations defined by modified Rodrigues parameters.
 

pytransform3d/rotations/_mrp.pyi

@@ -8,4 +8,7 @@ def norm_mrp(mrp: npt.ArrayLike) -> np.ndarray: ...
 def mrp_near_singularity(mrp: npt.ArrayLike, tolerance: float = ...) -> bool: ...
 
 
+def mrp_double(mrp: npt.ArrayLike) -> np.ndarray: ...
+
+
 def concatenate_mrp(mrp1: npt.ArrayLike, mrp2: npt.ArrayLike) -> np.ndarray: ...

pytransform3d/test/test_rotations.py

@@ -2426,6 +2426,17 @@ def test_norm_mrp():
         [1.0, 0.0, 0.0, 0.0], pr.axis_angle_from_mrp(mrp_norm))
 
 
+def test_mrp_double():
+    rng = np.random.default_rng(23238)
+    mrp = pr.random_vector(rng, 3)
+    mrp_double = pr.mrp_double(mrp)
+    q = pr.quaternion_from_mrp(mrp)
+    q_double = pr.quaternion_from_mrp(mrp_double)
+    assert not np.allclose(mrp, mrp_double)
+    pr.assert_quaternion_equal(q, q_double)
+    assert not np.allclose(q, q_double)
+
+
 def test_assert_euler_almost_equal():
     pr.assert_euler_equal(
         [0.2, 0.3, -0.5], [0.2 + np.pi, -0.3, -0.5 - np.pi], 0, 1, 0)