Skip to content

Instantly share code, notes, and snippets.

@abe-mart
Created Mar 31, 2020
Embed
What would you like to do?
%% Control 2D single integrator
clear all
%% discretization
N = 10;
T = 1; % time horizon length
x0 = [0; 0];
nlp_solver = 'sqp'; % sqp, sqp_rti
qp_solver = 'partial_condensing_hpipm';
qp_solver_cond_N = 5; % for partial condensing
% integrator type
sim_method = 'irk'; % erk, irk, irk_gnsf
%% model dynamics
model = model_SI_2D;
nx = model.nx;
nu = model.nu;
%% model to create the solver
ocp_model = acados_ocp_model();
model_name = 'SI_2D';
%% acados ocp model
ocp_model.set('name', model_name);
ocp_model.set('T', T);
% symbolics
ocp_model.set('sym_x', model.sym_x);
ocp_model.set('sym_u', model.sym_u);
ocp_model.set('sym_xdot', model.sym_xdot);
% cost
ocp_model.set('cost_type', 'auto')
ocp_model.set('cost_type_e', 'auto')
ocp_model.set('cost_expr_ext_cost', model.expr_ext_cost);
ocp_model.set('cost_expr_ext_cost_e', model.expr_ext_cost_e);
% dynamics
if (strcmp(sim_method, 'erk'))
ocp_model.set('dyn_type', 'explicit');
ocp_model.set('dyn_expr_f', model.expr_f_expl);
else % irk irk_gnsf
ocp_model.set('dyn_type', 'implicit');
ocp_model.set('dyn_expr_f', model.expr_f_impl);
end
% constraints
ocp_model.set('constr_type', 'auto');
ocp_model.set('constr_expr_h', model.expr_h);
ocp_model.set('constr_lh', 0); % lower bound on h
ocp_model.set('constr_uh', 1); % upper bound on h
ocp_model.set('constr_x0', x0);
%% acados ocp set opts
ocp_opts = acados_ocp_opts();
ocp_opts.set('param_scheme_N', N);
ocp_opts.set('nlp_solver', nlp_solver);
ocp_opts.set('sim_method', sim_method);
ocp_opts.set('qp_solver', qp_solver);
ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N);
% ... see ocp_opts.opts_struct to see what other fields can be set
%% create ocp solver
ocp = acados_ocp(ocp_model, ocp_opts);
x_traj_init = zeros(nx, N+1);
u_traj_init = zeros(nu, N);
%% call ocp solver
% update initial state
ocp.set('constr_x0', x0);
% set trajectory initialization
ocp.set('init_x', x_traj_init);
ocp.set('init_u', u_traj_init);
ocp.set('init_pi', zeros(nx, N))
% solve
ocp.solve();
% get solution
utraj = ocp.get('u');
xtraj = ocp.get('x');
status = ocp.get('status'); % 0 - success
ocp.print('stat')
%% Plots
figure; hold on;
States = {'x','y'};
for i=1:length(States)
subplot( length(States), 1, i);
plot(0:10,xtraj(i,:)); grid on;
legend(States{i});
end
figure
stairs(utraj')
grid on
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment