Acados only computes zero control inputs

Hi,

I am currently working on some benchmarking and I am having a problem with one OCP in particular which should definitely work with acados. I am looking at the nonlinear hanging chain as it is shown in

Kirches, C., et al. “Efficient direct multiple shooting for nonlinear model predictive control on long horizons.” Journal of Process Control 22.3 (2012): 540-550.

I know that there is an example for the nonlinear chain in the acados installation folder which is based on

Kouzoupis, Dimitris, et al. “Recent advances in quadratic programming algorithms for nonlinear model predictive control.” Vietnam Journal of Mathematics 46.4 (2018): 863-882.

Those two problems are only slightly different. Besides differing parameters, Kirches 2012 also does not consider the wall constraint or the initial disturbance, thus it should be easier to solve.

When I try to implement it, however, acados only iterates once (even though it is allowed to iterate 4 times) and always returns an optimal input of zero over all three input channels, which effectively results in the free response of the dynamic system. The extremely short timing, which I get via ocp_acados.get(‘time_qp_sol’) further suggests that acados is not even “trying” to actually solve the problem.

I am pretty confident in the implemented ODE of the dynamical system, since it works with other solvers. I am therefore convinced that I messed up some settings and I was wondering if any of you can spot a mistake that I have not been able to find for days.

keyfacts: 21 states, 3 inputs, 0.1s sampling time, 80 steps horizon, 4 max iterations

% Calculate prediction horizon in seconds
ocp_N = double(optim.N);
ocp_T = ocp_N * model.dT;

%% Define cost function
% The cost function on ACADOS is defined in terms of a virtual output y. In
% our case, we stack the states and inputs together to form y.

nx = model.nx;
nu = model.nu;
ny = nx + nu;                   % Dimension of the virtual output
Vx = [ eye(nx); zeros(nu,nx) ]; % Mapping from states to virtual output
Vu = [ zeros(nx,nu); eye(nu) ]; % Mapping from inputs to virtual output

% lower and upper bounds on the input
u_ub = optim.b(1:nu);
u_lb = - optim.b(nu+1:2*nu);

% set up reference to be tracked
y_ref = [model.xdes; model.udes];

% Weighting matrix for the virtual output
W  = blkdiag(optim.Q, optim.R); 

% set up terminal penalty
Vx_e = Vx;
W_e = blkdiag(optim.P, zeros(size(optim.R)));
y_ref_e = y_ref;


%% Build ACADOS OCP model
ocp_model = acados_ocp_model();
ocp_model.set('name', model.name);
ocp_model.set('T', ocp_T);

% Set dimension of the decision variables
ocp_model.set('dim_nx', nx);
ocp_model.set('dim_nu', nu);
ocp_model.set('dim_ny', ny);
ocp_model.set('dim_ny_e', 0); 
ocp_model.set('dim_nz', 0);   % No algebraic variables

% Set symbolic variables
sym_x = casadi.SX.sym('x', nx);
sym_u = casadi.SX.sym('u', nu);
ocp_model.set('sym_x', sym_x);
ocp_model.set('sym_u', sym_u);

% Define dynamic properties of the model
ocp_model.set('dyn_type', 'explicit'); % Use explicit continuous-time ODE as model
ocp_model.set('dyn_expr_f', model.ode(sym_x, sym_u));

% Set up cost function
ocp_model.set('cost_type', 'linear_ls');  % Linear least-squares cost function
ocp_model.set('cost_Vu', Vu);
ocp_model.set('cost_Vx', Vx);
ocp_model.set('cost_Vz', zeros(ny,0));    % No algebraic variables, so setting to 0
ocp_model.set('cost_W', W);
ocp_model.set('cost_y_ref', y_ref); % in this case we are considering a reference

% Set up input constraints
ocp_model.set('constr_C', zeros(nu, nx));  % No constraints on the states
ocp_model.set('constr_D', eye(nu));
ocp_model.set('constr_lg', u_lb);       % Lower and upper limit on the control
ocp_model.set('constr_ug', u_ub);

