skrobot.coordinates.math.matrix2rpy

skrobot.coordinates.math.matrix2rpy(matrix)

Extract roll, pitch, yaw angles from rotation matrix.

This function returns angles in roll-pitch-yaw order (X-Y-Z rotation order).

Parameters:

matrix (numpy.ndarray) – 3x3 rotation matrix

Returns:

angles – Array of [roll, pitch, yaw] angles in radians - roll: rotation around X-axis - pitch: rotation around Y-axis - yaw: rotation around Z-axis

Return type:

numpy.ndarray

Examples

>>> import numpy as np
>>> from skrobot.coordinates.math import rpy_matrix, matrix2rpy
>>> roll, pitch, yaw = np.pi/3, np.pi/4, np.pi/6
>>> rot = rpy_matrix(yaw, pitch, roll)  # Note: rpy_matrix takes yaw first
>>> angles = matrix2rpy(rot)
>>> np.allclose(angles, [roll, pitch, yaw])
True
>>> print("Roll: {:.3f}, Pitch: {:.3f}, Yaw: {:.3f}".format(angles[0], angles[1], angles[2]))
Roll: 1.047, Pitch: 0.785, Yaw: 0.524