├── README.md
├── S01_compare_origial_reshaped.m
├── S02_three_quadrotors_2D.m
├── S03_gain_impact_safety.m
└── half_space_2D.m


/README.md:
--------------------------------------------------------------------------------
 1 | # Examples
 2 | These are the simulations of ``Quadratic Programming for Continuous Control of Safety-Critical Multi-Agent Systems Under Uncertainty''.
 3 | 
 4 | **S01_compare_origial_reshaped.m** is the simulation of this paper (Fig 8 and 9).
 5 | 
 6 | <img src="https://user-images.githubusercontent.com/28443522/220050175-ffb18231-2b3d-4fef-85fe-0dd4cc2799e0.gif" width="400px">
 7 | 
 8 | **S02_three_quadrotors_2D.m** is a simulation of the experiment of this paper.
 9 | 
10 | **S03_gain_impact_safety.m** shows that the dynamic uncertainty may cause unsafety. 
11 | 
12 | # Reference
13 | 
14 | Paper link: https://ieeexplore.ieee.org/abstract/document/10043732
15 | ```
16 | @article{Wu-Liu-Egerstedt-Jiang-2023-TAC,
17 |   title={Quadratic programming for continuous control of safety-critical multi-agent systems under uncertainty},
18 |   author={Wu, Si and Liu, Tengfei and Egerstedt, Magnus and Jiang, Zhong-Ping},
19 |   journal={IEEE Transactions on Automatic Control},
20 |   year={2023},
21 |   publisher={IEEE}
22 | }
23 | ```
24 | 


--------------------------------------------------------------------------------
/S01_compare_origial_reshaped.m:
--------------------------------------------------------------------------------
  1 | %% MATLAB Version R2017b
  2 | clear all; clc; close all;
  3 | 
  4 | Time= 6;               % simulation time, sec
  5 | T   = 10e-3;            % step, sec
  6 | N   = floor(Time/T);    % steps
  7 | t   = 0;
  8 | 
  9 | % define the initial position
 10 | p1=[[-1.0; 1.0], ...        % position of agent-1
 11 |     [ 0.5; 0.5], ...
 12 |     [-0.5;-0.5]];           % position of obstacle-1
 13 | vp = [2, -2];
 14 | p2 = p1;
 15 | 
 16 | % agent and obstacle setting
 17 | na  = 1;              % number of mobile agents
 18 | no  = size(p1, 2)-1;  % number of obstacles
 19 | nao = na+no;
 20 | x1  = zeros(8, na);   % actuation state of agent1
 21 | x2  = zeros(8, na);   % actuation state of agent2
 22 | vp1 = zeros(2, na);   % ideal velocity
 23 | vp2 = zeros(2, na);   % ideal velocity
 24 | v_set1 = zeros(2, na);   % the input of the actuation
 25 | v_set2 = zeros(2, na);   % the input of the actuation
 26 | delta1 = zeros(1, na);   % the slack variable
 27 | delta2 = zeros(1, na);   % the slack variable
 28 | 
 29 | % quadraitc programming setting
 30 | Ap_i= getPolyAb(11, 1, 0);      %
 31 | ap_i= zeros(size(Ap_i, 1), na);
 32 | A1   = zeros(nao-1, 2, na);
 33 | b1   = zeros(nao-1, 1, na);
 34 | A2   = zeros(nao-1, 2, na);
 35 | b2   = zeros(nao-1, 1, na);
 36 | 
 37 | Ds  = 0.65;
 38 | % Ds  = sqrt(2)/2-10e-3;
 39 | Do  = 0.6;
 40 | delta_bar = 1e2;
 41 | alpha_v = 0.75;
 42 | vpd_bar = 5;
 43 | vp_bar  = 5;
 44 | 
 45 | % initialize graphic
 46 | H = myGraphic([], p1, x1, p2, x2, Ds, Ap_i, ap_i);
 47 | 
 48 | % simulation process
 49 | for k = 1:N
 50 |     % sample time
 51 |     t = k*T;
 52 |     
 53 |     % ideal velocity
 54 |     vp1(:, 1)= vp;
 55 |     vp2(:, 1)= vp;
 56 |     
 57 |     % collision avoidance
 58 |     [v_set1(:,1), delta1(1), ap_i(:,1), A1(:,:,1), b1(:,:,1)] = collision_avoidance_reshape(p1, vp1, Ds, Ap_i, delta_bar, alpha_v);
 59 |     [v_set2(:,1), delta2(1),            A2(:,:,1), b2(:,:,1)] = collision_avoidance(p2, vp2, Ds, delta_bar, alpha_v);
 60 |         
 61 |     % update model
 62 |     x1(:, 1) = rungekutta(@m_speed,    x1(:, 1), v_set1(  :, 1), T);     % ????-1
 63 |     x2(:, 1) = rungekutta(@m_speed,    x2(:, 1), v_set2(  :, 1), T);     % ????-2
 64 |     
 65 |     % Model 210918, res_06.mat
 66 |     C=[ 0.7761   -1.9815         0         0    2.1315    2.4144         0         0
 67 |              0         0   -2.2000   -2.8153         0         0   -1.5060   -0.9899];
 68 |     p1(:, 1) = rungekutta(@m_position, p1(:, 1),     C*x1, T);     % ????-1
 69 |     p2(:, 1) = rungekutta(@m_position, p2(:, 1),     C*x2, T);     % ????-2
 70 |     
 71 |     
 72 |     % update the graphic
 73 |     H = myGraphic(H, p1, x1, p2, x2, Ds, Ap_i, ap_i);
 74 | end
 75 | %% end of main.
 76 | 
 77 | %%
 78 | function [v_set, delta, A, b] = collision_avoidance(p, vp, Ds, delta_bar, alpha_v)
 79 | persistent opt;
 80 | if(isempty(opt))
 81 | %     opt = optimoptions('quadprog',  'Algorithm','interior-point-convex','Display','off', 'MaxIterations', 1e3);
 82 |     opt = optimoptions('quadprog',  'Display','off', 'MaxIterations', 1e3);
 83 | end
 84 | 
 85 | pm    = p(:, 1);
 86 | v_aim = vp(:, 1);
 87 | nao   = size(p, 2);
 88 | 
 89 | % generate conditions
 90 | A = zeros(nao-1, 2);
 91 | b = zeros(nao-1, 1);
 92 | count = 0;
 93 | for k = 2:nao
 94 |     count = count + 1;
 95 |     p_delta = pm - p(:, k);     % relative position
 96 |     dist    = norm(p_delta, 2); % distance
 97 |     h = dist - Ds;              % barrier function
 98 |     V = 1/(h+Ds);               % Lyapunov function
 99 |     % condition - k
