-
Notifications
You must be signed in to change notification settings - Fork 5
/
Copy pathutils.py
104 lines (78 loc) · 2.31 KB
/
utils.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
import numpy as np
def rot1(angle, degrees=False):
'''
Converts pitch angle (a rotation around the 1st body axis) to a rotation
matrix in SO(3).
Args:
angle: (numpy.ndarray) pitch angle
degrees: (bool) flag to use if the angles are in degrees,
default = False
Returns:
R: (numpy.ndarray) 3x3 rotation matrix in SO(3)
'''
if degrees:
angle = np.deg2rad(angle)
cos_a = np.cos(angle)
sin_a = np.sin(angle)
rot_mat = np.identity(3)
rot_mat[1, 1] = cos_a
rot_mat[1, 2] = -sin_a
rot_mat[2, 1] = sin_a
rot_mat[2, 2] = cos_a
return rot_mat
def rot2(angle, degrees=False):
'''
Converts roll angle (a rotation around the 2nd body axis) to a rotation
matrix in SO(3).
Args:
angle: (numpy.ndarray) roll angle
degrees: (bool) flag to use if the angles are in degrees,
default = False
Returns:
R: (numpy.ndarray) 3x3 rotation matrix in SO(3)
'''
if degrees:
angle = np.deg2rad(angle)
cos_a = np.cos(angle)
sin_a = np.sin(angle)
rot_mat = np.identity(3)
rot_mat[0, 0] = cos_a
rot_mat[0, 2] = sin_a
rot_mat[2, 0] = -sin_a
rot_mat[2, 2] = cos_a
return rot_mat
def rot3(angle, degrees=False):
'''
Converts yaw angle (a rotation around the 3rd body axis) to a rotation
matrix in SO(3).
Args:
angle: (numpy.ndarray) yaw angle
degrees: (bool) flag to use if the angles are in degrees,
default = False
Returns:
R: (numpy.ndarray) 3x3 rotation matrix in SO(3)
'''
if degrees:
angle = np.deg2rad(angle)
cos_a = np.cos(angle)
sin_a = np.sin(angle)
rot_mat = np.identity(3)
rot_mat[0, 0] = cos_a
rot_mat[0, 1] = -sin_a
rot_mat[1, 0] = sin_a
rot_mat[1, 1] = cos_a
return rot_mat
def ypr_to_R(ypr, degrees=False):
'''
Converts yaw, pitch, roll angles to a rotation matrix in SO(3).
Args:
ypr: (numpy.ndarray) 3x1 array with yaw, pitch, roll
degrees: (bool) flag to use if the angles are in degrees,
default = False
Returns:
R: (numpy.ndarray) 3x3 rotation matrix in SO(3)
'''
R3 = rot3(ypr[0], degrees)
R2 = rot2(ypr[1], degrees)
R1 = rot1(ypr[2], degrees)
return R3.dot(R2).dot(R1)