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:
- 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()));
}
- 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!