% Use additional terminal cost. 
ocp_model.set('cost_type_e', 'linear_ls');
ocp_model.set('cost_Vx_e', Vx_e); % for terminal cost
ocp_model.set('cost_W_e', W_e); % for terminal cost
ocp_model.set('cost_y_ref_e', y_ref_e); % for terminal cost

% Temporarily set initial conditions, without this the model contruction fails. Will be overwritten
% before the simulation anyway.
ocp_model.set('constr_x0', model.x0);

%% Configure ACADOS OCP solver
ocp_opts = acados_ocp_opts();
ocp_opts.set('param_scheme_N', ocp_N);

% Set up the internal simulator.
ocp_opts.set('sim_method', 'erk');        % Use explicit Runge-Kutta ODE solver
ocp_opts.set('sim_method_num_stages', 4); % Use 4th order ODE solver
ocp_opts.set('sim_method_num_steps', 2);

% Set up the NLP solver. If the solver is only allowed to perform a single iteration, switch to
% real-time iteration mode.
if optim.iter_max == 1
    ocp_opts.set('nlp_solver', 'sqp_rti');
else
    ocp_opts.set('nlp_solver', 'sqp');
end
ocp_opts.set('nlp_solver_max_iter', optim.iter_max); % Maximum number of iterations

% Set up the QP solver
ocp_opts.set('qp_solver', 'full_condensing_hpipm');

%% Generate OCP
ocp = acados_ocp(ocp_model, ocp_opts);

Any help, advice or idea would be much appreciated.

Best,
Artur

Hi Artur,

There is a C example and a Python example. The Python one is more recent and probably easier to follow.

The code you posted is not the full description of the OCP.
So, it is hard to tell what is going on.

Also, did you print the solver statistics and check the solver status?

Unfortunately I was unable to run the python example on my windows machine. But going through the code I still could not figure out why my OCP via the matlab interface should fail.

The solver status is always ‘4’. Besides your post in Error status 3: ACADOS_MINSTEP. Any configuration to alleviate it? - User Questions - acados forum I could not find further documentation on the error codes. So apparently my problem is described by

INCONS_EQ, // unconsistent equality constraints

which confuses me since I am not aware of any active equality constraints. The only active constraints should be the upper and lower bounds on the control input.

