I am trying to write a custom function that takes a quaternion and outputs the Euler angles. I thought it was quite easy: I am using the formulas found here while also looking at the C++ implementation in the same page. The function looks like this:
import math
def quaternion_to_euler(quaternion: tuple):
w, x, y, z = quaternion
roll = math.atan2(2*(w*x + y*z), 1 - 2*(x * x + y * y))
pitch = math.asin(2*(w*y - z*x))
yaw = math.atan2(2*(w*z + x*y), 1 - 2*(y * y + z * z))
return(roll, pitch, yaw)
To be sure of having the good results, I am comparing my conversion with the one performed by this page.
It seems that my results are completely off. As an example compare the results of my function for a specific quaternion:
quaternion = (0.73007, -0.03484, 0.68173, 0.03199)
roll, pitch, yaw = quaternion_to_euler(quaternion)
print(f'roll: {roll}, pitch: {pitch}, yaw: {yaw}')
roll: -0.10618318122804299, pitch: 1.5022308055111253, yaw: -0.011586976613645479
To the results of the page linked above:
roll: -0.9466037, pitch: 1.454085, yaw: 0.9425664
The formulas seem quite straightforward to me, and I cannot see any self-evident mistake in the implementation. Can someone explain to me what I am doing wrong ?
EDIT:
I found out that there is a scipy class that support the conversion between quaternion and euler angle. If I do:
from scipy.spatial.transform import Rotation as R
rr = R.from_quat([-0.03484, 0.68173, 0.03199, 0.73007])
print(rr.as_euler('xyz'))
I have exactly the same results as my custom function. Now I am even more confused.