J'ai écrit un code pour convertir quaternion en angle de graissage de type z-y-x avec Python

J'ai écrit un code pour convertir quaternion en angle de graissage de type z-y-x avec Python

J'ai dû calculer l'angle du graisseur z-y-x à partir du quaternion.

De plus, comme c'est une question qui doit être écrite en Python 2.7, vous ne pouvez pas en utiliser une pratique comme scipy.spatial.transform.Rotation.

Quand j'ai recherché la formule, j'ai trouvé un exemple de code C ++ sur Wikipedia.

Wikipedia / Quaternion to Euler Angles Conversion

Je l'ai écrit en Python

J'ai écrit le code Python 2.7 en me référant à l'exemple de code de Wikipedia.

import math
import numpy as np
from pyquaternion import Quaternion

def quaternion_to_euler_zyx(q):
    """
Quarternion z-y-Convertir en angle de graisseur de type X.

    Parameters
    ----------
    q : Quaternion
Quarternion(format pyquaternion)

    Returns
    -------
    np.array
        z-y-angle de graisseur basé sur x
    """

    # roll :rotation de l'axe 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 :rotation de l'axe 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 :rotation de l'axe 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)
    
    #Angle du graisseur
    retrun np.array([
        math.degrees(roll), 
        math.degrees(pitch), 
        math.degrees(yaw)
    ])

Bibliothèques dépendantes

numpy 1.16.6

C'est celui utilisé pour le calcul vectoriel. 1.16.6 semble être la dernière prise en charge de Python 2.7.

pyquaternion 0.9.5

J'utilise «pyquaternion» pour calculer Quaternion.

numpy-quaternion semble être plus compatible avec numpy, mais je n'ai pas pu l'installer dans l'environnement Python 2.7.

Scipy.spatial.transform.Rotation est facile pour Python 3 ou version ultérieure

Si vous utilisez scipy.spatial.transform.Rotation, vous pouvez l'écrire plus simplement.

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)

Cependant, scipy.spatial.transform.Rotation ne semble pas prendre en charge Python 2.7. Pardon.

à la fin

Je ne touche généralement que l'angle de graissage z-x-y d'Unity, donc le système z-y-x était assez confus.

En créant cet article, j'ai fait référence aux articles suivants. Merci beaucoup!

• Wikipedia / Quaternion to Euler Angles Conversion
• Le module de rotation Kami no Memo / Python scipy gère la rotation 3D
• scipy.spatial.transform.Rotation