Good afternoon,
I’m trying to run Acados through the NLP interface but I’m finding some problems. I’ve integrated the equations in Casadi in C++ and solved the problem there with success with IPOPT. Because of this, I understand that the equations are valid and should be able to be solved in Acados to improve the speed at which it’s solved. I’ve later generated the code of the dynamics and input these functions into Acados.
My problem is that once the explicit equations are loaded into Acados the solver immediately returns an “optimal solution” which is just the originally provided guess (i.e. does not iterate). When using the implicit equation instead, I get a segmentation fault inside Acados.
I wonder if these errors could come from a wrong derivation of the equations in Casadi or a wrong use of the C interface of Acados.
Motivation (Why use the C interface?)
I’m trying to use Acados to solve an NMPC problem regarding a quadruped robot with the main idea being to later integrate this into a C++ framework. Due to this, and having several other libraries already available in C++ to help with this task, I’ve explored the C interface of Acados. However, I found the documentation of the C interface a bit lacking. I understand the main idea is to use a high level language such as Python or MATLAB to later auto-generate the code but the readability and re-usability of this code is not great. Because of this, for now, I’m auto-generating my dynamic functions and necessary derivatives.
Since the idea is to later integrate this into a C++ framework I would prefer to auto-generate as little code as possible so that other people could reuse the rest.
For now I’m trying a very simplified case where I assume full contact and as such I only have one set of linear constraints and one set of non-linear constraints. These non-linear constraints have been deactivated for now as to simplify the first example as much as possible but I would need to integrate them later of course.
I’m not providing the proper equations of the dynamics and constraints but I could provide them later if more information is needed to maybe run the code locally.
My pipeline
For now my pipeline involves, auto-generating the dynamic functions to register into Casadi. I’ve tried doing this in an explicit way and in an implicit way. I can’t really find how or in which order it expects the parameters to be passed into the function so I’ve based myself off of the MATLAB functions found in the Acados repo. I’ve made C++ equivalents of these functions in the following way:
// From https://github.com/acados/acados/blob/master/interfaces/acados_matlab_octave/generate_c_code_explicit_ode.m
void generate_c_code_explicit_ode(
const std::string& model_name,
const SX& x,
const SX& u,
const SX& f_expl,
const bool& generate_hess = false,
const std::string& generate_path = "") {
int nx = x.size1();
int nu = u.size1();
// Setup expressions
SX Sx = SX::sym("Sx", nx, nx);
SX Su = SX::sym("Su", nx, nu);
SX lambdaX = SX::sym("lambdaX", nx, 1);
SX vdeX = SX::zeros(nx, nx);
SX vdeU = SX::zeros(nx, nu) + jacobian(f_expl, u);
vdeX = vdeX + jtimes(f_expl, x, Sx);
vdeU = vdeU + jtimes(f_expl, u, Su);
bool transpose_jacobian_before_multiplication = true;
SX adj = jtimes(f_expl, vertcat(x, u), lambdaX, transpose_jacobian_before_multiplication);
if (generate_hess) {
SX S_forw = vertcat(horzcat(Sx, Su), horzcat(SX::zeros(nu, nx), SX::eye(nu)));
SX hess = mtimes(S_forw.T(), jtimes(adj, vertcat(x, u), S_forw));
// TODO uncompress it ???
// std::vector<SX> hess2;
// for (int j = 0; j < nx + nu; ++j) {
// for (int i = j; i < nx + nu; ++i) {
// hess2.push_back(hess(i, j));
// }
// }
// Add hess function
Function expl_ode_hess = Function(model_name + "_expl_ode_hess", {x, Sx, Su, lambdaX, u}, {adj, hess});
CodeGenerator codegen3(model_name + "_expl_ode_hess");
codegen3.add(expl_ode_hess);
codegen3.generate(generate_path);
}
Function expl_ode_fun = Function(model_name + "_expl_ode_fun", {x, u}, {f_expl});
Function expl_vde_for = Function(model_name + "_expl_vde_for", {x, Sx, Su, u}, {f_expl, vdeX, vdeU});
Function expl_vde_adj = Function(model_name + "_expl_vde_adj", {x, lambdaX, u}, {adj});
CodeGenerator codegen0(model_name + "_expl_ode_fun");
codegen0.add(expl_ode_fun);
codegen0.generate(generate_path);
CodeGenerator codegen1(model_name + "_expl_vde_for");
codegen1.add(expl_vde_for);
codegen1.generate(generate_path);
CodeGenerator codegen2(model_name + "_expl_vde_adj");
codegen2.add(expl_vde_adj);
codegen2.generate(generate_path);
}
// From https://github.com/acados/acados/blob/master/interfaces/acados_matlab_octave/generate_c_code_implicit_ode.m
void generate_c_code_implicit_ode(
const std::string& model_name,
const SX& x,
const SX& xdot,
const SX& u,
const SX& z,
const SX& f_impl,
const bool& generate_hess = false,
const std::string& generate_path = "") {
int nx = x.size1();
int nz = z.size1();
// Generate jacobians
SX jac_x = jacobian(f_impl, x);
SX jac_xdot = jacobian(f_impl, xdot);
SX jac_u = jacobian(f_impl, u);
SX jac_z = jacobian(f_impl, z);
// Generate hessian
if (generate_hess) {
SX x_xdot_z_u = vertcat(x, xdot, z, u);
SX multiplier = SX::sym("multiplier", nx + nz);
// Hessian computed as forward over adjoint
SX adj = jtimes(f_impl, x_xdot_z_u, multiplier, true);
SX hess = jacobian(adj, x_xdot_z_u, Dict{{"symmetric", true}});
Function impl_dae_hess = Function(model_name + "_impl_dae_hess", {x, xdot, u, z, multiplier}, {hess});
CodeGenerator codegen4(model_name + "_impl_dae_hess");
codegen4.add(impl_dae_hess);
codegen4.generate(generate_path);
}
// Add function for the implicit DAE
Function impl_dae_fun = Function(model_name + "_impl_dae_fun", {x, xdot, u, z}, {f_impl});
Function impl_dae_fun_jac_x_xdot_z = Function(model_name + "_impl_dae_fun_jac_x_xdot_z", {x, xdot, u, z}, {f_impl, jac_x, jac_xdot, jac_z});
Function impl_dae_jac_x_xdot_u_z = Function(model_name + "_impl_dae_jac_x_xdot_u_z", {x, xdot, u, z}, {jac_x, jac_xdot, jac_u, jac_z});
Function impl_dae_fun_jac_x_xdot_u = Function(model_name + "_impl_dae_fun_jac_x_xdot_u", {x, xdot, u, z}, {f_impl, jac_x, jac_xdot, jac_u});
CodeGenerator codegen0(model_name + "_impl_dae_fun");
codegen0.add(impl_dae_fun);
codegen0.generate(generate_path);
// Add function for the Jacobians
CodeGenerator codegen1(model_name + "_impl_dae_fun_jac_x_xdot_z");
codegen1.add(impl_dae_fun_jac_x_xdot_z);
codegen1.generate(generate_path);
CodeGenerator codegen2(model_name + "_impl_dae_jac_x_xdot_u_z");
codegen2.add(impl_dae_jac_x_xdot_u_z);
codegen2.generate(generate_path);
CodeGenerator codegen3(model_name + "_impl_dae_fun_jac_x_xdot_u");
codegen3.add(impl_dae_fun_jac_x_xdot_u);
codegen3.generate(generate_path);
}
These functions are taken from generate_c_code_implicit_ode and generate_c_code_explicit_ode with minor changes and adapted to the Casadi C++ style. One of the main differences is the comment on the uncompression of the hessian as it gives compilation issues and I’m not sure how to solve them so for now it returns the hessian itself.
Once these functions have been auto-generated and compiled into C objects/library then I attempt to use the ocp_nlp_interface in C to solve my problem. Following some of the examples I’ve arrived at the following structure:
int main(int argc, char** argv) {
double dt = 0.01; // s
int nHorizon = 10;
double tHorizon = dt * nHorizon; // s
// Problem Dimensions
int nStates = 24;
int nInputs = 24;
int nMeasurements = 36;
int nLinearConstraints = 16;
int nNonLinearConstraints = 0; // For now
// --------------------------------------------------------------------------------------------
// Configure OCP
// --------------------------------------------------------------------------------------------
int nx[nHorizon + 1] = {0}; // States
int nu[nHorizon + 1] = {0}; // Inputs
int nz[nHorizon + 1] = {0}; // Algebraic variables
int ns[nHorizon + 1] = {0}; // Slack variables
int nbx[nHorizon + 1] = {0}; // Bounds State
int nbu[nHorizon + 1] = {0}; // Bounds Input
int ny[nHorizon + 1] = {0}; // Measurements
int ng[nHorizon + 1] = {0}; // General Linear constraints
int nh[nHorizon + 1] = {0}; // Nonlinear constraints
for (int i = 0; i < nHorizon; i++) {
nx[i] = nStates;
nu[i] = nInputs;
nz[i] = 0;
ns[i] = 0;
nbx[i] = nStates;
nbu[i] = nInputs;
ny[i] = nMeasurements;
ng[i] = nLinearConstraints;
nh[i] = nNonLinearConstraints;
}
nx[nHorizon] = nStates;
nu[nHorizon] = 0;
nz[nHorizon] = 0;
ns[nHorizon] = 0;
nbx[nHorizon] = nStates;
nbu[nHorizon] = 0;
ny[nHorizon] = 12;
ng[nHorizon] = 0;
nh[nHorizon] = nNonLinearConstraints;
// --------------------------------------------------------------------------------------------
// OCP Plan
// --------------------------------------------------------------------------------------------
ocp_nlp_plan_t* plan = ocp_nlp_plan_create(nHorizon);
plan->nlp_solver = SQP;
plan->regularization = NO_REGULARIZE;
plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM;
for (int i = 0; i < nHorizon; ++i) {
plan->nlp_cost[i] = LINEAR_LS;
plan->nlp_constraints[i] = BGH;
plan->nlp_dynamics[i] = CONTINUOUS_MODEL;
plan->sim_solver_plan[i].sim_solver = ERK; // IRK to use implicit dynamics
}
plan->nlp_cost[nHorizon] = LINEAR_LS;
plan->nlp_constraints[nHorizon] = BGH;
ocp_nlp_config* config = ocp_nlp_config_create(*plan);
ocp_nlp_dims* dims = ocp_nlp_dims_create(config);
ocp_nlp_dims_set_opt_vars(config, dims, "nx", nx);
ocp_nlp_dims_set_opt_vars(config, dims, "nu", nu);
ocp_nlp_dims_set_opt_vars(config, dims, "nz", nz);
ocp_nlp_dims_set_opt_vars(config, dims, "ns", ns);
for (int i = 0; i <= nHorizon; ++i) {
ocp_nlp_dims_set_cost(config, dims, i, "ny", &ny[i]);
ocp_nlp_dims_set_constraints(config, dims, i, "nbx", &nbx[i]);
ocp_nlp_dims_set_constraints(config, dims, i, "nbu", &nbu[i]);
ocp_nlp_dims_set_constraints(config, dims, i, "ng", &ng[i]);
ocp_nlp_dims_set_constraints(config, dims, i, "nh", &nh[i]);
}
ocp_nlp_sqp_opts* opts = static_cast<ocp_nlp_sqp_opts*>(ocp_nlp_solver_opts_create(config, dims));
ocp_nlp_opts* nlp_opts = opts->nlp_opts;
int verbosity = 5;
int max_iter = 5000;
double tol_stat = 1e-9;
double tol_eq = 1e-9;
double tol_ineq = 1e-9;
double tol_comp = 1e-9;
bool sens_forw = false;
bool sens_hess = false;
bool sens_adj = false;
bool exact_hess = false;
int num_steps = 5; // Number of sub-steps for the integration
int ns2 = 1; // Number of steps for each step with RK
ocp_nlp_solver_opts_set(config, opts, "exact_hess", &exact_hess);
ocp_nlp_solver_opts_set(config, opts, "print_level", &verbosity);
ocp_nlp_solver_opts_set(config, opts, "max_iter", &max_iter);
ocp_nlp_solver_opts_set(config, opts, "tol_stat", &tol_stat);
ocp_nlp_solver_opts_set(config, opts, "tol_eq", &tol_eq);
ocp_nlp_solver_opts_set(config, opts, "tol_ineq", &tol_ineq);
ocp_nlp_solver_opts_set(config, opts, "tol_comp", &tol_comp);
ocp_nlp_solver_opts_set(config, opts, "qp_hpipm_mode", (void*)"BALANCE");
for (int i = 0; i < nHorizon; ++i) {
ocp_nlp_solver_opts_set_at_stage(config, opts, i, "dynamics_num_steps", &num_steps);
ocp_nlp_solver_opts_set_at_stage(config, opts, i, "dynamics_ns", &ns2);
}
// If needed to deactivate the evaluation of hessian, adjoint, or forward
// for (int i = 0; i < nHorizon; i++) {
// ocp_nlp_dynamics_cont_opts* dynOpts = static_cast<ocp_nlp_dynamics_cont_opts*>(nlp_opts->dynamics[i]);
// ocp_nlp_dynamics_config* dynConfig = config->dynamics[i];
// dynConfig->sim_solver->opts_set(dynConfig->sim_solver, dynOpts->sim_solver, "sens_forw", &sens_forw);
// dynConfig->sim_solver->opts_set(dynConfig->sim_solver, dynOpts->sim_solver, "sens_hess", &sens_hess);
// dynConfig->sim_solver->opts_set(dynConfig->sim_solver, dynOpts->sim_solver, "sens_adj", &sens_adj);
// }
config->opts_update(config, dims, opts);
ocp_nlp_in* inputs = ocp_nlp_in_create(config, dims);
ocp_nlp_out* outputs = ocp_nlp_out_create(config, dims);
ocp_nlp_solver* solver = ocp_nlp_solver_create(config, dims, opts, inputs);
// Set sampling time
for (int i = 0; i < nHorizon; i++) {
ocp_nlp_in_set(config, dims, inputs, i, "Ts", &dt);
ocp_nlp_cost_model_set(config, dims, inputs, i, "scaling", &dt);
}
// --------------------------------------------------------------------------------------------
// Load Dynamics OCP
// --------------------------------------------------------------------------------------------
// Casadi Explicit Dynamics
external_function_casadi expl_ode_fun;
expl_ode_fun.casadi_fun = &quadruped_expl_ode_fun;
expl_ode_fun.casadi_work = &quadruped_expl_ode_fun_work;
expl_ode_fun.casadi_sparsity_in = &quadruped_expl_ode_fun_sparsity_in;
expl_ode_fun.casadi_sparsity_out = &quadruped_expl_ode_fun_sparsity_out;
expl_ode_fun.casadi_n_in = &quadruped_expl_ode_fun_n_in;
expl_ode_fun.casadi_n_out = &quadruped_expl_ode_fun_n_out;
external_function_casadi expl_vde_for;
expl_vde_for.casadi_fun = &quadruped_expl_vde_for;
expl_vde_for.casadi_work = &quadruped_expl_vde_for_work;
expl_vde_for.casadi_sparsity_in = &quadruped_expl_vde_for_sparsity_in;
expl_vde_for.casadi_sparsity_out = &quadruped_expl_vde_for_sparsity_out;
expl_vde_for.casadi_n_in = &quadruped_expl_vde_for_n_in;
expl_vde_for.casadi_n_out = &quadruped_expl_vde_for_n_out;
external_function_casadi expl_vde_adj;
expl_vde_adj.casadi_fun = &quadruped_expl_vde_adj;
expl_vde_adj.casadi_work = &quadruped_expl_vde_adj_work;
expl_vde_adj.casadi_sparsity_in = &quadruped_expl_vde_adj_sparsity_in;
expl_vde_adj.casadi_sparsity_out = &quadruped_expl_vde_adj_sparsity_out;
expl_vde_adj.casadi_n_in = &quadruped_expl_vde_adj_n_in;
expl_vde_adj.casadi_n_out = &quadruped_expl_vde_adj_n_out;
external_function_casadi expl_ode_hess;
expl_ode_hess.casadi_fun = &quadruped_expl_ode_hess;
expl_ode_hess.casadi_work = &quadruped_expl_ode_hess_work;
expl_ode_hess.casadi_sparsity_in = &quadruped_expl_ode_hess_sparsity_in;
expl_ode_hess.casadi_sparsity_out = &quadruped_expl_ode_hess_sparsity_out;
expl_ode_hess.casadi_n_in = &quadruped_expl_ode_hess_n_in;
expl_ode_hess.casadi_n_out = &quadruped_expl_ode_hess_n_out;
// Casadi Implicit Dynamics
external_function_casadi impl_dae_fun;
impl_dae_fun.casadi_fun = &quadruped_impl_dae_fun;
impl_dae_fun.casadi_work = &quadruped_impl_dae_fun_work;
impl_dae_fun.casadi_sparsity_in = &quadruped_impl_dae_fun_sparsity_in;
impl_dae_fun.casadi_sparsity_out = &quadruped_impl_dae_fun_sparsity_out;
impl_dae_fun.casadi_n_in = &quadruped_impl_dae_fun_n_in;
impl_dae_fun.casadi_n_out = &quadruped_impl_dae_fun_n_out;
external_function_casadi impl_dae_hess;
impl_dae_hess.casadi_fun = &quadruped_impl_dae_hess;
impl_dae_hess.casadi_work = &quadruped_impl_dae_hess_work;
impl_dae_hess.casadi_sparsity_in = &quadruped_impl_dae_hess_sparsity_in;
impl_dae_hess.casadi_sparsity_out = &quadruped_impl_dae_hess_sparsity_out;
impl_dae_hess.casadi_n_in = &quadruped_impl_dae_hess_n_in;
impl_dae_hess.casadi_n_out = &quadruped_impl_dae_hess_n_out;
external_function_casadi impl_dae_jac_x_xdot_u_z;
impl_dae_jac_x_xdot_u_z.casadi_fun = &quadruped_impl_dae_jac_x_xdot_u_z;
impl_dae_jac_x_xdot_u_z.casadi_work = &quadruped_impl_dae_jac_x_xdot_u_z_work;
impl_dae_jac_x_xdot_u_z.casadi_sparsity_in = &quadruped_impl_dae_jac_x_xdot_u_z_sparsity_in;
impl_dae_jac_x_xdot_u_z.casadi_sparsity_out = &quadruped_impl_dae_jac_x_xdot_u_z_sparsity_out;
impl_dae_jac_x_xdot_u_z.casadi_n_in = &quadruped_impl_dae_jac_x_xdot_u_z_n_in;
impl_dae_jac_x_xdot_u_z.casadi_n_out = &quadruped_impl_dae_jac_x_xdot_u_z_n_out;
external_function_casadi impl_dae_fun_jac_x_xdot_u;
impl_dae_fun_jac_x_xdot_u.casadi_fun = &quadruped_impl_dae_fun_jac_x_xdot_u;
impl_dae_fun_jac_x_xdot_u.casadi_work = &quadruped_impl_dae_fun_jac_x_xdot_u_work;
impl_dae_fun_jac_x_xdot_u.casadi_sparsity_in = &quadruped_impl_dae_fun_jac_x_xdot_u_sparsity_in;
impl_dae_fun_jac_x_xdot_u.casadi_sparsity_out = &quadruped_impl_dae_fun_jac_x_xdot_u_sparsity_out;
impl_dae_fun_jac_x_xdot_u.casadi_n_in = &quadruped_impl_dae_fun_jac_x_xdot_u_n_in;
impl_dae_fun_jac_x_xdot_u.casadi_n_out = &quadruped_impl_dae_fun_jac_x_xdot_u_n_out;
external_function_casadi impl_dae_fun_jac_x_xdot_z;
impl_dae_fun_jac_x_xdot_z.casadi_fun = &quadruped_impl_dae_fun_jac_x_xdot_z;
impl_dae_fun_jac_x_xdot_z.casadi_work = &quadruped_impl_dae_fun_jac_x_xdot_z_work;
impl_dae_fun_jac_x_xdot_z.casadi_sparsity_in = &quadruped_impl_dae_fun_jac_x_xdot_z_sparsity_in;
impl_dae_fun_jac_x_xdot_z.casadi_sparsity_out = &quadruped_impl_dae_fun_jac_x_xdot_z_sparsity_out;
impl_dae_fun_jac_x_xdot_z.casadi_n_in = &quadruped_impl_dae_fun_jac_x_xdot_z_n_in;
impl_dae_fun_jac_x_xdot_z.casadi_n_out = &quadruped_impl_dae_fun_jac_x_xdot_z_n_out;
external_function_opts ext_fun_opts;
external_function_opts_set_to_default(&ext_fun_opts);
ext_fun_opts.external_workspace = false;
external_function_casadi_create(&expl_ode_fun, &ext_fun_opts);
external_function_casadi_create(&expl_vde_for, &ext_fun_opts);
external_function_casadi_create(&expl_vde_adj, &ext_fun_opts);
external_function_casadi_create(&expl_ode_hess, &ext_fun_opts);
external_function_casadi_create(&impl_dae_fun, &ext_fun_opts);
external_function_casadi_create(&impl_dae_hess, &ext_fun_opts);
external_function_casadi_create(&impl_dae_jac_x_xdot_u_z, &ext_fun_opts);
external_function_casadi_create(&impl_dae_fun_jac_x_xdot_u, &ext_fun_opts);
external_function_casadi_create(&impl_dae_fun_jac_x_xdot_z, &ext_fun_opts);
for (int i = 0; i < nHorizon; i++) {
if (plan->sim_solver_plan[i].sim_solver == ERK) {
std::cout << "Setting ERK in stage: " << i << std::endl;
ocp_nlp_dynamics_model_set(config, dims, inputs, i, "expl_ode_fun", &expl_ode_fun);
ocp_nlp_dynamics_model_set(config, dims, inputs, i, "expl_vde_for", &expl_vde_for);
ocp_nlp_dynamics_model_set(config, dims, inputs, i, "expl_vde_adj", &expl_vde_adj);
ocp_nlp_dynamics_model_set(config, dims, inputs, i, "expl_ode_hess", &expl_ode_hess);
} else if(plan->sim_solver_plan[i].sim_solver == IRK) {
std::cout << "Setting IRK in stage: " << i << std::endl;
ocp_nlp_dynamics_model_set(config, dims, inputs, i, "impl_dae_fun", &impl_dae_fun);
ocp_nlp_dynamics_model_set(config, dims, inputs, i, "impl_dae_fun_jac_x_xdot_z", &impl_dae_fun_jac_x_xdot_z);
// ocp_nlp_dynamics_model_set(config, dims, inputs, i, "impl_dae_fun_jac_x_xdot_u", &impl_dae_fun_jac_x_xdot_u);
ocp_nlp_dynamics_model_set(config, dims, inputs, i, "impl_dae_jac_x_xdot_u_z", &impl_dae_jac_x_xdot_u_z);
ocp_nlp_dynamics_model_set(config, dims, inputs, i, "impl_dae_hess", &impl_dae_hess);
}
}
// --------------------------------------------------------------------------------------------
// Cost Function OCP
// --------------------------------------------------------------------------------------------
// Projection of the state (reference tracking first 12 values)
Eigen::Matrix<double, 36, 24> Vx = Eigen::Matrix<double, 36, 24>::Zero();
Vx.block<12, 12>(0, 0) = Eigen::Matrix<double, 12, 12>::Identity();
// Projection of the input (minimize all 24 values)
Eigen::Matrix<double, 36, 24> Vu = Eigen::Matrix<double, 36, 24>::Zero();
Vu.block<24, 24>(12, 0) = Eigen::Matrix<double, 24, 24>::Identity();
// Last stage is just the state so no input to minimize
Eigen::Matrix<double, 36, 24> Vuf = Eigen::Matrix<double, 36, 24>::Zero();
// Weight matrix
Eigen::Matrix<double, 36, 36> Q = Eigen::Matrix<double, 36, 36>::Zero();
Q.diagonal() = Eigen::Vector<double, 36>({
100, 100, 100, 100, 100, 100,
100, 100, 100, 100, 100, 100,
1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1});
// Reference, in this simplified case is cte and set to the starting value
Eigen::Vector<double, 36> yref = {
0, 0, 0, 0, 0, 0,
1e3, 10, 0.27, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0
};
for (std::size_t i = 0; i <= nHorizon; i++) {
ocp_nlp_cost_model_set(config, dims, inputs, i, "Vx", Vx.data());
ocp_nlp_cost_model_set(config, dims, inputs, i, "W", Q.data());
ocp_nlp_cost_model_set(config, dims, inputs, i, "y_ref", yref.data());
if (i == nHorizon) break;
ocp_nlp_cost_model_set(config, dims, inputs, i, "Vu", Vu.data());
}
ocp_nlp_cost_model_set(config, dims, inputs, nHorizon, "Vu", Vuf.data());
// --------------------------------------------------------------------------------------------
// Bounds and Constraints OCP
// --------------------------------------------------------------------------------------------
// Bounds on the state
Eigen::Vector<double, 24> ubx;
ubx <<
200, 200, 200,
200, 200, 200,
30, 30, 30,
30, 30, 30,
10, 10, 10,
10, 10, 10,
10, 10, 10,
10, 10, 10;
Eigen::Vector<double, 24> lbx;
lbx <<
-200, -200, -200,
-200, -200, -200,
-30, -30, 0,
-30, -30, -30,
-10, -10, -10,
-10, -10, -10,
-10, -10, -10,
-10, -10, -10;
// Bounds on the input
Eigen::Vector<double, 24> ubu;
ubu <<
200, 200, 200,
200, 200, 200,
200, 200, 200,
200, 200, 200,
10, 10, 10,
10, 10, 10,
10, 10, 10,
10, 10, 10;
Eigen::Vector<double, 24> lbu;
lbu <<
-200, -200, 0,
-200, -200, 0,
-200, -200, 0,
-200, -200, 0,
-10, -10, -10,
-10, -10, -10,
-10, -10, -10,
-10, -10, -10;
for (std::size_t stage = 0; stage < nHorizon; stage++) {
ocp_nlp_constraints_model_set(config, dims, inputs, stage, "ubx", ubx.data());
ocp_nlp_constraints_model_set(config, dims, inputs, stage, "lbx", lbx.data());
ocp_nlp_constraints_model_set(config, dims, inputs, stage, "ubu", ubu.data());
ocp_nlp_constraints_model_set(config, dims, inputs, stage, "lbu", lbu.data());
}
ocp_nlp_constraints_model_set(config, dims, inputs, nHorizon, "ubx", ubx.data());
ocp_nlp_constraints_model_set(config, dims, inputs, nHorizon, "lbx", lbx.data());
// Add linear constraints
// Inequality constraint f() < 0
Eigen::Matrix<double, 16, 48> C = frictionCone();
Eigen::Vector<double, 16> ug = Eigen::Vector<double, 16>::Zero();
Eigen::Vector<double, 16> lg = Eigen::Vector<double, 16>::Constant(-1e9);
for (std::size_t stage = 0; stage < nHorizon; stage++) {
ocp_nlp_constraints_model_set(config, dims, inputs, stage, "C", C.data());
ocp_nlp_constraints_model_set(config, dims, inputs, stage, "lg", lg.data());
ocp_nlp_constraints_model_set(config, dims, inputs, stage, "ug", ug.data());
}
// --------------------------------------------------------------------------------------------
// Initialize OCP
// --------------------------------------------------------------------------------------------
// Initialization
Eigen::Vector<double, 24> x = {0, 0, 0, 0, 0, 0, 0, 0, 0.27, 0, 0, 0, 0, 0.9, -1.8, 0, 0.9, -1.8, 0, 0.9, -1.8, 0, 0.9, -1.8};
Eigen::Vector<double, 24> u = {0, 0, 35, 0, 0, 29, 0, 0, 35, 0, 0, 35, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
for (int stage = 0; stage < nHorizon; stage++) {
ocp_nlp_out_set(config, dims, outputs, stage, "x", x.data());
ocp_nlp_out_set(config, dims, outputs, stage, "u", u.data());
}
ocp_nlp_out_set(config, dims, outputs, nHorizon, "x", x.data());
// ------------------------------------------------------------------------------------
// Solve QP
// ------------------------------------------------------------------------------------
int status = ocp_nlp_precompute(solver, inputs, outputs);
if (status != 0) {
std::cerr << "Pre-compute failed (" << status << ")" << std::endl;
return -1;
}
logger->debug("Solving QP");
status = ocp_nlp_solve(solver, inputs, outputs);
if (status != 0) {
std::cerr << "Solver failed (" << status << ")" << std::endl;
return -1;
}
// ------------------------------------------------------------------------------------
// Get Solution QP
// ------------------------------------------------------------------------------------
// Get first optimal output
Eigen::Vector<double, 24> u_opt;
Eigen::Vector<double, 24> x_opt;
ocp_nlp_out_get(config, dims, outputs, 0, "u", u_opt.data());
ocp_nlp_out_get(config, dims, outputs, 0, "x", x_opt.data());
std::cout << "State 0:\n" << x_opt << std::endl;
std::cout << "Input 0:\n" << u_opt << std::endl;
return 0;
}
Problem
When using the explicit dynamics together with the ERK the problem claims to have found the optimal solution:
# it stat eq ineq comp qp_stat qp_iter alpha
0 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0 0 1.000000e+00
Optimal Solution found! Converged to KKT point.
But instead this is just the provided initial guess which is not the optimal solution. I would expect it to either not converge or at least have some cost other than zero.
When examining the code with Valgrind I obtain the following error several times:
==844355== Invalid read of size 8
==844355== at 0x5395AD3: quadruped_expl_vde_for (in /my_path/my_repo/lib/libquadruped_model.so)
==844355== by 0x59E8F85: external_function_casadi_wrapper (external_function_generic.c:984)
==844355== by 0x59CEC92: sim_erk (sim_erk_integrator.c:749)
==844355== by 0x59A59B2: ocp_nlp_dynamics_cont_update_qp_matrices (ocp_nlp_dynamics_cont.c:809)
==844355== by 0x598854B: ocp_nlp_approximate_qp_matrices (ocp_nlp_common.c:2609)
==844355== by 0x59B91C2: ocp_nlp_sqp (ocp_nlp_sqp.c:620)
==844355== by 0x59F6E98: ocp_nlp_solve (ocp_nlp_interface.c:1271)
==844355== by 0x10EBA4: main (in /home/jplayang/Projects/VSCode/cpproboticframework/bin/SimulationCustomMPC2)
If I instead try with the implicit dynamics with IRK, I get a segmentation fault and the following when using a debugger such as GDB:
0x00007ffff65529fd in blasfeo_unpack_dvec (m=24, sa=0x7ffff567c3e0, ai=<optimized out>, x=0x0, xi=1) at /my_path/acados/external/blasfeo/auxiliary/d_aux_lib4.c:2502
2502 x[ii] = pa[ii];
(gdb) bt
#0 0x00007ffff65529fd in blasfeo_unpack_dvec (m=24, sa=0x7ffff567c3e0, ai=<optimized out>, x=0x0, xi=1) at /my_path/acados/external/blasfeo/auxiliary/d_aux_lib4.c:2502
#1 0x00007ffff6dcfbae in d_cvt_dvec_to_casadi (in=0x7ffff567c3e0, out=0x0, sparsity_out=0x7ffff7ef9140 <quadruped_impl_dae_jac_x_xdot_u_z_s0>, is_dense=1)
at /my_path/acados/acados/utils/external_function_generic.c:488
#2 0x00007ffff6dd0755 in d_cvt_ext_fun_arg_to_casadi (type=BLASFEO_DVEC, in=0x7ffff567c3e0, out=0x0, sparsity=0x7ffff7ef9140 <quadruped_impl_dae_jac_x_xdot_u_z_s0>, is_dense=1)
at /my_path/acados/acados/utils/external_function_generic.c:800
#3 0x00007ffff6dd0ef9 in external_function_casadi_wrapper (self=0x7fffffff8560, type_in=0x7fffffff6b70, in=0x7fffffff6c30, type_out=0x7fffffff6b60, out=0x7fffffff6c10)
at /my_path/acados/acados/utils/external_function_generic.c:974
#4 0x00007ffff6dc8bf2 in sim_irk (config_=0x555555575738, in=0x7ffff56723c8, out=0x7ffff5674b50, opts_=0x7ffff5b44be0, mem_=0x7ffff4e62808, work_=0x7ffff567bb70)
at /my_path/acados/acados/sim/sim_irk_integrator.c:1509
#5 0x00007ffff6d8d9b3 in ocp_nlp_dynamics_cont_update_qp_matrices (config_=0x5555555755e8, dims_=0x555555594a10, model_=0x7ffff5801020, opts_=0x7ffff5b44bd0, mem_=0x7ffff4e62768, work_=0x7ffff5672380)
at /my_path/acados/acados/ocp_nlp/ocp_nlp_dynamics_cont.c:809
#6 0x00007ffff6d7054c in ocp_nlp_approximate_qp_matrices (config=0x555555574f10, dims=0x5555555927f0, in=0x7ffff5800010, out=0x7ffff688a010, opts=0x7ffff5b44068, mem=0x7fffd6c00070, work=0x7ffff51e3100)
at /my_path/acados/acados/ocp_nlp/ocp_nlp_common.c:2609
#7 0x00007ffff6da11c3 in ocp_nlp_sqp (config_=0x555555574f10, dims_=0x5555555927f0, nlp_in_=0x7ffff5800010, nlp_out_=0x7ffff688a010, opts_=0x7ffff5b44010, mem_=0x7fffd6c00038, work_=0x7ffff51e30f8)
at /my_path/acados/acados/ocp_nlp/ocp_nlp_sqp.c:620
#8 0x00007ffff6ddee99 in ocp_nlp_solve (solver=0x7fffd6c00010, nlp_in=0x7ffff5800010, nlp_out=0x7ffff688a010) at /my_path/acados/interfaces/acados_c/ocp_nlp_interface.c:1271
#9 0x000055555555aba5 in main ()
Question
I was wondering if anyone has had similar issues working with the C API? And if so, how did you solve them or use the API correctly?
If someone could take a closer, maybe I forgot some call to some function, selected some incorrect options, or maybe derived the expressions incorrectly?
If needed I could also provide the dynamics in Casadi to generate the proper code and test it locally.
Thank you for your time and for any insight or idea on how to solve this,