回転表現チートシート

このシートは、姿勢を2つのライブラリ間で受け渡すときに、値を変えずに届けたい人のためのものです。よく使われる4つの表現、ツールごとに異なる慣例、実行できる変換コードをまとめます。導出については座標系と回転から読み始めてください。角度はすべてラジアンです。座標系はREP 103に従います（右手系、x前方、y左、z上）。

4つの表現

表現	保持する数値の数	制約	合成	弱点	主な用途
回転行列 R	9	列が正規直交、det R = +1	行列積、R_ac = R_ab R_bc	積を何度も重ねると丸め誤差が蓄積し、Rが正規直交でなくなる。SVDで再正規直交化するか、四元数に変換して戻す	ベクトルの変換、同次変換、ヤコビ行列
軸角度表現、回転ベクトル	3（単位軸に角度を掛けたもの）または4（軸と角度）	軸が単位長	先に変換する。簡単な直接の合成規則はない	角度0で軸が定まらない。角度piで軸の符号が曖昧	小さな補正、角速度の積分、コントローラ内の誤差
オイラー角	3	なし	先に変換する	ジンバルロック。慣例が24通り（12通りの軸順序それぞれに内因性と外因性）	人による入力と表示、URDFのrpy
四元数（クォータニオン） q	4	単位ノルム	四元数の積	qと-qは同じ回転。wの並び位置がライブラリによって異なる	保存、補間（slerp）、フィルタ、メッセージ

REP 103は、特異点を持たないことから四元数と回転行列を推奨しています。オイラー角は慣例が多数あるため、使用を避けるよう勧めています。

公式

Rodrigues (unit axis k, angle t, K = skew(k)):
    R = I + sin(t) K + (1 - cos(t)) K^2

Quaternion from axis-angle:
    q = (w, x, y, z) = (cos(t/2), sin(t/2) kx, sin(t/2) ky, sin(t/2) kz)

Angle from a rotation matrix:
    t = arccos((trace(R) - 1) / 2)
    k = (R32 - R23, R13 - R31, R21 - R12) / (2 sin t)     # undefined near t = 0 and t = pi

URDF rpy (fixed axes x, then y, then z):
    R = Rz(yaw) Ry(pitch) Rx(roll)

内因性と外因性のオイラー角

外因性（extrinsic）回転は、親座標系の固定軸まわりに回します。内因性（intrinsic）回転は、回転とともに動く座標系の軸まわりに回します。ある回転の列を外因性で適用した結果は、その列を逆順にして内因性で適用した結果と同じ回転になります。したがって、固定軸x、y、zまわりのロール・ピッチ・ヨーは、動く軸z、y’、x’’まわりのヨー・ピッチ・ロールと同じです。

ツール	内因性	外因性	デフォルト
SciPy Rotation.from_euler	大文字、"ZYX"	小文字、"xyz"	degrees=Trueを指定しない限りラジアン
MuJoCo MJCF compiler/eulerseq	小文字、"xyz"	大文字、"XYZ"	eulerseq="xyz"。compilerのangleのデフォルトは度
URDF origin rpy	使わない	xまわりのロール、yまわりのピッチ、zまわりのヨー（固定軸）	ラジアン

MuJoCoの大文字・小文字の規則はSciPyと逆です。MuJoCoのXMLリファレンスには、URDFのrpyはMJCFのeulerseq="XYZ"に相当すると書かれています。また、MuJoCoは<compiler angle="radian"/>を設定しない限り、eulerとaxisangleを度として読みます。

ライブラリごとの四元数の成分順

ライブラリまたは形式	順序	記載箇所
SciPy Rotation.as_quat、from_quat	デフォルトは(x, y, z, w)。scalar_first=Trueを渡すと(w, x, y, z)	SciPy from_quat
ROS 2 geometry_msgs/msg/Quaternion	フィールドはx, y, z, w、デフォルト値は0, 0, 0, 1	Jazzyでのros2 interface show geometry_msgs/msg/Quaternion
MuJoCo（quat属性、フリージョイントとボールジョイントのqpos、mju_*関数）	(w, x, y, z)、デフォルトは"1 0 0 0"	MuJoCoモデリングガイド
spatialmath-python UnitQuaternion	.vecは(s, vx, vy, vz)でスカラー部が先頭。.vec_xyzsはスカラー部が末尾	spatialmath四元数ドキュメント
PyBullet getQuaternionFromEulerと姿勢	(x, y, z, w)	下のコードで確認

