I had to calculate the z-y-x Euler angles from the quaternion.
Moreover, since it is a matter that must be written in Python 2.7, you cannot use a convenient one like scipy.spatial.transform.Rotation.
When I looked up the formula, I found C ++ sample code on Wikipedia.
Wikipedia / Quaternion to Euler Angles Conversion
I wrote the code for Python 2.7 by referring to the sample code on Wikipedia.
import math
import numpy as np
from pyquaternion import Quaternion
def quaternion_to_euler_zyx(q):
"""
Quaternion z-y-Convert to x-based Euler angles.
Parameters
----------
q : Quaternion
Quaternion(pyquaternion format)
Returns
-------
np.array
z-y-x Euler angles
"""
# roll :x-axis rotation
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-axis rotation
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-axis rotation
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)
#Euler angles
retrun np.array([
math.degrees(roll),
math.degrees(pitch),
math.degrees(yaw)
])
numpy 1.16.6
This is the one used for vector calculation. 1.16.6 seems to be the latest among Python 2.7 support.
pyquaternion 0.9.5
I use pyquaternion to calculate Quaternion.
numpy-quaternion seems to be more compatible with numpy, but I couldn't install it in Python 2.7 environment.
If you use scipy.spatial.transform.Rotation, you can write it more simply.
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)
However, scipy.spatial.transform.Rotation doesn't seem to support Python 2.7. Sorry.
I usually touch only Unity's z-x-y Euler angles, so the z-y-x system was quite confused.
In creating this article, I referred to the following articles. Thank you very much!
Recommended Posts