forked from HybridRobotics/MPC-CBF
-
Notifications
You must be signed in to change notification settings - Fork 0
/
testGamma.m
95 lines (88 loc) · 2.55 KB
/
testGamma.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
close all
clear
%% Setup and Parameters
x0 = [-5; -5; 0; 0];
time_total = 20.0;
dt = 0.2;
P = 100*eye(4);
Q = 10*eye(4);
R = eye(2);
N = 8;
xmin = [-5; -5; -5; -5];
xmax = [5; 5; 5; 5];
umin = [-1; -1];
umax = [1; 1];
%% Discrete-time double integrator 2D
system.dt = dt;
system.A = [1 0 dt 0;
0 1 0 dt;
0 0 1 0;
0 0 0 1];
system.B = [0.5*dt^2 0;
0 0.5*dt^2;
dt 0;
0 dt];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% MPC-CBF parameters
params_mpc_cbf.Q = Q;
params_mpc_cbf.R = R;
params_mpc_cbf.P = P;
params_mpc_cbf.N = N;
params_mpc_cbf.gamma = 0.5;
%% Obstacle
obs.pos = [-2; -2.25];
obs.r = 1.5;
%% Simulate MPC-CBF
gamma_list = [0.1, 0.2, 0.3, 1.0];
for ind = 1:size(gamma_list, 2)
fprintf('Run MPC-CBF with gamma %f\n', gamma_list(ind));
params_mpc_cbf.gamma = gamma_list(ind);
controller_mpc_cbf_list{ind} = MPCCBF(x0, system, params_mpc_cbf);
controller_mpc_cbf_list{ind}.obs = obs;
controller_mpc_cbf_list{ind}.sim(time_total);
end
%% Simulate MPC-DC
params_mpc_dc = params_mpc_cbf;
controller_mpc_dc = MPCDC(x0, system, params_mpc_cbf);
controller_mpc_dc.obs = obs;
controller_mpc_dc.sim(time_total);
%% Visualization benchmark
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on
for ind = 1:size(gamma_list, 2)
plot(controller_mpc_cbf_list{ind}.xlog(1,:), controller_mpc_cbf_list{ind}.xlog(2,:),...
'-', 'LineWidth', 1.0, 'MarkerSize', 4);
end
plot(controller_mpc_dc.xlog(1,:), controller_mpc_dc.xlog(2,:),...
'k--', 'LineWidth', 1.0, 'MarkerSize', 4);
% plot obstacle
pos = obs.pos;
r = obs.r;
th = linspace(0,2*pi*100);
x = cos(th) ; y = sin(th) ;
plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980],...
'LineWidth', 2);
plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980],...
'MarkerSize', 5, 'LineWidth', 2);
% plot target position
plot(x0(1), x0(2), 'db', 'LineWidth', 1);
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
h=get(gca,'Children');
h_legend = legend(h([end, end-1, end-2, end-3, end-4]),...
{'MPC-CBF ($\gamma=0.1$)', 'MPC-CBF ($\gamma=0.2$)',...
'MPC-CBF ($\gamma=0.3$)', 'MPC-CBF ($\gamma=1.0$)', 'MPC-DC'}, 'Location', 'SouthEast');
set(h_legend, 'Interpreter','latex');
set(gca,'LineWidth', 0.2, 'FontSize', 15);
grid on
xlabel('$x (m)$','interpreter','latex','FontSize',20);
ylabel('$y (m)$','interpreter','latex','FontSize',20);
xticks(-5:1);
yticks(-5:1);
xlim([-5,0.2]);
ylim([-5,0.2]);
print(gcf,'figures/benchmark-gamma.eps', '-depsc');
print(gcf,'figures/benchmark-gamma.png', '-dpng', '-r800');