forked from HybridRobotics/MPC-CBF
-
Notifications
You must be signed in to change notification settings - Fork 0
/
MPCDC.m
144 lines (140 loc) · 5.16 KB
/
MPCDC.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
classdef MPCDC < handle
% MPC with distance constraints
properties
system
params
x0
x_curr
time_curr = 0.0
xlog = []
ulog = []
distlog = []
solvertime = []
xopenloop = {}
uopenloop = {}
u_cost = 0
obs
end
methods
function self = MPCDC(x0, system, params)
% Define MPC_DC controller
self.x0 = x0;
self.x_curr = x0;
self.system = system;
self.params = params;
end
function sim(self, time)
% Simulate the system until a given time
xk = self.x_curr;
while self.time_curr <= time
% Solve CFTOC
[~, uk] = self.solveMPCDC(self.x_curr);
xk = self.system.A * xk + self.system.B * uk;
% update system
self.x_curr = xk;
self.time_curr = self.time_curr + self.system.dt;
self.xlog = [self.xlog, xk];
self.ulog = [self.ulog, uk];
self.u_cost = self.u_cost + uk'*uk*self.system.dt;
end
end
function [xopt, uopt] = solveMPCDC(self, xk)
% Solve MPC-DC
[feas, x, u, J] = self.solve_cftoc(xk);
if ~feas
xopt = [];
uopt = [];
return
else
xopt = x(:,2);
uopt = u(:,1);
end
end
function [feas, xopt, uopt, Jopt] = solve_cftoc(self, xk)
% Solve CFTOC
% extract variables
N = self.params.N;
% define variables and cost
x = sdpvar(4, N+1);
u = sdpvar(2, N);
constraints = [];
cost = 0;
% initial constraint
constraints = [constraints; x(:,1) == xk];
% add constraints and costs
for i = 1:N
constraints = [constraints;
self.system.xl <= x(:,i) <= self.system.xu;
self.system.ul <= u(:,i) <= self.system.uu
x(:,i+1) == self.system.A * x(:,i) + self.system.B * u(:,i)];
cost = cost + x(:,i)'*self.params.Q*x(:,i) + u(:,i)'*self.params.R*u(:,i);
end
% add CBF constraints
for i = 1:N
pos = self.obs.pos;
r = self.obs.r ;
b = (x([1:2],i)-pos)'*((x([1:2],i)-pos)) - r^2;
constraints = [constraints; b >= 0];
end
% add terminal cost
cost = cost + x(:,N+1)'*self.params.P*x(:,N+1);
ops = sdpsettings('solver','ipopt','verbose',0);
% solve optimization
diagnostics = optimize(constraints, cost, ops);
if diagnostics.problem == 0
feas = true;
xopt = value(x);
uopt = value(u);
Jopt = value(cost);
else
feas = false;
xopt = [];
uopt = [];
Jopt = [];
end
self.distlog = [self.distlog, sqrt((xk(1:2)-pos)'*(xk(1:2)-pos)-r^2)];
self.xopenloop{size(self.xopenloop,2)+1} = xopt;
self.uopenloop{size(self.uopenloop,2)+1} = uopt;
self.solvertime = [self.solvertime, diagnostics.solvertime];
fprintf('solver time: %f\n', diagnostics.solvertime);
end
function plot(self, figure_name)
% Plot simulation
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on;
% plot closed-loop trajectory
plot(self.xlog(1,:), self.xlog(2,:), 'ko-',...
'LineWidth', 1.0, 'MarkerSize', 4);
% plot open-loop trajectory
for i = 1:size(self.xopenloop, 2)
plot(self.xopenloop{i}(1,:), self.xopenloop{i}(2,:),...
'k*-.', 'LineWidth', 0.5,'MarkerSize',0.5)
end
% plot obstacle
pos = self.obs.pos;
r = self.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(self.x0(1), self.x0(2), 'db', 'LineWidth', 1);
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
h=get(gca,'Children');
h_legend = legend(h([end]), {'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:0);
yticks(-5:0);
xlim([-5,0.2]);
ylim([-5,0.2]);
print(gcf,strcat('figures/',figure_name), '-depsc');
end
end
end