Simulink OCP-solver S-function infeasible but Matlab equivalent OCP-solver feasible

Hi :wave:

I am working on a NMPC model in ACADOS, via the MATLAB interface. I have created a model that runs smoothly in MATLAB. Now I want to export it to Simulink, but the performance there is very different. I created an S-function (ocp solver) from my MATLAB based model following the examples of simulink_example_advanced.m in the getting started folder. I used repmat to duplicate my inputs enough to fit the S-function input size requirements (as they are they required for the full horizon).

The Simulink based model returns different results over the full timeline (which is not necessarily problematic as long as they are ‘good enough’ as well), but more importantly becomes infeasible at a certain point in my simulation. This comparison is shown in the graph below (both models were evaluated with the same stopping criterion, all default except: nlp_solver_tol_stat 5e-3):

Solver_status_100it_comparison

If I were to make a guess at what is causing this, I would start with looking at the differences between the simulink based solver and the Matlab one. The only real difference between them is the fact that there is one option that I can not use in the Simulink model that I do have in my Matlab model. I use this option to progress my initial state in my next MPC evaluation, this is the option (it is placed within my for loop over my simulation time where I simulate my MPC use case):

ocp.set('constr_x0', xtraj(:,2)); % set the initial state

My solution to still have the same functionality in my simulink model was to than use the option lb_x0 and ub_x0 to constrain this initial state. I ran the MATLAB model with this change as well, and this model was still feasible at all solver calls. There was however a noteworthy difference in the chosen inputs that this model selected as opposed to that where I use the constr_x0 option. I believe this difference is because now that I set it as a constrain the state only needs to be that result within the constraint tolerances, (visible if I look at the x-trajectory, i.e. if I constrain a state as 0, it will result in a state being 1e-9). This difference is marginally and the states are generally very much alike, up until the approximately the point where in the Simulink solver the the solutions tend to start becoming infeasible. Hinting at that the root of the infeasibilities may lay at the difference in setting this option.

Do you perhaps know what the root is of the Simulink model resulting in all these infeasibilities?

Am I searching in the right direction with assuming it is caused by this absence of setting constr_x0?

p.s.

The only other potential root of the issue that I can think of is the setting of the S-function input lh_0 and uh_0. All constraints are defined as inputs for shooting nodes 0 to N-1 already in the other S-function inputs (lbx, ubx, lbu, ubu, lg,ug, lh, uh). So these feel like a duplicate input, or have I misunderstood how I should use these two S-function inputs.

Is there someone who can help me answer this question?

Hi,

I would suggest to update as little data as possible when interacting with the Simulink solver.
I guess that you don’t need to update the bounds on u and for the nonlinear constraint h.

I used repmat to duplicate my inputs enough to fit the S-function input size requirements (as they are they required for the full horizon).

Here it sounds like you are using u_init. Is that the case?
Do you always initialize the u values also in your Matlab closed loop simulation?
I think that it is easy to do something differently in the initialization leading to the different behavior.

Best,
Jonathan

Hi Jonathan,

Thank you very much for your reply.

You are indeed correct that I am setting u_init. And you are also correct, that this is something that previously was not the case indeed. I have corrected this mistake, but it has not yet resolved the issue.

Maybe to provide more clarity here is the command window text after generating the S-function:

Successfully created sfunction:
acados_solver_sfunction_nmpc_model.mexw64

Note: Usage of Sfunction is as follows:
Inputs are:

  1. lbx_0 - lower bound on x for stage 0, size [18]
  2. ubx_0 - upper bound on x for stage 0, size [18]
  3. parameters - concatenated for all shooting nodes 0 to N, size [3036]
  4. lbx for shooting nodes 1 to N-1, size [360]
  5. ubx for shooting nodes 1 to N-1, size [360]
  6. lbu for shooting nodes 0 to N-1, size [63]
  7. ubu for shooting nodes 0 to N-1, size [63]
  8. lg for shooting nodes 0 to N-1, size [21]
  9. ug for shooting nodes 0 to N-1, size [21]
  10. lh for shooting nodes 0 to N-1, size [42]
  11. uh for shooting nodes 0 to N-1, size [42]
  12. lh_0, size [24]
  13. uh_0, size [24]
  14. initialization of x for all shooting nodes, size [396]

Outputs are:

  1. u0, control input at node 0, size [3]
  2. utraj, control input concatenated for nodes 0 to N-1, size [63]
  3. xtraj, state concatenated for nodes 0 to N, size [396]
  4. acados solver status (0 = SUCCESS)
  5. cost function value
  6. KKT residual
  7. x1, state at node 1
  8. CPU time
  9. SQP iterations

This is the code I use to generate the S-Function:

simulink_opts = get_acados_simulink_opts;



simulink_opts.inputs.x_init = 1;
simulink_opts.inputs.u_init = 0;
simulink_opts.outputs.utraj = 1;
simulink_opts.outputs.xtraj = 1;
simulink_opts.outputs.cost_value = 1;

