Concatenate MRPs

doc/source/api.rst

@@ -137,6 +137,15 @@ Rotors
    ~rotor_apply
    ~rotor_slerp
 
+Modified Rodrigues Parameters
+-----------------------------
+
+.. autosummary::
+   :toctree: _apidoc/
+   :template: function.rst
+
+   ~concatenate_mrp
+
 Plotting
 --------
 

doc/source/user_guide/rotations.rst

@@ -44,7 +44,7 @@ is not implemented in pytransform3d then it is shown in brackets.
 | Euler angles                           | `(1)`         | No                 | No            | No            |
 | :math:`(\alpha, \beta, \gamma)`        |               |                    |               |               |
 +----------------------------------------+---------------+--------------------+---------------+---------------+
-| Modified Rodrigues parameters          | Negative      | No                 | No            | No            |
+| Modified Rodrigues parameters          | Negative      | No                 | Yes           | No            |
 | :math:`\psi`                           |               |                    |               |               |
 +----------------------------------------+---------------+--------------------+---------------+---------------+
 

pytransform3d/rotations/__init__.py

@@ -87,6 +87,7 @@
 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
 from ._slerp import (slerp_weights, pick_closest_quaternion, quaternion_slerp,
                      axis_angle_slerp, rotor_slerp)
 from ._testing import (
@@ -207,6 +208,7 @@
     "euler_from_quaternion",
     "quaternion_from_angle",
     "quaternion_from_euler",
+    "concatenate_mrp",
     "cross_product_matrix",
     "mrp_from_quaternion",
     "quaternion_from_mrp",

pytransform3d/rotations/_mrp.py

@@ -0,0 +1,55 @@
+"""Modified Rodrigues parameters."""
+import numpy as np
+from ._utils import check_mrp
+
+
+def concatenate_mrp(mrp1, mrp2):
+    r"""Concatenate two rotations defined by modified Rodrigues parameters.
+
+    Suppose we want to apply two extrinsic rotations given by modified
+    Rodrigues parameters mrp1 and mrp2 to a vector v. We can either apply mrp2
+    to v and then mrp1 to the result or we can concatenate mrp1 and mrp2 and
+    apply the result to v.
+
+    The solution for concatenation of two rotations
+    :math:`\boldsymbol{p}_1,\boldsymbol{p}_2` is given by Shuster [1]_:
+
+    .. math::
+
+        \boldsymbol{p} =
+        \frac{
+        (1 - ||\boldsymbol{p}_1||^2) \boldsymbol{p}_2
+        + (1 - ||\boldsymbol{p}_2||^2) \boldsymbol{p}_1
+        - 2 \boldsymbol{p}_2 \times \boldsymbol{p}_1}
+        {1 + ||\boldsymbol{p}_2||^2 ||\boldsymbol{p}_1||^2
+        - 2 \boldsymbol{p}_2 \cdot \boldsymbol{p}_1}.
+
+    Parameters
+    ----------
+    mrp1 : array-like, shape (3,)
+        Modified Rodrigues parameters.
+
+    mrp2 : array-like, shape (3,)
+        Modified Rodrigues parameters.
+
+    Returns
+    -------
+    mrp12 : array, shape (3,)
+        Modified Rodrigues parameters that represent the concatenated rotation
+        of mrp1 and mrp2.
+
+    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
+    """
+    mrp1 = check_mrp(mrp1)
+    mrp2 = check_mrp(mrp2)
+    norm1_sq = np.linalg.norm(mrp1) ** 2
+    norm2_sq = np.linalg.norm(mrp2) ** 2
+    cross_product = np.cross(mrp2, mrp1)
+    scalar_product = np.dot(mrp2, mrp1)
+    return (
+        (1.0 - norm1_sq) * mrp2 + (1.0 - norm2_sq) * mrp1 - 2.0 * cross_product
+    ) / (1.0 + norm2_sq * norm1_sq - 2.0 * scalar_product)

pytransform3d/rotations/_mrp.pyi

@@ -0,0 +1,5 @@
+import numpy as np
+import numpy.typing as npt
+
+
+def concatenate_mrp(mrp1: npt.ArrayLike, mrp2: npt.ArrayLike) -> np.ndarray: ...

pytransform3d/rotations/_quaternion_operations.py

@@ -100,7 +100,7 @@ def concatenate_quaternions(q1, q2):
 
     Returns
     -------
-    q12 : array-like, shape (4,)
+    q12 : array, shape (4,)
         Quaternion that represents the concatenated rotation q1 * q2
 
     See Also

pytransform3d/test/test_rotations.py

@@ -2251,3 +2251,15 @@ def test_norm_angle_precision():
 
     assert_array_equal(pr.norm_angle(a_eps), a_eps)
     assert_array_equal(pr.norm_angle(a_epsneg), a_epsneg)
+
+
+def test_concatenate_mrp():
+    rng = np.random.default_rng(283)
+    for _ in range(5):
+        q1 = pr.random_quaternion(rng)
+        q2 = pr.random_quaternion(rng)
+        q12 = pr.concatenate_quaternions(q1, q2)
+        mrp1 = pr.mrp_from_quaternion(q1)
+        mrp2 = pr.mrp_from_quaternion(q2)
+        mrp12 = pr.concatenate_mrp(mrp1, mrp2)
+        pr.assert_quaternion_equal(q12, pr.quaternion_from_mrp(mrp12))