Different Solver Behavior Between EXTERNAL Cost and LINEAR_LS/NONLINEAR_LS After C Code Generation

Hello everyone!

I’ve been using Acados in Python to implement a path-following MPC, which has been working well in real-world applications. To run this in a C++ environment, I generated the C code and tested the solver.

However, I ran into an issue: when using LINEAR_LS or NONLINEAR_LS cost functions, the solver fails to work as expected, unlike when running in Python. Here’s an example setup for NONLINEAR_LS:

Python script:

# cost matrices
Q_mat = 2 * np.diag([10, 100, 10, 10, 0, 0])
R_mat = 2 * np.diag([10, 0.1])


# path cost
x = model.x
u = model.u

ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.model.cost_y_expr = ca.vertcat(x, u)
ocp.cost.yref = np.zeros((nx + nu,))
ocp.cost.W = ca.diagcat(Q_mat, R_mat).full()

# terminal cost
ocp.cost.cost_type_e = 'NONLINEAR_LS'
ocp.cost.yref_e = np.zeros((nx,))
ocp.model.cost_y_expr_e = ca.vertcat(x)
ocp.cost.W_e = Q_mat

Using the generated C code, I update y_ref and weights in C++ like this:

  1. y_ref update:
for (int i = 0; i < INTEGRATE_KINEMATIC_BICYCLE_NMPC_N; ++i) {
    // Define the reference state: [x, y, yaw, v, delta, a]
    std::vector<double> yref = {
        ref.control_point.at(i).x,
        ref.control_point.at(i).y,
        ref.control_point.at(i).yaw,
        ref.control_point.at(i).vx,
        0.0,
        0.0
    };
    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", const_cast<double*>(yref.data()));
}
  1. Weight update:
for (int i = 0; i < step_num_ - 1; ++i) {
    // Set cost weights
    std::vector<double> Q_diag = {params_.weight_x, params_.weight_y, params_.weight_yaw, params_.weight_speed, params_.weight_steer, params_.weight_accel};
    std::vector<double> R_diag = {params_.weight_dsteer, params_.weight_jerk};
    std::vector<double> W(INTEGRATE_KINEMATIC_BICYCLE_NMPC_NY * INTEGRATE_KINEMATIC_BICYCLE_NMPC_NY, 0.0);

    for (int i = 0; i < INTEGRATE_KINEMATIC_BICYCLE_NMPC_NX; ++i) {
        W[i * INTEGRATE_KINEMATIC_BICYCLE_NMPC_NY + i] = Q_diag[i];
    }
    for (int i = 0; i < INTEGRATE_KINEMATIC_BICYCLE_NMPC_NU; ++i) {
        W[(INTEGRATE_KINEMATIC_BICYCLE_NMPC_NX + i) * INTEGRATE_KINEMATIC_BICYCLE_NMPC_NY + INTEGRATE_KINEMATIC_BICYCLE_NX + i] = R_diag[i];
    }

    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W.data());
}

// Set terminal cost weights
std::vector<double> Qe_diag = {params_.weight_x_end, params_.weight_y_end, params_.weight_yaw_end, params_.weight_speed_end, params_.weight_steer_end, params_.weight_accel_end};
std::vector<double> W_e(INTEGRATE_KINEMATIC_BICYCLE_NMPC_NX * INTEGRATE_KINEMATIC_BICYCLE_NMPC_NX, 0.0);
for (int i = 0; i < INTEGRATE_KINEMATIC_BICYCLE_NMPC_NX; ++i) {
    W_e[i * INTEGRATE_KINEMATIC_BICYCLE_NMPC_NX + i] = Qe_diag[i];
}
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, INTEGRATE_KINEMATIC_BICYCLE_NMPC_N, "W", W_e.data());

On the other hand, using EXTERNAL cost functions works perfectly:

Python script:

# parameters
x_ref = SX.sym('x_ref')
y_ref = SX.sym('y_ref')
yaw_ref = SX.sym('yaw_ref')
speed_ref = SX.sym('speed_ref')
p = vertcat(x_ref, y_ref, yaw_ref, speed_ref)
ocp.model.p = p
ocp.parameter_values = np.zeros(4)

# cost matrices
Q_mat = 2 * np.diag([10, 100, 10, 10, 0, 0])
R_mat = 2 * np.diag([10, 0.1])

# path cost
x = model.x  # [x, y, yaw, delta, accel]
u = model.u  # [ddelta, jerk]

ocp.cost.cost_type = 'EXTERNAL'
cost_x_expr = vertcat(x[0] - x_ref, x[1] - y_ref, x[2] - yaw_ref, x[3] - speed_ref, x[4], x[5])
ocp.model.cost_expr_ext_cost = 0.5 * cost_x_expr.T @ mtimes(Q_mat, cost_x_expr) + 0.5 * u.T @ mtimes(R_mat, u)

# terminal cost
ocp.cost.cost_type_e = 'EXTERNAL'
ocp.model.cost_expr_ext_cost_e = 0.5 * cost_x_expr.T @ mtimes(Q_mat, cost_x_expr)

Parameter updates in C++:

for (int i = 0; i < INTEGRATE_KINEMATIC_BICYCLE_NMPC_N; ++i) {
    std::vector<double> param = {
        ref.control_point.at(i).x,
        ref.control_point.at(i).y,
        ref.control_point.at(i).yaw,
        ref.control_point.at(i).vx
    };
    ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "parameter_values", const_cast<double*>(param.data()));
}

I’m not sure if this issue stems from how I’m interfacing with the C code for LINEAR_LS/NONLINEAR_LS or if there’s a bug related to these cost function interfaces. Any insights or advice would be highly appreciated!

Hi,

I think that there are 4 different combinations.
Cost formulation: EXTERNAL vs NONLINEAR_LS.
Testing your solver in Python vs C++.
Do you get the same behavior between the formulations when just using Python?
I would advise to first make sure this is correct and then make sure the solver updates work correct using the same formulation in Python and C++.

Best,
Jonathan