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는 방향을 나타내는 일반적인 용어입니다. 각각은 단일 축을 중심으로 한 회전 각도를 나타내며 결합하여 완전한 회전을 나타냅니다. 그러나 정확히 무엇을 나타내는지는 명확하지 않습니다. 다음과 같은 혼란을 겪을 수 있습니다.

각각 어떤 축을 중심으로 회전합니까?
이 축은 고정되어 있습니까, 아니면 이동 중입니까?
회전은 어떤 순서로 정의됩니까(여러 가지 가능성이 있음)?

회전 순서는 종종 x-y-z 또는 z-y'-x'' 로 표시됩니다. 여기서 x, y 및 z 는 회전하는 축을 나타냅니다. ' 는 축의 고정 여부를 나타내는 데 사용됩니다. 고정 축을 중심으로 하는 회전을 extrinsic rotation 이라고 합니다. 움직이는 축을 중심으로 회전하는 것을 intrinsic rotation 이라고 합니다.

다음은 두 가지 다른 예입니다.

x-y-z 또는 z-y'-x'' 의 경우, 롤 \(\phi\), 피치 \(\theta\) 및 요 \(\psi\) 각도는 다음과 같이 회전 행렬 \(R\) 로 변환할 수 있습니다.:

\[\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}\]

z-y-x 또는 x-y'-z'' 의 경우, 롤 \(\phi\), 피치 \(\theta\) 및 요 \(\psi\) 각도는 다음과 같이 회전 행렬 \(R\) 로 변환할 수 있습니다.:

\[\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}\]

참고

두 예에서 각도가 동일하다고 가정하면 동일한 최종 회전 행렬을 나타내지 않습니다.

최종 회전 행렬이 동일하다고 가정하면 두 예제 간에 각도가 동일하지 않습니다.

정의가 도입되었습니다. 롤 각도는 움직이는 축에 대한 첫 번째 회전에 할당되고, 피치는 두 번째, 요는 세 번째 회전에 할당됩니다.

C++

rollPitchYawListToRotationMatrix(rpyList);

C#

RollPitchYawListToRotationMatrix(rpyList);

Python

roll_pitch_yaw_to_rotation_matrix(rpy_list)

Rotation Vector to Axis-Angle

회전 벡터 \(\boldsymbol{r}\) 는 다음과 같이 축 \(\boldsymbol{u}\) 과 각도 \(\theta\) 로 변환할 수 있습니다.:

\[\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

축 \(\boldsymbol{u}\) 과 각도 \(\theta\) 는 다음과 같이 unit quaternion \(\boldsymbol{q}\) 로 변환할 수 있습니다.:

\[\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

unit quaternion \(\boldsymbol{q}\) 는 다음과 같이 회전 행렬 \(\boldsymbol{R}\) 로 변환할 수 있습니다.:

\[\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}\]

quaternion이 \((q_w + q_x + q_y + q_z = 1)\) 와 같이 정규화되었다고 가정합니다. 그렇지 않은 경우 다음 방정식을 사용하여 변환을 수행하기 전에 정규화해야 합니다.

\[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

회전 행렬 \(\boldsymbol{R}\) 을 다음과 같이 unit quaternion \(\boldsymbol{q}\) 으로 변환할 수 있습니다.:

\[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

unit quaternion \(\boldsymbol{q}\) 는 다음과 같이 축 \(\boldsymbol{u}\) 과 각도 \(\theta\) 로 변환할 수 있습니다.:

\[\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}\]

이것은 회전 행렬에서 축 각도로 변환하는 데 유용합니다. 아래 코드 샘플 구현을 참조하십시오.

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

축 \(\boldsymbol{u}\) 과 각도 \(\theta\) 는 다음과 같이 회전 벡터 \(\boldsymbol{r}\) 로 변환할 수 있습니다.:

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

이것은 회전 행렬을 회전 벡터로 변환하는 데 유용합니다. 아래 코드 샘플 구현을 참조하십시오.

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

회전 매트릭스에서 롤, 피치, 요 각도를 결정하는 것은 간단하지 않습니다. 여러 가지, 때로는 무한한 솔루션이 있을 수도 있습니다. 이를 위해서는 몇 가지 기준에 따라 여러 솔루션 중 하나를 선택할 수 있는 알고리즘이 필요합니다.

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.

경고

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