-
Notifications
You must be signed in to change notification settings - Fork 22
/
run_main_basic.m
335 lines (286 loc) · 12.4 KB
/
run_main_basic.m
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
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
% Script for simulating multi-quad collision avoidance using mpc
%% Clean workspace
clear
close all
clearvars
clearvars -global
clc
%% Initialization
initialize;
%% Generate solver
if getNewSolver
mpc_generator_basic;
end
%% Create quad objects and intialize ROS
for iQuad = 1 : model.nQuad
Quadrotor(iQuad) = CDrone(iQuad, quad_ID(iQuad), cfg);
Quadrotor(iQuad).initializeROS();
end
%% Create moving obstacle objects in simulation mode
if cfg.modeSim == 1
for jObs = 1 : model.nDynObs
DynObs(jObs) = CDynObs(jObs, cfg);
DynObs(jObs).initializeROS();
DynObs(jObs).initializeState(zeros(3,1), zeros(3,1), ...
cfg.obs.noise.pos, cfg.obs.noise.vel);
DynObs(jObs).randomState();
end
end
%% Communication with gui
% quad
com_quad_state = zeros(9, model.nQuad); % state
com_quad_goal = cfg.quad.goal; % goal
com_quad_input = zeros(4, model.nQuad); % input
com_quad_slack = zeros(2, model.nQuad); % slack
com_quad_path = zeros(3, model.N, model.nQuad); % planned path
com_quad_pathcov= zeros(6, model.N, model.nQuad); % path cov, 6 independent
% path cov initialization
simNoisePos = cfg.quad.noise.pos;
for iQuad = 1 : model.nQuad
for iStage = 1 : model.N
com_quad_path(:, iStage, iQuad) = quadStartPos(1:3, iQuad);
com_quad_pathcov(:, iStage, iQuad) = ...
[simNoisePos(1, 1); simNoisePos(2, 2); simNoisePos(3, 3); ...
simNoisePos(1, 2); simNoisePos(2, 3); simNoisePos(1, 3)];
end
end
% para
com_mpc_coll = [cfg.quad.coll, cfg.obs.coll];
com_mpc_weights = [cfg.weightStage, cfg.weightN];
% obs
com_obs_size = zeros(3, model.nDynObs);
com_obs_state = zeros(6, model.nDynObs);
com_obs_path = zeros(3, model.N, model.nDynObs);
%% Quadrotor coordination info
coor_quad_path = com_quad_path;
coor_quad_pathcov = com_quad_pathcov;
%% Initialization quad simulated initial state and mpc plan
for iQuad = 1 : model.nQuad
% coordination mode
Quadrotor(iQuad).modeCoor_ = cfg.modeCoor;
% initial state
Quadrotor(iQuad).pos_real_(1:3) = quadStartPos(1:3, iQuad);
Quadrotor(iQuad).vel_real_(1:3) = quadStartVel(1:3, iQuad);
Quadrotor(iQuad).euler_real_(1:3) = zeros(3, 1);
Quadrotor(iQuad).euler_real_(3) = quadStartPos(4, iQuad);
% for mpc
x_start = [Quadrotor(iQuad).pos_real_; Quadrotor(iQuad).vel_real_; ...
Quadrotor(iQuad).euler_real_];
z_start = zeros(model.nvar, 1);
z_start([index.z.pos, index.z.vel, index.z.euler]) = x_start;
mpc_plan = repmat(z_start, 1, model.N);
% initialize MPC
Quadrotor(iQuad).initializeMPC(x_start, mpc_plan);
% to avoid whom in prioritied planning
if Quadrotor(iQuad).modeCoor_ == -1
Quadrotor(iQuad).nQuad_ = iQuad;
Quadrotor(iQuad).npar_ = 18 + ...
(Quadrotor(iQuad).nQuad_ - 1 + Quadrotor(iQuad).nDynObs_)*model.nParamPerObs;
Quadrotor(iQuad).quad_path_ = zeros(3, model.N, Quadrotor(iQuad).nQuad_);
Quadrotor(iQuad).quad_pathcov_ = zeros(6, model.N, Quadrotor(iQuad).nQuad_);
end
end
%% Initialization graphic communicator
GraphicCom = CGraphicCom(true, cfg, model.nQuad, model.nDynObs, model.N);
% initialize ROS
GraphicCom.initializeROS();
% set default quad and obs size
for iQuad = 1 : model.nQuad
GraphicCom.quad_size_(:, iQuad) = cfg.quad.size;
end
for jObs = 1 : model.nDynObs
GraphicCom.obs_size_(:, jObs) = cfg.obs.size;
end
%% Logging variables
logsize = 10000;
% time
log_time = zeros(1, logsize); % loop time
% quad
log_quad_goal = zeros(4, logsize, model.nQuad); % quad goal
log_quad_state_real = zeros(9, logsize, model.nQuad); % quad real state
log_quad_state_est = zeros(9, logsize, model.nQuad); % quad estimated state
log_quad_input = zeros(4, logsize, model.nQuad); % quad input
log_quad_path = zeros(3, model.N, logsize, model.nQuad); % quad mpc path
log_quad_mpc_plan = zeros(model.nvar, model.N, logsize, model.nQuad); % quad complete mpc plan
log_quad_mpc_info = NaN(3, logsize, model.nQuad); % quad mpc solving info
% obs
log_obs_state_est = zeros(6, logsize, model.nDynObs); % obs estimated state
log_obs_path = zeros(3, model.N, logsize, model.nDynObs); % obs predicted path
%% Main control loop
fprintf('[%s] Looping... \n',datestr(now,'HH:MM:SS'));
% timers
dt_loop = model.dt; % delta t of the loop
mpc_solve_time = 0;
% iter
iter_loop = 0; % number of loops
% flag
flag_quad_out = 0; % if any quad is out of space
while(true)
%% in each control loop
% timer, start of the loop
t_loop_start = tic;
% loop count
iter_loop = iter_loop + 1;
% print on screen every ten loops
if(mod(iter_loop, 10) == 0)
fprintf('Loop: %d, MPC time: %.3f s, Frequency: %.3f Hz, RTF: %.2f\n', ...
iter_loop, mpc_solve_time, 1/dt_loop, dt_loop/model.dt);
end
%% publish dyn obs path in simulation mode
if cfg.modeSim == 1 % if in simulation
for jObs = 1 : model.nDynObs
if DynObs(jObs).pos_real_(1) < -cfg.ws(1) || ...
DynObs(jObs).pos_real_(1) > cfg.ws(1) || ...
DynObs(jObs).pos_real_(2) < -cfg.ws(2) || ...
DynObs(jObs).pos_real_(2) > cfg.ws(2) || ...
DynObs(jObs).pos_real_(3) < 0.4 || ...
DynObs(jObs).pos_real_(3) > 2.0
DynObs(jObs).randomState();
DynObs(jObs).randomSize();
end
DynObs(jObs).getEstimatedObsState();
DynObs(jObs).predictPathConstantV();
DynObs(jObs).sendPath();
DynObs(jObs).sendSize();
end
end
%% reset quad initial states when out of space in simulation
if flag_quad_out == 1 && cfg.modeSim == 1
for iQuad = 1 : model.nQuad
% initial state
Quadrotor(iQuad).pos_real_(1:3) = quadStartPos(1:3, iQuad);
Quadrotor(iQuad).vel_real_(1:3) = quadStartVel(1:3, iQuad);
Quadrotor(iQuad).euler_real_(1:3) = zeros(3, 1);
Quadrotor(iQuad).euler_real_(3) = quadStartPos(4, iQuad);
% for mpc
x_start = [Quadrotor(iQuad).pos_real_; Quadrotor(iQuad).vel_real_; ...
Quadrotor(iQuad).euler_real_];
z_start = zeros(model.nvar, 1);
z_start([index.z.pos, index.z.vel, index.z.euler]) = x_start;
mpc_plan = repmat(z_start, 1, model.N);
% initialize MPC
Quadrotor(iQuad).initializeMPC(x_start, mpc_plan);
end
flag_quad_out = 0;
end
%% controller for each quad
for iQuad = 1 : model.nQuad
% ===== Get estimated state of the ego quad =====
Quadrotor(iQuad).getEstimatedSystemState();
% ===== Set configuration parameters =====
Quadrotor(iQuad).quad_goal_ = com_quad_goal(1:4, iQuad);
Quadrotor(iQuad).mpc_coll_ = com_mpc_coll;
Quadrotor(iQuad).mpc_weights_ = com_mpc_weights;
% ===== Get predicted obstacles path and size =====
Quadrotor(iQuad).getObsPredictedPath();
Quadrotor(iQuad).getObsSize();
% ===== Get path of other quads =====
if Quadrotor(iQuad).modeCoor_ == -1 % for prioritied planning
Quadrotor(iQuad).quad_path_(:, :, :) = ...
coor_quad_path(:, :, 1 : iQuad);
else
Quadrotor(iQuad).quad_path_ = coor_quad_path;
end
% ===== Set online parameters for the MPC =====
Quadrotor(iQuad).setOnlineParamters();
% ===== Solve the mpc problem =====
Quadrotor(iQuad).solveMPC();
mpc_solve_time = Quadrotor(iQuad).mpc_info_.solvetime;
% ===== Send and execute the control command =====
Quadrotor(iQuad).step();
% ===== Communicate the planned mpc path =====
if Quadrotor(iQuad).modeCoor_ == 0 ...
|| Quadrotor(iQuad).modeCoor_ == -1 % sequential (prioritied) planning
coor_quad_path(:, :, iQuad) = Quadrotor(iQuad).mpc_Path_;
elseif Quadrotor(iQuad).modeCoor_ == 2 % path prediction based on constant v
Quadrotor(iQuad).predictPathConstantV();
end
% ====== if quad is out of space =====
if Quadrotor(iQuad).pos_real_(1) < -cfg.ws(1) || ...
Quadrotor(iQuad).pos_real_(1) > cfg.ws(1) || ...
Quadrotor(iQuad).pos_real_(2) < -cfg.ws(2) || ...
Quadrotor(iQuad).pos_real_(2) > cfg.ws(2) || ...
Quadrotor(iQuad).pos_real_(3) < 0.2 || ...
Quadrotor(iQuad).pos_real_(3) > cfg.ws(3)
flag_quad_out = 1;
end
% ===== Data logging =====
log_quad_goal(:, iter_loop, iQuad) = Quadrotor(iQuad).quad_goal_;
log_quad_state_real(:, iter_loop, iQuad) = [Quadrotor(iQuad).pos_real_; ...
Quadrotor(iQuad).vel_real_; ...
Quadrotor(iQuad).euler_real_];
log_quad_state_est(:, iter_loop, iQuad) = [Quadrotor(iQuad).pos_est_; ...
Quadrotor(iQuad).vel_est_; ...
Quadrotor(iQuad).euler_est_];
log_quad_input(:, iter_loop, iQuad) = Quadrotor(iQuad).u_body_;
log_quad_path(:, :, iter_loop, iQuad) = Quadrotor(iQuad).mpc_Path_;
log_quad_mpc_plan(:, :, iter_loop, iQuad) = Quadrotor(iQuad).mpc_ZPlan_;
log_quad_mpc_info(:, iter_loop, iQuad) = ...
[Quadrotor(iQuad).mpc_info_.solvetime; ...
Quadrotor(iQuad).mpc_info_.it; ...
Quadrotor(iQuad).mpc_info_.pobj];
end
%% communicate to gui for visualization
% quad
for iTemp = 1 : model.nQuad
com_quad_state(:, iTemp) = [Quadrotor(iTemp).pos_real_; ...
Quadrotor(iTemp).vel_real_; ...
Quadrotor(iTemp).euler_real_];
com_quad_input(:, iTemp) = Quadrotor(iTemp).u_body_;
com_quad_slack(:, iTemp) = 10*Quadrotor(iTemp).mpc_Zk_(index.z.slack);
com_quad_path(:, :, iTemp) = Quadrotor(iTemp).mpc_Path_;
if Quadrotor(iTemp).modeCoor_ == 1 % path communication (distributed)
coor_quad_path(:, :, iTemp) = Quadrotor(iTemp).mpc_Path_;
elseif Quadrotor(iQuad).modeCoor_ == 2 % path prediction based on constant v
coor_quad_path(:, :, iTemp) = Quadrotor(iTemp).pred_path_;
end
end
% obs
com_obs_path = Quadrotor(model.nQuad).obs_path_;
com_obs_size = Quadrotor(model.nQuad).obs_size_;
com_obs_state(1:3, :) = com_obs_path(1:3, 1, :);
com_obs_state(4:6, :) = diff(com_obs_path(1:3, 1:2, :), 1, 2) / model.dt;
% data
GraphicCom.quad_state_ = com_quad_state;
GraphicCom.quad_input_ = com_quad_input;
GraphicCom.quad_slack_ = com_quad_slack;
GraphicCom.quad_path_ = com_quad_path;
GraphicCom.obs_state_ = com_obs_state;
GraphicCom.obs_path_ = com_obs_path;
GraphicCom.obs_size_ = com_obs_size;
% publishing
GraphicCom.sendQuadState();
GraphicCom.sendQuadInput();
GraphicCom.sendQuadSlack();
GraphicCom.sendQuadPath();
GraphicCom.sendObsState();
GraphicCom.sendObsPath();
GraphicCom.sendObsSize();
% para
if GraphicCom.cfg_.setParaGui == 0
GraphicCom.quad_goal_ = com_quad_goal;
GraphicCom.mpc_coll_ = com_mpc_coll;
GraphicCom.mpc_weights_= com_mpc_weights;
GraphicCom.sendQuadGoal();
GraphicCom.sendMPCColl();
GraphicCom.sendMPCWeights();
else
GraphicCom.getQuadGoal();
GraphicCom.getMPCColl();
GraphicCom.getMPCWeights();
com_quad_goal = GraphicCom.quad_goal_;
com_mpc_coll = GraphicCom.mpc_coll_;
com_mpc_weights = GraphicCom.mpc_weights_;
end
%% timer, end of the loop
dt_loop = toc(t_loop_start);
%% simulate dyn obs in simulation mode
if cfg.modeSim == 1
for jObs = 1 : model.nDynObs
DynObs(jObs).step(min(dt_loop, model.dt));
end
end
log_obs_state_est(:, iter_loop, :) = com_obs_state;
log_obs_path(:, :, iter_loop, :) = com_obs_path;
log_time(iter_loop) = dt_loop;
end