euler angle to rotation vector python
import math
import numpy as np
# RPY/Euler angles to Rotation Vector
def euler_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.0
else:
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
def euler_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.1684
print "roll = ", roll
print "pitch = ", pitch
print "yaw = ", yaw
print ""
rx, ry, rz = euler_to_rotVec(yaw, pitch, roll)
print rx, ry, rz
Are there any code examples left?
New code examples in category Python
-
Python 2023-04-11 03:04:20
-
Python 2022-03-27 22:40:04 pycharm no module named
-
Python 2022-03-27 22:25:05 assign multiple variablesin one line
-
Python 2022-03-27 22:20:02 levenshtein distance
-
Python 2022-03-27 21:35:09 get text from url python last slash
-
Python 2022-03-27 21:30:30 df concatenate df
-
Python 2022-03-27 21:25:09 python odd or even
-
Python 2022-03-27 21:15:32 python include function from another file
-
Python 2022-03-27 21:10:01 color module python
-
Python 2022-03-27 21:00:27 python tkinter cursor types