Add mrp_prod_vector

doc/source/api.rst

@@ -178,6 +178,7 @@ Modified Rodrigues Parameters
    ~mrp_double
 
    ~concatenate_mrp
+   ~mrp_prod_vector
 
    ~mrp_from_axis_angle
    ~mrp_from_quaternion

pytransform3d/rotations/__init__.py

@@ -93,7 +93,9 @@
     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 ._mrp import (
+    mrp_near_singularity, norm_mrp, mrp_double, concatenate_mrp,
+    mrp_prod_vector)
 from ._slerp import (slerp_weights, pick_closest_quaternion, quaternion_slerp,
                      axis_angle_slerp, rotor_slerp)
 from ._testing import (
@@ -223,6 +225,7 @@
     "norm_mrp",
     "mrp_double",
     "concatenate_mrp",
+    "mrp_prod_vector",
     "cross_product_matrix",
     "mrp_from_quaternion",
     "quaternion_from_mrp",

pytransform3d/rotations/_mrp.py

@@ -96,7 +96,8 @@ def concatenate_mrp(mrp1, mrp2):
     apply the result to v.
 
     The solution for concatenation of two rotations
-    :math:`\boldsymbol{\psi}_1,\boldsymbol{\psi}_2` is given by Shuster [1]_:
+    :math:`\boldsymbol{\psi}_1,\boldsymbol{\psi}_2` is given by Shuster [1]_
+    (Equation 257):
 
     .. math::
 
@@ -137,3 +138,33 @@ def concatenate_mrp(mrp1, mrp2):
     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)
+
+
+def mrp_prod_vector(mrp, v):
+    r"""Apply rotation represented by MRPs to a vector.
+
+    To apply the rotation defined by modified Rodrigues parameters
+    :math:`\boldsymbol{\psi} \in \mathbb{R}^3` to a vector
+    :math:`\boldsymbol{v} \in \mathbb{R}^3`, we left-concatenate the original
+    MRPs and then right-concatenate its inverted (negative) version.
+
+    Parameters
+    ----------
+    mrp : array-like, shape (3,)
+        Modified Rodrigues parameters.
+
+    v : array-like, shape (3,)
+        3d vector
+
+    Returns
+    -------
+    w : array, shape (3,)
+        3d vector
+
+    See Also
+    --------
+    concatenate_mrp : Concatenates MRPs.
+    q_prod_vector : The same operation with a quaternion.
+    """
+    mrp = check_mrp(mrp)
+    return concatenate_mrp(concatenate_mrp(mrp, v), -mrp)

pytransform3d/rotations/_mrp.pyi

@@ -12,3 +12,6 @@ def mrp_double(mrp: npt.ArrayLike) -> np.ndarray: ...
 
 
 def concatenate_mrp(mrp1: npt.ArrayLike, mrp2: npt.ArrayLike) -> np.ndarray: ...
+
+
+def mrp_prod_vector(mrp: npt.ArrayLike, v: npt.ArrayLike) -> np.ndarray: ...

pytransform3d/test/test_rotations.py

@@ -2440,6 +2440,19 @@ def test_mrp_double():
     assert_array_almost_equal(np.zeros(3), pr.mrp_double(np.zeros(3)))
 
 
+def test_mrp_prod_vector():
+    rng = np.random.default_rng(2183)
+    v = pr.random_vector(rng, 3)
+    assert_array_almost_equal(v, pr.mrp_prod_vector([0, 0, 0], v))
+
+    for _ in range(5):
+        mrp = pr.random_vector(rng, 3)
+        q = pr.quaternion_from_mrp(mrp)
+        v_mrp = pr.mrp_prod_vector(mrp, v)
+        v_q = pr.q_prod_vector(q, v)
+        assert_array_almost_equal(v_mrp, v_q)
+
+
 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)