Retrieving only zeros and no SQP iteration

User Questions
1

Hi dear acados community

I am currently using the c-interface to use the c-generated code in C++. The problem I am facing currently is that I am only retrieving zeros, and the sqp is not even iterating. I have come up with the model myself, the OCP might be therefore illposed, but I think there should be at least some infeasible solutions. The model for the ocp has seven states I want to update at each iteration, in the python script as follows:

# set up states
x_pos = SX.sym('x_pos')  #x position
y_pos = SX.sym('y_pos')  #y position 
psi = SX.sym('psi') # heading
v = SX.sym('v') # velocity
cte = SX.sym('cte') # cross track error
epsi = SX.sym('epsi') # heading error
evel = SX.sym('evel') # velocity error
x = vertcat(x_pos, y_pos, psi, v, cte, epsi, evel)

# set up controls
a = SX.sym('a') # acceleration 
delta = SX.sym('delta') # steering angle
u = vertcat(a, delta)

# x_dot and parameters
  x_pos_dot = SX.sym('x_pos_dot')
  y_pos_dot = SX.sym('y_pos_dot')
  psi_dot = SX.sym('psi_dot')
...
...
...
p = vertcat(...)

f_expl = vertcat(v * cos(psi),
                 v * sin(psi),
                 v * tan(delta)/wheelbase,
                 a,
                 f_1(x,p)
                 f_2(x,p),
                 f_2(x,p))

f_impl = x_dot - f_expl

Here, the dynamics of the error states are calculated through parameters set at each iteration, noted by f_i(\textbf{x},\textbf{p}). I only include those three states in the cost function, and y_ref is therefore zero for all states.

   ocp.model.cost_y_expr = vertcat(
       cte,
       epsi,
       evel,
       a,
       delta)

   ocp.model.cost_y_expr_e = vertcat(
       cte,
       epsi,
       evel
   )

Solver settings are:

ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" 
ocp.solver_options.nlp_solver_type = "SQP"  
ocp.solver_options.hessian_approx = "GAUSS_NEWTON"
ocp.solver_options.integrator_type = "ERK"

After (successfully) generating the code with

ocp_solver = AcadosOcpSolver(ocp, json_file=f"{ocp.code_export_directory}/acados_ocp_{ocp.model.name}.json", cmake_builder=CMakeBuilder())

I include all the necessary headers, and create all instantiate all capsules needed and update the solver in every iteration:

// initial condition
ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, 0, "idxbx", idxbx0_);
ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, 0, "lbx", x_current_);
ocp_nlp_constraints_model_set(nlp_config_, nlp_dims_, nlp_in_, 0, "ubx", x_current_); // updates all states
mpc_acados_update_params(acados_ocp_capsule_, i, param_buffer_[i], NP); // updates parameters
ocp_nlp_cost_model_set(nlp_config_, nlp_dims_, nlp_in_, i, "yref", y_ref_[i]); // updates yref, but anyways always zero

In the front end, everything looks quite promising, but the solver outputs only zeros and after searching for the reason for hours and hours, I am still clueless why this could be.

My output:
[INFO] [1712305176.414031349] [mpc_acados]: status: 1 (refers to the status of the solver)
[INFO] [1712305176.414042466] [mpc_acados]: Solver info:
SQP iterations 0
minimum time for 1 solve 0.000000 [ms]
KKT 0.000000e+00

— x_traj —
0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00
.
. (N time steps)
.
0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00

— u_traj —
0.000000e+00 0.000000e+00
.
. (N time steps)
.
0.000000e+00 0.000000e+00

Did I overlook or forget an important setting? Any hints on what I can change? Thank you all already! :pray:

2

Hi :wave:

could you give some more details about the cost? And what is the initial state constraint for the problem you are solving here? Could it be that all zeros is the solution to the problem?

3

Thank you for your quick anwser! Sure, here is the cost:

ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = "NONLINEAR_LS"

# output cost from cost_expr_y, cost_expr_y_ref:
# list of entries for cost
states_cost = vertcat(
  cte,
  epsi,
  evel)
ocp.model.cost_y_expr = vertcat(
  states_cost,
  u
)

# output weight matrix
Q = np.diag([
  100,  # cte
  100,  # epsi
  100,  # evel
])

# input weight matrix
R = np.zeros((n_u, n_u))
R[0, 0] = 0.1
R[1, 1] = 0.1

W = block_diag(Q, R)  # stage cost matrix
ocp.cost.W = W

# output for terminal cost
ocp.model.cost_y_expr_e = vertcat(
  cte,
  epsi,
  evel
)

Qe = np.diag([    # converge at terminal state
  1000,  # cte
  1000,  # epsi
  1000,  # evel
])

# terminal cost (mayer term), m(x,u,z) = ||y_e - yref_e||_W_e^2 ensures that the long-term feasibility is not
# impacted and helps align finite-horizon solution to inf.-time solution (what we actually want)
ocp.cost.W_e = Qe

Vx = np.zeros((n_y, n_x))
Vx[:n_x, :n_x] = np.eye(n_x)
ocp.cost.Vx = Vx
ocp.cost.Vx_e = np.eye(n_x)

Vu = np.zeros((n_y, n_u))
Vu[7, 0] = 1.0
Vu[8, 1] = 1.0
ocp.cost.Vu = Vu

# set intial references
ocp.cost.yref = np.zeros((ocp.model.cost_y_expr.shape[0],))
ocp.cost.yref_e = np.zeros((ocp.model.cost_y_expr_e.shape[0],))

In my C++ MPC class I set the weights at initialization:

// Cost weights (NY = 5) 
double W[NY * NY]{};
W[0 + 0 * (NY)] = 100.0;
W[1+ 1* (NY)] = 100.0;
W[2+ 2* (NY)] = 100.0;
W[3+ 3 * (NY)] = 1.0;
W[4 + 4 * (NY)] = 1.0;


double W_e[NYN * NYN]{};     // NYN = 3 
W_e[0 + 0 * (NYN)] = 1000.0;
W_e[1+ 1* (NYN)] = 1000.0;
W_e[2+ 2 * (NYN)] = 1000.0;

// Set cost weights
for (int i = 0; i < N; i++){
    ocp_nlp_cost_model_set(nlp_config_, nlp_dims_, nlp_in_, i, "W", W);
}
        
ocp_nlp_cost_model_set(nlp_config_, nlp_dims_, nlp_in_, N, "W", W_e);

The first four states (x_{pos}, y_{pos}, \psi, and v) can be zero, but are anyways not included in the cost function. The last three states which are the errors (cte, e_{\psi} and e_{v}) are never zero if not coincidental.

It would surprise me if the initial errors aren’t big enough to start the optimization at all :sweat_smile: