Skip to content

Instantly share code, notes, and snippets.

@lemire
Last active Nov 2, 2021
Embed
What would you like to do?
from math import *
def convert_to_degree(x):
return [int(x[0] / (2 * pi) *360),int(x[1] / (2 * pi) *360),int(x[2] / (2 * pi) *360)]
def toangles(q):
sqw = q[3] * q[3]
sqx = q[0] * q[0]
sqy = q[1] * q[1]
sqz = q[2] * q[2]
x = q[0]
y = q[1]
z = q[2]
w = q[3]
unit = sqx + sqy + sqz + sqw
test = x * y + z * w
#yaw (heading), roll, pitch
return [atan2(2 * y * w - 2 * x * z, sqx - sqy - sqz + sqw), asin(2 * test / unit), atan2(2 * x * w - 2 * y * z, -sqx + sqy - sqz + sqw)]
print("On affiche le yaw, roll, pitch. En degrés (360).")
print("Yaw : direction à vue d'oiseau.")
print("Roll: roulade.")
print("Pitch: pointé vers le haut ou vers le bas")
# quaternion, pied à terre, bien configuré (en avant)
t1=[0.06279543, 0.6439661, -0.09792635, 0.7561778]
# quaternion, pied à terre, avec capteur sur le côté
t1cot=[0.13297096, 0.8849193, -0.089683086, 0.43723747]
print("pied à terre, bien configuré (en avant)")
print(convert_to_degree(toangles(t1)))
print("pied à terre, capteur sur le côté")
print(convert_to_degree(toangles(t1cot)))
# quaternion, pied levé, bien configuré (en avant)
t1rot = [-0.43136793, 0.4158741, 0.4094003, 0.6879876]
# quaternion, pied levé, avec capteur sur le côté
t1rotcot = [-0.44004276, 0.7334766, 0.32424885, 0.40396234]
print("pied levé, bien configuré (en avant)")
print(convert_to_degree(toangles(t1rot)))
print("pied levé, capteur sur le côté")
print(convert_to_degree(toangles(t1rotcot)))
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment