diff --git a/doc/source/api.rst b/doc/source/api.rst index 8d87639da..27876c1d5 100644 --- a/doc/source/api.rst +++ b/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 -------- diff --git a/doc/source/user_guide/rotations.rst b/doc/source/user_guide/rotations.rst index 1b4b88d92..9cf74ba69 100644 --- a/doc/source/user_guide/rotations.rst +++ b/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` | | | | | +----------------------------------------+---------------+--------------------+---------------+---------------+ diff --git a/pytransform3d/rotations/__init__.py b/pytransform3d/rotations/__init__.py index 402e1eb22..23bbac453 100644 --- a/pytransform3d/rotations/__init__.py +++ b/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", diff --git a/pytransform3d/rotations/_mrp.py b/pytransform3d/rotations/_mrp.py new file mode 100644 index 000000000..bdf2ae46e --- /dev/null +++ b/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) diff --git a/pytransform3d/rotations/_mrp.pyi b/pytransform3d/rotations/_mrp.pyi new file mode 100644 index 000000000..ff1490f9f --- /dev/null +++ b/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: ... diff --git a/pytransform3d/rotations/_quaternion_operations.py b/pytransform3d/rotations/_quaternion_operations.py index a0eaa1b43..2187febe5 100644 --- a/pytransform3d/rotations/_quaternion_operations.py +++ b/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 diff --git a/pytransform3d/test/test_rotations.py b/pytransform3d/test/test_rotations.py index b21d2db74..6526e0cfb 100644 --- a/pytransform3d/test/test_rotations.py +++ b/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))