You're viewing an old version of the documentation. Click here to see the latest release.

Conversions Between Common Orientation Representations

This article presents equations for conversion between orientation representations common for robots. For more information on the pose representations check out Position, Orientation and Coordinate Transformations.

Roll-Pitch-Yaw to Rotation Matrix

Roll-pitch-yaw is a common term to denote an orientation. Each represents an angle of rotation around a single axis, combined they represent a complete rotation. However, they are not explicit in terms of exactly what they represent. They are subject to the following confusions:

• Which axis does each rotate about?

• Are these axes fixed, or moving?

• In which order is the rotation defined (There are multiple possibilities)?

Order of rotations are often denoted x-y-z or z-y'-x''. Here x, y and z denotes the axis for which it is rotated around. ' is used to indicate whether or not the axes are fixed or not. Rotation around fixed axes is called extrinsic rotation. Rotation around moving axes is called intrinsic rotation.

What follows are two different examples.

For x-y-z or z-y'-x'', roll $\varphi$, pitch $\theta$, and yaw $\psi$ angles can be converted to a rotation matrix $R$ as follows:

$$\begin{array}{rl}{\mathit{R}}_{x-y-z}=& {\mathit{R}}_{z-y^{{}^{\prime }}-x^{{}^{″}}}={\mathit{R}}_{z,\varphi }{\mathit{R}}_{y,\theta }{\mathit{R}}_{x,\psi }=\\ \left[\begin{array}{ccc}cos\varphi & -sin\varphi & 0\\ sin\varphi & cos\varphi & 0\\ 0& 0& 1\end{array}\right]& \left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\psi & -sin\psi \\ 0& sin\psi & cos\psi \end{array}\right]\end{array}$$

For z-y-x or x-y'-z'', roll $\varphi$, pitch $\theta$, and yaw $\psi$ angles can be converted to a rotation matrix $R$ as follows:

$$\begin{array}{rl}{\mathit{R}}_{z-y-x}=& {\mathit{R}}_{x-y^{{}^{\prime }}-z^{{}^{″}}}={\mathit{R}}_{x,\varphi }{\mathit{R}}_{y,\theta }{\mathit{R}}_{z,\psi }=\\ \left[\begin{array}{ccc}1& 0& 0\\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi & cos\varphi \end{array}\right]& \left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]\left[\begin{array}{ccc}cos\psi & -sin\psi & 0\\ sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right]\end{array}$$

Note

If one assumes that the angles are the same in the two examples, then they do not represent the same final rotation matrix.

If one assumes that the final rotation matrixes are the same, then the angles are not identical between the two examples.

A definition is introduced: roll angle is assigned to the first rotation about moving axes, pitch is the second and yaw is the third.

C++

source

rollPitchYawListToRotationMatrix(rpyList);

C#

source

rollPitchYawListToRotationMatrix(rpyList);

Python

source

roll_pitch_yaw_to_rotation_matrix(rpy_list)

Rotation Vector to Axis-Angle

A rotation vector $\mathit{r}$ can be converted to axis $\mathit{u}$ and angle $\theta$ as follows:

$$\theta =\sqrt{{r_x}^2+{r_y}^2+{r_z}^2}$$

$$\begin{array}{r}\mathit{u}=\left[\begin{array}{ccc}\frac{r_x}{\theta }& \frac{r_y}{\theta }& \frac{r_z}{\theta }\end{array}\right]\end{array}$$

Axis-Angle to Quaternion

Axis $\mathit{u}$ and angle $\theta$ can be converted to a unit quaternion $\mathit{q}$ as follows:

$$\begin{array}{r}\mathit{q}=\left[\begin{array}{cccc}\cos \frac{\theta }{2}& u_x\sin \frac{\theta }{2}& u_y\sin \frac{\theta }{2}& u_z\sin \frac{\theta }{2}\end{array}\right]\end{array}$$

Quaternion to Rotation Matrix

A unit quaternion $\mathit{q}$ can be converted to a rotation matrix $\mathit{R}$ as follows:

$$\begin{array}{r}\mathit{R}=\left[\begin{array}{ccc}1-2{q_y}^2-2{q_z}^2& 2q_x{q}_y-2q_z{q}_w& 2q_x{q}_z-2q_y{q}_w\\ 2q_x{q}_y-2q_z{q}_w& 1-2{q_x}^2-2{q_z}^2& 2q_y{q}_z-2q_x{q}_w\\ 2q_x{q}_z-2q_y{q}_w& 2q_y{q}_z-2q_x{q}_w& 1-2{q_x}^2-2{q_y}^2\end{array}\right]\end{array}$$

It is assumed that the quaternion is normalized $(q_w+q_x+q_y+q_z=1)$. If not, it should be normalized before doing the conversion using this equation:

$$n=\sqrt{{q_w}^2+{q_c}^2+{q_y}^2+{q_z}^2}$$

