Saltar al contenido

ejemplo de código de Python de matriz de ángulo de Euler a rotación

Luego de tanto batallar ya hallamos el resultado de este enigma que muchos los usuarios de nuestro sitio web han presentado. Si tienes alguna información que compartir no dejes de aportar tu conocimiento.

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

Sección de Reseñas y Valoraciones

Recuerda que puedes dar recomendación a este enunciado si si solucionó tu problema.

¡Haz clic para puntuar esta entrada!
(Votos: 0 Promedio: 0)



Utiliza Nuestro Buscador

Deja una respuesta

Tu dirección de correo electrónico no será publicada. Los campos obligatorios están marcados con *