A more detailed description of the OCP:

  1. ocp_model:
                   name: 'NLChain_4'
      cost_ext_fun_type: 'casadi'
    cost_ext_fun_type_e: 'casadi'
    cost_ext_fun_type_0: 'casadi'
       dyn_ext_fun_type: 'casadi'
            cost_type_0: []
              cost_type: 'linear_ls'
            cost_type_e: 'linear_ls'
               dyn_type: 'explicit'
            constr_type: 'bgh'
          constr_type_e: 'bgh'
                      T: 8
                 dim_nx: 21
                 dim_nu: 3
                 dim_ny: 24
               dim_ny_e: 0
                 dim_nz: 0
                  sym_x: [21×1 casadi.SX]
                  sym_u: [3×1 casadi.SX]
             dyn_expr_f: [21×1 casadi.SX]
                cost_Vu: [24×3 double]
                cost_Vx: [24×21 double]
                cost_Vz: [24×0 double]
                 cost_W: [24×24 double]
             cost_y_ref: [24×1 double]
             constr_Jbu: [3×3 double]
             constr_lbu: [3×1 double]
             constr_ubu: [3×1 double]
              cost_Vx_e: [21×21 double]
               cost_W_e: [21×21 double]
           cost_y_ref_e: [21×1 double]
           constr_lbx_0: [21×1 double]
           constr_ubx_0: [21×1 double]
           constr_Jbx_0: [21×21 double]
        constr_idxbxe_0: [0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20]
  1. ocp_opts
           compile_interface: 'auto'
                codgen_model: 'true'
               compile_model: 'true'
              param_scheme_N: 80
              shooting_nodes: []
                  time_steps: []
            parameter_values: []
                  nlp_solver: 'sqp'
    nlp_solver_exact_hessian: 'false'
         nlp_solver_max_iter: 4
         nlp_solver_tol_stat: 1.0000e-06
           nlp_solver_tol_eq: 1.0000e-06
         nlp_solver_tol_ineq: 1.0000e-06
         nlp_solver_tol_comp: 1.0000e-06
       nlp_solver_ext_qp_res: 0
      nlp_solver_step_length: 1
                   rti_phase: 0
                   qp_solver: 'partial_condensing_hpipm'
               globalization: 'fixed_step'
                   alpha_min: 0.0500
             alpha_reduction: 0.7000
          qp_solver_iter_max: 50
      qp_solver_cond_ric_alg: 0
           qp_solver_ric_alg: 0
        qp_solver_warm_start: 0
         warm_start_first_qp: 0
                  sim_method: 'erk'
            collocation_type: 'gauss_legendre'
       sim_method_num_stages: 2
        sim_method_num_steps: 2
      sim_method_newton_iter: 3
        sim_method_jac_reuse: 0
          gnsf_detect_struct: 'true'
           regularize_method: 'no_regularize'
                 print_level: 0
         levenberg_marquardt: 0
              exact_hess_dyn: 1
             exact_hess_cost: 1
           exact_hess_constr: 1
                  output_dir: 'C:\Users\Artur\Desktop\2022_project\build'
            qp_solver_cond_N: 80

I hope this is what you meant by the full description of the OCP

(Please note that in my original post I used ‘full_condensing_hpipm’ instead of ‘partial_condensing_hpipm’ in this post. I figured the problem has something to do with the hpipm solver so I unsuccessfully changed a few settings. In both cases the solver status is 4.)

Hi @Artur,

Status = 4 means you have a QP_Failure. This can occur for example, if your initialization of your problem is not properly chosen or you get some Inf/NaNs. Maybe you initialize it with all zeros, but your ODE is not defined for this particular working point.
Try to initialize it with an approximate solution of your problem to check whether that is your case.

Bye

1 Like

Thank you for your suggestion @mss .
Unfortunately it did not solve my problem. I augmented my initial conditions in several ways and the solver status remains 4 throughout the entire simulation.

The ODE can be simulated in matlab using the original inital condition via ode45, moreover I can solve the OCP using other solvers. Doesn’t this suggest that the ODE is not the problem here?
This is the reason why I assumed the problem to be somewhere in my acados settings.

Ok, thanks for your update.

Just to avoid confusion: are the initial conditions set with init_x, init_u? (constr_x0 as it is used in your code is just a constraint).

Options to proceed:

  • Simplify your problem by removing some/all constraints and check the output.
  • Test only the integrator of your ODE (without the optimization)
  • Create a minimal working example and share it with us.

I have set my initial conditions via constr_x0 , so I guess I did indeed have a constraint.
When I remove all the constraints, acados returns NaN as a control input for the first step of the MPC as well as a ‘0’ status. The next computed states (via ode45) are then also NaN and acados does not iterate at all.

I am unsure as to how I can only check the integrator without the optimiztation.

I have created a minimal working example for the nonlinear chain with 4 masses. You can just run the main_NLChain.m

Hopefully you can spot something that I cannot.

Ok, that sounds that your problem provides no solution if you initialize all states with zeros (which is the default behavior if you don’t set anything explicitely). Try to set init_x to some meaningful values.
For some more explanation see this discussion:

2 Likes

@mss, you have just saved the day (or week)! Thank you!

I initialized the solver (via init_x) with the kronecker product ones(N+1, 1) * x0 and suddenly it works like a charm.

So thank you for your time and effort.

1 Like

Glad to hear! :slight_smile:

All the best for your project…