Conversions Between Common Orientation Representations

This article explains how to convert between orientation representations commonly used in robotics. It includes the necessary mathematical equations as well as code examples showing how to integrate these conversions into your own applications.

If you want to utilize these transformations interactively, try our Pose Conversion GUI tool.

For a more in-depth explanation of pose representations, check out Position, Orientation and Coordinate Transformations.

These conversions only change the rotation (orientation) part of the pose; the translation part is carried over unchanged. The Zivid transformation matrix expects the translation in millimeters (mm), because Zivid point clouds are in millimeters. Most robot controllers report translation in meters, so multiply the translation by 1000 when converting a robot pose to the Zivid matrix.

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 \(\phi\), pitch \(\theta\), and yaw \(\psi\) angles can be converted to a rotation matrix \(R\) as follows:

\[\begin{split}\boldsymbol{R}_{x-y-z} = &\boldsymbol{R}_{z-{y}^{'}-{x}^{''}} =\boldsymbol{R}_{z,\phi}\boldsymbol{R}_{y,\theta}\boldsymbol{R}_{x,\psi} = \\ \begin{bmatrix} cos{\phi} & -sin{\phi} & 0 \\ sin{\phi} & cos{\phi} & 0\\ 0 & 0 & 1\end{bmatrix}& \begin{bmatrix} cos{\theta} & 0 & sin{\theta} \\ 0 & 1 & 0 \\ -sin{\theta} & 0 & cos{\theta} \end{bmatrix} \begin{bmatrix} 1 & 0 & 0\\ 0 & cos{\psi} & -sin{\psi} \\ 0 & sin{\psi} & cos{\psi} \end{bmatrix}\end{split}\]

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

\[\begin{split}\boldsymbol{R}_{z-y-x} = &\boldsymbol{R}_{x-{y}^{'}-{z}^{''}} =\boldsymbol{R}_{x,\phi}\boldsymbol{R}_{y,\theta}\boldsymbol{R}_{z,\psi} = \\ \begin{bmatrix} 1 & 0 & 0\\ 0 & cos{\phi} & -sin{\phi} \\ 0 & sin{\phi} & cos{\phi} \end{bmatrix}& \begin{bmatrix} cos{\theta} & 0 & sin{\theta} \\ 0 & 1 & 0 \\ -sin{\theta} & 0 & cos{\theta} \end{bmatrix} \begin{bmatrix} cos{\psi} & -sin{\psi} & 0 \\ sin{\psi} & cos{\psi} & 0\\ 0 & 0 & 1\end{bmatrix}\end{split}\]

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++

rollPitchYawListToRotationMatrix(rpyList);

C#

RollPitchYawListToRotationMatrix(rpyList);

Python

roll_pitch_yaw_to_rotation_matrix(rpy_list)

Rotation Vector to Axis-Angle

A rotation vector \(\boldsymbol{r}\) can be converted to axis \(\boldsymbol{u}\) and angle \(\theta\) as follows:

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

\[\begin{split}\boldsymbol{u} = \begin{bmatrix} \frac{r_x}{\theta} & \frac{r_y}{\theta} & \frac{r_z}{\theta} \\ \end{bmatrix}\end{split}\]

Axis-Angle to Quaternion

Axis \(\boldsymbol{u}\) and angle \(\theta\) can be converted to a unit quaternion \(\boldsymbol{q}\) as follows:

\[\begin{split}\boldsymbol{q} = \begin{bmatrix} \cos{\frac{\theta}{2}} & u_x \sin{\frac{\theta}{2}} & u_y \sin{\frac{\theta}{2}} & u_z \sin{\frac{\theta}{2}} \\ \end{bmatrix}\end{split}\]

Quaternion to Rotation Matrix

A unit quaternion \(\boldsymbol{q}\) can be converted to a rotation matrix \(\boldsymbol{R}\) as follows:

\[\begin{split}\boldsymbol{R} = \begin{bmatrix} 1 - 2 {q_{y}}^2 - 2 {q_{z}}^2 & 2 q_{x} q_{y} - 2 q_{z} q_{w} & 2 q_{x} q_{z} - 2 q_{y} q_{w} \\ 2 q_{x} q_{y} - 2 q_{z} q_{w} & 1 - 2 {q_{x}}^2 - 2 {q_{z}}^2 & 2 q_{y} q_{z} - 2 q_{x} q_{w} \\ 2 q_{x} q_{z} - 2 q_{y} q_{w} & 2 q_{y} q_{z} - 2 q_{x} q_{w} & 1 - 2 {q_{x}}^2 - 2 {q_{y}}^2\\ \end{bmatrix}\end{split}\]

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{split}\boldsymbol{q} = \begin{bmatrix} \frac{q_w}{n} & \frac{q_x }{n} & \frac{q_y }{n} & \frac{q_z }{ n}\\ \end{bmatrix}\end{split}\]

C++

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

C#

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

Python

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 \(\boldsymbol{R}\) can be converted to a unit quaternion \(\boldsymbol{q}\) as follows:

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

\[\begin{split}\boldsymbol{q} = \begin{bmatrix} q_w & \frac{r_{32} - r_{23}}{4 q_w} & \frac{r_{13} - r_{31}}{4 q_w} & \frac{r_{21} - r_{12}}{4 q_w} \\ \end{bmatrix}\end{split}\]

C++

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

C#

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

Python

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

Quaternion to Axis-Angle

A unit quaternion \(\boldsymbol{q}\) can be converted to axis \(\boldsymbol{u}\) and angle \(\theta\) as follows:

\[\theta = 2 \cos^{-1}{q_w}\]

\[\begin{split}\boldsymbol{u} = \begin{bmatrix} \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{bmatrix}\end{split}\]

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

C++

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

C#

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

Python

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 \(\boldsymbol{u}\) and angle \(\theta\) can be converted to a rotation vector \(\boldsymbol{r}\) as follows:

\[\begin{split}\boldsymbol{r} = \begin{bmatrix} u_x \theta & u_y \theta & u_z \theta \\ \end{bmatrix}\end{split}\]

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

C++

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

C#

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

Python

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++

const auto rpyList = rotationMatrixToRollPitchYawList(rotationMatrix);

C#

var rpyList = RotationMatrixToRollPitchYawList(rotationMatrix);

Python

rpy_list = rotation_matrix_to_roll_pitch_yaw(rotation_matrix)

Robot-Specific Pose Conventions

The hardest part of converting a robot pose to a transformation matrix is identifying which orientation representation the robot controller uses. The mathematical conversions above are unambiguous once you know the representation, but robot vendors rarely state it clearly, and the same term (for example roll-pitch-yaw) can mean different rotation orders on different robots.

This section collects the conventions we have seen on specific robots, so you can pick the right conversion above without rediscovering it.

Warning

This information is gathered from experience with specific robot models and is not exhaustively tested across firmware versions, models, or vendors. Always verify the result of a conversion before relying on it, for example by comparing the assembled transformation matrix against the pose shown on the robot teach pendant.

Universal Robots

Universal Robots reports the TCP pose as a six-element vector [x, y, z, rx, ry, rz]. The first three elements are the translation, and the last three are the orientation as a rotation vector (axis-angle scaled by the angle, also called the Rodrigues representation), not roll-pitch-yaw. The script and RTDE interfaces (for example get_actual_tcp_pose) report the translation in meters, while the teach pendant displays it in millimeters.

To build a 4x4 transformation matrix:

Ensure the translation is in millimeters, converting from meters if you read the pose from the script or RTDE interface.
Convert the rotation vector to a rotation matrix using the conversions above.
Place the rotation matrix and translation vector into a 4x4 transformation matrix.

Yaskawa

Yaskawa controllers report the pose as a translation followed by three Euler angles (Rx, Ry, Rz) in xyz extrinsic order.

To build a 4x4 transformation matrix:

Ensure the translation is in millimeters.
Convert the Euler angles to a rotation matrix using the Roll-Pitch-Yaw to Rotation Matrix conversion above, with the rotation order from the table.
Place the rotation matrix and translation vector into a 4x4 transformation matrix.

Known Euler-Angle Orders

The following table lists the Euler-angle orders we have seen per robot vendor. The steps are the same as for Yaskawa above; only the Euler-angle order changes from one vendor to the next. Use it as a starting point, but confirm against your own robot, as the order can differ between models and firmware from the same vendor.

Robot vendor	Euler angles order	Notes
Universal Robots	Rotation vector (not Euler)	Axis-angle / Rodrigues
Fanuc	xyz extrinsic	Reported as W-P-R
Yaskawa	xyz extrinsic
Kawasaki	zyz intrinsic
KUKA	zyx intrinsic	Reported as A-B-C
Doosan	zyz intrinsic
ABB	zyx intrinsic	Controller also exposes a quaternion
Hyundai	xyz intrinsic
Robostar	xyz extrinsic
Nachi	zyx intrinsic