ROSのメッセージはフィールドに名前が付いているため、順序が問題になるのはそれらのフィールドを配列に詰めるときだけです。その際は毎回、順序をコメントに書いてください。

ros2 interface show geometry_msgs/msg/Quaternion

SciPyによる変換

1つの回転を5通りの形で表します。最初の行で、前節で述べた内因性と外因性の等価性を確認します。

import numpy as np
from scipy.spatial.transform import Rotation as R

# Intrinsic Z-Y-X (yaw, pitch, roll) equals extrinsic x-y-z with the angles reversed.
roll, pitch, yaw = 0.1, -0.4, 1.2          # rad
r_intr = R.from_euler("ZYX", [yaw, pitch, roll])
r_extr = R.from_euler("xyz", [roll, pitch, yaw])
print("same rotation:", np.allclose(r_intr.as_matrix(), r_extr.as_matrix()))

m = r_intr.as_matrix()
print("orthonormal:", np.allclose(m.T @ m, np.eye(3)), "det =", round(np.linalg.det(m), 6))

rotvec = r_intr.as_rotvec()                # axis * angle, rad
angle = np.linalg.norm(rotvec)
print("axis:", np.round(rotvec / angle, 4), "angle (rad):", round(angle, 4))

q_xyzw = r_intr.as_quat()                  # SciPy default: scalar last
q_wxyz = r_intr.as_quat(scalar_first=True)
print("q (x, y, z, w):", np.round(q_xyzw, 4))
print("q (w, x, y, z):", np.round(q_wxyz, 4))
print("round trip rpy:", np.round(R.from_quat(q_xyzw).as_euler("xyz"), 4))

NumPyだけで書いた同じ変換

表に示した公式を実装し、SciPyと照合したものです。最後の行で、qと-qが同じ行列になることを確認します。

import numpy as np
from scipy.spatial.transform import Rotation as R


def skew(v: np.ndarray) -> np.ndarray:
    return np.array([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]])


def axis_angle_to_matrix(axis: np.ndarray, angle: float) -> np.ndarray:
    k = skew(axis / np.linalg.norm(axis))
    return np.eye(3) + np.sin(angle) * k + (1.0 - np.cos(angle)) * k @ k   # Rodrigues


def quat_wxyz_to_matrix(q: np.ndarray) -> np.ndarray:
    w, x, y, z = q / np.linalg.norm(q)
    return np.array([
        [1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)],
        [2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)],
        [2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)],
    ])


def matrix_to_axis_angle(m: np.ndarray) -> tuple[np.ndarray, float]:
    angle = np.arccos(np.clip((np.trace(m) - 1.0) / 2.0, -1.0, 1.0))
    axis = np.array([m[2, 1] - m[1, 2], m[0, 2] - m[2, 0], m[1, 0] - m[0, 1]]) / (2.0 * np.sin(angle))
    return axis, angle   # fails near 0 and pi, where sin(angle) -> 0


axis, angle = np.array([1.0, 2.0, 2.0]) / 3.0, 0.9
m1 = axis_angle_to_matrix(axis, angle)
q = np.concatenate([[np.cos(angle / 2)], np.sin(angle / 2) * axis])   # (w, x, y, z)
m2 = quat_wxyz_to_matrix(q)
m_ref = R.from_rotvec(axis * angle).as_matrix()
a_back, ang_back = matrix_to_axis_angle(m1)
print("Rodrigues matches SciPy:", np.allclose(m1, m_ref))
print("quaternion matches SciPy:", np.allclose(m2, m_ref))
print("recovered axis:", np.round(a_back, 4), "angle:", round(ang_back, 4))
print("q and -q same rotation:", np.allclose(quat_wxyz_to_matrix(-q), m2))

1つの回転を4つのライブラリで扱う

このブロックでは、1つの姿勢をMuJoCo、spatialmath、PyBulletに渡し、それぞれが同じ行列を返すことを確認します。変わるのは四元数の成分順だけです。

