euler angle to rotation vector python

1import math
2import numpy as np
3
4# RPY/Euler angles to Rotation Vector
5def euler_to_rotVec(yaw, pitch, roll):
6    # compute the rotation matrix
7    Rmat = euler_to_rotMat(yaw, pitch, roll)
8    
9    theta = math.acos(((Rmat[0, 0] + Rmat[1, 1] + Rmat[2, 2]) - 1) / 2)
10    sin_theta = math.sin(theta)
11    if sin_theta == 0:
12        rx, ry, rz = 0.0, 0.0, 0.0
13    else:
14        multi = 1 / (2 * math.sin(theta))
15        rx = multi * (Rmat[2, 1] - Rmat[1, 2]) * theta
16        ry = multi * (Rmat[0, 2] - Rmat[2, 0]) * theta
17        rz = multi * (Rmat[1, 0] - Rmat[0, 1]) * theta
18    return rx, ry, rz
19
20def euler_to_rotMat(yaw, pitch, roll):
21    Rz_yaw = np.array([
22        [np.cos(yaw), -np.sin(yaw), 0],
23        [np.sin(yaw),  np.cos(yaw), 0],
24        [          0,            0, 1]])
25    Ry_pitch = np.array([
26        [ np.cos(pitch), 0, np.sin(pitch)],
27        [             0, 1,             0],
28        [-np.sin(pitch), 0, np.cos(pitch)]])
29    Rx_roll = np.array([
30        [1,            0,             0],
31        [0, np.cos(roll), -np.sin(roll)],
32        [0, np.sin(roll),  np.cos(roll)]])
33    # R = RzRyRx
34    rotMat = np.dot(Rz_yaw, np.dot(Ry_pitch, Rx_roll))
35    return rotMat
36
37roll = 2.6335
38pitch = 0.4506
39yaw = 1.1684
40
41print "roll = ", roll
42print "pitch = ", pitch
43print "yaw = ", yaw
44print ""
45
46rx, ry, rz = euler_to_rotVec(yaw, pitch, roll)
47
48print rx, ry, rz
49
50