222 lines
6.7 KiB
Matlab
222 lines
6.7 KiB
Matlab
clear stages params outputs codeoptions
|
|
|
|
%% Generator for a FORCES QP solver used by the ACADO OCP solver
|
|
|
|
%
|
|
% User options
|
|
%
|
|
|
|
% Number of states
|
|
nx = @NX@;
|
|
% Number of controls
|
|
nu = @NU@;
|
|
% Number of discretization (shooting) nodes
|
|
N = @N@;
|
|
|
|
% Lower and upper bounds on stage variables, for all N stages
|
|
lbIdx = { ...
|
|
@LB_IDX@ ...
|
|
};
|
|
ubIdx = { ...
|
|
@UB_IDX@ ...
|
|
};
|
|
|
|
% Polytopic constraints (A_i*z_i <= b_i) on stage variables, for all N stages
|
|
AbDim = { ...
|
|
@AB_DIM@ ...
|
|
};
|
|
|
|
% Constant Hessian
|
|
constHessian = @CONST_HESSIAN@;
|
|
% Diagonal Hessian
|
|
diagHessian = @DIAG_HESSIAN@;
|
|
diagHessianN = @DIAG_HESSIAN_N@;
|
|
% Fixed initial state
|
|
fixedInitialState = @FIXED_INITIAL_STATE@;
|
|
|
|
%% Define FORCES multistage problem
|
|
|
|
stages = MultistageProblem( N );
|
|
|
|
for i = 1: N
|
|
if (i == 1)
|
|
|
|
%
|
|
% Initial stage
|
|
%
|
|
|
|
% Dimension
|
|
stages(i).dims.n = nx + nu; % number of stage variables
|
|
stages(i).dims.r = nx; % number of equality constraints
|
|
stages(i).dims.l = length( lbIdx{ i } ); % number of lower bounds
|
|
stages(i).dims.u = length( ubIdx{ i } ); % number of upper bounds
|
|
stages(i).dims.p = AbDim{ i }; % number of polytopic constraints
|
|
stages(i).dims.q = 0; % number of quadratic constraints
|
|
|
|
% Cost
|
|
if (constHessian == 1)
|
|
if (diagHessian == 1)
|
|
params( 1 ) = newParam('H1', 1: 1: N - 1, 'cost.H', 'diag');
|
|
else
|
|
params( 1 ) = newParam('H1', 1: 1: N - 1, 'cost.H');
|
|
end;
|
|
else
|
|
if (diagHessian == 1)
|
|
params( 1 ) = newParam('H1', 1, 'cost.H', 'diag');
|
|
else
|
|
params( 1 ) = newParam('H1', 1, 'cost.H');
|
|
end;
|
|
end;
|
|
params(end + 1) = newParam('f1', 1, 'cost.f');
|
|
|
|
% Lower bounds
|
|
stages(i).ineq.b.lbidx = cell2mat( lbIdx{ i } );
|
|
if (~isempty( lbIdx{ i } ))
|
|
params(end + 1) = newParam(sprintf('%s%d', 'lb', i), i, 'ineq.b.lb');
|
|
end;
|
|
|
|
% Upper bounds
|
|
stages(i).ineq.b.ubidx = cell2mat( ubIdx{ i } );
|
|
if (~isempty( ubIdx{ i } ))
|
|
params(end + 1) = newParam(sprintf('%s%d', 'ub', i), i, 'ineq.b.ub');
|
|
end;
|
|
|
|
% Polytopic constraints
|
|
% stages(i).ineq.p.bidx = cell2mat( AbIdx{ i } );
|
|
if (AbDim{ i } ~= 0)
|
|
params(end + 1) = newParam(sprintf('%s%d', 'Ab', i), i, 'ineq.p.b');
|
|
params(end + 1) = newParam(sprintf('%s%d', 'A', i), i, 'ineq.p.A');
|
|
end
|
|
|
|
% equality constraints
|
|
params(end + 1) = newParam('C1', 1, 'eq.C');
|
|
params(end + 1) = newParam('d1', 1, 'eq.c');
|
|
|
|
if (fixedInitialState == 1)
|
|
stages(i).eq.D = [eye(nx), zeros(nx,nu)];
|
|
end;
|
|
|
|
elseif (i < N)
|
|
|
|
%
|
|
% Stages along horizon
|
|
%
|
|
|
|
% Dimension
|
|
stages(i).dims.n = nx + nu; % number of stage variables
|
|
stages(i).dims.r = nx; % number of equality constraints
|
|
stages(i).dims.l = length( lbIdx{ i } ); % number of lower bounds
|
|
stages(i).dims.u = length( ubIdx{ i } ); % number of upper bounds
|
|
stages(i).dims.p = AbDim{ i }; % number of polytopic constraints
|
|
stages(i).dims.q = 0; % number of quadratic constraints
|
|
|
|
% Cost
|
|
if (constHessian == 0)
|
|
if (diagHessian == 1)
|
|
params(end + 1) = newParam(sprintf('%s%d', 'H', i), i, 'cost.H', 'diag');
|
|
else
|
|
params(end + 1) = newParam(sprintf('%s%d', 'H', i), i, 'cost.H');
|
|
end;
|
|
end;
|
|
params(end + 1) = newParam(sprintf('%s%d', 'f', i), i, 'cost.f');
|
|
|
|
% Lower bounds
|
|
stages(i).ineq.b.lbidx = cell2mat( lbIdx{ i } );
|
|
if (~isempty( lbIdx{ i } ))
|
|
params(end + 1) = newParam(sprintf('%s%d', 'lb', i), i, 'ineq.b.lb');
|
|
end;
|
|
|
|
% Upper bounds
|
|
stages(i).ineq.b.ubidx = cell2mat( ubIdx{ i } );
|
|
if (~isempty( ubIdx{ i } ))
|
|
params(end + 1) = newParam(sprintf('%s%d', 'ub', i), i, 'ineq.b.ub');
|
|
end;
|
|
|
|
% Polytopic constraints
|
|
% stages(i).ineq.p.bidx = cell2mat( AbIdx{ i } );
|
|
if (AbDim{ i } ~= 0)
|
|
params(end + 1) = newParam(sprintf('%s%d', 'Ab', i), i, 'ineq.p.b');
|
|
params(end + 1) = newParam(sprintf('%s%d', 'A', i), i, 'ineq.p.A');
|
|
end
|
|
|
|
% Equality constraints
|
|
params(end + 1) = newParam(sprintf('%s%d', 'C', i), i, 'eq.C');
|
|
params(end + 1) = newParam(sprintf('%s%d', 'd', i), i, 'eq.c');
|
|
|
|
stages(i).eq.D = [-eye( nx ), zeros(nx, nu)];
|
|
|
|
else
|
|
|
|
%
|
|
% Final stage
|
|
%
|
|
|
|
% Dimension
|
|
stages(i).dims.n = nx; % number of stage variables
|
|
|
|
if (fixedInitialState == 1)
|
|
stages(i).dims.r = nx; % number of equality constraints
|
|
else
|
|
stages(i).dims.r = 0;
|
|
end;
|
|
|
|
stages(i).dims.l = length( lbIdx{ i } ); % number of lower bounds
|
|
stages(i).dims.u = length( ubIdx{ i } ); % number of upper bounds
|
|
stages(i).dims.p = AbDim{ i }; % number of polytopic constraints
|
|
stages(i).dims.q = 0; % number of quadratic constraints
|
|
|
|
% Cost
|
|
if (diagHessianN == 1)
|
|
params(end + 1) = newParam(sprintf('%s%d', 'H', i), i, 'cost.H', 'diag');
|
|
else
|
|
params(end + 1) = newParam(sprintf('%s%d', 'H', i), i, 'cost.H');
|
|
end;
|
|
params(end + 1) = newParam(sprintf('%s%d', 'f', i), i, 'cost.f');
|
|
|
|
% Lower bounds
|
|
stages(i).ineq.b.lbidx = cell2mat( lbIdx{ i } );
|
|
if (~isempty( lbIdx{ i } ))
|
|
params(end + 1) = newParam(sprintf('%s%d', 'lb', i), i, 'ineq.b.lb');
|
|
end;
|
|
|
|
% Upper bounds
|
|
stages(i).ineq.b.ubidx = cell2mat( ubIdx{ i } );
|
|
if (~isempty( ubIdx{ i } ))
|
|
params(end + 1) = newParam(sprintf('%s%d', 'ub', i), i, 'ineq.b.ub');
|
|
end;
|
|
|
|
% Polytopic constraints
|
|
% stages(i).ineq.p.bidx = cell2mat( AbIdx{ i } );
|
|
if (AbDim{ i } ~= 0)
|
|
params(end + 1) = newParam(sprintf('%s%d', 'Ab', i), i, 'ineq.p.b');
|
|
params(end + 1) = newParam(sprintf('%s%d', 'A', i), i, 'ineq.p.A');
|
|
end
|
|
|
|
% Equality constraints
|
|
stages(i).eq.D = -eye(nx);
|
|
|
|
if (fixedInitialState == 1)
|
|
params(end + 1) = newParam(sprintf('%s%d', 'd', i), i, 'eq.c');
|
|
end;
|
|
|
|
end;
|
|
end;
|
|
|
|
%% Define outputs of the solver
|
|
for i = 1: N
|
|
outputs( i ) = newOutput(sprintf('%s%d', 'out', i), i, 1: 1: stages(i).dims.n);
|
|
end;
|
|
|
|
%% Solver settings
|
|
codeoptions = getOptions('@SOLVER_NAME@');
|
|
|
|
codeoptions.overwrite = 1; % Always overwrite the existing code
|
|
codeoptions.printlevel = @PRINT_LEVEL@;
|
|
codeoptions.maxit = @MAX_ITERATIONS@;
|
|
codeoptions.parallel = @PARALLEL@;
|
|
codeoptions.init = @WARM_START@;
|
|
|
|
|
|
%% Generate code
|
|
generateCode(stages, params, codeoptions, outputs);
|