import numpy as np
import mujoco
import pybullet as p
from scipy.spatial.transform import Rotation as R
from spatialmath import UnitQuaternion

rpy = [0.3, -0.2, 0.8]                             # URDF-style roll, pitch, yaw (rad)
r = R.from_euler("xyz", rpy)                       # extrinsic x, then y, then z
q_scipy = r.as_quat()                              # (x, y, z, w)
q_wfirst = np.roll(q_scipy, 1)                     # (w, x, y, z)

m_mj = np.zeros(9)
mujoco.mju_quat2Mat(m_mj, q_wfirst)                # MuJoCo expects w first
uq = UnitQuaternion(q_wfirst)                      # spatialmath: scalar part first
q_pb = np.array(p.getQuaternionFromEuler(rpy))     # PyBullet: (x, y, z, w), no server needed
print("MuJoCo agrees:", np.allclose(m_mj.reshape(3, 3), r.as_matrix()))
print("spatialmath agrees:", np.allclose(uq.R, r.as_matrix()))
print("PyBullet agrees:", np.allclose(q_pb, q_scipy) or np.allclose(q_pb, -q_scipy))
print("w first (MuJoCo, spatialmath .vec):", np.round(uq.vec, 4))
print("w last  (SciPy, ROS msg, PyBullet):", np.round(q_scipy, 4))

ジンバルロック

内因性のヨー・ピッチ・ロールでピッチが±90°になると、1番目と3番目の回転が同じ物理的な軸まわりの回転になります。意味を持つのは両者の差だけになるため、自由度（DoF）が1つ失われます。このとき異なる角度の組が同じ行列を与えるので、逆変換ではそのうち1つを選ぶ必要があります。SciPyは3番目の角度を0にして警告を出します。

import warnings
import numpy as np
from scipy.spatial.transform import Rotation as R

# At pitch = +90 degrees, roll and yaw act about the same axis: one degree of freedom is lost.
a = R.from_euler("ZYX", [0.7, np.pi / 2, 0.2])     # yaw, pitch, roll
b = R.from_euler("ZYX", [0.9, np.pi / 2, 0.4])     # yaw and roll both +0.2 rad
print("different angles, same rotation:", np.allclose(a.as_matrix(), b.as_matrix()))
with warnings.catch_warnings(record=True) as w:
    warnings.simplefilter("always")
    back = a.as_euler("ZYX")
print("recovered (yaw, pitch, roll):", np.round(back, 4))
print("SciPy warned about gimbal lock:", any("Gimbal lock" in str(x.message) for x in w))

特異点の近くでは、姿勢のわずかな変化がヨーとロールの大きな跳びを生みます。オイラー角で動作するコントローラやフィルタは、その跳びを巨大な誤差として受け取ります。状態は四元数か行列で保持してください。オイラー角への変換は表示のときだけにしてください。

落とし穴

• SciPyの四元数を並べ替えずにMJCFに貼り付けると、別の回転になります。(x, y, z, w)から(w, x, y, z)へはnp.roll(q, 1)、元に戻すにはnp.roll(q, -1)を使ってください。
• 2つの四元数の系列では、サンプル間で符号が反転することがあります（qの次に-q）。平均や補間をする前に、内積が正になるよう一方の符号を反転してください。
• 積分の後やファイルから読み込んだ後は、四元数を正規化してください。多くのライブラリは入力時に正規化しますが、自前の計算でもそうなるとは限りません。
• as_eulerは決まった範囲の角度を返します。ヨーの信号は±piで折り返します。プロットや微分の前にnp.unwrapで展開してください。
• ROSのカメラ光学座標系はz前方、x右、y下です。これは上記の機体の慣例と異なるため、camera_linkとcamera_optical_frameの間には固定の回転が入ります。
• 角速度はオイラー角の微分ではありません。両者の対応は現在の角度に依存し、ジンバルロックで特異になります。

関連ページ

• 座標系と回転と順運動学
• 空間数学のハブ
• DH法と指数積：回転をつなげてアームの運動学を組み立てる方法
• URDFとMJCF：ロボット記述のどこにrpyとquatが現れるか
• REP 103：ROSの単位と座標系の慣例