Pythonでクォータニオンをz-y-x系オイラー角に変換するコード書いてみた

クォータニオンからz-y-x系オイラー角の計算をしなければならなくなりました。

しかも、Python2.7で書かなければいけない案件なので、scipy.spatial.transform.Rotation みたいな便利なやつ使えません。

公式を調べてみたら、WikipediaにC++のサンプルコードが載っていました。

Wikipedia / Quaternion to Euler Angles Conversion

Pythonで書いてみた

Wikipediaのサンプルコードを参考にして、Python2.7のコードを書いてみました。

import math
import numpy as np
from pyquaternion import Quaternion

def quaternion_to_euler_zyx(q):
    """
    クォータニオンをz-y-x系オイラー角に変換する。

    Parameters
    ----------
    q : Quaternion
        クォータニオン(pyquaternion形式)

    Returns
    -------
    np.array
        z-y-x系オイラー角
    """

    # roll : x軸回転
    sinr_cosp = 2 * (q[0] * q[1] + q[2] * q[3])
    cosr_cosp = 1 - 2 * (q[1] * q[1] + q[2] * q[2])
    roll = math.atan2(sinr_cosp, cosr_cosp)

    # pitch : y軸回転
    sinp = 2 * (q[0] * q[2] - q[3] * q[1])
    if math.fabs(sinp) >= 1:
        pitch = math.copysign(math.pi / 2, sinp)
    else:
        pitch = math.asin(sinp)

    # yaw : z軸回転
    siny_cosp = 2 * (q[0] * q[3] + q[1] * q[2])
    cosy_cosp = 1 - 2 * (q[2] * q[2] + q[3] * q[3])
    yaw = math.atan2(siny_cosp, cosy_cosp)

    # オイラー角
    retrun np.array([
        math.degrees(roll), 
        math.degrees(pitch), 
        math.degrees(yaw)
    ])

依存ライブラリ

numpy 1.16.6

ベクトル計算とかに使うやつです。1.16.6がPython2.7対応の中では最新っぽいです。

pyquaternion 0.9.5

Quaternionの計算に pyquaternion 使ってます。

numpy-quaternion の方がnumpyと相性はよさそうなのですが、Python2.7環境だとインストールできませんでした。

Python3以降ならscipy.spatial.transform.Rotationが楽

scipy.spatial.transform.Rotation を使えば、もっとシンプルに書けそうです。

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

def quaternion_to_euler_zyx(q):
    r = R.from_quat([q[0], q[1], q[2], q[3]])
    return r.as_euler('zyx', degrees=True)

ただ、 scipy.spatial.transform.Rotation はPython2.7には対応してないみたいです。残念。

さいごに

普段はUnityのz-x-y系オイラー角ばかり触ってるので、z-y-x系はかなり混乱しました。
本記事作成にあたり、以下の記事を参考にさせていただきました。ありがとうございました！