-
Notifications
You must be signed in to change notification settings - Fork 0
/
dubins_polytope_obverver_acc.py
314 lines (260 loc) · 12.9 KB
/
dubins_polytope_obverver_acc.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
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
### SAME AS OTHER DUBINS POLYTOPE OBSERVER EXAMPLE
# This one uses acceleration inputs to the system
# u = (u_ang,u_trans) where u_ang is angular acceleration and u_trans is translational acceleration
from matplotlib import animation
import numpy as np
from utils.dubins_car.dubins_car import dubins_car # relative import of dubins car object
from utils.observer.polytope_observer import polytope_observer # importing polytope observer
from utils.dubins_car.utils import *
import matplotlib.pyplot as plt
from scipy.spatial import ConvexHull
from matplotlib.patches import Wedge
from matplotlib.animation import FuncAnimation,PillowWriter
from utils.shared.se2_x_R2_utils import *
from utils.shared.se2_utils import sample_simplex
seed = 0
rng = np.random.default_rng(seed=seed)
apx_se2 = True # should we approximate the SE(2) log and exp functions with taylor expansions?
# defining shortcut handles for the lie algebra exponential and logarithm maps
if apx_se2 :
exp = apx_se2_x_R2_exponential
log = apx_se2_x_R2_logarithm
else :
exp = se2_x_R2_exponential
log = se2_x_R2_logarithm
x0 = 2.0*(rng.random((5,))-0.5) # initial position and heading, random in [-1,1]^3
output = 'landmarks' # code is heavily tailored to this choice!
model = dubins_car(x0,output,acc_inputs=True)
landmarks = model.landmarks # pull the landmark locations
num_landmarks = landmarks.shape[1] # number of landmarks in simulation
T = 0.01 #simulation timestep
num_steps = 10
u = np.array([1.0,1.5]) # accelerate in a circle!
x = np.zeros((model.n_state,num_steps))
x[:,0] = x0
y = np.zeros((*model.n_output,num_steps))
y[:,:,0] = model.output_map(x[:,0],u)
# setting up reference system
xhat0 = 2.0*(rng.random((model.n_state,))-0.5)
reference = dubins_car(xhat0,output,acc_inputs=True) # initializing the reference model
# tracking reference trajectory
xref = np.zeros_like(x)
xref[:,0] = xhat0
estimate_center = np.zeros_like(x) # lie algebra (x,y,theta) representation of estimate center
estimate_center_group = (np.zeros((3,3,num_steps)),np.zeros((2,num_steps))) # group element representation of estimate center
set_radius = np.zeros((num_steps,))
error_LR = 'R' # left or right invariant error (this code is initially built for R)
if error_LR == 'L':
error = left_invariant_error_se2_x_R2 # function that computes error element in SE(2)
error_to_state = invert_left_invariant_error_se2_x_R2
elif error_LR == 'R':
error = right_invariant_error_se2_x_R2
error_to_state = invert_right_invariant_error_se2_x_R2
else :
raise ValueError('Invalid choice of left/right invariant error.')
# setting up polytope observer in lie algebra of group
set_n_state = 5 # we lift the error dynamics into R^3 so that the output map is linear
set_n_output = 2*num_landmarks # we have a 2D output for each landmark, we will vectorize it
set_sys_A = np.eye(set_n_state) # error dynamics are stationary
# construct the C matrix given the landmarks
set_sys_C = np.eye(N=set_n_state-2,M=set_n_state)
#set_sys_C[3:,3:] = 0
#for i in range(num_landmarks):
# set_sys_C[2*i,:] = np.array([-1.0, 0.0, landmarks[1,i]])
# set_sys_C[2*i+1,:] = np.array([0.0, -1.0, -landmarks[0,i]])
ub = 10.0*np.ones((set_n_state,1))
lb = -10.0*np.ones((set_n_state,1))
Aineq = np.zeros((2*set_n_state,set_n_state))
bineq = np.zeros((2*set_n_state,1))
for i in range(set_n_state):
Aineq[2*i,i] = 1.0 # upper bound
bineq[2*i] = ub[i]
Aineq[2*i+1,i] = -1.0 # lower bound
bineq[2*i+1] = -lb[i]
state_noise = 0.5
output_noise = 0.5
observer = polytope_observer(Aineq,bineq,state_noise,output_noise)
observer.compute_chebyshev_center(verbose=False)
error_center = observer.chebyshev_center
estimate_center_group[0][:,:,0],estimate_center_group[1][:,0] = error_to_state(exp(error_center),reference.state_group_rep())
estimate_center[:,0] = log((estimate_center_group[0][:,:,0],estimate_center_group[1][:,0]))
set_radius[0] = observer.chebyshev_radius
observer.fme_verbose = False
sim_verbose = True
print('='*75)
print('STEP {}'.format(0))
print('='*75)
group_error = error(model.state_group_rep(),reference.state_group_rep())
init_algebra_error = log(group_error) # error as lie algebra element in R^3
lifted_init_algebra_error = init_algebra_error # lifted error in R^4
#projected_init_algebra_error = project_error(lifted_init_algebra_error)
#print('Lift/project error:',projected_init_algebra_error-init_algebra_error)
#print('Group error:', *group_error)
print('Algebra error:', init_algebra_error)
#print('Lifted error:', lifted_init_algebra_error)
ybar = reference.state_group_rep()[0] @ y[:,:,0]# np.vstack((y[:,:,0],np.ones((1,num_landmarks))))
#print(reference.left_inverse(ybar))
vectorized_output = special_euclidean_logarithm(reference.left_inverse(ybar))
#print('xibar:',*xibar)
#lifted_output = reference.output_tangent(xibar)
#vectorized_output = np.reshape(xibar[0:2,:], (2*num_landmarks,), order='F')
print('ybar:',vectorized_output)
print('Output linearity error:', (set_sys_C @ lifted_init_algebra_error - vectorized_output))
#print('output mismatch:',vectorized_output-set_sys_C @ lifted_init_algebra_error)
print('Beginning in polytope?',observer.test_membership(lifted_init_algebra_error))
#print('Size of polytope:',observer.chebyshev_radius)
#print('Initial estimate:',estimate_center[:,0])
# tracking polytopes
polytopes = [observer.current_polytope()]
vertices = [observer.compute_vertices()]
# simulation loop
noise_scale = 0.0
for step in range(1,num_steps):
# step both models forward in time
model.step(u,T)
x[:,step] = model.x
y[:,:,step] = model.y + 2.0*(rng.random(model.y.shape)-0.5)*noise_scale
reference.step(u,T)
xref[:,step] = reference.x
ybar = reference.state_group_rep()[0] @ y[:,:,step]
vectorized_output = special_euclidean_logarithm(reference.left_inverse(ybar))#np.reshape(ybar[0:2,:], (2*num_landmarks,), order='F')
observer.step(set_sys_A, set_sys_C, vectorized_output)
polytopes.append(observer.current_polytope())
vertices.append(observer.compute_vertices())
# computing a state estimate with chebyshev center
observer.compute_chebyshev_center(verbose=False)
set_radius[step] = observer.chebyshev_radius
error_center = observer.chebyshev_center
estimate_center_group[0][:,:,step],estimate_center_group[1][:,step] = error_to_state(exp(error_center),reference.state_group_rep()) # group element representing center of estimate set
estimate_center[:,step] = se2_x_R2_state((estimate_center_group[0][:,:,step],estimate_center_group[1][:,step]))#log(estimate_center_group[:,:,step]) # lie algebra element in R^3 representing center of estimate set
# print out observer progress details if desired
if sim_verbose:
print('='*75)
print('STEP {}'.format(step))
print('='*75)
#print('True state SE(2):',*model.state_group_rep())
#print('Estimated state SE(2):',*estimate_center_group[:,:,step])
#print('True state (R^3):', model.x)
#print('Estimated state (R^3):', estimate_center[:,step])
#print('Error:', np.linalg.norm(model.x - estimate_center[:,step]))
group_error = error(model.state_group_rep(),reference.state_group_rep())
algebra_error = log(group_error)
lifted_algebra_error = algebra_error # error as lifted element in R^4
#true_estimate = error_to_state(exp(project_error(lifted_algebra_error)),reference.state_group_rep())
#print('True est - true state:', true_estimate-model.state_group_rep())
#print('Group error:', *group_error)
print('Algebra error:', algebra_error) # should be constnant!
#print('Lifted algebra error:', lifted_algebra_error)
print('ybar:',vectorized_output)
#print('Output error (wrt init):', np.linalg.norm(vectorized_output - set_sys_C @ lifted_init_algebra_error))
#print('error mismatch:',np.linalg.norm(init_true_error-true_error))
print('Output linearity error:', (set_sys_C @ lifted_algebra_error - vectorized_output))
print('In polytope:',observer.test_membership(lifted_algebra_error))
#print('Size of polytope:',observer.chebyshev_radius)
#print('Inequalities:')
#for i in range(observer.num_ineq):
# a = observer.Aineq[i,:]
# b = observer.bineq[i]
# flag = np.all(a @ lifted_algebra_error <= b)
# #flag = np.all(a @ observer.chebyshev_center <= b)
# print(a,b,flag)
f = plt.figure()
ax = f.add_subplot()
ax.scatter(x[0,0],x[1,0],marker="*",s=100,color='green',label='Initial position') # initial position
ax.scatter(x[0,-1],x[1,-1],marker="*",s=100,color='red',label='Final position') # terminal position
ax.plot(x[0,:],x[1,:],label='True trajectory',color='blue') # trajectory
ax.scatter(xref[0,0],xref[1,0],marker="*",s=100,color='green',label='Initial position') # initial position
ax.scatter(xref[0,-1],xref[1,-1],marker="*",s=100,color='red',label='Final position') # terminal position
ax.plot(xref[0,:],xref[1,:],label='Ref trajectory',color='red') # trajectory
ax.scatter(estimate_center[0,0],estimate_center[1,0],marker="*",s=100,color='green',label='Initial position') # initial position
ax.scatter(estimate_center[0,-1],estimate_center[1,-1],marker="*",s=100,color='red',label='Final position') # terminal position
ax.plot(estimate_center[0,:],estimate_center[1,:],label='Estimated trajectory',color='green') # trajectory
ax.grid()
ax.set_xlabel('x position')
ax.set_ylabel('y position')
ax.set_title('Trajectory')
ax.legend()
f2 = plt.figure()
ax2 = f2.add_subplot()
ax2.plot(set_radius)
ax2.set_xlabel('Time (steps)')
ax2.set_ylabel('Estimate set radius')
ax2.set_title('Convergence of set')
ax2.grid()
f3 = plt.figure()
ax3 = f3.add_subplot()
ax3.plot(np.linalg.norm(x - estimate_center,axis=0))
ax3.set_xlabel('Time (steps)')
ax3.set_ylabel('Estimate error')
ax3.set_title('Convergence of estimate')
ax3.grid()
# plot error region estimates
# roughly estimate the last plot by randomly sampling inside final polytope
num_vertices = vertices[-1].shape[0]
# if np.size(observer.rays) > 0:
# print('Polyhedron is unbounded!')
# randomly sample in the polyhedron
num_samples = 4000
sampled_simplex = sample_simplex(num_vertices,num_samples,seed=seed)
sampled_polytope = vertices[-1].T @ sampled_simplex # randomly sampled points in polytope
sampled_states = np.empty((set_n_state,num_samples))
for i in range(num_samples):
sampled_states[:,i] = se2_x_R2_state(error_to_state(exp(sampled_polytope[:,i]),reference.state_group_rep((xref[:3,-1],xref[3:,-1]))))
f4 = plt.figure()
ax4 = f4.add_subplot()
hull = ConvexHull(sampled_states.T[:,0:2])
P = plt.Polygon(sampled_states.T[hull.vertices,0:2],fill=True,alpha=0.5,color='blue')
ax4.add_patch(P)
ax4.scatter(sampled_states[0,:],sampled_states[1,:],marker='.',label='sample')
s = 0.05
lb = 180 * min(sampled_states[2,:])/np.pi
ub = 180* max(sampled_states[2,:])/np.pi
angle = Wedge((x[0,-1],x[1,-1]),1.2*s,lb,ub,alpha=0.5,color='blue')
ax4.add_patch(angle)
ax4.scatter(x[0,-1],x[1,-1],marker='*',s=100,label='true state',color='red')
ax4.arrow(x[0,-1],x[1,-1],s*np.cos(x[2,-1]),s*np.sin(x[2,-1]),width=0.002,color='red')
ax4.set_xlabel('x')
ax4.set_ylabel('y')
ax4.set_title('{} random samples in error polytope'.format(num_samples))
ax4.grid()
ax4.legend()
s = 0.1
num_samples = 500
f5, ax5 = plt.subplots()
#sample_sc = ax5.scatter(np.zeros((num_samples,)),np.zeros((num_samples,)),label='Samples')
poly = plt.Polygon(rng.random((4,2)),fill=True,ec='blue',alpha=0.5,closed=True,lw=1)
ax5.add_patch(poly)
est_angle = Wedge((0,0),1.2*s,0,1,alpha=0.5,color='blue')
ax5.add_patch(est_angle)
truth_sc = ax5.scatter(0,0,label='Truth',marker='*',color='red',s=100)
truth_angle = ax5.arrow(0,0,1,1,width=0.002,color='red')
ax5.set_xlim(0.0,1.5)
ax5.set_ylim(-1.0,1.0)
ax5.grid()
ax5.legend()
def sample_states(i,num_samples): # randomly generates num_samples feasible states from error polytope at timestep i
num_vertices = vertices[i].shape[0]
sampled_polytope = vertices[i].T @ sample_simplex(num_vertices,num_samples)
sampled_states = np.empty((5,num_samples))
for j in range(num_samples):
sampled_states[:,j] = se2_x_R2_state(error_to_state(exp(sampled_polytope[:,j]),reference.state_group_rep((xref[:3,i],xref[3:,i]))))
return sampled_states
def update(i):
title = 'Timestep {}'.format(i)
pts = sample_states(i,num_samples)
hull = ConvexHull(pts.T[:,0:2])
#sample_sc.set_offsets(np.c_[pts[0,:],pts[1,:]])
est_angle.set_center((x[0,i],x[1,i]))
lb = 180*min(pts[2,:]) / np.pi
ub = 180*max(pts[2,:]) / np.pi
est_angle.set_theta1(lb)
est_angle.set_theta2(ub)
truth_angle.set_data(x=x[0,i],y=x[1,i],dx=s*np.cos(x[2,i]),dy=s*np.sin(x[2,i]))
poly.set_xy(pts.T[hull.vertices,0:2])
truth_sc.set_offsets(np.c_[x[0,i],x[1,i]])
ax5.set_title(title)
return poly, truth_sc,truth_sc,est_angle
anim = FuncAnimation(f5,update,frames=np.arange(0,num_steps),repeat=True,interval=200,blit=True)
anim.save('set_evolution.gif', dpi=300, writer=PillowWriter(fps=5))
# TODO: Plotting the 4D Polyhedron by projecting it into two dimenions a few ways
plt.show()