Python convierte el cuaternión en el ejemplo de código de ángulos de Euler

Si te encuentras con algún detalle que no comprendes puedes dejarlo en los comentarios y te ayudaremos tan rápido como podamos.

Ejemplo: ángulo de Euler al vector de rotación python

import math
import numpy as np

# RPY/Euler angles to Rotation Vectordefeuler_to_rotVec(yaw, pitch, roll):# compute the rotation matrix
    Rmat = euler_to_rotMat(yaw, pitch, roll)
    
    theta = math.acos(((Rmat[0,0]+ Rmat[1,1]+ Rmat[2,2])-1)/2)
    sin_theta = math.sin(theta)if sin_theta ==0:
        rx, ry, rz =0.0,0.0,0.0else:
        multi =1/(2* math.sin(theta))
        rx = multi *(Rmat[2,1]- Rmat[1,2])* theta
        ry = multi *(Rmat[0,2]- Rmat[2,0])* theta
        rz = multi *(Rmat[1,0]- Rmat[0,1])* theta
    return rx, ry, rz

defeuler_to_rotMat(yaw, pitch, roll):
    Rz_yaw = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),  np.cos(yaw),0],[0,0,1]])
    Ry_pitch = np.array([[ np.cos(pitch),0, np.sin(pitch)],[0,1,0],[-np.sin(pitch),0, np.cos(pitch)]])
    Rx_roll = np.array([[1,0,0],[0, np.cos(roll),-np.sin(roll)],[0, np.sin(roll),  np.cos(roll)]])# R = RzRyRx
    rotMat = np.dot(Rz_yaw, np.dot(Ry_pitch, Rx_roll))return rotMat

roll =2.6335
pitch =0.4506
yaw =1.1684print"roll = ", roll
print"pitch = ", pitch
print"yaw = ", yaw
print""

rx, ry, rz = euler_to_rotVec(yaw, pitch, roll)print rx, ry, rz

Recuerda que puedes optar por la opción de agregar una reseña si diste con el arreglo.

---

---

Utiliza Nuestro Buscador