100 |     A(count, :) = -p_delta'/norm(p_delta);
101 |     b(count, :) = -alpha_v*(V-1/Ds)/delta_bar;
102 | %     b(count, :) = -alpha_v*(V-1/Ds);
103 | %     b(count, :) = alpha_v*(dist^2-Ds^2);
104 | end
105 | 
106 | % quadratic programming
107 | f_aim = [v_aim; delta_bar];
108 | 
109 | % Standard Quadratic Programming with Slack Variables
110 | v_set_det = quadprog(...
111 |     diag([1, 1, 1]), -f_aim,...                             % cost function
112 |     [A, -b], zeros(size(A, 1), 1), [], [],  ...                   % constraints
113 |     [-inf; -inf; 0], [inf; inf; delta_bar], zeros(3, 1), opt);    % limits and setting
114 | 
115 | v_set  = v_set_det(1:end-1);
116 | delta  = v_set_det(end);
117 | end
118 | 
119 | function [v_set, delta, ap_i, A, b] = collision_avoidance_reshape(p, vp, Ds, Ap_i, delta_bar, alpha_v)
120 | persistent opt;
121 | if(isempty(opt))
122 | %     opt = optimoptions('quadprog',  'Algorithm','interior-point-convex','Display','off', 'MaxIterations', 1e3);
123 |     opt = optimoptions('quadprog',  'Display','off', 'MaxIterations', 1e3);
124 | end
125 | 
126 | pm    =  p(:, 1);
127 | v_aim = vp(:, 1);
128 | nao   = size(p, 2);
129 | 
130 | % generate conditions
131 | A = zeros(nao-1, 2);
132 | b = zeros(nao-1, 1);
133 | count = 0;
134 | 
135 | for k = 2:nao
136 |     count = count + 1;
137 |     p_delta = pm - p(:, k);     % relative position
138 |     dist    = norm(p_delta, 2); % distance
139 |     h = dist - Ds;              % barrier function
140 |     V = 1/(h+Ds);               % Lyapunov function
141 |     
142 |     % condition - count
143 |     A(count, :) = -p_delta'/norm(p_delta);
144 |     b(count, :) = -alpha_v*(V-1/Ds)/delta_bar;
145 | %     b(count, :) = alpha_v*(dist^2-Ds^2);
146 | end
147 | 
148 | % quadratic programming
149 | f_aim = [v_aim; delta_bar];
150 | tau   = 2*sqrt(2)/delta_bar;
151 | ap_i  = gen_a(Ap_i, A, -b, tau);  % Ap_i x + a delta \le 0
152 | M     = [Ap_i, ap_i];
153 | 
154 | % Quadratic Programming with Slack Variables - Lipschitz
155 | [v_set_det, ~, ~, ~, ~] = quadprog(...
156 |     diag([1, 1, 1]), -f_aim,...                             % cost function
157 |     M, zeros(size(M, 1), 1), [], [],  ...                   % constraints
158 |     [-inf; -inf; 0], [inf; inf; delta_bar], zeros(3, 1), opt);    % limits and setting
159 | 
160 | v_set  = v_set_det(1:end-1);
161 | delta  = v_set_det(end);
162 | if(delta <=  1e-6)
163 |     1;
164 | end
165 | end
166 | 
167 | % function H = myGraphic(H, p1, x1, Ds, Ap_i, ap_i, delta)
168 | function H = myGraphic(H, p1, x1, p2, x2, Ds, Ap_i, ap_i)
169 | persistent crashflag_2;
170 | if(isempty(crashflag_2))
171 |     crashflag_2 = false;
172 | end
173 | 
174 | nao = size(p1, 2);
175 | na  = 1;
176 | no  = nao - na;
177 | Do  = evalin('base', 'Do');
178 | drawnow limitrate;
179 | if(isempty(H))
180 |     % initialize graphic
181 |     figure(1); clf;
182 |     axis equal;
183 |     hold on;
184 |     grid minor;
185 |     set(gcf, 'position', [0,0, 800, 700 ], 'color', 'w');
186 |     
187 |     % obstacles
188 |     for iO = (na+1):(na+no)
189 |         H.shapeO(iO) = patch(p1(1,iO)+Ds*sin(linspace(0, 2*pi, 30)), p1(2,iO)+Ds*cos(linspace(0, 2*pi, 30)), 'c', 'facealpha', 0.1, 'HandleVisibility', 'off', 'linestyle', '--', 'EdgeColor', 'k');
190 |         H.realO(iO)  = patch(p1(1,iO)+Do*sin(linspace(0, 2*pi, 30)), p1(2,iO)+Do*cos(linspace(0, 2*pi, 30)), 'c', 'facealpha', 1.0, 'HandleVisibility', 'off');
191 |     end
192 |     H.posO = plot(p1(1, na+1:end), p1(2, na+1:end), 'k.', 'markersize', 5, 'linestyle', 'none', 'HandleVisibility', 'off');
193 |     
194 |     % mobile agents
195 |     HandleVisibility  = 'on';
196 |     H.trajA(2) = animatedline('color', 'k', 'HandleVisibility', HandleVisibility, 'linewidth', 2, 'linestyle', ':', 'marker', 'none', 'Displayname', 'RP');
197 |     H.trajA(3) = animatedline('color', 'k', 'HandleVisibility', HandleVisibility, 'linewidth', 2, 'linestyle', '--', 'marker', 'none', 'Displayname', 'RP after crash');
198 |     H.trajA(1) = animatedline('color', 'b', 'HandleVisibility', HandleVisibility, 'linewidth', 2, 'linestyle', '-', 'marker', 'none', 'Displayname', 'RPRF');
199 |     H.A = line([p1(1, 1), p2(1, 1)], [p1(2, 1), p2(2, 1)], 'Marker', 'o', 'linestyle', 'none', 'color','k', 'HandleVisibility', 'off');
200 |     legend;
201 |     text(-0.85, 1.2, 'Initial Position', 'fontsize', 18, 'HorizontalAlignment', 'center');
202 |     text(-1.0, 1.02, '{\bf\times}', 'fontsize', 24, 'HorizontalAlignment', 'center');
203 |     axis(1.3*[-1, 1, -1, 1]);
204 | %     xlabel('$[p_1]_1$ (m)','Interpreter','latex'); ylabel('$[p_1]_2$ (m)','Interpreter','latex');
205 |     xlabel('[p_1]_1 (m)'); ylabel('[p_1]_2 (m)');
206 | %     set(gca, 'fontsize', 16, 'position', [0.06,0.13,0.85,0.85]);
207 |     set(gca, 'fontsize', 19, 'box', 'on');
208 |     fillwindows(gca);
209 |     
210 |     % feasible region
211 |     figure(2); clf;
212 |     axis equal;
213 |     hold on;
214 |     grid on;
215 |     set(gcf, 'position', [0, 530, 800, 500 ], 'color', 'w');
216 |     set(gca, 'fontsize', 18);
217 |     H.feasibleRegion_1 = half_space_2D([], Ap_i, ap_i, 2*[-1, 1, -1, 1], 'b', 'domain', 'reshaped');
218 |     H.feasibleRegion_2 = half_space_2D([], zeros(nao-1, 2), zeros(nao-1, 1), 2*[-1, 1, -1, 1], 'k', 'domain', 'original');
219 |     
220 |     %     H.v_p1_hdl   = quiver(0, 0, 0.0, 0.0, 1.0, 'linewidth', 2, 'color', 'b', 'MaxHeadSize',2, 'DisplayName', '$v^p_1$');
221 |     H.v_set1_hdl = quiver(0, 0, 0.0, 0.0, 1.0, 'linewidth', 2, 'color', 'b', 'MaxHeadSize',2, 'linestyle', '-', 'DisplayName', '$v^*_1$');
222 |     H.v_set2_hdl = quiver(0, 0, 0.0, 0.0, 1.0, 'linewidth', 2, 'color', 'k', 'MaxHeadSize',2, 'linestyle', '-', 'DisplayName', '$v^*_2$');
223 |     lgd2_handle = legend('location', 'northwest');
224 |     set(lgd2_handle,'Interpreter','latex', 'FontSize', 18);
225 |     
226 |     % trajectory
227 |     figure(3); clf;
228 |     set(gcf, 'position', [800, 530, 1000, 700 ], 'color', 'w');
229 |     subplot(2, 1, 1);
230 |     hold on;
231 |     grid on;
232 |     set(gca, 'fontsize', 16);
233 |     H.Dist1= animatedline('color', 'b', 'HandleVisibility', HandleVisibility, 'linewidth', 2, 'linestyle', '-', 'marker', 'none', 'DisplayName', 'RPRF');
234 |     H.Dist2= animatedline('color', 'k', 'HandleVisibility', HandleVisibility, 'linewidth', 2, 'linestyle', ':', 'marker', 'none', 'DisplayName', 'RP');
235 |     H.D_safe= plot([0 evalin('base', 'Time')], Do*[1, 1], 'k-', 'linestyle', '--', 'HandleVisibility', 'off');
236 |     lgd3_handle = legend('location', 'southwest','Orientation','horizontal');
237 |     set(lgd3_handle,'Interpreter','latex', 'FontSize', 18)
238 |     xlim([0 evalin('base', 'Time')]);
239 |     xlabel('time (sec)');
240 |     ylim([0 2]);
241 |     ylabel({'minimal distance', '$\min\{|\tilde{p}_{12}|, |\tilde{p}_{12}|\}$  (m)'}, 'Interpreter' , 'latex');
242 |     
243 |     % trajectory
244 |     subplot(2, 1, 2);
245 |     hold on;
246 |     grid on;
247 |     set(gca, 'fontsize', 16);
248 |     H.v_set1 = animatedline('color', 'b', 'HandleVisibility', HandleVisibility, 'linewidth', 2, 'linestyle', '-', 'marker', 'none', 'DisplayName', 'RPRF');
249 |     H.v_set2 = animatedline('color', 'k', 'HandleVisibility', HandleVisibility, 'linewidth', 2, 'linestyle', ':', 'marker', 'none', 'DisplayName', 'RP');
250 | %     H.D_safe= plot([0 evalin('base', 'Time')], Do*[1, 1], 'k-', 'linestyle', '--', 'HandleVisibility', 'off');
251 |     lgd3_handle = legend('location', 'northwest','Orientation','horizontal');
252 |     set(lgd3_handle,'Interpreter','latex', 'FontSize', 18)
253 |     xlim([0 evalin('base', 'Time')]);
254 |     xlabel('time (sec)');
255 |     ylim([0 3]);
256 |     ylabel({'velocity reference', '$|v^*_1|$ (m/s)'}, 'Interpreter' , 'latex');
257 |     
258 |     figure(4); clf;
259 |     set(gcf, 'position', [800, 530, 800, 400 ], 'color', 'w');
260 |     hold on;
261 |     grid on;
262 |     set(gca, 'fontsize', 18, 'box', 'on');
263 | %     set(gca, 'fontsize', 16, 'position', [0.15,0.22,0.80,0.78]);
264 |     H.v_set2 = animatedline('color', 'k', 'HandleVisibility', HandleVisibility, 'linewidth', 2, 'linestyle', ':', 'marker', 'none', 'DisplayName', 'RP');
265 |     H.v_set2_crash = animatedline('color', 'k', 'HandleVisibility', HandleVisibility, 'linewidth', 2, 'linestyle', '--', 'marker', 'none', 'Displayname', 'RP after crash');
266 |     H.v_set1 = animatedline('color', 'b', 'HandleVisibility', HandleVisibility, 'linewidth', 2, 'linestyle', '-', 'marker', 'none', 'DisplayName', 'RPRF');
267 |     
268 | %     H.D_safe= plot([0 evalin('base', 'Time')], Do*[1, 1], 'k-', 'linestyle', '--', 'HandleVisibility', 'off');
269 | %     lgd3_handle = legend('location', 'northwest','Orientation','horizontal');
270 |     lgd3_handle = legend('location', 'northwest');
271 |     set(lgd3_handle, 'FontSize', 18)
272 |     xlim([0 evalin('base', 'Time')]);
273 |     xlabel('time (sec)');
274 |     ylim([0 3.0]);
275 | %     ylabel({'velocity command', '$|v^*_1|$ (m/s)'}, 'Interpreter' , 'latex');
276 |     ylabel({'velocity reference', '|v^*_1| (m/s)'});
277 |     fillwindows(gca);
278 | else
279 |     % get variables from workspace
280 |     t     = evalin('base', 't');
281 |     T     = evalin('base', 'T');
282 |     
283 |     delta1= evalin('base', 'delta1');   % reashaped algorighm
284 |     v1_real= x1(3:4, :);
285 |     A1    = evalin('base', 'A1');
286 |     b1    = evalin('base', 'b1');
287 |     v_set1 = evalin('base', 'v_set1');
288 |     vp1   = evalin('base', 'vp1');
289 |     
290 |     delta2= evalin('base', 'delta2');   % original
291 |     v2_real= x2(3:4, :);
292 |     A2    = evalin('base', 'A2');
293 |     b2    = evalin('base', 'b2');
294 |     v_set2 = evalin('base', 'v_set2');
295 |     vp2   = evalin('base', 'vp2');
296 |     
297 |     % update the graphic
298 |     % mobile agents
299 |     H.A.XData = [p1(1, 1), p2(1, 1)];
300 |     H.A.YData = [p1(2, 1), p2(2, 1)];
301 |     
302 |     addpoints(H.trajA(1), p1(1, 1), p1(2, 1));    
303 |     if(min(vecnorm(p2(:, 1) - p2(:, 2:end))) < Do || crashflag_2)
304 |         addpoints(H.trajA(3), p2(1, 1), p2(2, 1));
305 |         addpoints(H.v_set2_crash, t,  norm(v_set2) );
306 |         crashflag_2 = true;
307 |     else
308 |         addpoints(H.trajA(2), p2(1, 1), p2(2, 1));
309 |         addpoints(H.v_set2, t,  norm(v_set2) );
310 |     end
311 |     
312 |     %   addpoints(H.V,         t, maxLog10_0(1/norm(p(:, 1)-p(:, 2))) );
313 |     addpoints(H.Dist1, t,  min(vecnorm(p1(:, 1) - p1(:, 2:end))));
314 |     addpoints(H.Dist2, t,  min(vecnorm(p2(:, 1) - p2(:, 2:end))));
315 |     addpoints(H.v_set1, t,  norm(v_set1) );
316 |     
317 |     
318 |     % update feasible region and arrow
319 |     half_space_2D(H.feasibleRegion_1,      Ap_i,  -ap_i(:, 1)*delta1(1));
320 |     half_space_2D(H.feasibleRegion_2, A2(:,:,1),    b2(:,:,1)*delta2(1));
321 |     H.v_set1_hdl.UData = v_set1(1, 1); H.v_set1_hdl.VData = v_set1(2, 1);
322 |     H.v_set2_hdl.UData = v_set2(1, 1); H.v_set2_hdl.VData = v_set2(2, 1);
323 | end
324 | end
325 | 
326 | function dx = m_speed(x, u)
327 | % Model 210918, res_06.mat
328 | A=[-1.5828    2.9188         0         0         0         0         0         0
329 |    -2.9188   -1.5828         0         0         0         0         0         0
330 |          0         0   -2.6833    7.1816         0         0         0         0
331 |          0         0   -7.1816   -2.6833         0         0         0         0
332 |          0         0         0         0   -2.5615    6.8558         0         0
333 |          0         0         0         0   -6.8558   -2.5615         0         0
334 |          0         0         0         0         0         0   -2.1391    3.7051
335 |          0         0         0         0         0         0   -3.7051   -2.1391];
336 |      
337 | B=[ 1.6527         0
338 |     0.6473         0
339 |     1.4972         0
340 |     0.9178         0
341 |          0    1.5791
342 |          0    0.8422
343 |          0    1.5056
344 |          0   -2.2906];
345 | dx = A*x + B*u;
346 | end
347 | 
348 | function dx = m_position(x, u)
349 | A  = 0*[-1, 0; 0, -1];
350 | B  = [1, 0; 0 1];
351 | dx = A*x + B*u;
352 | end
353 | 
354 | function x = rungekutta(fun, x0, u, h)
355 | % FcnHandlesUsed  = isa(fun,'function_handle');
356 | k1 = fun(x0       , u);
357 | k2 = fun(x0+h/2*k1, u);
358 | k3 = fun(x0+h/2*k2, u);
359 | k4 = fun(x0+  h*k3, u);
360 | x = x0 + h/6*(k1 + 2*k2 + 2*k3 + k4);
361 | end
362 | 
363 | function  res = limit(res, lower, upper)
364 | res = min(max(res, lower), upper);
365 | end
366 | 
367 | %%
368 | function ap = gen_a(Ap, A, a, tau)
369 | N  = size(Ap, 1);
370 | ap = zeros(N, 1);
371 | cos_tau = cos(2*pi/N);              % ????
372 | A_unit     = unit_vector(A);        % R^{m x n}, a\in R{m x 1}
373 | polyA_unit = unit_vector(Ap);       % R^{N x n}
374 | cos_theta  = polyA_unit*A_unit';    % R^{N x m}
375 | chi = cos_theta.*a' - limit(1*(cos_tau-cos_theta), 0, 1).*(tau+cos_theta.*a');
376 | ap  = max(chi, [], 2); % ?????????????????
377 | end
378 | 
379 | % ?????
380 | function A = getPolyAb(N, Radius, theta_bias)
381 | % 2???
382 | theta    = linspace(theta_bias, theta_bias+2*pi, N+1)';
383 | % position = Radius*[cos(theta(1:end-1)), sin(theta(1:end-1))];
384 | % [A, ~]   = getlines(position);
385 | A = [cos(theta(1:end-1)), sin(theta(1:end-1))];
386 | % fprintf('??????????????? %f \n', min(matrix_minsvd(A)));
387 | end
388 | 
389 | function [A, a] = getlines(p)
390 | N = size(p, 1);
391 | m = size(p, 2);
392 | A = zeros(N, m);
393 | a = -ones(N, 1);
394 | for idx = 1:N
395 |     p_index   = mod((idx:idx+(m-1))-1, N) + 1;
396 |     p_tmp     = p(p_index, :);
397 |     A(idx, :) = (p_tmp\ones(m, 1))';
398 |     len = norm(A(idx, :));
399 |     A(idx, :) = A(idx, :)/len;
400 |     a(idx, :) = a(idx, :)/len;
401 | end
402 | end
403 | 
404 | 
405 | function  M = unit_vector(M)
406 | 
407 | for idx = 1:size(M,1)
408 |     M(idx, :) = M(idx, :)/ norm(M(idx, :));
409 | end
410 | 
411 | end
412 | 
413 | 
414 | function res = maxLog10_0(in)
415 | res = max(log10(in), 0);
416 | end
417 | 
418 | function dx = m_idealvelocity(x, u, vp_bar, vpd_bar)
419 | T   = 1;
420 | x_r = u/norm(u)*min(norm(u), vp_bar);
421 | dx = -1/T*(x - x_r);
422 | dx = dx/norm(dx)*min(norm(dx), vpd_bar);
423 | end
424 | 
425 | function fillwindows(ax)
426 | outerpos = ax.OuterPosition;
427 | ti = ax.TightInset; 
428 | left = outerpos(1) + ti(1);
429 | bottom = outerpos(2) + ti(2);
430 | ax_width = outerpos(3) - ti(1) - ti(3);
431 | ax_height = outerpos(4) - ti(2) - ti(4);
432 | ax.Position = [left bottom ax_width ax_height];
433 | end