$$\begin{array}{r}\mathit{q}=\left[\begin{array}{cccc}\frac{q_w}{n}& \frac{q_x}{n}& \frac{q_y}{n}& \frac{q_z}{n}\end{array}\right]\end{array}$$

C++

source

const Eigen::Matrix3f rotationMatrixFromQuaternion = quaternion.toRotationMatrix();
std::cout << "Rotation Matrix from Quaternion:\n"
          << rotationMatrixFromQuaternion.format(matrixFormatRules) << std::endl;

C#

source

var rotationMatrixFromQuaternion = quaternionToRotationMatrix(quaternion);
Console.WriteLine("Rotation Matrix from Quaternion:\n" + matrixToString(rotationMatrixFromQuaternion));

Python

source

rotation_matrix_from_quaternion = quaternion_to_rotation_matrix(quaternion)
print(f"Rotation Matrix from Quaternion:\n{rotation_matrix_from_quaternion}")

Rotation Matrix to Quaternion

A rotation matrix $\mathit{R}$ can be converted to a unit quaternion $\mathit{q}$ as follows:

$$q_w=\frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2}$$

$$\begin{array}{r}\mathit{q}=\left[\begin{array}{cccc}{q}_w& \frac{r_{32}-r_{23}}{4q_w}& \frac{r_{13}-r_{31}}{4q_w}& \frac{r_{21}-r_{12}}{4q_w}\end{array}\right]\end{array}$$

C++

source

const Eigen::Quaternionf quaternion(rotationMatrix);
std::cout << "Quaternion:\n" << quaternion.coeffs().format(vectorFormatRules) << std::endl;

C#

source

var quaternion = rotationMatrixToQuaternion(rotationMatrix);
Console.WriteLine("Quaternion:\n" + matrixToString(quaternion.Transpose()));

Python

source

quaternion = rotation_matrix_to_quaternion(rotation_matrix)
print(f"Quaternion:\n{quaternion}")

Quaternion to Axis-Angle

A unit quaternion $\mathit{q}$ can be converted to axis $\mathit{u}$ and angle $\theta$ as follows:

$$\theta =2{\cos }^{-1}{q}_w$$

$$\begin{array}{r}\mathit{u}=\left[\begin{array}{ccc}\frac{q_x}{\sqrt{1-{q_w}^2}}& \frac{q_y}{\sqrt{1-{q_w}^2}}& \frac{q_z}{\sqrt{1-{q_w}^2}}\end{array}\right]\end{array}$$

This is useful for converting from rotation matrix to axis-angle. See our code sample implementation below.

C++

source

const Eigen::AngleAxisf axisAngle(rotationMatrix);
std::cout << "AxisAngle:\n"
          << axisAngle.axis().format(vectorFormatRules) << ", " << axisAngle.angle() << std::endl;

C#

source

var axisAngle = rotationMatrixToAxisAngle(rotationMatrix);
Console.WriteLine("AxisAngle:\n" + matrixToString(axisAngle.Axis.Transpose()) + ", " + String.Format(" {0:G4} ", axisAngle.Angle));

Python

source

axis_angle = rotation_matrix_to_axis_angle(rotation_matrix)
print(f"AxisAngle:\n{axis_angle.axis}, {axis_angle.angle:.4f}")

Axis-Angle to Rotation Vector

Axis $\mathit{u}$ and angle $\theta$ can be converted to a rotation vector $\mathit{r}$ as follows:

$$\begin{array}{r}\mathit{r}=\left[\begin{array}{ccc}{u}_x\theta & u_y\theta & u_z\theta \end{array}\right]\end{array}$$

This is useful for converting rotation matrix to rotation vector. See our code sample implementation below.

C++

source

const Eigen::Vector3f rotationVector = rotationMatrixToRotationVector(rotationMatrix);
std::cout << "Rotation Vector:\n" << rotationVector.format(vectorFormatRules) << std::endl;

C#

source

var rotationVector = axisAngle.Axis * axisAngle.Angle;
Console.WriteLine("Rotation Vector:\n" + matrixToString(rotationVector.Transpose()));

Python

source

rotation_vector = rotation_matrix_to_rotation_vector(rotation_matrix)
print(f"Rotation Vector:\n{rotation_vector}")

Rotation Matrix to Roll-Pitch-Yaw

Determining roll, pitch, yaw angles from a rotation matrix is not straightforward. There can be multiple and sometimes even infinite solutions. This requires an algorithm that can choose one of the multiple solutions based on some criteria.

C++

source

const auto rpyList = rotationMatrixToRollPitchYawList(rotationMatrix);

C#

source

var rpyList = rotationMatrixToRollPitchYawList(rotationMatrix);

Python

source

rpy_list = rotation_matrix_to_roll_pitch_yaw(rotation_matrix)