-
Notifications
You must be signed in to change notification settings - Fork 0
/
rotations.py
179 lines (153 loc) · 6.84 KB
/
rotations.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
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
def angle_normalize(a):
"""Normalize angles to lie in range -pi < a[i] <= pi."""
a = np.remainder(a, 2*np.pi)
a[a <= -np.pi] += 2*np.pi
a[a > np.pi] -= 2*np.pi
return a
def skew_symmetric(v):
"""Skew symmetric form of a 3x1 vector."""
return np.array(
[[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]], dtype=np.float64)
def rpy_jacobian_axis_angle(a):
"""Jacobian of RPY Euler angles with respect to axis-angle vector."""
if not (type(a) == np.ndarray and len(a) == 3):
raise ValueError("'a' must be a np.ndarray with length 3.")
# From three-parameter representation, compute u and theta.
na = np.sqrt(a @ a)
na3 = na**3
t = np.sqrt(a @ a)
u = a/t
# First-order approximation of Jacobian wrt u, t.
Jr = np.array([[t/(t**2*u[0]**2 + 1), 0, 0, u[0]/(t**2*u[0]**2 + 1)],
[0, t/np.sqrt(1 - t**2*u[1]**2), 0, u[1]/np.sqrt(1 - t**2*u[1]**2)],
[0, 0, t/(t**2*u[2]**2 + 1), u[2]/(t**2*u[2]**2 + 1)]])
# Jacobian of u, t wrt a.
Ja = np.array([[(a[1]**2 + a[2]**2)/na3, -(a[0]*a[1])/na3, -(a[0]*a[2])/na3],
[ -(a[0]*a[1])/na3, (a[0]**2 + a[2]**2)/na3, -(a[1]*a[2])/na3],
[ -(a[0]*a[2])/na3, -(a[1]*a[2])/na3, (a[0]**2 + a[1]**2)/na3],
[ a[0]/na, a[1]/na, a[2]/na]])
return Jr @ Ja
class Quaternion():
def __init__(self, w=1., x=0., y=0., z=0., axis_angle=None, euler=None):
"""
Allow initialization with explicit quaterion wxyz, axis-angle, or Euler XYZ (RPY) angles.
:param w: w (real) of quaternion.
:param x: x (i) of quaternion.
:param y: y (j) of quaternion.
:param z: z (k) of quaternion.
:param axis_angle: Set of three values from axis-angle representation, as list or [3,] or [3,1] np.ndarray.
See C2M5L2 for details.
:param euler: Set of three XYZ Euler angles.
"""
if axis_angle is None and euler is None:
self.w = w
self.x = x
self.y = y
self.z = z
elif euler is not None and axis_angle is not None:
raise AttributeError("Only one of axis_angle or euler can be specified.")
elif axis_angle is not None:
if not (type(axis_angle) == list or type(axis_angle) == np.ndarray) or len(axis_angle) != 3:
raise ValueError("axis_angle must be list or np.ndarray with length 3.")
axis_angle = np.array(axis_angle)
norm = np.linalg.norm(axis_angle)
self.w = np.cos(norm / 2)
if norm < 1e-50: # to avoid instabilities and nans
self.x = 0
self.y = 0
self.z = 0
else:
imag = axis_angle / norm * np.sin(norm / 2)
self.x = imag[0].item()
self.y = imag[1].item()
self.z = imag[2].item()
else:
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
cy = np.cos(yaw * 0.5)
sy = np.sin(yaw * 0.5)
cr = np.cos(roll * 0.5)
sr = np.sin(roll * 0.5)
cp = np.cos(pitch * 0.5)
sp = np.sin(pitch * 0.5)
# Fixed frame
self.w = cr * cp * cy + sr * sp * sy
self.x = sr * cp * cy - cr * sp * sy
self.y = cr * sp * cy + sr * cp * sy
self.z = cr * cp * sy - sr * sp * cy
# Rotating frame
# self.w = cr * cp * cy - sr * sp * sy
# self.x = cr * sp * sy + sr * cp * cy
# self.y = cr * sp * cy - sr * cp * sy
# self.z = cr * cp * sy + sr * sp * cy
def __repr__(self):
return "Quaternion (wxyz): [%2.5f, %2.5f, %2.5f, %2.5f]" % (self.w, self.x, self.y, self.z)
def to_axis_angle(self):
t = 2*np.arccos(self.w)
return np.array(t*np.array([self.x, self.y, self.z])/np.sin(t/2))
def to_mat(self):
v = np.array([self.x, self.y, self.z]).reshape(3,1)
return (self.w ** 2 - np.dot(v.T, v)) * np.eye(3) + \
2 * np.dot(v, v.T) + 2 * self.w * skew_symmetric(v)
def to_euler(self):
"""Return as xyz (roll pitch yaw) Euler angles."""
roll = np.arctan2(2 * (self.w * self.x + self.y * self.z), 1 - 2 * (self.x**2 + self.y**2))
pitch = np.arcsin(2 * (self.w * self.y - self.z * self.x))
yaw = np.arctan2(2 * (self.w * self.z + self.x * self.y), 1 - 2 * (self.y**2 + self.z**2))
return np.array([roll, pitch, yaw])
def to_numpy(self):
"""Return numpy wxyz representation."""
return np.array([self.w, self.x, self.y, self.z])
def normalize(self):
"""Return a (unit) normalized version of this quaternion."""
norm = np.linalg.norm([self.w, self.x, self.y, self.z])
return Quaternion(self.w / norm, self.x / norm, self.y / norm, self.z / norm)
def quat_mult_right(self, q, out='np'):
"""
Quaternion multiplication operation - in this case, perform multiplication
on the right, that is, q*self.
:param q: Either a Quaternion or 4x1 ndarray.
:param out: Output type, either np or Quaternion.
:return: Returns quaternion of desired type.
"""
v = np.array([self.x, self.y, self.z]).reshape(3, 1)
sum_term = np.zeros([4,4])
sum_term[0,1:] = -v[:,0]
sum_term[1:, 0] = v[:,0]
sum_term[1:, 1:] = -skew_symmetric(v)
sigma = self.w * np.eye(4) + sum_term
if type(q).__name__ == "Quaternion":
quat_np = np.dot(sigma, q.to_numpy())
else:
quat_np = np.dot(sigma, q)
if out == 'np':
return quat_np
elif out == 'Quaternion':
quat_obj = Quaternion(quat_np[0], quat_np[1], quat_np[2], quat_np[3])
return quat_obj
def quat_mult_left(self, q, out='np'):
"""
Quaternion multiplication operation - in this case, perform multiplication
on the left, that is, self*q.
:param q: Either a Quaternion or 4x1 ndarray.
:param out: Output type, either np or Quaternion.
:return: Returns quaternion of desired type.
"""
v = np.array([self.x, self.y, self.z]).reshape(3, 1)
sum_term = np.zeros([4,4])
sum_term[0,1:] = -v[:,0]
sum_term[1:, 0] = v[:,0]
sum_term[1:, 1:] = skew_symmetric(v)
sigma = self.w * np.eye(4) + sum_term
if type(q).__name__ == "Quaternion":
quat_np = np.dot(sigma, q.to_numpy())
else:
quat_np = np.dot(sigma, q)
if out == 'np':
return quat_np
elif out == 'Quaternion':
quat_obj = Quaternion(quat_np[0], quat_np[1], quat_np[2], quat_np[3])
return quat_obj