Hi there,
I have set up in Matlab an control example and have the problem that I get a solution that is not fulfilling a constraints of an expression (B in the code) on the first shooting step.But the status is 0, so the simulation is converging.
I am not sure if the error lies in setting the parameter p, which is indirectly used in the dynamic state equation. I understood that a parameter is set at the nodes, but I am using it in dynamic equation on stage level. Perhaps there lies my problem.
I hope with the code snippet below somebody of you can give me a hint where my implementation is faulty.
Thanks,
Andreas
% Acados example for testing constraint behavior
clear all, close all;
import casadi.*
% Input Data
%p_input = [-0.323143 -0.31754875 -0.3147516 -0.3147516 -0.3147516 -0.184362059791171 -0.0302246820201596 -0.0258752213670949 -0.0775569526658236 -0.114436025660026 -0.0912961184128824];
p_input = [-0.323143 -0.31754875 -0.3147516 -0.3147516 -0.3147516 -0.184362059791171 -0.0302246820201596 -0.0258752213670949 -0.0775569526658236 -0.114436025660026];
x_start = 0.6;
x_end = 0.6098;
%% Solver parameters
compile_interface = 'auto'; % auto, true, false
codgen_model = 'true'; % false | true
nlp_solver = 'sqp'; % sqp, sqp_rti
qp_solver = 'partial_condensing_hpipm';
% full_condensing_hpipm, partial_condensing_hpipm, full_condensing_qpoases
nlp_solver_exact_hessian = 'false'; % false=gauss_newton, true=exact
qp_solver_cond_N = 5; % for partial condensing
%regularize_method = 'no_regularize';
%regularize_method = 'project';
%regularize_method = 'mirror';
regularize_method = 'convexify';
% integrator type
sim_method = 'erk'; % erk, irk, irk_gnsf
%% horizon parameters
N = 10; %100;
T = 10; % time horizon length
% states
X = MX.sym('X');
x = vertcat(X);
% controls
U1 = MX.sym('U1');
U2 = MX.sym('U2');
u = vertcat(U1, U2);
% xdot (change of state)
xdot = MX.sym('xdot');
% Parameter input
P1 = MX.sym('P1');
p = vertcat(P1);
% dynamic equation
B = P1 - U2 - U1;
xdota = - B / 293.04;
f_expl = xdota;
%%%%%%%%%%%%%%%%%%%%%%%%%%
% state bounds
X_min = 0.3;
X_max = 0.7;
% Control bounds
U1_min = 0.01;
U1_max = 0.3;
U2_min = -1;
U2_max = 0;
% nonlinear constraint
B_min = -0.3;
B_max = 0.5;
%%%%%%%%%%%%%%%%%%%%%%%%%%%
nx = length(x);
nu = length(u);
%% model to create the solver
ocp_model = acados_ocp_model();
%% acados ocp model
ocp_model.set('name', 'Test');
ocp_model.set('T', T);
% symbolics
ocp_model.set('sym_x', x);
ocp_model.set('sym_u', u);
ocp_model.set('sym_p', p);
ocp_model.set('sym_xdot', xdot);
% dynamics
ocp_model.set('dyn_type', 'explicit');
ocp_model.set('dyn_expr_f', f_expl);
% State Constraints
Jbx(1,1) = 1;
ocp_model.set('constr_Jbx', Jbx);
ocp_model.set('constr_lbx', X_min);
ocp_model.set('constr_ubx', X_max);
% Final State Constraints
Jbx_e(1,1) = 1;
ocp_model.set('constr_Jbx_e', Jbx_e);
ocp_model.set('constr_lbx_e', [X_min]);
ocp_model.set('constr_ubx_e', [X_max]);
% Control Constraints
Jbu = zeros(2,2);
Jbu(1,1) = 1;
Jbu(2,2) = 1;
ocp_model.set('constr_Jbu', Jbu);
ocp_model.set('constr_lbu', [U1_min, U2_min]);
ocp_model.set('constr_ubu', [U1_max, U2_max]);
%Constraint expression bounds
ocp_model.set('constr_expr_h', B);
ocp_model.set('constr_lh', [B_min]);
ocp_model.set('constr_uh', [B_max]);
% set intial condition
ocp_model.set('constr_x0', x_start);
% cost
ocp_model.set('cost_type', 'ext_cost');
ocp_model.set('cost_type_e', 'ext_cost');
% Casadi stage cost
% Cost function
a0 = 3.872966;
a1 = 1.448040;
a2 = 0.004095;
expr_ext_cost = 0.5 .* a0 .* u(1) .* u(1) + a1 .* u(1) + 2 * a2;
ocp_model.set('cost_ext_fun_type', 'casadi');
ocp_model.set('cost_expr_ext_cost', expr_ext_cost);
% Casadi terminal cost
expr_ext_cost_e = 0;
ocp_model.set('cost_ext_fun_type_e', 'casadi');
ocp_model.set('cost_expr_ext_cost_e', expr_ext_cost_e);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% acados ocp set opts
ocp_opts = acados_ocp_opts();
%ocp_opts.set('compile_interface', compile_interface);
%ocp_opts.set('codgen_model', codgen_model);
ocp_opts.set('param_scheme_N', N);
ocp_opts.set('nlp_solver', nlp_solver);
ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian);
% PARTIAL_CONDENSING_HPIPM
ocp_opts.set('sim_method', sim_method);
ocp_opts.set('sim_method_num_stages', 4);
ocp_opts.set('sim_method_num_steps', 3);
ocp_opts.set('qp_solver', qp_solver);
%ocp_opts.set('regularize_method', regularize_method);
ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N);
ocp_opts.set('nlp_solver_tol_stat', 1e-4);
ocp_opts.set('nlp_solver_tol_eq', 1e-4);
ocp_opts.set('nlp_solver_tol_ineq', 1e-4); % 1e-4
ocp_opts.set('nlp_solver_tol_comp', 1e-4);
% ... see ocp_opts.opts_struct to see what other fields can be set
%% create ocp solver
ocp = acados_ocp(ocp_model, ocp_opts);
% Set end state
ocp.set('constr_lbx', x_end, N);
% set paramater
for k=1:N
ocp.set('p', p_input(k),k-1);
end
% set trajectory initialization
U1_init = smoothdata(p_input,"movmedian",10);
U1_min_init = U1_min + 0.03;
U2_init = min(max(U1_min_init, U1_init), U1_max);
U2_init = min(p_input(1:end) - B_min, 0);
ocp.set('init_x', x_start' * ones(1,N+1));
ocp.set('init_u', [U1_init, U2_init]);
% Solve
t_tic = tic();
ocp.solve();
elapsed = toc(t_tic);
status = ocp.get('status');
sqp_iter = ocp.get('sqp_iter');
time_tot = ocp.get('time_tot');
time_lin = ocp.get('time_lin');
time_qp_sol = ocp.get('time_qp_sol');
fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n',...
status, sqp_iter, elapsed*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3);
for k = 1:N
x_res(k,:) = ocp.get('x', k-1);
u_res(k,:) = ocp.get('u', k-1);
end
x_res(k+1,:) = ocp.get('x', k);
% Verification that limits are kept (B_res at fist shooting interval < B_min = 0.3)
B_res = p_input' - u_res(:,1) - u_res(:,2);