--------------------------------------------------------------------------------
/S02_three_quadrotors_2D.m:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/EXPSIN/Quadratic-Programming-for-Continuous-Control-of-Safety-Critical-Multi-Agent-Systems-Under-Uncertaint/c20f6129b3b5190754c293ec1d164788bdcc6ef5/S02_three_quadrotors_2D.m


--------------------------------------------------------------------------------
/S03_gain_impact_safety.m:
--------------------------------------------------------------------------------
  1 | clear all; close all; clc;
  2 | 
  3 | Time= 5;                 % simulation time, sec
  4 | T   = 10e-3;             % step, sec
  5 | N   = floor(Time/T);     % steps
  6 | t   = 0;
  7 | p   = 0.4;
  8 | x   = zeros(2, 1);
  9 | Ds  = 0.8;
 10 | alpha_v =[1,4,8,16];
 11 | 
 12 | 
 13 | figure(1);
 14 | set(gcf, 'position', [10, 10, 800, 400], 'color', 'w');
 15 | subplot(1, 5, [2, 5]); hold on;
 16 | 
 17 | px = [  0, Time, Time,  0,  0];
 18 | py = [-Ds,  -Ds,   Ds, Ds, -Ds];
 19 | patch(px, py+1, 'c', 'facealpha', 0.2, 'displayname', 'unsafe area');
 20 | patch(px, py-1, 'c', 'facealpha', 0.2, 'displayname', 'unsafe area', 'handlevisibility', 'off');
 21 | klinestyle = {'-', '--', ':', '-.'};
 22 | for i_Sim  = 1:length(alpha_v)
 23 |     H(i_Sim) = animatedline('color', 'k', 'displayname', sprintf('$p_1(t)~(k=%d)$', alpha_v(i_Sim)), 'linewidth', 2, 'linestyle', klinestyle{i_Sim}); 
 24 | end
 25 | plot([0, Time], [ 1,  1], 'r--', 'linewidth', 2, 'displayname', '$p_2$');
 26 | plot([0, Time], [-1, -1], 'r--', 'linewidth', 2, 'displayname', '$p_3$');
 27 | set(gca, 'position', [0.1, 0.2, 0.9, 0.7]);
 28 | 
 29 | 
 30 | % lH = legend('location', 'southwest','Orientation','horizontal');
 31 | lH = legend('location', 'northeastoutside');
 32 | set(lH, 'Interpreter','latex');
 33 | axis([0, Time, -1.5, 1.5]);
 34 | xlabel('time (sec)');
 35 | ylabel('position (m)');
 36 | set(gca, 'fontsize', 16);
 37 | 
 38 | % subplot(1, 5, 1); hold on;
 39 | % plot([0, 0], [1, -1], 'k*','linestyle', 'none');
 40 | % H2 = line('marker', 'o'); 
 41 | % set(gca, 'position', [0.1, 0.1, 0.1, 0.8]);
 42 | % axis equal;
 43 | % xticks([]);
 44 | % ylabel('position (meter)');
 45 | % set(gca, 'fontsize', 16);
 46 | 
 47 | % simulation process
 48 | for i_Sim  = 1:length(alpha_v)
 49 |     p   = 0.4;
 50 |     x   = zeros(2, 1);
 51 |     for k = 1:N
 52 |         % sample time
 53 |         t = k*T;
 54 | 
 55 |         % collision avoidance
 56 |         v_set = collision_avoidance(p, 0, Ds, alpha_v(i_Sim));
 57 | 
 58 |         % update model
 59 |         x = rungekutta(@m_speed,    x, v_set, T);     % velocity model
 60 |         C=[ 0, 1];
 61 |         p = rungekutta(@m_position, p,  C*x, T);     % position model
 62 | 
 63 |         addpoints(H(i_Sim), t, p);
 64 |     %     H2.XData = 0;
 65 |     %     H2.YData = p;
 66 |         drawnow limitrate;
 67 |     end
 68 | end
 69 | 
 70 | 
 71 | 
 72 | function dx = m_speed(x, u)
 73 | A=[-1, 2; -2 -1];
 74 | B=[-2.5; 0];
 75 | C=[0, 1];
 76 | dx = A*x + B*u;
 77 | end
 78 | 
 79 | function dx = m_position(x, u)
 80 | A  = 0;
 81 | B  = 1;
 82 | dx = A*x + B*u;
 83 | end
 84 | 
 85 | function x = rungekutta(fun, x0, u, h)
 86 | % FcnHandlesUsed  = isa(fun,'function_handle');
 87 | k1 = fun(x0       , u);
 88 | k2 = fun(x0+h/2*k1, u);
 89 | k3 = fun(x0+h/2*k2, u);
 90 | k4 = fun(x0+  h*k3, u);
 91 | x = x0 + h/6*(k1 + 2*k2 + 2*k3 + k4);
 92 | end
 93 | 
 94 | 
 95 | function v_set= collision_avoidance(p, vp, Ds, alpha_v)
 96 | persistent opt;
 97 | if(isempty(opt))
 98 |     opt = optimoptions('quadprog',  'Algorithm','interior-point-convex','Display','off', 'MaxIterations', 1e3);
 99 | end