Create_solver_ACADOS;

%% Compile Sfunctions
cd c_generated_code
make_sfun; % ocp solver
make_sfun_sim; % integrator

The script Create_solver_ACADOS:


if ~exist('simulink_opts','var')
    disp('using acados simulink default options')
    simulink_opts = get_acados_simulink_opts;
    % simulink_opts.inputs.reset_solver = 1;
    simulink_opts.inputs.x_init = 1;
    simulink_opts.inputs.u_init = 0;
    % simulink_opts.inputs.constr_x0 =  1;
    
    simulink_opts.outputs.utraj = 1;
    simulink_opts.outputs.xtraj = 1;
    simulink_opts.outputs.cost_value = 1;
end

model_path = fullfile(pwd,'..','nmpc_model');
addpath(model_path)

check_acados_requirements()

%% solver settings
% N = 21; % number of discretization steps
T_predict = Ts*N; % [s] prediction horizon length
x0 = p.x_0; % initial state

nlp_solver = 'sqp'; % sqp, sqp_rti
qp_solver = 'partial_condensing_hpipm';
qp_solver_cond_N = 5; % condensing horizon (for partial condensing)
sim_method = 'irk'; % erk, irk, irk_gnsf

%% model dynamics
model = Define_NMPC_model_ACADOS(p, p_c); % dynamics, cost, constraints

%% acados ocp model
ocp_model = acados_ocp_model();
ocp_model.set('name', 'nmpc_model');
ocp_model.set('T', T_predict);  % prediction horizon

% symbolics
ocp_model.set('sym_x', model.sym_x);
ocp_model.set('sym_u', model.sym_u);
ocp_model.set('sym_p',model.sym_p);
ocp_model.set('sym_xdot', model.sym_xdot);

% cost
cost_type = 'ext_cost';  % auto, ext_cost, linear_ls
ocp_model.set('cost_type', cost_type);
ocp_model.set('cost_expr_ext_cost_0', model.cost_expr_ext_cost_0);
ocp_model.set('cost_expr_ext_cost', model.cost_expr_ext_cost);
ocp_model.set('cost_expr_ext_cost_e', model.cost_expr_ext_cost_e);


% dynamics
if (strcmp(sim_method, 'erk'))
    ocp_model.set('dyn_type', 'explicit');
    ocp_model.set('dyn_expr_f', model.dyn_expr_f_expl);
else % irk irk_gnsf
    ocp_model.set('dyn_type', 'implicit');
    ocp_model.set('dyn_expr_f', model.dyn_expr_f_impl);
end

% constraints (separate for initial, intermediate and terminal stages)
ocp_model.set('constr_type', 'auto');
ocp_model.set('constr_expr_h_0', model.constr_expr_h_0);
ocp_model.set('constr_expr_h', model.constr_expr_h);

lb_x                        = p.lb_x;
ub_x                        = p.ub_x;
lb_u                        = p.lb_u;
ub_u                        = p.ub_u;
lb_v                        = p.lb_v;
ub_v                        = p.ub_v;
lb_a                        = p.lb_a;
ub_a                        = p.ub_a;
lb_j                        = p.lb_j;
ub_j                        = p.ub_j;
ocp_model.set('constr_lh_0', [lb_x; lb_v; lb_u; lb_a; lb_j]); % lower bound on h
ocp_model.set('constr_uh_0', [ub_x; ub_v; ub_u; ub_a; ub_j]);  % upper bound on h
ocp_model.set('constr_lh', [lb_x; lb_v; lb_u; lb_a; lb_j]);
ocp_model.set('constr_uh', [ub_x; ub_v; ub_u; ub_a; ub_j]);

ocp_model.set('constr_x0', [x0; zeros(12,1)]);  % set the initial state

%% acados ocp options
ocp_opts = acados_ocp_opts();
ocp_opts.set('param_scheme_N', N);
ocp_opts.set('nlp_solver', nlp_solver);
ocp_opts.set('nlp_solver_max_iter',100);
ocp_opts.set('nlp_solver_tol_stat', 5e-3);
ocp_opts.set('sim_method', sim_method);

ocp_opts.set('ext_fun_compile_flags', ''); % '-O2'
ocp_opts.set('globalization', 'merit_backtracking') % turns on globalization



%% create ocp solver
ocp = acados_ocp(ocp_model, ocp_opts, simulink_opts);

The S-function in Simulink now looks like this:

I call this Simulink function multiple times within a for loop in a matlab script. All parameters that are highlighted can differ between calls in the for loop. All other parameters are set before the for loop and the first call and are not changed any more afterwards.

This means that indeed as you mention I do not require to update the bounds on u, and the non-linear constraint h. Should I then change the formulation and remove all non-highlighted parameters as an input parameter to the Simulink model?

The top two highlighted inputs are lb_x0 and ub_x0, these where constant in my original non-simulink script. But now they vary as I use them to constrain my initial shooting step position, as the option “constrain_x0” was no longer possible here (as explained in my original post).