Figure 8.11:
Gauss-Newton iterations for the direct multiple-shooting method
Code for Figure 8.11
Text of the GNU GPL.
main.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 | % Solves the minimization problem with T=10
% minimize 1/2*integral{t=0 until t=T}(x1^2 + x2^2 + u^2) dt
% subject to dot(x1) = (1-x2^2)*x1 - x2 + u, x1(0)=0, x1(T)=0
% dot(x2) = x1, x2(0)=1, x2(T)=0
% x1(t) >= -0.25, 0<=t<=T
% Joel Andersson, UW Madison 2017
%
% States and control
x1 = casadi.SX.sym('x1');
x2 = casadi.SX.sym('x2');
u = casadi.SX.sym('u');
% Model equations
x1_dot = (1-x2^2)*x1 - x2 + u;
x2_dot = x1;
% Least squares objective terms
lsq =[x1; x2; u];
% Define the problem structure
ocp = struct('x',[x1; x2],'u',u, 'ode', [x1_dot; x2_dot], 'lsq',lsq);
% Specify problem data
data = struct('T', 10,...
'x0', [0;1],...
'xN', [ 0; 0],...
'x_min', [-0.25; -inf],...
'x_max', [ inf; inf],...
'x_guess', [0; 0],...
'u_min', -1,...
'u_max', 1,...
'u_guess', 0);
% Specify solver options
opts = struct('N', 20,...
'verbose', false);
% Create an OCP solver instance
s = dms_gn(ocp, data, opts);
% Display sparsities
figure();
subplot(1,2,1);
spy(sparse(casadi.DM(s.nlp.J.sparsity(), 1)), '.')
title('J sparsity pattern:')
subplot(1,2,2);
spy(sparse(casadi.DM(s.nlp.H.sparsity(), 1)), '.')
title('H sparsity pattern:')
% Initializing figure
figure;
clf;
hold on;
grid on;
% Plot solution
x1_plot = plot(s.t, s.sol.x(1,:), 'r--');
x2_plot = plot(s.t, s.sol.x(2,:), 'b-');
u_plot = stairs(s.t, [s.sol.u(1,:) nan], 'g-.');
xlabel('t')
legend('x1','x2','u')
pause(2);
iter=0;
while s.sol.norm_dw > 1e-8
% SQP iteration
s.sqpstep();
iter = iter + 1;
% Update plots
set(x1_plot, 'Ydata', s.sol.x(1, :));
set(x2_plot, 'Ydata', s.sol.x(2, :));
set(u_plot, 'Ydata', s.sol.u(1,:));
title(sprintf('Iteration %d, |dw| = %g', iter, s.sol.norm_dw))
pause(2);
end
|
dms_gn.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 | classdef dms_gn < handle
properties
% Symbolic representation of the OCP
ocp
% OCP data
data
% Solver options
opts
% Verbose output
verbose
% Dimensions
N
nx
nu
% Solution time grid
t
% Control interval length
dt
% CasADi function objects
fun
% Nonlinear program
nlp
% Current iteration
n_iter
% Current solution
sol
end
methods
function self = dms_gn(ocp, data, opts)
% Constructor
self.ocp = ocp;
self.data = data;
self.opts = opts;
% Verbosity?
if (isfield(opts, 'verbose'))
self.verbose = opts.verbose;
else
self.verbose = true;
end
% Problem dimensions
self.N = self.opts.N;
self.nx = numel(self.ocp.x);
self.nu = numel(self.ocp.u);
% Time grid
self.t = linspace(0, data.T, self.N+1);
% Interval length
self.dt = data.T / self.N;
% Get discrete-time dynamics
self.rk4();
% Transcribe to NLP
self.transcribe();
% Initialize x
self.sol.w = self.nlp.w0;
% Extract x,u from w
[x_traj,u_traj] = self.fun.traj(self.sol.w);
self.sol.x = full(x_traj);
self.sol.u = full(u_traj);
% Initialize Gauss-Newton method
self.init_gauss_newton();
end
function init_gauss_newton(self)
% Linearize the problem w.r.t. w
self.nlp.J = jacobian(self.nlp.g, self.nlp.w);
self.nlp.JM = jacobian(self.nlp.M, self.nlp.w);
% Gauss-Newton Hessian and gradient
self.nlp.H = self.nlp.JM' * self.nlp.JM;
self.nlp.c = self.nlp.JM' * self.nlp.M;
% Functions for calculating g, J, H
self.fun.g = casadi.Function('g', {self.nlp.w}, {self.nlp.g},...
{'w'}, {'g'});
self.fun.J = casadi.Function('J', {self.nlp.w}, {self.nlp.J},...
{'w'}, {'J'});
self.fun.H = casadi.Function('H', {self.nlp.w}, {self.nlp.H, self.nlp.c},...
{'w'}, {'H', 'c'});
% Out new step is determined by solving the quadratic program for
% dw = w_new-w
% minimize 1/2 dw'*H*dw + c'*dw
% subject to g + A*dw = 0
% lbw-w <= dw <= ubw-w
% qp = struct('h', self.nlp.H.sparsity(), 'a', self.nlp.J.sparsity());
qp.h = self.nlp.H.sparsity();
qp.a = self.nlp.J.sparsity();
qp_options = struct();
if ~self.verbose
qp_options.printLevel = 'none';
end
self.fun.qp_solver = casadi.conic('qp_solver', 'qpoases', qp, qp_options);
% Iteration counter
self.n_iter = 0;
% Residual
self.sol.norm_dw = inf;
end
function rk4(self)
% Continuous-time dynamics
x = self.ocp.x;
u = self.ocp.u;
ode = self.ocp.ode;
f = casadi.Function('f', {x, u}, {ode}, {'x','p'}, {'ode'});
% Implement RK4 integrator that takes a single step
k1 = f(x, u);
k2 = f(x+0.5*self.dt*k1, u);
k3 = f(x+0.5*self.dt*k2, u);
k4 = f(x+self.dt*k3, u);
xk = x+self.dt/6.0*(k1+2*k2+2*k3+k4);
% Return as a function
self.fun.F = casadi.Function('RK4', {x,u}, {xk}, {'x0','p'}, {'xf'});
% Least squares objective function
lsq = self.ocp.lsq;
self.fun.Lsq = casadi.Function('Lsq', {x, u}, {lsq}, {'x','p'}, {'lsq'});
end
function transcribe(self)
% Start with an empty NLP
w = {}; % Variables
g = {}; % Equality constraints
M = {}; % Least squares objective function
lbw = {}; % Lower bound on w
ubw = {}; % Upper bound on w
w0 = {}; % Initial guess for w
% Expressions corresponding to the trajectories we want to plot
x_plot = {};
u_plot = {};
% Initial conditions
xk = casadi.MX.sym('x0', self.nx);
w{end+1} = xk;
lbw{end+1} = self.data.x0;
ubw{end+1} = self.data.x0;
w0{end+1} = self.data.x_guess;
x_plot{end+1} = xk;
% Loop over all times
for k=0:self.N-1
% Define local control
uk = casadi.MX.sym(['u' num2str(k)], self.nu);
w{end+1} = uk;
lbw{end+1} = self.data.u_min;
ubw{end+1} = self.data.u_max;
w0{end+1} = self.data.u_guess;
u_plot{end+1} = uk;
% Simulate the system forward in time
Fk = self.fun.F('x0', xk, 'p', uk);
x_next = Fk.xf;
% Add least squares term to the objective
M{end+1} = self.fun.Lsq(xk, uk);
% Define state at the end of the interval
xk = casadi.MX.sym(['x' num2str(k+1)], self.nx);
w{end+1} = xk;
if k==self.N-1
lbw{end+1} = self.data.xN;
ubw{end+1} = self.data.xN;
else
lbw{end+1} = self.data.x_min;
ubw{end+1} = self.data.x_max;
end
w0{end+1} = self.data.x_guess;
x_plot{end+1} = xk;
% Impose continuity
g{end+1} = xk - x_next;
end
% Concatenate variables and constraints
self.nlp.w = vertcat(w{:});
self.nlp.g = vertcat(g{:});
self.nlp.M = vertcat(M{:});
self.nlp.lbw = vertcat(lbw{:});
self.nlp.ubw = vertcat(ubw{:});
self.nlp.w0 = vertcat(w0{:});
% Create a function that maps the NLP decision variable to the x and u trajectories
self.fun.traj = casadi.Function('traj', {self.nlp.w}, {horzcat(x_plot{:}), horzcat(u_plot{:})},...
{'w'}, {'x', 'u'});
end
function sqpstep(self)
% Update iteration counter
self.n_iter = self.n_iter + 1;
% Calculate the QP matrices
self.sol.g = self.fun.g(self.sol.w);
self.sol.J = self.fun.J(self.sol.w);
[self.sol.H, self.sol.c] = self.fun.H(self.sol.w);
% Solve the QP to get the step in in w
qp_solution = self.fun.qp_solver('a', self.sol.J, 'h', self.sol.H, 'g', self.sol.c,...
'lbx', self.nlp.lbw-self.sol.w,...
'ubx', self.nlp.ubw-self.sol.w,...
'lba', -self.sol.g, 'uba', -self.sol.g, 'x0', 0);
dw = full(qp_solution.x);
% Check convergence criteria
self.sol.norm_dw = norm(dw);
% Take (full) step
self.sol.w = self.sol.w + dw;
% Extract x,u from w
[x_traj,u_traj] = self.fun.traj(self.sol.w);
self.sol.x = full(x_traj);
self.sol.u = full(u_traj);
% Print progress
if self.verbose || mod(self.n_iter,10)==1
disp(repmat('-', 1, 70))
fprintf('%15s %15s\n', 'SQP iteration', 'norm(dw)');
end
fprintf('%15d %15g\n', self.n_iter, self.sol.norm_dw);
end
end
end
|