CBF-Pointwise-Feasibility/AdaptiveCruiseControl/testSafety.m at master · HybridRobotics/CBF-Pointwise-Feasibility · GitHub

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
close all; clear all; clc;
%% ACC parameters
param = AdaptiveCruiseControlParam();

%% Create different initial conditions
hyperparameter_list = [
    [1.0, 10^2];...
    [1.0, 10^4];...
    ];

%% Simulate system with different control policies
data_acc_system_soft_decay_clf_cbf_qp = {};
for i = 1:size(hyperparameter_list, 1)
    % simulate system with Soft-decay CLF-CBF-QP
    acc_system = AdaptiveCruiseControl([0; 32; 100]);
    % different hyperparameters
    new_param = param; % deep copy
    new_param.omega0 = hyperparameter_list(i,1);
    new_param.p_sb = hyperparameter_list(i,2);
    fprintf('simulate system under Soft-decay CLF-CBF-QP with omega0 as %f and pomega as %f\n', [new_param.omega0, new_param.p_sb]);
    acc_system.param = new_param;
    acc_system.method_flag = true;
    acc_system.dt = 0.01;
    acc_system.sim(12);
    data_acc_system_soft_decay_clf_cbf_qp{i} = acc_system;
end

%% Compare performance
color_band = [[0, 0.4470, 0.7410]
    [0.8500, 0.3250, 0.0980];
    [0.9290, 0.6940, 0.1250];
    [0.4940, 0.1840, 0.5560];
    [0.4660, 0.6740, 0.1880];
    [0.6350, 0.0780, 0.1840];
    ];

%% velocity plotting
figure('Renderer', 'painters', 'Position', [0 0 600 600]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on
grid on
for i = 1:size(hyperparameter_list, 1)
    acc_system = data_acc_system_soft_decay_clf_cbf_qp{i};
    plot(acc_system.time_log, acc_system.x_log(2,:), 'Color', color_band(i,:), 'LineWidth', 3.0);
end
ax = gca;
ax.XTick = [0:3:15];
ax.YTick = [16:2:32];
set(gca,'LineWidth', 0.2, 'FontSize', 25);
xlabel('Time (s)','interpreter','latex','FontSize', 25);
ylabel('$x_2(t)$ (m/s)','interpreter','latex','FontSize', 25);
print(gcf,'figures/safety/benchmark-safety-velocity.png', '-dpng', '-r1000');
print(gcf,'figures/safety/benchmark-safety-velocity.eps', '-depsc');

%% control input plotting
figure('Renderer', 'painters', 'Position', [0 0 600 600]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on
grid on
for i = 1:size(hyperparameter_list, 1)
    acc_system = data_acc_system_soft_decay_clf_cbf_qp{i};
    plot(acc_system.time_log, acc_system.u_log/1e3, 'Color', color_band(i,:), 'LineWidth', 3.0);
end
% plot input lower bound
plot(acc_system.time_log, -param.c_a * param.m * param.g .* ones(size(acc_system.time_log,2))/1e3, 'k--', 'LineWidth', 1.5);
ax = gca;
ax.XTick = [0:3:15];
ax.YTick = [-5:1:5];
set(gca,'LineWidth', 0.2, 'FontSize', 25);
xlabel('Time (s)','interpreter','latex','FontSize', 25);
ylabel('$\mathbf{u}(t)$ (kN)','interpreter','latex','FontSize', 25);
print(gcf,'figures/safety/benchmark-safety-control-input.png', '-dpng', '-r1000');
print(gcf,'figures/safety/benchmark-safety-control-input.eps', '-depsc', '-opengl', '-r600');

%% cbf plotting
figure('Renderer', 'painters', 'Position', [0 0 600 600]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on
grid on
for i = 1:size(hyperparameter_list, 1)
    acc_system = data_acc_system_soft_decay_clf_cbf_qp{i};
    plot(acc_system.time_log, acc_system.cbf_log, 'Color', color_band(i,:), 'LineWidth', 3.0);
end
plot(acc_system.time_log, zeros(1, size(acc_system.time_log, 2)), 'k--', 'LineWidth', 1.5);
ax = gca;
ax.XTick = [0:3:15];
ax.YTick = [0:20:60];
ax.GridAlpha = 0.5;
ax.GridLineStyle = '--';
set(gca,'LineWidth', 0.2, 'FontSize', 25);
xlabel('Time (s)','interpreter','latex','FontSize', 25);
ylabel('$h(t)$','interpreter','latex','FontSize', 25);
h=get(gca,'Children');
h_legend = legend(h([end, end-1]),...
    {'$\omega_0=1.0, p_{\omega} = 10^2$',...
    '$\omega_0=1.0, p_{\omega} = 10^4$'}, 'Location', 'NorthEast');
h_legend.FontSize = 20;
set(h_legend, 'Interpreter','latex');
print(gcf,'figures/safety/benchmark-safety-cbf.png', '-dpng', '-r1000');
print(gcf,'figures/safety/benchmark-safety-cbf.eps', '-depsc');

%% omega plotting
figure('Renderer', 'painters', 'Position', [0 0 600 600]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on
grid on
for i = 1:size(hyperparameter_list, 1)
    acc_system = data_acc_system_soft_decay_clf_cbf_qp{i};
    plot(acc_system.time_log, acc_system.omega_log, 'Color', color_band(i,:), 'LineWidth', 3.0);
end
ax = gca;
ax.XTick = [0:3:15];
% ax.YTick = [1:0.3:1.5];
set(gca,'LineWidth', 0.2, 'FontSize', 25);
xlabel('Time (s)','interpreter','latex','FontSize', 25);
ylabel('$\omega(t)$','interpreter','latex','FontSize', 25);
print(gcf,'figures/safety/benchmark-safety-omega.png', '-dpng', '-r1000');
print(gcf,'figures/safety/benchmark-safety-omega.eps', '-depsc');