100 | 
101 | % generate conditions
102 | A = zeros(2, 1);
103 | A(1, :) = -(p-1)'/norm(p-1);
104 | A(2, :) = -(p+1)'/norm(p+1);
105 | % a(1, :) = alpha_v*(1/norm(p-1)-1/Ds);
106 | % a(2, :) = alpha_v*(1/norm(p+1)-1/Ds);
107 | a(1, :) = -alpha_v*(norm(p-1)-Ds);
108 | a(2, :) = -alpha_v*(norm(p+1)-Ds);
109 | b = -a;
110 | 
111 | % Quadratic programming
112 | % Standard Quadratic Programming with Slack Variables
113 | v_set = quadprog(...
114 |     1, -vp, ...                             % cost function
115 |     A,   b, [], [],  ...             % constraints
116 |     [], [], zeros(1, 1), opt);    % limits and setting
117 | 
118 | if(isempty(v_set))
119 |     1;
120 | end
121 | end


--------------------------------------------------------------------------------
/half_space_2D.m:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/EXPSIN/Quadratic-Programming-for-Continuous-Control-of-Safety-Critical-Multi-Agent-Systems-Under-Uncertaint/c20f6129b3b5190754c293ec1d164788bdcc6ef5/half_space_2D.m


--------------------------------------------------------------------------------