From 5f69606649f4a94392fca4c4236bae4c29946f08 Mon Sep 17 00:00:00 2001
From: Alexander Fabisch <afabisch@googlemail.com>
Date: Tue, 22 Oct 2024 18:50:06 +0200
Subject: [PATCH] Add mrp_double

---
 doc/source/api.rst                   |  1 +
 pytransform3d/rotations/__init__.py  |  3 ++-
 pytransform3d/rotations/_mrp.py      | 29 ++++++++++++++++++++++++++++
 pytransform3d/rotations/_mrp.pyi     |  3 +++
 pytransform3d/test/test_rotations.py | 11 +++++++++++
 5 files changed, 46 insertions(+), 1 deletion(-)

diff --git a/doc/source/api.rst b/doc/source/api.rst
index 5b1f88460..29143f213 100644
--- a/doc/source/api.rst
+++ b/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
diff --git a/pytransform3d/rotations/__init__.py b/pytransform3d/rotations/__init__.py
index 83e1e69ca..d1c61c910 100644
--- a/pytransform3d/rotations/__init__.py
+++ b/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",
diff --git a/pytransform3d/rotations/_mrp.py b/pytransform3d/rotations/_mrp.py
index be9b61950..5a63034e7 100644
--- a/pytransform3d/rotations/_mrp.py
+++ b/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.
 
diff --git a/pytransform3d/rotations/_mrp.pyi b/pytransform3d/rotations/_mrp.pyi
index 4a99461f1..6d358f39c 100644
--- a/pytransform3d/rotations/_mrp.pyi
+++ b/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: ...
diff --git a/pytransform3d/test/test_rotations.py b/pytransform3d/test/test_rotations.py
index 21ec68ac2..9215e3b8e 100644
--- a/pytransform3d/test/test_rotations.py
+++ b/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)