From df4ed6d57cd3c68547c50518cb1406c150ad76e8 Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Tue, 8 Apr 2025 15:06:52 +0200 Subject: [PATCH 01/24] Consistent mask and multipliers (#1333) This PR addresses the issue that masked constraints and multipliers might be inconsistent: If a constraint is masked, the corresponding multiplier should always be zero. - The mask is updated whenever the constraint bounds are updated. - lambdas are multiplied with the mask whenever they or the bounds are updated `dmask` is now stored in `ocp_nlp_in` as the mask ist now required in `ocp_nlp_out_set` and `ocp_nlp_constraints_model_set`. BREAKING CHANGES in C interface: - `ocp_nlp_out_set` now requires `nlp_in` as additional argument - `ocp_nlp_constraints_model_set` now requires `nlp_out` as additional argument - `ocp_nlp_out` needs to be created before `ocp_nlp_in` --------- Co-authored-by: Jonathan Frey --- acados/dense_qp/dense_qp_ooqp.c | 6 +- acados/dense_qp/dense_qp_qore.c | 16 +- acados/dense_qp/dense_qp_qpoases.c | 16 +- acados/ocp_nlp/ocp_nlp_common.c | 123 ++++++------ acados/ocp_nlp/ocp_nlp_common.h | 4 +- acados/ocp_nlp/ocp_nlp_constraints_bgh.c | 143 +++++++++----- acados/ocp_nlp/ocp_nlp_constraints_bgh.h | 2 +- acados/ocp_nlp/ocp_nlp_constraints_bgp.c | 141 ++++++++------ acados/ocp_nlp/ocp_nlp_constraints_bgp.h | 4 +- acados/ocp_nlp/ocp_nlp_constraints_common.c | 2 +- acados/ocp_nlp/ocp_nlp_constraints_common.h | 2 +- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 4 +- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 2 +- .../tests/one_sided_constraints_test.py | 23 ++- examples/c/engine_example.c | 41 ++-- examples/c/nonlinear_chain_ocp_nlp.c | 44 +++-- examples/c/simple_dae_example.c | 17 +- examples/c/wind_turbine_nmpc.c | 64 +++--- examples/c/wind_turbine_nmpc_soft.c | 72 +++---- .../pendulum/acados_solver_pendulum_ode.c | 60 +++--- .../pendulum/main_pendulum_ode.c | 10 +- interfaces/acados_c/ocp_nlp_interface.c | 18 +- interfaces/acados_c/ocp_nlp_interface.h | 4 +- .../acados_template/acados_ocp_solver.py | 17 +- .../acados_template/acados_ocp_solver_pyx.pyx | 6 +- .../acados_template/acados_solver_common.pxd | 4 +- .../c_templates_tera/acados_multi_solver.in.c | 181 ++++++++--------- .../c_templates_tera/acados_solver.in.c | 182 +++++++++--------- .../c_templates_tera/main.in.c | 10 +- .../c_templates_tera/main_multi.in.c | 6 +- .../matlab_templates/acados_mex_set.in.c | 26 +-- .../matlab_templates/acados_solver_sfun.in.c | 32 +-- .../custom_update_function_zoro_template.in.c | 32 +-- test/ocp_nlp/test_wind_turbine.cpp | 61 +++--- 34 files changed, 754 insertions(+), 621 deletions(-) diff --git a/acados/dense_qp/dense_qp_ooqp.c b/acados/dense_qp/dense_qp_ooqp.c index feafc10573..b25497a946 100644 --- a/acados/dense_qp/dense_qp_ooqp.c +++ b/acados/dense_qp/dense_qp_ooqp.c @@ -40,6 +40,7 @@ // blasfeo #include "blasfeo_d_aux.h" #include "blasfeo_d_aux_ext_dep.h" +#include "blasfeo_d_blas.h" // acados #include "acados/dense_qp/dense_qp_ooqp.h" @@ -601,8 +602,11 @@ int_t dense_qp_ooqp(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, voi acados_tic(&interface_timer); fill_in_qp_out(qp_in, qp_out, work); dense_qp_compute_t(qp_in, qp_out); - info->interface_time += acados_toc(&interface_timer); + // multiply with mask to ensure that multipliers associated with masked constraints are zero + blasfeo_dvecmul(2*(qp_in->dim->nb + qp_in->dim->ng + qp_in->dim->ns), qp_in->d_mask, 0, qp_out->lam, 0, qp_out->lam, 0); + + info->interface_time += acados_toc(&interface_timer); info->total_time = acados_toc(&tot_timer); info->num_iter = -1; info->t_computed = 1; diff --git a/acados/dense_qp/dense_qp_qore.c b/acados/dense_qp/dense_qp_qore.c index 3e37a68e5f..3da2c73909 100644 --- a/acados/dense_qp/dense_qp_qore.c +++ b/acados/dense_qp/dense_qp_qore.c @@ -36,6 +36,7 @@ // blasfeo #include "blasfeo_d_aux.h" #include "blasfeo_d_aux_ext_dep.h" +#include "blasfeo_d_blas.h" // acados #include "acados/dense_qp/dense_qp_common.h" #include "acados/dense_qp/dense_qp_qore.h" @@ -537,12 +538,8 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void qp_out->lam->pa[2*nb + 2*ng + ns + ii] = dual_sol[nv + ns + ii] - offset_l; } - info->interface_time += acados_toc(&interface_timer); - info->total_time = acados_toc(&tot_timer); - info->num_iter = num_iter; - - mem->time_qp_solver_call = info->solve_QP_time; - mem->iter = num_iter; + // multiply with mask to ensure that multipliers associated with masked constraints are zero + blasfeo_dvecmul(2*(qp_in->dim->nb + qp_in->dim->ng + qp_in->dim->ns), qp_in->d_mask, 0, qp_out->lam, 0, qp_out->lam, 0); // compute slacks if (opts->compute_t) @@ -551,6 +548,13 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void info->t_computed = 1; } + info->interface_time += acados_toc(&interface_timer); + info->total_time = acados_toc(&tot_timer); + info->num_iter = num_iter; + + mem->time_qp_solver_call = info->solve_QP_time; + mem->iter = num_iter; + int acados_status = qore_status; if (qore_status == QPSOLVER_DENSE_OPTIMAL) acados_status = ACADOS_SUCCESS; if (qore_status == QPSOLVER_DENSE_ITER_LIMIT) acados_status = ACADOS_MAXITER; diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index 36cc6fa30f..f7ac8355c4 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -742,6 +742,16 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo qp_out->lam->pa[2*nb + 2*ng + ns + ii] = dual_sol[nv + ns + ii] - offset_l; } + // compute slacks + if (opts->compute_t) + { + dense_qp_compute_t(qp_in, qp_out); + info->t_computed = 1; + } + + // multiply with mask to ensure that multipliers associated with masked constraints are zero + blasfeo_dvecmul(2*(qp_in->dim->nb + qp_in->dim->ng + qp_in->dim->ns), qp_in->d_mask, 0, qp_out->lam, 0, qp_out->lam, 0); + info->interface_time += acados_toc(&interface_timer); info->total_time = acados_toc(&tot_timer); info->num_iter = nwsr; @@ -749,12 +759,6 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo memory->time_qp_solver_call = info->solve_QP_time; memory->iter = nwsr; - // compute slacks - if (opts->compute_t) - { - dense_qp_compute_t(qp_in, qp_out); - info->t_computed = 1; - } int acados_status = qpoases_status; if (qpoases_status == SUCCESSFUL_RETURN) acados_status = ACADOS_SUCCESS; diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 1ffbce8f55..65d6636454 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -779,17 +779,21 @@ void ocp_nlp_dims_set_dynamics(void *config_, void *dims_, int stage, * in ************************************************/ -static acados_size_t ocp_nlp_in_calculate_size_self(ocp_nlp_dims *dims) +acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims) { int N = dims->N; + int i; + acados_size_t size = sizeof(ocp_nlp_in); size += N * sizeof(double); // Ts + // parameter values - for (int i = 0; i <= N; i++) + for (i = 0; i <= N; i++) { size += dims->np[i] * sizeof(double); } + // global_data size += dims->n_global_data * sizeof(double); @@ -801,38 +805,34 @@ static acados_size_t ocp_nlp_in_calculate_size_self(ocp_nlp_dims *dims) size += (N + 1) * sizeof(void *); // constraints - size += 4*8; // aligns - return size; -} - + size += (N + 1) * sizeof(struct blasfeo_dvec); // dmask - -acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims) -{ - int N = dims->N; - - acados_size_t size = ocp_nlp_in_calculate_size_self(dims); + for (i = 0; i <= N; i++) + { + size += blasfeo_memsize_dvec(2*dims->ni[i]); // dmask + } // dynamics - for (int i = 0; i < N; i++) + for (i = 0; i < N; i++) { - size += - config->dynamics[i]->model_calculate_size(config->dynamics[i], dims->dynamics[i]); + size += config->dynamics[i]->model_calculate_size(config->dynamics[i], dims->dynamics[i]); } // cost - for (int i = 0; i <= N; i++) + for (i = 0; i <= N; i++) { size += config->cost[i]->model_calculate_size(config->cost[i], dims->cost[i]); } // constraints - for (int i = 0; i <= N; i++) + for (i = 0; i <= N; i++) { size += config->constraints[i]->model_calculate_size(config->constraints[i], - dims->constraints[i]); + dims->constraints[i]); } + size += 4*8 + 64; // aligns + make_int_multiple_of(8, &size); return size; @@ -840,9 +840,10 @@ acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *di -static ocp_nlp_in *ocp_nlp_in_assign_self(ocp_nlp_dims *dims, void *raw_memory) +ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory) { int N = dims->N; + char *c_ptr = (char *) raw_memory; // initial align @@ -852,30 +853,7 @@ static ocp_nlp_in *ocp_nlp_in_assign_self(ocp_nlp_dims *dims, void *raw_memory) ocp_nlp_in *in = (ocp_nlp_in *) c_ptr; c_ptr += sizeof(ocp_nlp_in); - // align - align_char_to(8, &c_ptr); - - // double pointers - assign_and_advance_double_ptrs(N+1, &in->parameter_values, &c_ptr); - - align_char_to(8, &c_ptr); - - // doubles - // Ts - assign_and_advance_double(N, &in->Ts, &c_ptr); - - // parameter values - for (int i = 0; i <= N; i++) - { - assign_and_advance_double(dims->np[i], &in->parameter_values[i], &c_ptr); - for (int ip = 0; ip < dims->np[i]; ip++) - { - in->parameter_values[i][ip] = 0.0; - } - } - assign_and_advance_double(dims->n_global_data, &in->global_data, &c_ptr); - - + // ** pointers to substructures ** // dynamics in->dynamics = (void **) c_ptr; c_ptr += N * sizeof(void *); @@ -888,24 +866,13 @@ static ocp_nlp_in *ocp_nlp_in_assign_self(ocp_nlp_dims *dims, void *raw_memory) in->constraints = (void **) c_ptr; c_ptr += (N + 1) * sizeof(void *); + // align align_char_to(8, &c_ptr); - assert((char *) raw_memory + ocp_nlp_in_calculate_size_self(dims) >= c_ptr); - - return in; -} - - - -ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory) -{ - int N = dims->N; - - char *c_ptr = (char *) raw_memory; + // ** substructures ** - // struct - ocp_nlp_in *in = ocp_nlp_in_assign_self(dims, c_ptr); - c_ptr += ocp_nlp_in_calculate_size_self(dims); + // dmask + assign_and_advance_blasfeo_dvec_structs(N + 1, &in->dmask, &c_ptr); // dynamics for (int i = 0; i < N; i++) @@ -932,8 +899,44 @@ ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void * dims->constraints[i]); } + // ** doubles ** + // Ts + assign_and_advance_double(N, &in->Ts, &c_ptr); + + // double pointers + assign_and_advance_double_ptrs(N+1, &in->parameter_values, &c_ptr); + align_char_to(8, &c_ptr); + + // parameter values + for (int i = 0; i <= N; i++) + { + assign_and_advance_double(dims->np[i], &in->parameter_values[i], &c_ptr); + for (int ip = 0; ip < dims->np[i]; ip++) + { + in->parameter_values[i][ip] = 0.0; + } + } + assign_and_advance_double(dims->n_global_data, &in->global_data, &c_ptr); + + // blasfeo_mem align + align_char_to(64, &c_ptr); + + // dmask + for (int i = 0; i <= N; ++i) + { + assign_and_advance_blasfeo_dvec_mem(2 * dims->ni[i], in->dmask + i, &c_ptr); + } + + align_char_to(8, &c_ptr); + assert((char *) raw_memory + ocp_nlp_in_calculate_size(config, dims) >= c_ptr); + for (int i = 0; i <= N; i++) + { + blasfeo_dvecse(2*dims->ni[i], 1.0, &in->dmask[i], 0); + config->constraints[i]->model_set_dmask_ptr(&in->dmask[i], in->constraints[i]); + } + return in; } @@ -2610,7 +2613,6 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di config->constraints[i]->memory_set_idxb_ptr(nlp_mem->qp_in->idxb[i], nlp_mem->constraints[i]); config->constraints[i]->memory_set_idxs_rev_ptr(nlp_mem->qp_in->idxs_rev[i], nlp_mem->constraints[i]); config->constraints[i]->memory_set_idxe_ptr(nlp_mem->qp_in->idxe[i], nlp_mem->constraints[i]); - config->constraints[i]->memory_set_dmask_ptr(nlp_mem->qp_in->d_mask+i, nlp_mem->constraints[i]); if (opts->with_solution_sens_wrt_params) { config->constraints[i]->memory_set_jac_lag_stat_p_global_ptr(nlp_mem->jac_lag_stat_p_global+i, nlp_mem->constraints[i]); @@ -2618,6 +2620,9 @@ void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *di } } + // set pointer to dmask in qp_in to dmask in nlp_in + nlp_mem->qp_in->d_mask = nlp_in->dmask; + // alias to regularize memory ocp_nlp_regularize_set_qp_in_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, nlp_mem->qp_in); ocp_nlp_regularize_set_qp_out_ptrs(config->regularize, dims->regularize, nlp_mem->regularize, nlp_mem->qp_out); diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index 669c890d1f..f1a90e5a36 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -66,7 +66,6 @@ extern "C" { #include "acados/utils/types.h" - /************************************************ * config ************************************************/ @@ -233,6 +232,9 @@ typedef struct ocp_nlp_in /// Global data double *global_data; + /// Constraint mask + struct blasfeo_dvec *dmask; + /// Pointers to cost functions (TBC). void **cost; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c index 21ba85761f..0f3425a3b0 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c @@ -376,6 +376,29 @@ void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims_, void *raw_ } +void ocp_nlp_constraints_bgh_update_mask_lower(ocp_nlp_constraints_bgh_model *model, int size, int offset) +{ + for (int ii = 0; ii < size; ii++) + { + if (BLASFEO_DVECEL(&model->d, offset + ii) <= -ACADOS_INFTY) + BLASFEO_DVECEL(model->dmask, offset + ii) = 0; + else + BLASFEO_DVECEL(model->dmask, offset + ii) = 1; + } +} + + +void ocp_nlp_constraints_bgh_update_mask_upper(ocp_nlp_constraints_bgh_model *model, int size, int offset) +{ + for (int ii = 0; ii < size; ii++) + { + if (BLASFEO_DVECEL(&model->d, offset + ii) >= ACADOS_INFTY) + BLASFEO_DVECEL(model->dmask, offset + ii) = 0; + else + BLASFEO_DVECEL(model->dmask, offset + ii) = 1; + } +} + int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, void *model_, const char *field, void *value) @@ -385,10 +408,11 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, int ii; int *ptr_i; + int offset; if (!dims || !model || !field || !value) { - printf("ocp_nlp_constraints_bgh_model_set: got Null pointer \n"); + printf("ocp_nlp_constraints_bgh_model_set: got null pointer \n"); exit(1); } @@ -409,6 +433,7 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, int nge = dims->nge; int nhe = dims->nhe; + // If model->d is updated, we always also update dmask. 0 means unconstrained. if (!strcmp(field, "idxbx")) { ptr_i = (int *) value; @@ -417,11 +442,15 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lbx")) { - blasfeo_pack_dvec(nbx, value, 1, &model->d, nbu); + offset = nbu; + blasfeo_pack_dvec(nbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nbx, offset); } else if (!strcmp(field, "ubx")) { - blasfeo_pack_dvec(nbx, value, 1, &model->d, nb + ng + nh + nbu); + offset = nb + ng + nh + nbu; + blasfeo_pack_dvec(nbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_upper(model, nbx, offset); } else if (!strcmp(field, "idxbu")) { @@ -431,11 +460,16 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lbu")) { - blasfeo_pack_dvec(nbu, value, 1, &model->d, 0); + offset = 0; + blasfeo_pack_dvec(nbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nbu, offset); + } else if (!strcmp(field, "ubu")) { - blasfeo_pack_dvec(nbu, value, 1, &model->d, nb + ng + nh); + offset = nb + ng + nh; + blasfeo_pack_dvec(nbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_upper(model, nbu, offset); } else if (!strcmp(field, "C")) { @@ -447,11 +481,17 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lg")) { - blasfeo_pack_dvec(ng, value, 1, &model->d, nb); + offset = nb; + blasfeo_pack_dvec(ng, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, ng, offset); + } else if (!strcmp(field, "ug")) { - blasfeo_pack_dvec(ng, value, 1, &model->d, 2*nb+ng+nh); + offset = 2*nb+ng+nh; + blasfeo_pack_dvec(ng, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_upper(model, ng, offset); + } else if (!strcmp(field, "nl_constr_h_fun")) { @@ -475,11 +515,15 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lh")) { - blasfeo_pack_dvec(nh, value, 1, &model->d, nb+ng); + offset = nb+ng; + blasfeo_pack_dvec(nh, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nh, offset); } else if (!strcmp(field, "uh")) { - blasfeo_pack_dvec(nh, value, 1, &model->d, 2*nb+2*ng+nh); + offset = 2*nb+2*ng+nh; + blasfeo_pack_dvec(nh, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_upper(model, nh, offset); } else if (!strcmp(field, "idxsbu")) { @@ -489,11 +533,15 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsbu")) { - blasfeo_pack_dvec(nsbu, value, 1, &model->d, 2*nb+2*ng+2*nh); + offset = 2*nb+2*ng+2*nh; + blasfeo_pack_dvec(nsbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsbu, offset); } else if (!strcmp(field, "usbu")) { - blasfeo_pack_dvec(nsbu, value, 1, &model->d, 2*nb+2*ng+2*nh+ns); + offset = 2*nb+2*ng+2*nh+ns; + blasfeo_pack_dvec(nsbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsbu, offset); } else if (!strcmp(field, "idxsbx")) { @@ -503,11 +551,16 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsbx")) { - blasfeo_pack_dvec(nsbx, value, 1, &model->d, 2*nb+2*ng+2*nh+nsbu); + offset = 2*nb+2*ng+2*nh+nsbu; + blasfeo_pack_dvec(nsbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsbx, offset); } else if (!strcmp(field, "usbx")) { - blasfeo_pack_dvec(nsbx, value, 1, &model->d, 2*nb+2*ng+2*nh+ns+nsbu); + offset = 2*nb+2*ng+2*nh+ns+nsbu; + blasfeo_pack_dvec(nsbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsbx, offset); + } else if (!strcmp(field, "idxsg")) { @@ -517,11 +570,15 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsg")) { - blasfeo_pack_dvec(nsg, value, 1, &model->d, 2*nb+2*ng+2*nh+nsbu+nsbx); + offset = 2*nb+2*ng+2*nh+nsbu+nsbx; + blasfeo_pack_dvec(nsg, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsg, offset); } else if (!strcmp(field, "usg")) { - blasfeo_pack_dvec(nsg, value, 1, &model->d, 2*nb+2*ng+2*nh+ns+nsbu+nsbx); + offset = 2*nb+2*ng+2*nh+ns+nsbu+nsbx; + blasfeo_pack_dvec(nsg, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsg, offset); } else if (!strcmp(field, "idxsh")) { @@ -531,11 +588,16 @@ int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsh")) { - blasfeo_pack_dvec(nsh, value, 1, &model->d, 2*nb+2*ng+2*nh+nsbu+nsbx+nsg); + offset = 2*nb+2*ng+2*nh+nsbu+nsbx+nsg; + blasfeo_pack_dvec(nsh, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsh, offset); + } else if (!strcmp(field, "ush")) { - blasfeo_pack_dvec(nsh, value, 1, &model->d, 2*nb+2*ng+2*nh+ns+nsbu+nsbx+nsg); + offset = 2*nb+2*ng+2*nh+ns+nsbu+nsbx+nsg; + blasfeo_pack_dvec(nsh, value, 1, &model->d, offset); + ocp_nlp_constraints_bgh_update_mask_lower(model, nsh, offset); } else if (!strcmp(field, "idxbue")) { @@ -837,6 +899,14 @@ void *ocp_nlp_constraints_bgh_memory_assign(void *config_, void *dims_, void *op } +void ocp_nlp_constraints_bgh_model_set_dmask_ptr(struct blasfeo_dvec *dmask, void *model_) +{ + ocp_nlp_constraints_bgh_model *model = model_; + + model->dmask = dmask; +} + + struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_fun_ptr(void *memory_) { @@ -902,13 +972,6 @@ void ocp_nlp_constraints_bgh_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, vo memory->z_alg = z_alg; } -void ocp_nlp_constraints_bgh_memory_set_dmask_ptr(struct blasfeo_dvec *dmask, void *memory_) -{ - ocp_nlp_constraints_bgh_memory *memory = memory_; - - memory->dmask = dmask; -} - void ocp_nlp_constraints_bgh_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_) { @@ -1418,7 +1481,7 @@ void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims_, void *model blasfeo_daxpy(2*ns, -1.0, ux, nu+nx, &model->d, 2*nb+2*ng+2*nh, &memory->fun, 2*nb+2*ng+2*nh); // fun = fun * mask - blasfeo_dvecmul(2*(nb+ng+nh+ns), memory->dmask, 0, &memory->fun, 0, &memory->fun, 0); + blasfeo_dvecmul(2*(nb+ng+nh+ns), model->dmask, 0, &memory->fun, 0, &memory->fun, 0); return; } @@ -1458,34 +1521,8 @@ void ocp_nlp_constraints_bgh_update_qp_vectors(void *config_, void *dims_, void // fun[2*ni : 2*(ni+ns)] = - slack + slack_bounds blasfeo_daxpy(2*ns, -1.0, memory->ux, nu+nx, &model->d, 2*nb+2*ng+2*nh, &memory->fun, 2*nb+2*ng+2*nh); - // Set dmask for QP: 0 means unconstrained. - for (int i = 0; i < nb+ng+nh; i++) - { - if (BLASFEO_DVECEL(&model->d, i) <= -ACADOS_INFTY) - { - // printf("found upper infinity bound\n"); - BLASFEO_DVECEL(memory->dmask, i) = 0; - } - } - for (int i = nb+ng+nh; i < 2*(nb+ng+nh); i++) - { - if (BLASFEO_DVECEL(&model->d, i) >= ACADOS_INFTY) - { - // printf("found upper infinity bound\n"); - BLASFEO_DVECEL(memory->dmask, i) = 0; - } - } - for (int i = 2*(nb+ng+nh); i < 2*(nb+ng+nh+ns); i++) - { - if (BLASFEO_DVECEL(&model->d, i) <= -ACADOS_INFTY) - { - // printf("found lower infinity bound on slacks\n"); - BLASFEO_DVECEL(memory->dmask, i) = 0; - } - } - // fun = fun * mask - blasfeo_dvecmul(2*(nb+ng+nh+ns), memory->dmask, 0, &memory->fun, 0, &memory->fun, 0); + blasfeo_dvecmul(2*(nb+ng+nh+ns), model->dmask, 0, &memory->fun, 0, &memory->fun, 0); return; } @@ -1677,6 +1714,7 @@ void ocp_nlp_constraints_bgh_config_initialize_default(void *config_, int stage) config->model_assign = &ocp_nlp_constraints_bgh_model_assign; config->model_set = &ocp_nlp_constraints_bgh_model_set; config->model_get = &ocp_nlp_constraints_bgh_model_get; + config->model_set_dmask_ptr = &ocp_nlp_constraints_bgh_model_set_dmask_ptr; config->opts_calculate_size = &ocp_nlp_constraints_bgh_opts_calculate_size; config->opts_assign = &ocp_nlp_constraints_bgh_opts_assign; config->opts_initialize_default = &ocp_nlp_constraints_bgh_opts_initialize_default; @@ -1691,7 +1729,6 @@ void ocp_nlp_constraints_bgh_config_initialize_default(void *config_, int stage) config->memory_set_DCt_ptr = &ocp_nlp_constraints_bgh_memory_set_DCt_ptr; config->memory_set_RSQrq_ptr = &ocp_nlp_constraints_bgh_memory_set_RSQrq_ptr; config->memory_set_z_alg_ptr = &ocp_nlp_constraints_bgh_memory_set_z_alg_ptr; - config->memory_set_dmask_ptr = &ocp_nlp_constraints_bgh_memory_set_dmask_ptr; config->memory_set_dzdux_tran_ptr = &ocp_nlp_constraints_bgh_memory_set_dzduxt_ptr; config->memory_set_idxb_ptr = &ocp_nlp_constraints_bgh_memory_set_idxb_ptr; config->memory_set_idxs_rev_ptr = &ocp_nlp_constraints_bgh_memory_set_idxs_rev_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.h b/acados/ocp_nlp/ocp_nlp_constraints_bgh.h index 951b1e3db7..e3241fbd55 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.h @@ -95,6 +95,7 @@ typedef struct int *idxb; int *idxs; int *idxe; + struct blasfeo_dvec *dmask; // pointer to dmask in ocp_nlp_in struct blasfeo_dvec d; // gathers bounds struct blasfeo_dmat DCt; // general linear constraint matrix // lg <= [D, C] * [u; x] <= ug @@ -156,7 +157,6 @@ typedef struct struct blasfeo_dvec *ux; // pointer to ux in nlp_out struct blasfeo_dvec *lam; // pointer to lam in nlp_out struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory - struct blasfeo_dvec *dmask; // pointer to dmask in ocp_nlp memory struct blasfeo_dmat *DCt; // pointer to DCt in qp_in struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c index e08cc6d068..9516da69c3 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgp.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgp.c @@ -381,6 +381,31 @@ void *ocp_nlp_constraints_bgp_model_assign(void *config, void *dims_, void *raw_ } +void ocp_nlp_constraints_bgp_update_mask_lower(ocp_nlp_constraints_bgp_model *model, int size, int offset) +{ + for (int ii = 0; ii < size; ii++) + { + if (BLASFEO_DVECEL(&model->d, offset + ii) <= -ACADOS_INFTY) + BLASFEO_DVECEL(model->dmask, offset + ii) = 0; + else + BLASFEO_DVECEL(model->dmask, offset + ii) = 1; + } +} + + +void ocp_nlp_constraints_bgp_update_mask_upper(ocp_nlp_constraints_bgp_model *model, int size, int offset) +{ + for (int ii = 0; ii < size; ii++) + { + if (BLASFEO_DVECEL(&model->d, offset + ii) >= ACADOS_INFTY) + BLASFEO_DVECEL(model->dmask, offset + ii) = 0; + else + BLASFEO_DVECEL(model->dmask, offset + ii) = 1; + } +} + + + int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, void *model_, const char *field, void *value) { @@ -390,10 +415,11 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, int ii; int *ptr_i; + int offset; if (!dims || !model || !field || !value) { - printf("ocp_nlp_constraints_bgp_model_set: got Null pointer \n"); + printf("ocp_nlp_constraints_bgp_model_set: got null pointer \n"); exit(1); } @@ -414,6 +440,7 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, int nge = dims->nge; int nphie = dims->nphie; + // If model->d is updated, we always also update dmask. 0 means unconstrained. if (!strcmp(field, "idxbx")) { ptr_i = (int *) value; @@ -422,11 +449,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lbx")) { - blasfeo_pack_dvec(nbx, value, 1, &model->d, nbu); + offset = nbu; + blasfeo_pack_dvec(nbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nbx, offset); } else if (!strcmp(field, "ubx")) { - blasfeo_pack_dvec(nbx, value, 1, &model->d, nb + ng + nphi + nbu); + offset = nb + ng + nphi + nbu; + blasfeo_pack_dvec(nbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_upper(model, nbx, offset); } else if (!strcmp(field, "idxbu")) { @@ -436,11 +467,16 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lbu")) { + offset = 0; blasfeo_pack_dvec(nbu, value, 1, &model->d, 0); + ocp_nlp_constraints_bgp_update_mask_lower(model, nbu, offset); + } else if (!strcmp(field, "ubu")) { - blasfeo_pack_dvec(nbu, value, 1, &model->d, nb + ng + nphi); + offset = nb + ng + nphi; + blasfeo_pack_dvec(nbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_upper(model, nbu, offset); } else if (!strcmp(field, "C")) { @@ -452,11 +488,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lg")) { - blasfeo_pack_dvec(ng, value, 1, &model->d, nb); + offset = nb; + blasfeo_pack_dvec(ng, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, ng, offset); } else if (!strcmp(field, "ug")) { - blasfeo_pack_dvec(ng, value, 1, &model->d, 2*nb+ng+nphi); + offset = 2*nb+ng+nphi; + blasfeo_pack_dvec(ng, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_upper(model, ng, offset); } else if (!strcmp(field, "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux")) { @@ -468,11 +508,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lphi")) { - blasfeo_pack_dvec(nphi, value, 1, &model->d, nb+ng); + offset = nb + ng; + blasfeo_pack_dvec(nphi, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nphi, offset); } else if (!strcmp(field, "uphi")) { - blasfeo_pack_dvec(nphi, value, 1, &model->d, 2*nb+2*ng+nphi); + offset = 2*nb+2*ng+nphi; + blasfeo_pack_dvec(nphi, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_upper(model, nphi, offset); } else if (!strcmp(field, "idxsbu")) { @@ -482,11 +526,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsbu")) { - blasfeo_pack_dvec(nsbu, value, 1, &model->d, 2*nb+2*ng+2*nphi); + offset = 2*nb+2*ng+2*nphi; + blasfeo_pack_dvec(nsbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsbu, offset); } else if (!strcmp(field, "usbu")) { - blasfeo_pack_dvec(nsbu, value, 1, &model->d, 2*nb+2*ng+2*nphi+ns); + offset = 2*nb+2*ng+2*nphi+ns; + blasfeo_pack_dvec(nsbu, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsbu, offset); } else if (!strcmp(field, "idxsbx")) { @@ -496,11 +544,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsbx")) { - blasfeo_pack_dvec(nsbx, value, 1, &model->d, 2*nb+2*ng+2*nphi+nsbu); + offset = 2*nb+2*ng+2*nphi+nsbu; + blasfeo_pack_dvec(nsbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsbx, offset); } else if (!strcmp(field, "usbx")) { - blasfeo_pack_dvec(nsbx, value, 1, &model->d, 2*nb+2*ng+2*nphi+ns+nsbu); + offset = 2*nb+2*ng+2*nphi+ns+nsbu; + blasfeo_pack_dvec(nsbx, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsbx, offset); } else if (!strcmp(field, "idxsg")) { @@ -510,11 +562,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsg")) { - blasfeo_pack_dvec(nsg, value, 1, &model->d, 2*nb+2*ng+2*nphi+nsbu+nsbx); + offset = 2*nb+2*ng+2*nphi+nsbu+nsbx; + blasfeo_pack_dvec(nsg, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsg, offset); } else if (!strcmp(field, "usg")) { - blasfeo_pack_dvec(nsg, value, 1, &model->d, 2*nb+2*ng+2*nphi+ns+nsbu+nsbx); + offset = 2*nb+2*ng+2*nphi+ns+nsbu+nsbx; + blasfeo_pack_dvec(nsg, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsg, offset); } else if (!strcmp(field, "idxsphi")) { @@ -524,11 +580,15 @@ int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, } else if (!strcmp(field, "lsphi")) { - blasfeo_pack_dvec(nsphi, value, 1, &model->d, 2*nb+2*ng+2*nphi+nsbu+nsbx+nsg); + offset = 2*nb+2*ng+2*nphi+nsbu+nsbx+nsg; + blasfeo_pack_dvec(nsphi, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsphi, offset); } else if (!strcmp(field, "usphi")) { - blasfeo_pack_dvec(nsphi, value, 1, &model->d, 2*nb+2*ng+2*nphi+ns+nsbu+nsbx+nsg); + offset = 2*nb+2*ng+2*nphi+ns+nsbu+nsbx+nsg; + blasfeo_pack_dvec(nsphi, value, 1, &model->d, offset); + ocp_nlp_constraints_bgp_update_mask_lower(model, nsphi, offset); } else if (!strcmp(field, "idxbue")) { @@ -820,6 +880,12 @@ void *ocp_nlp_constraints_bgp_memory_assign(void *config_, void *dims_, void *op } +void ocp_nlp_constraints_bgp_model_set_dmask_ptr(struct blasfeo_dvec *dmask, void *model_) +{ + ocp_nlp_constraints_bgp_model *model = model_; + model->dmask = dmask; +} + struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_fun_ptr(void *memory_) { @@ -865,13 +931,6 @@ void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void * } -void ocp_nlp_constraints_bgp_memory_set_dmask_ptr(struct blasfeo_dvec *dmask, void *memory_) -{ - ocp_nlp_constraints_bgp_memory *memory = memory_; - memory->dmask = dmask; -} - - void ocp_nlp_constraints_bgp_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory_) { ocp_nlp_constraints_bgp_memory *memory = memory_; @@ -1312,7 +1371,7 @@ void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims_, void *model blasfeo_daxpy(2*ns, -1.0, ux, nu+nx, &model->d, 2*nb+2*ng+2*nphi, &memory->fun, 2*nb+2*ng+2*nphi); // fun = fun * mask - blasfeo_dvecmul(2*(nb+ng+nphi), memory->dmask, 0, &memory->fun, 0, &memory->fun, 0); + blasfeo_dvecmul(2*(nb+ng+nphi), model->dmask, 0, &memory->fun, 0, &memory->fun, 0); return; @@ -1355,38 +1414,8 @@ void ocp_nlp_constraints_bgp_update_qp_vectors(void *config_, void *dims_, void // fun[2*ni : 2*(ni+ns)] = - slack + slack_bounds blasfeo_daxpy(2*ns, -1.0, memory->ux, nu+nx, &model->d, 2*nb+2*ng+2*nphi, &memory->fun, 2*nb+2*ng+2*nphi); - // Set dmask for QP: 0 means unconstrained. - for (int i = 0; i < nb+ng+nphi; i++) - { - if (BLASFEO_DVECEL(&model->d, i) <= -ACADOS_INFTY) - { - // printf("found upper infinity bound\n"); - BLASFEO_DVECEL(memory->dmask, i) = 0; - } - } - for (int i = nb+ng+nphi; i < 2*(nb+ng+nphi); i++) - { - if (BLASFEO_DVECEL(&model->d, i) >= ACADOS_INFTY) - { - // printf("found upper infinity bound\n"); - BLASFEO_DVECEL(memory->dmask, i) = 0; - } - } - for (int i = 2*(nb+ng+nphi); i < 2*(nb+ng+nphi+ns); i++) - { - if (BLASFEO_DVECEL(&model->d, i) <= -ACADOS_INFTY) - { - // printf("found lower infinity bound on slacks\n"); - BLASFEO_DVECEL(memory->dmask, i) = 0; - } - } - // fun = fun * mask - blasfeo_dvecmul(2*(nb+ng+nphi+ns), memory->dmask, 0, &memory->fun, 0, &memory->fun, 0); - - // printf("bgp mask\n"); - // blasfeo_print_tran_dvec(2*(nb+ng+nphi), memory->dmask, 0); - // blasfeo_print_exp_tran_dvec(2*(nb+ng+nphi), &model->d, 0); + blasfeo_dvecmul(2*(nb+ng+nphi+ns), model->dmask, 0, &memory->fun, 0, &memory->fun, 0); return; } @@ -1445,6 +1474,7 @@ void ocp_nlp_constraints_bgp_config_initialize_default(void *config_, int stage) config->model_assign = &ocp_nlp_constraints_bgp_model_assign; config->model_set = &ocp_nlp_constraints_bgp_model_set; config->model_get = &ocp_nlp_constraints_bgp_model_get; + config->model_set_dmask_ptr = &ocp_nlp_constraints_bgp_model_set_dmask_ptr; config->opts_calculate_size = &ocp_nlp_constraints_bgp_opts_calculate_size; config->opts_assign = &ocp_nlp_constraints_bgp_opts_assign; config->opts_initialize_default = &ocp_nlp_constraints_bgp_opts_initialize_default; @@ -1459,7 +1489,6 @@ void ocp_nlp_constraints_bgp_config_initialize_default(void *config_, int stage) config->memory_set_DCt_ptr = &ocp_nlp_constraints_bgp_memory_set_DCt_ptr; config->memory_set_RSQrq_ptr = &ocp_nlp_constraints_bgp_memory_set_RSQrq_ptr; config->memory_set_z_alg_ptr = &ocp_nlp_constraints_bgp_memory_set_z_alg_ptr; - config->memory_set_dmask_ptr = &ocp_nlp_constraints_bgp_memory_set_dmask_ptr; config->memory_set_dzdux_tran_ptr = &ocp_nlp_constraints_bgp_memory_set_dzduxt_ptr; config->memory_set_idxb_ptr = &ocp_nlp_constraints_bgp_memory_set_idxb_ptr; config->memory_set_idxs_rev_ptr = &ocp_nlp_constraints_bgp_memory_set_idxs_rev_ptr; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgp.h b/acados/ocp_nlp/ocp_nlp_constraints_bgp.h index 97dfea0bef..86c992c43b 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgp.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgp.h @@ -89,6 +89,7 @@ typedef struct int *idxb; int *idxs; int *idxe; + struct blasfeo_dvec *dmask; // pointer to dmask in ocp_nlp_in struct blasfeo_dvec d; struct blasfeo_dmat DCt; external_function_generic *nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux; @@ -136,7 +137,6 @@ typedef struct struct blasfeo_dvec *ux; // pointer to ux in nlp_out struct blasfeo_dvec *lam; // pointer to lam in nlp_out struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory - struct blasfeo_dvec *dmask; // pointer to dmask in qp_in struct blasfeo_dmat *DCt; // pointer to DCt in qp_in struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory @@ -163,8 +163,6 @@ void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void * // void ocp_nlp_constraints_bgp_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); // -void ocp_nlp_constraints_bgp_memory_set_dmask_ptr(struct blasfeo_dvec *dmask, void *memory_); -// void ocp_nlp_constraints_bgp_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_); // void ocp_nlp_constraints_bgp_memory_set_idxb_ptr(int *idxb, void *memory_); diff --git a/acados/ocp_nlp/ocp_nlp_constraints_common.c b/acados/ocp_nlp/ocp_nlp_constraints_common.c index 719c8edc21..4bc5378547 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_common.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_common.c @@ -65,4 +65,4 @@ ocp_nlp_constraints_config *ocp_nlp_constraints_config_assign(void *raw_memory) c_ptr += sizeof(ocp_nlp_constraints_config); return config; -} +} \ No newline at end of file diff --git a/acados/ocp_nlp/ocp_nlp_constraints_common.h b/acados/ocp_nlp/ocp_nlp_constraints_common.h index c194f4f270..fdfca35594 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_common.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_common.h @@ -61,6 +61,7 @@ typedef struct void *(*model_assign)(void *config, void *dims, void *raw_memory); int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value); void (*model_get)(void *config_, void *dims_, void *model_, const char *field, void *value); + void (*model_set_dmask_ptr)(struct blasfeo_dvec *dmask, void *model_); acados_size_t (*opts_calculate_size)(void *config, void *dims); void *(*opts_assign)(void *config, void *dims, void *raw_memory); void (*opts_initialize_default)(void *config, void *dims, void *opts); @@ -74,7 +75,6 @@ typedef struct void (*memory_set_DCt_ptr)(struct blasfeo_dmat *DCt, void *memory); void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory); void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory); - void (*memory_set_dmask_ptr)(struct blasfeo_dvec *dmask, void *memory); void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzduxt, void *memory); void (*memory_set_idxb_ptr)(int *idxb, void *memory); void (*memory_set_idxs_rev_ptr)(int *idxs_rev, void *memory); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index 8a75be3fd9..833a06938e 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -749,8 +749,8 @@ static void as_rti_advance_problem(ocp_nlp_config *config, ocp_nlp_dims *dims, o } if (opts->as_rti_advancement_strategy != NO_ADVANCE) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", nlp_work->tmp_nv_double); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", nlp_work->tmp_nv_double); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", nlp_work->tmp_nv_double); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", nlp_work->tmp_nv_double); } // printf("advanced x value\n"); // blasfeo_print_exp_tran_dvec(dims->nx[1], nlp_out->ux+1, dims->nu[1]); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index 4fe685f69c..aab1ac0886 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -1344,7 +1344,7 @@ void ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(ocp_nlp_conf blasfeo_dveccp(2*n_nominal_ineq_nlp+ns[i], nominal_qp_in->d + i, 0, relaxed_qp_in->d + i, 0); blasfeo_dveccp(ns[i], nominal_qp_in->d + i, 2*n_nominal_ineq_nlp+ns[i], relaxed_qp_in->d + i, 2*n_nominal_ineq_nlp+ns[i]+nns[i]); } - // setup d_mask + // setup d_mask; TODO: this is only needed at the start of each NLP solve if (sqp_iter == 0) { int offset_dmask; diff --git a/examples/acados_python/tests/one_sided_constraints_test.py b/examples/acados_python/tests/one_sided_constraints_test.py index 05e0e838db..f78f948b3c 100644 --- a/examples/acados_python/tests/one_sided_constraints_test.py +++ b/examples/acados_python/tests/one_sided_constraints_test.py @@ -144,10 +144,29 @@ def main(constraint_variant='one_sided', # if unbounded constraint is defined properly, lambda should be zero i_infty = 1 if constraint_variant == 'one_sided': - assert lam[i_infty] == 0 + for lam in lambdas: + assert lam[i_infty] == 0 elif (constraint_variant == 'one_sided_wrong_infty' and qp_solver in ['FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_HPIPM']): - assert lam[i_infty] != 0 + for lam in lambdas: + assert lam[i_infty] != 0 + + if constraint_variant == 'one_sided': + stage = 1 + # check setting lambdas + ocp_solver.set(stage, "lam", np.ones(lambdas[0].shape)) + lam = ocp_solver.get(stage, "lam") + assert lam[i_infty] == 0 + + # check updating bound + i_infty_new = 2 # lam is ordered as lbu, lbx, ubu, ubx + ocp_solver.constraints_set(stage, "lbx", -10.) + ocp_solver.set(stage, "lam", np.ones(lambdas[0].shape)) + ocp_solver.constraints_set(stage, "ubu", ACADOS_INFTY) + lam = ocp_solver.get(stage, "lam") + + assert lam[i_infty_new] == 0 + assert lam[i_infty] == 1. if PLOT: plot_pendulum(np.linspace(0, Tf, N_horizon + 1), Fmax, simU0, simX0, latexify=False, plt_show=True, X_true_label=f'N={N_horizon}, Tf={Tf}') diff --git a/examples/c/engine_example.c b/examples/c/engine_example.c index 328cf2bc68..3aef29bc37 100644 --- a/examples/c/engine_example.c +++ b/examples/c/engine_example.c @@ -226,6 +226,8 @@ int main() ocp_nlp_dims_set_constraints(config, dims, i, "nbu", &nbu[i]); } + // out + ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); // in ocp_nlp_in *nlp_in = ocp_nlp_in_create(config, dims); @@ -256,22 +258,22 @@ int main() } // constraints - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", x0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", x0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "idxbx", idxbx); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", x0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", x0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "idxbx", idxbx); for (int i = 1; i <= N; i++) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbx", lbx); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubx", ubx); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxbx", idxbx); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbx", lbx); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubx", ubx); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxbx", idxbx); } for (int i = 0; i < N; i++) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbu", lbu); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubu", ubu); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxbu", idxbu); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbu", lbu); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubu", ubu); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxbu", idxbu); } // options @@ -288,20 +290,17 @@ int main() ocp_nlp_solver_opts_set(config, nlp_opts, "tol_ineq", &tol); ocp_nlp_solver_opts_set(config, nlp_opts, "tol_comp", &tol); - // out - ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); - // solver ocp_nlp_solver *solver = ocp_nlp_solver_create(config, dims, nlp_opts, nlp_in); // initialize for (int i = 0; i < N; ++i) { - ocp_nlp_out_set(config, dims, nlp_out, i, "u", u); - ocp_nlp_out_set(config, dims, nlp_out, i, "x", x0); - ocp_nlp_out_set(config, dims, nlp_out, i, "z", z0); + ocp_nlp_out_set(config, dims, nlp_out, nlp_in, i, "u", u); + ocp_nlp_out_set(config, dims, nlp_out, nlp_in, i, "x", x0); + ocp_nlp_out_set(config, dims, nlp_out, nlp_in, i, "z", z0); } - ocp_nlp_out_set(config, dims, nlp_out, N, "x", x0); + ocp_nlp_out_set(config, dims, nlp_out, nlp_in, N, "x", x0); status = ocp_nlp_precompute(solver, nlp_in, nlp_out); @@ -330,16 +329,16 @@ int main() ocp_nlp_out_get(config, dims, nlp_out, jj, "z", z_sol+jj*nz_); // set x0 - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", x_sol+1*nx_); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", x_sol+1*nx_); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", x_sol+1*nx_); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", x_sol+1*nx_); // shift guess for(jj=0; jjget(config, dims, solver->mem, "sqp_iter", &sqp_iter); config->get(config, dims, solver->mem, "time_tot", &time_tot); diff --git a/examples/c/nonlinear_chain_ocp_nlp.c b/examples/c/nonlinear_chain_ocp_nlp.c index b4d90d730b..fd6183bb4a 100644 --- a/examples/c/nonlinear_chain_ocp_nlp.c +++ b/examples/c/nonlinear_chain_ocp_nlp.c @@ -1148,7 +1148,13 @@ int main() // #endif /************************************************ - * nlp_in + * ocp_nlp_out + ************************************************/ + + ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); + + /************************************************ + * ocp_nlp_in ************************************************/ ocp_nlp_in *nlp_in = ocp_nlp_in_create(config, dims); @@ -1259,10 +1265,10 @@ int main() // fist stage #if CONSTRAINTS==0 // box constraints - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbu", lb0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", lb0+NU); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubu", ub0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", ub0+NU); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbu", lb0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", lb0+NU); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubu", ub0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", ub0+NU); constraints[0]->idxb = idxb0; #elif CONSTRAINTS==1 // general constraints double *Cu0; d_zeros(&Cu0, ng[0], nu[0]); @@ -1275,8 +1281,8 @@ int main() blasfeo_pack_tran_dmat(ng[0], nu[0], Cu0, ng[0], &constraints[0]->DCt, 0, 0); blasfeo_pack_tran_dmat(ng[0], nx[0], Cx0, ng[0], &constraints[0]->DCt, nu[0], 0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lg", lb0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ug", ub0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lg", lb0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ug", ub0); d_free(Cu0); d_free(Cx0); @@ -1288,23 +1294,23 @@ int main() // ocp_nlp_constraints_bgh_model **nl_constr = (ocp_nlp_constraints_bgh_model **) nlp_in->constraints; // nl_constr[0]->nl_constr_h_fun_jac = &nonlin_constr_generic; -// ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lg", lb0); -// ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ug", ub0); -// ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lh", &lb0[ng[0]]); -// ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "uh", &ub0[ng[0]]); +// ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lg", lb0); +// ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ug", ub0); +// ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lh", &lb0[ng[0]]); +// ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "uh", &ub0[ng[0]]); #endif // other stages for (int i = 1; i < NN; i++) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbu", lb1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbx", lb1+NU); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubu", ub1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubx", ub1+NU); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbu", lb1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbx", lb1+NU); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubu", ub1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubx", ub1+NU); constraints[i]->idxb = idxb1; } - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "lbx", lbN); - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "ubx", ubN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "lbx", lbN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "ubx", ubN); constraints[NN]->idxb = idxbN; @@ -1357,11 +1363,9 @@ int main() ocp_nlp_solver_opts_set(config, nlp_opts, "tol_comp", &tol_comp); /************************************************ - * ocp_nlp out + * ocp_nlp_solver ************************************************/ - ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); - ocp_nlp_solver *solver = ocp_nlp_solver_create(config, dims, nlp_opts, nlp_in); int status = ocp_nlp_precompute(solver, nlp_in, nlp_out); diff --git a/examples/c/simple_dae_example.c b/examples/c/simple_dae_example.c index 8cb8bc08c8..4a1e91d3bf 100644 --- a/examples/c/simple_dae_example.c +++ b/examples/c/simple_dae_example.c @@ -293,6 +293,8 @@ int main() { c_ptr += external_function_param_casadi_calculate_size(impl_ode_jac_x_xdot_u_z+ii, 0, &ext_fun_opts); } + // ocp_nlp_out, ocp_nlp_in + ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); ocp_nlp_in *nlp_in = ocp_nlp_in_create(config, dims); for (int i = 0; i < N; ++i) @@ -333,8 +335,8 @@ int main() { // bounds ocp_nlp_constraints_bgh_model **constraints = (ocp_nlp_constraints_bgh_model **) nlp_in->constraints; - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", x0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", x0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", x0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", x0); constraints[0]->idxb = idxb_0; if (FORMULATION == 2) { @@ -349,14 +351,14 @@ int main() { nl_constr_h_fun_jac[ii].casadi_n_in = &simple_dae_constr_h_fun_jac_ut_xt_n_in; nl_constr_h_fun_jac[ii].casadi_n_out = &simple_dae_constr_h_fun_jac_ut_xt_n_out; external_function_param_casadi_create(&nl_constr_h_fun_jac[ii], 0, &ext_fun_opts); - ocp_nlp_constraints_model_set(config, dims, nlp_in, ii, "lh", lh); - ocp_nlp_constraints_model_set(config, dims, nlp_in, ii, "uh", uh); - ocp_nlp_constraints_model_set(config, dims, nlp_in, ii, "nl_constr_h_fun_jac", &nl_constr_h_fun_jac[ii]); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, ii, "lh", lh); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, ii, "uh", uh); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, ii, "nl_constr_h_fun_jac", &nl_constr_h_fun_jac[ii]); } } else { for (int ii = 1; ii < N; ++ii) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, ii, "ubx", uh); - ocp_nlp_constraints_model_set(config, dims, nlp_in, ii, "lbx", lh); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, ii, "ubx", uh); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, ii, "lbx", lh); constraints[ii]->idxb = idxb; } } @@ -392,7 +394,6 @@ int main() { ocp_nlp_solver_opts_set(config, nlp_opts, "qp_cond_N", &N2); } - ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); for (int i = 0; i <= N; ++i) { blasfeo_dvecse(nu[i]+nx[i], 0.0, nlp_out->ux+i, 0); blasfeo_dvecse(2, 0.0, nlp_out->ux+i, 0); diff --git a/examples/c/wind_turbine_nmpc.c b/examples/c/wind_turbine_nmpc.c index f435070ab3..60bf0acd3e 100644 --- a/examples/c/wind_turbine_nmpc.c +++ b/examples/c/wind_turbine_nmpc.c @@ -652,9 +652,15 @@ int main() } } + /************************************************ + * ocp_nlp_out + ************************************************/ + + ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); + ocp_nlp_out *sens_nlp_out = ocp_nlp_out_create(config, dims); /************************************************ - * nlp_in + * ocp_nlp_in ************************************************/ ocp_nlp_in *nlp_in = ocp_nlp_in_create(config, dims); @@ -751,26 +757,26 @@ int main() /* box constraints */ // fist stage - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "idxbu", idxbu0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbu", lbu0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubu", ubu0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "idxbx", idxbx0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", lbx0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", ubx0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "idxbu", idxbu0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbu", lbu0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubu", ubu0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", ubx0); // middle stages for (int i = 1; i < NN; i++) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxbu", idxbu1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbu", lbu1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubu", ubu1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxbx", idxbx1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbx", lbx1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubx", ubx1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxbu", idxbu1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbu", lbu1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubu", ubu1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxbx", idxbx1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbx", lbx1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubx", ubx1); } // last stage - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "idxbx", idxbxN); - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "lbx", lbxN); - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "ubx", ubxN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "idxbx", idxbxN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "lbx", lbxN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "ubx", ubxN); /* nonlinear constraints */ @@ -779,9 +785,9 @@ int main() { if(nh[i]>0) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lh", lh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "uh", uh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "nl_constr_h_fun_jac", &h1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lh", lh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "uh", uh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "nl_constr_h_fun_jac", &h1); } } @@ -792,9 +798,9 @@ int main() { if (ns[i]>0) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lsh", lsh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ush", ush1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxsh", idxsh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lsh", lsh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ush", ush1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxsh", idxsh1); } } @@ -894,13 +900,9 @@ int main() } /************************************************ - * ocp_nlp_out & solver + * ocp_nlp_solver ************************************************/ - ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); - - ocp_nlp_out *sens_nlp_out = ocp_nlp_out_create(config, dims); - ocp_nlp_solver *solver = ocp_nlp_solver_create(config, dims, nlp_opts, nlp_in); /************************************************ @@ -932,8 +934,8 @@ int main() } // set x0 as box constraint - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", x0_ref); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", x0_ref); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", x0_ref); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", x0_ref); // store x0 for(int ii=0; ii0) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lh", lh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "uh", uh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "nl_constr_h_fun_jac", &h1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lh", lh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "uh", uh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "nl_constr_h_fun_jac", &h1); } } @@ -835,19 +840,19 @@ int main() { if (ns[i]>0) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lsh", lsh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ush", ush1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxsh", idxsh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lsh", lsh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ush", ush1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxsh", idxsh1); } - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxsbx", idxsbx1); // Added for testing soft constraints - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lsbx", lsbx1); // Added for testing soft constraints - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "usbx", usbx1); // Added for testing soft constraints + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxsbx", idxsbx1); // Added for testing soft constraints + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lsbx", lsbx1); // Added for testing soft constraints + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "usbx", usbx1); // Added for testing soft constraints } - // ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "idxsbx", idxsbxN); // Added for testing soft constraints - // ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "lsbx", lsbxN); // Added for testing soft constraints - // ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "usbx", usbxN); // Added for testing soft constraints + // ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "idxsbx", idxsbxN); // Added for testing soft constraints + // ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "lsbx", lsbxN); // Added for testing soft constraints + // ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "usbx", usbxN); // Added for testing soft constraints /************************************************ @@ -944,12 +949,11 @@ int main() ocp_nlp_solver_opts_set(config, nlp_opts, "qp_cond_N", &cond_N); } + /************************************************ - * ocp_nlp out + * ocp_nlp_solver ************************************************/ - ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); - ocp_nlp_solver *solver = ocp_nlp_solver_create(config, dims, nlp_opts, nlp_in); /************************************************ @@ -981,8 +985,8 @@ int main() } // set x0 as box constraint - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", x0_ref); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", x0_ref); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", x0_ref); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", x0_ref); // store x0 for(int ii=0; iinlp_in = nlp_in; ocp_nlp_in * nlp_in = capsule->nlp_in; + ocp_nlp_out * nlp_out = capsule->nlp_out; // set up time_steps @@ -512,9 +511,9 @@ void pendulum_ode_acados_setup_nlp_in(pendulum_ode_solver_capsule* capsule, cons lbx0[1] = 3.141592653589793; ubx0[1] = 3.141592653589793; - 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", lbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); free(idxbx0); free(lubx0); // idxbxe_0 @@ -524,7 +523,7 @@ void pendulum_ode_acados_setup_nlp_in(pendulum_ode_solver_capsule* capsule, cons idxbxe_0[1] = 1; idxbxe_0[2] = 2; idxbxe_0[3] = 3; - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbxe", idxbxe_0); free(idxbxe_0); /* constraints that are the same for initial and intermediate */ @@ -541,9 +540,9 @@ void pendulum_ode_acados_setup_nlp_in(pendulum_ode_solver_capsule* capsule, cons for (int i = 0; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbu", idxbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbu", lbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubu", ubu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbu", idxbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbu", lbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubu", ubu); } free(idxbu); free(lubu); @@ -645,6 +644,7 @@ void pendulum_ode_acados_set_nlp_out(pendulum_ode_solver_capsule* capsule) ocp_nlp_config* nlp_config = capsule->nlp_config; ocp_nlp_dims* nlp_dims = capsule->nlp_dims; ocp_nlp_out* nlp_out = capsule->nlp_out; + ocp_nlp_in* nlp_in = capsule->nlp_in; // initialize primal solution double* xu0 = calloc(NX+NU, sizeof(double)); @@ -659,11 +659,11 @@ void pendulum_ode_acados_set_nlp_out(pendulum_ode_solver_capsule* capsule) for (int i = 0; i < N; i++) { // x0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x0); // u0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); } - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x0); free(xu0); } @@ -708,25 +708,25 @@ int pendulum_ode_acados_create_with_discretization(pendulum_ode_solver_capsule* capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims); pendulum_ode_acados_create_set_opts(capsule); - // 4) create nlp_in + // 4) create and set nlp_out + // 4.1) nlp_out + capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + // 4.2) sens_out + capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + pendulum_ode_acados_set_nlp_out(capsule); + + // 5) create nlp_in capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims); - // 5) setup functions, nlp_in and default parameters + // 6) setup functions, nlp_in and default parameters pendulum_ode_acados_create_setup_functions(capsule); pendulum_ode_acados_setup_nlp_in(capsule, N, new_time_steps); pendulum_ode_acados_create_set_default_parameters(capsule); - // 6) create solver + // 7) create solver capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts, capsule->nlp_in); - // 7) create and set nlp_out - // 7.1) nlp_out - capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - // 7.2) sens_out - capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - pendulum_ode_acados_set_nlp_out(capsule); - - // 8) do precomputations + // 9) do precomputations int status = pendulum_ode_acados_create_precompute(capsule); return status; @@ -749,15 +749,15 @@ int pendulum_ode_acados_reset(pendulum_ode_solver_capsule* capsule, int reset_qp for(int i=0; iconstraints[stage]; - return constr_config->model_set(constr_config, dims->constraints[stage], + // this updates both the bounds and the mask + int status = constr_config->model_set(constr_config, dims->constraints[stage], in->constraints[stage], field, value); + // multiply lam with new mask to ensure that multipliers associated with masked constraints are zero. + blasfeo_dvecmul(2*dims->ni[stage], &in->dmask[stage], 0, &out->lam[stage], 0, &out->lam[stage], 0); + + return status; } @@ -625,7 +633,7 @@ void ocp_nlp_out_destroy(void *out_) -void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, +void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, ocp_nlp_in *in, int stage, const char *field, void *value) { double *double_values = value; @@ -654,6 +662,8 @@ void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou else if (!strcmp(field, "lam")) { blasfeo_pack_dvec(2*dims->ni[stage], double_values, 1, &out->lam[stage], 0); + // multiply with mask to ensure that multiplier associated with masked constraints are zero + blasfeo_dvecmul(2*dims->ni[stage], &in->dmask[stage], 0, &out->lam[stage], 0, &out->lam[stage], 0); } else if (!strcmp(field, "z")) { @@ -1830,6 +1840,8 @@ void ocp_nlp_set_all(ocp_nlp_solver *solver, ocp_nlp_in *in, ocp_nlp_out *out, c { tmp_int = 2*dims->ni[stage]; blasfeo_pack_dvec(tmp_int, double_values + tmp_offset, 1, &out->lam[stage], 0); + // multiply with mask to ensure that multiplier associated with masked constraints are zero + blasfeo_dvecmul(2*dims->ni[stage], &in->dmask[stage], 0, &out->lam[stage], 0, &out->lam[stage], 0); tmp_offset += tmp_int; } } diff --git a/interfaces/acados_c/ocp_nlp_interface.h b/interfaces/acados_c/ocp_nlp_interface.h index fc3cb0cb7c..361c6a1ff1 100644 --- a/interfaces/acados_c/ocp_nlp_interface.h +++ b/interfaces/acados_c/ocp_nlp_interface.h @@ -265,7 +265,7 @@ ACADOS_SYMBOL_EXPORT int ocp_nlp_cost_model_get(ocp_nlp_config *config, ocp_nlp_ /// \param field The name of the field, either lb, ub (others TBC) /// \param value Constraints function or values. ACADOS_SYMBOL_EXPORT int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, - ocp_nlp_in *in, int stage, const char *field, void *value); + ocp_nlp_in *in, ocp_nlp_out *out, int stage, const char *field, void *value); /// ACADOS_SYMBOL_EXPORT void ocp_nlp_constraints_model_get(ocp_nlp_config *config, ocp_nlp_dims *dims, @@ -293,7 +293,7 @@ ACADOS_SYMBOL_EXPORT void ocp_nlp_out_destroy(void *out); /// \param stage Stage number. /// \param field The name of the field, either x, u, pi. /// \param value Initialization values. -ACADOS_SYMBOL_EXPORT void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, +ACADOS_SYMBOL_EXPORT void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, ocp_nlp_in *in, int stage, const char *field, void *value); diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index baa434f0c5..903b927c4d 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -350,13 +350,12 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp, None], json self.__acados_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p] self.__acados_lib.ocp_nlp_eval_residuals.argtypes = [c_void_p, c_void_p, c_void_p] - - self.__acados_lib.ocp_nlp_constraints_model_set.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] - self.__acados_lib.ocp_nlp_constraints_model_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.__acados_lib.ocp_nlp_constraints_model_set.argtypes = [c_void_p, c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.__acados_lib.ocp_nlp_constraints_model_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] # TODO check again self.__acados_lib.ocp_nlp_cost_model_set.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] self.__acados_lib.ocp_nlp_cost_model_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] - self.__acados_lib.ocp_nlp_out_set.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.__acados_lib.ocp_nlp_out_set.argtypes = [c_void_p, c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] self.__acados_lib.ocp_nlp_set.argtypes = [c_void_p, c_int, c_char_p, c_void_p] self.__acados_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] @@ -1827,20 +1826,18 @@ def set(self, stage_: int, field_: str, value_: np.ndarray): if field_ in constraints_fields: self.__acados_lib.ocp_nlp_constraints_model_set(self.nlp_config, \ - self.nlp_dims, self.nlp_in, stage, field, value_data_p) + self.nlp_dims, self.nlp_in, self.nlp_out, stage, field, value_data_p) elif field_ in cost_fields: self.__acados_lib.ocp_nlp_cost_model_set(self.nlp_config, \ self.nlp_dims, self.nlp_in, stage, field, value_data_p) elif field_ in out_fields: self.__acados_lib.ocp_nlp_out_set(self.nlp_config, \ - self.nlp_dims, self.nlp_out, stage, field, value_data_p) + self.nlp_dims, self.nlp_out, self.nlp_in, stage, field, value_data_p) elif field_ in mem_fields: self.__acados_lib.ocp_nlp_set(self.nlp_solver, stage, field, value_data_p) elif field_ in sens_fields: - self.__acados_lib.ocp_nlp_out_set.argtypes = \ - [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] self.__acados_lib.ocp_nlp_out_set(self.nlp_config, \ - self.nlp_dims, self.sens_out, stage, field, value_data_p) + self.nlp_dims, self.sens_out, self.nlp_in, stage, field, value_data_p) # also set z_guess, when setting z. if field_ == 'z': field = 'z_guess'.encode('utf-8') @@ -2057,7 +2054,7 @@ def constraints_set(self, stage_: int, field_: str, value_: np.ndarray, api='war value_data_p = cast((value_data), c_void_p) self.__acados_lib.ocp_nlp_constraints_model_set(self.nlp_config, \ - self.nlp_dims, self.nlp_in, stage, field, value_data_p) + self.nlp_dims, self.nlp_in, self.nlp_out, stage, field, value_data_p) return diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx b/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx index 16ada19927..7c159768a5 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx +++ b/interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx @@ -711,13 +711,13 @@ cdef class AcadosOcpSolverCython: if field_ in constraints_fields: acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, - self.nlp_dims, self.nlp_in, stage, field, value.data) + self.nlp_dims, self.nlp_in, self.nlp_out, stage, field, value.data) elif field_ in cost_fields: acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, self.nlp_dims, self.nlp_in, stage, field, value.data) elif field_ in out_fields: acados_solver_common.ocp_nlp_out_set(self.nlp_config, - self.nlp_dims, self.nlp_out, stage, field, value.data) + self.nlp_dims, self.nlp_out, self.nlp_in, stage, field, value.data) elif field_ in mem_fields: acados_solver_common.ocp_nlp_set(self.nlp_solver, stage, field, value.data) @@ -794,7 +794,7 @@ cdef class AcadosOcpSolverCython: f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})') acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \ - self.nlp_dims, self.nlp_in, stage, field, &value[0][0]) + self.nlp_dims, self.nlp_in, self.nlp_out, stage, field, &value[0][0]) return diff --git a/interfaces/acados_template/acados_template/acados_solver_common.pxd b/interfaces/acados_template/acados_template/acados_solver_common.pxd index 200071d391..f40d6b6fef 100644 --- a/interfaces/acados_template/acados_template/acados_solver_common.pxd +++ b/interfaces/acados_template/acados_template/acados_solver_common.pxd @@ -69,10 +69,10 @@ cdef extern from "acados_c/ocp_nlp_interface.h": int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in_, int start_stage, const char *field, void *value) int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, - ocp_nlp_in *in_, int stage, const char *field, void *value) + ocp_nlp_in *in_, ocp_nlp_out *out_, int stage, const char *field, void *value) # out - void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, ocp_nlp_in *in_, int stage, const char *field, void *value) void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field, void *value) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c index 2db025a1fc..cf44923d66 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c @@ -871,6 +871,10 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i * nlp_in ************************************************/ ocp_nlp_in * nlp_in = capsule->nlp_in; + /************************************************ + * nlp_out + ************************************************/ + ocp_nlp_out * nlp_out = capsule->nlp_out; // set up time_steps @@ -1069,9 +1073,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - 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", lbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); free(idxbx0); free(lubx0); {%- endif %} @@ -1082,7 +1086,7 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- for i in range(end=dims_0.nbxe_0) %} idxbxe_0[{{ i }}] = {{ constraints_0.idxbxe_0[i] }}; {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbxe", idxbxe_0); free(idxbxe_0); {%- endif %} @@ -1110,6 +1114,8 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_0_fun_jac_hess); {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lh", lh_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "uh", uh_0); {% if solver_options.with_solution_sens_wrt_params %} ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_h_jac_p_hess_xu_p", &capsule->nl_constr_h_0_jac_p_hess_xu_p); @@ -1118,8 +1124,6 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_h_adj_p", &capsule->nl_constr_h_0_adj_p); {% endif %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lh", lh_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "uh", uh_0); free(luh_0); {%- elif dims_0.nphi_0 > 0 and constraints_0.constr_type_0 == "BGP" %} // set up convex-over-nonlinear constraints for first stage @@ -1135,8 +1139,8 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lphi", lphi_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "uphi", uphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lphi", lphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "uphi", uphi_0); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_phi_o_r_fun", &capsule->phi_0_constraint_fun); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, @@ -1162,9 +1166,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsh", idxsh_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsh", lsh_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ush", ush_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsh", idxsh_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsh", lsh_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ush", ush_0); free(idxsh_0); free(lush_0); {%- endif %} @@ -1187,9 +1191,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsphi", idxsphi_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsphi", lsphi_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usphi", usphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsphi", idxsphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsphi", lsphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "usphi", usphi_0); free(idxsphi_0); free(lusphi_0); {%- endif %} @@ -1498,9 +1502,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i /* constraints that are the same for initial and intermediate */ {%- if phases_dims[jj].nsbx > 0 %} {# TODO: introduce nsbx0 & move this block down!! #} - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsbx", idxsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsbx", lsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usbx", usbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsbx", idxsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsbx", lsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "usbx", usbx); // soft bounds on x idxsbx = malloc({{ phases_dims[jj].nsbx }} * sizeof(int)); @@ -1522,9 +1526,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbx", idxsbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbx", lsbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbx", usbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsbx", idxsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsbx", lsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usbx", usbx); } free(idxsbx); free(lusbx); @@ -1551,9 +1555,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbu", idxbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbu", lbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubu", ubu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbu", idxbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbu", lbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubu", ubu); } free(idxbu); free(lubu); @@ -1578,9 +1582,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endfor %} for (int i = {{ start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbu", idxsbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbu", lsbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbu", usbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsbu", idxsbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsbu", lsbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usbu", usbu); } free(idxsbu); free(lusbu); @@ -1606,9 +1610,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsg", idxsg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsg", lsg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usg", usg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsg", idxsg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsg", lsg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usg", usg); } free(idxsg); free(lusg); @@ -1634,9 +1638,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsh", idxsh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsh", lsh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ush", ush); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsh", idxsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsh", lsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ush", ush); } free(idxsh); free(lush); @@ -1662,9 +1666,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsphi", idxsphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsphi", lsphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usphi", usphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsphi", idxsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsphi", lsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usphi", usphi); } free(idxsphi); free(lusphi); @@ -1690,9 +1694,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ cost_start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbx", idxbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbx", lbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubx", ubx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbx", idxbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbx", lbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubx", ubx); } free(idxbx); free(lubx); @@ -1736,10 +1740,10 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i for (int i = {{ start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "D", D); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "C", C); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lg", lg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ug", ug); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "D", D); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "C", C); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lg", lg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ug", ug); } free(D); free(C); @@ -1775,6 +1779,8 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_fun_jac_hess_{{ jj }}[i_fun]); {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lh", lh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "uh", uh); {% if solver_options.with_solution_sens_wrt_params %} ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_jac_p_hess_xu_p", &capsule->nl_constr_h_jac_p_hess_xu_p_{{ jj }}[i_fun]); @@ -1783,8 +1789,6 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_adj_p", &capsule->nl_constr_h_adj_p_{{ jj }}[i_fun]); {% endif %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lh", lh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uh", uh); } free(luh); {%- endif %} @@ -1812,8 +1816,8 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_phi_o_r_fun", &capsule->phi_constraint_fun[i_fun]); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_constraint_fun_jac_hess[i_fun]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lphi", lphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uphi", uphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lphi", lphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "uphi", uphi); } free(luphi); {%- endif %} @@ -1947,9 +1951,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ubx_e[{{ i }}] = {{ constraints_e.ubx_e[i] }}; {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxbx", idxbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", lbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", ubx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxbx", idxbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lbx", lbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ubx", ubx_e); free(idxbx_e); free(lubx_e); {%- endif %} @@ -1972,9 +1976,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsg", idxsg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsg", lsg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usg", usg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsg", idxsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsg", lsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usg", usg_e); free(idxsg_e); free(lusg_e); {%- endif %} @@ -1997,9 +2001,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsh", idxsh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsh", lsh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ush", ush_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsh", idxsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsh", lsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ush", ush_e); free(idxsh_e); free(lush_e); {%- endif %} @@ -2022,9 +2026,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsphi", idxsphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsphi", lsphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usphi", usphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsphi", idxsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsphi", lsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usphi", usphi_e); free(idxsphi_e); free(lusphi_e); {%- endif %} @@ -2047,9 +2051,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsbx", idxsbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsbx", lsbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usbx", usbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsbx", idxsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsbx", lsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usbx", usbx_e); free(idxsbx_e); free(lusbx_e); {% endif %} @@ -2078,9 +2082,9 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "C", C_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", lg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", ug_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "C", C_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lg", lg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ug", ug_e); free(C_e); free(lug_e); {%- endif %} @@ -2108,6 +2112,8 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_e_fun_jac_hess); {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lh", lh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uh", uh_e); {% if solver_options.with_solution_sens_wrt_params %} ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_jac_p_hess_xu_p", &capsule->nl_constr_h_e_jac_p_hess_xu_p); @@ -2116,8 +2122,6 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_adj_p", &capsule->nl_constr_h_e_adj_p); {% endif %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", lh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", uh_e); free(luh_e); {%- elif dims_e.nphi_e > 0 and constraints_e.constr_type_e == "BGP" %} // set up convex-over-nonlinear constraints for last stage @@ -2133,8 +2137,8 @@ void {{ name }}_acados_create_setup_nlp_in({{ name }}_solver_capsule* capsule, i {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lphi", lphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uphi", uphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lphi", lphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uphi", uphi_e); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_phi_o_r_fun", &capsule->phi_e_constraint_fun); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_e_constraint_fun_jac_hess); @@ -2521,6 +2525,8 @@ void {{ name }}_acados_create_set_nlp_out({{ name }}_solver_capsule* capsule) ocp_nlp_config* nlp_config = capsule->nlp_config; ocp_nlp_dims* nlp_dims = capsule->nlp_dims; ocp_nlp_out* nlp_out = capsule->nlp_out; + ocp_nlp_in* nlp_in = capsule->nlp_in; + int nx_max = {{ nx_max }}; int nu_max = {{ nu_max }}; @@ -2544,11 +2550,11 @@ void {{ name }}_acados_create_set_nlp_out({{ name }}_solver_capsule* capsule) for (int i = 0; i < N; i++) { // x0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x0); // u0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); } - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x0); free(xu0); } @@ -2591,23 +2597,24 @@ int {{ name }}_acados_create_with_discretization({{ name }}_solver_capsule* caps capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims); {{ name }}_acados_create_set_opts(capsule); - // 4) create nlp_in + // 4) create and set nlp_out + // 4.1) nlp_out + capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + // 4.2) sens_out + capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + {{ name }}_acados_create_set_nlp_out(capsule); + + // 5) create nlp_in capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims); - // 5) set default parameters in functions + // 6) set default parameters in functions {{ name }}_acados_create_setup_functions(capsule); {{ name }}_acados_create_setup_nlp_in(capsule, N); {{ name }}_acados_create_set_default_parameters(capsule); - // 6) create solver + // 7) create solver capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts, capsule->nlp_in); - // 7) create and set nlp_out - // 7.1) nlp_out - capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - // 7.2) sens_out - capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - {{ name }}_acados_create_set_nlp_out(capsule); // 8) do precomputations int status = {{ name }}_acados_create_precompute(capsule); @@ -2669,15 +2676,15 @@ int {{ name }}_acados_reset({{ name }}_solver_capsule* capsule, int reset_qp_sol // Reset stage {{ jj }} for (int i = {{ start_idx[jj] }}; i < {{ end_idx[jj] }}; i++) { - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", buffer); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", buffer); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "sl", buffer); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "su", buffer); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "lam", buffer); - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "z", buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "sl", buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "su", buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "lam", buffer); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "z", buffer); if (inlp_in = nlp_in; ocp_nlp_in * nlp_in = capsule->nlp_in; + /************************************************ + * nlp_out + ************************************************/ + ocp_nlp_out * nlp_out = capsule->nlp_out; // set up time_steps and cost_scaling {%- set all_equal = true -%} @@ -1487,9 +1489,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - 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", lbx0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); free(idxbx0); free(lubx0); {%- endif %} @@ -1500,7 +1502,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- for i in range(end=dims.nbxe_0) %} idxbxe_0[{{ i }}] = {{ constraints.idxbxe_0[i] }}; {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbxe", idxbxe_0); free(idxbxe_0); {%- endif %} @@ -1528,6 +1530,8 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_0_fun_jac_hess); {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lh", lh_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "uh", uh_0); {% if solver_options.with_solution_sens_wrt_params %} ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_h_jac_p_hess_xu_p", &capsule->nl_constr_h_0_jac_p_hess_xu_p); @@ -1536,8 +1540,6 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_h_adj_p", &capsule->nl_constr_h_0_adj_p); {% endif %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lh", lh_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "uh", uh_0); free(luh_0); {%- elif dims.nphi_0 > 0 and constraints.constr_type_0 == "BGP" %} // set up convex-over-nonlinear constraints for last stage @@ -1553,8 +1555,8 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lphi", lphi_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "uphi", uphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lphi", lphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "uphi", uphi_0); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nl_constr_phi_o_r_fun", &capsule->phi_0_constraint_fun); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, @@ -1580,9 +1582,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsh", idxsh_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsh", lsh_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ush", ush_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsh", idxsh_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsh", lsh_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ush", ush_0); free(idxsh_0); free(lush_0); {%- endif %} @@ -1605,9 +1607,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsphi", idxsphi_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsphi", lsphi_0); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usphi", usphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsphi", idxsphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsphi", lsphi_0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "usphi", usphi_0); free(idxsphi_0); free(lusphi_0); {%- endif %} @@ -1615,9 +1617,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu /* constraints that are the same for initial and intermediate */ {%- if dims.nsbx > 0 %} {# TODO: introduce nsbx0 & move this block down!! #} - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsbx", idxsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsbx", lsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usbx", usbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxsbx", idxsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lsbx", lsbx); + // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "usbx", usbx); // soft bounds on x int* idxsbx = malloc(NSBX * sizeof(int)); @@ -1639,9 +1641,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 1; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbx", idxsbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbx", lsbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbx", usbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsbx", idxsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsbx", lsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usbx", usbx); } free(idxsbx); free(lusbx); @@ -1668,9 +1670,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 0; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbu", idxbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbu", lbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubu", ubu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbu", idxbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbu", lbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubu", ubu); } free(idxbu); free(lubu); @@ -1695,9 +1697,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endfor %} for (int i = 0; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbu", idxsbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbu", lsbu); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbu", usbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsbu", idxsbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsbu", lsbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usbu", usbu); } free(idxsbu); free(lusbu); @@ -1723,9 +1725,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 0; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsg", idxsg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsg", lsg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usg", usg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsg", idxsg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsg", lsg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usg", usg); } free(idxsg); free(lusg); @@ -1751,9 +1753,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 1; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsh", idxsh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsh", lsh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ush", ush); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsh", idxsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsh", lsh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ush", ush); } free(idxsh); free(lush); @@ -1779,9 +1781,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 1; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsphi", idxsphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsphi", lsphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usphi", usphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxsphi", idxsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lsphi", lsphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "usphi", usphi); } free(idxsphi); free(lusphi); @@ -1807,9 +1809,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 1; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbx", idxbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbx", lbx); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubx", ubx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbx", idxbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbx", lbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubx", ubx); } free(idxbx); free(lubx); @@ -1853,10 +1855,10 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu for (int i = 0; i < N; i++) { - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "D", D); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "C", C); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lg", lg); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ug", ug); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "D", D); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "C", C); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lg", lg); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ug", ug); } free(D); free(C); @@ -1891,6 +1893,8 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_fun_jac_hess[i-1]); {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lh", lh); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "uh", uh); {% if solver_options.with_solution_sens_wrt_params %} ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_jac_p_hess_xu_p", &capsule->nl_constr_h_jac_p_hess_xu_p[i-1]); @@ -1899,8 +1903,6 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_adj_p", &capsule->nl_constr_h_adj_p[i-1]); {% endif %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lh", lh); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uh", uh); } free(luh); {%- endif %} @@ -1928,8 +1930,8 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu "nl_constr_phi_o_r_fun", &capsule->phi_constraint_fun[i-1]); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_constraint_fun_jac_hess[i-1]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lphi", lphi); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uphi", uphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lphi", lphi); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "uphi", uphi); } free(luphi); {%- endif %} @@ -1953,9 +1955,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ubx_e[{{ i }}] = {{ constraints.ubx_e[i] }}; {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxbx", idxbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", lbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", ubx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxbx", idxbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lbx", lbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ubx", ubx_e); free(idxbx_e); free(lubx_e); {%- endif %} @@ -1978,9 +1980,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsg", idxsg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsg", lsg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usg", usg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsg", idxsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsg", lsg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usg", usg_e); free(idxsg_e); free(lusg_e); {%- endif %} @@ -2003,9 +2005,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsh", idxsh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsh", lsh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ush", ush_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsh", idxsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsh", lsh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ush", ush_e); free(idxsh_e); free(lush_e); {%- endif %} @@ -2028,9 +2030,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsphi", idxsphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsphi", lsphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usphi", usphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsphi", idxsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsphi", lsphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usphi", usphi_e); free(idxsphi_e); free(lusphi_e); {%- endif %} @@ -2053,9 +2055,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsbx", idxsbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsbx", lsbx_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usbx", usbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "idxsbx", idxsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lsbx", lsbx_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "usbx", usbx_e); free(idxsbx_e); free(lusbx_e); {% endif %} @@ -2084,9 +2086,9 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "C", C_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", lg_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", ug_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "C", C_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lg", lg_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ug", ug_e); free(C_e); free(lug_e); {%- endif %} @@ -2114,6 +2116,8 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_e_fun_jac_hess); {% endif %} + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lh", lh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uh", uh_e); {% if solver_options.with_solution_sens_wrt_params %} ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_jac_p_hess_xu_p", &capsule->nl_constr_h_e_jac_p_hess_xu_p); @@ -2122,8 +2126,6 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_adj_p", &capsule->nl_constr_h_e_adj_p); {% endif %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", lh_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", uh_e); free(luh_e); {%- elif dims.nphi_e > 0 and constraints.constr_type_e == "BGP" %} // set up convex-over-nonlinear constraints for last stage @@ -2139,8 +2141,8 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lphi", lphi_e); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uphi", uphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lphi", lphi_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uphi", uphi_e); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nl_constr_phi_o_r_fun", &capsule->phi_e_constraint_fun); ocp_nlp_constraints_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, @@ -2562,6 +2564,7 @@ void {{ model.name }}_acados_set_nlp_out({{ model.name }}_solver_capsule* capsul ocp_nlp_config* nlp_config = capsule->nlp_config; ocp_nlp_dims* nlp_dims = capsule->nlp_dims; ocp_nlp_out* nlp_out = capsule->nlp_out; + ocp_nlp_in* nlp_in = capsule->nlp_in; // initialize primal solution double* xu0 = calloc(NX+NU, sizeof(double)); @@ -2582,11 +2585,11 @@ void {{ model.name }}_acados_set_nlp_out({{ model.name }}_solver_capsule* capsul for (int i = 0; i < N; i++) { // x0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x0); // u0 - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); } - ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x0); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x0); free(xu0); } @@ -2632,23 +2635,24 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims); {{ model.name }}_acados_create_set_opts(capsule); - // 4) create nlp_in + // 4) create and set nlp_out + // 4.1) nlp_out + capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + // 4.2) sens_out + capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + {{ model.name }}_acados_set_nlp_out(capsule); + + // 5) create nlp_in capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims); - // 5) setup functions, nlp_in and default parameters + // 6) setup functions, nlp_in and default parameters {{ model.name }}_acados_create_setup_functions(capsule); {{ model.name }}_acados_setup_nlp_in(capsule, N, new_time_steps); {{ model.name }}_acados_create_set_default_parameters(capsule); - // 6) create solver + // 7) create solver capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts, capsule->nlp_in); - // 7) create and set nlp_out - // 7.1) nlp_out - capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - // 7.2) sens_out - capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); - {{ model.name }}_acados_set_nlp_out(capsule); // 8) do precomputations int status = {{ model.name }}_acados_create_precompute(capsule); @@ -2707,15 +2711,15 @@ int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule, int for(int i=0; i 0 allows this to work // also for lbu/ubu. for which dimension at terminal stage is 0 MEX_DIM_CHECK_VEC_STAGE(fun_name, field, ii, matlab_size, acados_size) - ocp_nlp_constraints_model_set(config, dims, in, ii, field_name, value); + ocp_nlp_constraints_model_set(config, dims, in, out, ii, field_name, value); } } } @@ -189,7 +189,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) offset = 0; for (ii=0; ii<=N; ii++) // TODO implement set_all { - ocp_nlp_constraints_model_set(config, dims, in, ii, field_name, value+offset); + ocp_nlp_constraints_model_set(config, dims, in, out, ii, field_name, value+offset); tmp_int = ocp_nlp_dims_get_from_attr(config, dims, out, ii, field_name); offset += tmp_int; } @@ -200,7 +200,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, field_name); MEX_DIM_CHECK_VEC_STAGE(fun_name, field, s0, matlab_size, acados_size) if (matlab_size != 0) - ocp_nlp_constraints_model_set(config, dims, in, s0, field_name, value); + ocp_nlp_constraints_model_set(config, dims, in, out, s0, field_name, value); } } // cost: @@ -379,7 +379,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "x"); MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - ocp_nlp_out_set(config, dims, out, s0, "x", value); + ocp_nlp_out_set(config, dims, out, in, s0, "x", value); } } else if (!strcmp(field, "init_u") || !strcmp(field, "u")) @@ -394,7 +394,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "u"); MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - ocp_nlp_out_set(config, dims, out, s0, "u", value); + ocp_nlp_out_set(config, dims, out, in, s0, "u", value); } } else if (!strcmp(field, "init_z")||!strcmp(field, "z")) @@ -499,7 +499,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "pi"); MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - ocp_nlp_out_set(config, dims, out, s0, "pi", value); + ocp_nlp_out_set(config, dims, out, in, s0, "pi", value); } } else if (!strcmp(field, "init_lam")||!strcmp(field, "lam")) @@ -514,7 +514,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "lam"); MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - ocp_nlp_out_set(config, dims, out, s0, "lam", value); + ocp_nlp_out_set(config, dims, out, in, s0, "lam", value); } } else if (!strcmp(field, "init_sl")||!strcmp(field, "sl")) @@ -529,7 +529,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "sl"); MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - ocp_nlp_out_set(config, dims, out, s0, field, value); + ocp_nlp_out_set(config, dims, out, in, s0, field, value); } } else if (!strcmp(field, "init_su")||!strcmp(field, "su")) @@ -544,7 +544,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "su"); MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - ocp_nlp_out_set(config, dims, out, s0, field, value); + ocp_nlp_out_set(config, dims, out, in, s0, field, value); } } else if (!strcmp(field, "p")) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c index 579399b31c..1381bd4d48 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c @@ -799,7 +799,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) for (int i = 0; i < {{ dims_0.nbx_0 }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", buffer); {%- endif %} {%- if dims_0.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #} @@ -808,7 +808,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); for (int i = 0; i < {{ dims_0.nbx_0 }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", buffer); {%- endif %} {%- if np_total > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} @@ -889,7 +889,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "lbx"); for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "lbx", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "lbx", (void *) buffer); tmp_offset += tmp_int; } {%- endif %} @@ -903,7 +903,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "ubx"); for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "ubx", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "ubx", (void *) buffer); tmp_offset += tmp_int; } {%- endif %} @@ -916,7 +916,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) for (int i = 0; i < {{ dims_e.nbx_e }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lbx", buffer); {%- endif %} {%- if dims_e.nbx_e > 0 and solver_options.N_horizon > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #} // ubx_e @@ -925,7 +925,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) for (int i = 0; i < {{ dims_e.nbx_e }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ubx", buffer); {%- endif %} @@ -939,7 +939,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "lbu"); for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "lbu", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "lbu", (void *) buffer); tmp_offset += tmp_int; } {%- endif -%} @@ -953,7 +953,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "ubu"); for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "ubu", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "ubu", (void *) buffer); tmp_offset += tmp_int; } {%- endif -%} @@ -968,7 +968,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) { for (int jj = 0; jj < {{ dims.ng }}; jj++) buffer[jj] = (double)(*in_sign[stage*{{ dims.ng }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "lg", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "lg", (void *) buffer); } {%- endif -%} {%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #} @@ -980,7 +980,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) { for (int jj = 0; jj < {{ dims.ng }}; jj++) buffer[jj] = (double)(*in_sign[stage*{{ dims.ng }}+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "ug", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "ug", (void *) buffer); } {%- endif -%} {%- endif -%} @@ -995,7 +995,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "lh"); for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "lh", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "lh", (void *) buffer); tmp_offset += tmp_int; } {%- endif -%} @@ -1009,7 +1009,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) tmp_int = ocp_nlp_dims_get_from_attr(nlp_config, nlp_dims, nlp_out, stage, "uh"); for (int jj = 0; jj < tmp_int; jj++) buffer[jj] = (double)(*in_sign[tmp_offset+jj]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, stage, "uh", (void *) buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, stage, "uh", (void *) buffer); tmp_offset += tmp_int; } {%- endif -%} @@ -1020,7 +1020,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); for (int i = 0; i < {{ dims_0.nh_0 }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lh", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lh", buffer); {%- endif -%} {%- if dims_0.nh_0 > 0 and simulink_opts.inputs.uh_0 -%} {#- uh_0 #} // uh_0 @@ -1028,7 +1028,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); for (int i = 0; i < {{ dims_0.nh_0 }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "uh", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "uh", buffer); {%- endif -%} {%- if dims_e.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #} @@ -1037,7 +1037,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); for (int i = 0; i < {{ dims_e.nh_e }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lh", buffer); {%- endif -%} {%- if dims_e.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #} // uh_e @@ -1045,7 +1045,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); for (int i = 0; i < {{ dims_e.nh_e }}; i++) buffer[i] = (double)(*in_sign[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uh", buffer); {%- endif -%} {% if problem_class == "OCP" %} diff --git a/interfaces/acados_template/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c b/interfaces/acados_template/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c index d6bdf71647..1fc3bf3cdc 100644 --- a/interfaces/acados_template/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c +++ b/interfaces/acados_template/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c @@ -670,7 +670,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbu", custom_mem->d_lbu_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbu", custom_mem->d_lbu_tightened); {%- endif %} {%- if zoro_description.nubu_t > 0 %} // backoff ubu @@ -680,7 +680,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubu", custom_mem->d_ubu_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubu", custom_mem->d_ubu_tightened); {%- endif %} {%- endif %} // Middle Stages @@ -714,7 +714,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1], custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbx", custom_mem->d_lbx_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "lbx", custom_mem->d_lbx_tightened); {%- endif %} {% if zoro_description.nubx_t > 0 %} // ubx @@ -723,7 +723,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1], custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubx", custom_mem->d_ubx_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "ubx", custom_mem->d_ubx_tightened); {%- endif %} {%- endif %} @@ -739,7 +739,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbu", custom_mem->d_lbu_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "lbu", custom_mem->d_lbu_tightened); {%- endif %} {%- if zoro_description.nubu_t > 0 %} {%- for it in zoro_description.idx_ubu_t %} @@ -747,7 +747,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat, custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubu", custom_mem->d_ubu_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "ubu", custom_mem->d_ubu_tightened); {%- endif %} {%- endif %} @@ -764,7 +764,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in = custom_mem->d_lg[{{it}}] + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lg", custom_mem->d_lg_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "lg", custom_mem->d_lg_tightened); {%- endif %} {%- if zoro_description.nug_t > 0 %} {%- for it in zoro_description.idx_ug_t %} @@ -772,7 +772,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in = custom_mem->d_ug[{{it}}] - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ug", custom_mem->d_ug_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "ug", custom_mem->d_ug_tightened); {%- endif %} {%- endif %} @@ -808,14 +808,14 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in = custom_mem->d_lh[{{it}}] + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lh", custom_mem->d_lh_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "lh", custom_mem->d_lh_tightened); {%- endif %} {%- if zoro_description.nuh_t > 0 %} {%- for it in zoro_description.idx_uh_t %} custom_mem->d_uh_tightened[{{it}}] = custom_mem->d_uh[{{it}}] - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "uh", custom_mem->d_uh_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, ii+1, "uh", custom_mem->d_uh_tightened); {%- endif %} {%- endif %} } @@ -848,7 +848,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N], custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lbx", custom_mem->d_lbx_e_tightened); {%- endif %} {% if zoro_description.nubx_e_t > 0 %} // ubx_e @@ -857,7 +857,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N], custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}])); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ubx", custom_mem->d_ubx_e_tightened); {%- endif %} {%- endif %} @@ -874,7 +874,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in = custom_mem->d_lg_e[{{it}}] + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lg", custom_mem->d_lg_e_tightened); {%- endif %} {%- if zoro_description.nug_e_t > 0 %} {%- for it in zoro_description.idx_ug_e_t %} @@ -882,7 +882,7 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in = custom_mem->d_ug_e[{{it}}] - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "ug", custom_mem->d_ug_e_tightened); {%- endif %} {%- endif %} @@ -905,14 +905,14 @@ static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in = custom_mem->d_lh_e[{{it}}] + backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "lh", custom_mem->d_lh_e_tightened); {%- endif %} {%- if zoro_description.nuh_e_t > 0 %} {%- for it in zoro_description.idx_uh_e_t %} custom_mem->d_uh_e_tightened[{{it}}] = custom_mem->d_uh_e[{{it}}] - backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}})); {%- endfor %} - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e_tightened); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, N, "uh", custom_mem->d_uh_e_tightened); {%- endif %} {%- endif %} diff --git a/test/ocp_nlp/test_wind_turbine.cpp b/test/ocp_nlp/test_wind_turbine.cpp index a93a7d939b..a36be7692c 100644 --- a/test/ocp_nlp/test_wind_turbine.cpp +++ b/test/ocp_nlp/test_wind_turbine.cpp @@ -737,9 +737,13 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q } /************************************************ - * nlp_in + * ocp_nlp out ************************************************/ + ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); + /************************************************ + * ocp_nlp_in + ************************************************/ ocp_nlp_in *nlp_in = ocp_nlp_in_create(config, dims); // sampling times @@ -855,26 +859,26 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q /* box constraints */ // fist stage - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "idxbu", idxbu0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbu", lbu0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubu", ubu0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "idxbx", idxbx0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", lbx0); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", ubx0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "idxbu", idxbu0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbu", lbu0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubu", ubu0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", ubx0); // middle stages for (int i = 1; i < NN; i++) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxbu", idxbu1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbu", lbu1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubu", ubu1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxbx", idxbx1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lbx", lbx1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ubx", ubx1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxbu", idxbu1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbu", lbu1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubu", ubu1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxbx", idxbx1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lbx", lbx1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ubx", ubx1); } // last stage - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "idxbx", idxbxN); - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "lbx", lbxN); - ocp_nlp_constraints_model_set(config, dims, nlp_in, NN, "ubx", ubxN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "idxbx", idxbxN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "lbx", lbxN); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, NN, "ubx", ubxN); /* nonlinear constraints */ @@ -884,9 +888,9 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q { if (nh[i] > 0) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lh", lh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "uh", uh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "nl_constr_h_fun_jac", &h1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lh", lh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "uh", uh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "nl_constr_h_fun_jac", &h1); } } @@ -897,9 +901,9 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q { if (ns[i] > 0) { - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "lsh", lsh1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "ush", ush1); - ocp_nlp_constraints_model_set(config, dims, nlp_in, i, "idxsh", idxsh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "lsh", lsh1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "ush", ush1); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, i, "idxsh", idxsh1); } } @@ -975,11 +979,8 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q config->opts_update(config, dims, nlp_opts); /************************************************ - * ocp_nlp out + * ocp_nlp_solver ************************************************/ - - ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); - ocp_nlp_solver *solver = ocp_nlp_solver_create(config, dims, nlp_opts, nlp_in); /************************************************ @@ -1006,8 +1007,8 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q } // set x0 as box constraint - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", x0_ref); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", x0_ref); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", x0_ref); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", x0_ref); double total_power = 0.0; // sum up to get total objective double electrical_power = 0.0; @@ -1068,8 +1069,8 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q // update initial condition // TODO(dimitris): maybe simulate system instead of passing x[1] as next state ocp_nlp_out_get(config, dims, nlp_out, 1, "x", specific_x); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "lbx", specific_x); - ocp_nlp_constraints_model_set(config, dims, nlp_in, 0, "ubx", specific_x); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "lbx", specific_x); + ocp_nlp_constraints_model_set(config, dims, nlp_in, nlp_out, 0, "ubx", specific_x); int sqp_iter; double time_lin, time_qp_sol, time_tot; From 0d9b6aa6e5021c35c061ae3b1944600633406344 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 9 Apr 2025 10:05:22 +0200 Subject: [PATCH 02/24] Globalization: fix iteration number in merit backtracking (#1492) --- acados/ocp_nlp/ocp_nlp_globalization_common.h | 9 +- ...ocp_nlp_globalization_merit_backtracking.c | 333 +++++++++--------- ...ocp_nlp_globalization_merit_backtracking.h | 9 +- .../non_ocp_nlp/maratos_test_problem.py | 14 +- 4 files changed, 174 insertions(+), 191 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_globalization_common.h b/acados/ocp_nlp/ocp_nlp_globalization_common.h index d8c6cead55..02cc9071e8 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_common.h +++ b/acados/ocp_nlp/ocp_nlp_globalization_common.h @@ -64,15 +64,10 @@ typedef struct /* functions */ int (*find_acceptable_iterate)(void *nlp_config, void *nlp_dims, void *nlp_in, void *nlp_out, void *nlp_mem, void *solver_mem, void *nlp_work, void *nlp_opts, double *step_size); void (*print_iteration_header)(); - void (*print_iteration)(double objective_value, - void *globalization_opts, - void* globalization_mem); + void (*print_iteration)(double objective_value, void *globalization_opts, void* globalization_mem); int (*needs_objective_value)(); int (*needs_qp_objective_value)(); - void (*initialize_memory)(void *config_, - void *dims_, - void *nlp_mem_, - void *nlp_opts_); + void (*initialize_memory)(void *config_, void *dims_, void *nlp_mem_, void *nlp_opts_); } ocp_nlp_globalization_config; // diff --git a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c index 1b7e3faa42..97c60b066d 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c +++ b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.c @@ -143,9 +143,151 @@ void *ocp_nlp_globalization_merit_backtracking_memory_assign(void *config_, void * functions ************************************************/ -int ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, +static double ocp_nlp_compute_merit_gradient(ocp_nlp_config *config, ocp_nlp_dims *dims, + ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts, + ocp_nlp_memory *mem, ocp_nlp_workspace *work) +{ + /* computes merit function gradient at iterate: out -- using already evaluated gradients of submodules + with weights: work->weight_merit_fun */ + int i, j; + + int N = dims->N; + int *nv = dims->nv; + int *nx = dims->nx; + int *nu = dims->nu; + int *ni = dims->ni; + + double merit_grad = 0.0; + double weight; + + // NOTE: step is in: mem->qp_out->ux + struct blasfeo_dvec *tmp_vec; // size nv + struct blasfeo_dvec tmp_vec_nxu = work->tmp_nv; // size nxu + struct blasfeo_dvec dxnext_dy = work->dxnext_dy; // size nx + + // cost + for (i=0; i<=N; i++) + { + tmp_vec = config->cost[i]->memory_get_grad_ptr(mem->cost[i]); + merit_grad += blasfeo_ddot(nv[i], tmp_vec, 0, mem->qp_out->ux + i, 0); + } + double merit_grad_cost = merit_grad; + + /* dynamics */ + double merit_grad_dyn = 0.0; + for (i=0; idynamics[i]->memory_get_fun_ptr(mem->dynamics[i]); + + /* compute directional derivative of xnext with direction y -> dxnext_dy */ + blasfeo_dgemv_t(nx[i]+nu[i], nx[i+1], 1.0, mem->qp_in->BAbt+i, 0, 0, mem->qp_out->ux+i, 0, + 0.0, &dxnext_dy, 0, &dxnext_dy, 0); + + /* add merit gradient contributions depending on sign of shooting gap */ + for (j = 0; j < nx[i+1]; j++) + { + weight = BLASFEO_DVECEL(work->weight_merit_fun->pi+i, j); + double deqj_dy = BLASFEO_DVECEL(&dxnext_dy, j) - BLASFEO_DVECEL(mem->qp_out->ux+(i+1), nu[i+1]+j); + { + if (BLASFEO_DVECEL(tmp_vec, j) > 0) + { + merit_grad_dyn += weight * deqj_dy; + // printf("\ndyn_contribution +%e, weight %e, deqj_dy %e, i %d, j %d", weight * deqj_dy, weight, deqj_dy, i, j); + } + else + { + merit_grad_dyn -= weight * deqj_dy; + // printf("\ndyn_contribution %e, weight %e, deqj_dy %e, i %d, j %d", -weight * deqj_dy, weight, deqj_dy, i, j); + } + } + } + } + + /* inequality contributions */ + // NOTE: slack bound inequalities are not considered here. + // They should never be infeasible. Only if explicitly initialized infeasible from outside. + int constr_index, slack_index_in_ux, slack_index; + ocp_qp_dims* qp_dims = mem->qp_in->dim; + int *nb = qp_dims->nb; + int *ng = qp_dims->ng; + int *ns = qp_dims->ns; + double merit_grad_ineq = 0.0; + double slack_step; + + for (i=0; i<=N; i++) + { + tmp_vec = config->constraints[i]->memory_get_fun_ptr(mem->constraints[i]); + int *idxb = mem->qp_in->idxb[i]; + if (ni[i] > 0) + { + // NOTE: loop could be simplified handling lower and upper constraints together. + for (j = 0; j < 2 * (nb[i] + ng[i]); j++) // 2 * ni + { + double constraint_val = BLASFEO_DVECEL(tmp_vec, j); + if (constraint_val > 0) + { + weight = BLASFEO_DVECEL(work->weight_merit_fun->lam+i, j); + + // find corresponding slack value + constr_index = j < nb[i]+ng[i] ? j : j-(nb[i]+ng[i]); + slack_index = mem->qp_in->idxs_rev[i][constr_index]; + // if softened: add slack contribution + if (slack_index >= 0) + { + slack_index_in_ux = j < (nb[i]+ng[i]) ? nx[i] + nu[i] + slack_index + : nx[i] + nu[i] + slack_index + ns[i]; + slack_step = BLASFEO_DVECEL(mem->qp_out->ux+i, slack_index_in_ux); + merit_grad_ineq -= weight * slack_step; + // printf("at node %d, ineq %d, idxs_rev[%d] = %d\n", i, j, constr_index, slack_index); + // printf("slack contribution: uxs[%d] = %e\n", slack_index_in_ux, slack_step); + } + + + // NOTE: the inequalities are internally organized in the following order: + // [ lbu lbx lg lh lphi ubu ubx ug uh uphi; + // lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + // printf("constraint %d %d is active with value %e", i, j, constraint_val); + if (j < nb[i]) + { + // printf("lower idxb[%d] = %d dir %f, constraint_val %f, nb = %d\n", j, idxb[j], BLASFEO_DVECEL(mem->qp_out->ux, idxb[j]), constraint_val, nb[i]); + merit_grad_ineq += weight * BLASFEO_DVECEL(mem->qp_out->ux+i, idxb[j]); + } + else if (j < nb[i] + ng[i]) + { + // merit_grad_ineq += weight * mem->qp_in->DCt_j * dux + blasfeo_dcolex(nx[i] + nu[i], mem->qp_in->DCt+i, j - nb[i], 0, &tmp_vec_nxu, 0); + merit_grad_ineq += weight * blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0); + // printf("general linear constraint lower contribution = %e, val = %e\n", blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0), constraint_val); + } + else if (j < 2*nb[i] + ng[i]) + { + // printf("upper idxb[%d] = %d dir %f, constraint_val %f, nb = %d\n", j-nb[i]-ng[i], idxb[j-nb[i]-ng[i]], BLASFEO_DVECEL(mem->qp_out->ux, idxb[j-nb[i]-ng[i]]), constraint_val, nb[i]); + merit_grad_ineq += weight * BLASFEO_DVECEL(mem->qp_out->ux+i, idxb[j-nb[i]-ng[i]]); + } + else if (j < 2*nb[i] + 2*ng[i]) + { + blasfeo_dcolex(nx[i] + nu[i], mem->qp_in->DCt+i, j - 2*nb[i] - ng[i], 0, &tmp_vec_nxu, 0); + merit_grad_ineq += weight * blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0); + // printf("general linear constraint upper contribution = %e, val = %e\n", blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0), constraint_val); + } + } + } + } + } + // print_ocp_qp_dims(qp_dims); + // print_ocp_qp_in(mem->qp_in); + + merit_grad = merit_grad_cost + merit_grad_dyn + merit_grad_ineq; + if (opts->print_level > 1) + printf("computed merit_grad = %e, merit_grad_cost = %e, merit_grad_dyn = %e, merit_grad_ineq = %e\n", merit_grad, merit_grad_cost, merit_grad_dyn, merit_grad_ineq); + + return merit_grad; +} + +static int ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, - int sqp_iter, double *alpha_reference) + double *alpha_reference) { ocp_nlp_globalization_merit_backtracking_opts *merit_opts = opts->globalization; ocp_nlp_globalization_opts *globalization_opts = merit_opts->globalization_opts; @@ -174,7 +316,7 @@ int ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in * // } /* modify/initialize merit function weights (Leineweber1999 M5.1, p.89) */ - if (sqp_iter==0) + if (mem->iter==0) { merit_backtracking_initialize_weights(dims, work->weight_merit_fun, qp_out); } @@ -184,9 +326,6 @@ int ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in * } // TODO: why does Leineweber do full step in first SQP iter? - // if (sqp_iter == 0) - // { - // } double merit_fun0 = ocp_nlp_evaluate_merit_fun(config, dims, in, out, opts, mem, work); @@ -350,7 +489,7 @@ static void copy_multipliers_qp_to_nlp(ocp_nlp_dims *dims, ocp_qp_out *from, ocp } static int ocp_nlp_line_search_merit_check_full_step(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, - ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, int sqp_iter) + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work) { int N = dims->N; int *nv = dims->nv; @@ -364,9 +503,8 @@ static int ocp_nlp_line_search_merit_check_full_step(ocp_nlp_config *config, ocp blasfeo_dveccp(nv[i], out->ux+i, 0, work->tmp_nlp_out->ux+i, 0); // NOTE: copying duals not needed, as they dont enter the merit function, see ocp_nlp_line_search - /* modify/initialize merit function weights (Leineweber1999 M5.1, p.89) */ - if (sqp_iter==0) + if (mem->iter==0) { merit_backtracking_initialize_weights(dims, work->weight_merit_fun, qp_out); } @@ -398,7 +536,7 @@ static int ocp_nlp_line_search_merit_check_full_step(ocp_nlp_config *config, ocp if (isnan(merit_fun1) || isinf(merit_fun1)) { // do nothing and continue with normal line search, i.e. step reduction - if (sqp_iter != 0) + if (mem->iter != 0) { // reset merit function weights; copy_multipliers_qp_to_nlp(dims, work->tmp_qp_out, work->weight_merit_fun); @@ -414,7 +552,7 @@ static int ocp_nlp_line_search_merit_check_full_step(ocp_nlp_config *config, ocp else { // trigger SOC - if (sqp_iter != 0) + if (mem->iter != 0) { // reset merit function weights; copy_multipliers_qp_to_nlp(dims, work->tmp_qp_out, work->weight_merit_fun); @@ -424,7 +562,7 @@ static int ocp_nlp_line_search_merit_check_full_step(ocp_nlp_config *config, ocp } static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *nlp_in, - ocp_nlp_out *nlp_out, ocp_nlp_opts *nlp_opts, ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work, int sqp_iter) + ocp_nlp_out *nlp_out, ocp_nlp_opts *nlp_opts, ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work) { int ii; int N = dims->N; @@ -441,7 +579,7 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, // NOTE: the "and" is interpreted as an "or" in the current implementation // preliminary line search - int line_search_status = ocp_nlp_line_search_merit_check_full_step(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, sqp_iter); + int line_search_status = ocp_nlp_line_search_merit_check_full_step(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); // return bool do_line_search; if (line_search_status == ACADOS_NAN_DETECTED) @@ -539,12 +677,12 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, if (nlp_opts->print_level > 3) { - printf("\n\nSQP: SOC ocp_qp_in at iteration %d\n", sqp_iter); + printf("\n\nSQP: SOC ocp_qp_in at iteration %d\n", nlp_mem->iter); // print_ocp_qp_in(qp_in); } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_in_to_file(qp_in, sqp_iter, 1); + ocp_nlp_dump_qp_in_to_file(qp_in, nlp_mem->iter, 1); #endif // solve QP @@ -562,29 +700,29 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, // save statistics of last qp solver call // TODO: SOC QP solver call should be warm / hot started! - // if (sqp_iter+1 < nlp_mem->stat_m) + // if (nlp_mem->iter+1 < nlp_mem->stat_m) // { - // // mem->stat[mem->stat_n*(sqp_iter+1)+4] = qp_status; + // // mem->stat[mem->stat_n*(nlp_mem->iter+1)+4] = qp_status; // // add qp_iter; should maybe be in a seperate statistic - // nlp_mem->stat[nlp_mem->stat_n*(sqp_iter+1)+5] += qp_iter; + // nlp_mem->stat[nlp_mem->stat_n*(nlp_mem->iter+1)+5] += qp_iter; // } // compute external QP residuals (for debugging) // if (nlp_opts->ext_qp_res) // { // ocp_qp_res_compute(qp_in, qp_out, nlp_work->qp_res, nlp_work->qp_res_ws); - // if (sqp_iter+1 < nlp_mem->stat_m) - // ocp_qp_res_compute_nrm_inf(nlp_work->qp_res, nlp_mem->stat+(nlp_mem->stat_n*(sqp_iter+1)+7)); + // if (nlp_mem->iter+1 < nlp_mem->stat_m) + // ocp_qp_res_compute_nrm_inf(nlp_work->qp_res, nlp_mem->stat+(nlp_mem->stat_n*(nlp_mem->iter+1)+7)); // } // if (nlp_opts->print_level > 3) // { - // printf("\n\nSQP: SOC ocp_qp_out at iteration %d\n", sqp_iter); + // printf("\n\nSQP: SOC ocp_qp_out at iteration %d\n", nlp_mem->iter); // print_ocp_qp_out(qp_out); // } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_out_to_file(qp_out, sqp_iter, 1); + ocp_nlp_dump_qp_out_to_file(qp_out, nlp_mem->iter, 1); #endif // exit conditions on QP status @@ -592,7 +730,7 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, { #ifndef ACADOS_SILENT printf("\nQP solver returned error status %d in SQP iteration %d for SOC QP.\n", - qp_status, sqp_iter); + qp_status, nlp_mem->iter); #endif // if (nlp_opts->print_level > 1) // { @@ -600,157 +738,13 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, // if (nlp_opts->print_level > 3) // print_ocp_qp_in(qp_in); // } - nlp_mem->status = ACADOS_QP_FAILURE; - nlp_mem->iter = sqp_iter; return true; } return true; } -double ocp_nlp_compute_merit_gradient(ocp_nlp_config *config, ocp_nlp_dims *dims, - ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts, - ocp_nlp_memory *mem, ocp_nlp_workspace *work) -{ - /* computes merit function gradient at iterate: out -- using already evaluated gradients of submodules - with weights: work->weight_merit_fun */ - int i, j; - - int N = dims->N; - int *nv = dims->nv; - int *nx = dims->nx; - int *nu = dims->nu; - int *ni = dims->ni; - - double merit_grad = 0.0; - double weight; - - // NOTE: step is in: mem->qp_out->ux - struct blasfeo_dvec *tmp_vec; // size nv - struct blasfeo_dvec tmp_vec_nxu = work->tmp_nv; // size nxu - struct blasfeo_dvec dxnext_dy = work->dxnext_dy; // size nx - - // cost - for (i=0; i<=N; i++) - { - tmp_vec = config->cost[i]->memory_get_grad_ptr(mem->cost[i]); - merit_grad += blasfeo_ddot(nv[i], tmp_vec, 0, mem->qp_out->ux + i, 0); - } - double merit_grad_cost = merit_grad; - - /* dynamics */ - double merit_grad_dyn = 0.0; - for (i=0; idynamics[i]->memory_get_fun_ptr(mem->dynamics[i]); - - /* compute directional derivative of xnext with direction y -> dxnext_dy */ - blasfeo_dgemv_t(nx[i]+nu[i], nx[i+1], 1.0, mem->qp_in->BAbt+i, 0, 0, mem->qp_out->ux+i, 0, - 0.0, &dxnext_dy, 0, &dxnext_dy, 0); - - /* add merit gradient contributions depending on sign of shooting gap */ - for (j = 0; j < nx[i+1]; j++) - { - weight = BLASFEO_DVECEL(work->weight_merit_fun->pi+i, j); - double deqj_dy = BLASFEO_DVECEL(&dxnext_dy, j) - BLASFEO_DVECEL(mem->qp_out->ux+(i+1), nu[i+1]+j); - { - if (BLASFEO_DVECEL(tmp_vec, j) > 0) - { - merit_grad_dyn += weight * deqj_dy; - // printf("\ndyn_contribution +%e, weight %e, deqj_dy %e, i %d, j %d", weight * deqj_dy, weight, deqj_dy, i, j); - } - else - { - merit_grad_dyn -= weight * deqj_dy; - // printf("\ndyn_contribution %e, weight %e, deqj_dy %e, i %d, j %d", -weight * deqj_dy, weight, deqj_dy, i, j); - } - } - } - } - - /* inequality contributions */ - // NOTE: slack bound inequalities are not considered here. - // They should never be infeasible. Only if explicitly initialized infeasible from outside. - int constr_index, slack_index_in_ux, slack_index; - ocp_qp_dims* qp_dims = mem->qp_in->dim; - int *nb = qp_dims->nb; - int *ng = qp_dims->ng; - int *ns = qp_dims->ns; - double merit_grad_ineq = 0.0; - double slack_step; - - for (i=0; i<=N; i++) - { - tmp_vec = config->constraints[i]->memory_get_fun_ptr(mem->constraints[i]); - int *idxb = mem->qp_in->idxb[i]; - if (ni[i] > 0) - { - // NOTE: loop could be simplified handling lower and upper constraints together. - for (j = 0; j < 2 * (nb[i] + ng[i]); j++) // 2 * ni - { - double constraint_val = BLASFEO_DVECEL(tmp_vec, j); - if (constraint_val > 0) - { - weight = BLASFEO_DVECEL(work->weight_merit_fun->lam+i, j); - - // find corresponding slack value - constr_index = j < nb[i]+ng[i] ? j : j-(nb[i]+ng[i]); - slack_index = mem->qp_in->idxs_rev[i][constr_index]; - // if softened: add slack contribution - if (slack_index >= 0) - { - slack_index_in_ux = j < (nb[i]+ng[i]) ? nx[i] + nu[i] + slack_index - : nx[i] + nu[i] + slack_index + ns[i]; - slack_step = BLASFEO_DVECEL(mem->qp_out->ux+i, slack_index_in_ux); - merit_grad_ineq -= weight * slack_step; - // printf("at node %d, ineq %d, idxs_rev[%d] = %d\n", i, j, constr_index, slack_index); - // printf("slack contribution: uxs[%d] = %e\n", slack_index_in_ux, slack_step); - } - - - // NOTE: the inequalities are internally organized in the following order: - // [ lbu lbx lg lh lphi ubu ubx ug uh uphi; - // lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] - // printf("constraint %d %d is active with value %e", i, j, constraint_val); - if (j < nb[i]) - { - // printf("lower idxb[%d] = %d dir %f, constraint_val %f, nb = %d\n", j, idxb[j], BLASFEO_DVECEL(mem->qp_out->ux, idxb[j]), constraint_val, nb[i]); - merit_grad_ineq += weight * BLASFEO_DVECEL(mem->qp_out->ux+i, idxb[j]); - } - else if (j < nb[i] + ng[i]) - { - // merit_grad_ineq += weight * mem->qp_in->DCt_j * dux - blasfeo_dcolex(nx[i] + nu[i], mem->qp_in->DCt+i, j - nb[i], 0, &tmp_vec_nxu, 0); - merit_grad_ineq += weight * blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0); - // printf("general linear constraint lower contribution = %e, val = %e\n", blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0), constraint_val); - } - else if (j < 2*nb[i] + ng[i]) - { - // printf("upper idxb[%d] = %d dir %f, constraint_val %f, nb = %d\n", j-nb[i]-ng[i], idxb[j-nb[i]-ng[i]], BLASFEO_DVECEL(mem->qp_out->ux, idxb[j-nb[i]-ng[i]]), constraint_val, nb[i]); - merit_grad_ineq += weight * BLASFEO_DVECEL(mem->qp_out->ux+i, idxb[j-nb[i]-ng[i]]); - } - else if (j < 2*nb[i] + 2*ng[i]) - { - blasfeo_dcolex(nx[i] + nu[i], mem->qp_in->DCt+i, j - 2*nb[i] - ng[i], 0, &tmp_vec_nxu, 0); - merit_grad_ineq += weight * blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0); - // printf("general linear constraint upper contribution = %e, val = %e\n", blasfeo_ddot(nx[i] + nu[i], &tmp_vec_nxu, 0, mem->qp_out->ux+i, 0), constraint_val); - } - } - } - } - } - // print_ocp_qp_dims(qp_dims); - // print_ocp_qp_in(mem->qp_in); - - merit_grad = merit_grad_cost + merit_grad_dyn + merit_grad_ineq; - if (opts->print_level > 1) - printf("computed merit_grad = %e, merit_grad_cost = %e, merit_grad_dyn = %e, merit_grad_ineq = %e\n", merit_grad, merit_grad_cost, merit_grad_dyn, merit_grad_ineq); - - return merit_grad; -} - double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, @@ -1018,12 +1012,11 @@ int ocp_nlp_globalization_merit_backtracking_find_acceptable_iterate(void *nlp_c ocp_nlp_globalization_merit_backtracking_opts *merit_opts = nlp_opts->globalization; ocp_nlp_globalization_opts *globalization_opts = merit_opts->globalization_opts; - int sqp_iter = 1; // NEEDS TO BE CHANGED HERE bool do_line_search = true; if (merit_opts->globalization_opts->use_SOC) { - do_line_search = ocp_nlp_soc_line_search(nlp_config, nlp_dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, sqp_iter); + do_line_search = ocp_nlp_soc_line_search(nlp_config, nlp_dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); if (nlp_mem->status == ACADOS_QP_FAILURE) { return nlp_mem->status; @@ -1033,7 +1026,7 @@ int ocp_nlp_globalization_merit_backtracking_find_acceptable_iterate(void *nlp_c if (do_line_search) { int line_search_status; - line_search_status = ocp_nlp_line_search(nlp_config, nlp_dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, sqp_iter, &mem->alpha); + line_search_status = ocp_nlp_line_search(nlp_config, nlp_dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, &mem->alpha); if (line_search_status == ACADOS_NAN_DETECTED) { nlp_mem->status = ACADOS_NAN_DETECTED; diff --git a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.h b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.h index 263d80d77c..3d6f368c80 100644 --- a/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.h +++ b/acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.h @@ -89,14 +89,7 @@ void *ocp_nlp_globalization_merit_backtracking_memory_assign(void *config, void /************************************************ * functions ************************************************/ -// -double ocp_nlp_compute_merit_gradient(ocp_nlp_config *config, ocp_nlp_dims *dims, - ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts, - ocp_nlp_memory *mem, ocp_nlp_workspace *work); -// -int ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, - ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, - int sqp_iter, double *alpha_ref); + // double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); diff --git a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py index f792505493..9be5054f7e 100644 --- a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py +++ b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py @@ -71,6 +71,8 @@ def solve_maratos_problem_with_setting(setting): line_search_use_sufficient_descent = setting['line_search_use_sufficient_descent'] globalization_use_SOC = setting['globalization_use_SOC'] + print(f"running maratos test problem with settings {setting}") + # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -139,10 +141,10 @@ def solve_maratos_problem_with_setting(setting): if FOR_LOOPING: # call solver in for loop to get all iterates ocp.solver_options.nlp_solver_max_iter = 1 - ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') + ocp_solver = AcadosOcpSolver(ocp, verbose=False) else: ocp.solver_options.nlp_solver_max_iter = SQP_max_iter - ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') + ocp_solver = AcadosOcpSolver(ocp, verbose=False) # initialize solver rad_init = 0.1 #0.1 #np.pi / 4 @@ -210,8 +212,8 @@ def solve_maratos_problem_with_setting(setting): if any(alphas[:iter] != 1.0): raise Exception(f"Expected all alphas = 1.0 when using full step SQP on Maratos problem") elif globalization == 'MERIT_BACKTRACKING': - if max_infeasibility > 0.5: - raise Exception(f"Expected max_infeasibility < 0.5 when using globalized SQP on Maratos problem") + if max_infeasibility > 1.5: + raise Exception(f"Expected max_infeasibility < 1.5 when using globalized SQP on Maratos problem") if globalization_use_SOC == 0: if FOR_LOOPING and iter != 57: raise Exception(f"Expected 57 SQP iterations when using globalized SQP without SOC on Maratos problem, got {iter}") @@ -224,8 +226,8 @@ def solve_maratos_problem_with_setting(setting): # Jonathan Laptop: merit_grad = -1.737950e-01, merit_grad_cost = -1.737950e-01, merit_grad_dyn = 0.000000e+00, merit_grad_ineq = 0.000000e+00 raise Exception(f"Expected SQP iterations in range(29, 37) when using globalized SQP with SOC on Maratos problem, got {iter}") else: - if iter != 16: - raise Exception(f"Expected 16 SQP iterations when using globalized SQP with SOC on Maratos problem, got {iter}") + if iter != 10: + raise Exception(f"Expected 10 SQP iterations when using globalized SQP with SOC on Maratos problem, got {iter}") elif globalization == 'FUNNEL_L1PEN_LINESEARCH': if iter > 12: raise Exception(f"Expected not more than 12 SQP iterations when using Funnel Method SQP, got {iter}") From e2fa504f811bf8018d486b0e2dc4304852f8fe9b Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 9 Apr 2025 14:54:34 +0200 Subject: [PATCH 03/24] C: cleanup `qp_res` option and memory (#1494) It was already in the common workspace. - move `qp_res` memory management to common fully - remove option ext_qp_res from nlp solvers, as it was moved to common in #1254 --- acados/ocp_nlp/ocp_nlp_common.c | 21 ++++++++++ acados/ocp_nlp/ocp_nlp_ddp.c | 35 ++--------------- acados/ocp_nlp/ocp_nlp_ddp.h | 1 - acados/ocp_nlp/ocp_nlp_sqp.c | 20 ---------- acados/ocp_nlp/ocp_nlp_sqp.h | 1 - acados/ocp_nlp/ocp_nlp_sqp_rti.c | 39 +++---------------- acados/ocp_nlp/ocp_nlp_sqp_rti.h | 1 - acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 1 - 8 files changed, 31 insertions(+), 88 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 65d6636454..2635d0f709 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -2105,6 +2105,16 @@ acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_d ext_fun_workspace_size += dynamics[i]->get_external_fun_workspace_requirement(dynamics[i], dims->dynamics[i], opts->dynamics[i], in->dynamics[i]); } } + + if (opts->ext_qp_res) + { + // qp_res + size += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); + + // qp_res_ws + size += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); + } + size += 64; // ext_fun_workspace_size align size += ext_fun_workspace_size; @@ -2349,6 +2359,17 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims } } + if (opts->ext_qp_res) + { + // qp res + work->qp_res = ocp_qp_res_assign(dims->qp_solver->orig_dims, c_ptr); + c_ptr += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); + + // qp res ws + work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver->orig_dims, c_ptr); + c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); + } + assert((char *) work + mem->workspace_size >= c_ptr); return work; diff --git a/acados/ocp_nlp/ocp_nlp_ddp.c b/acados/ocp_nlp/ocp_nlp_ddp.c index 13143d2a22..ba8fcec33f 100644 --- a/acados/ocp_nlp/ocp_nlp_ddp.c +++ b/acados/ocp_nlp/ocp_nlp_ddp.c @@ -117,8 +117,6 @@ void ocp_nlp_ddp_opts_initialize_default(void *config_, void *dims_, void *opts_ opts->tol_comp = 1e-8; opts->tol_zero_res = 1e-12; - opts->ext_qp_res = 0; - opts->warm_start_first_qp = false; opts->warm_start_first_qp_from_nlp = false; opts->eval_residual_at_max_iter = false; @@ -195,11 +193,6 @@ void ocp_nlp_ddp_opts_set(void *config_, void *opts_, const char *field, void* v // TODO: set accuracy of the qp_solver to the minimum of current QP accuracy and the one specified. config->qp_solver->opts_set(config->qp_solver, opts->nlp_opts->qp_solver_opts, "tol_comp", value); } - else if (!strcmp(field, "ext_qp_res")) - { - int* ext_qp_res = (int *) value; - opts->ext_qp_res = *ext_qp_res; - } else if (!strcmp(field, "warm_start_first_qp")) { bool* warm_start_first_qp = (bool *) value; @@ -265,7 +258,7 @@ acados_size_t ocp_nlp_ddp_memory_calculate_size(void *config_, void *dims_, void // stat int stat_m = opts->nlp_opts->max_iter+1; int stat_n = 7; - if (opts->ext_qp_res) + if (nlp_opts->ext_qp_res) stat_n += 4; size += stat_n*stat_m*sizeof(double); @@ -332,9 +325,9 @@ void *ocp_nlp_ddp_memory_assign(void *config_, void *dims_, void *opts_, void *i // stat mem->stat = (double *) c_ptr; - mem->stat_m = opts->nlp_opts->max_iter+1; + mem->stat_m = nlp_opts->max_iter+1; mem->stat_n = 7; - if (opts->ext_qp_res) + if (nlp_opts->ext_qp_res) mem->stat_n += 4; c_ptr += mem->stat_m*mem->stat_n*sizeof(double); @@ -373,15 +366,6 @@ acados_size_t ocp_nlp_ddp_workspace_calculate_size(void *config_, void *dims_, v // nlp size += ocp_nlp_workspace_calculate_size(config, dims, nlp_opts, nlp_in); - if (opts->ext_qp_res) - { - // qp res - size += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - size += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - return size; } @@ -401,17 +385,6 @@ static void ocp_nlp_ddp_cast_workspace(ocp_nlp_config *config, ocp_nlp_dims *dim work->nlp_work = ocp_nlp_workspace_assign(config, dims, nlp_opts, nlp_in, nlp_mem, c_ptr); c_ptr += ocp_nlp_workspace_calculate_size(config, dims, nlp_opts, nlp_in); - if (opts->ext_qp_res) - { - // qp res - work->qp_res = ocp_qp_res_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - assert((char *) work + ocp_nlp_ddp_workspace_calculate_size(config, dims, opts, nlp_in) >= c_ptr); return; @@ -820,7 +793,7 @@ int ocp_nlp_ddp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // compute external QP residuals (for debugging) - if (opts->ext_qp_res) + if (nlp_opts->ext_qp_res) { ocp_qp_res_compute(qp_in, qp_out, work->qp_res, work->qp_res_ws); if (ddp_iter+1 < mem->stat_m) diff --git a/acados/ocp_nlp/ocp_nlp_ddp.h b/acados/ocp_nlp/ocp_nlp_ddp.h index a1b0a2cb8d..a8ddafa584 100644 --- a/acados/ocp_nlp/ocp_nlp_ddp.h +++ b/acados/ocp_nlp/ocp_nlp_ddp.h @@ -61,7 +61,6 @@ typedef struct double tol_ineq; // exit tolerance on inequality constraints double tol_comp; // exit tolerance on complementarity condition double tol_zero_res; // exit tolerance if objective function is 0 for least-squares problem - int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging) bool warm_start_first_qp; // to set qp_warm_start in first iteration bool warm_start_first_qp_from_nlp; bool eval_residual_at_max_iter; // if convergence should be checked after last iterations or only throw max_iter reached diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 2b16f5a216..a824c2fffc 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -379,15 +379,6 @@ acados_size_t ocp_nlp_sqp_workspace_calculate_size(void *config_, void *dims_, v // nlp size += ocp_nlp_workspace_calculate_size(config, dims, nlp_opts, in); - if (nlp_opts->ext_qp_res) - { - // qp res - size += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - size += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - return size; } @@ -407,17 +398,6 @@ static void ocp_nlp_sqp_cast_workspace(ocp_nlp_config *config, ocp_nlp_dims *dim work->nlp_work = ocp_nlp_workspace_assign(config, dims, nlp_opts, in, nlp_mem, c_ptr); c_ptr += ocp_nlp_workspace_calculate_size(config, dims, nlp_opts, in); - if (nlp_opts->ext_qp_res) - { - // qp res - work->nlp_work->qp_res = ocp_qp_res_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - work->nlp_work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - assert((char *) work + ocp_nlp_sqp_workspace_calculate_size(config, dims, opts, in) >= c_ptr); return; diff --git a/acados/ocp_nlp/ocp_nlp_sqp.h b/acados/ocp_nlp/ocp_nlp_sqp.h index 58cf72dc57..07ca3d5564 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp.h @@ -62,7 +62,6 @@ typedef struct double tol_comp; // exit tolerance on complementarity condition double tol_unbounded; // exit threshold when objective function seems to be unbounded double tol_min_step_norm; // exit tolerance for small step - int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging) int log_primal_step_norm; // compute and log the max norm of the primal steps bool warm_start_first_qp; // to set qp_warm_start in first iteration bool warm_start_first_qp_from_nlp; // if True first QP will be initialized using values from NLP iterate, otherwise from previous QP solution. diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index 833a06938e..945d985593 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -113,7 +113,6 @@ void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, ocp_nlp_opts_initialize_default(config, dims, nlp_opts); // SQP RTI opts - opts->ext_qp_res = 0; opts->warm_start_first_qp = false; opts->warm_start_first_qp_from_nlp = true; opts->rti_phase = 0; @@ -161,12 +160,7 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, } else // nlp opts { - if (!strcmp(field, "ext_qp_res")) - { - int* ext_qp_res = (int *) value; - opts->ext_qp_res = *ext_qp_res; - } - else if (!strcmp(field, "warm_start_first_qp")) + if (!strcmp(field, "warm_start_first_qp")) { bool* warm_start_first_qp = (bool *) value; opts->warm_start_first_qp = *warm_start_first_qp; @@ -256,7 +250,7 @@ acados_size_t ocp_nlp_sqp_rti_memory_calculate_size(void *config_, int stat_n = 2; // qp_status, qp_iter if (opts->rti_log_residuals) stat_n += 4; // nlp_res - if (opts->ext_qp_res) + if (nlp_opts->ext_qp_res) stat_n += 4; // qp_res size += stat_n*stat_m*sizeof(double); @@ -296,7 +290,7 @@ void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, mem->stat_n = 2; // qp_status, qp_iter if (opts->rti_log_residuals) mem->stat_n += 4; // nlp_res - if (opts->ext_qp_res) + if (nlp_opts->ext_qp_res) mem->stat_n += 4; // qp_res c_ptr += mem->stat_m*mem->stat_n*sizeof(double); @@ -336,15 +330,6 @@ acados_size_t ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, // nlp size += ocp_nlp_workspace_calculate_size(config, dims, nlp_opts, in); - if (opts->ext_qp_res) - { - // qp res - size += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - size += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - return size; } @@ -366,19 +351,6 @@ static void ocp_nlp_sqp_rti_cast_workspace( config, dims, nlp_opts, nlp_in, nlp_mem, c_ptr); c_ptr += ocp_nlp_workspace_calculate_size(config, dims, nlp_opts, nlp_in); - if (opts->ext_qp_res) - { - // qp res - work->nlp_work->qp_res = ocp_qp_res_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - work->nlp_work->qp_res_ws = ocp_qp_res_workspace_assign( - dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_workspace_calculate_size( - dims->qp_solver->orig_dims); - } - assert((char *) work + ocp_nlp_sqp_rti_workspace_calculate_size(config, dims, opts, nlp_in) >= c_ptr); @@ -404,9 +376,10 @@ static void rti_store_residuals_in_stats(ocp_nlp_sqp_rti_opts *opts, ocp_nlp_sqp { ocp_nlp_memory *nlp_mem = mem->nlp_mem; ocp_nlp_res *nlp_res = nlp_mem->nlp_res; + ocp_nlp_opts *nlp_opts = opts->nlp_opts; if (nlp_mem->iter < mem->stat_m) { - int m_offset = 2 + 4 * opts->ext_qp_res; + int m_offset = 2 + 4 * nlp_opts->ext_qp_res; // printf("storing residuals AS RTI, m_offset %d\n", m_offset); // printf("%e\t%e\t%e\t%e\n", nlp_res->inf_norm_res_stat, nlp_res->inf_norm_res_eq, nlp_res->inf_norm_res_ineq, nlp_res->inf_norm_res_comp); mem->stat[mem->stat_n * nlp_mem->iter+0+m_offset] = nlp_res->inf_norm_res_stat; @@ -626,7 +599,7 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims * } // compute external QP residuals (for debugging) - if (opts->ext_qp_res) + if (nlp_opts->ext_qp_res) { ocp_qp_res_compute(nlp_mem->qp_in, nlp_mem->qp_out, work->qp_res, work->qp_res_ws); ocp_qp_res_compute_nrm_inf(work->qp_res, mem->stat+(mem->stat_n*1+2)); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/acados/ocp_nlp/ocp_nlp_sqp_rti.h index 8f34708cf3..40ddaa0bdf 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.h @@ -80,7 +80,6 @@ typedef struct { ocp_nlp_opts *nlp_opts; int compute_dual_sol; - int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging) bool warm_start_first_qp; // to set qp_warm_start in first iteration bool warm_start_first_qp_from_nlp; rti_phase_t rti_phase; diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index aab1ac0886..6a68558907 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -1226,7 +1226,6 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, if (config->globalization->needs_objective_value() == 1) { - l1_inf_QP_feasibility = calculate_qp_l1_infeasibility(dims, mem, work, opts, relaxed_qp_in, relaxed_qp_out); mem->pred_l1_inf_QP = calculate_pred_l1_inf(opts, mem, l1_inf_QP_feasibility); } From 20ac01be7fc552acb0b4c341c46ce930eeab1f83 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 9 Apr 2025 15:23:55 +0200 Subject: [PATCH 04/24] Step norm logging (#1495) - move primal_step_norm to common memory, as option is also there - Interfaces: fix log_primal_step_norm for SQP_WFQP and multi-phase - add option ` log_dual_step_norm` - test getting step norms in MATLAB and Python Bit unrelated: - SQP_WFQP: remove max_iter duplicated from common --- acados/ocp_nlp/ocp_nlp_common.c | 88 +++++++++++++++++++ acados/ocp_nlp/ocp_nlp_common.h | 6 ++ acados/ocp_nlp/ocp_nlp_sqp.c | 47 +++------- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 42 ++------- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h | 3 - .../getting_started/extensive_example_ocp.m | 9 ++ .../ocp/example_ocp_dynamics_formulations.py | 15 +++- .../acados_matlab_octave/AcadosOcpOptions.m | 2 + interfaces/acados_matlab_octave/ocp_get.c | 8 ++ .../acados_template/acados_ocp_options.py | 31 +++++-- .../acados_template/acados_ocp_solver.py | 2 +- .../c_templates_tera/acados_multi_solver.in.c | 15 +++- .../c_templates_tera/acados_solver.in.c | 5 +- 13 files changed, 190 insertions(+), 83 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 2635d0f709..893a19a3bc 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -1207,6 +1207,7 @@ void ocp_nlp_opts_initialize_default(void *config_, void *dims_, void *opts_) opts->print_level = 0; opts->levenberg_marquardt = 0.0; opts->log_primal_step_norm = 0; + opts->log_dual_step_norm = 0; opts->max_iter = 1; /* submodules opts */ @@ -1433,6 +1434,11 @@ void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value int* log_primal_step_norm = (int *) value; opts->log_primal_step_norm = *log_primal_step_norm; } + else if (!strcmp(field, "log_dual_step_norm")) + { + int* log_dual_step_norm = (int *) value; + opts->log_dual_step_norm = *log_dual_step_norm; + } else if (!strcmp(field, "max_iter")) { int* max_iter = (int *) value; @@ -1631,6 +1637,16 @@ acados_size_t ocp_nlp_memory_calculate_size(ocp_nlp_config *config, ocp_nlp_dims size += sizeof(struct ocp_nlp_timings); size += (N+1)*sizeof(bool); // set_sim_guess + // primal step norm + if (opts->log_primal_step_norm) + { + size += opts->max_iter*sizeof(double); + } + // dual step norm + if (opts->log_dual_step_norm) + { + size += opts->max_iter*sizeof(double); + } size += (N+1)*sizeof(struct blasfeo_dmat); // dzduxt size += 6*(N+1)*sizeof(struct blasfeo_dvec); // cost_grad ineq_fun ineq_adj dyn_adj sim_guess z_alg @@ -1816,6 +1832,20 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims // sim_guess assign_and_advance_blasfeo_dvec_structs(N + 1, &mem->sim_guess, &c_ptr); + // primal step norm + if (opts->log_primal_step_norm) + { + mem->primal_step_norm = (double *) c_ptr; + c_ptr += opts->max_iter*sizeof(double); + } + + // dual step norm + if (opts->log_dual_step_norm) + { + mem->dual_step_norm = (double *) c_ptr; + c_ptr += opts->max_iter*sizeof(double); + } + // set_sim_guess assign_and_advance_bool(N+1, &mem->set_sim_guess, &c_ptr); for (i = 0; i <= N; ++i) @@ -3383,6 +3413,32 @@ void ocp_nlp_res_get_inf_norm(ocp_nlp_res *res, double *out) } +double ocp_nlp_compute_delta_dual_norm_inf(ocp_nlp_dims *dims, ocp_nlp_workspace *work, ocp_nlp_out *nlp_out, ocp_qp_out *qp_out) +{ + /* computes the inf norm of multipliers in qp_out and nlp_out */ + int N = dims->N; + int *nx = dims->nx; + int *ni = dims->ni; + double tmp; + double norm = 0.0; + // compute delta dual + for (int i = 0; i <= N; i++) + { + blasfeo_daxpy(2*ni[i], -1.0, nlp_out->lam+i, 0, qp_out->lam+i, 0, &work->tmp_2ni, 0); + blasfeo_dvecnrm_inf(2*ni[i], &work->tmp_2ni, 0, &tmp); + norm = norm > tmp ? norm : tmp; + if (i < N) + { + blasfeo_daxpy(nx[i+1], -1.0, nlp_out->pi+i, 0, qp_out->pi+i, 0, &work->tmp_nv, 0); + blasfeo_dvecnrm_inf(nx[i+1], &work->tmp_nv, 0, &tmp); + norm = norm > tmp ? norm : tmp; + } + } + return norm; +} + + + void copy_ocp_nlp_out(ocp_nlp_dims *dims, ocp_nlp_out *from, ocp_nlp_out *to) { // extract dims @@ -4106,6 +4162,38 @@ void ocp_nlp_memory_get(ocp_nlp_config *config, ocp_nlp_memory *nlp_mem, const c double *value = return_value_; *value = nlp_mem->cost_value; } + else if (!strcmp("primal_step_norm", field)) + { + if (nlp_mem->primal_step_norm == NULL) + { + printf("\nerror: options log_primal_step_norm was not set\n"); + exit(1); + } + else + { + double *value = return_value_; + for (int ii=0; iiiter; ii++) + { + value[ii] = nlp_mem->primal_step_norm[ii]; + } + } + } + else if (!strcmp("dual_step_norm", field)) + { + if (nlp_mem->dual_step_norm == NULL) + { + printf("\nerror: options log_dual_step_norm was not set\n"); + exit(1); + } + else + { + double *value = return_value_; + for (int ii=0; iiiter; ii++) + { + value[ii] = nlp_mem->dual_step_norm[ii]; + } + } + } else { printf("\nerror: field %s not available in ocp_nlp_memory_get\n", field); diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index f1a90e5a36..c8b9e6aa43 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -299,6 +299,7 @@ typedef struct ocp_nlp_opts int print_level; int fixed_hess; int log_primal_step_norm; // compute and log the max norm of the primal steps + int log_dual_step_norm; // compute and log the max norm of the dual steps int max_iter; // maximum number of (SQP/DDP) iterations int qp_iter_max; // maximum iter of QP solver, stored to remember. double tau_min; // minimum value of the barrier parameter, for IPMs @@ -446,6 +447,9 @@ typedef struct ocp_nlp_memory double adaptive_levenberg_marquardt_mu_bar; bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables + double *primal_step_norm; + double *dual_step_norm; + struct blasfeo_dvec *sim_guess; acados_size_t workspace_size; @@ -544,6 +548,8 @@ void ocp_nlp_initialize_qp_from_nlp(ocp_nlp_config *config, ocp_nlp_dims *dims, // void ocp_nlp_res_compute(ocp_nlp_dims *dims, ocp_nlp_opts *opts, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_res *res, ocp_nlp_memory *mem, ocp_nlp_workspace *work); + +double ocp_nlp_compute_delta_dual_norm_inf(ocp_nlp_dims *dims, ocp_nlp_workspace *work, ocp_nlp_out *nlp_out, ocp_qp_out *qp_out); // void copy_ocp_nlp_out(ocp_nlp_dims *dims, ocp_nlp_out *from, ocp_nlp_out *to); diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index a824c2fffc..640fcc5a56 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -286,11 +286,6 @@ acados_size_t ocp_nlp_sqp_memory_calculate_size(void *config_, void *dims_, void // nlp mem size += ocp_nlp_memory_calculate_size(config, dims, nlp_opts, in); - // primal step norm - if (opts->nlp_opts->log_primal_step_norm) - { - size += opts->nlp_opts->max_iter*sizeof(double); - } // stat int stat_m = opts->nlp_opts->max_iter+1; int stat_n = 7; @@ -332,12 +327,7 @@ void *ocp_nlp_sqp_memory_assign(void *config_, void *dims_, void *opts_, void *i mem->nlp_mem = ocp_nlp_memory_assign(config, dims, nlp_opts, in, c_ptr); c_ptr += ocp_nlp_memory_calculate_size(config, dims, nlp_opts, in); - // primal step norm - if (opts->nlp_opts->log_primal_step_norm) - { - mem->primal_step_norm = (double *) c_ptr; - c_ptr += opts->nlp_opts->max_iter*sizeof(double); - } + // stat mem->stat = (double *) c_ptr; @@ -826,9 +816,16 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // Compute the step norm if (opts->tol_min_step_norm > 0.0 || nlp_opts->log_primal_step_norm) { - mem->step_norm = ocp_qp_out_compute_primal_nrm_inf(nlp_mem->qp_out); + mem->step_norm = ocp_qp_out_compute_primal_nrm_inf(qp_out); if (nlp_opts->log_primal_step_norm) - mem->primal_step_norm[sqp_iter] = mem->step_norm; + nlp_mem->primal_step_norm[sqp_iter] = mem->step_norm; + } + if (nlp_opts->log_dual_step_norm) + { + if (nlp_opts->log_dual_step_norm) + { + nlp_mem->dual_step_norm[sqp_iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, qp_out); + } } /* end solve QP */ @@ -1006,12 +1003,12 @@ void ocp_nlp_sqp_eval_solution_sens_adj_p(void *config_, void *dims_, } -// TODO: write getter for things in nlp_mem void ocp_nlp_sqp_get(void *config_, void *dims_, void *mem_, const char *field, void *return_value_) { ocp_nlp_config *config = config_; ocp_nlp_dims *dims = dims_; ocp_nlp_sqp_memory *mem = mem_; + ocp_nlp_memory *nlp_mem = mem->nlp_mem; char *ptr_module = NULL; int module_length = 0; @@ -1021,32 +1018,16 @@ void ocp_nlp_sqp_get(void *config_, void *dims_, void *mem_, const char *field, if ( ptr_module!=NULL && (!strcmp(ptr_module, "time")) ) { // call timings getter - ocp_nlp_timings_get(config, mem->nlp_mem->nlp_timings, field, return_value_); + ocp_nlp_timings_get(config, nlp_mem->nlp_timings, field, return_value_); } else if (!strcmp("stat", field)) { double **value = return_value_; *value = mem->stat; } - else if (!strcmp("primal_step_norm", field)) - { - if (mem->primal_step_norm == NULL) - { - printf("\nerror: options log_primal_step_norm was not set\n"); - exit(1); - } - else - { - double *value = return_value_; - for (int ii=0; iinlp_mem->iter; ii++) - { - value[ii] = mem->primal_step_norm[ii]; - } - } - } else if (!strcmp("statistics", field)) { - int n_row = mem->stat_mnlp_mem->iter+1 ? mem->stat_m : mem->nlp_mem->iter+1; + int n_row = mem->stat_miter+1 ? mem->stat_m : nlp_mem->iter+1; double *value = return_value_; for (int ii=0; iinlp_mem, field, return_value_); + ocp_nlp_memory_get(config, nlp_mem, field, return_value_); } } diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index 6a68558907..c78b946dd5 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -316,11 +316,6 @@ acados_size_t ocp_nlp_sqp_wfqp_memory_calculate_size(void *config_, void *dims_, size += ocp_qp_in_calculate_size(dims->relaxed_qp_solver->orig_dims); size += ocp_qp_out_calculate_size(dims->relaxed_qp_solver->orig_dims); - // primal step norm - if (opts->nlp_opts->log_primal_step_norm) - { - size += opts->nlp_opts->max_iter*sizeof(double); - } // stat int stat_m = opts->nlp_opts->max_iter+1; int stat_n = 13; @@ -434,13 +429,6 @@ void *ocp_nlp_sqp_wfqp_memory_assign(void *config_, void *dims_, void *opts_, vo // RSQ_constr assign_and_advance_blasfeo_dmat_structs(N + 1, &mem->RSQ_constr, &c_ptr); - // primal step norm - if (opts->nlp_opts->log_primal_step_norm) - { - mem->primal_step_norm = (double *) c_ptr; - c_ptr += opts->nlp_opts->max_iter*sizeof(double); - } - // stat mem->stat_m = opts->nlp_opts->max_iter+1; mem->stat_n = 13; @@ -1693,7 +1681,11 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { mem->step_norm = ocp_qp_out_compute_primal_nrm_inf(nominal_qp_out); if (nlp_opts->log_primal_step_norm) - mem->primal_step_norm[sqp_iter] = mem->step_norm; + nlp_mem->primal_step_norm[sqp_iter] = mem->step_norm; + } + if (nlp_opts->log_dual_step_norm) + { + nlp_mem->dual_step_norm[sqp_iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, nominal_qp_out); } /* globalization */ @@ -1961,7 +1953,7 @@ void ocp_nlp_sqp_wfqp_get(void *config_, void *dims_, void *mem_, const char *fi ocp_nlp_config *config = config_; ocp_nlp_dims *dims = dims_; ocp_nlp_sqp_wfqp_memory *mem = mem_; - + ocp_nlp_memory *nlp_mem = mem->nlp_mem; char module[MAX_STR_LEN]; char *ptr_module = NULL; @@ -1981,32 +1973,16 @@ void ocp_nlp_sqp_wfqp_get(void *config_, void *dims_, void *mem_, const char *fi if ( ptr_module!=NULL && (!strcmp(ptr_module, "time")) ) { // call timings getter - ocp_nlp_timings_get(config, mem->nlp_mem->nlp_timings, field, return_value_); + ocp_nlp_timings_get(config, nlp_mem->nlp_timings, field, return_value_); } else if (!strcmp("stat", field)) { double **value = return_value_; *value = mem->stat; } - else if (!strcmp("primal_step_norm", field)) - { - if (mem->primal_step_norm == NULL) - { - printf("\nerror: options log_primal_step_norm was not set\n"); - exit(1); - } - else - { - double *value = return_value_; - for (int ii=0; iinlp_mem->iter; ii++) - { - value[ii] = mem->primal_step_norm[ii]; - } - } - } else if (!strcmp("statistics", field)) { - int n_row = mem->stat_mnlp_mem->iter+1 ? mem->stat_m : mem->nlp_mem->iter+1; + int n_row = mem->stat_miter+1 ? mem->stat_m : nlp_mem->iter+1; double *value = return_value_; for (int ii=0; iinlp_mem, field, return_value_); + ocp_nlp_memory_get(config, nlp_mem, field, return_value_); } } diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h index 5cb6b9d360..e3542378a3 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h @@ -60,8 +60,6 @@ typedef struct double tol_comp; double tol_unbounded; // exit threshold when objective function seems to be unbounded double tol_min_step_norm; // exit tolerance for small step - int max_iter; - int log_primal_step_norm; // compute and log the max norm of the primal steps bool log_pi_norm_inf; // compute and log the max norm of the pi multipliers bool log_lam_norm_inf; // compute and log the max norm of the lam multipliers bool warm_start_first_qp; @@ -101,7 +99,6 @@ typedef struct ocp_nlp_memory *nlp_mem; double alpha; - double *primal_step_norm; int *nns; // number of non-slacked constraints in NLP int **idxns; // indices of non-slacked constraints in NLP diff --git a/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m b/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m index c8604484ad..443abfc937 100644 --- a/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m +++ b/examples/acados_matlab_octave/getting_started/extensive_example_ocp.m @@ -83,6 +83,8 @@ ocp.solver_options.exact_hess_constr = 1; ocp.solver_options.print_level = 1; ocp.solver_options.store_iterates = true; +ocp.solver_options.log_dual_step_norm = true; +ocp.solver_options.log_primal_step_norm = true; % can vary for integrators sim_method_num_stages = 1 * ones(N,1); @@ -254,6 +256,13 @@ % get cost value cost_val_ocp = ocp_solver.get_cost(); +primal_step_norm = ocp_solver.get('primal_step_norm'); +dual_step_norm = ocp_solver.get('dual_step_norm'); +disp('primal step norms') +disp(primal_step_norm); +disp('dual step norms') +disp(dual_step_norm); + %% get QP matrices: % See https://docs.acados.org/problem_formulation % |----- dynamics -----|------ cost --------|---------------------------- constraints ------------------------| diff --git a/examples/acados_python/pendulum_on_cart/ocp/example_ocp_dynamics_formulations.py b/examples/acados_python/pendulum_on_cart/ocp/example_ocp_dynamics_formulations.py index a4819f4b93..97c4ce8e32 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/example_ocp_dynamics_formulations.py +++ b/examples/acados_python/pendulum_on_cart/ocp/example_ocp_dynamics_formulations.py @@ -46,7 +46,7 @@ if __name__ == "__main__": parser = argparse.ArgumentParser(description='test Python interface on pendulum example.') - parser.add_argument('--INTEGRATOR_TYPE', dest='INTEGRATOR_TYPE', + parser.add_argument('--INTEGRATOR_TYPE', dest='INTEGRATOR_TYPE', default="ERK", help=f'INTEGRATOR_TYPE: supports {INTEGRATOR_TYPES}') parser.add_argument('--BUILD_SYSTEM', dest='BUILD_SYSTEM', default='make', @@ -62,7 +62,7 @@ build_system = args.BUILD_SYSTEM if build_system not in BUILD_SYSTEMS: - msg = f'Invalid unit test value {build_system} for parameter INTEGRATOR_TYPE. Possible values are' \ + msg = f'Invalid unit test value {build_system} for parameter BUILD_SYSTEM. Possible values are' \ f' {BUILD_SYSTEMS}, got {build_system}.' raise Exception(msg) @@ -118,6 +118,8 @@ ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = integrator_type ocp.solver_options.print_level = 1 + ocp.solver_options.log_dual_step_norm = True + ocp.solver_options.log_primal_step_norm = True if ocp.solver_options.integrator_type == 'GNSF': from acados_template import acados_dae_model_json_dump @@ -171,4 +173,13 @@ simU[i, :] = ocp_solver.get(i, "u") simX[N, :] = ocp_solver.get(N, "x") + # test getting step norms + primal_step_norms = ocp_solver.get_stats('primal_step_norm') + dual_step_norms = ocp_solver.get_stats('dual_step_norm') + print(f"primal step norms: {primal_step_norms}") + print(f"dual step norms: {dual_step_norms}") + # Assert that step norms are decreasing + assert np.all(np.diff(primal_step_norms)[1:] <= 0), "Primal step norms are not decreasing." + assert np.all(np.diff(dual_step_norms) <= 0), "Dual step norms are not decreasing." + plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX) diff --git a/interfaces/acados_matlab_octave/AcadosOcpOptions.m b/interfaces/acados_matlab_octave/AcadosOcpOptions.m index 6b6bf79b8d..4d2465bbbc 100644 --- a/interfaces/acados_matlab_octave/AcadosOcpOptions.m +++ b/interfaces/acados_matlab_octave/AcadosOcpOptions.m @@ -116,6 +116,7 @@ adaptive_levenberg_marquardt_mu_min adaptive_levenberg_marquardt_mu0 log_primal_step_norm + log_dual_step_norm store_iterates eval_residual_at_max_iter @@ -228,6 +229,7 @@ obj.adaptive_levenberg_marquardt_mu_min = 1e-16; obj.adaptive_levenberg_marquardt_mu0 = 1e-3; obj.log_primal_step_norm = 0; + obj.log_dual_step_norm = 0; obj.store_iterates = false; obj.eval_residual_at_max_iter = []; obj.timeout_max_time = 0.; diff --git a/interfaces/acados_matlab_octave/ocp_get.c b/interfaces/acados_matlab_octave/ocp_get.c index a6f7af7746..977ffad633 100644 --- a/interfaces/acados_matlab_octave/ocp_get.c +++ b/interfaces/acados_matlab_octave/ocp_get.c @@ -514,6 +514,14 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) mat_ptr[ii+(jj+1)*min_size] = stat[jj+ii*stat_n]; } } + else if (!strcmp(field, "primal_step_norm") || !strcmp(field, "dual_step_norm")) + { + int nlp_iter; + ocp_nlp_get(solver, "nlp_iter", &nlp_iter); + plhs[0] = mxCreateNumericMatrix(nlp_iter, 1, mxDOUBLE_CLASS, mxREAL); + double *mat_ptr = mxGetPr( plhs[0] ); + ocp_nlp_get(solver, field, mat_ptr); + } else if (!strcmp(field, "residuals")) { if (plan->nlp_solver == SQP_RTI) diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index eb311c1eec..5753216d20 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -125,6 +125,7 @@ def __init__(self): self.__adaptive_levenberg_marquardt_mu_min = 1e-16 self.__adaptive_levenberg_marquardt_mu0 = 1e-3 self.__log_primal_step_norm: bool = False + self.__log_dual_step_norm: bool = False self.__store_iterates: bool = False self.__timeout_max_time = 0. self.__timeout_heuristic = 'LAST' @@ -647,16 +648,25 @@ def adaptive_levenberg_marquardt_mu0(self): return self.__adaptive_levenberg_marquardt_mu0 @property - def log_primal_step_norm(self,): + def log_primal_step_norm(self): """ Flag indicating whether the max norm of the primal steps should be logged. - This is implemented only for solver type `SQP`. + This is implemented only for solver types `SQP`, `SQP_WITH_FEASIBLE_QP`. Default: False """ return self.__log_primal_step_norm @property - def store_iterates(self,): + def log_dual_step_norm(self): + """ + Flag indicating whether the max norm of the dual steps should be logged. + This is implemented only for solver types `SQP`, `SQP_WITH_FEASIBLE_QP`. + Default: False + """ + return self.__log_dual_step_norm + + @property + def store_iterates(self): """ Flag indicating whether the intermediate primal-dual iterates should be stored. This is implemented only for solver types `SQP` and `DDP`. @@ -665,7 +675,7 @@ def store_iterates(self,): return self.__store_iterates @property - def timeout_max_time(self,): + def timeout_max_time(self): """ Maximum time before solver timeout. If 0, there is no timeout. A timeout is triggered if the condition @@ -678,7 +688,7 @@ def timeout_max_time(self,): return self.__timeout_max_time @property - def timeout_heuristic(self,): + def timeout_heuristic(self): """ Heuristic to be used for predicting the runtime of the next SQP iteration, cf. `timeout_max_time`. Possible values are "MAX_CALL", "MAX_OVERALL", "LAST", "AVERAGE", "ZERO". @@ -1728,10 +1738,15 @@ def adaptive_levenberg_marquardt_mu0(self, adaptive_levenberg_marquardt_mu0): @log_primal_step_norm.setter def log_primal_step_norm(self, val): - if isinstance(val, bool): - self.__log_primal_step_norm = val - else: + if not isinstance(val, bool): raise TypeError('Invalid log_primal_step_norm value. Expected bool.') + self.__log_primal_step_norm = val + + @log_dual_step_norm.setter + def log_dual_step_norm(self, val): + if not isinstance(val, bool): + raise TypeError('Invalid log_dual_step_norm value. Expected bool.') + self.__log_dual_step_norm = val @store_iterates.setter def store_iterates(self, val): diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 903b927c4d..8493f43b16 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -1611,7 +1611,7 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: self.__acados_lib.ocp_nlp_get(self.nlp_solver, field, out_data) return out - elif field_ == 'primal_step_norm': + elif field_ in ['primal_step_norm', 'dual_step_norm']: nlp_iter = self.get_stats("nlp_iter") out = np.zeros((nlp_iter,), dtype=np.float64, order="C") out_data = cast(out.ctypes.data, POINTER(c_double)) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c index cf44923d66..4b2e68f927 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c @@ -2291,11 +2291,22 @@ void {{ name }}_acados_create_set_opts({{ name }}_solver_capsule* capsule) ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "reg_adaptive_eps", ®_adaptive_eps); {%- endif %} + int nlp_solver_ext_qp_res = {{ solver_options.nlp_solver_ext_qp_res }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res); + bool store_iterates = {{ solver_options.store_iterates }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "store_iterates", &store_iterates); - int nlp_solver_ext_qp_res = {{ solver_options.nlp_solver_ext_qp_res }}; - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res); +{%- if solver_options.nlp_solver_type == "SQP" or solver_options.nlp_solver_type == "SQP_WITH_FEASIBLE_QP" %} + int log_primal_step_norm = {{ solver_options.log_primal_step_norm }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_primal_step_norm", &log_primal_step_norm); + + int log_dual_step_norm = {{ solver_options.log_dual_step_norm }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_dual_step_norm", &log_dual_step_norm); + + double nlp_solver_tol_min_step_norm = {{ solver_options.nlp_solver_tol_min_step_norm }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_min_step_norm", &nlp_solver_tol_min_step_norm); +{%- endif %} {%- if solver_options.qp_solver is containing("HPIPM") %} // set HPIPM mode: should be done before setting other QP solver options diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c index 32538cdbc6..c37da7aed1 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c @@ -2408,10 +2408,13 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps bool store_iterates = {{ solver_options.store_iterates }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "store_iterates", &store_iterates); -{%- if solver_options.nlp_solver_type == "SQP" %} +{%- if solver_options.nlp_solver_type == "SQP" or solver_options.nlp_solver_type == "SQP_WITH_FEASIBLE_QP" %} int log_primal_step_norm = {{ solver_options.log_primal_step_norm }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_primal_step_norm", &log_primal_step_norm); + int log_dual_step_norm = {{ solver_options.log_dual_step_norm }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_dual_step_norm", &log_dual_step_norm); + double nlp_solver_tol_min_step_norm = {{ solver_options.nlp_solver_tol_min_step_norm }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_min_step_norm", &nlp_solver_tol_min_step_norm); {%- endif %} From 053dbdafc4fd377cf297d14c18a602184e27892e Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 10 Apr 2025 13:20:01 +0200 Subject: [PATCH 05/24] C: avoid using local variable `sqp_iter` (#1496) Follow-up of https://github.com/acados/acados/pull/1492. The changed tests in https://github.com/acados/acados/pull/1492 have been reverted as the iteration reference was still wrong there. Now I verified by printing. --- acados/ocp_nlp/ocp_nlp_sqp.c | 87 ++++++------- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c | 120 +++++++++--------- acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h | 2 +- .../non_ocp_nlp/maratos_test_problem.py | 8 +- 4 files changed, 104 insertions(+), 113 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 640fcc5a56..d582c5faaa 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -568,7 +568,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, int qp_iter = 0; mem->alpha = 0.0; mem->step_norm = 0.0; - mem->nlp_mem->status = ACADOS_SUCCESS; + nlp_mem->status = ACADOS_SUCCESS; nlp_mem->objective_multiplier = 1.0; if (opts->timeout_heuristic != MAX_OVERALL) @@ -586,7 +586,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, /************************************************ * main sqp loop ************************************************/ - int sqp_iter = 0; + nlp_mem->iter = 0; double prev_levenberg_marquardt = 0.0; int globalization_status; qp_info *qp_info_; @@ -594,17 +594,17 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, double timeout_previous_time_tot = 0.; double timeout_time_prev_iter = 0.; - for (; sqp_iter <= opts->nlp_opts->max_iter; sqp_iter++) // <= needed such that after last iteration KKT residuals are checked before max_iter is thrown. + for (; nlp_mem->iter <= opts->nlp_opts->max_iter; nlp_mem->iter++) // <= needed such that after last iteration KKT residuals are checked before max_iter is thrown. { // We always evaluate the residuals until the last iteration // If the option "eval_residual_at_max_iter" is set, we also // evaluate the residuals after the last iteration. - if (sqp_iter != opts->nlp_opts->max_iter || opts->eval_residual_at_max_iter) + if (nlp_mem->iter != opts->nlp_opts->max_iter || opts->eval_residual_at_max_iter) { // store current iterate if (nlp_opts->store_iterates) { - copy_ocp_nlp_out(dims, nlp_out, nlp_mem->iterates[sqp_iter]); + copy_ocp_nlp_out(dims, nlp_out, nlp_mem->iterates[nlp_mem->iter]); } /* Prepare the QP data */ // linearize NLP and update QP matrices @@ -617,7 +617,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { ocp_nlp_get_cost_value_from_submodules(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); } - ocp_nlp_add_levenberg_marquardt_term(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, mem->alpha, sqp_iter, nlp_mem->qp_in); + ocp_nlp_add_levenberg_marquardt_term(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, mem->alpha, nlp_mem->iter, nlp_mem->qp_in); nlp_timings->time_lin += acados_toc(&timer1); // compute nlp residuals @@ -626,24 +626,24 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // Initialize globalization strategies (do not move outside the SQP loop) - if (sqp_iter == 0) + if (nlp_mem->iter == 0) { config->globalization->initialize_memory(config, dims, nlp_mem, nlp_opts); } // save statistics - if (sqp_iter < mem->stat_m) + if (nlp_mem->iter < mem->stat_m) { - mem->stat[mem->stat_n*sqp_iter+0] = nlp_res->inf_norm_res_stat; - mem->stat[mem->stat_n*sqp_iter+1] = nlp_res->inf_norm_res_eq; - mem->stat[mem->stat_n*sqp_iter+2] = nlp_res->inf_norm_res_ineq; - mem->stat[mem->stat_n*sqp_iter+3] = nlp_res->inf_norm_res_comp; + mem->stat[mem->stat_n*nlp_mem->iter+0] = nlp_res->inf_norm_res_stat; + mem->stat[mem->stat_n*nlp_mem->iter+1] = nlp_res->inf_norm_res_eq; + mem->stat[mem->stat_n*nlp_mem->iter+2] = nlp_res->inf_norm_res_ineq; + mem->stat[mem->stat_n*nlp_mem->iter+3] = nlp_res->inf_norm_res_comp; } // Output if (nlp_opts->print_level > 0) { - print_iteration(sqp_iter, config, nlp_res, mem, nlp_opts, prev_levenberg_marquardt, qp_status, qp_iter); + print_iteration(nlp_mem->iter, config, nlp_res, mem, nlp_opts, prev_levenberg_marquardt, qp_status, qp_iter); } prev_levenberg_marquardt = nlp_opts->levenberg_marquardt; @@ -659,7 +659,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { nlp_timings->time_tot = acados_toc(&timer0); - if (sqp_iter > 0) + if (nlp_mem->iter > 0) { timeout_time_prev_iter = nlp_timings->time_tot - timeout_previous_time_tot; @@ -673,7 +673,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, mem->timeout_estimated_per_iteration_time = timeout_time_prev_iter > mem->timeout_estimated_per_iteration_time ? timeout_time_prev_iter : mem->timeout_estimated_per_iteration_time; break; case AVERAGE: - if (sqp_iter == 0) + if (nlp_mem->iter == 0) { mem->timeout_estimated_per_iteration_time = timeout_time_prev_iter; } @@ -695,21 +695,20 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // Termination - if (check_termination(sqp_iter, dims, nlp_res, mem, opts)) + if (check_termination(nlp_mem->iter, dims, nlp_res, mem, opts)) { #if defined(ACADOS_WITH_OPENMP) // restore number of threads omp_set_num_threads(num_threads_bkp); #endif - nlp_mem->iter = sqp_iter; nlp_timings->time_tot = acados_toc(&timer0); - return mem->nlp_mem->status; + return nlp_mem->status; } /* solve QP */ // warm start of first QP - if (sqp_iter == 0) + if (nlp_mem->iter == 0) { if (!opts->warm_start_first_qp) { @@ -727,47 +726,47 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // Show input to QP if (nlp_opts->print_level > 3) { - printf("\n\nSQP: ocp_qp_in at iteration %d\n", sqp_iter); + printf("\n\nSQP: ocp_qp_in at iteration %d\n", nlp_mem->iter); print_ocp_qp_in(qp_in); } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_in_to_file(qp_in, sqp_iter, 0); + ocp_nlp_dump_qp_in_to_file(qp_in, nlp_mem->iter, 0); #endif qp_status = ocp_nlp_solve_qp_and_correct_dual(config, dims, nlp_opts, nlp_mem, nlp_work, false, NULL, NULL, NULL); // restore default warm start - if (sqp_iter==0) + if (nlp_mem->iter==0) { qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "warm_start", &nlp_opts->qp_warm_start); } if (nlp_opts->print_level > 3) { - printf("\n\nSQP: ocp_qp_out at iteration %d\n", sqp_iter); + printf("\n\nSQP: ocp_qp_out at iteration %d\n", nlp_mem->iter); print_ocp_qp_out(qp_out); } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_out_to_file(qp_out, sqp_iter, 0); + ocp_nlp_dump_qp_out_to_file(qp_out, nlp_mem->iter, 0); #endif ocp_qp_out_get(qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; // save statistics of last qp solver call - if (sqp_iter+1 < mem->stat_m) + if (nlp_mem->iter+1 < mem->stat_m) { - mem->stat[mem->stat_n*(sqp_iter+1)+4] = qp_status; - mem->stat[mem->stat_n*(sqp_iter+1)+5] = qp_iter; + mem->stat[mem->stat_n*(nlp_mem->iter+1)+4] = qp_status; + mem->stat[mem->stat_n*(nlp_mem->iter+1)+5] = qp_iter; } // compute external QP residuals (for debugging) if (nlp_opts->ext_qp_res) { ocp_qp_res_compute(qp_in, qp_out, nlp_work->qp_res, nlp_work->qp_res_ws); - if (sqp_iter+1 < mem->stat_m) - ocp_qp_res_compute_nrm_inf(nlp_work->qp_res, mem->stat+(mem->stat_n*(sqp_iter+1)+7)); + if (nlp_mem->iter+1 < mem->stat_m) + ocp_qp_res_compute_nrm_inf(nlp_work->qp_res, mem->stat+(mem->stat_n*(nlp_mem->iter+1)+7)); } // exit conditions on QP status @@ -775,17 +774,17 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { if (nlp_opts->print_level > 0) { - printf("%i\t%e\t%e\t%e\t%e.\n", sqp_iter, nlp_res->inf_norm_res_stat, + printf("%i\t%e\t%e\t%e\t%e.\n", nlp_mem->iter, nlp_res->inf_norm_res_stat, nlp_res->inf_norm_res_eq, nlp_res->inf_norm_res_ineq, nlp_res->inf_norm_res_comp ); printf("\n\n"); } - // increment sqp_iter to return full statistics and improve output below. - sqp_iter++; + // increment nlp_mem->iter to return full statistics and improve output below. + nlp_mem->iter++; #ifndef ACADOS_SILENT printf("\nQP solver returned error status %d in SQP iteration %d, QP iteration %d.\n", - qp_status, sqp_iter, qp_iter); + qp_status, nlp_mem->iter, qp_iter); #endif #if defined(ACADOS_WITH_OPENMP) // restore number of threads @@ -798,11 +797,10 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, print_ocp_qp_in(qp_in); } - mem->nlp_mem->status = ACADOS_QP_FAILURE; - nlp_mem->iter = sqp_iter; + nlp_mem->status = ACADOS_QP_FAILURE; nlp_timings->time_tot = acados_toc(&timer0); - return mem->nlp_mem->status; + return nlp_mem->status; } // Calculate optimal QP objective (needed for globalization) @@ -814,17 +812,17 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // Compute the step norm - if (opts->tol_min_step_norm > 0.0 || nlp_opts->log_primal_step_norm) + if (opts->tol_min_step_norm > 0.0 || nlp_opts->log_primal_step_norm || nlp_opts->print_level > 0) { mem->step_norm = ocp_qp_out_compute_primal_nrm_inf(qp_out); if (nlp_opts->log_primal_step_norm) - nlp_mem->primal_step_norm[sqp_iter] = mem->step_norm; + nlp_mem->primal_step_norm[nlp_mem->iter] = mem->step_norm; } if (nlp_opts->log_dual_step_norm) { if (nlp_opts->log_dual_step_norm) { - nlp_mem->dual_step_norm[sqp_iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, qp_out); + nlp_mem->dual_step_norm[nlp_mem->iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, qp_out); } } /* end solve QP */ @@ -842,17 +840,16 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { printf("\nFailure in globalization, got status %d!\n", globalization_status); } - mem->nlp_mem->status = globalization_status; - nlp_mem->iter = sqp_iter; + nlp_mem->status = globalization_status; nlp_timings->time_tot = acados_toc(&timer0); #if defined(ACADOS_WITH_OPENMP) // restore number of threads omp_set_num_threads(num_threads_bkp); #endif - return mem->nlp_mem->status; + return nlp_mem->status; } - if (sqp_iter+1 < mem->stat_m) - mem->stat[mem->stat_n*(sqp_iter+1)+6] = mem->alpha; + if (nlp_mem->iter+1 < mem->stat_m) + mem->stat[mem->stat_n*(nlp_mem->iter+1)+6] = mem->alpha; } // end SQP loop @@ -864,7 +861,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // restore number of threads omp_set_num_threads(num_threads_bkp); #endif - return mem->nlp_mem->status; + return nlp_mem->status; } diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c index c78b946dd5..f44acb229a 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.c @@ -917,7 +917,7 @@ static void set_pointers_for_hessian_evaluation(ocp_nlp_config *config, static void setup_hessian_matrices_for_qps(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_sqp_wfqp_opts *opts, - ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_workspace *work, int sqp_iter) + ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_workspace *work) { ocp_nlp_memory *nlp_mem = mem->nlp_mem; ocp_qp_in *nominal_qp_in = nlp_mem->qp_in; @@ -951,7 +951,7 @@ static void setup_hessian_matrices_for_qps(ocp_nlp_config *config, blasfeo_dveccpsc(2*ns[i], 1.0, mem->Z_cost_module+i, 0, nominal_qp_in->Z+i, 0); } // Levenberg Marquardt term for nominal QP - ocp_nlp_add_levenberg_marquardt_term(config, dims, in, out, opts->nlp_opts, nlp_mem, work, mem->alpha, sqp_iter, nlp_mem->qp_in); + ocp_nlp_add_levenberg_marquardt_term(config, dims, in, out, opts->nlp_opts, nlp_mem, work, mem->alpha, nlp_mem->iter, nlp_mem->qp_in); } /* @@ -959,7 +959,7 @@ Solves the QP. Either solves feasibility QP or nominal QP */ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* opts, ocp_qp_in* qp_in, ocp_qp_out* qp_out, ocp_nlp_dims *dims, ocp_nlp_sqp_wfqp_memory* mem, ocp_nlp_in* nlp_in, ocp_nlp_out* nlp_out, - ocp_nlp_memory* nlp_mem, ocp_nlp_workspace* nlp_work, int sqp_iter, bool solve_feasibility_qp, + ocp_nlp_memory* nlp_mem, ocp_nlp_workspace* nlp_work, bool solve_feasibility_qp, acados_timer timer_tot) { acados_timer timer_qp; @@ -969,7 +969,7 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o int qp_status = ACADOS_SUCCESS; // warm start of first QP - if (sqp_iter == 0) + if (nlp_mem->iter == 0) { if (!opts->warm_start_first_qp) { @@ -985,12 +985,12 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o } } - if (mem->qps_solved_in_sqp_iter < 2 && (!solve_feasibility_qp || opts->use_constraint_hessian_in_feas_qp)) + if (mem->qps_solved_in_iter < 2 && (!solve_feasibility_qp || opts->use_constraint_hessian_in_feas_qp)) { if (solve_feasibility_qp && opts->use_constraint_hessian_in_feas_qp) { // LM for feasibility QP - ocp_nlp_add_levenberg_marquardt_term(config, dims, nlp_in, nlp_out, opts->nlp_opts, nlp_mem, nlp_work, mem->alpha, sqp_iter, qp_in); + ocp_nlp_add_levenberg_marquardt_term(config, dims, nlp_in, nlp_out, opts->nlp_opts, nlp_mem, nlp_work, mem->alpha, nlp_mem->iter, qp_in); } // regularize Hessian @@ -1002,13 +1002,13 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o // Show input to QP if (nlp_opts->print_level > 3) { - printf("\n\nSQP: ocp_qp_in at iteration %d\n", sqp_iter); + printf("\n\nSQP: ocp_qp_in at iteration %d\n", nlp_mem->iter); print_ocp_qp_dims(qp_in->dim); print_ocp_qp_in(qp_in); } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_in_to_file(qp_in, sqp_iter, 0); + ocp_nlp_dump_qp_in_to_file(qp_in, nlp_mem->iter, 0); #endif if (solve_feasibility_qp) @@ -1032,31 +1032,30 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o nlp_mem, nlp_work, false, NULL, NULL, NULL); } - - mem->qps_solved_in_sqp_iter += 1; + mem->qps_solved_in_iter += 1; // restore default warm start - if (sqp_iter==0) + if (nlp_mem->iter==0) { qp_solver->opts_set(qp_solver, nlp_opts->qp_solver_opts, "warm_start", &nlp_opts->qp_warm_start); } if (nlp_opts->print_level > 3) { - printf("\n\nSQP: ocp_qp_out at iteration %d\n", sqp_iter); + printf("\n\nSQP: ocp_qp_out at iteration %d\n", nlp_mem->iter); print_ocp_qp_dims(qp_out->dim); print_ocp_qp_out(qp_out); } #if defined(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE) - ocp_nlp_dump_qp_out_to_file(qp_out, sqp_iter, 0); + ocp_nlp_dump_qp_out_to_file(qp_out, nlp_mem->iter, 0); #endif // exit conditions on QP status if ((qp_status!=ACADOS_SUCCESS) & (qp_status!=ACADOS_MAXITER)) { - // increment sqp_iter to return full statistics and improve output below. - sqp_iter++; + // increment nlp_mem->iter to return full statistics and improve output below. + nlp_mem->iter++; if (nlp_opts->print_level > 1) { @@ -1066,7 +1065,6 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o } mem->nlp_mem->status = ACADOS_QP_FAILURE; - nlp_mem->iter = sqp_iter; nlp_timings->time_tot = acados_toc(&timer_tot); return mem->nlp_mem->status; @@ -1074,43 +1072,45 @@ static int prepare_and_solve_QP(ocp_nlp_config* config, ocp_nlp_sqp_wfqp_opts* o return qp_status; } -static void log_qp_stats(ocp_nlp_sqp_wfqp_memory *mem, int sqp_iter, bool solve_feasibility_qp, +static void log_qp_stats(ocp_nlp_sqp_wfqp_memory *mem, bool solve_feasibility_qp, int qp_status, int qp_iter) { - if (sqp_iter < mem->stat_m) + int nlp_iter = mem->nlp_mem->iter; + if (nlp_iter < mem->stat_m) { if (mem->search_direction_mode == NOMINAL_QP) { - mem->stat[mem->stat_n*(sqp_iter)+4] = qp_status; - mem->stat[mem->stat_n*(sqp_iter)+5] = qp_iter; + mem->stat[mem->stat_n*(nlp_iter)+4] = qp_status; + mem->stat[mem->stat_n*(nlp_iter)+5] = qp_iter; } else if (mem->search_direction_mode == BYRD_OMOJOKUN) { if (solve_feasibility_qp) { - mem->stat[mem->stat_n*(sqp_iter)+6] = qp_status; - mem->stat[mem->stat_n*(sqp_iter)+7] = qp_iter; + mem->stat[mem->stat_n*(nlp_iter)+6] = qp_status; + mem->stat[mem->stat_n*(nlp_iter)+7] = qp_iter; } else { - mem->stat[mem->stat_n*(sqp_iter)+8] = qp_status; - mem->stat[mem->stat_n*(sqp_iter)+9] = qp_iter; + mem->stat[mem->stat_n*(nlp_iter)+8] = qp_status; + mem->stat[mem->stat_n*(nlp_iter)+9] = qp_iter; } } } } -static void log_multiplier_norms(int sqp_iter, ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_sqp_wfqp_opts *opts, ocp_nlp_out *nlp_out, ocp_nlp_dims *dims) +static void log_multiplier_norms(ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_sqp_wfqp_opts *opts, ocp_nlp_out *nlp_out, ocp_nlp_dims *dims) { + int nlp_iter = mem->nlp_mem->iter; if (opts->log_pi_norm_inf) { mem->norm_inf_pi = ocp_nlp_compute_dual_pi_norm_inf(dims, nlp_out); - mem->stat[mem->stat_n*(sqp_iter)+11] = mem->norm_inf_pi; + mem->stat[mem->stat_n*(nlp_iter)+11] = mem->norm_inf_pi; } if (opts->log_lam_norm_inf) { mem->norm_inf_lam = ocp_nlp_compute_dual_lam_norm_inf(dims, nlp_out); - mem->stat[mem->stat_n*(sqp_iter)+12] = mem->norm_inf_lam; + mem->stat[mem->stat_n*(nlp_iter)+12] = mem->norm_inf_lam; } } @@ -1175,7 +1175,6 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out, ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_sqp_wfqp_workspace *work, - int sqp_iter, acados_timer timer_tot) { ocp_nlp_memory* nlp_mem = mem->nlp_mem; @@ -1196,10 +1195,10 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, /* Solve Feasibility QP: Objective: Only constraint Hessian/Identity AND only gradient of slack variables */ print_debug_output("Solve Feasibility QP!\n", nlp_opts->print_level, 2); qp_status = prepare_and_solve_QP(config, opts, relaxed_qp_in, relaxed_qp_out, dims, mem, nlp_in, nlp_out, - nlp_mem, nlp_work, sqp_iter, true, timer_tot); + nlp_mem, nlp_work, true, timer_tot); ocp_qp_out_get(relaxed_qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; - log_qp_stats(mem, sqp_iter, true, qp_status, qp_iter); + log_qp_stats(mem, true, qp_status, qp_iter); if (qp_status != ACADOS_SUCCESS) { if (nlp_opts->print_level >=1) @@ -1207,7 +1206,6 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, printf("\nError in feasibility QP in iteration %d, got qp_status %d!\n", qp_iter, qp_status); } nlp_mem->status = ACADOS_QP_FAILURE; - nlp_mem->iter = sqp_iter; nlp_timings->time_tot = acados_toc(&timer_tot); return nlp_mem->status; } @@ -1223,10 +1221,10 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, setup_byrd_omojokun_bounds(dims, nlp_mem, mem, work, opts); // solve_feasibility_qp --> false in prepare_and_solve_QP qp_status = prepare_and_solve_QP(config, opts, nominal_qp_in, nominal_qp_out, dims, mem, nlp_in, nlp_out, - nlp_mem, nlp_work, sqp_iter, false, timer_tot); + nlp_mem, nlp_work, false, timer_tot); ocp_qp_out_get(nominal_qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; - log_qp_stats(mem, sqp_iter, false, qp_status, qp_iter); + log_qp_stats(mem, false, qp_status, qp_iter); if (qp_status != ACADOS_SUCCESS) { @@ -1235,7 +1233,6 @@ static int byrd_omojokun_direction_computation(ocp_nlp_dims *dims, printf("\nError in nominal QP in iteration %d, got qp_status %d!\n", qp_iter, qp_status); } nlp_mem->status = ACADOS_QP_FAILURE; - nlp_mem->iter = sqp_iter; nlp_timings->time_tot = acados_toc(&timer_tot); return nlp_mem->status; } @@ -1310,7 +1307,7 @@ update QP rhs for feasibility QP (step prim var, abs dual var) */ void ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts, - ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_workspace *work, int sqp_iter) + ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_workspace *work) { ocp_nlp_memory *nlp_mem = mem->nlp_mem; ocp_qp_in *nominal_qp_in = nlp_mem->qp_in; @@ -1332,7 +1329,7 @@ void ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(ocp_nlp_conf blasfeo_dveccp(ns[i], nominal_qp_in->d + i, 2*n_nominal_ineq_nlp+ns[i], relaxed_qp_in->d + i, 2*n_nominal_ineq_nlp+ns[i]+nns[i]); } // setup d_mask; TODO: this is only needed at the start of each NLP solve - if (sqp_iter == 0) + if (nlp_mem->iter == 0) { int offset_dmask; for (int i=0; i<=dims->N; i++) @@ -1436,14 +1433,13 @@ static int calculate_search_direction(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out, ocp_nlp_sqp_wfqp_memory *mem, ocp_nlp_sqp_wfqp_workspace *work, - int sqp_iter, acados_timer timer_tot) { ocp_nlp_memory *nlp_mem = mem->nlp_mem; qp_info* qp_info_; int qp_iter = 0; int search_direction_status; - mem->qps_solved_in_sqp_iter = 0; + mem->qps_solved_in_iter = 0; if (mem->search_direction_mode == NOMINAL_QP) { @@ -1451,10 +1447,10 @@ static int calculate_search_direction(ocp_nlp_dims *dims, // otherwise, we change the mode to Byrd-Omojokun and we continue. search_direction_status = prepare_and_solve_QP(config, opts, nlp_mem->qp_in, nlp_mem->qp_out, dims, mem, nlp_in, nlp_out, nlp_mem, work->nlp_work, - sqp_iter, false, timer_tot); + false, timer_tot); ocp_qp_out_get(nlp_mem->qp_out, "qp_info", &qp_info_); qp_iter = qp_info_->num_iter; - log_qp_stats(mem, sqp_iter, false, search_direction_status, qp_iter); + log_qp_stats(mem, false, search_direction_status, qp_iter); if (search_direction_status != ACADOS_SUCCESS) { if (nlp_opts->print_level >1) @@ -1482,7 +1478,7 @@ static int calculate_search_direction(ocp_nlp_dims *dims, { // We solve two QPs and return the search direction that we found! // if the second QP is feasible, we change back to nominal QP mode. - if (mem->qps_solved_in_sqp_iter == 1) + if (mem->qps_solved_in_iter == 1) { mem->search_direction_type = "NFN"; } @@ -1490,7 +1486,7 @@ static int calculate_search_direction(ocp_nlp_dims *dims, { mem->search_direction_type = "FN"; } - search_direction_status = byrd_omojokun_direction_computation(dims, config, opts, nlp_opts, nlp_in, nlp_out, mem, work, sqp_iter, timer_tot); + search_direction_status = byrd_omojokun_direction_computation(dims, config, opts, nlp_opts, nlp_in, nlp_out, mem, work, timer_tot); double l1_inf = calculate_qp_l1_infeasibility(dims, mem, work, opts, mem->relaxed_qp_in, mem->relaxed_qp_out); if (l1_inf/(fmax(1.0, (double) mem->absolute_nns)) < opts->tol_ineq) { @@ -1578,7 +1574,7 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, /************************************************ * main sqp loop ************************************************/ - int sqp_iter = 0; + nlp_mem->iter = 0; double prev_levenberg_marquardt = 0.0; int search_direction_status = 0; @@ -1587,17 +1583,17 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, print_indices(dims, work, mem); } - for (; sqp_iter <= opts->nlp_opts->max_iter; sqp_iter++) // <= needed such that after last iteration KKT residuals are checked before max_iter is thrown. + for (; nlp_mem->iter <= opts->nlp_opts->max_iter; nlp_mem->iter++) // <= needed such that after last iteration KKT residuals are checked before max_iter is thrown. { // We always evaluate the residuals until the last iteration // If the option "eval_residual_at_max_iter" is set, we also // evaluate the residuals after the last iteration. - if (sqp_iter != opts->nlp_opts->max_iter || opts->eval_residual_at_max_iter) + if (nlp_mem->iter != opts->nlp_opts->max_iter || opts->eval_residual_at_max_iter) { // store current iterate if (nlp_opts->store_iterates) { - copy_ocp_nlp_out(dims, nlp_out, nlp_mem->iterates[sqp_iter]); + copy_ocp_nlp_out(dims, nlp_out, nlp_mem->iterates[nlp_mem->iter]); } /* Prepare the QP data */ // linearize NLP and update QP matrices @@ -1609,14 +1605,14 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // relaxed QP solver // matrices for relaxed QP solver evaluated in nominal QP solver - ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(config, dims, nlp_in, nlp_out, nlp_opts, mem, nlp_work, sqp_iter); + ocp_nlp_sqp_wfqp_approximate_feasibility_qp_constraint_vectors(config, dims, nlp_in, nlp_out, nlp_opts, mem, nlp_work); if (nlp_opts->with_adaptive_levenberg_marquardt || config->globalization->needs_objective_value() == 1) { ocp_nlp_get_cost_value_from_submodules(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work); } - setup_hessian_matrices_for_qps(config, dims, nlp_in, nlp_out, opts, mem, nlp_work, sqp_iter); + setup_hessian_matrices_for_qps(config, dims, nlp_in, nlp_out, opts, mem, nlp_work); // nlp_timings->time_lin += acados_toc(&timer1); // compute nlp residuals @@ -1625,37 +1621,36 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // Initialize the memory for different globalization strategies - if (sqp_iter == 0) + if (nlp_mem->iter == 0) { config->globalization->initialize_memory(config, dims, nlp_mem, nlp_opts); } // save statistics - if (sqp_iter < mem->stat_m) + if (nlp_mem->iter < mem->stat_m) { - mem->stat[mem->stat_n*sqp_iter+0] = nlp_res->inf_norm_res_stat; - mem->stat[mem->stat_n*sqp_iter+1] = nlp_res->inf_norm_res_eq; - mem->stat[mem->stat_n*sqp_iter+2] = nlp_res->inf_norm_res_ineq; - mem->stat[mem->stat_n*sqp_iter+3] = nlp_res->inf_norm_res_comp; + mem->stat[mem->stat_n*nlp_mem->iter+0] = nlp_res->inf_norm_res_stat; + mem->stat[mem->stat_n*nlp_mem->iter+1] = nlp_res->inf_norm_res_eq; + mem->stat[mem->stat_n*nlp_mem->iter+2] = nlp_res->inf_norm_res_ineq; + mem->stat[mem->stat_n*nlp_mem->iter+3] = nlp_res->inf_norm_res_comp; } - log_multiplier_norms(sqp_iter, mem, opts, nlp_out, dims); + log_multiplier_norms(mem, opts, nlp_out, dims); /* Output */ if (nlp_opts->print_level > 0) { - print_iteration(sqp_iter, config, nlp_res, mem, nlp_opts, prev_levenberg_marquardt, qp_status, qp_iter); + print_iteration(nlp_mem->iter, config, nlp_res, mem, nlp_opts, prev_levenberg_marquardt, qp_status, qp_iter); } prev_levenberg_marquardt = nlp_opts->levenberg_marquardt; /* Termination */ - if (check_termination(sqp_iter, dims, nlp_res, mem, opts)) + if (check_termination(nlp_mem->iter, dims, nlp_res, mem, opts)) { #if defined(ACADOS_WITH_OPENMP) // restore number of threads omp_set_num_threads(num_threads_bkp); #endif - nlp_mem->iter = sqp_iter; nlp_timings->time_tot = acados_toc(&timer_tot); return mem->nlp_mem->status; } @@ -1666,7 +1661,7 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } /* Search Direction Computation */ - search_direction_status = calculate_search_direction(dims, config, opts, nlp_opts, nlp_in, nlp_out, mem, work, sqp_iter, timer_tot); + search_direction_status = calculate_search_direction(dims, config, opts, nlp_opts, nlp_in, nlp_out, mem, work, timer_tot); if (search_direction_status != ACADOS_SUCCESS) { #if defined(ACADOS_WITH_OPENMP) @@ -1681,11 +1676,11 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, { mem->step_norm = ocp_qp_out_compute_primal_nrm_inf(nominal_qp_out); if (nlp_opts->log_primal_step_norm) - nlp_mem->primal_step_norm[sqp_iter] = mem->step_norm; + nlp_mem->primal_step_norm[nlp_mem->iter] = mem->step_norm; } if (nlp_opts->log_dual_step_norm) { - nlp_mem->dual_step_norm[sqp_iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, nominal_qp_out); + nlp_mem->dual_step_norm[nlp_mem->iter] = ocp_nlp_compute_delta_dual_norm_inf(dims, nlp_work, nlp_out, nominal_qp_out); } /* globalization */ @@ -1711,7 +1706,6 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, printf("\nFailure in globalization, got status %d!\n", globalization_status); } nlp_mem->status = globalization_status; - nlp_mem->iter = sqp_iter; nlp_timings->time_tot = acados_toc(&timer_tot); #if defined(ACADOS_WITH_OPENMP) // restore number of threads @@ -1720,7 +1714,7 @@ int ocp_nlp_sqp_wfqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, return nlp_mem->status; } - mem->stat[mem->stat_n*(sqp_iter+1)+10] = mem->alpha; + mem->stat[mem->stat_n*(nlp_mem->iter+1)+10] = mem->alpha; nlp_timings->time_glob += acados_toc(&timer1); } // end SQP loop diff --git a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h index e3542378a3..39a9efbdf2 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h @@ -124,7 +124,7 @@ typedef struct int watchdog_zero_slacks_counter; // counts number of consecutive BYRD_OMOJOKUN iter with slack sum == 0 int absolute_nns; // sum of all nns[i] - int qps_solved_in_sqp_iter; + int qps_solved_in_iter; // QP solver with always feasible QPs ocp_qp_xcond_solver relaxed_qp_solver; diff --git a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py index 9be5054f7e..13a3561525 100644 --- a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py +++ b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py @@ -212,8 +212,8 @@ def solve_maratos_problem_with_setting(setting): if any(alphas[:iter] != 1.0): raise Exception(f"Expected all alphas = 1.0 when using full step SQP on Maratos problem") elif globalization == 'MERIT_BACKTRACKING': - if max_infeasibility > 1.5: - raise Exception(f"Expected max_infeasibility < 1.5 when using globalized SQP on Maratos problem") + if max_infeasibility > 0.5: + raise Exception(f"Expected max_infeasibility < 0.5 when using globalized SQP on Maratos problem") if globalization_use_SOC == 0: if FOR_LOOPING and iter != 57: raise Exception(f"Expected 57 SQP iterations when using globalized SQP without SOC on Maratos problem, got {iter}") @@ -226,8 +226,8 @@ def solve_maratos_problem_with_setting(setting): # Jonathan Laptop: merit_grad = -1.737950e-01, merit_grad_cost = -1.737950e-01, merit_grad_dyn = 0.000000e+00, merit_grad_ineq = 0.000000e+00 raise Exception(f"Expected SQP iterations in range(29, 37) when using globalized SQP with SOC on Maratos problem, got {iter}") else: - if iter != 10: - raise Exception(f"Expected 10 SQP iterations when using globalized SQP with SOC on Maratos problem, got {iter}") + if iter != 16: + raise Exception(f"Expected 16 SQP iterations when using globalized SQP with SOC on Maratos problem, got {iter}") elif globalization == 'FUNNEL_L1PEN_LINESEARCH': if iter > 12: raise Exception(f"Expected not more than 12 SQP iterations when using Funnel Method SQP, got {iter}") From 0e55eeecc62cd4fd4a25de3647a961cb9ec754cf Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 11 Apr 2025 17:59:24 +0200 Subject: [PATCH 06/24] Template: avoid openmp dependency when not batched (#1497) If got lost in #1483 --- .../acados_template/c_templates_tera/acados_solver.in.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c index c37da7aed1..676d3970dc 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c @@ -37,8 +37,10 @@ #include "acados_c/ocp_nlp_interface.h" #include "acados_c/external_function_interface.h" +{%- if solver_options.with_batch_functionality %} // openmp #include +{%- endif %} // example specific #include "{{ model.name }}_model/{{ model.name }}_model.h" From 0f042a191d8fea0cbcfcf9044dc28a31c67f9c6f Mon Sep 17 00:00:00 2001 From: Confectio <30325218+Confectio@users.noreply.github.com> Date: Mon, 14 Apr 2025 09:38:34 +0200 Subject: [PATCH 07/24] Allow `solver_options.N_horizon=0` (#1498) This PR modifies the Matlab and Python interfaces to allow formulating optimization problems without any shooting-structure and `solver_options.N_horizon=0` . If `solver_options.N_horizon=0`, only the terminal costs and constraints will be regarded. In particular, also no dynamics, other costs, and constraints have to be defined. We have also updated some examples to use this feature instead of the workarounds. --------- Co-authored-by: Jonathan Frey --- .../convex_problem_globalization_necessary.m | 8 +- .../env.sh | 75 +++ .../acados_matlab_octave/generic_nlp/env.sh | 75 +++ .../acados_matlab_octave/generic_nlp/main.m | 18 +- .../non_ocp_nlp/maratos_test_problem.py | 153 +++-- .../non_ocp_example.py | 18 +- external/hpipm | 2 +- interfaces/acados_matlab_octave/AcadosOcp.m | 583 +++++++++++------- .../acados_template/acados_dims.py | 2 +- .../acados_template/acados_ocp.py | 538 ++++++++++------ .../acados_template/acados_ocp_options.py | 6 +- .../acados_template/acados_ocp_solver.py | 3 - .../c_templates_tera/CMakeLists.in.txt | 23 +- .../c_templates_tera/acados_solver.in.c | 72 ++- 14 files changed, 1017 insertions(+), 559 deletions(-) create mode 100644 examples/acados_matlab_octave/convex_problem_globalization_needed/env.sh create mode 100644 examples/acados_matlab_octave/generic_nlp/env.sh diff --git a/examples/acados_matlab_octave/convex_problem_globalization_needed/convex_problem_globalization_necessary.m b/examples/acados_matlab_octave/convex_problem_globalization_needed/convex_problem_globalization_necessary.m index fe0252eb53..0797a11e2b 100644 --- a/examples/acados_matlab_octave/convex_problem_globalization_needed/convex_problem_globalization_necessary.m +++ b/examples/acados_matlab_octave/convex_problem_globalization_needed/convex_problem_globalization_necessary.m @@ -48,13 +48,11 @@ x = SX.sym('x'); % dynamics: identity - model.disc_dyn_expr = x; model.x = x; model.name = strcat('convex_problem_', globalization); %% solver settings - T = 1; - N_horizon = 1; + N_horizon = 0; %% OCP formulation object ocp = AcadosOcp(); @@ -63,11 +61,8 @@ % terminal cost term ocp.cost.cost_type_e = 'EXTERNAL'; ocp.model.cost_expr_ext_cost_e = log(exp(model.x) + exp(-model.x)); - ocp.cost.cost_type = 'EXTERNAL'; - ocp.model.cost_expr_ext_cost = 0; % define solver options - ocp.solver_options.tf = T; ocp.solver_options.N_horizon = N_horizon; ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'; ocp.solver_options.hessian_approx = 'EXACT'; @@ -87,7 +82,6 @@ % set trajectory initialization ocp_solver.set('init_x', xinit, 0); - ocp_solver.set('init_x', xinit, 1); % solve ocp_solver.solve(); diff --git a/examples/acados_matlab_octave/convex_problem_globalization_needed/env.sh b/examples/acados_matlab_octave/convex_problem_globalization_needed/env.sh new file mode 100644 index 0000000000..f37d215665 --- /dev/null +++ b/examples/acados_matlab_octave/convex_problem_globalization_needed/env.sh @@ -0,0 +1,75 @@ +#! /usr/bin/bash +# +# Copyright (c) The acados authors. +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + + +if [[ "${BASH_SOURCE[0]}" != "${0}" ]] +then + echo "Script is being sourced" +else + echo "ERROR: Script is a subshell" + echo "To affect your current shell enviroment source this script with:" + echo "source env.sh" + exit +fi + +# check that this file is run +export ENV_RUN=true + +# if acados folder not specified assume parent of the folder of the single examples +ACADOS_INSTALL_DIR=${ACADOS_INSTALL_DIR:-"$(pwd)/../../.."} +export ACADOS_INSTALL_DIR +echo +echo "ACADOS_INSTALL_DIR=$ACADOS_INSTALL_DIR" + +# export casadi folder and matlab/octave mex folder +# MATLAB case +export MATLABPATH=$MATLABPATH:$ACADOS_INSTALL_DIR/external/casadi-matlab/ +export MATLABPATH=$MATLABPATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/ +export MATLABPATH=$MATLABPATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/ + +echo +echo "MATLABPATH=$MATLABPATH" +# Octave case +export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/external/casadi-octave/ +export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/ +export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/ +echo +echo "OCTAVE_PATH=$OCTAVE_PATH" + +# export acados mex flags +#export ACADOS_MEX_FLAGS="GCC=/usr/bin/gcc-4.9" + +# if model folder not specified assume this folder +MODEL_FOLDER=${MODEL_FOLDER:-"./build"} +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$ACADOS_INSTALL_DIR/lib:$MODEL_FOLDER +export LD_RUN_PATH="$(pwd)"/c_generated_code +echo +echo "LD_LIBRARY_PATH=$LD_LIBRARY_PATH" diff --git a/examples/acados_matlab_octave/generic_nlp/env.sh b/examples/acados_matlab_octave/generic_nlp/env.sh new file mode 100644 index 0000000000..f37d215665 --- /dev/null +++ b/examples/acados_matlab_octave/generic_nlp/env.sh @@ -0,0 +1,75 @@ +#! /usr/bin/bash +# +# Copyright (c) The acados authors. +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + + +if [[ "${BASH_SOURCE[0]}" != "${0}" ]] +then + echo "Script is being sourced" +else + echo "ERROR: Script is a subshell" + echo "To affect your current shell enviroment source this script with:" + echo "source env.sh" + exit +fi + +# check that this file is run +export ENV_RUN=true + +# if acados folder not specified assume parent of the folder of the single examples +ACADOS_INSTALL_DIR=${ACADOS_INSTALL_DIR:-"$(pwd)/../../.."} +export ACADOS_INSTALL_DIR +echo +echo "ACADOS_INSTALL_DIR=$ACADOS_INSTALL_DIR" + +# export casadi folder and matlab/octave mex folder +# MATLAB case +export MATLABPATH=$MATLABPATH:$ACADOS_INSTALL_DIR/external/casadi-matlab/ +export MATLABPATH=$MATLABPATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/ +export MATLABPATH=$MATLABPATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/ + +echo +echo "MATLABPATH=$MATLABPATH" +# Octave case +export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/external/casadi-octave/ +export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/ +export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/ +echo +echo "OCTAVE_PATH=$OCTAVE_PATH" + +# export acados mex flags +#export ACADOS_MEX_FLAGS="GCC=/usr/bin/gcc-4.9" + +# if model folder not specified assume this folder +MODEL_FOLDER=${MODEL_FOLDER:-"./build"} +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$ACADOS_INSTALL_DIR/lib:$MODEL_FOLDER +export LD_RUN_PATH="$(pwd)"/c_generated_code +echo +echo "LD_LIBRARY_PATH=$LD_LIBRARY_PATH" diff --git a/examples/acados_matlab_octave/generic_nlp/main.m b/examples/acados_matlab_octave/generic_nlp/main.m index b111b6a582..926497bcc8 100644 --- a/examples/acados_matlab_octave/generic_nlp/main.m +++ b/examples/acados_matlab_octave/generic_nlp/main.m @@ -47,7 +47,6 @@ model.name = 'generic_nlp'; model.x = x; model.p = p; -model.f_expl_expr = casadi.SX.zeros(length(model.x),1); %% acados ocp formulation ocp = AcadosOcp(); @@ -66,19 +65,10 @@ % initial parameter values ocp.parameter_values = zeros(length(model.p),1); -% set additional fields to prevent errors/warnings -ocp.cost.cost_type_0 = 'EXTERNAL'; -ocp.model.cost_expr_ext_cost_0 = 0; -ocp.cost.cost_type = 'EXTERNAL'; -ocp.model.cost_expr_ext_cost = 0; - %% solver options -ocp.solver_options.tf = 1; -ocp.solver_options.N_horizon = 1; +ocp.solver_options.N_horizon = 0; ocp.solver_options.nlp_solver_type = 'SQP'; -ocp.solver_options.integrator_type = 'ERK'; -ocp.solver_options.sim_method_num_stages = 1; -ocp.solver_options.sim_method_num_steps = 1; +ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'; %TODO: Change after PARTIAL_CONDENSING_HPIPM fix %% create the solver ocp_solver = AcadosOcpSolver(ocp); @@ -86,7 +76,7 @@ %% solve the NLP % initial guess init_x = [2.5; 3.0]; -ocp_solver.set('init_x', repmat(init_x,1,2)); +ocp_solver.set('init_x', init_x); % set the parameters p_value = [1;1]; @@ -104,7 +94,7 @@ end % display results -x_opt = ocp_solver.get('x',1); +x_opt = ocp_solver.get('x', 0); disp('Optimal solution:') % should be [1;1] for p = [1;1] disp(x_opt) disp(['Total time: ', num2str(1e3*total_time), ' ms']) diff --git a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py index 13a3561525..8ed7512374 100644 --- a/examples/acados_python/non_ocp_nlp/maratos_test_problem.py +++ b/examples/acados_python/non_ocp_nlp/maratos_test_problem.py @@ -41,7 +41,6 @@ # Settings PLOT = False -FOR_LOOPING = False # call solver in for loop to get all iterates TOL = 1e-6 def main(): @@ -63,6 +62,12 @@ def main(): pass else: solve_maratos_problem_with_setting(setting) + # exit(1) + # pass + # setting = {"globalization": "MERIT_BACKTRACKING", + # "line_search_use_sufficient_descent": 0, + # "globalization_use_SOC": 1} + # solve_maratos_problem_with_setting(setting) def solve_maratos_problem_with_setting(setting): @@ -77,57 +82,66 @@ def solve_maratos_problem_with_setting(setting): ocp = AcadosOcp() # set model - model = AcadosModel() x1 = SX.sym('x1') x2 = SX.sym('x2') x = vertcat(x1, x2) # dynamics: identity - model.disc_dyn_expr = x - model.x = x - model.u = SX.sym('u', 0, 0) # [] / None doesnt work - model.p = [] - model.name = f'maratos_problem' - ocp.model = model + ocp.model.x = x + ocp.model.name = f'maratos_problem' # discretization - Tf = 1 N = 1 ocp.solver_options.N_horizon = N - ocp.solver_options.tf = Tf - # cost - ocp.cost.cost_type_e = 'EXTERNAL' - ocp.model.cost_expr_ext_cost_e = x1 - - # constarints - ocp.model.con_h_expr_0 = x1 ** 2 + x2 ** 2 - ocp.constraints.lh_0 = np.array([1.0]) - ocp.constraints.uh_0 = np.array([1.0]) + if N == 0: + # cost + ocp.cost.cost_type_e = 'EXTERNAL' + ocp.model.cost_expr_ext_cost_e = x1 + + # constraints + ocp.model.con_h_expr_e = x1 ** 2 + x2 ** 2 + ocp.constraints.lh_e = np.array([1.0]) + ocp.constraints.uh_e = np.array([1.0]) + elif N == 1: + # dynamics: identity + ocp.model.disc_dyn_expr = x + ocp.model.u = SX.sym('u', 0, 0) # [] / None doesnt work + + # discretization + ocp.solver_options.tf = 1.0 + ocp.solver_options.integrator_type = 'DISCRETE' + + # cost + ocp.cost.cost_type_e = 'EXTERNAL' + ocp.model.cost_expr_ext_cost_e = x1 + + # constarints + ocp.model.con_h_expr_0 = x1 ** 2 + x2 ** 2 + ocp.constraints.lh_0 = np.array([1.0]) + ocp.constraints.uh_0 = np.array([1.0]) + else: + raise NotImplementedError('N > 1 not implemented') # # soften - # ocp.constraints.idxsh = np.array([0]) - # ocp.cost.zl = 1e5 * np.array([1]) - # ocp.cost.zu = 1e5 * np.array([1]) - # ocp.cost.Zl = 1e5 * np.array([1]) - # ocp.cost.Zu = 1e5 * np.array([1]) + # ocp.constraints.idxsh_e = np.array([0]) + # ocp.cost.zl_e = 1e5 * np.array([1]) + # ocp.cost.zu_e = 1e5 * np.array([1]) + # ocp.cost.Zl_e = 1e5 * np.array([1]) + # ocp.cost.Zu_e = 1e5 * np.array([1]) # add bounds on x # nx = 2 - # ocp.constraints.idxbx_0 = np.array(range(nx)) - # ocp.constraints.lbx_0 = -2 * np.ones((nx)) - # ocp.constraints.ubx_0 = 2 * np.ones((nx)) + # ocp.constraints.idxbx_e = np.array(range(nx)) + # ocp.constraints.lbx_e = -2 * np.ones((nx)) + # ocp.constraints.ubx_e = 2 * np.ones((nx)) # set options - ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES - # PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM, - # PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP + ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # TODO: check difference wrt FULL_CONDENSING ocp.solver_options.hessian_approx = 'EXACT' - ocp.solver_options.integrator_type = 'DISCRETE' - if globalization == 'FUNNEL_L1PEN_LINESEARCH': - ocp.solver_options.print_level = 1 + # ocp.solver_options.print_level = 2 ocp.solver_options.tol = TOL ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP - ocp.solver_options.levenberg_marquardt = 1e-1 + ocp.solver_options.levenberg_marquardt = 1e-1 # / (N+1) SQP_max_iter = 300 ocp.solver_options.qp_solver_iter_max = 400 ocp.solver_options.qp_tol = 5e-7 @@ -138,13 +152,10 @@ def solve_maratos_problem_with_setting(setting): ocp.solver_options.globalization_line_search_use_sufficient_descent = line_search_use_sufficient_descent ocp.solver_options.globalization_use_SOC = globalization_use_SOC ocp.solver_options.globalization_eps_sufficient_descent = 1e-1 + ocp.solver_options.store_iterates = True - if FOR_LOOPING: # call solver in for loop to get all iterates - ocp.solver_options.nlp_solver_max_iter = 1 - ocp_solver = AcadosOcpSolver(ocp, verbose=False) - else: - ocp.solver_options.nlp_solver_max_iter = SQP_max_iter - ocp_solver = AcadosOcpSolver(ocp, verbose=False) + ocp.solver_options.nlp_solver_max_iter = SQP_max_iter + ocp_solver = AcadosOcpSolver(ocp, verbose=False) # initialize solver rad_init = 0.1 #0.1 #np.pi / 4 @@ -153,34 +164,18 @@ def solve_maratos_problem_with_setting(setting): [ocp_solver.set(i, "x", xinit) for i in range(N+1)] # solve - if FOR_LOOPING: # call solver in for loop to get all iterates - iterates = np.zeros((SQP_max_iter+1, 2)) - iterates[0, :] = xinit - alphas = np.zeros((SQP_max_iter,)) - qp_iters = np.zeros((SQP_max_iter,)) - iter = SQP_max_iter - residuals = np.zeros((4, SQP_max_iter)) - - # solve - for i in range(SQP_max_iter): - status = ocp_solver.solve() - ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") - # print(f'acados returned status {status}.') - iterates[i+1, :] = ocp_solver.get(0, "x") - if status in [0, 4]: - iter = i - break - alphas[i] = ocp_solver.get_stats('alpha')[1] - qp_iters[i] = ocp_solver.get_stats('qp_iter')[1] - residuals[:, i] = ocp_solver.get_stats('residuals') - - else: - ocp_solver.solve() - ocp_solver.print_statistics() - iter = ocp_solver.get_stats('sqp_iter') - alphas = ocp_solver.get_stats('alpha')[1:] - qp_iters = ocp_solver.get_stats('qp_iter') - residuals = ocp_solver.get_stats('statistics')[1:5,1:iter] + ocp_solver.solve() + ocp_solver.print_statistics() + iter = ocp_solver.get_stats('sqp_iter') + alphas = ocp_solver.get_stats('alpha')[1:] + qp_iters = ocp_solver.get_stats('qp_iter') + residuals = ocp_solver.get_stats('statistics')[1:5,1:iter] + iterates = ocp_solver.get_iterates() + x_iterates = iterates.as_array('x') + if N > 0: + xdiff = x_iterates[:, 0, :] - x_iterates[:, 1, :] + xdiff = np.linalg.norm(xdiff, axis=1) + print(f"xdiff = {xdiff}") # get solution solution = ocp_solver.get(0, "x") @@ -199,7 +194,7 @@ def solve_maratos_problem_with_setting(setting): # checks if sol_err > TOL*1e1: - raise Exception(f"error of numerical solution wrt exact solution = {sol_err} > tol = {TOL*1e1}") + print(f"error of numerical solution wrt exact solution = {sol_err} > tol = {TOL*1e1}") else: print(f"matched analytical solution with tolerance {TOL}") @@ -214,9 +209,9 @@ def solve_maratos_problem_with_setting(setting): elif globalization == 'MERIT_BACKTRACKING': if max_infeasibility > 0.5: raise Exception(f"Expected max_infeasibility < 0.5 when using globalized SQP on Maratos problem") - if globalization_use_SOC == 0: - if FOR_LOOPING and iter != 57: - raise Exception(f"Expected 57 SQP iterations when using globalized SQP without SOC on Maratos problem, got {iter}") + elif globalization_use_SOC == 0: + if iter not in range(56, 61): + raise Exception(f"Expected 56 to 60 SQP iterations when using globalized SQP without SOC on Maratos problem, got {iter}") elif line_search_use_sufficient_descent == 1: if iter not in range(29, 37): # NOTE: got 29 locally and 36 on Github actions. @@ -230,13 +225,12 @@ def solve_maratos_problem_with_setting(setting): raise Exception(f"Expected 16 SQP iterations when using globalized SQP with SOC on Maratos problem, got {iter}") elif globalization == 'FUNNEL_L1PEN_LINESEARCH': if iter > 12: - raise Exception(f"Expected not more than 12 SQP iterations when using Funnel Method SQP, got {iter}") + raise Exception(f"Expected not more than 12 SQP iterations when using Funnel Method SQP, got {iter}") except Exception as inst: - if FOR_LOOPING and globalization == "MERIT_BACKTRACKING": - print("\nAcados globalized OCP solver behaves different when for looping due to different merit function weights.", - "Following exception is not raised\n") - print(inst, "\n") + if N == 0: + print(f"Exceptions in this file are tailored to formulation with N=1, difference should be investigated.") + print(f"got Exception {inst} in test with settings {setting}") else: raise(inst) @@ -244,10 +238,9 @@ def solve_maratos_problem_with_setting(setting): plt.figure() axs = plt.plot(solution[0], solution[1], 'x', label='solution') - if FOR_LOOPING: # call solver in for loop to get all iterates - cm = plt.cm.get_cmap('RdYlBu') - axs = plt.scatter(iterates[:iter+1,0], iterates[:iter+1,1], c=range(iter+1), s=35, cmap=cm, label='iterates') - plt.colorbar(axs) + cm = plt.cm.get_cmap('RdYlBu') + axs = plt.scatter(x_iterates[:iter+1, 0, 0], x_iterates[:iter+1, 0, 1], c=range(iter+1), s=35, cmap=cm, label='iterates') + plt.colorbar(axs) ts = np.linspace(0,2*np.pi,100) plt.plot(1 * np.cos(ts)+0,1 * np.sin(ts)-0, 'r') diff --git a/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py b/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py index f208fff186..66346fadc4 100644 --- a/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py +++ b/examples/acados_python/solution_sensitivities_convex_example/non_ocp_example.py @@ -40,23 +40,19 @@ def export_parametric_ocp() -> AcadosOcp: model = AcadosModel() model.x = ca.SX.sym("x", 1) model.p_global = ca.SX.sym("p_global", 1) - model.disc_dyn_expr = model.x - model.cost_expr_ext_cost = (model.x - model.p_global**2)**2 - model.cost_expr_ext_cost_e = 0 + model.cost_expr_ext_cost_e = (model.x - model.p_global**2)**2 model.name = "non_ocp" ocp = AcadosOcp() ocp.model = model - ocp.constraints.lbx_0 = np.array([-1.0]) - ocp.constraints.ubx_0 = np.array([1.0]) - ocp.constraints.idxbx_0 = np.array([0]) + ocp.constraints.lbx_e = np.array([-1.0]) + ocp.constraints.ubx_e = np.array([1.0]) + ocp.constraints.idxbx_e = np.array([0]) - ocp.cost.cost_type = "EXTERNAL" - ocp.solver_options.integrator_type = "DISCRETE" + ocp.cost.cost_type_e = "EXTERNAL" ocp.solver_options.qp_solver = "FULL_CONDENSING_HPIPM" ocp.solver_options.hessian_approx = "EXACT" - ocp.solver_options.N_horizon = 1 - ocp.solver_options.tf = 1.0 + ocp.solver_options.N_horizon = 0 ocp.p_global_values = np.zeros((1,)) ocp.solver_options.with_solution_sens_wrt_params = True @@ -92,7 +88,7 @@ def solve_and_compute_sens(p_test, tau): # breakpoint() # Calculate the policy gradient - out_dict = ocp_solver.eval_solution_sensitivity(0, "p_global", return_sens_x=True) + out_dict = ocp_solver.eval_solution_sensitivity(0, "p_global", return_sens_x=True, return_sens_u=False) sens_x[i] = out_dict['sens_x'].item() return solution, sens_x diff --git a/external/hpipm b/external/hpipm index e0f5466418..aebf3bcfb4 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit e0f5466418db49a44141b0afc4c58938ec8fb67d +Subproject commit aebf3bcfb4082a7b0d0c20ccea2f8a7dc00049ff diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index 79b638c958..e958c4b740 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -91,99 +91,13 @@ end end - function make_consistent(self, is_mocp_phase) - if nargin < 2 - is_mocp_phase = false; - end - self.model.make_consistent(self.dims); - - model = self.model; - dims = self.dims; + function make_consistent_cost_initial(self) cost = self.cost; - constraints = self.constraints; - opts = self.solver_options; - - N = opts.N_horizon; - self.detect_cost_and_constraints(); - - % check if nx != nx_next - if ~is_mocp_phase && dims.nx ~= dims.nx_next && opts.N_horizon > 1 - error(['nx_next = ', num2str(dims.nx_next), ' must be equal to nx = ', num2str(dims.nx), ' if more than one shooting interval is used.']); - end - - % detect GNSF structure - if strcmp(opts.integrator_type, 'GNSF') - if dims.gnsf_nx1 + dims.gnsf_nx2 ~= dims.nx - % TODO: properly interface those. - gnsf_transcription_opts = struct(); - detect_gnsf_structure(model, dims, gnsf_transcription_opts); - else - warning('No GNSF model detected, assuming all required fields are set.') - end - end - - % sanity checks on options, which are done in setters in Python - qp_solvers = {'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', 'FULL_CONDENSING_DAQP'}; - if ~ismember(opts.qp_solver, qp_solvers) - error(['Invalid qp_solver: ', opts.qp_solver, '. Available options are: ', strjoin(qp_solvers, ', ')]); - end - - regularize_methods = {'NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY', 'GERSHGORIN_LEVENBERG_MARQUARDT'}; - if ~ismember(opts.regularize_method, regularize_methods) - error(['Invalid regularize_method: ', opts.regularize_method, '. Available options are: ', strjoin(regularize_methods, ', ')]); - end - hpipm_modes = {'BALANCE', 'SPEED_ABS', 'SPEED', 'ROBUST'}; - if ~ismember(opts.hpipm_mode, hpipm_modes) - error(['Invalid hpipm_mode: ', opts.hpipm_mode, '. Available options are: ', strjoin(hpipm_modes, ', ')]); - end - INTEGRATOR_TYPES = {'ERK', 'IRK', 'GNSF', 'DISCRETE', 'LIFTED_IRK'}; - if ~ismember(opts.integrator_type, INTEGRATOR_TYPES) - error(['Invalid integrator_type: ', opts.integrator_type, '. Available options are: ', strjoin(INTEGRATOR_TYPES, ', ')]); - end - - COLLOCATION_TYPES = {'GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE', 'EXPLICIT_RUNGE_KUTTA'}; - if ~ismember(opts.collocation_type, COLLOCATION_TYPES) - error(['Invalid collocation_type: ', opts.collocation_type, '. Available options are: ', strjoin(COLLOCATION_TYPES, ', ')]); - end - - COST_DISCRETIZATION_TYPES = {'EULER', 'INTEGRATOR'}; - if ~ismember(opts.cost_discretization, COST_DISCRETIZATION_TYPES) - error(['Invalid cost_discretization: ', opts.cost_discretization, '. Available options are: ', strjoin(COST_DISCRETIZATION_TYPES, ', ')]); - end - - search_direction_modes = {'NOMINAL_QP', 'BYRD_OMOJOKUN', 'FEASIBILITY_QP'}; - if ~ismember(opts.search_direction_mode, search_direction_modes) - error(['Invalid search_direction_mode: ', opts.search_direction_mode, '. Available options are: ', strjoin(search_direction_modes, ', ')]); - end - - % OCP name - self.name = model.name; - - % parameters - if isempty(self.parameter_values) - if dims.np > 0 - warning(['self.parameter_values are not set.', ... - 10 'Using zeros(np,1) by default.' 10 'You can update them later using set().']); - end - self.parameter_values = zeros(self.dims.np,1); - elseif length(self.parameter_values) ~= self.dims.np - error(['parameter_values has the wrong shape. Expected: ' num2str(self.dims.np)]) - end - - - % parameters - if isempty(self.p_global_values) - if dims.np_global > 0 - warning(['self.p_global_values are not set.', ... - 10 'Using zeros(np_global,1) by default.' 10 'You can update them later using set().']); - end - self.p_global_values = zeros(self.dims.np_global,1); - elseif length(self.p_global_values) ~= self.dims.np_global - error(['p_global_values has the wrong shape. Expected: ' num2str(self.dims.np_global)]) + dims = self.dims; + model = self.model; + if self.solver_options.N_horizon == 0 + return end - - %% cost - % initial if strcmp(cost.cost_type_0, 'LINEAR_LS') if ~isempty(cost.W_0) && ~isempty(cost.Vx_0) && ~isempty(cost.Vu_0) ny = length(cost.W_0); @@ -214,8 +128,15 @@ function make_consistent(self, is_mocp_phase) end dims.ny_0 = ny; end + end - % path + function make_consistent_cost_path(self) + cost = self.cost; + dims = self.dims; + model = self.model; + if self.solver_options.N_horizon == 0 + return + end if strcmp(cost.cost_type, 'LINEAR_LS') if ~isempty(cost.W) && ~isempty(cost.Vx) && ~isempty(cost.Vu) ny = length(cost.W); @@ -245,8 +166,13 @@ function make_consistent(self, is_mocp_phase) end dims.ny = ny; end + end + + function make_consistent_cost_terminal(self) + cost = self.cost; + dims = self.dims; + model = self.model; - % terminal if strcmp(cost.cost_type_e, 'LINEAR_LS') if ~isempty(cost.W_e) && ~isempty(cost.Vx_e) ny_e = length(cost.W_e); @@ -279,20 +205,16 @@ function make_consistent(self, is_mocp_phase) end dims.ny_e = ny_e; end + end - % cost integration - if strcmp(opts.cost_discretization, "INTEGRATOR") - if ~(strcmp(cost.cost_type, "NONLINEAR_LS") || strcmp(cost.cost_type, "CONVEX_OVER_NONLINEAR")) - error('INTEGRATOR cost discretization requires CONVEX_OVER_NONLINEAR or NONLINEAR_LS cost type for path cost.') - end - if ~(strcmp(cost.cost_type_0, "NONLINEAR_LS") || strcmp(cost.cost_type_0, "CONVEX_OVER_NONLINEAR")) - error('INTEGRATOR cost discretization requires CONVEX_OVER_NONLINEAR or NONLINEAR_LS cost type for initial cost.') - end + function make_consistent_constraints_initial(self) + dims = self.dims; + constraints = self.constraints; + model = self.model; + if self.solver_options.N_horizon == 0 + return end - - %% constraints - % initial if ~isempty(constraints.idxbx_0) && ~isempty(constraints.lbx_0) && ~isempty(constraints.ubx_0) nbx_0 = length(constraints.lbx_0); if nbx_0 ~= length(constraints.ubx_0) || nbx_0 ~= length(constraints.idxbx_0) @@ -312,7 +234,29 @@ function make_consistent(self, is_mocp_phase) dims.nbxe_0 = length(constraints.idxbxe_0); - % path + if ~isempty(model.con_h_expr_0) && ... + ~isempty(constraints.lh_0) && ~isempty(constraints.uh_0) + nh_0 = length(constraints.lh_0); + if nh_0 ~= length(constraints.uh_0) || nh_0 ~= length(model.con_h_expr_0) + error('inconsistent dimension nh_0, regarding expr_h_0, lh_0, uh_0.'); + end + elseif ~isempty(model.con_h_expr_0) || ... + ~isempty(constraints.lh_0) || ~isempty(constraints.uh_0) + error('setting external constraint function h: need expr_h_0, lh_0, uh_0 at least one missing.'); + else + nh_0 = 0; + end + dims.nh_0 = nh_0; + end + + function make_consistent_constraints_path(self) + dims = self.dims; + constraints = self.constraints; + model = self.model; + if self.solver_options.N_horizon == 0 + return + end + if ~isempty(constraints.idxbx) && ~isempty(constraints.lbx) && ~isempty(constraints.ubx) nbx = length(constraints.lbx); if nbx ~= length(constraints.ubx) || nbx ~= length(constraints.idxbx) @@ -358,7 +302,7 @@ function make_consistent(self, is_mocp_phase) dims.ng = ng; if ~isempty(model.con_h_expr) && ... - ~isempty(constraints.lh) && ~isempty(constraints.uh) + ~isempty(constraints.lh) && ~isempty(constraints.uh) nh = length(constraints.lh); if nh ~= length(constraints.uh) || nh ~= length(model.con_h_expr) error('inconsistent dimension nh, regarding expr_h, lh, uh.'); @@ -370,22 +314,13 @@ function make_consistent(self, is_mocp_phase) nh = 0; end dims.nh = nh; + end - if ~isempty(model.con_h_expr_0) && ... - ~isempty(constraints.lh_0) && ~isempty(constraints.uh_0) - nh_0 = length(constraints.lh_0); - if nh_0 ~= length(constraints.uh_0) || nh_0 ~= length(model.con_h_expr_0) - error('inconsistent dimension nh_0, regarding expr_h_0, lh_0, uh_0.'); - end - elseif ~isempty(model.con_h_expr_0) || ... - ~isempty(constraints.lh_0) || ~isempty(constraints.uh_0) - error('setting external constraint function h: need expr_h_0, lh_0, uh_0 at least one missing.'); - else - nh_0 = 0; - end - dims.nh_0 = nh_0; + function make_consistent_constraints_terminal(self) + dims = self.dims; + constraints = self.constraints; + model = self.model; - % terminal if ~isempty(constraints.idxbx_e) && ~isempty(constraints.lbx_e) && ~isempty(constraints.ubx_e) nbx_e = length(constraints.lbx_e); if nbx_e ~= length(constraints.ubx_e) || nbx_e ~= length(constraints.idxbx_e) @@ -416,7 +351,7 @@ function make_consistent(self, is_mocp_phase) dims.ng_e = ng_e; if ~isempty(model.con_h_expr_e) && ... - ~isempty(constraints.lh_e) && ~isempty(constraints.uh_e) + ~isempty(constraints.lh_e) && ~isempty(constraints.uh_e) nh_e = length(constraints.lh_e); if nh_e ~= length(constraints.uh_e) || nh_e ~= length(model.con_h_expr_e) error('inconsistent dimension nh_e, regarding expr_h_e, lh_e, uh_e.'); @@ -428,8 +363,16 @@ function make_consistent(self, is_mocp_phase) nh_e = 0; end dims.nh_e = nh_e; + end + + function make_consistent_slack_dimensions_path(self) + constraints = self.constraints; + dims = self.dims; + cost = self.cost; + if self.solver_options.N_horizon == 0 + return + end - %% slack dimensions nsbx = length(constraints.idxsbx); nsbu = length(constraints.idxsbu); nsg = length(constraints.idxsg); @@ -481,8 +424,18 @@ function make_consistent(self, is_mocp_phase) dims.nsg = nsg; dims.nsh = nsh; dims.nsphi = nsphi; + end + + function make_consistent_slack_dimensions_initial(self) + constraints = self.constraints; + dims = self.dims; + cost = self.cost; + nsbu = dims.nsbu; + nsg = dims.nsg; + if self.solver_options.N_horizon == 0 + return + end - % slacks at initial stage nsh_0 = length(constraints.idxsh_0); nsphi_0 = length(constraints.idxsphi_0); @@ -522,8 +475,13 @@ function make_consistent(self, is_mocp_phase) dims.ns_0 = ns_0; dims.nsh_0 = nsh_0; dims.nsphi_0 = nsphi_0; + end - %% terminal slack dimensions + function make_consistent_slack_dimensions_terminal(self) + constraints = self.constraints; + dims = self.dims; + cost = self.cost; + nsbx_e = length(constraints.idxsbx_e); nsg_e = length(constraints.idxsg_e); nsh_e = length(constraints.idxsh_e); @@ -572,46 +530,27 @@ function make_consistent(self, is_mocp_phase) dims.nsh_e = nsh_e; dims.nsphi_e = nsphi_e; - % check for ACADOS_INFTY - if ~ismember(opts.qp_solver, {'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_HPIPM', 'FULL_CONDENSING_DAQP'}) - ACADOS_INFTY = get_acados_infty(); - % loop over all bound vectors - fields = {'lbx_0', 'ubx_0', 'lbx', 'ubx', 'lbx_e', 'ubx_e', 'lg', 'ug', 'lg_e', 'ug_e', 'lh', 'uh', 'lh_e', 'uh_e', 'lbu', 'ubu', 'lphi', 'uphi', 'lphi_e', 'uphi_e'}; - for i = 1:length(fields) - field = fields{i}; - bound = constraints.(field); - if any(bound >= ACADOS_INFTY) || any(bound <= -ACADOS_INFTY) - error(['Field ', field, ' contains values outside the interval (-ACADOS_INFTY, ACADOS_INFTY) with ACADOS_INFTY = ', num2str(ACADOS_INFTY, '%.2e'), '. One-sided constraints are not supported by the chosen QP solver ', opts.qp_solver, '.']); - end - end - end + end - % shooting nodes -> time_steps - % discretization - if isempty(opts.N_horizon) && isempty(dims.N) - error('N_horizon not provided.'); - elseif isempty(opts.N_horizon) && ~isempty(dims.N) - opts.N_horizon = dims.N; - disp(['field AcadosOcpDims.N has been migrated to AcadosOcpOptions.N_horizon.',... - ' setting AcadosOcpOptions.N_horizon = N.',... - ' For future comppatibility, please use AcadosOcpOptions.N_horizon directly.']); - elseif ~isempty(opts.N_horizon) && ~isempty(dims.N) && opts.N_horizon ~= dims.N - error(['Inconsistent dimension N, regarding N = ', num2str(dims.N),... - ', N_horizon = ', num2str(opts.N_horizon), '.']); - else - dims.N = opts.N_horizon; + function make_consistent_discretization(self) + dims = self.dims; + opts = self.solver_options; + + if opts.N_horizon == 0 + opts.shooting_nodes = zeros(1, 1); + opts.time_steps = ones(1, 1); + return end - N = opts.N_horizon; if length(opts.tf) ~= 1 || opts.tf < 0 error('time horizon tf should be a nonnegative number'); end if ~isempty(opts.shooting_nodes) - if N + 1 ~= length(opts.shooting_nodes) + if opts.N_horizon + 1 ~= length(opts.shooting_nodes) error('inconsistent dimension N regarding shooting nodes.'); end - for i=1:N + for i=1:opts.N_horizon opts.time_steps(i) = opts.shooting_nodes(i+1) - opts.shooting_nodes(i); end sum_time_steps = sum(opts.time_steps); @@ -620,7 +559,7 @@ function make_consistent(self, is_mocp_phase) opts.time_steps = opts.time_steps * opts.tf / sum_time_steps; end elseif ~isempty(opts.time_steps) - if N ~= length(opts.time_steps) + if opts.N_horizon ~= length(opts.time_steps) error('inconsistent dimension N regarding time steps.'); end sum_time_steps = sum(opts.time_steps); @@ -629,12 +568,12 @@ function make_consistent(self, is_mocp_phase) 'got tf = ' num2str(opts.tf) '; sum(time_steps) = ' num2str(sum_time_steps) '.']); end else - opts.time_steps = opts.tf/N * ones(N,1); + opts.time_steps = opts.tf/opts.N_horizon * ones(opts.N_horizon,1); end % add consistent shooting_nodes e.g. for plotting; if isempty(opts.shooting_nodes) - opts.shooting_nodes = zeros(N+1, 1); - for i = 1:N + opts.shooting_nodes = zeros(opts.N_horizon+1, 1); + for i = 1:opts.N_horizon opts.shooting_nodes(i+1) = sum(opts.time_steps(1:i)); end end @@ -642,12 +581,12 @@ function make_consistent(self, is_mocp_phase) error(['ocp discretization: time_steps between shooting nodes must all be > 0', ... ' got: ' num2str(opts.time_steps)]) end + end - % cost_scaling - if isempty(opts.cost_scaling) - opts.cost_scaling = [opts.time_steps(:); 1.0]; - elseif length(opts.cost_scaling) ~= N+1 - error(['cost_scaling must have length N+1 = ', num2str(N+1)]); + function make_consistent_simulation(self) + opts = self.solver_options + if opts.N_horizon == 0 + return end % set integrator time automatically @@ -662,29 +601,191 @@ function make_consistent(self, is_mocp_phase) end end - % qpdunes - if ~isempty(strfind(opts.qp_solver,'qpdunes')) - constraints.idxbxe_0 = []; - dims.nbxe_0 = 0; - end - %% options sanity checks if length(opts.sim_method_num_steps) == 1 - opts.sim_method_num_steps = opts.sim_method_num_steps * ones(1, N); - elseif length(opts.sim_method_num_steps) ~= N + opts.sim_method_num_steps = opts.sim_method_num_steps * ones(1, opts.N_horizon); + elseif length(opts.sim_method_num_steps) ~= opts.N_horizon error('sim_method_num_steps must be a scalar or a vector of length N'); end if length(opts.sim_method_num_stages) == 1 - opts.sim_method_num_stages = opts.sim_method_num_stages * ones(1, N); - elseif length(opts.sim_method_num_stages) ~= N + opts.sim_method_num_stages = opts.sim_method_num_stages * ones(1, opts.N_horizon); + elseif length(opts.sim_method_num_stages) ~= opts.N_horizon error('sim_method_num_stages must be a scalar or a vector of length N'); end if length(opts.sim_method_jac_reuse) == 1 - opts.sim_method_jac_reuse = opts.sim_method_jac_reuse * ones(1, N); - elseif length(opts.sim_method_jac_reuse) ~= N + opts.sim_method_jac_reuse = opts.sim_method_jac_reuse * ones(1, opts.N_horizon); + elseif length(opts.sim_method_jac_reuse) ~= opts.N_horizon error('sim_method_jac_reuse must be a scalar or a vector of length N'); end + + end + + function make_consistent(self, is_mocp_phase) + if nargin < 2 + is_mocp_phase = false; + end + self.model.make_consistent(self.dims); + + model = self.model; + dims = self.dims; + cost = self.cost; + constraints = self.constraints; + opts = self.solver_options; + + self.detect_cost_and_constraints(); + + if isempty(opts.N_horizon) && isempty(dims.N) + error('N_horizon not provided.'); + elseif isempty(opts.N_horizon) && ~isempty(dims.N) + opts.N_horizon = dims.N; + disp(['field AcadosOcpDims.N has been migrated to AcadosOcpOptions.N_horizon.',... + ' setting AcadosOcpOptions.N_horizon = N.',... + ' For future comppatibility, please use AcadosOcpOptions.N_horizon directly.']); + elseif ~isempty(opts.N_horizon) && ~isempty(dims.N) && opts.N_horizon ~= dims.N + error(['Inconsistent dimension N, regarding N = ', num2str(dims.N),... + ', N_horizon = ', num2str(opts.N_horizon), '.']); + else + dims.N = opts.N_horizon; + end + + % check if nx != nx_next + if ~is_mocp_phase && dims.nx ~= dims.nx_next && opts.N_horizon > 1 + error(['nx_next = ', num2str(dims.nx_next), ' must be equal to nx = ', num2str(dims.nx), ' if more than one shooting interval is used.']); + end + + % detect GNSF structure + if strcmp(opts.integrator_type, 'GNSF') && opts.N_horizon > 0 + if dims.gnsf_nx1 + dims.gnsf_nx2 ~= dims.nx + % TODO: properly interface those. + gnsf_transcription_opts = struct(); + detect_gnsf_structure(model, dims, gnsf_transcription_opts); + else + warning('No GNSF model detected, assuming all required fields are set.') + end + end + + % sanity checks on options, which are done in setters in Python + qp_solvers = {'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', 'FULL_CONDENSING_DAQP'}; + if ~ismember(opts.qp_solver, qp_solvers) + error(['Invalid qp_solver: ', opts.qp_solver, '. Available options are: ', strjoin(qp_solvers, ', ')]); + end + + regularize_methods = {'NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY', 'GERSHGORIN_LEVENBERG_MARQUARDT'}; + if ~ismember(opts.regularize_method, regularize_methods) + error(['Invalid regularize_method: ', opts.regularize_method, '. Available options are: ', strjoin(regularize_methods, ', ')]); + end + hpipm_modes = {'BALANCE', 'SPEED_ABS', 'SPEED', 'ROBUST'}; + if ~ismember(opts.hpipm_mode, hpipm_modes) + error(['Invalid hpipm_mode: ', opts.hpipm_mode, '. Available options are: ', strjoin(hpipm_modes, ', ')]); + end + INTEGRATOR_TYPES = {'ERK', 'IRK', 'GNSF', 'DISCRETE', 'LIFTED_IRK'}; + if ~ismember(opts.integrator_type, INTEGRATOR_TYPES) + error(['Invalid integrator_type: ', opts.integrator_type, '. Available options are: ', strjoin(INTEGRATOR_TYPES, ', ')]); + end + + COLLOCATION_TYPES = {'GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE', 'EXPLICIT_RUNGE_KUTTA'}; + if ~ismember(opts.collocation_type, COLLOCATION_TYPES) + error(['Invalid collocation_type: ', opts.collocation_type, '. Available options are: ', strjoin(COLLOCATION_TYPES, ', ')]); + end + + COST_DISCRETIZATION_TYPES = {'EULER', 'INTEGRATOR'}; + if ~ismember(opts.cost_discretization, COST_DISCRETIZATION_TYPES) + error(['Invalid cost_discretization: ', opts.cost_discretization, '. Available options are: ', strjoin(COST_DISCRETIZATION_TYPES, ', ')]); + end + + search_direction_modes = {'NOMINAL_QP', 'BYRD_OMOJOKUN', 'FEASIBILITY_QP'}; + if ~ismember(opts.search_direction_mode, search_direction_modes) + error(['Invalid search_direction_mode: ', opts.search_direction_mode, '. Available options are: ', strjoin(search_direction_modes, ', ')]); + end + + % OCP name + self.name = model.name; + + % parameters + if isempty(self.parameter_values) + if dims.np > 0 + warning(['self.parameter_values are not set.', ... + 10 'Using zeros(np,1) by default.' 10 'You can update them later using set().']); + end + self.parameter_values = zeros(self.dims.np,1); + elseif length(self.parameter_values) ~= self.dims.np + error(['parameter_values has the wrong shape. Expected: ' num2str(self.dims.np)]) + end + + + % parameters + if isempty(self.p_global_values) + if dims.np_global > 0 + warning(['self.p_global_values are not set.', ... + 10 'Using zeros(np_global,1) by default.' 10 'You can update them later using set().']); + end + self.p_global_values = zeros(self.dims.np_global,1); + elseif length(self.p_global_values) ~= self.dims.np_global + error(['p_global_values has the wrong shape. Expected: ' num2str(self.dims.np_global)]) + end + + %% cost + self.make_consistent_cost_initial(); + self.make_consistent_cost_path(); + self.make_consistent_cost_terminal(); + + % cost integration + if strcmp(opts.cost_discretization, "INTEGRATOR") && opts.N_horizon > 0 + if ~(strcmp(cost.cost_type, "NONLINEAR_LS") || strcmp(cost.cost_type, "CONVEX_OVER_NONLINEAR")) + error('INTEGRATOR cost discretization requires CONVEX_OVER_NONLINEAR or NONLINEAR_LS cost type for path cost.') + end + if ~(strcmp(cost.cost_type_0, "NONLINEAR_LS") || strcmp(cost.cost_type_0, "CONVEX_OVER_NONLINEAR")) + error('INTEGRATOR cost discretization requires CONVEX_OVER_NONLINEAR or NONLINEAR_LS cost type for initial cost.') + end + end + + + %% constraints + self.make_consistent_constraints_initial(); + self.make_consistent_constraints_path(); + self.make_consistent_constraints_terminal(); + + %% slack dimensions + self.make_consistent_slack_dimensions_path(); + self.make_consistent_slack_dimensions_initial(); + self.make_consistent_slack_dimensions_terminal(); + + % check for ACADOS_INFTY + if ~ismember(opts.qp_solver, {'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_HPIPM', 'FULL_CONDENSING_DAQP'}) + ACADOS_INFTY = get_acados_infty(); + % loop over all bound vectors + if opts.N_horizon > 0 + fields = {'lbx_e', 'ubx_e', 'lg_e', 'ug_e', 'lh_e', 'uh_e', 'lphi_e', 'uphi_e'}; + else + fields = {'lbx_0', 'ubx_0', 'lbx', 'ubx', 'lbx_e', 'ubx_e', 'lg', 'ug', 'lg_e', 'ug_e', 'lh', 'uh', 'lh_e', 'uh_e', 'lbu', 'ubu', 'lphi', 'uphi', 'lphi_e', 'uphi_e'}; + end + for i = 1:length(fields) + field = fields{i}; + bound = constraints.(field); + if any(bound >= ACADOS_INFTY) || any(bound <= -ACADOS_INFTY) + error(['Field ', field, ' contains values outside the interval (-ACADOS_INFTY, ACADOS_INFTY) with ACADOS_INFTY = ', num2str(ACADOS_INFTY, '%.2e'), '. One-sided constraints are not supported by the chosen QP solver ', opts.qp_solver, '.']); + end + end + end + + self.make_consistent_discretization(); + + % cost_scaling + if isempty(opts.cost_scaling) + opts.cost_scaling = [opts.time_steps(:); 1.0]; + elseif length(opts.cost_scaling) ~= opts.N_horizon+1 + error(['cost_scaling must have length N+1 = ', num2str(N+1)]); + end + + self.make_consistent_simulation(); + + % qpdunes + if ~isempty(strfind(opts.qp_solver,'qpdunes')) + constraints.idxbxe_0 = []; + dims.nbxe_0 = 0; + end + if strcmp(opts.qp_solver, "PARTIAL_CONDENSING_HPMPC") || ... strcmp(opts.qp_solver, "PARTIAL_CONDENSING_QPDUNES") || ... strcmp(opts.qp_solver, "PARTIAL_CONDENSING_OOQP") @@ -704,8 +805,14 @@ function make_consistent(self, is_mocp_phase) if opts.hessian_approx == 'EXACT' error('fixed_hess and hessian_approx = EXACT are incompatible') end - if ~(strcmp(cost.cost_type_0, "LINEAR_LS") && strcmp(cost.cost_type, "LINEAR_LS") && strcmp(cost.cost_type_e, "LINEAR_LS")) - error('fixed_hess requires LINEAR_LS cost type') + if ~strcmp(cost.cost_type_0, "LINEAR_LS") && opts.N_horizon > 0 + error('fixed_hess requires LINEAR_LS cost_type_0') + end + if ~strcmp(cost.cost_type, "LINEAR_LS") && opts.N_horizon > 0 + error('fixed_hess requires LINEAR_LS cost_type') + end + if ~strcmp(cost.cost_type_e, "LINEAR_LS") + error('fixed_hess requires LINEAR_LS cost_type_e') end end @@ -713,11 +820,14 @@ function make_consistent(self, is_mocp_phase) % check if qp_solver_cond_N is set if isempty(opts.qp_solver_cond_N) - opts.qp_solver_cond_N = N; + opts.qp_solver_cond_N = opts.N_horizon; + end + if opts.qp_solver_cond_N > opts.N_horizon + error('qp_solver_cond_N > N_horizon is not supported.'); end if ~isempty(opts.qp_solver_cond_block_size) - if sum(opts.qp_solver_cond_block_size) ~= N + if sum(opts.qp_solver_cond_block_size) ~= opts.N_horizon error(['sum(qp_solver_cond_block_size) =', num2str(sum(opts.qp_solver_cond_block_size)), ' != N = {opts.N_horizon}.']); end if length(opts.qp_solver_cond_block_size) ~= opts.qp_solver_cond_N+1 @@ -726,8 +836,11 @@ function make_consistent(self, is_mocp_phase) end if strcmp(opts.nlp_solver_type, "DDP") + if opts.N_horizon == 0 + error('DDP solver only supported for N_horizon > 0.'); + end if ~strcmp(opts.qp_solver, "PARTIAL_CONDENSING_HPIPM") || (opts.qp_solver_cond_N ~= opts.N_horizon) - error('DDP solver only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N.'); + error('DDP solver only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N_horizon.'); end if any([dims.nbu, dims.nbx, dims.ng, dims.nh, dims.nphi]) error('DDP only supports initial state constraints, got path constraints.') @@ -745,9 +858,14 @@ function make_consistent(self, is_mocp_phase) error('tau_min > 0 is only compatible with HPIPM.'); end - if (opts.as_rti_level == 1 || opts.as_rti_level == 2) && any([strcmp(cost.cost_type, {'LINEAR_LS', 'NONLINEAR_LS'}) ... - strcmp(cost.cost_type_0, {'LINEAR_LS', 'NONLINEAR_LS'}) ... - strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})]) + if opts.N_horizon == 0 + cost_types_to_check = [strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})] + else + cost_types_to_check = [strcmp(cost.cost_type, {'LINEAR_LS', 'NONLINEAR_LS'}) ... + strcmp(cost.cost_type_0, {'LINEAR_LS', 'NONLINEAR_LS'}) ... + strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})] + end + if (opts.as_rti_level == 1 || opts.as_rti_level == 2) && any(cost_types_to_check) error('as_rti_level in [1, 2] not supported for LINEAR_LS and NONLINEAR_LS cost type.'); end @@ -861,12 +979,26 @@ function make_consistent(self, is_mocp_phase) end if isa(self.zoro_description, 'ZoroDescription') + if opts.N_horizon == 0 + error('ZORO only supported for N_horizon > 0.'); + end self.zoro_description.process(); end end function [] = detect_cost_and_constraints(self) % detect cost type + N = self.solver_options.N_horizon + if N == 0 + if strcmp(self.cost.cost_type_e, 'AUTO') + detect_cost_type(self.model, self.cost, self.dims, 'terminal'); + end + if strcmp(self.constraints.constr_type_e, 'AUTO') + detect_constraint_structure(self.model, self.constraints, 'terminal'); + end + return + end + stage_types = {'initial', 'path', 'terminal'}; cost_types = {self.cost.cost_type_0, self.cost.cost_type, self.cost.cost_type_e}; @@ -941,44 +1073,20 @@ function make_consistent(self, is_mocp_phase) cost = ocp.cost; dims = ocp.dims; - % dynamics - model_dir = fullfile(pwd, code_gen_opts.code_export_directory, [ocp.name '_model']); + setup_code_generation_context_dynamics(ocp, context); - if strcmp(ocp.model.dyn_ext_fun_type, 'generic') - check_dir_and_create(model_dir); - copyfile(fullfile(pwd, ocp.model.dyn_generic_source), model_dir); - context.add_external_function_file(ocp.model.dyn_generic_source, model_dir); - elseif strcmp(ocp.model.dyn_ext_fun_type, 'casadi') - check_casadi_version(); - switch solver_opts.integrator_type - case 'ERK' - generate_c_code_explicit_ode(context, ocp.model, model_dir); - case 'IRK' - generate_c_code_implicit_ode(context, ocp.model, model_dir); - case 'LIFTED_IRK' - if ~(isempty(ocp.model.t) || length(ocp.model.t) == 0) - error('NOT LIFTED_IRK with time-varying dynamics not implemented yet.') - end - generate_c_code_implicit_ode(context, ocp.model, model_dir); - case 'GNSF' - generate_c_code_gnsf(context, ocp.model, model_dir); - case 'DISCRETE' - generate_c_code_discrete_dynamics(context, ocp.model, model_dir); - otherwise - error('Unknown integrator type.') - end - else - error('Unknown dyn_ext_fun_type.') - end - - if ignore_initial && ignore_terminal - stage_type_indices = [2]; - elseif ignore_terminal - stage_type_indices = [1, 2]; - elseif ignore_initial - stage_type_indices = [2, 3]; + if solver_opts.N_horizon == 0 + stage_type_indices = [3]; else - stage_type_indices = [1, 2, 3]; + if ignore_initial && ignore_terminal + stage_type_indices = [2]; + elseif ignore_terminal + stage_type_indices = [1, 2]; + elseif ignore_initial + stage_type_indices = [2, 3]; + else + stage_type_indices = [1, 2, 3]; + end end stage_types = {'initial', 'path', 'terminal'}; @@ -1030,6 +1138,43 @@ function make_consistent(self, is_mocp_phase) end end + function setup_code_generation_context_dynamics(ocp, context) + code_gen_opts = context.opts; + solver_opts = ocp.solver_options; + if solver_opts.N_horizon == 0 + return + end + + model_dir = fullfile(pwd, code_gen_opts.code_export_directory, [ocp.name '_model']); + + if strcmp(ocp.model.dyn_ext_fun_type, 'generic') + check_dir_and_create(model_dir); + copyfile(fullfile(pwd, ocp.model.dyn_generic_source), model_dir); + context.add_external_function_file(ocp.model.dyn_generic_source, model_dir); + elseif strcmp(ocp.model.dyn_ext_fun_type, 'casadi') + check_casadi_version(); + switch solver_opts.integrator_type + case 'ERK' + generate_c_code_explicit_ode(context, ocp.model, model_dir); + case 'IRK' + generate_c_code_implicit_ode(context, ocp.model, model_dir); + case 'LIFTED_IRK' + if ~(isempty(ocp.model.t) || length(ocp.model.t) == 0) + error('NOT LIFTED_IRK with time-varying dynamics not implemented yet.') + end + generate_c_code_implicit_ode(context, ocp.model, model_dir); + case 'GNSF' + generate_c_code_gnsf(context, ocp.model, model_dir); + case 'DISCRETE' + generate_c_code_discrete_dynamics(context, ocp.model, model_dir); + otherwise + error('Unknown integrator type.') + end + else + error('Unknown dyn_ext_fun_type.') + end + end + function render_templates(self) %% render templates diff --git a/interfaces/acados_template/acados_template/acados_dims.py b/interfaces/acados_template/acados_template/acados_dims.py index 1ca68b8103..9b6056d2a8 100644 --- a/interfaces/acados_template/acados_template/acados_dims.py +++ b/interfaces/acados_template/acados_template/acados_dims.py @@ -597,5 +597,5 @@ def ng_e(self, ng_e): @N.setter def N(self, N): - check_int_value("N", N, positive=True) + check_int_value("N", N, nonnegative=True) self.__N = N diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 2be43613e5..85c3228d20 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -163,35 +163,16 @@ def json_file(self): def json_file(self, json_file): self.__json_file = json_file - def make_consistent(self, is_mocp_phase=False) -> None: - """ - Detect dimensions, perform sanity checks - """ + + def _make_consistent_cost_initial(self): dims = self.dims cost = self.cost - constraints = self.constraints model = self.model opts = self.solver_options + if opts.N_horizon == 0: + return - model.make_consistent(dims) - self.name = model.name - - # check if nx != nx_next - if not is_mocp_phase and dims.nx != dims.nx_next and opts.N_horizon > 1: - raise ValueError('nx_next should be equal to nx if more than one shooting interval is used.') - - # parameters - if self.parameter_values.shape[0] != dims.np: - raise ValueError('inconsistent dimension np, regarding model.p and parameter_values.' + \ - f'\nGot np = {dims.np}, self.parameter_values.shape = {self.parameter_values.shape[0]}\n') - - # p_global_values - if self.p_global_values.shape[0] != dims.np_global: - raise ValueError('inconsistent dimension np_global, regarding model.p_global and p_global_values.' + \ - f'\nGot np_global = {dims.np_global}, self.p_global_values.shape = {self.p_global_values.shape[0]}\n') - - ## cost - # initial stage - if not set, copy fields from path constraints + # if not set, copy fields from path constraints if cost.cost_type_0 is None: self.copy_path_cost_to_stage_0() @@ -259,26 +240,15 @@ def make_consistent(self, is_mocp_phase=False) -> None: if model.cost_expr_ext_cost_custom_hess_0.shape != (dims.nx+dims.nu, dims.nx+dims.nu): raise ValueError('cost_expr_ext_cost_custom_hess_0 should have shape (nx+nu, nx+nu).') - # GN check - gn_warning_0 = (cost.cost_type_0 == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_0)) - gn_warning_path = (cost.cost_type == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess)) - gn_warning_terminal = (cost.cost_type_e == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_e)) - if any([gn_warning_0, gn_warning_path, gn_warning_terminal]): - external_cost_types = [] - if gn_warning_0: - external_cost_types.append('cost_type_0') - if gn_warning_path: - external_cost_types.append('cost_type') - if gn_warning_terminal: - external_cost_types.append('cost_type_e') - print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not well defined!\n" - f"got cost_type EXTERNAL for {', '.join(external_cost_types)}, hessian_approx: 'GAUSS_NEWTON'.\n" - "With this setting, acados will proceed computing the exact Hessian for the cost term and no Hessian contribution from constraints and dynamics.\n" - "If the external cost is a linear least squares cost, this coincides with the Gauss-Newton Hessian.\n" - "Note: There is also the option to use the external cost module with a numerical Hessian approximation (see `ext_cost_num_hess`).\n" - "OR the option to provide a symbolic custom Hessian approximation (see `cost_expr_ext_cost_custom_hess`).\n") - # path + def _make_consistent_cost_path(self): + dims = self.dims + cost = self.cost + model = self.model + opts = self.solver_options + if opts.N_horizon == 0: + return + if cost.cost_type == 'AUTO': self.detect_cost_type(model, cost, dims, "path") @@ -342,7 +312,14 @@ def make_consistent(self, is_mocp_phase=False) -> None: if not is_empty(model.cost_expr_ext_cost_custom_hess): if model.cost_expr_ext_cost_custom_hess.shape != (dims.nx+dims.nu, dims.nx+dims.nu): raise ValueError('cost_expr_ext_cost_custom_hess should have shape (nx+nu, nx+nu).') - # terminal + + + def _make_consistent_cost_terminal(self): + dims = self.dims + cost = self.cost + model = self.model + opts = self.solver_options + if cost.cost_type_e == 'AUTO': self.detect_cost_type(model, cost, dims, "terminal") @@ -401,14 +378,15 @@ def make_consistent(self, is_mocp_phase=False) -> None: if model.cost_expr_ext_cost_custom_hess_e.shape != (dims.nx, dims.nx): raise ValueError('cost_expr_ext_cost_custom_hess_e should have shape (nx, nx).') - # cost integration - supports_cost_integration = lambda type : type in ['NONLINEAR_LS', 'CONVEX_OVER_NONLINEAR'] - if opts.cost_discretization == 'INTEGRATOR' and \ - any([not supports_cost_integration(cost) for cost in [cost.cost_type_0, cost.cost_type]]): - raise ValueError('cost_discretization == INTEGRATOR only works with cost in ["NONLINEAR_LS", "CONVEX_OVER_NONLINEAR"] costs.') - ## constraints - # initial + def _make_consistent_constraints_initial(self): + constraints = self.constraints + dims = self.dims + model = self.model + opts = self.solver_options + if opts.N_horizon == 0: + return + nbx_0 = constraints.idxbx_0.shape[0] if constraints.ubx_0.shape[0] != nbx_0 or constraints.lbx_0.shape[0] != nbx_0: raise ValueError('inconsistent dimension nbx_0, regarding idxbx_0, ubx_0, lbx_0.') @@ -447,7 +425,15 @@ def make_consistent(self, is_mocp_phase=False) -> None: else: dims.nr_0 = casadi_length(model.con_r_expr_0) - # path + + def _make_consistent_constraints_path(self): + constraints = self.constraints + dims = self.dims + model = self.model + opts = self.solver_options + if opts.N_horizon == 0: + return + nbx = constraints.idxbx.shape[0] if constraints.ubx.shape[0] != nbx or constraints.lbx.shape[0] != nbx: raise ValueError('inconsistent dimension nbx, regarding idxbx, ubx, lbx.') @@ -500,7 +486,11 @@ def make_consistent(self, is_mocp_phase=False) -> None: dims.nr = casadi_length(model.con_r_expr) - # terminal + def _make_consistent_constraints_terminal(self): + dims = self.dims + constraints = self.constraints + model = self.model + nbx_e = constraints.idxbx_e.shape[0] if constraints.ubx_e.shape[0] != nbx_e or constraints.lbx_e.shape[0] != nbx_e: raise ValueError('inconsistent dimension nbx_e, regarding idxbx_e, ubx_e, lbx_e.') @@ -536,7 +526,101 @@ def make_consistent(self, is_mocp_phase=False) -> None: else: dims.nr_e = casadi_length(model.con_r_expr_e) - # Slack dimensions + + def _make_consistent_slacks_initial(self): + constraints = self.constraints + dims = self.dims + opts = self.solver_options + cost = self.cost + if opts.N_horizon == 0: + return + + nh_0 = dims.nh_0 + nsbu = dims.nsbu + nsg = dims.nsg + ns = dims.ns + nsh_0 = constraints.idxsh_0.shape[0] + if nsh_0 > nh_0: + raise ValueError(f'inconsistent dimension nsh_0 = {nsh_0}. Is greater than nh_0 = {nh_0}.') + if any(constraints.idxsh_0 >= nh_0): + raise ValueError(f'idxsh_0 = {constraints.idxsh_0} contains value >= nh_0 = {nh_0}.') + if is_empty(constraints.lsh_0): + constraints.lsh_0 = np.zeros((nsh_0,)) + elif constraints.lsh_0.shape[0] != nsh_0: + raise ValueError('inconsistent dimension nsh_0, regarding idxsh_0, lsh_0.') + if is_empty(constraints.ush_0): + constraints.ush_0 = np.zeros((nsh_0,)) + elif constraints.ush_0.shape[0] != nsh_0: + raise ValueError('inconsistent dimension nsh_0, regarding idxsh_0, ush_0.') + dims.nsh_0 = nsh_0 + + nsphi_0 = constraints.idxsphi_0.shape[0] + if nsphi_0 > dims.nphi_0: + raise ValueError(f'inconsistent dimension nsphi_0 = {nsphi_0}. Is greater than nphi_0 = {dims.nphi_0}.') + if any(constraints.idxsphi_0 >= dims.nphi_0): + raise ValueError(f'idxsphi_0 = {constraints.idxsphi_0} contains value >= nphi_0 = {dims.nphi_0}.') + if is_empty(constraints.lsphi_0): + constraints.lsphi_0 = np.zeros((nsphi_0,)) + elif constraints.lsphi_0.shape[0] != nsphi_0: + raise ValueError('inconsistent dimension nsphi_0, regarding idxsphi_0, lsphi_0.') + if is_empty(constraints.usphi_0): + constraints.usphi_0 = np.zeros((nsphi_0,)) + elif constraints.usphi_0.shape[0] != nsphi_0: + raise ValueError('inconsistent dimension nsphi_0, regarding idxsphi_0, usphi_0.') + dims.nsphi_0 = nsphi_0 + + # Note: at stage 0 bounds on x are not slacked! + ns_0 = nsbu + nsg + nsphi_0 + nsh_0 # NOTE: nsbx not supported at stage 0 + + if cost.zl_0 is None and cost.zu_0 is None and cost.Zl_0 is None and cost.Zu_0 is None: + if ns_0 == 0: + cost.zl_0 = np.array([]) + cost.zu_0 = np.array([]) + cost.Zl_0 = np.array([]) + cost.Zu_0 = np.array([]) + elif ns_0 == ns: + cost.zl_0 = cost.zl + cost.zu_0 = cost.zu + cost.Zl_0 = cost.Zl + cost.Zu_0 = cost.Zu + print("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided.") + print("Using entries [zl, zu, Zl, Zu] at intial node for slack penalties.\n") + else: + raise ValueError("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided and cannot be inferred from other fields.\n") + + wrong_fields = [] + if cost.Zl_0.shape[0] != ns_0: + wrong_fields += ["Zl_0"] + dim = cost.Zl_0.shape[0] + elif cost.Zu_0.shape[0] != ns_0: + wrong_fields += ["Zu_0"] + dim = cost.Zu_0.shape[0] + elif cost.zl_0.shape[0] != ns_0: + wrong_fields += ["zl_0"] + dim = cost.zl_0.shape[0] + elif cost.zu_0.shape[0] != ns_0: + wrong_fields += ["zu_0"] + dim = cost.zu_0.shape[0] + + if wrong_fields != []: + raise ValueError(f'Inconsistent size for fields {", ".join(wrong_fields)}, with dimension {dim}, \n\t' + + f'Detected ns_0 = {ns_0} = nsbu + nsg + nsh_0 + nsphi_0.\n\t'\ + + f'With nsbu = {nsbu}, nsg = {nsg}, nsh_0 = {nsh_0}, nsphi_0 = {nsphi_0}.') + dims.ns_0 = ns_0 + + + def _make_consistent_slacks_path(self): + constraints = self.constraints + dims = self.dims + opts = self.solver_options + cost = self.cost + if opts.N_horizon == 0: + return + + nbx = dims.nbx + nbu = dims.nbu + nh = dims.nh + ng = dims.ng nsbx = constraints.idxsbx.shape[0] if nsbx > nbx: raise ValueError(f'inconsistent dimension nsbx = {nsbx}. Is greater than nbx = {nbx}.') @@ -633,77 +717,15 @@ def make_consistent(self, is_mocp_phase=False) -> None: + f'With nsbx = {nsbx}, nsbu = {nsbu}, nsg = {nsg}, nsh = {nsh}, nsphi = {nsphi}.') dims.ns = ns - # slack dimensions at initial node - nsh_0 = constraints.idxsh_0.shape[0] - if nsh_0 > nh_0: - raise ValueError(f'inconsistent dimension nsh_0 = {nsh_0}. Is greater than nh_0 = {nh_0}.') - if any(constraints.idxsh_0 >= nh_0): - raise ValueError(f'idxsh_0 = {constraints.idxsh_0} contains value >= nh_0 = {nh_0}.') - if is_empty(constraints.lsh_0): - constraints.lsh_0 = np.zeros((nsh_0,)) - elif constraints.lsh_0.shape[0] != nsh_0: - raise ValueError('inconsistent dimension nsh_0, regarding idxsh_0, lsh_0.') - if is_empty(constraints.ush_0): - constraints.ush_0 = np.zeros((nsh_0,)) - elif constraints.ush_0.shape[0] != nsh_0: - raise ValueError('inconsistent dimension nsh_0, regarding idxsh_0, ush_0.') - dims.nsh_0 = nsh_0 - - nsphi_0 = constraints.idxsphi_0.shape[0] - if nsphi_0 > dims.nphi_0: - raise ValueError(f'inconsistent dimension nsphi_0 = {nsphi_0}. Is greater than nphi_0 = {dims.nphi_0}.') - if any(constraints.idxsphi_0 >= dims.nphi_0): - raise ValueError(f'idxsphi_0 = {constraints.idxsphi_0} contains value >= nphi_0 = {dims.nphi_0}.') - if is_empty(constraints.lsphi_0): - constraints.lsphi_0 = np.zeros((nsphi_0,)) - elif constraints.lsphi_0.shape[0] != nsphi_0: - raise ValueError('inconsistent dimension nsphi_0, regarding idxsphi_0, lsphi_0.') - if is_empty(constraints.usphi_0): - constraints.usphi_0 = np.zeros((nsphi_0,)) - elif constraints.usphi_0.shape[0] != nsphi_0: - raise ValueError('inconsistent dimension nsphi_0, regarding idxsphi_0, usphi_0.') - dims.nsphi_0 = nsphi_0 - - # Note: at stage 0 bounds on x are not slacked! - ns_0 = nsbu + nsg + nsphi_0 + nsh_0 # NOTE: nsbx not supported at stage 0 - if cost.zl_0 is None and cost.zu_0 is None and cost.Zl_0 is None and cost.Zu_0 is None: - if ns_0 == 0: - cost.zl_0 = np.array([]) - cost.zu_0 = np.array([]) - cost.Zl_0 = np.array([]) - cost.Zu_0 = np.array([]) - elif ns_0 == ns: - cost.zl_0 = cost.zl - cost.zu_0 = cost.zu - cost.Zl_0 = cost.Zl - cost.Zu_0 = cost.Zu - print("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided.") - print("Using entries [zl, zu, Zl, Zu] at intial node for slack penalties.\n") - else: - raise ValueError("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided and cannot be inferred from other fields.\n") - - wrong_fields = [] - if cost.Zl_0.shape[0] != ns_0: - wrong_fields += ["Zl_0"] - dim = cost.Zl_0.shape[0] - elif cost.Zu_0.shape[0] != ns_0: - wrong_fields += ["Zu_0"] - dim = cost.Zu_0.shape[0] - elif cost.zl_0.shape[0] != ns_0: - wrong_fields += ["zl_0"] - dim = cost.zl_0.shape[0] - elif cost.zu_0.shape[0] != ns_0: - wrong_fields += ["zu_0"] - dim = cost.zu_0.shape[0] - - if wrong_fields != []: - raise ValueError(f'Inconsistent size for fields {", ".join(wrong_fields)}, with dimension {dim}, \n\t' - + f'Detected ns_0 = {ns_0} = nsbu + nsg + nsh_0 + nsphi_0.\n\t'\ - + f'With nsbu = {nsbu}, nsg = {nsg}, nsh_0 = {nsh_0}, nsphi_0 = {nsphi_0}.') - dims.ns_0 = ns_0 + def _make_consistent_slacks_terminal(self): + constraints = self.constraints + dims = self.dims + cost = self.cost - # slacks at terminal node + nbx_e = dims.nbx_e + nh_e = dims.nh_e + ng_e = dims.ng_e nsbx_e = constraints.idxsbx_e.shape[0] if nsbx_e > nbx_e: raise ValueError(f'inconsistent dimension nsbx_e = {nsbx_e}. Is greater than nbx_e = {nbx_e}.') @@ -787,24 +809,13 @@ def make_consistent(self, is_mocp_phase=False) -> None: dims.ns_e = ns_e - # check for ACADOS_INFTY - if opts.qp_solver not in ["PARTIAL_CONDENSING_HPIPM", "FULL_CONDENSING_HPIPM", "FULL_CONDENSING_DAQP"]: - # loop over all bound vectors - for field in ['lbx_0', 'ubx_0', 'lbx', 'ubx', 'lbx_e', 'ubx_e', 'lg', 'ug', 'lg_e', 'ug_e', 'lh', 'uh', 'lh_e', 'uh_e', 'lbu', 'ubu', 'lphi', 'uphi', 'lphi_e', 'uphi_e']: - bound = getattr(constraints, field) - if any(bound >= ACADOS_INFTY) or any(bound <= -ACADOS_INFTY): - raise ValueError(f"Field {field} contains values outside the interval (-ACADOS_INFTY, ACADOS_INFTY) with ACADOS_INFTY = {ACADOS_INFTY:.2e}. One-sided constraints are not supported by the chosen QP solver {opts.qp_solver}.") - # discretization - if opts.N_horizon is None and dims.N is None: - raise ValueError('N_horizon not provided.') - elif opts.N_horizon is None and dims.N is not None: - opts.N_horizon = dims.N - print("field AcadosOcpDims.N has been migrated to AcadosOcpOptions.N_horizon. setting AcadosOcpOptions.N_horizon = N. For future comppatibility, please use AcadosOcpOptions.N_horizon directly.") - elif opts.N_horizon is not None and dims.N is not None and opts.N_horizon != dims.N: - raise ValueError(f'Inconsistent dimension N, regarding N = {dims.N}, N_horizon = {opts.N_horizon}.') - else: - dims.N = opts.N_horizon + def _make_consistent_discretization(self): + opts = self.solver_options + if opts.N_horizon == 0: + opts.shooting_nodes = np.array([0.]) + opts.time_steps = np.array([]) + return if not isinstance(opts.tf, (float, int)): raise TypeError(f'Time horizon tf should be float provided, got tf = {opts.tf}.') @@ -841,11 +852,11 @@ def make_consistent(self, is_mocp_phase=False) -> None: raise ValueError(f'Inconsistent discretization: {opts.tf}' f' = tf != sum(opts.time_steps) = {tf}.') - # cost scaling - if opts.cost_scaling is None: - opts.cost_scaling = np.append(opts.time_steps, 1.0) - if opts.cost_scaling.shape[0] != opts.N_horizon + 1: - raise ValueError(f'cost_scaling should be of length N+1 = {opts.N_horizon+1}, got {opts.cost_scaling.shape[0]}.') + + def _make_consistent_simulation(self): + opts = self.solver_options + if opts.N_horizon == 0: + return # set integrator time automatically opts.Tsim = opts.time_steps[0] @@ -886,32 +897,141 @@ def make_consistent(self, is_mocp_phase=False) -> None: else: raise ValueError("Wrong value for sim_method_jac_reuse. Should be either int or array of ints of shape (N,).") + + def make_consistent(self, is_mocp_phase=False) -> None: + """ + Detect dimensions, perform sanity checks + """ + dims = self.dims + cost = self.cost + constraints = self.constraints + model = self.model + opts = self.solver_options + + model.make_consistent(dims) + self.name = model.name + + if opts.N_horizon is None and dims.N is None: + raise ValueError('N_horizon not provided.') + elif opts.N_horizon is None and dims.N is not None: + opts.N_horizon = dims.N + print("field AcadosOcpDims.N has been migrated to AcadosOcpOptions.N_horizon. setting AcadosOcpOptions.N_horizon = N. For future comppatibility, please use AcadosOcpOptions.N_horizon directly.") + elif opts.N_horizon is not None and dims.N is not None and opts.N_horizon != dims.N: + raise ValueError(f'Inconsistent dimension N, regarding N = {dims.N}, N_horizon = {opts.N_horizon}.') + else: + dims.N = opts.N_horizon + + # check if nx != nx_next + if not is_mocp_phase and dims.nx != dims.nx_next and opts.N_horizon > 1: + raise ValueError('nx_next should be equal to nx if more than one shooting interval is used.') + + # parameters + if self.parameter_values.shape[0] != dims.np: + raise ValueError('inconsistent dimension np, regarding model.p and parameter_values.' + \ + f'\nGot np = {dims.np}, self.parameter_values.shape = {self.parameter_values.shape[0]}\n') + + # p_global_values + if self.p_global_values.shape[0] != dims.np_global: + raise ValueError('inconsistent dimension np_global, regarding model.p_global and p_global_values.' + \ + f'\nGot np_global = {dims.np_global}, self.p_global_values.shape = {self.p_global_values.shape[0]}\n') + + ## cost + self._make_consistent_cost_initial() + self._make_consistent_cost_path() + self._make_consistent_cost_terminal() + + # GN check + gn_warning_0 = (opts.N_horizon > 0 and cost.cost_type_0 == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_0)) + gn_warning_path = (opts.N_horizon > 0 and cost.cost_type == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess)) + gn_warning_terminal = (cost.cost_type_e == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_e)) + if any([gn_warning_0, gn_warning_path, gn_warning_terminal]): + external_cost_types = [] + if gn_warning_0: + external_cost_types.append('cost_type_0') + if gn_warning_path: + external_cost_types.append('cost_type') + if gn_warning_terminal: + external_cost_types.append('cost_type_e') + print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not well defined!\n" + f"got cost_type EXTERNAL for {', '.join(external_cost_types)}, hessian_approx: 'GAUSS_NEWTON'.\n" + "With this setting, acados will proceed computing the exact Hessian for the cost term and no Hessian contribution from constraints and dynamics.\n" + "If the external cost is a linear least squares cost, this coincides with the Gauss-Newton Hessian.\n" + "Note: There is also the option to use the external cost module with a numerical Hessian approximation (see `ext_cost_num_hess`).\n" + "OR the option to provide a symbolic custom Hessian approximation (see `cost_expr_ext_cost_custom_hess`).\n") + + # cost integration + if opts.N_horizon > 0: + supports_cost_integration = lambda type : type in ['NONLINEAR_LS', 'CONVEX_OVER_NONLINEAR'] + if opts.cost_discretization == 'INTEGRATOR' and \ + any([not supports_cost_integration(cost) for cost in [cost.cost_type_0, cost.cost_type]]): + raise ValueError('cost_discretization == INTEGRATOR only works with cost in ["NONLINEAR_LS", "CONVEX_OVER_NONLINEAR"] costs.') + + ## constraints + self._make_consistent_constraints_initial() + self._make_consistent_constraints_path() + self._make_consistent_constraints_terminal() + + self._make_consistent_slacks_path() + self._make_consistent_slacks_initial() + self._make_consistent_slacks_terminal() + + # check for ACADOS_INFTY + if opts.qp_solver not in ["PARTIAL_CONDENSING_HPIPM", "FULL_CONDENSING_HPIPM", "FULL_CONDENSING_DAQP"]: + # loop over all bound vectors + if opts.N_horizon > 0: + fields_to_check = ['lbx_0', 'ubx_0', 'lbx', 'ubx', 'lbx_e', 'ubx_e', 'lg', 'ug', 'lg_e', 'ug_e', 'lh', 'uh', 'lh_e', 'uh_e', 'lbu', 'ubu', 'lphi', 'uphi', 'lphi_e', 'uphi_e'] + else: + fields_to_check = ['lbx_0', 'ubx_0', 'lbx_e', 'ubx_e', 'lg_e', 'ug_e', 'lh_e', 'uh_e''lphi_e', 'uphi_e'] + for field in fields_to_check: + bound = getattr(constraints, field) + if any(bound >= ACADOS_INFTY) or any(bound <= -ACADOS_INFTY): + raise ValueError(f"Field {field} contains values outside the interval (-ACADOS_INFTY, ACADOS_INFTY) with ACADOS_INFTY = {ACADOS_INFTY:.2e}. One-sided constraints are not supported by the chosen QP solver {opts.qp_solver}.") + + self._make_consistent_discretization() + + # cost scaling + if opts.cost_scaling is None: + opts.cost_scaling = np.append(opts.time_steps, 1.0) + if opts.cost_scaling.shape[0] != opts.N_horizon + 1: + raise ValueError(f'cost_scaling should be of length N+1 = {opts.N_horizon+1}, got {opts.cost_scaling.shape[0]}.') + + self._make_consistent_simulation() + + if opts.qp_solver == 'PARTIAL_CONDENSING_QPDUNES': + self.remove_x0_elimination() + # fixed hessian if opts.fixed_hess: if opts.hessian_approx == 'EXACT': raise ValueError('fixed_hess is not compatible with hessian_approx == EXACT.') - if cost.cost_type != "LINEAR_LS": + if cost.cost_type != "LINEAR_LS" and opts.N_horizon > 0: raise ValueError('fixed_hess is only compatible LINEAR_LS cost_type.') - if cost.cost_type_0 != "LINEAR_LS": + if cost.cost_type_0 != "LINEAR_LS" and opts.N_horizon > 0: raise ValueError('fixed_hess is only compatible LINEAR_LS cost_type_0.') if cost.cost_type_e != "LINEAR_LS": raise ValueError('fixed_hess is only compatible LINEAR_LS cost_type_e.') # solution sensitivities - bgp_type_constraint_pairs = [ - ("path", model.con_phi_expr), ("initial", model.con_phi_expr_0), ("terminal", model.con_phi_expr_e), - ("path", model.con_r_expr), ("initial", model.con_r_expr_0), ("terminal", model.con_r_expr_e) - ] - bgh_type_constraint_pairs = [ - ("path", model.con_h_expr), ("initial", model.con_h_expr_0), ("terminal", model.con_h_expr_e), - ] + if opts.N_horizon > 0: + bgp_type_constraint_pairs = [ + ("path", model.con_phi_expr), ("initial", model.con_phi_expr_0), ("terminal", model.con_phi_expr_e), + ("path", model.con_r_expr), ("initial", model.con_r_expr_0), ("terminal", model.con_r_expr_e) + ] + cost_types_to_check = [cost.cost_type, cost.cost_type_0, cost.cost_type_e] + suffix = f", got cost_types {cost.cost_type_0, cost.cost_type, cost.cost_type_e}." + else: + bgp_type_constraint_pairs = [ + ("terminal", model.con_phi_expr_e), ("terminal", model.con_r_expr_e) + ] + cost_types_to_check = [cost.cost_type_e] + suffix = f", got cost_type_e {cost.cost_type_e}." if opts.with_solution_sens_wrt_params: if dims.np_global == 0: raise ValueError('with_solution_sens_wrt_params is only compatible if global parameters `p_global` are provided. Sensitivities wrt parameters have been refactored to use p_global instead of p in https://github.com/acados/acados/pull/1316. Got emty p_global.') - if any([cost_type not in ["EXTERNAL", "LINEAR_LS"] for cost_type in [cost.cost_type, cost.cost_type_0, cost.cost_type_e]]): - raise ValueError(f'with_solution_sens_wrt_params is only compatible with EXTERNAL and LINEAR_LS cost_type, got cost_types {cost.cost_type_0, cost.cost_type, cost.cost_type_e}.') - if opts.integrator_type != "DISCRETE": + if any([cost_type not in ["EXTERNAL", "LINEAR_LS"] for cost_type in cost_types_to_check]): + raise ValueError('with_solution_sens_wrt_params is only compatible with EXTERNAL and LINEAR_LS cost_type' + suffix) + if opts.N_horizon > 0 and opts.integrator_type != "DISCRETE": raise NotImplementedError('with_solution_sens_wrt_params is only compatible with DISCRETE dynamics.') for horizon_type, constraint in bgp_type_constraint_pairs: if constraint is not None and any(ca.which_depends(constraint, model.p_global)): @@ -920,19 +1040,21 @@ def make_consistent(self, is_mocp_phase=False) -> None: if opts.with_value_sens_wrt_params: if dims.np_global == 0: raise ValueError('with_value_sens_wrt_params is only compatible if global parameters `p_global` are provided. Sensitivities wrt parameters have been refactored to use p_global instead of p in https://github.com/acados/acados/pull/1316. Got emty p_global.') - if any([cost_type not in ["EXTERNAL", "LINEAR_LS"] for cost_type in [cost.cost_type, cost.cost_type_0, cost.cost_type_e]]): - raise ValueError('with_value_sens_wrt_params is only compatible with EXTERNAL cost_type.') - if opts.integrator_type != "DISCRETE": + if any([cost_type not in ["EXTERNAL", "LINEAR_LS"] for cost_type in cost_types_to_check]): + raise ValueError('with_value_sens_wrt_params is only compatible with EXTERNAL cost_type' + suffix) + if opts.N_horizon > 0 and opts.integrator_type != "DISCRETE": raise NotImplementedError('with_value_sens_wrt_params is only compatible with DISCRETE dynamics.') for horizon_type, constraint in bgp_type_constraint_pairs: if constraint is not None and any(ca.which_depends(constraint, model.p_global)): raise NotImplementedError(f"with_value_sens_wrt_params is not supported for BGP constraints that depend on p_global. Got dependency on p_global for {horizon_type} constraint.") + if opts.tau_min > 0 and "HPIPM" not in opts.qp_solver: + raise ValueError('tau_min > 0 is only compatible with HPIPM.') + if opts.qp_solver_cond_N is None: opts.qp_solver_cond_N = opts.N_horizon - - if opts.tau_min > 0 and not "HPIPM" in opts.qp_solver: - raise ValueError('tau_min > 0 is only compatible with HPIPM.') + if opts.qp_solver_cond_N > opts.N_horizon: + raise ValueError("qp_solver_cond_N > N_horizon is not supported.") if opts.qp_solver_cond_block_size is not None: if sum(opts.qp_solver_cond_block_size) != opts.N_horizon: @@ -941,6 +1063,8 @@ def make_consistent(self, is_mocp_phase=False) -> None: raise ValueError(f'qp_solver_cond_block_size = {opts.qp_solver_cond_block_size} should have length qp_solver_cond_N+1 = {opts.qp_solver_cond_N+1}.') if opts.nlp_solver_type == "DDP": + if opts.N_horizon == 0: + raise ValueError("DDP solver only supported for N_horizon > 0.") if opts.qp_solver != "PARTIAL_CONDENSING_HPIPM" or opts.qp_solver_cond_N != opts.N_horizon: raise ValueError(f'DDP solver only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N, got qp solver {opts.qp_solver} and qp_solver_cond_N {opts.qp_solver_cond_N}, N {opts.N_horizon}.') if any([dims.nbu, dims.nbx, dims.ng, dims.nh, dims.nphi]): @@ -981,7 +1105,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: opts.globalization_full_step_dual = 0 # AS-RTI - if opts.as_rti_level in [1, 2] and any([cost.cost_type.endswith('LINEAR_LS'), cost.cost_type_0.endswith('LINEAR_LS'), cost.cost_type_e.endswith('LINEAR_LS')]): + if opts.as_rti_level in [1, 2] and any([cost_type.endswith("LINEAR_LS") for cost_type in cost_types_to_check]): raise NotImplementedError('as_rti_level in [1, 2] not supported for LINEAR_LS and NONLINEAR_LS cost type.') # sanity check for Funnel globalization and SQP @@ -989,7 +1113,7 @@ def make_consistent(self, is_mocp_phase=False) -> None: raise NotImplementedError('FUNNEL_L1PEN_LINESEARCH only supports SQP.') # termination - if opts.nlp_solver_tol_min_step_norm == None: + if opts.nlp_solver_tol_min_step_norm is None: if ddp_with_merit_or_funnel: opts.nlp_solver_tol_min_step_norm = 1e-12 else: @@ -997,6 +1121,8 @@ def make_consistent(self, is_mocp_phase=False) -> None: # zoRO if self.zoro_description is not None: + if opts.N_horizon == 0: + raise ValueError('zoRO only supported for N_horizon > 0.') if not isinstance(self.zoro_description, ZoroDescription): raise TypeError('zoro_description should be of type ZoroDescription or None') else: @@ -1011,11 +1137,13 @@ def make_consistent(self, is_mocp_phase=False) -> None: def _get_external_function_header_templates(self, ) -> list: dims = self.dims name = self.model.name + opts = self.solver_options template_list = [] # dynamics - model_dir = os.path.join(self.code_export_directory, f'{name}_model') - template_list.append(('model.in.h', f'{name}_model.h', model_dir)) + if opts.N_horizon > 0: + model_dir = os.path.join(self.code_export_directory, f'{name}_model') + template_list.append(('model.in.h', f'{name}_model.h', model_dir)) # constraints if any(np.array([dims.nh, dims.nh_e, dims.nh_0, dims.nphi, dims.nphi_e, dims.nphi_0]) > 0): constraints_dir = os.path.join(self.code_export_directory, f'{name}_constraints') @@ -1036,6 +1164,7 @@ def __get_template_list(self, cmake_builder=None) -> list: (input_filename, output_filname, output_directory) """ name = self.model.name + opts = self.solver_options template_list = [] template_list.append(('main.in.c', f'main_{name}.c')) @@ -1048,7 +1177,7 @@ def __get_template_list(self, cmake_builder=None) -> list: template_list.append(('Makefile.in', 'Makefile')) # sim - if self.solver_options.integrator_type != 'DISCRETE': + if opts.N_horizon > 0 and self.solver_options.integrator_type != 'DISCRETE': template_list.append(('acados_sim_solver.in.c', f'acados_sim_solver_{name}.c')) template_list.append(('acados_sim_solver.in.h', f'acados_sim_solver_{name}.h')) template_list.append(('main_sim.in.c', f'main_sim_{name}.c')) @@ -1156,6 +1285,50 @@ def _setup_code_generation_context(self, context: GenerateContext, ignore_initia model = self.model constraints = self.constraints + opts = self.solver_options + + check_casadi_version() + self._setup_code_generation_context_dynamics(context) + + if opts.N_horizon > 0: + if ignore_initial and ignore_terminal: + stage_type_indices = [1] + elif ignore_initial: + stage_type_indices = [1, 2] + elif ignore_terminal: + stage_type_indices = [0, 1] + else: + stage_type_indices = [0, 1, 2] + else: + stage_type_indices = [2] + + stage_types = [val for i, val in enumerate(['initial', 'path', 'terminal']) if i in stage_type_indices] + nhs = [val for i, val in enumerate(['nh_0', 'nh', 'nh_e']) if i in stage_type_indices] + nphis = [val for i, val in enumerate(['nphi_0', 'nphi', 'nphi_e']) if i in stage_type_indices] + cost_types = [val for i, val in enumerate(['cost_type_0', 'cost_type', 'cost_type_e']) if i in stage_type_indices] + + for attr_nh, attr_nphi, stage_type in zip(nhs, nphis, stage_types): + if getattr(self.dims, attr_nh) > 0 or getattr(self.dims, attr_nphi) > 0: + generate_c_code_constraint(context, model, constraints, stage_type) + + for attr, stage_type in zip(cost_types, stage_types): + if getattr(self.cost, attr) == 'NONLINEAR_LS': + generate_c_code_nls_cost(context, model, stage_type) + elif getattr(self.cost, attr) == 'CONVEX_OVER_NONLINEAR': + generate_c_code_conl_cost(context, model, stage_type) + elif getattr(self.cost, attr) == 'EXTERNAL': + generate_c_code_external_cost(context, model, stage_type) + # TODO: generic + + return context + + + def _setup_code_generation_context_dynamics(self, context: GenerateContext): + opts = self.solver_options + model = self.model + + if opts.N_horizon == 0: + return code_gen_opts = context.opts @@ -1164,7 +1337,6 @@ def _setup_code_generation_context(self, context: GenerateContext, ignore_initia if not os.path.exists(model_dir): os.makedirs(model_dir) - check_casadi_version() if self.model.dyn_ext_fun_type == 'casadi': if self.solver_options.integrator_type == 'ERK': generate_c_code_explicit_ode(context, model, model_dir) @@ -1186,34 +1358,6 @@ def _setup_code_generation_context(self, context: GenerateContext, ignore_initia shutil.copyfile(model.dyn_generic_source, target_location) context.add_external_function_file(model.dyn_generic_source, target_dir) - if ignore_initial and ignore_terminal: - stage_type_indices = [1] - elif ignore_initial: - stage_type_indices = [1, 2] - elif ignore_terminal: - stage_type_indices = [0, 1] - else: - stage_type_indices = [0, 1, 2] - - stage_types = [val for i, val in enumerate(['initial', 'path', 'terminal']) if i in stage_type_indices] - nhs = [val for i, val in enumerate(['nh_0', 'nh', 'nh_e']) if i in stage_type_indices] - nphis = [val for i, val in enumerate(['nphi_0', 'nphi', 'nphi_e']) if i in stage_type_indices] - cost_types = [val for i, val in enumerate(['cost_type_0', 'cost_type', 'cost_type_e']) if i in stage_type_indices] - - for attr_nh, attr_nphi, stage_type in zip(nhs, nphis, stage_types): - if getattr(self.dims, attr_nh) > 0 or getattr(self.dims, attr_nphi) > 0: - generate_c_code_constraint(context, model, constraints, stage_type) - - for attr, stage_type in zip(cost_types, stage_types): - if getattr(self.cost, attr) == 'NONLINEAR_LS': - generate_c_code_nls_cost(context, model, stage_type) - elif getattr(self.cost, attr) == 'CONVEX_OVER_NONLINEAR': - generate_c_code_conl_cost(context, model, stage_type) - elif getattr(self.cost, attr) == 'EXTERNAL': - generate_c_code_external_cost(context, model, stage_type) - # TODO: generic - - return context def remove_x0_elimination(self) -> None: """Remove the elimination of x0 from the constraints, bounds on x0 are handled as general bounds on x.""" diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 5753216d20..8855b42355 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -1118,7 +1118,7 @@ def tf(self): def N_horizon(self): """ Number of shooting intervals. - Type: int > 0 + Type: int >= 0 Default: :code:`None` """ return self.__N_horizon @@ -1384,10 +1384,10 @@ def tf(self, tf): @N_horizon.setter def N_horizon(self, N_horizon): - if isinstance(N_horizon, int) and N_horizon > 0: + if isinstance(N_horizon, int) and N_horizon >= 0: self.__N_horizon = N_horizon else: - raise ValueError('Invalid N_horizon value, expected positive integer.') + raise ValueError('Invalid N_horizon value, expected non-negative integer.') @time_steps.setter def time_steps(self, time_steps): diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 8493f43b16..eb8f585b5d 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -121,9 +121,6 @@ def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: else: detect_gnsf_structure(acados_ocp) - if acados_ocp.solver_options.qp_solver == 'PARTIAL_CONDENSING_QPDUNES': - acados_ocp.remove_x0_elimination() - if acados_ocp.solver_options.qp_solver in ['FULL_CONDENSING_QPOASES', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP']: print(f"NOTE: The selected QP solver {acados_ocp.solver_options.qp_solver} does not support one-sided constraints yet.") diff --git a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt index 64923291dd..d9c4d56609 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt @@ -108,12 +108,15 @@ endif() # object target names +{%- if problem_class == "SIM" or solver_options.N_horizon > 0 %} set(MODEL_OBJ model_{{ model.name }}) +{%- endif %} set(OCP_OBJ ocp_{{ model.name }}) {%- if solver_options.integrator_type != "DISCRETE" %} set(SIM_OBJ sim_{{ model.name }}) {%- endif %} +{%- if problem_class == "SIM" or solver_options.N_horizon > 0 %} # model set(MODEL_SRC {%- for filename in external_function_files_model %} @@ -121,7 +124,7 @@ set(MODEL_SRC {%- endfor %} ) add_library(${MODEL_OBJ} OBJECT ${MODEL_SRC} ) - +{%- endif %} {% if problem_class != "SIM" %} # optimal control problem - mostly CasADi exports @@ -223,7 +226,11 @@ endif() # bundled_shared_lib if(${BUILD_ACADOS_SOLVER_LIB}) set(LIB_ACADOS_SOLVER acados_solver_{{ model.name }}) - add_library(${LIB_ACADOS_SOLVER} SHARED $ $ + add_library(${LIB_ACADOS_SOLVER} SHARED + {%- if solver_options.N_horizon > 0 %} + $ + {%- endif %} + $ {%- if solver_options.integrator_type != "DISCRETE" %} $ {%- endif -%} @@ -234,7 +241,11 @@ endif(${BUILD_ACADOS_SOLVER_LIB}) # ocp_shared_lib if(${BUILD_ACADOS_OCP_SOLVER_LIB}) set(LIB_ACADOS_OCP_SOLVER acados_ocp_solver_{{ model.name }}) - add_library(${LIB_ACADOS_OCP_SOLVER} SHARED $ $) + add_library(${LIB_ACADOS_OCP_SOLVER} SHARED + {%- if solver_options.N_horizon > 0 %} + $ + {%- endif %} + $) # Specify libraries or flags to use when linking a given target and/or its dependents. target_link_libraries(${LIB_ACADOS_OCP_SOLVER} PRIVATE ${EXTERNAL_LIB}) target_link_directories(${LIB_ACADOS_OCP_SOLVER} PRIVATE ${EXTERNAL_DIR}) @@ -243,7 +254,11 @@ endif(${BUILD_ACADOS_OCP_SOLVER_LIB}) # example if(${BUILD_EXAMPLE}) - add_executable(${EX_EXE} ${EX_SRC} $ $ + add_executable(${EX_EXE} ${EX_SRC} + {%- if solver_options.N_horizon > 0 %} + $ + {%- endif %} + $ {%- if solver_options.integrator_type != "DISCRETE" %} $ {%- endif -%} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c index 676d3970dc..f1ce6e7f33 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c @@ -43,7 +43,9 @@ {%- endif %} // example specific +{% if solver_options.N_horizon > 0 %} #include "{{ model.name }}_model/{{ model.name }}_model.h" +{%- endif %} {% if dims.n_global_data > 0 %} #include "{{ name }}_p_global_precompute_fun.h" @@ -142,6 +144,10 @@ int {{ model.name }}_acados_create({{ model.name }}_solver_capsule* capsule) int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule* capsule, int N, double* new_time_steps) { +{% if solver_options.N_horizon == 0 %} + printf("\nacados_update_time_steps() not implemented, since N_horizon = 0!\n\n"); + exit(1); +{% else %} if (N != capsule->nlp_solver_plan->N) { fprintf(stderr, "{{ model.name }}_acados_update_time_steps: given number of time steps (= %d) " \ "differs from the currently allocated number of " \ @@ -161,6 +167,7 @@ int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule* c ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &new_time_steps[i]); } return 0; +{% endif %} } /** @@ -179,9 +186,11 @@ void {{ model.name }}_acados_create_set_plan(ocp_nlp_plan_t* nlp_solver_plan, co nlp_solver_plan->ocp_qp_solver_plan.qp_solver = {{ solver_options.qp_solver }}; nlp_solver_plan->relaxed_ocp_qp_solver_plan.qp_solver = {{ solver_options.qp_solver }}; + {%- if solver_options.N_horizon > 0 %} nlp_solver_plan->nlp_cost[0] = {{ cost.cost_type_0 }}; for (int i = 1; i < N; i++) nlp_solver_plan->nlp_cost[i] = {{ cost.cost_type }}; + {%- endif %} nlp_solver_plan->nlp_cost[N] = {{ cost.cost_type_e }}; @@ -271,7 +280,9 @@ static ocp_nlp_dims* {{ model.name }}_acados_create_setup_dimensions({{ model.na nbx[0] = NBX0; nsbx[0] = 0; ns[0] = NS0; + {% if solver_options.N_horizon > 0 %} nbxe[0] = {{ dims.nbxe_0 }}; + {% endif %} ny[0] = NY0; nh[0] = NH0; nsh[0] = NSH0; @@ -321,12 +332,12 @@ static ocp_nlp_dims* {{ model.name }}_acados_create_setup_dimensions({{ model.na ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsg", &nsg[i]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]); } - -{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" or cost.cost_type_0 == "CONVEX_OVER_NONLINEAR"%} +{%- if solver_options.N_horizon > 0 %} +{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" or cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]); {%- endif %} -{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" or cost.cost_type == "CONVEX_OVER_NONLINEAR"%} +{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" or cost.cost_type == "CONVEX_OVER_NONLINEAR" %} for (int i = 1; i < N; i++) ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]); {%- endif %} @@ -351,6 +362,7 @@ static ocp_nlp_dims* {{ model.name }}_acados_create_setup_dimensions({{ model.na ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsphi", &nsphi[i]); {%- endif %} } +{%- endif %}{# solver_options.N_horizon > 0 #} {%- if constraints.constr_type_e == "BGH" %} ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]); @@ -364,6 +376,7 @@ static ocp_nlp_dims* {{ model.name }}_acados_create_setup_dimensions({{ model.na ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]); {%- endif %} +{%- if solver_options.N_horizon > 0 %} {%- if solver_options.integrator_type == "GNSF" -%} // GNSF specific dimensions int gnsf_nx1 = {{ dims.gnsf_nx1 }}; @@ -386,7 +399,7 @@ static ocp_nlp_dims* {{ model.name }}_acados_create_setup_dimensions({{ model.na for (int i = 0; i < N; i++) ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "ny", &ny[i]); {%- endif %} - +{%- endif %}{# solver_options.N_horizon > 0 #} free(intNp1mem); return nlp_dims; @@ -453,7 +466,7 @@ void {{ model.name }}_acados_create_setup_functions({{ model.name }}_solver_caps {%- endif %} ext_fun_opts.external_workspace = true; - +{%- if solver_options.N_horizon > 0 %} {%- if constraints.constr_type_0 == "BGH" and dims.nh_0 > 0 %} MAP_CASADI_FNC(nl_constr_h_0_fun_jac, {{ model.name }}_constr_h_0_fun_jac_uxt_zt); MAP_CASADI_FNC(nl_constr_h_0_fun, {{ model.name }}_constr_h_0_fun); @@ -811,6 +824,7 @@ void {{ model.name }}_acados_create_setup_functions({{ model.name }}_solver_caps } {%- endif %} {%- endif %} +{%- endif %}{# solver_options.N_horizon > 0 #} {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} @@ -886,7 +900,7 @@ void {{ model.name }}_acados_create_setup_functions({{ model.name }}_solver_caps /** - * Internal function for {{ model.name }}_acados_create: step 4 + * Internal function for {{ model.name }}_acados_create: step 5 */ void {{ model.name }}_acados_create_set_default_parameters({{ model.name }}_solver_capsule* capsule) { @@ -947,14 +961,16 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu ocp_nlp_out * nlp_out = capsule->nlp_out; // set up time_steps and cost_scaling - {%- set all_equal = true -%} - {%- set val = solver_options.time_steps[0] %} - {%- for j in range(start=1, end=solver_options.N_horizon) %} - {%- if val != solver_options.time_steps[j] %} - {%- set_global all_equal = false %} - {%- break %} - {%- endif %} - {%- endfor %} + {%- if solver_options.N_horizon > 0 -%} + {%- set all_equal = true -%} + {%- set val = solver_options.time_steps[0] %} + {%- for j in range(start=1, end=solver_options.N_horizon) %} + {%- if val != solver_options.time_steps[j] %} + {%- set_global all_equal = false %} + {%- break %} + {%- endif %} + {%- endfor %} + {%- endif %} if (new_time_steps) { @@ -964,6 +980,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu else { // set time_steps + {%- if solver_options.N_horizon > 0 %} {% if all_equal == true -%}{# all time_steps are identical #} double time_step = {{ solver_options.time_steps[0] }}; for (int i = 0; i < N; i++) @@ -981,6 +998,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu } free(time_steps); {%- endif %} + {%- endif %}{# solver_options.N_horizon > 0 #} // set cost scaling double* cost_scaling = malloc((N+1)*sizeof(double)); {%- for j in range(end=solver_options.N_horizon+1) %} @@ -994,6 +1012,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu } +{% if solver_options.N_horizon > 0 %} /**** Dynamics ****/ for (int i = 0; i < N; i++) { @@ -1226,6 +1245,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endif %}{# LINEAR LS #} {%- endif %}{# ny != 0 #} +{%- endif %}{# solver_options.N_horizon > 0 #} {%- if dims.ny_e != 0 %} @@ -1270,6 +1290,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} {%- endif %}{# ny_e != 0 #} +{%- if solver_options.N_horizon > 0 %} {%- if cost.cost_type_0 == "NONLINEAR_LS" %} ocp_nlp_cost_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun", &capsule->cost_y_0_fun); ocp_nlp_cost_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun_jac", &capsule->cost_y_0_fun_jac_ut_xt); @@ -1320,6 +1341,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {% endif %} } {%- endif %} +{%- endif %}{# solver_options.N_horizon > 0 #} {%- if cost.cost_type_e == "NONLINEAR_LS" %} ocp_nlp_cost_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun); @@ -1345,6 +1367,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu {%- endif %} +{% if solver_options.N_horizon > 0 %} {% if dims.ns_0 > 0 %} // slacks initial double* zlu0_mem = calloc(4*NS0, sizeof(double)); @@ -1427,6 +1450,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu } free(zlumem); {%- endif %} +{%- endif %}{# solver_options.N_horizon > 0 #} {% if dims.ns_e > 0 %} // slacks terminal @@ -1471,6 +1495,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu /**** Constraints ****/ // bounds for initial stage +{%- if solver_options.N_horizon > 0 %} {%- if dims.nbx_0 > 0 %} // x0 int* idxbx0 = malloc(NBX0 * sizeof(int)); @@ -1937,6 +1962,7 @@ void {{ model.name }}_acados_setup_nlp_in({{ model.name }}_solver_capsule* capsu } free(luphi); {%- endif %} +{%- endif %}{# solver_options.N_horizon > 0 #} /* terminal constraints */ {% if dims.nbx_e > 0 %} @@ -2249,6 +2275,7 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps } {%- endif %} +{%- if solver_options.N_horizon > 0 %} {%- if solver_options.integrator_type != "DISCRETE" %} // set collocation type (relevant for implicit integrators) @@ -2351,6 +2378,7 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps } {%- endif %} {%- endif %}{# solver_options.integrator_type != "DISCRETE" #} +{%- endif %}{# solver_options.N_horizon > 0 #} {%- if solver_options.nlp_solver_warm_start_first_qp %} int nlp_solver_warm_start_first_qp = {{ solver_options.nlp_solver_warm_start_first_qp }}; @@ -2378,7 +2406,7 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps {%- endif %} ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N); - {%- if solver_options.qp_solver_cond_block_size -%} + {%- if solver_options.qp_solver_cond_block_size and solver_options.N_horizon > 0 -%} int* qp_solver_cond_block_size = malloc((qp_solver_cond_N+1) * sizeof(int)); {%- for i in range(end=solver_options.qp_solver_cond_N+1) %} qp_solver_cond_block_size[{{ i }}] = {{ solver_options.qp_solver_cond_block_size[i] }}; @@ -2548,7 +2576,7 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps {% endif %} int ext_cost_num_hess = {{ solver_options.ext_cost_num_hess }}; -{%- if cost.cost_type == "EXTERNAL" %} +{%- if cost.cost_type == "EXTERNAL" and solver_options.N_horizon > 0 %} for (int i = 0; i < N; i++) { ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "cost_numerical_hessian", &ext_cost_num_hess); @@ -2574,7 +2602,7 @@ void {{ model.name }}_acados_set_nlp_out({{ model.name }}_solver_capsule* capsul // initialize primal solution double* xu0 = calloc(NX+NU, sizeof(double)); double* x0 = xu0; -{% if dims.nbx_0 == dims.nx %} +{% if dims.nbx_0 == dims.nx and solver_options.N_horizon > 0 %} // initialize with x0 {%- for item in constraints.lbx_0 %} {%- if item != 0 %} @@ -2675,7 +2703,10 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c */ int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule* capsule, int qp_solver_cond_N) { -{%- if solver_options.qp_solver is starting_with("PARTIAL_CONDENSING") %} +{%- if solver_options.N_horizon == 0 %} + printf("\nacados_update_qp_solver_cond_N() not implemented, since N_horizon = 0!\n\n"); + exit(1); +{%- elif solver_options.qp_solver is starting_with("PARTIAL_CONDENSING") %} // 1) destroy solver ocp_nlp_solver_destroy(capsule->nlp_solver); @@ -2695,7 +2726,6 @@ int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_caps {%- else %} printf("\nacados_update_qp_solver_cond_N() not implemented, since no partial condensing solver is used!\n\n"); exit(1); - return -1; {%- endif %} } @@ -3000,6 +3030,7 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) /* free external function */ // dynamics +{%- if solver_options.N_horizon > 0 %} {%- if solver_options.integrator_type == "IRK" %} for (int i = 0; i < N; i++) { @@ -3160,6 +3191,7 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) free(capsule->ext_cost_grad_p); {%- endif %} {%- endif %} +{%- endif %}{# if solver_options.N_horizon > 0 #} {%- if cost.cost_type_e == "NONLINEAR_LS" %} external_function_external_param_casadi_free(&capsule->cost_y_e_fun); external_function_external_param_casadi_free(&capsule->cost_y_e_fun_jac_ut_xt); @@ -3182,6 +3214,7 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) {%- endif %} // constraints +{%- if solver_options.N_horizon > 0 %} {%- if constraints.constr_type == "BGH" and dims.nh > 0 %} for (int i = 0; i < N-1; i++) { @@ -3229,6 +3262,7 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) external_function_external_param_casadi_free(&capsule->phi_0_constraint_fun); external_function_external_param_casadi_free(&capsule->phi_0_constraint_fun_jac_hess); {%- endif %} +{%- endif %}{# if solver_options.N_horizon > 0 #} {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} external_function_external_param_casadi_free(&capsule->nl_constr_h_e_fun_jac); From bcf01c1d110c3235508465d79fd37935d032f02a Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 15 Apr 2025 13:31:51 +0200 Subject: [PATCH 08/24] Modify `AcadosOcpBatchSolver` to use flexible batch size (#1460) --- .../ocp/minimal_example_batch_ocp_solver.py | 22 ++- ...ch_adjoint_solution_sensitivity_example.py | 4 +- .../acados_ocp_batch_solver.py | 135 ++++++++++-------- 3 files changed, 96 insertions(+), 65 deletions(-) diff --git a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_batch_ocp_solver.py b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_batch_ocp_solver.py index e23efe7170..adb708d83c 100644 --- a/examples/acados_python/pendulum_on_cart/ocp/minimal_example_batch_ocp_solver.py +++ b/examples/acados_python/pendulum_on_cart/ocp/minimal_example_batch_ocp_solver.py @@ -122,8 +122,10 @@ def main_batch(Xinit, simU, tol, num_threads_in_batch_solve=1): N_batch = Xinit.shape[0] - 1 ocp = setup_ocp(tol) - batch_solver = AcadosOcpBatchSolver(ocp, N_batch, num_threads_in_batch_solve=num_threads_in_batch_solve, verbose=False) - + N_batch_max = N_batch + 3 # to test with more than N_batch + + batch_solver = AcadosOcpBatchSolver(ocp, N_batch_max, num_threads_in_batch_solve=num_threads_in_batch_solve, verbose=False) + assert batch_solver.num_threads_in_batch_solve == num_threads_in_batch_solve batch_solver.num_threads_in_batch_solve = 1337 assert batch_solver.num_threads_in_batch_solve == 1337 @@ -144,16 +146,24 @@ def main_batch(Xinit, simU, tol, num_threads_in_batch_solve=1): # solve t0 = time.time() - batch_solver.solve() + batch_solver.solve(N_batch) t_elapsed = 1e3 * (time.time() - t0) print(f"main_batch: with {num_threads_in_batch_solve} threads, solve: {t_elapsed:.3f}ms") - U_batch = batch_solver.get_flat("u") + U_batch = batch_solver.get_flat("u", N_batch) for n in range(N_batch): - if not np.linalg.norm(U_batch[n, :ocp.dims.nu] -simU[n]) < tol*10: - raise Exception(f"solution should match sequential call up to {tol*10} got error {np.linalg.norm(u-simU[n])} for {n}th batch solve") + if not np.linalg.norm(U_batch[n, :ocp.dims.nu] - simU[n]) < tol*10: + raise Exception(f"solution should match sequential call up to {tol*10} got error {np.linalg.norm(U_batch[n, :ocp.dims.nu] - simU[n])} for {n}th batch solve") + + # test N_batch_max is respected + try: + batch_solver.get_flat("x", N_batch_max+1) + except Exception as e: + print(f"error raised correctly: {e}") + else: + raise Exception("using n_batch > N_batch_max should raise an error") if __name__ == "__main__": diff --git a/examples/acados_python/solution_sensitivities_convex_example/batch_adjoint_solution_sensitivity_example.py b/examples/acados_python/solution_sensitivities_convex_example/batch_adjoint_solution_sensitivity_example.py index e18d60c615..fe1dbc925a 100644 --- a/examples/acados_python/solution_sensitivities_convex_example/batch_adjoint_solution_sensitivity_example.py +++ b/examples/acados_python/solution_sensitivities_convex_example/batch_adjoint_solution_sensitivity_example.py @@ -99,7 +99,7 @@ def main_batch(Xinit, simU, param_vals, adjoints_ref, tol, num_threads_in_batch_ # solve t0 = time.time() - batch_solver.solve() + batch_solver.solve(N_batch) t_elapsed = 1e3 * (time.time() - t0) print(f"main_batch: with {num_threads_in_batch_solve} threads, solve: {t_elapsed:.3f} ms") @@ -111,7 +111,7 @@ def main_batch(Xinit, simU, param_vals, adjoints_ref, tol, num_threads_in_batch_ raise Exception(f"solution should match sequential call up to {tol} got error {diff} for {n}th batch solve") # actually not needed for convex problem but we want to test it - batch_solver.setup_qp_matrices_and_factorize() + batch_solver.setup_qp_matrices_and_factorize(N_batch) # eval adjoint t0 = time.time() diff --git a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py index e4855ff3f9..6f75d164e2 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py @@ -42,8 +42,8 @@ class AcadosOcpBatchSolver(): Batch OCP solver for parallel solves. :param ocp: type :py:class:`~acados_template.acados_ocp.AcadosOcp` - :param N_batch: batch size, positive integer :param num_threads_in_batch_solve: number of threads used for parallelizing the batch methods. Default: 1 + :param N_batch_max: maximum batch size, positive integer :param json_file: Default: 'acados_ocp.json' :param build: Flag indicating whether solver should be (re)compiled. If False an attempt is made to load an already compiled shared library for the solver. Default: True :param generate: Flag indicating whether problem functions should be code generated. Default: True @@ -52,10 +52,13 @@ class AcadosOcpBatchSolver(): __ocp_solvers : List[AcadosOcpSolver] - def __init__(self, ocp: AcadosOcp, N_batch: int, num_threads_in_batch_solve: Union[int, None] = None, json_file: str = 'acados_ocp.json', build: bool = True, generate: bool = True, verbose: bool=True): + def __init__(self, ocp: AcadosOcp, N_batch_max: int, + num_threads_in_batch_solve: Union[int, None] = None, + json_file: str = 'acados_ocp.json', + build: bool = True, generate: bool = True, verbose: bool=True): - if not isinstance(N_batch, int) or N_batch <= 0: - raise ValueError("AcadosOcpBatchSolver: argument N_batch should be a positive integer.") + if not isinstance(N_batch_max, int) or N_batch_max <= 0: + raise ValueError("AcadosOcpBatchSolver: argument N_batch_max should be a positive integer.") if num_threads_in_batch_solve is None: num_threads_in_batch_solve = ocp.solver_options.num_threads_in_batch_solve print(f"Warning: num_threads_in_batch_solve is None. Using value {num_threads_in_batch_solve} set in ocp.solver_options instead.") @@ -66,27 +69,27 @@ def __init__(self, ocp: AcadosOcp, N_batch: int, num_threads_in_batch_solve: Uni print("Warning: Using AcadosOcpBatchSolver, but ocp.solver_options.with_batch_functionality is False.") print("Attempting to compile with openmp nonetheless.") ocp.solver_options.with_batch_functionality = True - + self.__num_threads_in_batch_solve = num_threads_in_batch_solve - self.__N_batch = N_batch + self.__N_batch_max = N_batch_max self.__ocp_solvers = [AcadosOcpSolver(ocp, json_file=json_file, build=n==0 if build else False, generate=n==0 if generate else False, verbose=verbose) - for n in range(self.N_batch)] + for n in range(self.N_batch_max)] self.__shared_lib = self.ocp_solvers[0].shared_lib self.__acados_lib = self.ocp_solvers[0].acados_lib self.__name = self.ocp_solvers[0].name - self.__ocp_solvers_pointer = (c_void_p * self.N_batch)() + self.__ocp_solvers_pointer = (c_void_p * self.N_batch_max)() - for i in range(self.N_batch): + for i in range(self.N_batch_max): self.__ocp_solvers_pointer[i] = self.ocp_solvers[i].capsule # out data for solve - self.__status = np.zeros((self.N_batch,), dtype=np.intc, order="C") + self.__status = np.zeros((self.N_batch_max,), dtype=np.intc, order="C") self.__status_p = cast(self.__status.ctypes.data, POINTER(c_int)) getattr(self.__shared_lib, f"{self.__name}_acados_batch_solve").argtypes = [POINTER(c_void_p), POINTER(c_int), c_int, c_int] @@ -119,40 +122,41 @@ def ocp_solvers(self): """List of AcadosOcpSolvers.""" return self.__ocp_solvers - @property - def N_batch(self): - """Batch size.""" - return self.__N_batch - + def N_batch_max(self): + """Maximum batch size.""" + return self.__N_batch_max + @property def num_threads_in_batch_solve(self): """Number of threads used for parallelizing the batch methods.""" return self.__num_threads_in_batch_solve - + @num_threads_in_batch_solve.setter def num_threads_in_batch_solve(self, num_threads_in_batch_solve): self.__num_threads_in_batch_solve = num_threads_in_batch_solve - def solve(self): + def solve(self, n_batch: Optional[int] = None) -> None: """ - Call solve for all `N_batch` solvers. + Call solve for the first `n_batch` solvers. Or `N_batch_max` if `n_batch` is None. """ + n_batch = self.__check_n_batch(n_batch) - getattr(self.__shared_lib, f"{self.__name}_acados_batch_solve")(self.__ocp_solvers_pointer, self.__status_p, self.__N_batch, self.__num_threads_in_batch_solve) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_solve")(self.__ocp_solvers_pointer, self.__status_p, n_batch, self.__num_threads_in_batch_solve) # to be consistent with non-batched solve for s, solver in zip(self.__status, self.ocp_solvers): solver.status = s - def setup_qp_matrices_and_factorize(self): + def setup_qp_matrices_and_factorize(self, n_batch: Optional[int] = None) -> None: """ - Call setup_qp_matrices_and_factorize for all `N_batch` solvers. + Call setup_qp_matrices_and_factorize for the first `n_batch` solvers. """ + n_batch = self.__check_n_batch(n_batch) - getattr(self.__shared_lib, f"{self.__name}_acados_batch_setup_qp_matrices_and_factorize")(self.__ocp_solvers_pointer, self.__status_p, self.__N_batch, self.__num_threads_in_batch_solve) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_setup_qp_matrices_and_factorize")(self.__ocp_solvers_pointer, self.__status_p, n_batch, self.__num_threads_in_batch_solve) # to be consistent with non-batched solve for s, solver in zip(self.__status, self.ocp_solvers): @@ -168,12 +172,12 @@ def eval_adjoint_solution_sensitivity(self, """ Evaluate the adjoint sensitivity of the solution with respect to the parameters. :param seed_x : Sequence of tuples of the form (stage: int, seed_vec: np.ndarray). - The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the states at that stage with shape (N_batch, nx, n_seeds) + The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the states at that stage with shape (n_batch, nx, n_seeds) :param seed_u : Sequence of tuples of the form (stage: int, seed_vec: np.ndarray). - The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the controls at that stage with shape (N_batch, nu, n_seeds) + The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the controls at that stage with shape (n_batch, nu, n_seeds) :param with_respect_to : string in ["p_global"] :param sanity_checks : bool - whether to perform sanity checks, turn off for minimal overhead, default: True - :returns : np.ndarray of shape (N_batch, n_seeds, np_global) + :returns : np.ndarray of shape (n_batch, n_seeds, np_global) """ if seed_x is None: @@ -196,6 +200,7 @@ def eval_adjoint_solution_sensitivity(self, if not isinstance(s, np.ndarray): raise TypeError(f"seed_x[0][1] should be np.ndarray, got {type(s)}") n_seeds = seed_x[0][1].shape[2] + n_batch = seed_x[0][1].shape[0] if len(seed_u) > 0: if not isinstance(seed_u[0], tuple) or len(seed_u[0]) != 2: @@ -204,11 +209,14 @@ def eval_adjoint_solution_sensitivity(self, if not isinstance(s, np.ndarray): raise TypeError(f"seed_u[0][1] should be np.ndarray, got {type(s)}") n_seeds = seed_u[0][1].shape[2] + n_batch = seed_u[0][1].shape[0] + + n_batch = self.__check_n_batch(n_batch) if sanity_checks: N_horizon = self.__ocp_solvers[0].acados_ocp.solver_options.N_horizon - for n in range(self.N_batch): + for n in range(n_batch): self.__ocp_solvers[n]._sanity_check_solution_sensitivities() nx = self.__acados_lib.ocp_nlp_dims_get_from_attr( @@ -223,8 +231,8 @@ def eval_adjoint_solution_sensitivity(self, raise ValueError(f"AcadosOcpBatchSolver.eval_adjoint_solution_sensitivity(): stage {stage} for {name} is not valid.") if not isinstance(seed_stage, np.ndarray): raise TypeError(f"{name} for stage {stage} should be np.ndarray, got {type(seed_stage)}") - if seed_stage.shape != (self.N_batch, dim, n_seeds): - raise ValueError(f"{name} for stage {stage} should have shape (N_batch, dim, n_seeds) = ({self.N_batch}, {dim}, {n_seeds}), got {seed_stage.shape}.") + if seed_stage.shape != (n_batch, dim, n_seeds): + raise ValueError(f"{name} for stage {stage} should have shape (n_batch, dim, n_seeds) = ({n_batch}, {dim}, {n_seeds}), got {seed_stage.shape}.") if with_respect_to == "p_global": field = "p_global".encode('utf-8') @@ -234,20 +242,20 @@ def eval_adjoint_solution_sensitivity(self, # compute jacobian wrt params t0 = time.time() - getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_params_jac")(self.__ocp_solvers_pointer, self.__N_batch, self.__num_threads_in_batch_solve) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_params_jac")(self.__ocp_solvers_pointer, n_batch, self.__num_threads_in_batch_solve) self.time_solution_sens_lin = time.time() - t0 t1 = time.time() - grad_p = np.zeros((self.N_batch, n_seeds, np_global), order="C", dtype=np.float64) + grad_p = np.zeros((n_batch, n_seeds, np_global), order="C", dtype=np.float64) offset = n_seeds*np_global for i_seed in range(n_seeds): # set seed: - self.reset_sens_out() + self._reset_sens_out(n_batch) # TODO this could be a batch_set - for m in range(self.N_batch): + for m in range(n_batch): for (stage, sx) in seed_x: self.ocp_solvers[m].set(stage, 'sens_x', sx[m, :, i_seed]) for (stage, su) in seed_u: @@ -257,7 +265,7 @@ def eval_adjoint_solution_sensitivity(self, # solve adjoint sensitivities getattr(self.__shared_lib, f"{self.__name}_acados_batch_eval_solution_sens_adj_p")( - self.__ocp_solvers_pointer, field, 0, c_grad_p, offset, self.__N_batch, self.__num_threads_in_batch_solve) + self.__ocp_solvers_pointer, field, 0, c_grad_p, offset, n_batch, self.__num_threads_in_batch_solve) self.time_solution_sens_solve = time.time() - t1 @@ -266,19 +274,19 @@ def eval_adjoint_solution_sensitivity(self, raise NotImplementedError(f"with_respect_to {with_respect_to} not implemented.") - def reset_sens_out(self, ): + def _reset_sens_out(self, n_batch: int) -> None: # TODO batch this - for n in range(self.N_batch): + for n in range(n_batch): self.__acados_lib.ocp_nlp_out_set_values_to_zero(self.__ocp_solvers[n].nlp_config, self.__ocp_solvers[n].nlp_dims, self.__ocp_solvers[n].sens_out) def set_flat(self, field_: str, value_: np.ndarray) -> None: """ - Set concatenation solver initialization for all `N_batch` solvers. + Set concatenation solver initialization for the first `n_batch` solvers. :param field_: string in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p'] - :param value_: np.array of shape (N_batch, n_field_total) + :param value_: np.array of shape (n_batch, n_field_total) """ field = field_.encode('utf-8') @@ -286,9 +294,12 @@ def set_flat(self, field_: str, value_: np.ndarray) -> None: raise ValueError(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') dim = self.ocp_solvers[0].get_dim_flat(field_) + n_batch = value_.shape[0] - if value_.shape != (self.N_batch, dim): - raise ValueError(f'AcadosOcpBatchSolver.set_flat(field={field_}, value): value has wrong shape, expected ({self.N_batch}, {dim}), got {value_.shape}.') + n_batch = self.__check_n_batch(n_batch) + + if value_.shape != (n_batch, dim): + raise ValueError(f'AcadosOcpBatchSolver.set_flat(field={field_}, value): value has wrong shape, expected ({n_batch}, {dim}), got {value_.shape}.') value_ = value_.reshape((-1,), order='C') N_data = value_.shape[0] @@ -296,10 +307,10 @@ def set_flat(self, field_: str, value_: np.ndarray) -> None: value_ = value_.astype(float) value_data = cast(value_.ctypes.data, POINTER(c_double)) - getattr(self.__shared_lib, f"{self.__name}_acados_batch_set_flat")(self.__ocp_solvers_pointer, field, value_data, N_data, self.__N_batch, self.__num_threads_in_batch_solve) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_set_flat")(self.__ocp_solvers_pointer, field, value_data, N_data, n_batch, self.__num_threads_in_batch_solve) - def get_flat(self, field_: str) -> np.ndarray: + def get_flat(self, field_: str, n_batch: Optional[int] = None) -> np.ndarray: """ Get concatenation of all stages of last solution of the solver. @@ -308,40 +319,43 @@ def get_flat(self, field_: str) -> np.ndarray: """ if field_ not in ['x', 'u', 'z', 'pi', 'lam', 'sl', 'su', 'p']: raise ValueError(f'AcadosOcpSolver.get_flat(field={field_}): \'{field_}\' is an invalid argument.') + n_batch = self.__check_n_batch(n_batch) field = field_.encode('utf-8') dim = self.ocp_solvers[0].get_dim_flat(field_) - out = np.ascontiguousarray(np.zeros((self.N_batch, dim,)), dtype=np.float64) + out = np.ascontiguousarray(np.zeros((n_batch, dim,)), dtype=np.float64) out_data = cast(out.ctypes.data, POINTER(c_double)) - getattr(self.__shared_lib, f"{self.__name}_acados_batch_get_flat")(self.__ocp_solvers_pointer, field, out_data, self.N_batch*dim, self.__N_batch, self.__num_threads_in_batch_solve) + getattr(self.__shared_lib, f"{self.__name}_acados_batch_get_flat")(self.__ocp_solvers_pointer, field, out_data, n_batch*dim, n_batch, self.__num_threads_in_batch_solve) return out - def store_iterate_to_flat_obj(self) -> AcadosOcpFlattenedBatchIterate: + def store_iterate_to_flat_obj(self, n_batch: Optional[int] = None) -> AcadosOcpFlattenedBatchIterate: """ - Returns the current iterate of the OCP solvers as an AcadosOcpFlattenedBatchIterate. + Returns the current iterate of the first `n_batch` OCP solvers as an AcadosOcpFlattenedBatchIterate. """ - return AcadosOcpFlattenedBatchIterate(x = self.get_flat("x"), - u = self.get_flat("u"), - z = self.get_flat("z"), - sl = self.get_flat("sl"), - su = self.get_flat("su"), - pi = self.get_flat("pi"), - lam = self.get_flat("lam"), - N_batch=self.N_batch) + n_batch = self.__check_n_batch(n_batch) + return AcadosOcpFlattenedBatchIterate(x = self.get_flat("x", n_batch), + u = self.get_flat("u", n_batch), + z = self.get_flat("z", n_batch), + sl = self.get_flat("sl", n_batch), + su = self.get_flat("su", n_batch), + pi = self.get_flat("pi", n_batch), + lam = self.get_flat("lam", n_batch), + N_batch=n_batch) def load_iterate_from_flat_obj(self, iterate: AcadosOcpFlattenedBatchIterate) -> None: """ - Loads the provided iterate into the OCP solvers. + Loads the provided iterate into the first `n_batch` OCP solvers. + n_batch is determined by the iterate object. + Note: The iterate object does not contain the the parameters. """ - - if self.N_batch != iterate.N_batch: - raise ValueError(f"Wrong batch dimension. Expected {self.N_batch}, got {iterate.N_batch}") + n_batch = iterate.N_batch + n_batch = self.__check_n_batch(n_batch) self.set_flat("x", iterate.x) self.set_flat("u", iterate.u) @@ -350,3 +364,10 @@ def load_iterate_from_flat_obj(self, iterate: AcadosOcpFlattenedBatchIterate) -> self.set_flat("su", iterate.su) self.set_flat("pi", iterate.pi) self.set_flat("lam", iterate.lam) + + def __check_n_batch(self, n_batch: Optional[int]) -> int: + if n_batch is None: + n_batch = self.N_batch_max + if n_batch > self.N_batch_max: + raise Exception(f"AcadosOcpBatchSolver: n_batch {n_batch} is larger than N_batch_max {self.N_batch_max}.") + return n_batch From c9b5d614abbe2f5408a66eb59328790cb497986f Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 15 Apr 2025 17:42:51 +0200 Subject: [PATCH 09/24] Fix typo: intial -> initial (#1500) - NOTE: function `AcadosOcp.translate_initial_cost_term_to_external` recently introduced has now a new name. --- examples/acados_matlab_octave/race_cars/main.m | 4 ++-- .../test/create_ocp_qp_solver_formulation.m | 2 +- .../acados_matlab_octave/test/create_parametric_ocp_qp.m | 2 +- examples/acados_python/race_cars/acados_settings.py | 4 ++-- examples/acados_python/race_cars/acados_settings_dev.py | 4 ++-- interfaces/acados_template/acados_template/acados_ocp.py | 6 +++--- 6 files changed, 11 insertions(+), 11 deletions(-) diff --git a/examples/acados_matlab_octave/race_cars/main.m b/examples/acados_matlab_octave/race_cars/main.m index ce3c0f658b..32a091cfbc 100644 --- a/examples/acados_matlab_octave/race_cars/main.m +++ b/examples/acados_matlab_octave/race_cars/main.m @@ -149,7 +149,7 @@ ocp_model.set('cost_Zl', eye(nsh,nsh)); ocp_model.set('cost_Zu', eye(nsh,nsh)); -% set intial condition +% set initial condition ocp_model.set('constr_x0', model.x0); % cost = define linear cost on x and u @@ -188,7 +188,7 @@ ocp_model.set('cost_W', W); ocp_model.set('cost_W_e', W_e); -% set intial references +% set initial references y_ref = zeros(ny,1); y_ref_e = zeros(ny_e,1); y_ref(1) = 1; % set reference on 's' to 1 to push the car forward (progress) diff --git a/examples/acados_matlab_octave/test/create_ocp_qp_solver_formulation.m b/examples/acados_matlab_octave/test/create_ocp_qp_solver_formulation.m index 2603534f1c..a7c403f73f 100644 --- a/examples/acados_matlab_octave/test/create_ocp_qp_solver_formulation.m +++ b/examples/acados_matlab_octave/test/create_ocp_qp_solver_formulation.m @@ -36,7 +36,7 @@ Vx_e = zeros(ny_e,nx); Vx_e(1:nx,:) = eye(nx); % state-to-output matrix in mayer term W = eye(ny); W_e = 5 * W(1:ny_e,1:ny_e); % cost weights in mayer term -y_ref = zeros(ny,1); % set intial references +y_ref = zeros(ny,1); % set initial references y_ref_e = zeros(ny_e,1); % cost diff --git a/examples/acados_matlab_octave/test/create_parametric_ocp_qp.m b/examples/acados_matlab_octave/test/create_parametric_ocp_qp.m index 00446c39c3..d96ea92922 100644 --- a/examples/acados_matlab_octave/test/create_parametric_ocp_qp.m +++ b/examples/acados_matlab_octave/test/create_parametric_ocp_qp.m @@ -33,7 +33,7 @@ Vx_e = zeros(ny_e,nx); Vx_e(1:nx,:) = eye(nx); % state-to-output matrix in mayer term W = eye(ny); W_e = 5 * W(1:ny_e,1:ny_e); % cost weights in mayer term - y_ref = zeros(ny,1); % set intial references + y_ref = zeros(ny,1); % set initial references y_ref_e = zeros(ny_e,1); ocp.cost.cost_type = cost_type; diff --git a/examples/acados_python/race_cars/acados_settings.py b/examples/acados_python/race_cars/acados_settings.py index bd2b65b1e1..bb51001162 100644 --- a/examples/acados_python/race_cars/acados_settings.py +++ b/examples/acados_python/race_cars/acados_settings.py @@ -102,7 +102,7 @@ def acados_settings(Tf, N, track_file): ocp.cost.zu = 100 * np.ones((ns,)) ocp.cost.Zu = 0 * np.ones((ns,)) - # set intial references + # set initial references ocp.cost.yref = np.array([1, 0, 0, 0, 0, 0, 0, 0]) ocp.cost.yref_e = np.array([0, 0, 0, 0, 0, 0]) @@ -138,7 +138,7 @@ def acados_settings(Tf, N, track_file): ocp.constraints.ush = np.zeros(nsh) ocp.constraints.idxsh = np.array([0, 2]) - # set intial condition + # set initial condition ocp.constraints.x0 = model.x0 # set QP solver and integration diff --git a/examples/acados_python/race_cars/acados_settings_dev.py b/examples/acados_python/race_cars/acados_settings_dev.py index c7604afdb4..256242694b 100644 --- a/examples/acados_python/race_cars/acados_settings_dev.py +++ b/examples/acados_python/race_cars/acados_settings_dev.py @@ -106,7 +106,7 @@ def acados_settings(Tf, N, track_file): ocp.cost.Zl = 1 * np.ones((ns,)) ocp.cost.Zu = 1 * np.ones((ns,)) - # set intial references + # set initial references ocp.cost.yref = np.array([1, 0, 0, 0, 0, 0, 0, 0]) ocp.cost.yref_e = np.array([0, 0, 0, 0, 0, 0]) @@ -144,7 +144,7 @@ def acados_settings(Tf, N, track_file): ocp.constraints.ush = np.zeros(nsh) ocp.constraints.idxsh = np.array(range(nsh)) - # set intial condition + # set initial condition ocp.constraints.x0 = model.x0 # set QP solver and integration diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 85c3228d20..f1be8b93b5 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -584,7 +584,7 @@ def _make_consistent_slacks_initial(self): cost.Zl_0 = cost.Zl cost.Zu_0 = cost.Zu print("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided.") - print("Using entries [zl, zu, Zl, Zu] at intial node for slack penalties.\n") + print("Using entries [zl, zu, Zl, Zu] at initial node for slack penalties.\n") else: raise ValueError("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided and cannot be inferred from other fields.\n") @@ -1500,12 +1500,12 @@ def translate_cost_to_external_cost(self, self.model.p_global = ca.vertcat(self.model.p_global, p_global) self.p_global_values = np.concatenate((self.p_global_values, p_global_values)) - self.translate_intial_cost_term_to_external(yref_0, W_0, cost_hessian) + self.translate_initial_cost_term_to_external(yref_0, W_0, cost_hessian) self.translate_intermediate_cost_term_to_external(yref, W, cost_hessian) self.translate_terminal_cost_term_to_external(yref_e, W_e, cost_hessian) - def translate_intial_cost_term_to_external(self, yref_0: Optional[Union[ca.SX, ca.MX]] = None, W_0: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): + def translate_initial_cost_term_to_external(self, yref_0: Optional[Union[ca.SX, ca.MX]] = None, W_0: Optional[Union[ca.SX, ca.MX]] = None, cost_hessian: str = 'EXACT'): if cost_hessian not in ['EXACT', 'GAUSS_NEWTON']: raise Exception(f"Invalid cost_hessian {cost_hessian}, should be 'EXACT' or 'GAUSS_NEWTON'.") From 9ab52ee00bf7993e5695a4bcd520beaa12aff189 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 17 Apr 2025 18:20:20 +0200 Subject: [PATCH 10/24] Python verbosity (#1501) - add verbose to `make_consistent()` for `AcadosOcp` and `AcadosMultiphaseOcp` - for batch solvers: only use verbose when creating the first solver --- .../acados_template/acados_multiphase_ocp.py | 4 +- .../acados_template/acados_ocp.py | 37 ++++++++++--------- .../acados_ocp_batch_solver.py | 3 +- .../acados_template/acados_ocp_solver.py | 9 +++-- .../acados_sim_batch_solver.py | 3 +- 5 files changed, 30 insertions(+), 26 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py index 6df5df8176..9282eb8528 100644 --- a/interfaces/acados_template/acados_template/acados_multiphase_ocp.py +++ b/interfaces/acados_template/acados_template/acados_multiphase_ocp.py @@ -256,7 +256,7 @@ def set_phase(self, ocp: AcadosOcp, phase_idx: int) -> None: return - def make_consistent(self) -> None: + def make_consistent(self, verbose: bool = True) -> None: self.N_horizon = sum(self.N_list) self.solver_options.N_horizon = self.N_horizon # NOTE: to not change options when making ocp consistent @@ -330,7 +330,7 @@ def make_consistent(self) -> None: print(f"Phase {i} contains non-default initial fields: {nondefault_fields}, which will be ignored.") print(f"Calling make_consistent for phase {i}.") - ocp.make_consistent(is_mocp_phase=True) + ocp.make_consistent(is_mocp_phase=True, verbose=verbose) self.dummy_ocp_list.append(ocp) diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index f1be8b93b5..4aa465568e 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -898,7 +898,7 @@ def _make_consistent_simulation(self): raise ValueError("Wrong value for sim_method_jac_reuse. Should be either int or array of ints of shape (N,).") - def make_consistent(self, is_mocp_phase=False) -> None: + def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None: """ Detect dimensions, perform sanity checks """ @@ -941,23 +941,24 @@ def make_consistent(self, is_mocp_phase=False) -> None: self._make_consistent_cost_terminal() # GN check - gn_warning_0 = (opts.N_horizon > 0 and cost.cost_type_0 == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_0)) - gn_warning_path = (opts.N_horizon > 0 and cost.cost_type == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess)) - gn_warning_terminal = (cost.cost_type_e == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_e)) - if any([gn_warning_0, gn_warning_path, gn_warning_terminal]): - external_cost_types = [] - if gn_warning_0: - external_cost_types.append('cost_type_0') - if gn_warning_path: - external_cost_types.append('cost_type') - if gn_warning_terminal: - external_cost_types.append('cost_type_e') - print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not well defined!\n" - f"got cost_type EXTERNAL for {', '.join(external_cost_types)}, hessian_approx: 'GAUSS_NEWTON'.\n" - "With this setting, acados will proceed computing the exact Hessian for the cost term and no Hessian contribution from constraints and dynamics.\n" - "If the external cost is a linear least squares cost, this coincides with the Gauss-Newton Hessian.\n" - "Note: There is also the option to use the external cost module with a numerical Hessian approximation (see `ext_cost_num_hess`).\n" - "OR the option to provide a symbolic custom Hessian approximation (see `cost_expr_ext_cost_custom_hess`).\n") + if verbose: + gn_warning_0 = (opts.N_horizon > 0 and cost.cost_type_0 == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_0)) + gn_warning_path = (opts.N_horizon > 0 and cost.cost_type == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess)) + gn_warning_terminal = (cost.cost_type_e == 'EXTERNAL' and opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and is_empty(model.cost_expr_ext_cost_custom_hess_e)) + if any([gn_warning_0, gn_warning_path, gn_warning_terminal]): + external_cost_types = [] + if gn_warning_0: + external_cost_types.append('cost_type_0') + if gn_warning_path: + external_cost_types.append('cost_type') + if gn_warning_terminal: + external_cost_types.append('cost_type_e') + print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not well defined!\n" + f"got cost_type EXTERNAL for {', '.join(external_cost_types)}, hessian_approx: 'GAUSS_NEWTON'.\n" + "With this setting, acados will proceed computing the exact Hessian for the cost term and no Hessian contribution from constraints and dynamics.\n" + "If the external cost is a linear least squares cost, this coincides with the Gauss-Newton Hessian.\n" + "Note: There is also the option to use the external cost module with a numerical Hessian approximation (see `ext_cost_num_hess`).\n" + "OR the option to provide a symbolic custom Hessian approximation (see `cost_expr_ext_cost_custom_hess`).\n") # cost integration if opts.N_horizon > 0: diff --git a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py index 6f75d164e2..6f9068ed58 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_batch_solver.py @@ -77,7 +77,8 @@ def __init__(self, ocp: AcadosOcp, N_batch_max: int, json_file=json_file, build=n==0 if build else False, generate=n==0 if generate else False, - verbose=verbose) + verbose=verbose if n==0 else False, + ) for n in range(self.N_batch_max)] self.__shared_lib = self.ocp_solvers[0].shared_lib diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index eb8f585b5d..2f443f0377 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -90,7 +90,7 @@ def shared_lib(self,): return self.__shared_lib @classmethod - def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: str, simulink_opts=None, cmake_builder: CMakeBuilder = None): + def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: str, simulink_opts=None, cmake_builder: CMakeBuilder = None, verbose=True): """ Generates the code for an acados OCP solver, given the description in acados_ocp. :param acados_ocp: type Union[AcadosOcp, AcadosMultiphaseOcp] - description of the OCP for acados @@ -100,6 +100,7 @@ def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: :param cmake_builder: type :py:class:`~acados_template.builders.CMakeBuilder` generate a `CMakeLists.txt` and use the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with `MS Visual Studio`); default: `None` + :param verbose: indicating if warnings are printed """ acados_ocp.code_export_directory = os.path.abspath(acados_ocp.code_export_directory) @@ -112,7 +113,7 @@ def generate(cls, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp], json_file: acados_ocp.simulink_opts = simulink_opts # make consistent - acados_ocp.make_consistent() + acados_ocp.make_consistent(verbose=verbose) # module dependent post processing if acados_ocp.solver_options.integrator_type == 'GNSF': @@ -225,11 +226,11 @@ def __init__(self, acados_ocp: Union[AcadosOcp, AcadosMultiphaseOcp, None], json if generate: if json_file is not None: acados_ocp.json_file = json_file - self.generate(acados_ocp, json_file=acados_ocp.json_file, simulink_opts=simulink_opts, cmake_builder=cmake_builder) + self.generate(acados_ocp, json_file=acados_ocp.json_file, simulink_opts=simulink_opts, cmake_builder=cmake_builder, verbose=verbose) json_file = acados_ocp.json_file else: if acados_ocp is not None: - acados_ocp.make_consistent() + acados_ocp.make_consistent(verbose=verbose) # load json, store options in object with open(json_file, 'r') as f: diff --git a/interfaces/acados_template/acados_template/acados_sim_batch_solver.py b/interfaces/acados_template/acados_template/acados_sim_batch_solver.py index 5d0ac0d466..0893f8d731 100644 --- a/interfaces/acados_template/acados_template/acados_sim_batch_solver.py +++ b/interfaces/acados_template/acados_template/acados_sim_batch_solver.py @@ -70,7 +70,8 @@ def __init__(self, sim: AcadosSim, N_batch: int, num_threads_in_batch_solve: Uni json_file=json_file, build=n==0 if build else False, generate=n==0 if generate else False, - verbose=verbose) + verbose=verbose if n==0 else False, + ) for n in range(self.N_batch)] self.__shared_lib = self.sim_solvers[0].shared_lib From 39fb391fff862251126ae2dd6951b8131b8de753 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 23 Apr 2025 19:05:52 +0200 Subject: [PATCH 11/24] Interfaces: check for u, z dependencies of terminal cost and constraints (#1502) --- .../generate_c_code_ext_cost.m | 6 +++++ .../generate_c_code_nonlinear_constr.m | 6 +++++ .../generate_c_code_nonlinear_least_squares.m | 6 +++++ .../casadi_function_generation.py | 26 ++++++++++++++++--- 4 files changed, 41 insertions(+), 3 deletions(-) diff --git a/interfaces/acados_matlab_octave/generate_c_code_ext_cost.m b/interfaces/acados_matlab_octave/generate_c_code_ext_cost.m index b01b0e5ae3..732b3c43e1 100644 --- a/interfaces/acados_matlab_octave/generate_c_code_ext_cost.m +++ b/interfaces/acados_matlab_octave/generate_c_code_ext_cost.m @@ -80,6 +80,12 @@ function generate_c_code_ext_cost(context, model, target_dir, stage_type) error('Field `cost_expr_ext_cost_e` is required for cost_type_e == EXTERNAL.') end ext_cost_e = model.cost_expr_ext_cost_e; + if any(which_depends(ext_cost_e, model.u)) + error('terminal cost cannot depend on u.'); + end + if any(which_depends(ext_cost_e, model.z)) + error('terminal cost cannot depend on z.'); + end % generate jacobians jac_x_e = jacobian(ext_cost_e, x); % generate hessians diff --git a/interfaces/acados_matlab_octave/generate_c_code_nonlinear_constr.m b/interfaces/acados_matlab_octave/generate_c_code_nonlinear_constr.m index cc20f83d25..9df8ed0754 100644 --- a/interfaces/acados_matlab_octave/generate_c_code_nonlinear_constr.m +++ b/interfaces/acados_matlab_octave/generate_c_code_nonlinear_constr.m @@ -110,6 +110,12 @@ function generate_c_code_nonlinear_constr(context, model, target_dir, stage_type elseif strcmp(stage_type, 'terminal') % NOTE: terminal node has no u, z h_e = model.con_h_expr_e; + if any(which_depends(h_e, model.u)) + error('terminal constraints cannot depend on u.'); + end + if any(which_depends(h_e, model.z)) + error('terminal constraints cannot depend on z.'); + end % multipliers for hessian nh_e = length(h_e); if isSX diff --git a/interfaces/acados_matlab_octave/generate_c_code_nonlinear_least_squares.m b/interfaces/acados_matlab_octave/generate_c_code_nonlinear_least_squares.m index 4fae2178fc..3e3c20f5a2 100644 --- a/interfaces/acados_matlab_octave/generate_c_code_nonlinear_least_squares.m +++ b/interfaces/acados_matlab_octave/generate_c_code_nonlinear_least_squares.m @@ -118,6 +118,12 @@ function generate_c_code_nonlinear_least_squares(context, model, target_dir, sta dy_dz = jacobian(fun, z); % output symbolics ny_e = length(fun); + if any(which_depends(fun, model.u)) + error('terminal cost cannot depend on u.'); + end + if any(which_depends(fun, model.z)) + error('terminal cost cannot depend on z.'); + end if isSX y_e = SX.sym('y', ny_e, 1); u = SX.sym('u', 0, 0); diff --git a/interfaces/acados_template/acados_template/casadi_function_generation.py b/interfaces/acados_template/acados_template/casadi_function_generation.py index 56eb08390f..4f379376e1 100644 --- a/interfaces/acados_template/acados_template/casadi_function_generation.py +++ b/interfaces/acados_template/acados_template/casadi_function_generation.py @@ -459,6 +459,11 @@ def generate_c_code_external_cost(context: GenerateContext, model: AcadosModel, ext_cost = model.cost_expr_ext_cost_e custom_hess = model.cost_expr_ext_cost_custom_hess_e # Last stage cannot depend on u and z + if any(ca.which_depends(ext_cost, model.u)): + raise ValueError("terminal cost cannot depend on u.") + if any(ca.which_depends(ext_cost, model.z)): + raise ValueError("terminal cost cannot depend on z.") + # create dummy u, z u = symbol("u", 0, 0) z = symbol("z", 0, 0) @@ -532,8 +537,14 @@ def generate_c_code_nls_cost(context: GenerateContext, model: AcadosModel, stage if stage_type == 'terminal': middle_name = '_cost_y_e' - u = symbol('u', 0, 0) y_expr = model.cost_y_expr_e + if any(ca.which_depends(y_expr, model.u)): + raise ValueError("terminal cost cannot depend on u.") + if any(ca.which_depends(y_expr, model.z)): + raise ValueError("terminal cost cannot depend on z.") + # create dummy u, z + u = symbol("u", 0, 0) + z = symbol("z", 0, 0) elif stage_type == 'initial': middle_name = '_cost_y_0' @@ -589,7 +600,6 @@ def generate_c_code_conl_cost(context: GenerateContext, model: AcadosModel, stag p_global = symbol('p_global', 0, 0) if stage_type == 'terminal': - u = symbol('u', 0, 0) yref = model.cost_r_in_psi_expr_e inner_expr = model.cost_y_expr_e - yref @@ -600,7 +610,13 @@ def generate_c_code_conl_cost(context: GenerateContext, model: AcadosModel, stag suffix_name_fun_jac_hess = '_conl_cost_e_fun_jac_hess' custom_hess = model.cost_conl_custom_outer_hess_e - + if any(ca.which_depends(inner_expr, model.u)): + raise ValueError("terminal cost cannot depend on u.") + if any(ca.which_depends(inner_expr, model.z)): + raise ValueError("terminal cost cannot depend on z.") + # create dummy u, z + u = symbol("u", 0, 0) + z = symbol("z", 0, 0) elif stage_type == 'initial': u = model.u @@ -688,6 +704,10 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con constr_type = constraints.constr_type_e con_h_expr = model.con_h_expr_e con_phi_expr = model.con_phi_expr_e + if any(ca.which_depends(con_h_expr, model.u)) or any(ca.which_depends(con_phi_expr, model.u)): + raise ValueError("terminal constraints cannot depend on u.") + if any(ca.which_depends(con_h_expr, model.z)) or any(ca.which_depends(con_phi_expr, model.z)): + raise ValueError("terminal constraints cannot depend on z.") # create dummy u, z u = symbol('u', 0, 0) z = symbol('z', 0, 0) From a7ea2eb9dc770aae205e81ae61afa04c33560a22 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 24 Apr 2025 09:57:01 +0200 Subject: [PATCH 12/24] Condense `qp_out` (#1474) Closes https://github.com/acados/acados/issues/1433 Also: - fix setting warm_start_first_qp_from_nlp in RTI - Python: allow updating warm start options on the fly - update documentation and requirements for solution sensitivity computation --- .github/workflows/full_build.yml | 1 + acados/ocp_nlp/ocp_nlp_sqp_rti.c | 5 + acados/ocp_qp/ocp_qp_full_condensing.c | 13 +- acados/ocp_qp/ocp_qp_partial_condensing.c | 60 +------- .../ocp/initialization_test.py | 130 ++++++++++++++++++ .../sensitivity_utils.py | 1 + .../smooth_policy_gradients.py | 3 + external/hpipm | 2 +- .../acados_template/acados_ocp.py | 3 + .../acados_template/acados_ocp_solver.py | 13 +- 10 files changed, 166 insertions(+), 65 deletions(-) create mode 100644 examples/acados_python/pendulum_on_cart/ocp/initialization_test.py diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 59b63508d1..cae5cddaed 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -207,6 +207,7 @@ jobs: python batch_adjoint_solution_sensitivity_example.py python non_ocp_example.py cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart/ocp + python initialization_test.py python ocp_example_cost_formulations.py diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index 945d985593..2c0d8793c1 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -191,6 +191,11 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, int* rti_log_only_available_residuals = (int *) value; opts->rti_log_only_available_residuals = *rti_log_only_available_residuals; } + else if (!strcmp(field, "warm_start_first_qp_from_nlp")) + { + bool* warm_start_first_qp_from_nlp = (bool *) value; + opts->warm_start_first_qp_from_nlp = *warm_start_first_qp_from_nlp; + } else if (!strcmp(field, "as_rti_level")) { int* as_rti_level = (int *) value; diff --git a/acados/ocp_qp/ocp_qp_full_condensing.c b/acados/ocp_qp/ocp_qp_full_condensing.c index 7ae076e1ad..72f229e9c8 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing.c +++ b/acados/ocp_qp/ocp_qp_full_condensing.c @@ -486,9 +486,16 @@ int ocp_qp_full_condensing(void *qp_in_, void *fcond_qp_in_, void *opts_, void * int ocp_qp_full_condensing_condense_qp_out(void *qp_in_, void *fcond_qp_in_, void *qp_out_, void *fcond_qp_out_, void *opts_, void *mem_, void *work) { - printf("ocp_qp_full_condensing_condense_qp_out: not implemented\n"); - printf("what about implementing it? :) do it for acados!\n"); - exit(1); + ocp_qp_in *qp_in = qp_in_; + ocp_qp_out *qp_out = qp_out_; + dense_qp_out *fcond_qp_out = fcond_qp_out_; + ocp_qp_full_condensing_opts *opts = opts_; + ocp_qp_full_condensing_memory *mem = mem_; + + d_ocp_qp_reduce_eq_dof_sol(qp_in, qp_out, mem->red_sol, opts->hpipm_red_opts, mem->hpipm_red_work); + d_cond_qp_cond_sol(mem->red_qp, mem->red_sol, fcond_qp_out, opts->hpipm_cond_opts, mem->hpipm_cond_work); + + return ACADOS_SUCCESS; } diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.c b/acados/ocp_qp/ocp_qp_partial_condensing.c index ae4f538a42..0ff4ab68e2 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing.c @@ -541,67 +541,15 @@ int ocp_qp_partial_condensing(void *qp_in_, void *pcond_qp_in_, void *opts_, voi int ocp_qp_partial_condensing_condense_qp_out(void *qp_in_, void *pcond_qp_in_, void *qp_out_, void *pcond_qp_out_, void *opts_, void *mem_, void *work) { - // ocp_qp_in *qp_in = qp_in_; - // ocp_qp_in *pcond_qp_in = pcond_qp_in_; + ocp_qp_in *qp_in = qp_in_; + ocp_qp_in *pcond_qp_in = pcond_qp_in_; ocp_qp_out *qp_out = qp_out_; ocp_qp_out *pcond_qp_out = pcond_qp_out_; ocp_qp_partial_condensing_opts *opts = opts_; ocp_qp_partial_condensing_memory *mem = mem_; - assert(opts->N2 == opts->N2_bkp); - ocp_qp_dims *orig_dims = mem->dims->orig_dims; - - if (opts->N2 != mem->dims->orig_dims->N) - { - printf("\nocp_qp_partial_condensing_condense_qp_out: only works if N==N2 for now.\n"); - exit(1); - } - if (orig_dims->nbxe[0] != 0 && (orig_dims->nbxe[0] != orig_dims->nbx[0] || orig_dims->nx[0] != orig_dims->nbx[0])) - { - printf("\nocp_qp_partial_condensing_condense_qp_out: only works if nbxe[0] == nbx[0] == nx[0], or nbxe[0] == 0 for now.\n"); - exit(1); - } - - int N = orig_dims->N; - int *nx = orig_dims->nx; - int *nu = orig_dims->nu; - int *nbu = orig_dims->nbu; - int *nbx = orig_dims->nbx; - int *ng = orig_dims->ng; - int *ns = orig_dims->ns; - int i; - - if (orig_dims->nbxe[0] != 0) - { - // uxs 0 - blasfeo_dveccp(nu[0], qp_out->ux+0, 0, pcond_qp_out->ux+0, 0); - blasfeo_dveccp(2 * ns[0], qp_out->ux+0, nu[0]+nx[0], pcond_qp_out->ux+0, nu[0]); - // lam 0 - blasfeo_dveccp(nbu[0], qp_out->lam+0, 0, pcond_qp_out->lam+0, 0); - blasfeo_dveccp(ng[0], qp_out->lam+0, nbu[0]+nbx[0], pcond_qp_out->lam+0, nbu[0]); - blasfeo_dveccp(nbu[0], qp_out->lam+0, nbu[0]+nbx[0]+ng[0], pcond_qp_out->lam+0, nbu[0]+ng[0]); - blasfeo_dveccp(ng[0], qp_out->lam+0, 2*(nbu[0]+nbx[0])+ng[0], pcond_qp_out->lam+0, 2*nbu[0]+ng[0]); - // t 0 - blasfeo_dveccp(nbu[0], qp_out->t+0, 0, pcond_qp_out->t+0, 0); - blasfeo_dveccp(ng[0], qp_out->t+0, nbu[0]+nbx[0], pcond_qp_out->t+0, nbu[0]); - blasfeo_dveccp(nbu[0], qp_out->t+0, nbu[0]+nbx[0]+ng[0], pcond_qp_out->t+0, nbu[0]+ng[0]); - blasfeo_dveccp(ng[0], qp_out->t+0, 2*(nbu[0]+nbx[0])+ng[0], pcond_qp_out->t+0, 2*nbu[0]+ng[0]); - } - else - { - i = 0; - blasfeo_dveccp(nx[i] + nu[i] + 2 * ns[i], qp_out->ux+i, 0, pcond_qp_out->ux+i, 0); - blasfeo_dveccp(2 * (nbu[i] + nbx[i] + ng[i] + ns[i]), qp_out->lam+i, 0, pcond_qp_out->lam+i, 0); - blasfeo_dveccp(2 * (nbu[i] + nbx[i] + ng[i] + ns[i]), qp_out->t+i, 0, pcond_qp_out->t+i, 0); - } - for (i = 1; i<=N; i++) - { - blasfeo_dveccp(nx[i] + nu[i] + 2 * ns[i], qp_out->ux+i, 0, pcond_qp_out->ux+i, 0); - blasfeo_dveccp(2 * (nbu[i] + nbx[i] + ng[i] + ns[i]), qp_out->lam+i, 0, pcond_qp_out->lam+i, 0); - blasfeo_dveccp(2 * (nbu[i] + nbx[i] + ng[i] + ns[i]), qp_out->t+i, 0, pcond_qp_out->t+i, 0); - } - for (i = 0; ipi+i, 0, pcond_qp_out->pi+i, 0); + d_ocp_qp_reduce_eq_dof_sol(qp_in, qp_out, mem->red_sol, opts->hpipm_red_opts, mem->hpipm_red_work); + d_part_cond_qp_cond_sol(mem->red_qp, pcond_qp_in, mem->red_sol, pcond_qp_out, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); return ACADOS_SUCCESS; } diff --git a/examples/acados_python/pendulum_on_cart/ocp/initialization_test.py b/examples/acados_python/pendulum_on_cart/ocp/initialization_test.py new file mode 100644 index 0000000000..f2259acdaa --- /dev/null +++ b/examples/acados_python/pendulum_on_cart/ocp/initialization_test.py @@ -0,0 +1,130 @@ +# -*- coding: future_fstrings -*- +# +# Copyright (c) The acados authors. +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +import sys +sys.path.insert(0, '../common') + +from acados_template import AcadosOcp, AcadosOcpSolver +from pendulum_model import export_pendulum_ode_model +import numpy as np +from utils import plot_pendulum + +import casadi as ca + + +def main(qp_solver: str = 'PARTIAL_CONDENSING_HPIPM'): + # create ocp object to formulate the OCP + ocp = AcadosOcp() + + # set model + model = export_pendulum_ode_model() + ocp.model = model + + Tf = 1.0 + nx = model.x.rows() + nu = model.u.rows() + N = 20 + + # set prediction horizon + ocp.solver_options.N_horizon = N + ocp.solver_options.tf = Tf + + # cost matrices + Q_mat = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) + R_mat = 2*np.diag([1e-2]) + + # path cost + ocp.cost.cost_type = 'NONLINEAR_LS' + ocp.model.cost_y_expr = ca.vertcat(model.x, model.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 = model.x + ocp.cost.W_e = Q_mat + + # set constraints + Fmax = 80 + ocp.constraints.lbu = np.array([-Fmax]) + ocp.constraints.ubu = np.array([+Fmax]) + ocp.constraints.idxbu = np.array([0]) + + ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) + + # set options + ocp.solver_options.qp_solver = qp_solver + ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' + ocp.solver_options.integrator_type = 'IRK' + ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP + ocp.solver_options.globalization = 'MERIT_BACKTRACKING' + ocp.solver_options.qp_tol = 1e-8 + + ocp_solver = AcadosOcpSolver(ocp, verbose=False) + + status = ocp_solver.solve() + ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") + + if status != 0: + raise Exception(f'acados returned status {status}.') + + sol = ocp_solver.store_iterate_to_obj() + qp_iter = ocp_solver.get_stats("qp_iter") + nlp_iter = ocp_solver.get_stats("nlp_iter") + assert nlp_iter == 10, f"cold start should require 10 iterations, got {nlp_iter}" + + ocp_solver.load_iterate_from_obj(sol) + ocp_solver.solve() + ocp_solver.print_statistics() + nlp_iter = ocp_solver.get_stats("nlp_iter") + assert nlp_iter == 0, f"hot start should require 0 iterations, got {nlp_iter}" + + disturbed_sol = sol + disturbed_sol.x_traj[0] += 0.1 * disturbed_sol.x_traj[0] + ocp_solver.load_iterate_from_obj(disturbed_sol) + status = ocp_solver.solve() + ocp_solver.print_statistics() + nlp_iter = ocp_solver.get_stats("nlp_iter") + assert nlp_iter == 6, f"warm start should require 6 iterations, got {nlp_iter}" + + ocp_solver.load_iterate_from_obj(disturbed_sol) + ocp_solver.options_set("qp_warm_start", 1) + ocp_solver.options_set("warm_start_first_qp_from_nlp", True) + ocp_solver.options_set("warm_start_first_qp", True) + # ocp_solver.options_set("qp_mu0", 1e-3) + status = ocp_solver.solve() + ocp_solver.print_statistics() + + +if __name__ == '__main__': + main('FULL_CONDENSING_HPIPM') + main('PARTIAL_CONDENSING_HPIPM') diff --git a/examples/acados_python/pendulum_on_cart/solution_sensitivities/sensitivity_utils.py b/examples/acados_python/pendulum_on_cart/solution_sensitivities/sensitivity_utils.py index a8b8da621a..59daada8a4 100644 --- a/examples/acados_python/pendulum_on_cart/solution_sensitivities/sensitivity_utils.py +++ b/examples/acados_python/pendulum_on_cart/solution_sensitivities/sensitivity_utils.py @@ -167,6 +167,7 @@ def export_parametric_ocp( ocp.solver_options.tf = T_horizon ocp.solver_options.qp_solver_ric_alg = qp_solver_ric_alg + ocp.solver_options.qp_solver_cond_ric_alg = qp_solver_ric_alg ocp.solver_options.qp_solver_mu0 = 1e3 # makes HPIPM converge more robustly ocp.solver_options.hessian_approx = hessian_approx ocp.solver_options.nlp_solver_max_iter = 400 diff --git a/examples/acados_python/pendulum_on_cart/solution_sensitivities/smooth_policy_gradients.py b/examples/acados_python/pendulum_on_cart/solution_sensitivities/smooth_policy_gradients.py index e44bc8acd3..255d780656 100644 --- a/examples/acados_python/pendulum_on_cart/solution_sensitivities/smooth_policy_gradients.py +++ b/examples/acados_python/pendulum_on_cart/solution_sensitivities/smooth_policy_gradients.py @@ -103,6 +103,9 @@ def create_solvers(x0, use_cython=False, qp_solver_ric_alg=0, # create sensitivity solver ocp = export_parametric_ocp(x0=x0, N_horizon=N_horizon, T_horizon=T_horizon, Fmax=Fmax, hessian_approx='EXACT', qp_solver_ric_alg=qp_solver_ric_alg, with_parametric_constraint=with_parametric_constraint, with_nonlinear_constraint=with_nonlinear_constraint) + # test with QP solver that does condensing: not recommended for sensitivtity solver + ocp.solver_options.qp_solver_cond_N = int(N_horizon/4) + ocp.model.name = 'sensitivity_solver' ocp.code_export_directory = f'c_generated_code_{ocp.model.name}' if use_cython: diff --git a/external/hpipm b/external/hpipm index aebf3bcfb4..185517cf53 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit aebf3bcfb4082a7b0d0c20ccea2f8a7dc00049ff +Subproject commit 185517cf539598cf173b45fd4d4ea2a74eed3f38 diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 4aa465568e..229f0dc1c9 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -1037,6 +1037,9 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None for horizon_type, constraint in bgp_type_constraint_pairs: if constraint is not None and any(ca.which_depends(constraint, model.p_global)): raise NotImplementedError(f"with_solution_sens_wrt_params is not supported for BGP constraints that depend on p_global. Got dependency on p_global for {horizon_type} constraint.") + if opts.qp_solver_cond_N != opts.N_horizon or opts.qp_solver.startswith("FULL_CONDENSING"): + if opts.qp_solver_cond_ric_alg != 0: + print("Warning: Parametric sensitivities with condensing should be used with qp_solver_cond_ric_alg=0, as otherwise the full space Hessian needs to be factorized and the algorithm cannot handle indefinite ones.") if opts.with_value_sens_wrt_params: if dims.np_global == 0: diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index 2f443f0377..03cfc6e1aa 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -497,8 +497,8 @@ def setup_qp_matrices_and_factorize(self) -> int: This is only implemented for HPIPM QP solver without condensing. """ - if self.__solver_options["qp_solver"] != 'PARTIAL_CONDENSING_HPIPM' or self.__solver_options["qp_solver_cond_N"] != self.N: - raise NotImplementedError('This function is only implemented for HPIPM QP solver without condensing!') + if self.__solver_options["qp_solver"] != 'PARTIAL_CONDENSING_HPIPM': + raise NotImplementedError('This function is only implemented for PARTIAL_CONDENSING_HPIPM!') self.status = getattr(self.shared_lib, f"{self.name}_acados_setup_qp_matrices_and_factorize")(self.capsule) @@ -742,11 +742,12 @@ def eval_solution_sensitivity(self, (3) positive definiteness of the full-space Hessian if the square-root version of the Riccati recursion is used OR positive definiteness of the reduced Hessian if the classic Riccati recursion is used (compare: `solver_options.qp_solver_ric_alg`), \n - (4) the solution of at least one QP in advance to evaluation of the sensitivities as the factorization is reused. + (4) the last interaction before calling this function should involve the solution of the QP at the NLP solution. + This can happen as call to `solve()` with at least 1 QP being solved or `setup_qp_matrices_and_factorize()`, \n .. note:: Timing of the sensitivities computation consists of time_solution_sens_lin, time_solution_sens_solve. .. note:: Solution sensitivities with respect to parameters are currently implemented assuming the parameter vector p is global within the OCP, i.e. p=p_i with i=0, ..., N. - .. note:: Solution sensitivities with respect to parameters are currently implemented only for parametric discrete dynamics and parametric external costs (in particular, parametric constraints are not covered). + .. note:: Solution sensitivities with respect to parameters are currently implemented only for parametric discrete dynamics, parametric external costs and parametric nonlinear constraints (h). """ if with_respect_to == "params_global": @@ -881,6 +882,8 @@ def eval_adjoint_solution_sensitivity(self, The stage is the stage at which the seed_vec is applied, and seed_vec is the seed for the controls at that stage with shape (nu, n_seeds). :param with_respect_to : string in ["p_global"] :param sanity_checks : bool - whether to perform sanity checks, turn off for minimal overhead, default: True + + The correct computation of solution of adjoint sensitivities has the same requirements as the computation of solution sensitivities, see the documentation of `eval_solution_sensitivity`. """ # get n_seeds @@ -2268,7 +2271,7 @@ def options_set(self, field_, value_): 'qp_tau_min', 'qp_mu0'] string_fields = [] - bool_fields = ['with_adaptive_levenberg_marquardt'] + bool_fields = ['with_adaptive_levenberg_marquardt', 'warm_start_first_qp_from_nlp', 'warm_start_first_qp'] # check field availability and type if field_ in int_fields: From 3cb5f3102060b51aaecfc9abd7a42a0ea411a1d9 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 24 Apr 2025 09:59:35 +0200 Subject: [PATCH 13/24] Document cmake options (#1503) Plus: set CMake default `HPIPM_TARGET` to `GENERIC`, previously used `X64_AUTOMATIC` default, does actually not exist, see https://github.com/giaf/hpipm/pull/184 --- CMakeLists.txt | 2 +- docs/installation/index.md | 38 ++++++++++++++++++++++++++++++++++---- 2 files changed, 35 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 17fc936b3e..f7e6bf8aaa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,8 +64,8 @@ if ("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "aarch64") else() set(BLASFEO_TARGET "X64_AUTOMATIC" CACHE STRING "BLASFEO Target architecture") endif() -set(HPIPM_TARGET "X64_AUTOMATIC" CACHE STRING "HPIPM Target architecture") set(LA "HIGH_PERFORMANCE" CACHE STRING "Linear algebra optimization level") +set(HPIPM_TARGET "GENERIC" CACHE STRING "HPIPM Target architecture") if (CMAKE_SYSTEM_NAME MATCHES "Windows") set(BUILD_SHARED_LIBS OFF CACHE STRING "Build shared libraries") diff --git a/docs/installation/index.md b/docs/installation/index.md index fbcb942b2c..ca54cba5e6 100644 --- a/docs/installation/index.md +++ b/docs/installation/index.md @@ -24,12 +24,42 @@ Install `acados` as follows: mkdir -p build cd build cmake -DACADOS_WITH_QPOASES=ON .. -# add more optional arguments e.g. -DACADOS_WITH_OSQP=OFF/ON -DACADOS_INSTALL_DIR= above +# add more optional arguments e.g. -DACADOS_WITH_DAQP=ON, a list of CMake options is provided below make install -j4 ``` -NOTE: you can set the `BLASFEO_TARGET` in `/CMakeLists.txt`. -For a list of supported targets, we refer to https://github.com/giaf/blasfeo/blob/master/README.md . -The default is `X64_AUTOMATIC`, which attempts to determine the best available target for your machine. + +#### CMake options: +Below is a list of CMake options available for configuring the `acados` build. +These options can be passed to the `cmake` command using the `-D` flag, e.g., `cmake -DOPTION_NAME=VALUE ..`. +Adjust these options based on your requirements. + +| **Option Name** | **Description** | **Default Value** | +|--------------------------------|------------------------------------------------------------|-------------------| +| `ACADOS_WITH_QPOASES` | Compile acados with optional QP solver qpOASES | `OFF` | +| `ACADOS_WITH_DAQP` | Compile acados with optional QP solver DAQP | `OFF` | +| `ACADOS_WITH_QPDUNES` | Compile acados with optional QP solver qpDUNES | `OFF` | +| `ACADOS_WITH_OSQP` | Compile acados with optional QP solver OSQP | `OFF` | +| `ACADOS_WITH_HPMPC` | Compile acados with optional QP solver HPMPC | `OFF` | +| `ACADOS_WITH_QORE` | Compile acados with optional QP solver QORE (experimental) | `OFF` | +| `ACADOS_WITH_OOQP` | Compile acados with optional QP solver OOQP (experimental) | `OFF` | +| `BLASFEO_TARGET` | BLASFEO Target architecture, see BLASFEO repository for more information. Possible values include: `X64_AUTOMATIC`, `GENERIC`, `X64_INTEL_SKYLAKE_X`, `X64_INTEL_HASWELL`, `X64_INTEL_SANDY_BRIDGE`, `X64_INTEL_CORE`, `X64_AMD_BULLDOZER`, `ARMV8A_APPLE_M1`, `ARMV8A_ARM_CORTEX_A76`, `ARMV8A_ARM_CORTEX_A73`, `ARMV8A_ARM_CORTEX_A57`, `ARMV8A_ARM_CORTEX_A55`, `ARMV8A_ARM_CORTEX_A53`, `ARMV7A_ARM_CORTEX_A15`, `ARMV7A_ARM_CORTEX_A9`, `ARMV7A_ARM_CORTEX_A7` | `X64_AUTOMATIC` | +| `LA` | Linear algebra optimization level for BLASFEO | `HIGH_PERFORMANCE`| +| `ACADOS_WITH_SYSTEM_BLASFEO` | Use BLASFEO found via `find_package(blasfeo)` instead of compiling it | `OFF` | +| `HPIPM_TARGET` | HPIPM Target architecture. Possible values: `AVX`, `GENERIC` | `GENERIC` | +| `ACADOS_WITH_OPENMP` | OpenMP parallelization | `OFF` | +| `ACADOS_NUM_THREADS` | Number of threads for OpenMP parallelization within one NLP solver. If not set, `omp_get_max_threads` will be used to determine the number of threads. If multiple solves should be parallelized, e.g. with an `AcadosOcpBatchSolver` or `AcadosSimBatchSolver`, set this to 1. | Not set | +| `ACADOS_SILENT` | No console status output | `OFF` | +| `ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE` | Print QP inputs and outputs to file in SQP | `OFF` | +| `CMAKE_BUILD_TYPE` | Build type (e.g., Release, Debug, etc.) | `Release` | +| `ACADOS_UNIT_TESTS` | Compile unit tests | `OFF` | +| `ACADOS_EXAMPLES` | Compile C examples | `OFF` | +| `ACADOS_OCTAVE` | Octave interface CMake tests | `OFF` | +| `ACADOS_PYTHON` | Python interface CMake tests (Note: Python interface installation is independent of this) | `OFF` | +| `BUILD_SHARED_LIBS` | Build shared libraries | `ON` (non-Windows)| + + + +For more details on specific options, refer to the comments in the `CMakeLists.txt` file. #### **Make** (not recommended) NOTE: This build system is not actively tested and might be removed in the future! It is strongly recommended to use the `CMake` build system. From 2be594dad9fe97336b17f9294dec48d75552f9e8 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 24 Apr 2025 10:18:24 +0200 Subject: [PATCH 14/24] Update README to highlight the most important acados features (#1504) Co-authored-by: sandmaennchen --- README.md | 47 ++++++++++++++++++++++++++++++++++++++++----- docs/index.md | 53 +++++++++++++++++++++++++++++++++++++++++---------- 2 files changed, 85 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 4ffd7aa7ff..383364a6a6 100644 --- a/README.md +++ b/README.md @@ -6,14 +6,28 @@ ![Github actions full build workflow](https://github.com/acados/acados/actions/workflows/full_build.yml/badge.svg) -`acados` provides fast and embedded solvers for nonlinear optimal control. +`acados` provides fast and embedded solvers for nonlinear optimal control, specifically designed for real-time applications and embedded systems. It is written in `C` and offers interfaces to the programming languages `Python`, `MATLAB` and `Octave`. ## General -- `acados` implements - 1. fast SQP-type solvers for Nonlinear Programming (NLP) formulations with an Optimal Control Problem (OCP) structure - 2. efficient integration methods, also called *integrators*, to solve initial value problems with dynamic systems given as an ODE or index-1 DAE. - These integrators can efficiently compute first and second-order sensitivities of the results. +`acados` is a modular and efficient software package for solving nonlinear programs (NLP) with an optimal control problem (OCP) structure. +Such problems have to be solved repeatedly in **model predictive control (MPC)** and **moving horizon estimation (MHE)**. +The computational efficiency and modularity make `acados` an ideal choice for real-time applications. +It is designed for high-performance applications, embedded computations, and has been successfully used in [a wide range of applications](#fields-of-applications). + +### Key Features: +Some key features of `acados` are summarized in the following. +The [software design](#design-paradigms) allows to implement many algorithms beyond this list. +- **Nonlinear and economic model predictive control (NMPC)**: Solve challenging control problems with nonlinear dynamics and cost functions. +- **Moving horizon estimation (MHE)**: Estimate states and parameters of dynamic systems in real-time. +- **Support for differential algebraic equations (DAE)**: Efficiently handle systems with algebraic constraints. +- **Multiple shooting method**: Leverage the multiple shooting approach for time discretization, enabling fast and robust solutions. +- **Efficient integration methods**: Include advanced integrators for solving ODEs and DAEs, with support for first- and second-order sensitivities. +- **Real-time performance**: Optimized for high-frequency control loops, enabling reliable solutions for time-critical applications. +- **High-performance solvers**: Implement fast SQP-type solvers tailored for optimal control problems. +- **Modular design**: Easily extend and combine components for simulation, estimation, and control to fit diverse applications. +- **Solution sensitivity computation and combination with reinforcement learning (RL)**: The combination of MPC and RL is a hot research topic in control. Many learning algorithms can profit from the availability of solution sensitivities or in particular policy gradients. +`acados` offers the possibility to embed an NLP solver as a differentiable layer in an ML architecture as is demonstrated in the [`leap-c` project](https://github.com/leap-c/leap-c). ## Documentation - Documentation can be found on [docs.acados.org](https://docs.acados.org/) @@ -28,3 +42,26 @@ It is written in `C` and offers interfaces to the programming languages `Python` ## Installation - Instructions can be found at [docs.acados.org/installation](https://docs.acados.org/installation) + +### Design paradigms +The main design paradigms of `acados` are +- **efficiency**: realized by rigorously exploiting the OCP structure via tailored quadratic programming (QP) solvers, such as `HPIPM`, and (partial) condensing methods to transform QPs, enabling their efficient treatment. +Moreover, the common structure of slack variables, which for example occur when formulating soft constraints, can be exploited. +Additionally, a structure exploiting Runge-Kutta method is implemented, allowing to utilize linear dependencies within dynamical system models. +- **modularity**: +`acados` offers an extremely flexible problem formulation, allowing to not only formulate problems which occur in MPC and MHE. +More precisely, all problem functions and dimensions can vary between all stages. +Such problems are often called *multi-stage* or *multi-phase* problems. +Different NLP solvers, QP solvers, integration methods, regularization methods and globalization methods can be combined freely. +Moreover, cost and constraint functions can be declared by explicitly providing general *convex-over-nonlinear* structures, which can be exploited in the solvers. +- **usability**: The interfaces to Python, MATLAB, Simulink and Octave allow users to conveniently specify their problem in different domains and to specify their nonlinear expressions via the popular [`CasADi`](https://web.casadi.org/) symbolic software framework. +The interfaces allow to conveniently specify commonly used problem formulations via the `AcadosOcp` class and additionally expose the full flexibility of the internal `acados` problem formulation, via multi-phase formulations and `AcadosMultiphaseOcp`. + +## Fields of applications +A non-exhaustive list of projects featuring `acados` is available at [docs.acados.org/list_of_projects](https://docs.acados.org/list_of_projects/index.html). +Contributions to this list are very welcome and allow to increase visibility of your work among other `acados` users. +- Robotics: Real-time NMPC for quadrotors, legged locomotion, and agile robotic platforms. +- Autonomous Vehicles: Used in projects like openpilot in driving assistance systems. +- Energy Systems: Optimization-based control for microgrids and wind turbines. +- Biomechanics: Optimal control in biomechanics through libraries like bioptim. +- Aerospace: Applications in trajectory optimization and control for drones and morphing-wing aircraft. diff --git a/docs/index.md b/docs/index.md index a2f6118401..f31ad6fddd 100644 --- a/docs/index.md +++ b/docs/index.md @@ -32,21 +32,54 @@ Contributions via pull requests are welcome! ## About `acados` -`acados` is a software package providing fast and embedded solvers for nonlinear optimal control. -Problems can be conveniently formulated using the [`CasADi`](https://web.casadi.org/) symbolic framework and the high-level `acados` interfaces. - -`acados` provides a collection of computationally efficient building blocks tailored to optimal control structured problems, most prominently optimal control problems (OCP) and moving horizon estimation (MHE) problems. -Among others, `acados` implements: -- modules for the integration of ordinary differential equations (ODE) and differential-algebraic equations (DAE), -- interfaces to state-of-the-art QP solvers like [`HPIPM`](https://github.com/giaf/hpipm), `qpOASES`, [`DAQP`](https://github.com/darnstrom/daqp) and [`OSQP`](https://github.com/osqp/osqp) -- (partial) condensing routines, provided by `HPIPM` -- nonlinear programming solvers for optimal control structured problems -- real-time algorithms, such as the real-time iteration (RTI) and advanced-step real-time iteration (AS-RTI) algorithms +`acados` is a modular and efficient software package for solving nonlinear programs (NLP) with an optimal control problem (OCP) structure. +Such problems have to be solved repeatedly in **model predictive control (MPC)** and **moving horizon estimation (MHE)**. +The computational efficiency and modularity make `acados` an ideal choice for real-time applications. +It is designed for high-performance applications, embedded computations, and has been successfully used in [a wide range of applications](#fields-of-applications). + +`acados` is written in `C`, but control problems can be conveniently formulated using the [`CasADi`](https://web.casadi.org/) symbolic framework via the high-level `acados` interfaces to the programming languages `Python`, `MATLAB` and `Octave`. + +Some key features of `acados` are summarized in the following. +The [software design](#design-paradigms) allows to implement many algorithms beyond this list. +- **Nonlinear and economic model predictive control (NMPC)**: Solve challenging control problems with nonlinear dynamics and cost functions. +- **Moving horizon estimation (MHE)**: Estimate states and parameters of dynamic systems in real-time. +- **Support for differential algebraic equations (DAE)**: Efficiently handle systems with algebraic constraints. +- **Multiple shooting method**: Leverage the multiple shooting approach for time discretization, enabling fast and robust solutions. +- **Efficient integration methods**: Include advanced integrators for solving ODEs and DAEs, with support for first- and second-order sensitivities. +- **Real-time performance**: Optimized for high-frequency control loops, enabling reliable solutions for time-critical applications. +- **High-performance solvers**: Implement fast SQP-type solvers tailored for optimal control problems. +- **Modular design**: Easily extend and combine components for simulation, estimation, and control to fit diverse applications. +- **Solution sensitivity computation and combination with reinforcement learning (RL)**: The combination of MPC and RL is a hot research topic in control. Many learning algorithms can profit from the availability of solution sensitivities or in particular policy gradients. +`acados` offers the possibility to embed an NLP solver as a differentiable layer in an ML architecture as is demonstrated in the [`leap-c` project](https://github.com/leap-c/leap-c). The back-end of acados uses the high-performance linear algebra package [`BLASFEO`](https://github.com/giaf/blasfeo), in order to boost computational efficiency for small to medium scale matrices typical of embedded optimization applications. `MATLAB`, `Octave` and `Python` interfaces can be used to conveniently describe optimal control problems and generate self-contained C code that can be readily deployed on embedded platforms. +### Design paradigms +The main design paradigms of `acados` are +- **efficiency**: realized by rigorously exploiting the OCP structure via tailored quadratic programming (QP) solvers, such as `HPIPM`, and (partial) condensing methods to transform QPs, enabling their efficient treatment. +Moreover, the common structure of slack variables, which for example occur when formulating soft constraints, can be exploited. +Additionally, a structure exploiting Runge-Kutta method is implemented, allowing to utilize linear dependencies within dynamical system models. +- **modularity**: +`acados` offers an extremely flexible problem formulation, allowing to not only formulate problems which occur in MPC and MHE. +More precisely, all problem functions and dimensions can vary between all stages. +Such problems are often called *multi-stage* or *multi-phase* problems. +Different NLP solvers, QP solvers, integration methods, regularization methods and globalization methods can be combined freely. +Moreover, cost and constraint functions can be declared by explicitly providing general *convex-over-nonlinear* structures, which can be exploited in the solvers. +- **usability**: The interfaces to Python, MATLAB, Simulink and Octave allow users to conveniently specify their problem in different domains and to specify their nonlinear expressions via the popular [`CasADi`](https://web.casadi.org/) symbolic software framework. +The interfaces allow to conveniently specify commonly used problem formulations via the `AcadosOcp` class and additionally expose the full flexibility of the internal `acados` problem formulation, via multi-phase formulations and `AcadosMultiphaseOcp`. + +## Fields of applications +A non-exhaustive list of projects featuring `acados` is available [here](https://docs.acados.org/list_of_projects/index.html). +Contributions to this list are very welcome and allow to increase visibility of your work among other `acados` users. +- Robotics: Real-time NMPC for quadrotors, legged locomotion, and agile robotic platforms. +- Autonomous Vehicles: Used in projects like openpilot in driving assistance systems. +- Energy Systems: Optimization-based control for microgrids and wind turbines. +- Biomechanics: Optimal control in biomechanics through libraries like bioptim. +- Aerospace: Applications in trajectory optimization and control for drones and morphing-wing aircraft. + + # Documentation page overview ```eval_rst From 398d7a6c98af535c7c6a343cbc4d99511f6695d3 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 24 Apr 2025 18:02:00 +0200 Subject: [PATCH 15/24] Simulink: `levenberg_marquardt` as optional input (#1507) --- .../matlab_templates/acados_solver_sfun.in.c | 15 +++++++++++++++ .../matlab_templates/make_sfun.in.m | 6 ++++++ .../acados_template/simulink_default_opts.json | 3 ++- 3 files changed, 23 insertions(+), 1 deletion(-) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c index 1381bd4d48..c1c8f12cac 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c @@ -286,6 +286,10 @@ static void mdlInitializeSizes (SimStruct *S) {%- set n_inputs = n_inputs + 1 -%} {%- endif -%} + {%- if simulink_opts.inputs.levenberg_marquardt -%} {#- levenberg_marquardt #} + {%- set n_inputs = n_inputs + 1 -%} + {%- endif -%} + {%- if simulink_opts.customizable_inputs %} {#- customizable inputs #} {%- for input_name, input_spec in simulink_opts.customizable_inputs -%} @@ -589,6 +593,11 @@ static void mdlInitializeSizes (SimStruct *S) ssSetInputPortVectorDimension(S, {{ i_input }}, 1); {%- endif -%} + {%- if simulink_opts.inputs.levenberg_marquardt -%} {#- levenberg_marquardt #} + {%- set i_input = i_input + 1 %} + // levenberg_marquardt + ssSetInputPortVectorDimension(S, {{ i_input }}, 1); + {%- endif -%} {%- if simulink_opts.customizable_inputs %} {#- customizable inputs #} @@ -1235,6 +1244,12 @@ static void mdlOutputs(SimStruct *S, int_T tid) ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase); {%- endif %} + {%- if simulink_opts.inputs.levenberg_marquardt %} {#- levenberg_marquardt #} + {%- set i_input = i_input + 1 %} + in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); + double levenberg_marquardt = (double)(*in_sign[0]); + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "levenberg_marquardt", &levenberg_marquardt); + {%- endif %} {%- if simulink_opts.customizable_inputs %} {#- customizable inputs #} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m index 4bb9f3f3d8..d5b9b6336f 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m +++ b/interfaces/acados_template/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m @@ -430,6 +430,12 @@ i_in = i_in + 1; {%- endif %} +{%- if simulink_opts.inputs.levenberg_marquardt %} {#- levenberg_marquardt #} +input_note = strcat(input_note, num2str(i_in), ') levenberg_marquardt, size [1]\n '); +sfun_input_names = [sfun_input_names; 'levenberg_marquardt [1]']; +i_in = i_in + 1; +{%- endif %} + {%- if simulink_opts.customizable_inputs %} {#- customizable inputs #} {%- for input_name, input_spec in simulink_opts.customizable_inputs -%} diff --git a/interfaces/acados_template/acados_template/simulink_default_opts.json b/interfaces/acados_template/acados_template/simulink_default_opts.json index 0ed61ec488..852229681b 100644 --- a/interfaces/acados_template/acados_template/simulink_default_opts.json +++ b/interfaces/acados_template/acados_template/simulink_default_opts.json @@ -53,7 +53,8 @@ "u_init": 0, "pi_init": 0, "slacks_init": 0, - "rti_phase": 0 + "rti_phase": 0, + "levenberg_marquardt": 0 }, "samplingtime": "t0" } From cca1902a8162a1534724e2da1d1837056e461ceb Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 25 Apr 2025 16:54:37 +0200 Subject: [PATCH 16/24] Add real-world examples with videos to documentation page (#1508) --- docs/conf.py | 6 +- docs/index.md | 117 --------------------------- docs/index.rst | 124 +++++++++++++++++++++++++++++ docs/list_of_projects/index.md | 6 +- docs/real_world_examples/index.rst | 80 +++++++++++++++++++ docs/requirements.txt | 6 +- docs/troubleshooting/index.md | 4 + 7 files changed, 221 insertions(+), 122 deletions(-) delete mode 100644 docs/index.md create mode 100644 docs/index.rst create mode 100644 docs/real_world_examples/index.rst diff --git a/docs/conf.py b/docs/conf.py index a090199e46..310c0d9830 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -81,7 +81,11 @@ # Add any Sphinx extension module names here, as strings. They can be # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. -extensions = ['sphinx.ext.mathjax', 'breathe', 'recommonmark', 'sphinx.ext.autodoc', 'sphinx.ext.graphviz', 'sphinx_markdown_tables'] +extensions = ['sphinx.ext.mathjax', 'breathe', 'recommonmark', 'sphinx.ext.autodoc', 'sphinx.ext.graphviz', 'sphinx_markdown_tables', + 'sphinx.ext.intersphinx', + 'sphinxcontrib.youtube', + 'sphinxemoji.sphinxemoji', +] # Add any paths that contain templates here, relative to this directory. diff --git a/docs/index.md b/docs/index.md deleted file mode 100644 index f31ad6fddd..0000000000 --- a/docs/index.md +++ /dev/null @@ -1,117 +0,0 @@ -# acados - - -```eval_rst - -|github-workflow-full-build| -|github-workflow-c_test_blasfeo_reference| -|appveyor-build| - -.. |github-workflow-full-build| image:: https://github.com/acados/acados/actions/workflows/full_build.yml/badge.svg - :target: https://github.com/acados/acados/actions/workflows/full_build.yml - :alt: Github workflow status - -.. |github-workflow-c_test_blasfeo_reference| image:: https://github.com/acados/acados/actions/workflows/c_test_blasfeo_reference.yml/badge.svg - :target: https://github.com/acados/acados/actions/workflows/c_test_blasfeo_reference.yml - :alt: Github workflow status - - -.. |appveyor-build| image:: https://ci.appveyor.com/api/projects/status/q0b2nohk476u5clg?svg=true - :target: https://ci.appveyor.com/project/roversch/acados - :alt: Appveyor workflow status - -``` - -Fast and embedded solvers for nonlinear optimal control. - -- `acados` source code is hosted on [Github](https://github.com/acados/acados). -Contributions via pull requests are welcome! -- `acados` has a discourse based [forum](https://discourse.acados.org/). -- `acados` is mainly developed by the [syscop group around Prof. Moritz Diehl, the Systems Control and Optimization Laboratory, at the University of Freiburg](https://www.syscop.de/). - - -## About `acados` - -`acados` is a modular and efficient software package for solving nonlinear programs (NLP) with an optimal control problem (OCP) structure. -Such problems have to be solved repeatedly in **model predictive control (MPC)** and **moving horizon estimation (MHE)**. -The computational efficiency and modularity make `acados` an ideal choice for real-time applications. -It is designed for high-performance applications, embedded computations, and has been successfully used in [a wide range of applications](#fields-of-applications). - -`acados` is written in `C`, but control problems can be conveniently formulated using the [`CasADi`](https://web.casadi.org/) symbolic framework via the high-level `acados` interfaces to the programming languages `Python`, `MATLAB` and `Octave`. - -Some key features of `acados` are summarized in the following. -The [software design](#design-paradigms) allows to implement many algorithms beyond this list. -- **Nonlinear and economic model predictive control (NMPC)**: Solve challenging control problems with nonlinear dynamics and cost functions. -- **Moving horizon estimation (MHE)**: Estimate states and parameters of dynamic systems in real-time. -- **Support for differential algebraic equations (DAE)**: Efficiently handle systems with algebraic constraints. -- **Multiple shooting method**: Leverage the multiple shooting approach for time discretization, enabling fast and robust solutions. -- **Efficient integration methods**: Include advanced integrators for solving ODEs and DAEs, with support for first- and second-order sensitivities. -- **Real-time performance**: Optimized for high-frequency control loops, enabling reliable solutions for time-critical applications. -- **High-performance solvers**: Implement fast SQP-type solvers tailored for optimal control problems. -- **Modular design**: Easily extend and combine components for simulation, estimation, and control to fit diverse applications. -- **Solution sensitivity computation and combination with reinforcement learning (RL)**: The combination of MPC and RL is a hot research topic in control. Many learning algorithms can profit from the availability of solution sensitivities or in particular policy gradients. -`acados` offers the possibility to embed an NLP solver as a differentiable layer in an ML architecture as is demonstrated in the [`leap-c` project](https://github.com/leap-c/leap-c). - -The back-end of acados uses the high-performance linear algebra package [`BLASFEO`](https://github.com/giaf/blasfeo), in order to boost computational efficiency for small to medium scale matrices typical of embedded optimization applications. -`MATLAB`, `Octave` and `Python` interfaces can be used to conveniently describe optimal control problems and generate self-contained C code that can be readily deployed on embedded platforms. - - -### Design paradigms -The main design paradigms of `acados` are -- **efficiency**: realized by rigorously exploiting the OCP structure via tailored quadratic programming (QP) solvers, such as `HPIPM`, and (partial) condensing methods to transform QPs, enabling their efficient treatment. -Moreover, the common structure of slack variables, which for example occur when formulating soft constraints, can be exploited. -Additionally, a structure exploiting Runge-Kutta method is implemented, allowing to utilize linear dependencies within dynamical system models. -- **modularity**: -`acados` offers an extremely flexible problem formulation, allowing to not only formulate problems which occur in MPC and MHE. -More precisely, all problem functions and dimensions can vary between all stages. -Such problems are often called *multi-stage* or *multi-phase* problems. -Different NLP solvers, QP solvers, integration methods, regularization methods and globalization methods can be combined freely. -Moreover, cost and constraint functions can be declared by explicitly providing general *convex-over-nonlinear* structures, which can be exploited in the solvers. -- **usability**: The interfaces to Python, MATLAB, Simulink and Octave allow users to conveniently specify their problem in different domains and to specify their nonlinear expressions via the popular [`CasADi`](https://web.casadi.org/) symbolic software framework. -The interfaces allow to conveniently specify commonly used problem formulations via the `AcadosOcp` class and additionally expose the full flexibility of the internal `acados` problem formulation, via multi-phase formulations and `AcadosMultiphaseOcp`. - -## Fields of applications -A non-exhaustive list of projects featuring `acados` is available [here](https://docs.acados.org/list_of_projects/index.html). -Contributions to this list are very welcome and allow to increase visibility of your work among other `acados` users. -- Robotics: Real-time NMPC for quadrotors, legged locomotion, and agile robotic platforms. -- Autonomous Vehicles: Used in projects like openpilot in driving assistance systems. -- Energy Systems: Optimization-based control for microgrids and wind turbines. -- Biomechanics: Optimal control in biomechanics through libraries like bioptim. -- Aerospace: Applications in trajectory optimization and control for drones and morphing-wing aircraft. - - -# Documentation page overview - -```eval_rst -Documentation latest build: |today| -``` - -```eval_rst -.. toctree:: - :maxdepth: 2 - - Home - citing/index - installation/index - list_of_projects/index - developer_guide/index - -.. toctree:: - :maxdepth: 2 - :caption: Interfaces - - interfaces/index - python_interface/index - matlab_octave_interface/index - embedded_workflow/index - -.. toctree:: - :maxdepth: 2 - :caption: User Guide - - problem_formulation/index - troubleshooting/index - features/index -``` - - \ No newline at end of file diff --git a/docs/index.rst b/docs/index.rst new file mode 100644 index 0000000000..bdc33bbd3f --- /dev/null +++ b/docs/index.rst @@ -0,0 +1,124 @@ +acados +====== + +.. |github-workflow-full-build| image:: https://github.com/acados/acados/actions/workflows/full_build.yml/badge.svg + :target: https://github.com/acados/acados/actions/workflows/full_build.yml + :alt: Github workflow status + +.. |github-workflow-c_test_blasfeo_reference| image:: https://github.com/acados/acados/actions/workflows/c_test_blasfeo_reference.yml/badge.svg + :target: https://github.com/acados/acados/actions/workflows/c_test_blasfeo_reference.yml + :alt: Github workflow status + +.. |appveyor-build| image:: https://ci.appveyor.com/api/projects/status/q0b2nohk476u5clg?svg=true + :target: https://ci.appveyor.com/project/roversch/acados + :alt: Appveyor workflow status + +|github-workflow-full-build| +|github-workflow-c_test_blasfeo_reference| +|appveyor-build| + +Fast and embedded solvers for real-world applications of nonlinear optimal control. + + +Important links +---------------- +|:cinema:| Get inspired by `real-world applications using acados `_ |:rocket:| + +|:star:| The ``acados`` **source code** is hosted on `Github `_. +Contributions via pull requests are welcome! + +|:handshake:| ``acados`` has a discourse-based `forum `_. + +|:homes:| ``acados`` is mainly developed by the `syscop group around Prof. Moritz Diehl, at the University of Freiburg `_. + +About ``acados`` +---------------- + +``acados`` is a modular and efficient software package for solving nonlinear programs (NLP) with an optimal control problem (OCP) structure. +Such problems have to be solved repeatedly in **model predictive control (MPC)** and **moving horizon estimation (MHE)**. +The computational efficiency and modularity make ``acados`` an ideal choice for real-time applications. +It is designed for high-performance applications, embedded computations, and has been successfully used in `a wide range of applications `_. + +``acados`` is written in ``C``, but control problems can be conveniently formulated using the `CasADi `_ symbolic framework via the high-level ``acados`` interfaces to the programming languages ``Python``, ``MATLAB``, and ``Octave``. + +Some key features of ``acados`` are summarized in the following. +The `software design <#design-paradigms>`_ allows implementing many algorithms beyond this list: + +- **Nonlinear and economic model predictive control (NMPC)**: Solve challenging control problems with nonlinear dynamics and cost functions. +- **Moving horizon estimation (MHE)**: Estimate states and parameters of dynamic systems in real-time. +- **Support for differential algebraic equations (DAE)**: Efficiently handle systems with algebraic constraints. +- **Multiple shooting method**: Leverage the multiple shooting approach for time discretization, enabling fast and robust solutions. +- **Efficient integration methods**: Include advanced integrators for solving ODEs and DAEs, with support for first- and second-order sensitivities. +- **Real-time performance**: Optimized for high-frequency control loops, enabling reliable solutions for time-critical applications. +- **High-performance solvers**: Implement fast SQP-type solvers tailored for optimal control problems. +- **Modular design**: Easily extend and combine components for simulation, estimation, and control to fit diverse applications. +- **Solution sensitivity computation and combination with reinforcement learning (RL)**: The combination of MPC and RL is a hot research topic in control. Many learning algorithms can profit from the availability of solution sensitivities or, in particular, policy gradients. + ``acados`` offers the possibility to embed an NLP solver as a differentiable layer in an ML architecture, as demonstrated in the `leap-c project `_. + +The back-end of ``acados`` uses the high-performance linear algebra package `BLASFEO `_, in order to boost computational efficiency for small to medium-scale matrices typical of embedded optimization applications. +``MATLAB``, ``Octave``, and ``Python`` interfaces can be used to conveniently describe optimal control problems and generate self-contained C code that can be readily deployed on embedded platforms. + +Design paradigms +---------------- + +The main design paradigms of ``acados`` are: + +- **Efficiency**: Realized by rigorously exploiting the OCP structure via tailored quadratic programming (QP) solvers, such as ``HPIPM``, and (partial) condensing methods to transform QPs, enabling their efficient treatment. + Moreover, the common structure of slack variables, which, for example, occur when formulating soft constraints, can be exploited. + Additionally, a structure-exploiting Runge-Kutta method is implemented, allowing the utilization of linear dependencies within dynamical system models. +- **Modularity**: ``acados`` offers an extremely flexible problem formulation, allowing not only the formulation of problems that occur in MPC and MHE. + More precisely, all problem functions and dimensions can vary between all stages. + Such problems are often called *multi-stage* or *multi-phase* problems. + Different NLP solvers, QP solvers, integration methods, regularization methods, and globalization methods can be combined freely. + Moreover, cost and constraint functions can be declared by explicitly providing general *convex-over-nonlinear* structures, which can be exploited in the solvers. +- **Usability**: The interfaces to Python, MATLAB, Simulink, and Octave allow users to conveniently specify their problem in different domains and to specify their nonlinear expressions via the popular `CasADi `_ symbolic software framework. + The interfaces allow users to conveniently specify commonly used problem formulations via the ``AcadosOcp`` class and additionally expose the full flexibility of the internal ``acados`` problem formulation, via multi-phase formulations and ``AcadosMultiphaseOcp``. + +Fields of applications +---------------------- + +A non-exhaustive list of projects featuring ``acados`` is available `here `_. +Contributions to this list are very welcome and allow increasing the visibility of your work among other ``acados`` users. + +- **Robotics**: Real-time NMPC for quadrotors, legged locomotion, and agile robotic platforms. +- **Autonomous Vehicles**: Used in projects like openpilot in driving assistance systems. +- **Energy Systems**: Optimization-based control for microgrids and wind turbines. +- **Biomechanics**: Optimal control in biomechanics through libraries like bioptim. +- **Aerospace**: Applications in trajectory optimization and control for drones and morphing-wing aircraft. + +Documentation page overview +--------------------------- + +Documentation latest build: |today| + +.. toctree:: + :maxdepth: 2 + + Home + real_world_examples/index + citing/index + installation/index + list_of_projects/index + +.. toctree:: + :maxdepth: 2 + :caption: Interfaces + + interfaces/index + python_interface/index + matlab_octave_interface/index + +.. toctree:: + :maxdepth: 2 + :caption: User Guide + + problem_formulation/index + troubleshooting/index + features/index + +.. toctree:: + :maxdepth: 2 + :caption: Advanced + + developer_guide/index + embedded_workflow/index diff --git a/docs/list_of_projects/index.md b/docs/list_of_projects/index.md index d8015e0f6f..de39697cd4 100644 --- a/docs/list_of_projects/index.md +++ b/docs/list_of_projects/index.md @@ -1,5 +1,8 @@ -# Other Projects that feature `acados` +# Related Projects + + ## Software interfaced with `acados` - [Rockit (Rapid Optimal Control kit)](https://gitlab.kuleuven.be/meco-software/rockit) @@ -39,7 +42,6 @@ It also provides a higher level interface to `acados`, which is based on the Mat - [NMPC for Racing Using a Singularity-Free Path-Parametric Model with Obstacle Avoidance](https://cdn.syscop.de/publications/Kloeser2020.pdf) - [Mobility-enhanced MPC for Legged Locomotion on Rough Terrain](https://arxiv.org/abs/2105.05998) - - [Video to Mobility-enhanced MPC for Legged Locomotion on Rough Terrain](https://www.dropbox.com/sh/mkr4pftcug6jlo7/AABNqu1AsGED2WSR8IqvaiUla?dl=0) - [Continuous Control Set Nonlinear Model Predictive Control of Reluctance Synchronous Machines - IEEE Transactions on Control System Technology](https://ieeexplore.ieee.org/document/9360312) diff --git a/docs/real_world_examples/index.rst b/docs/real_world_examples/index.rst new file mode 100644 index 0000000000..ce4249e321 --- /dev/null +++ b/docs/real_world_examples/index.rst @@ -0,0 +1,80 @@ +.. _real_world_examples: + +================== +Real-world examples +================== + +This page shows some real-world examples enabled by ``acados``. +The list is not complete and is meant to show a variety of domains and applications. +It is intended to inspire and impress. +If you have awesome videos with ``acados``-based controllers in action, reach out to get featured. +Enjoy! |:popcorn:| + +.. Check this documentation for embedding YouTube videos: +.. https://sphinxcontrib-youtube.readthedocs.io/en/latest/usage.html + + +.. rubric:: Drone racing: Quadrotor control +|:linked_paperclips:| `Romero et al. (2022) `_ + +.. youtube:: zBVpx3bgI6E + :width: 50% + :url_parameters: ?start=86 + + +.. rubric:: Driving assistance: acados is used within openpilot +See blog post `here `_ + +.. youtube:: 0aq4Wi2rsOk + :width: 50% + :url_parameters: ?start=152 + + +.. rubric:: Bird-like drones +|:linked_paperclips:| `Wüest et al. (2024) `_ + +.. youtube:: Mv3I3Bv8UyQ + :width: 50% + :url_parameters: ?start=116 + + +.. rubric:: Flying Hand: End-Effector-Centric Framework for Versatile Aerial Manipulation Teleoperation and Policy Learning +|:linked_paperclips:| `He et al. (2024) `_ + +.. raw:: html + + + +
+
+ +
+
+```` + +.. rubric:: Swinging up a Custom-Made Furuta Pendulum with NMPC using acados (Slow Motion) +.. youtube:: oJYyD5beMqM + :width: 30% + :aspect: 9:16 diff --git a/docs/requirements.txt b/docs/requirements.txt index 9a0e9524cb..11d6d12d2c 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -1,8 +1,10 @@ sphinx==5.3 recommonmark==0.7.1 breathe==4.35.0 -sphinx_rtd_theme==2.0.0 -docutils<=0.16 +sphinx_rtd_theme>=2.0.0 +docutils jinja2==3.1.5 sphinx-book-theme==1.1.3 sphinx-markdown-tables==0.0.17 +sphinxcontrib-youtube +sphinxemoji \ No newline at end of file diff --git a/docs/troubleshooting/index.md b/docs/troubleshooting/index.md index 7740aa2d21..a046195074 100644 --- a/docs/troubleshooting/index.md +++ b/docs/troubleshooting/index.md @@ -7,6 +7,10 @@ Next, check the NLP residuals by calling `solver.print_statistics()` in Python a In order to asses the QP solver status, you need to check the corresponding QP solver status definitions. For `HPIPM`, they are given [here](https://github.com/giaf/hpipm/blob/deb7808e49a3cc2b1bdb721cba23f13869c0a35c/include/hpipm_common.h#L57). +### Solver initialization +Use the `set` method of `AcadosOcpSolver` to provide different initializations. +Are all problem functions defined at the initial guess? +`NaN` errors can often be mitigated by solver initializations. ### QP diagnostics - [Python](examples/acados_python/pendulum_on_cart/solution_sensitivities/policy_gradient_example.py) From ce6a9fa6ef7d1673c81a6b286e83e8517662d653 Mon Sep 17 00:00:00 2001 From: "LI, Jinjie" <45286479+Li-Jinjie@users.noreply.github.com> Date: Mon, 28 Apr 2025 23:20:26 +0900 Subject: [PATCH 17/24] add our application on tiltable quadrotors to "real_world_examples" (#1510) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This PR provides an example of using NMPC for overactuated tiltable quadrotors. We have some more impressive demos, and we’ll update them after we submit the paper. --- docs/real_world_examples/index.rst | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/docs/real_world_examples/index.rst b/docs/real_world_examples/index.rst index ce4249e321..ce0d358aca 100644 --- a/docs/real_world_examples/index.rst +++ b/docs/real_world_examples/index.rst @@ -78,3 +78,11 @@ See blog post `here `_ .. youtube:: oJYyD5beMqM :width: 30% :aspect: 9:16 + + +.. rubric:: Flight Control: Overactuated Tiltable-Quadrotors +|:linked_paperclips:| `Li et al. (2024) `_ + +.. youtube:: 8_pYdeuQnC0 + :width: 50% + :url_parameters: ?start=59 From c22b4a8728006f9af99a2baeb531970fc6d28e04 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 29 Apr 2025 11:38:13 +0200 Subject: [PATCH 18/24] Docs: minor changes and update build requirements (#1509) Co-authored-by: sandmaennchen --- docs/conf.py | 26 +++++++++++++++++++++++++- docs/features/index.md | 8 ++++++-- docs/index.rst | 11 ++++++++--- docs/real_world_examples/index.rst | 6 ++++++ docs/requirements.txt | 9 ++++----- 5 files changed, 49 insertions(+), 11 deletions(-) diff --git a/docs/conf.py b/docs/conf.py index 310c0d9830..5ec81ac428 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -45,6 +45,24 @@ from recommonmark.transform import AutoStructify +import sphinx.ext.autodoc as autodoc + +_original_get_doc = autodoc.Documenter.get_doc + +def patched_get_doc(self, *args, **kwargs): + try: + if self.directive and hasattr(self.directive, "state"): + doc = getattr(self.directive.state, "document", None) + if doc and hasattr(doc, "settings") and not hasattr(doc.settings, "tab_width"): + doc.settings.tab_width = 4 + except Exception: + pass # Silent fail-safe + + return _original_get_doc(self, *args, **kwargs) + +autodoc.Documenter.get_doc = patched_get_doc + + source_suffix = { '.rst': 'restructuredtext', '.txt': 'markdown', @@ -81,7 +99,13 @@ # Add any Sphinx extension module names here, as strings. They can be # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. -extensions = ['sphinx.ext.mathjax', 'breathe', 'recommonmark', 'sphinx.ext.autodoc', 'sphinx.ext.graphviz', 'sphinx_markdown_tables', +extensions = [ + 'sphinx.ext.mathjax', + 'breathe', + 'recommonmark', + 'sphinx.ext.autodoc', + 'sphinx.ext.graphviz', + 'sphinx_markdown_tables', 'sphinx.ext.intersphinx', 'sphinxcontrib.youtube', 'sphinxemoji.sphinxemoji', diff --git a/docs/features/index.md b/docs/features/index.md index be480b5226..da2d757ec6 100644 --- a/docs/features/index.md +++ b/docs/features/index.md @@ -5,9 +5,13 @@ If you are new to `acados`, we highly recommend you to start with the `getting_s - [Python](https://github.com/acados/acados/blob/main/examples/acados_python/getting_started) - [MATLAB/Octave](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started) +### Getting started with Model Predictive Control +In particular, the **closed-loop** examples are a great starting point to develop model predictive control (MPC) in a simulation. +Here, an OCP solver (`AcadosOcpSolver`) is used to compute the control inputs and an integrator (`AcadosSimSolver`) is used to simulate the real system. +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/getting_started/minimal_example_closed_loop.py) +- [MATLAB/Octave](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started/minimal_example_closed_loop.m) ## Simulation and Sensitivity propagation - - [Python](https://github.com/acados/acados/blob/main/examples/acados_python/pendulum_on_cart/sim/extensive_example_sim.py) - [MATLAB/Octave and Simulink](https://github.com/acados/acados/blob/main/examples/acados_matlab_octave/getting_started/minimal_example_sim.m) @@ -23,7 +27,7 @@ If you are new to `acados`, we highly recommend you to start with the `getting_s `acados` supports general nonlinear cost, but can also exploit particular cost structures such as (non)linear least-squares costs and convex-over-nonlinear costs. -- [Python](examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py) +- [Python](https://github.com/acados/acados/blob/main/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py) ### Soft constraints diff --git a/docs/index.rst b/docs/index.rst index bdc33bbd3f..b06259c5f0 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -1,6 +1,10 @@ acados ====== +.. meta:: + :description: acados is an open-source software package for fast and embedded nonlinear model predictive control (MPC) and moving horizon estimation (MHE). It is written in C and has interfaces to Python, MATLAB, Octave, and Simulink. The software is designed for real-time applications and is used in various fields such as robotics, autonomous vehicles, energy systems, biomechanics, and aerospace. + :keywords: optimal control, nonlinear programming, nonlinear model predictive control, embedded optimization, real-time optimization, moving horizon estimation, open-source software, software library, C library, Python interface, MATLAB interface, Octave interface, Simulink + :google-site-verification: otz_joxGlxsY8wbpkkqOty47dLxqqWPa9JCC5HeyCg8 .. |github-workflow-full-build| image:: https://github.com/acados/acados/actions/workflows/full_build.yml/badge.svg :target: https://github.com/acados/acados/actions/workflows/full_build.yml :alt: Github workflow status @@ -77,15 +81,16 @@ The main design paradigms of ``acados`` are: Fields of applications ---------------------- -A non-exhaustive list of projects featuring ``acados`` is available `here `_. -Contributions to this list are very welcome and allow increasing the visibility of your work among other ``acados`` users. - +``acados`` is used in a wide range of applications. Some examples include: - **Robotics**: Real-time NMPC for quadrotors, legged locomotion, and agile robotic platforms. - **Autonomous Vehicles**: Used in projects like openpilot in driving assistance systems. - **Energy Systems**: Optimization-based control for microgrids and wind turbines. - **Biomechanics**: Optimal control in biomechanics through libraries like bioptim. - **Aerospace**: Applications in trajectory optimization and control for drones and morphing-wing aircraft. +Please also check this non-exhaustive `list of projects `_ featuring ``acados``. +Contributions to this list are very welcome and allow increasing the visibility of your work among other ``acados`` users. + Documentation page overview --------------------------- diff --git a/docs/real_world_examples/index.rst b/docs/real_world_examples/index.rst index ce0d358aca..2b03fa44c5 100644 --- a/docs/real_world_examples/index.rst +++ b/docs/real_world_examples/index.rst @@ -86,3 +86,9 @@ See blog post `here `_ .. youtube:: 8_pYdeuQnC0 :width: 50% :url_parameters: ?start=59 + +.. rubric:: MPC of an Autonomous Warehouse Vehicle with Tricycle Kinematic +|:linked_paperclips:| `Subash et al. (2024) `_ +.. youtube:: NDta6AD5WCA + :width: 50% + :url_parameters: ?start=0 diff --git a/docs/requirements.txt b/docs/requirements.txt index 11d6d12d2c..46c342767b 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -1,10 +1,9 @@ -sphinx==5.3 +Sphinx==6.0.0 recommonmark==0.7.1 breathe==4.35.0 -sphinx_rtd_theme>=2.0.0 -docutils +docutils==0.19 jinja2==3.1.5 sphinx-book-theme==1.1.3 sphinx-markdown-tables==0.0.17 -sphinxcontrib-youtube -sphinxemoji \ No newline at end of file +sphinxcontrib-youtube==1.3.0 +sphinxemoji==0.3.1 From 78aee36e92bd66f557ceac0af9e6cb9a043f65c1 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 29 Apr 2025 12:54:57 +0200 Subject: [PATCH 19/24] Improve Solution Sensitivity related tests and examples (#1513) --- .github/workflows/full_build.yml | 2 + .../solution_sensitivity_example.py | 41 +++-- .../example_solution_sens_closed_loop.py | 8 +- .../forw_vs_adj_param_sens.py | 151 ++++++++---------- .../test_solution_sens_and_exact_hess.py | 21 +-- .../mass_spring_example.c | 6 +- .../mass_spring_nmpc_example.c | 1 + 7 files changed, 120 insertions(+), 110 deletions(-) diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index cae5cddaed..8b7175e517 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -209,6 +209,8 @@ jobs: cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart/ocp python initialization_test.py python ocp_example_cost_formulations.py + cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart + python example_solution_sens_closed_loop.py - name: Python Furuta pendulum timeout test diff --git a/examples/acados_python/chain_mass/solution_sensitivity_example.py b/examples/acados_python/chain_mass/solution_sensitivity_example.py index b1198a147f..77b1bbd4e6 100644 --- a/examples/acados_python/chain_mass/solution_sensitivity_example.py +++ b/examples/acados_python/chain_mass/solution_sensitivity_example.py @@ -47,12 +47,12 @@ import time -def export_discrete_erk4_integrator_step(f_expl: SX, x: SX, u: SX, p: ssymStruct, h: float, n_stages: int = 2) -> ca.SX: +def export_discrete_erk4_integrator_step(f_expl: SX, x: SX, u: SX, p: ssymStruct, h: float, n_steps: int = 2) -> ca.SX: """Define ERK4 integrator for continuous dynamics.""" - dt = h / n_stages + dt = h / n_steps ode = ca.Function("f", [x, u, p], [f_expl]) xnext = x - for _ in range(n_stages): + for _ in range(n_steps): k1 = ode(xnext, u, p) k2 = ode(xnext + dt / 2 * k1, u, p) k3 = ode(xnext + dt / 2 * k2, u, p) @@ -61,6 +61,17 @@ def export_discrete_erk4_integrator_step(f_expl: SX, x: SX, u: SX, p: ssymStruct return xnext +def export_discrete_euler_integrator_step(f_expl: SX, x: SX, u: SX, p: ssymStruct, h: float, n_steps: int = 2) -> ca.SX: + """Define Euler integrator for continuous dynamics.""" + dt = h / n_steps + ode = ca.Function("f", [x, u, p], [f_expl]) + xnext = x + for _ in range(n_steps): + k1 = ode(xnext, u, p) + xnext = xnext + dt * k1 + + return xnext + def define_param_ssymStruct(n_mass: int, disturbance: bool = True) -> ssymStruct: """Define parameter struct.""" @@ -97,7 +108,7 @@ def find_idx_for_labels(sub_vars: SX, sub_label: str) -> list[int]: return [i for i, label in enumerate(sub_vars.str().strip("[]").split(", ")) if sub_label in label] -def export_chain_mass_model(n_mass: int, Ts: float = 0.2, disturbance: bool = False) -> Tuple[AcadosModel, DMStruct]: +def export_chain_mass_model(n_mass: int, Ts: float = 0.2, disturbance: bool = False, discrete_dyn_type: str = "RK4") -> Tuple[AcadosModel, DMStruct]: """Export chain mass model for acados.""" x0 = np.array([0, 0, 0]) # fix mass (at wall) @@ -170,7 +181,12 @@ def export_chain_mass_model(n_mass: int, Ts: float = 0.2, disturbance: bool = Fa f_expl = vertcat(xvel, u, f) f_impl = xdot - f_expl - f_disc = export_discrete_erk4_integrator_step(f_expl, x, u, p, Ts) + if discrete_dyn_type == "RK4": + f_disc = export_discrete_erk4_integrator_step(f_expl, x, u, p, Ts) + elif discrete_dyn_type == "EULER": + f_disc = export_discrete_euler_integrator_step(f_expl, x, u, p, Ts) + else: + raise ValueError("discrete_dyn_type must be either 'RK4' or 'EULER'") model = AcadosModel() @@ -239,6 +255,7 @@ def export_parametric_ocp( hessian_approx: str = "GAUSS_NEWTON", integrator_type: str = "IRK", nlp_solver_type: str = "SQP", + discrete_dyn_type: str = "RK4", nlp_iter: int = 50, nlp_tol: float = 1e-5, random_scale: dict = {"m": 0.0, "D": 0.0, "L": 0.0, "C": 0.0}, @@ -248,7 +265,7 @@ def export_parametric_ocp( ocp.solver_options.N_horizon = chain_params_["N"] # export model - ocp.model, p = export_chain_mass_model(n_mass=chain_params_["n_mass"], Ts=chain_params_["Ts"], disturbance=True) + ocp.model, p = export_chain_mass_model(n_mass=chain_params_["n_mass"], Ts=chain_params_["Ts"], disturbance=True, discrete_dyn_type=discrete_dyn_type) # parameters np.random.seed(chain_params_["seed"]) @@ -349,11 +366,17 @@ def export_parametric_ocp( return ocp, p -def main_parametric(qp_solver_ric_alg: int = 0, chain_params_: dict = get_chain_params(), generate_code: bool = True) -> None: +def main_parametric(qp_solver_ric_alg: int = 0, + discrete_dyn_type: str = "RK4", + chain_params_: dict = get_chain_params(), + with_more_adjoints = True, + generate_code: bool = True) -> None: + if discrete_dyn_type == "EULER": + print("Warning: OCP solver does not converge with EULER integrator.") ocp, parameter_values = export_parametric_ocp( chain_params_=chain_params_, qp_solver_ric_alg=qp_solver_ric_alg, integrator_type="DISCRETE", + discrete_dyn_type=discrete_dyn_type ) - with_more_adjoints = True ocp_json_file = "acados_ocp_" + ocp.model.name + ".json" # Check if json_file exists @@ -592,4 +615,4 @@ def print_timings(timing_results: dict, metric: str = "median"): if __name__ == "__main__": chain_params = get_chain_params() chain_params["n_mass"] = 3 - main_parametric(qp_solver_ric_alg=0, chain_params_=chain_params, generate_code=True) + main_parametric(qp_solver_ric_alg=0, discrete_dyn_type="RK4", chain_params_=chain_params, generate_code=True, with_more_adjoints=True) diff --git a/examples/acados_python/pendulum_on_cart/example_solution_sens_closed_loop.py b/examples/acados_python/pendulum_on_cart/example_solution_sens_closed_loop.py index 8b8fa42187..b0cb8bd3b3 100644 --- a/examples/acados_python/pendulum_on_cart/example_solution_sens_closed_loop.py +++ b/examples/acados_python/pendulum_on_cart/example_solution_sens_closed_loop.py @@ -129,7 +129,11 @@ def main(): acados_ocp_solver = create_ocp_solver() acados_integrator = create_integrator() - print("Please check the documentation fo eval_solution_sensitivities() for the requirements on exact solution sensitivities with acados.") + print("Please check the documentation of eval_solution_sensitivities() for the requirements on exact solution sensitivities with acados.") + # NOTE: the correct computation of solution sensitivities requires an exact Hessian. + # The formulation here uses the Gauss-Newton Hessian approximation, + # which is exact as a linear dynamics and constraints are used + # and the cost function is of linear least squares type. nx = acados_ocp_solver.acados_ocp.dims.nx nu = acados_ocp_solver.acados_ocp.dims.nu @@ -163,7 +167,7 @@ def main(): u_lin = simU[i,:] x_lin = xcurrent - out_dict = acados_ocp_solver.eval_solution_sensitivity(0, with_respect_to="initial_state", return_sens_u=True, return_sens_x=False) + out_dict = acados_ocp_solver.eval_solution_sensitivity(0, with_respect_to="initial_state", return_sens_u=True, return_sens_x=False, sanity_checks=False) sens_u = out_dict['sens_u'] else: diff --git a/examples/acados_python/pendulum_on_cart/solution_sensitivities/forw_vs_adj_param_sens.py b/examples/acados_python/pendulum_on_cart/solution_sensitivities/forw_vs_adj_param_sens.py index e911a5d84a..af9877d652 100644 --- a/examples/acados_python/pendulum_on_cart/solution_sensitivities/forw_vs_adj_param_sens.py +++ b/examples/acados_python/pendulum_on_cart/solution_sensitivities/forw_vs_adj_param_sens.py @@ -34,121 +34,113 @@ TOL = 1e-6 -def main(qp_solver_ric_alg: int, use_cython=False, generate_solvers=True, plot_trajectory=False): +def main(qp_solver_ric_alg: int, generate_solvers=True, plot_trajectory=False): """ Evaluate policy and calculate its gradient for the pendulum on a cart with a parametric model. """ p_nominal = 1.0 x0 = np.array([0.0, np.pi / 2, 0.0, 0.0]) - p_test = p_nominal + 0.2 nx = len(x0) - nu = 1 - N_horizon = 10 - T_horizon = 1.0 - - # TODO: look more into why the test fails with these settings: - # adjoint sensitivities of first parameter dont match with forward sensitivities - # only "small" difference: in 5th digit, might be due to numerical inaccuracies - # N_horizon = 50 - # T_horizon = 2.0 - # p_test = p_nominal + 0.3 + N_horizon = 20 + T_horizon = 2.0 + p_test = p_nominal + 0.3 Fmax = 80.0 - cost_scale_as_param = True # test with 2 parameters + cost_scale_as_param = True with_parametric_constraint = True with_nonlinear_constraint = True ocp = export_parametric_ocp(x0=x0, N_horizon=N_horizon, T_horizon=T_horizon, Fmax=Fmax, qp_solver_ric_alg=1, cost_scale_as_param=cost_scale_as_param, with_parametric_constraint=with_parametric_constraint, with_nonlinear_constraint=with_nonlinear_constraint) - if use_cython: - raise NotImplementedError() - AcadosOcpSolver.generate(ocp, json_file="parameter_augmented_acados_ocp.json") - AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True) - ocp_solver = AcadosOcpSolver.create_cython_solver("parameter_augmented_acados_ocp.json") - else: - ocp_solver = AcadosOcpSolver(ocp, json_file="parameter_augmented_acados_ocp.json", generate=generate_solvers, build=generate_solvers) + ocp_solver = AcadosOcpSolver(ocp, json_file="parameter_augmented_acados_ocp.json", generate=generate_solvers, build=generate_solvers) # create sensitivity solver ocp = export_parametric_ocp(x0=x0, N_horizon=N_horizon, T_horizon=T_horizon, Fmax=Fmax, hessian_approx='EXACT', qp_solver_ric_alg=qp_solver_ric_alg, cost_scale_as_param=cost_scale_as_param, with_parametric_constraint=with_parametric_constraint, with_nonlinear_constraint=with_nonlinear_constraint) ocp.model.name = 'sensitivity_solver' ocp.code_export_directory = f'c_generated_code_{ocp.model.name}' + sensitivity_solver = AcadosOcpSolver(ocp, json_file=f"{ocp.model.name}.json", generate=generate_solvers, build=generate_solvers) - hp_sens_solver = True - if hp_sens_solver: - original_ocp = ocp_solver.acados_ocp - # undo some settings that are not needed for HP sens solver - ocp.solver_options.globalization_fixed_step_length = 1.0 - ocp.solver_options.nlp_solver_max_iter = original_ocp.solver_options.nlp_solver_max_iter - # to "force" a QP solve - ocp.solver_options.tol = original_ocp.solver_options.tol - ocp.solver_options.qp_tol = original_ocp.solver_options.tol - ocp.solver_options.nlp_solver_max_iter = original_ocp.solver_options.nlp_solver_max_iter - # QP warm start - # ocp.solver_options.qp_solver_warm_start = 3 - # ocp.solver_options.nlp_solver_warm_start_first_qp = True - # ocp.solver_options.nlp_solver_warm_start_first_qp_from_nlp = True - # # HPIPM settings - # ocp.solver_options.qp_solver_iter_max = 0 - # ocp.remove_x0_elimination() - - sensitivity_solver = AcadosOcpSolver(ocp, json_file=f"{ocp.model.name}.json", generate=generate_solvers, build=generate_solvers) - else: - if use_cython: - AcadosOcpSolver.generate(ocp, json_file=f"{ocp.model.name}.json") - AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True) - sensitivity_solver = AcadosOcpSolver.create_cython_solver(f"{ocp.model.name}.json") - else: - ocp.solver_options.nlp_solver_warm_start_first_qp = True - ocp.solver_options.qp_solver_warm_start = 2 - sensitivity_solver = AcadosOcpSolver(ocp, json_file=f"{ocp.model.name}.json", generate=generate_solvers, build=generate_solvers) - - # set parameter value if cost_scale_as_param: - p_val = np.array([p_test, 1.0]) + p_vals = [ + np.array([p_nominal, 1.0]), + np.array([p_nominal + 0.2, 1.0]), + np.array([p_nominal + 0.3, 1.0]), + np.array([p_nominal + 0.4, 1.0]), + ] else: - p_val = np.array([p_test]) + p_vals = np.array([p_test]) + + for p_val in p_vals: + print(f"Testing with p_val = {p_val}") + + # solve and compare forward and adjoint + solve_and_compare_fwd_and_adj(ocp_solver, sensitivity_solver, x0, p_val, with_parametric_constraint) + + if plot_trajectory: + nx = ocp.dims.nx + nu = ocp.dims.nu + simX = np.zeros((N_horizon+1, nx)) + simU = np.zeros((N_horizon, nu)) + + # get solution + for i in range(N_horizon): + simX[i,:] = ocp_solver.get(i, "x") + simU[i,:] = ocp_solver.get(i, "u") + simX[N_horizon,:] = ocp_solver.get(N_horizon, "x") + + plot_pendulum(ocp.solver_options.shooting_nodes, Fmax, simU, simX, latexify=True, time_label=ocp.model.t_label, x_labels=ocp.model.x_labels, u_labels=ocp.model.u_labels) + - ocp_solver.set_p_global_and_precompute_dependencies(p_val) + +def solve_and_compare_fwd_and_adj(ocp_solver: AcadosOcpSolver, + sensitivity_solver: AcadosOcpSolver, + x0: np.ndarray, + p_val: np.ndarray, + with_parametric_constraint: bool): + + N_horizon = ocp_solver.N + nx = ocp_solver.acados_ocp.dims.nx + nu = ocp_solver.acados_ocp.dims.nu + + # set parameter value + ocp_solver.set_p_global_and_precompute_dependencies(p_val) sensitivity_solver.set_p_global_and_precompute_dependencies(p_val) + # nominal solve u_opt = ocp_solver.solve_for_x0(x0)[0] - tau_iter = ocp_solver.get_stats("qp_tau_iter") print(f"qp tau iter: {tau_iter}\n") - iterate = ocp_solver.store_iterate_to_obj() if with_parametric_constraint: lambdas = np.zeros((N_horizon-1, 2)) for i in range(1, N_horizon): lam_ = ocp_solver.get(i, "lam") lambdas[i-1] = np.array([lam_[1], lam_[3]]) - print(f"lambdas of parametric constraints: {lambdas}\n") + # print(f"lambdas of parametric constraints: {lambdas}\n") max_lam = np.max(np.abs(lambdas)) print(f"max lambda of parametric constraints: {max_lam:.2f}\n") + # transfer iterate to sensitivity solver + iterate = ocp_solver.store_iterate_to_obj() sensitivity_solver.load_iterate_from_obj(iterate) - if hp_sens_solver: - # sensitivity_solver.solve_for_x0(x0, fail_on_nonzero_status=False, print_stats_on_failure=False) - sensitivity_solver.setup_qp_matrices_and_factorize() - - else: - sensitivity_solver.solve_for_x0(x0, fail_on_nonzero_status=False, print_stats_on_failure=False) + # setup QP matrices and factorize + sensitivity_solver.setup_qp_matrices_and_factorize() - sensitivity_solver.print_statistics() qp_iter = sensitivity_solver.get_stats("qp_iter") print(f"qp iter: {qp_iter}\n") - for i in range(1, N_horizon-1): - P_mat = sensitivity_solver.get_from_qp_in(i, "P") - K_mat = sensitivity_solver.get_from_qp_in(i, "K") - Lr_mat = sensitivity_solver.get_from_qp_in(i, "Lr") - print(f"stage {i} got factorization") - # print(f"P_mat = {P_mat}") - # print(f"K_mat = {K_mat}") - print(f"Lr_mat = {Lr_mat}") + # check factorization + # for i in range(1, N_horizon-1): + # P_mat = sensitivity_solver.get_from_qp_in(i, "P") + # K_mat = sensitivity_solver.get_from_qp_in(i, "K") + # Lr_mat = sensitivity_solver.get_from_qp_in(i, "Lr") + # print(f"stage {i} got factorization") + # # print(f"P_mat = {P_mat}") + # # print(f"K_mat = {K_mat}") + # print(f"Lr_mat = {Lr_mat}") if sensitivity_solver.get_status() not in [0, 2]: breakpoint() @@ -181,7 +173,6 @@ def main(qp_solver_ric_alg: int, use_cython=False, generate_solvers=True, plot_t print(f"{adj_p=} {adj_p_ref=}") if not np.allclose(adj_p, adj_p_ref, atol=TOL): test_failure_message("adj_p and adj_p_ref should match.") - # print("ERROR: adj_p and adj_p_ref should match.") else: print("Success: adj_p and adj_p_ref match!") @@ -223,24 +214,10 @@ def main(qp_solver_ric_alg: int, use_cython=False, generate_solvers=True, plot_t else: print(f"Success: adj_p_vec and adj_p_mat[{i}, :] match!") - if plot_trajectory: - nx = ocp.dims.nx - nu = ocp.dims.nu - simX = np.zeros((N_horizon+1, nx)) - simU = np.zeros((N_horizon, nu)) - - # get solution - for i in range(N_horizon): - simX[i,:] = ocp_solver.get(i, "x") - simU[i,:] = ocp_solver.get(i, "u") - simX[N_horizon,:] = ocp_solver.get(N_horizon, "x") - - plot_pendulum(ocp.solver_options.shooting_nodes, Fmax, simU, simX, latexify=True, time_label=ocp.model.t_label, x_labels=ocp.model.x_labels, u_labels=ocp.model.u_labels) - def test_failure_message(msg): # print(f"ERROR: {msg}") raise Exception(msg) if __name__ == "__main__": - main(qp_solver_ric_alg=0, use_cython=False, generate_solvers=True, plot_trajectory=False) + main(qp_solver_ric_alg=0, generate_solvers=True, plot_trajectory=False) diff --git a/examples/acados_python/pendulum_on_cart/solution_sensitivities/test_solution_sens_and_exact_hess.py b/examples/acados_python/pendulum_on_cart/solution_sensitivities/test_solution_sens_and_exact_hess.py index a71273749d..41d021061b 100644 --- a/examples/acados_python/pendulum_on_cart/solution_sensitivities/test_solution_sens_and_exact_hess.py +++ b/examples/acados_python/pendulum_on_cart/solution_sensitivities/test_solution_sens_and_exact_hess.py @@ -114,7 +114,7 @@ def create_ocp_description(hessian_approx, linearized_dynamics=False, discrete=F ocp.solver_options.qp_solver_iter_max = 500 ocp.solver_options.nlp_solver_max_iter = 1000 if hessian_approx == 'EXACT': - ocp.solver_options.nlp_solver_step_length = 0.5 + ocp.solver_options.globalization_fixed_step_length = 0.5 ocp.solver_options.nlp_solver_max_iter = 1 ocp.solver_options.tol = 1e-14 # set prediction horizon @@ -242,14 +242,14 @@ def sensitivity_experiment(linearized_dynamics=False, discrete=False, show=True) for i, p0 in enumerate(p_vals): x0[idxp] = p0 u0 = acados_ocp_solver_gn.solve_for_x0(x0) - u0_values[i] = u0 + u0_values[i] = u0[0] du0_dp_finite_diff = np.gradient(u0_values, p_vals[1]-p_vals[0]) for i, p0 in enumerate(p_vals): x0[idxp] = p0 u0 = acados_ocp_solver_gn.solve_for_x0(x0) - acados_ocp_solver_gn.store_iterate(filename='iterate.json', overwrite=True, verbose=False) - acados_ocp_solver_exact.load_iterate(filename='iterate.json', verbose=False) + iterate = acados_ocp_solver_gn.store_iterate_to_flat_obj() + acados_ocp_solver_exact.load_iterate_from_flat_obj(iterate) acados_ocp_solver_exact.set(0, 'u', u0+1e-7) acados_ocp_solver_exact.solve_for_x0(x0, fail_on_nonzero_status=False, print_stats_on_failure=False) @@ -260,8 +260,7 @@ def sensitivity_experiment(linearized_dynamics=False, discrete=False, show=True) exact_hessian_status[i] = acados_ocp_solver_exact.get_stats('qp_stat')[-1] residuals = acados_ocp_solver_exact.get_stats("residuals") - print(f"residuals sensitivity_solver {residuals}") - + # print(f"residuals sensitivity_solver {residuals}") # solve with casadi and compare hessians nlp_sol = casadi_solver(p=x0, lbg=lbg, ubg=ubg) @@ -281,9 +280,13 @@ def sensitivity_experiment(linearized_dynamics=False, discrete=False, show=True) if max_hess_error > 1e-4: raise Exception(f"Hessian error {max_hess_error} > 1e-4 when comparing to casadi.") - solution_sens_mean_diff = np.mean(np.abs(du0_dp_values -du0_dp_finite_diff)) + solution_sens_mean_diff = np.mean(np.abs(du0_dp_values - du0_dp_finite_diff)) + solution_sens_median_diff = np.median(np.abs(du0_dp_values - du0_dp_finite_diff)) + print(f"Difference of solution sensitivity difference wrt finite differences: mean: {solution_sens_mean_diff:.2e} median: {solution_sens_median_diff:.2e}") if solution_sens_mean_diff > 1.0: raise Exception(f"Mean of solution sensitivity difference wrt finite differences {solution_sens_mean_diff} > 1.0.") + if solution_sens_median_diff > 0.1: + raise Exception(f"Median of solution sensitivity difference wrt finite differences {solution_sens_median_diff} > 0.1.") # plot_tangents(p_vals, u0_values, du0_dp_values) # Finite difference comparison @@ -392,8 +395,8 @@ def run_hessian_comparison(linearized_dynamics=False, discrete=False): casadi_hess_l = lag_hess_fun(x=nlp_sol['x'], p=x0, lam_f=1.0, lam_g=nlp_sol['lam_g'])['triu_hess_gamma_x_x'] casadi_hess = ca.triu2symm(ca.triu(casadi_hess_l)).full() acados_ocp_solver_gn.solve_for_x0(x0) - acados_ocp_solver_gn.store_iterate(filename='iterate.json', overwrite=True) - acados_ocp_solver_exact.load_iterate(filename='iterate.json') + iterate = acados_ocp_solver_gn.store_iterate_to_flat_obj() + acados_ocp_solver_exact.load_iterate_from_flat_obj(iterate) acados_ocp_solver_exact.solve_for_x0(x0, fail_on_nonzero_status=False, print_stats_on_failure=False) _ = compare_hessian(casadi_hess, acados_ocp_solver_exact) diff --git a/examples/c/no_interface_examples/mass_spring_example.c b/examples/c/no_interface_examples/mass_spring_example.c index a4d2a3da70..e14857e37b 100644 --- a/examples/c/no_interface_examples/mass_spring_example.c +++ b/examples/c/no_interface_examples/mass_spring_example.c @@ -188,9 +188,9 @@ int main() { // bool ok = false; // if (ok == true) config++; // dummy command to shut up Werror in Release -#ifdef ACADOS_WITH_QPDUNES - int clipping; -#endif +// #ifdef ACADOS_WITH_QPDUNES +// int clipping; +// #endif #ifdef SOFT_CONSTRAINTS double mu0 = 1e2; diff --git a/examples/c/no_interface_examples/mass_spring_nmpc_example.c b/examples/c/no_interface_examples/mass_spring_nmpc_example.c index 0d2985cb30..fff57f5205 100644 --- a/examples/c/no_interface_examples/mass_spring_nmpc_example.c +++ b/examples/c/no_interface_examples/mass_spring_nmpc_example.c @@ -61,6 +61,7 @@ #include "acados/ocp_nlp/ocp_nlp_constraints_bgh.h" #include "acados/ocp_nlp/ocp_nlp_reg_common.h" #include "acados/ocp_nlp/ocp_nlp_reg_noreg.h" +#include "acados/ocp_nlp/ocp_nlp_globalization_fixed_step.h" #include "acados/ocp_nlp/ocp_nlp_sqp.h" // temp From eeab1a3b78d682afe73e8f9cfd33ae203e8a8453 Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Tue, 29 Apr 2025 14:12:46 +0200 Subject: [PATCH 20/24] Fix docs (#1514) --- docs/real_world_examples/index.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/real_world_examples/index.rst b/docs/real_world_examples/index.rst index 2b03fa44c5..493717bc0c 100644 --- a/docs/real_world_examples/index.rst +++ b/docs/real_world_examples/index.rst @@ -89,6 +89,7 @@ See blog post `here `_ .. rubric:: MPC of an Autonomous Warehouse Vehicle with Tricycle Kinematic |:linked_paperclips:| `Subash et al. (2024) `_ + .. youtube:: NDta6AD5WCA :width: 50% :url_parameters: ?start=0 From 49f85de76dad2aa0d71e8378fe3ffcdb630157be Mon Sep 17 00:00:00 2001 From: Katrin Baumgaertner Date: Tue, 29 Apr 2025 15:52:10 +0200 Subject: [PATCH 21/24] Python/MATLAB: Check CasADi expressions of terminal stage (#1506) Check whether CasADi expressions of terminal stage depend on `u` or `z` + minor cleanup in `make_consistent` and code generation --- interfaces/acados_matlab_octave/AcadosOcp.m | 16 ++- .../acados_template/acados_model.py | 16 +-- .../acados_template/acados_ocp.py | 116 +++++++----------- .../casadi_function_generation.py | 63 ++++------ 4 files changed, 80 insertions(+), 131 deletions(-) diff --git a/interfaces/acados_matlab_octave/AcadosOcp.m b/interfaces/acados_matlab_octave/AcadosOcp.m index e958c4b740..36affd3fcc 100644 --- a/interfaces/acados_matlab_octave/AcadosOcp.m +++ b/interfaces/acados_matlab_octave/AcadosOcp.m @@ -859,11 +859,11 @@ function make_consistent(self, is_mocp_phase) end if opts.N_horizon == 0 - cost_types_to_check = [strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})] + cost_types_to_check = [strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})]; else cost_types_to_check = [strcmp(cost.cost_type, {'LINEAR_LS', 'NONLINEAR_LS'}) ... strcmp(cost.cost_type_0, {'LINEAR_LS', 'NONLINEAR_LS'}) ... - strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})] + strcmp(cost.cost_type_e, {'LINEAR_LS', 'NONLINEAR_LS'})]; end if (opts.as_rti_level == 1 || opts.as_rti_level == 2) && any(cost_types_to_check) error('as_rti_level in [1, 2] not supported for LINEAR_LS and NONLINEAR_LS cost type.'); @@ -984,6 +984,18 @@ function make_consistent(self, is_mocp_phase) end self.zoro_description.process(); end + + % check terminal stage + fields = {'cost_expr_ext_cost_e', 'cost_expr_ext_cost_custom_hess_e', ... + 'cost_y_expr_e', 'cost_psi_expr_e', 'cost_conl_custom_outer_hess_e', ... + 'con_h_expr_e', 'con_phi_expr_e', 'con_r_expr_e'}; + for i = 1:length(fields) + field = fields{i}; + val = model.(field); + if ~isempty(val) && (depends_on(val, model.u) || depends_on(val, model.z)) + error([field ' can not depend on u or z.']) + end + end end function [] = detect_cost_and_constraints(self) diff --git a/interfaces/acados_template/acados_template/acados_model.py b/interfaces/acados_template/acados_template/acados_model.py index 0089c73354..e0a75c3f56 100644 --- a/interfaces/acados_template/acados_template/acados_model.py +++ b/interfaces/acados_template/acados_template/acados_model.py @@ -850,31 +850,23 @@ def make_consistent(self, dims: Union[AcadosOcpDims, AcadosSimDims]) -> None: # nu if is_empty(self.u): - dims.nu = 0 self.u = casadi_symbol('u', 0, 1) - else: - dims.nu = casadi_length(self.u) + dims.nu = casadi_length(self.u) # nz if is_empty(self.z): - dims.nz = 0 self.z = casadi_symbol('z', 0, 1) - else: - dims.nz = casadi_length(self.z) + dims.nz = casadi_length(self.z) # np if is_empty(self.p): - dims.np = 0 self.p = casadi_symbol('p', 0, 1) - else: - dims.np = casadi_length(self.p) + dims.np = casadi_length(self.p) # np_global if is_empty(self.p_global): - dims.np_global = 0 self.p_global = casadi_symbol('p_global', 0, 1) - else: - dims.np_global = casadi_length(self.p_global) + dims.np_global = casadi_length(self.p_global) # sanity checks for symbol, name in [(self.x, 'x'), (self.xdot, 'xdot'), (self.u, 'u'), (self.z, 'z'), (self.p, 'p'), (self.p_global, 'p_global')]: diff --git a/interfaces/acados_template/acados_template/acados_ocp.py b/interfaces/acados_template/acados_template/acados_ocp.py index 229f0dc1c9..6d732dc845 100644 --- a/interfaces/acados_template/acados_template/acados_ocp.py +++ b/interfaces/acados_template/acados_template/acados_ocp.py @@ -330,26 +330,24 @@ def _make_consistent_cost_terminal(self): if isinstance(cost.W_e, (ca.SX, ca.MX, ca.DM)): raise Exception("W_e should be numpy array, symbolics are only supported before solver creation, to allow reformulating costs, e.g. using translate_cost_to_external_cost().") - if cost.cost_type_e == 'LINEAR_LS': ny_e = cost.W_e.shape[0] check_if_square(cost.W_e, 'W_e') - if cost.Vx_e.shape[0] != ny_e: - raise ValueError('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.' + \ - f'\nGot W_e[{cost.W_e.shape}], Vx_e[{cost.Vx_e.shape}]') - if cost.Vx_e.shape[1] != dims.nx and ny_e != 0: - raise ValueError('inconsistent dimension: Vx_e should have nx columns.') - if cost.yref_e.shape[0] != ny_e: - raise ValueError('inconsistent dimension: regarding W_e, yref_e.') dims.ny_e = ny_e - elif cost.cost_type_e == 'NONLINEAR_LS': - ny_e = cost.W_e.shape[0] - check_if_square(cost.W_e, 'W_e') - if (is_empty(model.cost_y_expr_e) and ny_e != 0) or casadi_length(model.cost_y_expr_e) != ny_e or cost.yref_e.shape[0] != ny_e: - raise ValueError('inconsistent dimension ny_e: regarding W_e, cost_y_expr.' + - f'\nGot W_e[{cost.W_e.shape}], yref_e[{cost.yref_e.shape}], ', - f'cost_y_expr_e [{casadi_length(model.cost_y_expr_e)}]\n') - dims.ny_e = ny_e + if cost.cost_type_e == 'LINEAR_LS': + if cost.Vx_e.shape[0] != ny_e: + raise ValueError('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.' + \ + f'\nGot W_e[{cost.W_e.shape}], Vx_e[{cost.Vx_e.shape}]') + if cost.Vx_e.shape[1] != dims.nx and ny_e != 0: + raise ValueError('inconsistent dimension: Vx_e should have nx columns.') + if cost.yref_e.shape[0] != ny_e: + raise ValueError('inconsistent dimension: regarding W_e, yref_e.') + + elif cost.cost_type_e == 'NONLINEAR_LS': + if (is_empty(model.cost_y_expr_e) and ny_e != 0) or casadi_length(model.cost_y_expr_e) != ny_e or cost.yref_e.shape[0] != ny_e: + raise ValueError('inconsistent dimension ny_e: regarding W_e, cost_y_expr.' + + f'\nGot W_e[{cost.W_e.shape}], yref_e[{cost.yref_e.shape}], ', + f'cost_y_expr_e [{casadi_length(model.cost_y_expr_e)}]\n') elif cost.cost_type_e == 'CONVEX_OVER_NONLINEAR': if is_empty(model.cost_y_expr_e): @@ -388,9 +386,9 @@ def _make_consistent_constraints_initial(self): return nbx_0 = constraints.idxbx_0.shape[0] + dims.nbx_0 = nbx_0 if constraints.ubx_0.shape[0] != nbx_0 or constraints.lbx_0.shape[0] != nbx_0: raise ValueError('inconsistent dimension nbx_0, regarding idxbx_0, ubx_0, lbx_0.') - dims.nbx_0 = nbx_0 if any(constraints.idxbx_0 >= dims.nx): raise ValueError(f'idxbx_0 = {constraints.idxbx_0} contains value >= nx = {dims.nx}.') @@ -404,10 +402,7 @@ def _make_consistent_constraints_initial(self): if any(constraints.idxbxe_0 >= dims.nbx_0): raise ValueError(f'idxbxe_0 = {constraints.idxbxe_0} contains value >= nbx_0 = {dims.nbx_0}.') - if not is_empty(model.con_h_expr_0): - nh_0 = casadi_length(model.con_h_expr_0) - else: - nh_0 = 0 + nh_0 = 0 if is_empty(model.con_h_expr_0) else casadi_length(model.con_h_expr_0) if constraints.uh_0.shape[0] != nh_0 or constraints.lh_0.shape[0] != nh_0: raise ValueError('inconsistent dimension nh_0, regarding lh_0, uh_0, con_h_expr_0.') @@ -505,10 +500,7 @@ def _make_consistent_constraints_terminal(self): else: dims.ng_e = ng_e - if not is_empty(model.con_h_expr_e): - nh_e = casadi_length(model.con_h_expr_e) - else: - nh_e = 0 + nh_e = 0 if is_empty(model.con_h_expr_e) else casadi_length(model.con_h_expr_e) if constraints.uh_e.shape[0] != nh_e or constraints.lh_e.shape[0] != nh_e: raise ValueError('inconsistent dimension nh_e, regarding lh_e, uh_e, con_h_expr_e.') @@ -588,22 +580,10 @@ def _make_consistent_slacks_initial(self): else: raise ValueError("Fields cost.[zl_0, zu_0, Zl_0, Zu_0] are not provided and cannot be inferred from other fields.\n") - wrong_fields = [] - if cost.Zl_0.shape[0] != ns_0: - wrong_fields += ["Zl_0"] - dim = cost.Zl_0.shape[0] - elif cost.Zu_0.shape[0] != ns_0: - wrong_fields += ["Zu_0"] - dim = cost.Zu_0.shape[0] - elif cost.zl_0.shape[0] != ns_0: - wrong_fields += ["zl_0"] - dim = cost.zl_0.shape[0] - elif cost.zu_0.shape[0] != ns_0: - wrong_fields += ["zu_0"] - dim = cost.zu_0.shape[0] - - if wrong_fields != []: - raise ValueError(f'Inconsistent size for fields {", ".join(wrong_fields)}, with dimension {dim}, \n\t' + for field in ("Zl_0", "Zu_0", "zl_0", "zu_0"): + dim = getattr(cost, field).shape[0] + if dim != ns_0: + raise Exception(f'Inconsistent size for fields {field}, with dimension {dim}, \n\t'\ + f'Detected ns_0 = {ns_0} = nsbu + nsg + nsh_0 + nsphi_0.\n\t'\ + f'With nsbu = {nsbu}, nsg = {nsg}, nsh_0 = {nsh_0}, nsphi_0 = {nsphi_0}.') dims.ns_0 = ns_0 @@ -697,24 +677,12 @@ def _make_consistent_slacks_path(self): dims.nsg = nsg ns = nsbx + nsbu + nsh + nsg + nsphi - wrong_fields = [] - if cost.Zl.shape[0] != ns: - wrong_fields += ["Zl"] - dim = cost.Zl.shape[0] - elif cost.Zu.shape[0] != ns: - wrong_fields += ["Zu"] - dim = cost.Zu.shape[0] - elif cost.zl.shape[0] != ns: - wrong_fields += ["zl"] - dim = cost.zl.shape[0] - elif cost.zu.shape[0] != ns: - wrong_fields += ["zu"] - dim = cost.zu.shape[0] - - if wrong_fields != []: - raise ValueError(f'Inconsistent size for fields {", ".join(wrong_fields)}, with dimension {dim}, \n\t' - + f'Detected ns = {ns} = nsbx + nsbu + nsg + nsh + nsphi.\n\t'\ - + f'With nsbx = {nsbx}, nsbu = {nsbu}, nsg = {nsg}, nsh = {nsh}, nsphi = {nsphi}.') + for field in ("Zl", "Zu", "zl", "zu"): + dim = getattr(cost, field).shape[0] + if dim != ns: + raise Exception(f'Inconsistent size for fields {field}, with dimension {dim}, \n\t'\ + + f'Detected ns = {ns} = nsbx + nsbu + nsg + nsh + nsphi.\n\t'\ + + f'With nsbx = {nsbx}, nsbu = {nsbu}, nsg = {nsg}, nsh = {nsh}, nsphi = {nsphi}.') dims.ns = ns @@ -788,22 +756,10 @@ def _make_consistent_slacks_terminal(self): # terminal ns_e = nsbx_e + nsh_e + nsg_e + nsphi_e - wrong_field = "" - if cost.Zl_e.shape[0] != ns_e: - wrong_field = "Zl_e" - dim = cost.Zl_e.shape[0] - elif cost.Zu_e.shape[0] != ns_e: - wrong_field = "Zu_e" - dim = cost.Zu_e.shape[0] - elif cost.zl_e.shape[0] != ns_e: - wrong_field = "zl_e" - dim = cost.zl_e.shape[0] - elif cost.zu_e.shape[0] != ns_e: - wrong_field = "zu_e" - dim = cost.zu_e.shape[0] - - if wrong_field != "": - raise ValueError(f'Inconsistent size for field {wrong_field}, with dimension {dim}, \n\t' + for field in ("Zl_e", "Zu_e", "zl_e", "zu_e"): + dim = getattr(cost, field).shape[0] + if dim != ns_e: + raise Exception(f'Inconsistent size for fields {field}, with dimension {dim}, \n\t'\ + f'Detected ns_e = {ns_e} = nsbx_e + nsg_e + nsh_e + nsphi_e.\n\t'\ + f'With nsbx_e = {nsbx_e}, nsg_e = {nsg_e}, nsh_e = {nsh_e}, nsphi_e = {nsphi_e}.') @@ -1077,6 +1033,7 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None raise ValueError('DDP only supports initial state constraints, got terminal constraints.') ddp_with_merit_or_funnel = opts.globalization == 'FUNNEL_L1PEN_LINESEARCH' or (opts.nlp_solver_type == "DDP" and opts.globalization == 'MERIT_BACKTRACKING') + # Set default parameters for globalization if opts.globalization_alpha_min is None: if ddp_with_merit_or_funnel: @@ -1135,6 +1092,15 @@ def make_consistent(self, is_mocp_phase: bool=False, verbose: bool=True) -> None # nlp_solver_warm_start_first_qp_from_nlp if opts.nlp_solver_warm_start_first_qp_from_nlp and (opts.qp_solver != "PARTIAL_CONDENSING_HPIPM" or opts.qp_solver_cond_N != opts.N_horizon): raise NotImplementedError('nlp_solver_warm_start_first_qp_from_nlp only supported for PARTIAL_CONDENSING_HPIPM with qp_solver_cond_N == N.') + + # check terminal stage + for field in ('cost_expr_ext_cost_e', 'cost_expr_ext_cost_custom_hess_e', + 'cost_y_expr_e', 'cost_psi_expr_e', 'cost_conl_custom_outer_hess_e', + 'con_h_expr_e', 'con_phi_expr_e', 'con_r_expr_e',): + val = getattr(model, field) + if not is_empty(val) and (ca.depends_on(val, model.u) or ca.depends_on(val, model.z)): + raise ValueError(f'{field} can not depend on u or z.') + return diff --git a/interfaces/acados_template/acados_template/casadi_function_generation.py b/interfaces/acados_template/acados_template/casadi_function_generation.py index 4f379376e1..940fd7b169 100644 --- a/interfaces/acados_template/acados_template/casadi_function_generation.py +++ b/interfaces/acados_template/acados_template/casadi_function_generation.py @@ -344,7 +344,6 @@ def generate_c_code_implicit_ode(context: GenerateContext, model: AcadosModel, m jac_z = ca.jacobian(f_impl, z) # Set up functions - p = model.p fun_name = model_name + '_impl_dae_fun' context.add_function_definition(fun_name, [x, xdot, u, z, t, p], [f_impl], model_dir, 'dyn') @@ -399,7 +398,7 @@ def generate_c_code_gnsf(context: GenerateContext, model: AcadosModel, model_dir x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1) z1 = symbol("gnsf_z1", gnsf_nz1, 1) dummy = symbol("gnsf_dummy", 1, 1) - empty_var = symbol("gnsf_empty_var", 0, 0) + empty_var = symbol("gnsf_empty_var", 0, 1) ## generate C code fun_name = model_name + '_gnsf_phi_fun' @@ -448,6 +447,7 @@ def generate_c_code_external_cost(context: GenerateContext, model: AcadosModel, u = model.u z = model.z p_global = model.p_global + symbol = model.get_casadi_symbol() if stage_type == 'terminal': @@ -458,14 +458,10 @@ def generate_c_code_external_cost(context: GenerateContext, model: AcadosModel, suffix_name_value_sens = "_cost_ext_cost_e_grad_p" ext_cost = model.cost_expr_ext_cost_e custom_hess = model.cost_expr_ext_cost_custom_hess_e - # Last stage cannot depend on u and z - if any(ca.which_depends(ext_cost, model.u)): - raise ValueError("terminal cost cannot depend on u.") - if any(ca.which_depends(ext_cost, model.z)): - raise ValueError("terminal cost cannot depend on z.") + # create dummy u, z - u = symbol("u", 0, 0) - z = symbol("z", 0, 0) + u = symbol("u", 0, 1) + z = symbol("z", 0, 1) elif stage_type == 'path': suffix_name = "_cost_ext_cost_fun" @@ -538,13 +534,10 @@ def generate_c_code_nls_cost(context: GenerateContext, model: AcadosModel, stage if stage_type == 'terminal': middle_name = '_cost_y_e' y_expr = model.cost_y_expr_e - if any(ca.which_depends(y_expr, model.u)): - raise ValueError("terminal cost cannot depend on u.") - if any(ca.which_depends(y_expr, model.z)): - raise ValueError("terminal cost cannot depend on z.") + # create dummy u, z - u = symbol("u", 0, 0) - z = symbol("z", 0, 0) + u = symbol("u", 0, 1) + z = symbol("z", 0, 1) elif stage_type == 'initial': middle_name = '_cost_y_0' @@ -591,13 +584,14 @@ def generate_c_code_conl_cost(context: GenerateContext, model: AcadosModel, stag opts = context.opts x = model.x + u = model.u z = model.z p = model.p p_global = model.p_global t = model.t + u = model.u symbol = model.get_casadi_symbol() - p_global = symbol('p_global', 0, 0) if stage_type == 'terminal': @@ -605,44 +599,37 @@ def generate_c_code_conl_cost(context: GenerateContext, model: AcadosModel, stag inner_expr = model.cost_y_expr_e - yref outer_expr = model.cost_psi_expr_e res_expr = model.cost_r_in_psi_expr_e + custom_hess = model.cost_conl_custom_outer_hess_e suffix_name_fun = '_conl_cost_e_fun' suffix_name_fun_jac_hess = '_conl_cost_e_fun_jac_hess' - custom_hess = model.cost_conl_custom_outer_hess_e - if any(ca.which_depends(inner_expr, model.u)): - raise ValueError("terminal cost cannot depend on u.") - if any(ca.which_depends(inner_expr, model.z)): - raise ValueError("terminal cost cannot depend on z.") # create dummy u, z - u = symbol("u", 0, 0) - z = symbol("z", 0, 0) + u = symbol("u", 0, 1) + z = symbol("z", 0, 1) + elif stage_type == 'initial': - u = model.u yref = model.cost_r_in_psi_expr_0 inner_expr = model.cost_y_expr_0 - yref outer_expr = model.cost_psi_expr_0 res_expr = model.cost_r_in_psi_expr_0 + custom_hess = model.cost_conl_custom_outer_hess_0 suffix_name_fun = '_conl_cost_0_fun' suffix_name_fun_jac_hess = '_conl_cost_0_fun_jac_hess' - custom_hess = model.cost_conl_custom_outer_hess_0 - elif stage_type == 'path': - u = model.u yref = model.cost_r_in_psi_expr inner_expr = model.cost_y_expr - yref outer_expr = model.cost_psi_expr res_expr = model.cost_r_in_psi_expr + custom_hess = model.cost_conl_custom_outer_hess suffix_name_fun = '_conl_cost_fun' suffix_name_fun_jac_hess = '_conl_cost_fun_jac_hess' - custom_hess = model.cost_conl_custom_outer_hess - # set up function names fun_name_cost_fun = model.name + suffix_name_fun fun_name_cost_fun_jac_hess = model.name + suffix_name_fun_jac_hess @@ -661,13 +648,13 @@ def generate_c_code_conl_cost(context: GenerateContext, model: AcadosModel, stag outer_hess_fun = ca.Function('outer_hess', [res_expr, t, p, p_global], [hess]) outer_hess_expr = outer_hess_fun(inner_expr, t, p, p_global) outer_hess_is_diag = outer_hess_expr.sparsity().is_diag() + if casadi_length(res_expr) <= 4: outer_hess_is_diag = 0 Jt_ux_expr = ca.jacobian(inner_expr, ca.vertcat(u, x)).T Jt_z_expr = ca.jacobian(inner_expr, z).T - # change directory cost_dir = os.path.abspath(os.path.join(opts.code_export_directory, f'{model.name}_cost')) context.add_function_definition( @@ -704,13 +691,11 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con constr_type = constraints.constr_type_e con_h_expr = model.con_h_expr_e con_phi_expr = model.con_phi_expr_e - if any(ca.which_depends(con_h_expr, model.u)) or any(ca.which_depends(con_phi_expr, model.u)): - raise ValueError("terminal constraints cannot depend on u.") - if any(ca.which_depends(con_h_expr, model.z)) or any(ca.which_depends(con_phi_expr, model.z)): - raise ValueError("terminal constraints cannot depend on z.") + # create dummy u, z - u = symbol('u', 0, 0) - z = symbol('z', 0, 0) + u = symbol('u', 0, 1) + z = symbol('z', 0, 1) + elif stage_type == 'initial': constr_type = constraints.constr_type_0 con_h_expr = model.con_h_expr_0 @@ -727,12 +712,6 @@ def generate_c_code_constraint(context: GenerateContext, model: AcadosModel, con # both empty -> nothing to generate return - if is_empty(p): - p = symbol('p', 0, 0) - - if is_empty(z): - z = symbol('z', 0, 0) - # multipliers for hessian nh = casadi_length(con_h_expr) lam_h = symbol('lam_h', nh, 1) From b4bf5cd640cd8e55863e41e06509435025f9c20b Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Tue, 29 Apr 2025 15:56:45 +0200 Subject: [PATCH 22/24] Test forward vs. adjoint solution sensitivities on CI with quick settings (#1515) --- .github/workflows/full_build.yml | 3 + .../solution_sensitivity_example.py | 190 +++++++++++------- 2 files changed, 118 insertions(+), 75 deletions(-) diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 8b7175e517..99ed1ac620 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -211,6 +211,9 @@ jobs: python ocp_example_cost_formulations.py cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart python example_solution_sens_closed_loop.py + cd ${{runner.workspace}}/acados/examples/acados_python/chain_mass/ + python solution_sensitivity_example.py + - name: Python Furuta pendulum timeout test diff --git a/examples/acados_python/chain_mass/solution_sensitivity_example.py b/examples/acados_python/chain_mass/solution_sensitivity_example.py index 77b1bbd4e6..5f68ad44e1 100644 --- a/examples/acados_python/chain_mass/solution_sensitivity_example.py +++ b/examples/acados_python/chain_mass/solution_sensitivity_example.py @@ -42,7 +42,7 @@ import matplotlib.pyplot as plt from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver from utils import get_chain_params -from typing import Tuple +from typing import Tuple, Optional from plot_utils import plot_timings import time @@ -259,6 +259,7 @@ def export_parametric_ocp( nlp_iter: int = 50, nlp_tol: float = 1e-5, random_scale: dict = {"m": 0.0, "D": 0.0, "L": 0.0, "C": 0.0}, + ext_fun_compile_flags: Optional[str] = None, ) -> Tuple[AcadosOcp, DMStruct]: # create ocp object to formulate the OCP ocp = AcadosOcp() @@ -362,6 +363,8 @@ def export_parametric_ocp( ocp.solver_options.tol = nlp_tol ocp.solver_options.tf = ocp.solver_options.N_horizon * chain_params_["Ts"] + if ext_fun_compile_flags is not None: + ocp.solver_options.ext_fun_compile_flags = ext_fun_compile_flags return ocp, p @@ -370,12 +373,17 @@ def main_parametric(qp_solver_ric_alg: int = 0, discrete_dyn_type: str = "RK4", chain_params_: dict = get_chain_params(), with_more_adjoints = True, - generate_code: bool = True) -> None: + generate_code: bool = True, + ext_fun_compile_flags: Optional[str] = None, + np_test: int = 20, + generate_plots: bool = True, + ) -> None: if discrete_dyn_type == "EULER": print("Warning: OCP solver does not converge with EULER integrator.") ocp, parameter_values = export_parametric_ocp( chain_params_=chain_params_, qp_solver_ric_alg=qp_solver_ric_alg, integrator_type="DISCRETE", - discrete_dyn_type=discrete_dyn_type + discrete_dyn_type=discrete_dyn_type, + ext_fun_compile_flags=ext_fun_compile_flags, ) ocp_json_file = "acados_ocp_" + ocp.model.name + ".json" @@ -390,6 +398,8 @@ def main_parametric(qp_solver_ric_alg: int = 0, qp_solver_ric_alg=qp_solver_ric_alg, hessian_approx="EXACT", integrator_type="DISCRETE", + discrete_dyn_type=discrete_dyn_type, + ext_fun_compile_flags=ext_fun_compile_flags, ) sensitivity_ocp.model.name = f"{ocp.model.name}_sensitivity" @@ -410,8 +420,6 @@ def main_parametric(qp_solver_ric_alg: int = 0, nx = ocp.dims.nx nu = ocp.dims.nu - np_test = 20 - # p_label = "L_2_0" # p_label = "D_2_0" p_label = f"C_{M}_0" @@ -505,9 +513,15 @@ def main_parametric(qp_solver_ric_alg: int = 0, timings_solve_params_adj[i] = sensitivity_solver.get_stats("time_solution_sens_solve") sens_adj_ref = seed_u.T @ sens_u_ + seed_x.T @ sens_x_ + # print(f"{sens_adj_ref=}") + # print(f"{sens_adj=}") + # print(f"{sens_u_=}") + # print(f"{sens_x_=}") - assert np.allclose(sens_adj_ref.ravel(), sens_adj) - # print(np.abs(sens_adj_ref.ravel() - sens_adj)) + test_tol = 1e-5 + diff_sens_adj_vs_ref = np.max(np.abs(sens_adj_ref.ravel() - sens_adj)) + if diff_sens_adj_vs_ref > test_tol: + raise_test_failure_message(f"diff_sens_adj_vs_ref = {diff_sens_adj_vs_ref} should be < {test_tol}") sens_u.append(sens_u_[:, p_idx]) @@ -523,79 +537,80 @@ def main_parametric(qp_solver_ric_alg: int = 0, print(f"i {i} {timings_solve_params_adj[i]*1e3:.5f} \t {timings_solve_params[i]*1e3:.5f} \t {timings_solve_params_adj_uforw[i]*1e3:.5f} \t {timings_solve_params_adj_all_primals[i]*1e3:.5f}") # check wrt forward - assert np.allclose(sens_adj, out_dict['sens_u']) - - timings_common = { - "NLP solve (S1)": timings_solve_ocp_solver * 1e3, - "store \& load iterates": timings_store_load * 1e3, - "parameter update": timings_parameter_update * 1e3, - "setup exact Lagrange Hessian (S2)": timings_lin_exact_hessian_qp * 1e3, - "factorize exact Lagrange Hessian (S3)": timings_lin_and_factorize * 1e3, - r"evaluate $J_\star$ (S4)": timings_lin_params * 1e3, - } - timing_results_forward = timings_common.copy() - timing_results_adjoint = timings_common.copy() - timing_results_adj_uforw = timings_common.copy() - timing_results_adj_all_primals = timings_common.copy() - - backsolve_label = "sensitivity solve given factorization (S5)" - timing_results_forward[backsolve_label] = timings_solve_params * 1e3 - timing_results_adjoint[backsolve_label] = timings_solve_params_adj * 1e3 - - timings_list = [timing_results_forward, timing_results_adjoint] - labels = [r'$\frac{\partial w^\star}{\partial \theta}$ via forward', r'$\nu^\top \frac{\partial w^\star}{\partial \theta}$ via adjoint'] + print(np.abs(sens_adj- out_dict['sens_u'])) + # assert np.allclose(sens_adj, out_dict['sens_u']) + + if generate_plots: + timings_common = { + "NLP solve (S1)": timings_solve_ocp_solver * 1e3, + "store \& load iterates": timings_store_load * 1e3, + "parameter update": timings_parameter_update * 1e3, + "setup exact Lagrange Hessian (S2)": timings_lin_exact_hessian_qp * 1e3, + "factorize exact Lagrange Hessian (S3)": timings_lin_and_factorize * 1e3, + r"evaluate $J_\star$ (S4)": timings_lin_params * 1e3, + } + timing_results_forward = timings_common.copy() + timing_results_adjoint = timings_common.copy() + timing_results_adj_uforw = timings_common.copy() + timing_results_adj_all_primals = timings_common.copy() + + backsolve_label = "sensitivity solve given factorization (S5)" + timing_results_forward[backsolve_label] = timings_solve_params * 1e3 + timing_results_adjoint[backsolve_label] = timings_solve_params_adj * 1e3 + + timings_list = [timing_results_forward, timing_results_adjoint] + labels = [r'$\frac{\partial w^\star}{\partial \theta}$ via forward', r'$\nu^\top \frac{\partial w^\star}{\partial \theta}$ via adjoint'] - if with_more_adjoints: - timing_results_adj_uforw[backsolve_label] = timings_solve_params_adj_uforw * 1e3 - timing_results_adj_all_primals[backsolve_label] = timings_solve_params_adj_all_primals * 1e3 - timings_list += [timing_results_adj_uforw, timing_results_adj_all_primals] - labels += [r'$\frac{\partial u_0^\star}{\partial \theta}$ via adjoints', r'$\frac{\partial z^\star}{\partial \theta} $ via adjoints'] - - - print_timings(timing_results_forward, metric="median") - print_timings(timing_results_forward, metric="min") - - u_opt = np.vstack(u_opt) - sens_u = np.vstack(sens_u) - - # Compare to numerical gradients - sens_u_fd = np.gradient(u_opt, p_var, axis=0) - u_opt_reconstructed_fd = np.cumsum(sens_u_fd, axis=0) * delta_p + u_opt[0, :] - u_opt_reconstructed_fd += u_opt[0, :] - u_opt_reconstructed_fd[0, :] - - u_opt_reconstructed_acados = np.cumsum(sens_u, axis=0) * delta_p + u_opt[0, :] - u_opt_reconstructed_acados += u_opt[0, :] - u_opt_reconstructed_acados[0, :] - - # TODO move to plot utils - plt.figure(figsize=(7, 7)) - for col in range(3): - plt.subplot(4, 1, col + 1) - plt.plot(p_var, u_opt[:, col], label=f"$u^*_{col}$") - plt.plot(p_var, u_opt_reconstructed_fd[:, col], label=f"$u^*_{col}$, reconstructed with fd gradients", linestyle="--") - plt.plot( - p_var, u_opt_reconstructed_acados[:, col], label=f"$u^*_{col}$, reconstructed with acados gradients", linestyle=":" - ) - plt.ylabel(f"$u^*_{col}$") + if with_more_adjoints: + timing_results_adj_uforw[backsolve_label] = timings_solve_params_adj_uforw * 1e3 + timing_results_adj_all_primals[backsolve_label] = timings_solve_params_adj_all_primals * 1e3 + timings_list += [timing_results_adj_uforw, timing_results_adj_all_primals] + labels += [r'$\frac{\partial u_0^\star}{\partial \theta}$ via adjoints', r'$\frac{\partial z^\star}{\partial \theta} $ via adjoints'] + + + print_timings(timing_results_forward, metric="median") + print_timings(timing_results_forward, metric="min") + + u_opt = np.vstack(u_opt) + sens_u = np.vstack(sens_u) + + # Compare to numerical gradients + sens_u_fd = np.gradient(u_opt, p_var, axis=0) + u_opt_reconstructed_fd = np.cumsum(sens_u_fd, axis=0) * delta_p + u_opt[0, :] + u_opt_reconstructed_fd += u_opt[0, :] - u_opt_reconstructed_fd[0, :] + + u_opt_reconstructed_acados = np.cumsum(sens_u, axis=0) * delta_p + u_opt[0, :] + u_opt_reconstructed_acados += u_opt[0, :] - u_opt_reconstructed_acados[0, :] + + plt.figure(figsize=(7, 7)) + for col in range(3): + plt.subplot(4, 1, col + 1) + plt.plot(p_var, u_opt[:, col], label=f"$u^*_{col}$") + plt.plot(p_var, u_opt_reconstructed_fd[:, col], label=f"$u^*_{col}$, reconstructed with fd gradients", linestyle="--") + plt.plot( + p_var, u_opt_reconstructed_acados[:, col], label=f"$u^*_{col}$, reconstructed with acados gradients", linestyle=":" + ) + plt.ylabel(f"$u^*_{col}$") + plt.grid(True) + plt.legend() + plt.xlim(p_var[0], p_var[-1]) + + for col in range(3): + plt.subplot(4, 1, 4) + plt.plot(p_var, np.abs(sens_u[:, col] - sens_u_fd[:, col]), label=f"$u^*_{col}$", linestyle="--") + + plt.ylabel("abs difference") plt.grid(True) plt.legend() + plt.yscale("log") + plt.xlabel(p_label) plt.xlim(p_var[0], p_var[-1]) + plt.tight_layout() + plt.savefig("chain_adj_fwd_sens.pdf") - for col in range(3): - plt.subplot(4, 1, 4) - plt.plot(p_var, np.abs(sens_u[:, col] - sens_u_fd[:, col]), label=f"$u^*_{col}$", linestyle="--") + plot_timings(timings_list, labels, figure_filename="timing_adj_fwd_sens_chain.png", t_max=10, horizontal=True, figsize=(12, 3), with_patterns=True) - plt.ylabel("abs difference") - plt.grid(True) - plt.legend() - plt.yscale("log") - plt.xlabel(p_label) - plt.xlim(p_var[0], p_var[-1]) - plt.tight_layout() - plt.savefig("chain_adj_fwd_sens.pdf") - - plot_timings(timings_list, labels, figure_filename="timing_adj_fwd_sens_chain.png", t_max=10, horizontal=True, figsize=(12, 3), with_patterns=True) - - plt.show() + plt.show() def print_timings(timing_results: dict, metric: str = "median"): @@ -612,7 +627,32 @@ def print_timings(timing_results: dict, metric: str = "median"): for key, value in timing_results.items(): print(f"{key}: {timing_func(value):.3f} ms") -if __name__ == "__main__": +def raise_test_failure_message(msg: str): + # print(f"ERROR: {msg}") + raise Exception(msg) + +def main_test(): + chain_params = get_chain_params() + chain_params['N'] = 4 + chain_params["n_mass"] = 3 + chain_params["Ts"] = 0.01 + main_parametric(qp_solver_ric_alg=0, + discrete_dyn_type="EULER", + chain_params_=chain_params, + generate_code=True, + with_more_adjoints=False, + ext_fun_compile_flags="", + np_test=2, + generate_plots=False, + ) + +def main_experiment(): chain_params = get_chain_params() chain_params["n_mass"] = 3 main_parametric(qp_solver_ric_alg=0, discrete_dyn_type="RK4", chain_params_=chain_params, generate_code=True, with_more_adjoints=True) + +if __name__ == "__main__": + # use settings for fast testing -> test with this on CI + main_test() + # use settings for full experiment + # main_experiment() From 1bbdd66df2e93eca7b8012d18dfdf6a07852486d Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 2 May 2025 10:47:06 +0200 Subject: [PATCH 23/24] Ignore sim stuff in build templates if N==0 (#1516) --- .../c_templates_tera/CMakeLists.in.txt | 22 +++++++++------ .../c_templates_tera/Makefile.in | 27 +++++++++++++------ 2 files changed, 33 insertions(+), 16 deletions(-) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt index d9c4d56609..7617c5350e 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt +++ b/interfaces/acados_template/acados_template/c_templates_tera/CMakeLists.in.txt @@ -34,6 +34,12 @@ {%- set qp_solver = "FULL_CONDENSING_HPIPM" %} {%- endif %} +{%- if solver_options.N_horizon %} + {%- set N_horizon = solver_options.N_horizon %} +{%- else %} + {%- set N_horizon = 1 %} +{%- endif %} + {%- if solver_options.hessian_approx %} {%- set hessian_approx = solver_options.hessian_approx %} {%- elif solver_options.sens_hess %} @@ -83,7 +89,7 @@ option(BUILD_EXAMPLE "Should the example main_{{ model.name }} be build?" OFF) {%- endif %} -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} option(BUILD_SIM_EXAMPLE "Should the simulation example main_sim_{{ model.name }} be build?" OFF) option(BUILD_ACADOS_SIM_SOLVER_LIB "Should the simulation solver library acados_sim_solver_{{ model.name }} be build?" OFF) {%- endif %} @@ -112,7 +118,7 @@ endif() set(MODEL_OBJ model_{{ model.name }}) {%- endif %} set(OCP_OBJ ocp_{{ model.name }}) -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} set(SIM_OBJ sim_{{ model.name }}) {%- endif %} @@ -144,10 +150,10 @@ if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMP endif() {%- endif %} -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} # for sim solver if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_EXAMPLE} - {%- if solver_options.integrator_type != "DISCRETE" %} + {%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} OR ${BUILD_SIM_EXAMPLE} OR ${BUILD_ACADOS_SIM_SOLVER_LIB} {%- endif -%} ) @@ -231,7 +237,7 @@ if(${BUILD_ACADOS_SOLVER_LIB}) $ {%- endif %} $ - {%- if solver_options.integrator_type != "DISCRETE" %} + {%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} $ {%- endif -%} ) @@ -259,7 +265,7 @@ if(${BUILD_EXAMPLE}) $ {%- endif %} $ - {%- if solver_options.integrator_type != "DISCRETE" %} + {%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} $ {%- endif -%} ) @@ -267,7 +273,7 @@ if(${BUILD_EXAMPLE}) endif(${BUILD_EXAMPLE}) {%- endif %} -{% if solver_options.integrator_type != "DISCRETE" -%} +{% if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 -%} # example_sim if(${BUILD_SIM_EXAMPLE}) set(EX_SIM_SRC main_sim_{{ model.name }}.c) @@ -290,7 +296,7 @@ unset(BUILD_ACADOS_SOLVER_LIB CACHE) unset(BUILD_ACADOS_OCP_SOLVER_LIB CACHE) unset(BUILD_EXAMPLE CACHE) {%- endif %} -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} unset(BUILD_SIM_EXAMPLE CACHE) unset(BUILD_ACADOS_SIM_SOLVER_LIB CACHE) {%- endif %} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in index fbfda16be8..7f074d2010 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in +++ b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in @@ -35,6 +35,12 @@ {%- set qp_solver = "FULL_CONDENSING_HPIPM" %} {%- endif %} +{%- if solver_options.N_horizon %} + {%- set N_horizon = solver_options.N_horizon %} +{%- else %} + {%- set N_horizon = 1 %} +{%- endif %} + {%- if solver_options.hessian_approx %} {%- set hessian_approx = solver_options.hessian_approx %} {%- elif solver_options.sens_hess %} @@ -104,7 +110,7 @@ OCP_OBJ := $(OCP_SRC:.c=.o) {%- endif %} -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} # for sim solver SIM_SRC= acados_sim_solver_{{ model.name }}.c SIM_OBJ := $(SIM_SRC:.c=.o) @@ -126,7 +132,7 @@ EX_EXE := $(EX_SRC:.c=) # combine model, (potentially) sim and ocp object files OBJ= OBJ+= $(MODEL_OBJ) -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} OBJ+= $(SIM_OBJ) {%- endif %} @@ -194,7 +200,7 @@ LDLIBS+= {{ link_libs }} # libraries LIBACADOS_SOLVER=libacados_solver_{{ model.name }}{{ shared_lib_ext }} LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c={{ shared_lib_ext }}) {%- endif %} @@ -206,8 +212,13 @@ LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c={{ shared_lib_ext }}) all: clean example shared_lib: ocp_shared_lib {%- else %} + {% if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 -%} all: clean example_sim example shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib + {%-else %} +all: clean example +shared_lib: bundled_shared_lib ocp_shared_lib + {%- endif %} {%- endif %} {%-else %} all: clean example_sim @@ -221,7 +232,7 @@ example: $(EX_OBJ) $(OBJ) $(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS) {%- endif %} -{% if solver_options.integrator_type != "DISCRETE" -%} +{% if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 -%} example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) $(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS) @@ -268,7 +279,7 @@ ocp_cython: ocp_cython_o {%- endif %} -{% if solver_options.integrator_type != "DISCRETE" -%} +{% if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 -%} # Sim Cython targets sim_cython_c: sim_shared_lib cython \ @@ -318,7 +329,7 @@ clean_ocp_cython: del \Q acados_ocp_solver_pyx.o 2>nul {%- endif %} -{% if solver_options.integrator_type != "DISCRETE" -%} +{% if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 -%} clean_sim_cython: del \Q libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }} 2>nul del \Q acados_sim_solver_{{ model.name }}.o 2>nul @@ -329,7 +340,7 @@ clean_sim_cython: {%- else %} clean: -{%- if solver_options.integrator_type != "DISCRETE" %} +{%- if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 %} $(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ) $(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER) $(RM) $(EX_EXE) $(EX_SIM_EXE) @@ -351,7 +362,7 @@ clean_ocp_cython: $(RM) acados_ocp_solver_pyx.o {%- endif %} -{% if solver_options.integrator_type != "DISCRETE" -%} +{% if solver_options.integrator_type != "DISCRETE" and N_horizon > 0 -%} clean_sim_cython: $(RM) libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }} $(RM) acados_sim_solver_{{ model.name }}.o From fd231104913cc9c305e0dff736281b91dc1dbc5b Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 2 May 2025 12:00:03 +0200 Subject: [PATCH 24/24] Updates with respect to new HPIPM sensitivity API (#1512) --- .github/workflows/full_build.yml | 1 + acados/dense_qp/dense_qp_common.c | 30 +++++++ acados/dense_qp/dense_qp_common.h | 11 ++- acados/dense_qp/dense_qp_daqp.c | 8 +- acados/dense_qp/dense_qp_daqp.h | 2 - acados/dense_qp/dense_qp_hpipm.c | 10 +-- acados/dense_qp/dense_qp_hpipm.h | 2 - acados/dense_qp/dense_qp_ooqp.c | 7 +- acados/dense_qp/dense_qp_ooqp.h | 2 - acados/dense_qp/dense_qp_qore.c | 6 +- acados/dense_qp/dense_qp_qore.h | 2 +- acados/dense_qp/dense_qp_qpoases.c | 8 +- acados/dense_qp/dense_qp_qpoases.h | 2 - acados/ocp_nlp/ocp_nlp_common.c | 86 ++++++++++--------- acados/ocp_nlp/ocp_nlp_common.h | 3 + acados/ocp_qp/ocp_qp_common.c | 27 ++++++ acados/ocp_qp/ocp_qp_common.h | 13 ++- acados/ocp_qp/ocp_qp_full_condensing.c | 72 ++++++++++++++-- acados/ocp_qp/ocp_qp_full_condensing.h | 3 + acados/ocp_qp/ocp_qp_hpipm.c | 10 +-- acados/ocp_qp/ocp_qp_hpipm.h | 2 - acados/ocp_qp/ocp_qp_hpmpc.c | 6 +- acados/ocp_qp/ocp_qp_hpmpc.h | 2 +- acados/ocp_qp/ocp_qp_ooqp.c | 6 +- acados/ocp_qp/ocp_qp_ooqp.h | 2 - acados/ocp_qp/ocp_qp_osqp.c | 8 +- acados/ocp_qp/ocp_qp_osqp.h | 2 - acados/ocp_qp/ocp_qp_partial_condensing.c | 75 +++++++++++++++- acados/ocp_qp/ocp_qp_partial_condensing.h | 3 + acados/ocp_qp/ocp_qp_qpdunes.c | 8 +- acados/ocp_qp/ocp_qp_qpdunes.h | 2 - acados/ocp_qp/ocp_qp_xcond_solver.c | 19 ++-- acados/ocp_qp/ocp_qp_xcond_solver.h | 5 +- .../solution_sensitivity_example.py | 14 ++- external/hpipm | 2 +- 35 files changed, 337 insertions(+), 124 deletions(-) diff --git a/.github/workflows/full_build.yml b/.github/workflows/full_build.yml index 99ed1ac620..5962663278 100644 --- a/.github/workflows/full_build.yml +++ b/.github/workflows/full_build.yml @@ -202,6 +202,7 @@ jobs: python policy_gradient_example.py python test_solution_sens_and_exact_hess.py python forw_vs_adj_param_sens.py + python smooth_policy_gradients.py cd ${{runner.workspace}}/acados/examples/acados_python/solution_sensitivities_convex_example python value_gradient_example_linear.py python batch_adjoint_solution_sensitivity_example.py diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index 8ee3067889..705f621e89 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -41,6 +41,7 @@ #include "hpipm/include/hpipm_d_dense_qp_ipm.h" #include "hpipm/include/hpipm_d_dense_qp_kkt.h" #include "hpipm/include/hpipm_d_dense_qp_res.h" +#include "hpipm/include/hpipm_d_dense_qp_seed.h" #include "hpipm/include/hpipm_d_dense_qp_sol.h" #include "hpipm/include/hpipm_d_dense_qp_dim.h" @@ -228,6 +229,35 @@ void dense_qp_out_get(dense_qp_out *out, const char *field, void *value) return; } +/************************************************ + * seed + ************************************************/ + +acados_size_t dense_qp_seed_calculate_size(dense_qp_dims *dims) +{ + acados_size_t size = sizeof(dense_qp_seed); + size += d_dense_qp_seed_memsize(dims); + + make_int_multiple_of(8, &size); + return size; +} + + + +dense_qp_seed *dense_qp_seed_assign(dense_qp_dims *dims, void *raw_memory) +{ + char *c_ptr = (char *) raw_memory; + + dense_qp_seed *qp_seed = (dense_qp_seed *) c_ptr; + c_ptr += sizeof(dense_qp_seed); + + align_char_to(8, &c_ptr); + d_dense_qp_seed_create(dims, qp_seed, c_ptr); + c_ptr += d_dense_qp_seed_memsize(dims); + assert((char *) raw_memory + dense_qp_seed_calculate_size(dims) >= c_ptr); + + return qp_seed; +} /************************************************ diff --git a/acados/dense_qp/dense_qp_common.h b/acados/dense_qp/dense_qp_common.h index 54c742a71b..42ea02d69f 100644 --- a/acados/dense_qp/dense_qp_common.h +++ b/acados/dense_qp/dense_qp_common.h @@ -48,6 +48,7 @@ typedef struct d_dense_qp dense_qp_in; typedef struct d_dense_qp_sol dense_qp_out; typedef struct d_dense_qp_res dense_qp_res; typedef struct d_dense_qp_res_ws dense_qp_res_ws; +typedef struct d_dense_qp_seed dense_qp_seed; @@ -69,8 +70,8 @@ typedef struct int (*evaluate)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); void (*solver_get)(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); void (*memory_reset)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); - void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); - void (*eval_adj_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); + void (*eval_forw_sens)(void *config, void *qp_in, void *seed, void *qp_out, void *opts, void *mem, void *work); + void (*eval_adj_sens)(void *config, void *qp_in, void *seed, void *qp_out, void *opts, void *mem, void *work); void (*terminate)(void *config, void *mem, void *work); } qp_solver_config; #endif @@ -137,6 +138,12 @@ void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res // void dense_qp_res_compute_nrm_inf(dense_qp_res *qp_res, double res[4]); +/* seed */ +// +acados_size_t dense_qp_seed_calculate_size(dense_qp_dims *dims); +// +dense_qp_seed *dense_qp_seed_assign(dense_qp_dims *dims, void *raw_memory); + /* misc */ // void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out); diff --git a/acados/dense_qp/dense_qp_daqp.c b/acados/dense_qp/dense_qp_daqp.c index ed22c6f7f2..22f8adee29 100644 --- a/acados/dense_qp/dense_qp_daqp.c +++ b/acados/dense_qp/dense_qp_daqp.c @@ -781,14 +781,14 @@ int dense_qp_daqp(void* config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void } -void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void dense_qp_daqp_eval_forw_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: dense_qp_daqp_eval_sens: not implemented yet\n"); + printf("\nerror: dense_qp_daqp_eval_forw_sens: not implemented yet\n"); exit(1); } -void dense_qp_daqp_eval_adj_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void dense_qp_daqp_eval_adj_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { printf("\nerror: dense_qp_daqp_eval_adj_sens: not implemented yet\n"); exit(1); @@ -834,7 +834,7 @@ void dense_qp_daqp_config_initialize_default(void *config_) config->memory_get = &dense_qp_daqp_memory_get; config->workspace_calculate_size = (acados_size_t (*)(void *, void *, void *)) & dense_qp_daqp_workspace_calculate_size; - config->eval_sens = &dense_qp_daqp_eval_sens; + config->eval_forw_sens = &dense_qp_daqp_eval_forw_sens; config->eval_adj_sens = &dense_qp_daqp_eval_adj_sens; config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & dense_qp_daqp; config->memory_reset = &dense_qp_daqp_memory_reset; diff --git a/acados/dense_qp/dense_qp_daqp.h b/acados/dense_qp/dense_qp_daqp.h index 4b0503b77f..425c734a4a 100644 --- a/acados/dense_qp/dense_qp_daqp.h +++ b/acados/dense_qp/dense_qp_daqp.h @@ -97,8 +97,6 @@ void *dense_qp_daqp_memory_assign(void *config, dense_qp_dims *dims, void *opts_ // functions int dense_qp_daqp(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); // -void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void dense_qp_daqp_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); // void dense_qp_daqp_config_initialize_default(void *config_); diff --git a/acados/dense_qp/dense_qp_hpipm.c b/acados/dense_qp/dense_qp_hpipm.c index 97171a296d..2e5b7d1027 100644 --- a/acados/dense_qp/dense_qp_hpipm.c +++ b/acados/dense_qp/dense_qp_hpipm.c @@ -320,7 +320,7 @@ int dense_qp_hpipm(void *config, void *qp_in_, void *qp_out_, void *opts_, void -void dense_qp_hpipm_eval_sens(void *config_, void *param_qp_in_, void *sens_qp_out_, void *opts_, void *mem_, void *work_) +void dense_qp_hpipm_eval_forw_sens(void *config_, void *param_qp_in_, void *seed, void *sens_qp_out_, void *opts_, void *mem_, void *work_) { dense_qp_in *param_qp_in = param_qp_in_; dense_qp_out *sens_qp_out = sens_qp_out_; @@ -328,13 +328,13 @@ void dense_qp_hpipm_eval_sens(void *config_, void *param_qp_in_, void *sens_qp_o dense_qp_hpipm_opts *opts = opts_; dense_qp_hpipm_memory *memory = mem_; - d_dense_qp_ipm_sens(param_qp_in, sens_qp_out, opts->hpipm_opts, memory->hpipm_workspace); + d_dense_qp_ipm_sens_frw(param_qp_in, seed, sens_qp_out, opts->hpipm_opts, memory->hpipm_workspace); return; } -void dense_qp_hpipm_eval_adj_sens(void *config_, void *param_qp_in_, void *sens_qp_out_, void *opts_, void *mem_, void *work_) +void dense_qp_hpipm_eval_adj_sens(void *config_, void *param_qp_in_, void *seed, void *sens_qp_out_, void *opts_, void *mem_, void *work_) { dense_qp_in *param_qp_in = param_qp_in_; dense_qp_out *sens_qp_out = sens_qp_out_; @@ -342,7 +342,7 @@ void dense_qp_hpipm_eval_adj_sens(void *config_, void *param_qp_in_, void *sens_ dense_qp_hpipm_opts *opts = opts_; dense_qp_hpipm_memory *memory = mem_; - d_dense_qp_ipm_sens_adj(param_qp_in, sens_qp_out, opts->hpipm_opts, memory->hpipm_workspace); + d_dense_qp_ipm_sens_adj(param_qp_in, seed, sens_qp_out, opts->hpipm_opts, memory->hpipm_workspace); return; } @@ -384,7 +384,7 @@ void dense_qp_hpipm_config_initialize_default(void *config_) config->memory_get = &dense_qp_hpipm_memory_get; config->workspace_calculate_size = &dense_qp_hpipm_workspace_calculate_size; config->evaluate = &dense_qp_hpipm; - config->eval_sens = &dense_qp_hpipm_eval_sens; + config->eval_forw_sens = &dense_qp_hpipm_eval_forw_sens; config->eval_adj_sens = &dense_qp_hpipm_eval_adj_sens; config->memory_reset = &dense_qp_hpipm_memory_reset; config->solver_get = &dense_qp_hpipm_solver_get; diff --git a/acados/dense_qp/dense_qp_hpipm.h b/acados/dense_qp/dense_qp_hpipm.h index 7d05fe5138..7d94d7f2e0 100644 --- a/acados/dense_qp/dense_qp_hpipm.h +++ b/acados/dense_qp/dense_qp_hpipm.h @@ -81,8 +81,6 @@ acados_size_t dense_qp_hpipm_calculate_workspace_size(void *dims, void *opts_); // int dense_qp_hpipm(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); // -void dense_qp_hpipm_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void dense_qp_hpipm_config_initialize_default(void *config_); // void dense_qp_hpipm_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); diff --git a/acados/dense_qp/dense_qp_ooqp.c b/acados/dense_qp/dense_qp_ooqp.c index b25497a946..42e0c8652f 100644 --- a/acados/dense_qp/dense_qp_ooqp.c +++ b/acados/dense_qp/dense_qp_ooqp.c @@ -626,10 +626,9 @@ void dense_qp_ooqp_destroy(void *mem_, void *work) } - -void dense_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void dense_qp_ooqp_eval_forw_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: dense_qp_ooqp_eval_sens: not implemented yet\n"); + printf("\nerror: dense_qp_ooqp_eval_forw_sens: not implemented yet\n"); exit(1); } @@ -678,7 +677,7 @@ void dense_qp_ooqp_config_initialize_default(void *config_) config->workspace_calculate_size = (acados_size_t (*)(void *, void *, void *)) & dense_qp_ooqp_workspace_calculate_size; config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & dense_qp_ooqp; - config->eval_sens = &dense_qp_ooqp_eval_sens; + config->eval_forw_sens = &dense_qp_ooqp_eval_forw_sens; config->eval_adj_sens = &dense_qp_ooqp_eval_adj_sens; config->memory_reset = &dense_qp_ooqp_memory_reset; config->solver_get = &dense_qp_ooqp_solver_get; diff --git a/acados/dense_qp/dense_qp_ooqp.h b/acados/dense_qp/dense_qp_ooqp.h index dff92f3cf5..37a8628658 100644 --- a/acados/dense_qp/dense_qp_ooqp.h +++ b/acados/dense_qp/dense_qp_ooqp.h @@ -114,8 +114,6 @@ int dense_qp_ooqp(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void // void dense_qp_ooqp_destroy(void *mem_, void *work); // -void dense_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void dense_qp_ooqp_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); // void dense_qp_ooqp_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); diff --git a/acados/dense_qp/dense_qp_qore.c b/acados/dense_qp/dense_qp_qore.c index 3da2c73909..f4eca21c0d 100644 --- a/acados/dense_qp/dense_qp_qore.c +++ b/acados/dense_qp/dense_qp_qore.c @@ -563,9 +563,9 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void -void dense_qp_qore_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void dense_qp_qore_eval_forw_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: dense_qp_qore_eval_sens: not implemented yet\n"); + printf("\nerror: dense_qp_qore_eval_forw_sens: not implemented yet\n"); exit(1); } @@ -612,7 +612,7 @@ void dense_qp_qore_config_initialize_default(void *config_) config->workspace_calculate_size = (acados_size_t (*)(void *, void *, void *)) & dense_qp_qore_workspace_calculate_size; config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & dense_qp_qore; - config->eval_sens = &dense_qp_qore_eval_sens; + config->eval_forw_sens = &dense_qp_qore_eval_forw_sens; config->eval_adj_sens = &dense_qp_qore_eval_adj_sens; config->memory_reset = &dense_qp_qore_memory_reset; config->solver_get = &dense_qp_qore_solver_get; diff --git a/acados/dense_qp/dense_qp_qore.h b/acados/dense_qp/dense_qp_qore.h index cc19476509..5569aa0a0e 100644 --- a/acados/dense_qp/dense_qp_qore.h +++ b/acados/dense_qp/dense_qp_qore.h @@ -113,7 +113,7 @@ acados_size_t dense_qp_qore_workspace_calculate_size(void *config, dense_qp_dims // int dense_qp_qore(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); // -void dense_qp_qore_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +void dense_qp_qore_eval_forw_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); // void dense_qp_qore_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); // diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index f7ac8355c4..0a15bbe601 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -768,14 +768,14 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo -void dense_qp_qpoases_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void dense_qp_qpoases_eval_forw_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: dense_qp_qpoases_eval_sens: not implemented yet\n"); + printf("\nerror: dense_qp_qpoases_eval_forw_sens: not implemented yet\n"); exit(1); } -void dense_qp_qpoases_eval_adj_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void dense_qp_qpoases_eval_adj_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { printf("\nerror: dense_qp_qpoases_eval_adj_sens: not implemented yet\n"); exit(1); @@ -819,7 +819,7 @@ void dense_qp_qpoases_config_initialize_default(void *config_) config->memory_get = &dense_qp_qpoases_memory_get; config->workspace_calculate_size = (acados_size_t (*)(void *, void *, void *)) & dense_qp_qpoases_workspace_calculate_size; - config->eval_sens = &dense_qp_qpoases_eval_sens; + config->eval_forw_sens = &dense_qp_qpoases_eval_forw_sens; config->eval_adj_sens = &dense_qp_qpoases_eval_adj_sens; config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & dense_qp_qpoases; config->memory_reset = &dense_qp_qpoases_memory_reset; diff --git a/acados/dense_qp/dense_qp_qpoases.h b/acados/dense_qp/dense_qp_qpoases.h index 1758a4ec27..2c17540f2e 100644 --- a/acados/dense_qp/dense_qp_qpoases.h +++ b/acados/dense_qp/dense_qp_qpoases.h @@ -113,8 +113,6 @@ acados_size_t dense_qp_qpoases_workspace_calculate_size(void *config, dense_qp_d // int dense_qp_qpoases(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); // -void dense_qp_qpoases_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void dense_qp_qpoases_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); // void dense_qp_qpoases_config_initialize_default(void *config_); diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 893a19a3bc..e878149025 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -41,6 +41,8 @@ #include "blasfeo_d_blas.h" // hpipm #include "hpipm/include/hpipm_d_ocp_qp_dim.h" +#include "hpipm/include/hpipm_d_ocp_qp_res.h" +#include "hpipm/include/hpipm_d_ocp_qp_seed.h" // acados #include "acados/utils/mem.h" #include "acados/utils/print.h" @@ -1962,6 +1964,17 @@ acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_d // tmp_qp_out size += ocp_qp_out_calculate_size(dims->qp_solver->orig_dims); + // qp_seed + size += ocp_qp_seed_calculate_size(dims->qp_solver->orig_dims); + + if (opts->ext_qp_res) + { + // qp_res + size += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); + // qp_res_ws + size += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); + } + // blasfeo_dvec int nv_max = 0; int nx_max = 0; @@ -2136,15 +2149,6 @@ acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_d } } - if (opts->ext_qp_res) - { - // qp_res - size += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp_res_ws - size += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - size += 64; // ext_fun_workspace_size align size += ext_fun_workspace_size; @@ -2220,6 +2224,20 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims work->weight_merit_fun = ocp_nlp_out_assign(config, dims, c_ptr); c_ptr += ocp_nlp_out_calculate_size(config, dims); + // qp seed + work->qp_seed = ocp_qp_seed_assign(dims->qp_solver->orig_dims, c_ptr); + c_ptr += ocp_qp_seed_calculate_size(dims->qp_solver->orig_dims); + + if (opts->ext_qp_res) + { + // qp res + work->qp_res = ocp_qp_res_assign(dims->qp_solver->orig_dims, c_ptr); + c_ptr += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); + // qp res ws + work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver->orig_dims, c_ptr); + c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); + } + assign_and_advance_double(nv_max, &work->tmp_nv_double, &c_ptr); assign_and_advance_int(ni_max+ns_max, &work->tmp_nins, &c_ptr); @@ -2389,17 +2407,6 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims } } - if (opts->ext_qp_res) - { - // qp res - work->qp_res = ocp_qp_res_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_calculate_size(dims->qp_solver->orig_dims); - - // qp res ws - work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver->orig_dims, c_ptr); - c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver->orig_dims); - } - assert((char *) work + mem->workspace_size >= c_ptr); return work; @@ -3657,31 +3664,31 @@ void ocp_nlp_common_eval_param_sens(ocp_nlp_config *config, ocp_nlp_dims *dims, struct blasfeo_dmat *jac_ineq_p_global = mem->jac_ineq_p_global; struct blasfeo_dmat *jac_dyn_p_global = mem->jac_dyn_p_global; - ocp_qp_in *tmp_qp_in = work->tmp_qp_in; ocp_qp_out *tmp_qp_out = work->tmp_qp_out; - d_ocp_qp_copy_all(mem->qp_in, tmp_qp_in); - d_ocp_qp_set_rhs_zero(tmp_qp_in); + ocp_qp_seed *qp_seed = work->qp_seed; + d_ocp_qp_seed_set_zero(qp_seed); if ((!strcmp("ex", field)) && (stage==0)) { - double one = 1.0; - d_ocp_qp_set_el("lbx", stage, index, &one, tmp_qp_in); - d_ocp_qp_set_el("ubx", stage, index, &one, tmp_qp_in); + int tmp_nbu; + config->constraints[0]->dims_get(config->constraints[0], dims->constraints[0], "nbu", &tmp_nbu); + BLASFEO_DVECEL(qp_seed->seed_d+0, tmp_nbu+index) = 1.0; + BLASFEO_DVECEL(qp_seed->seed_d+0, tmp_nbu+index+nb[0]+ng[0]+ni_nl[0]) = 1.0; } else if (!strcmp("p_global", field)) { for (i = 0; i <= N; i++) { // stationarity - blasfeo_dcolex(nv[i], &jac_lag_stat_p_global[i], 0, index, &tmp_qp_in->rqz[i], 0); + blasfeo_dcolex(nv[i], &jac_lag_stat_p_global[i], 0, index, &qp_seed->seed_g[i], 0); // dynamics if (i < N) - blasfeo_dcolex(nx[i+1], &jac_dyn_p_global[i], 0, index, &tmp_qp_in->b[i], 0); + blasfeo_dcolex(nx[i+1], &jac_dyn_p_global[i], 0, index, &qp_seed->seed_b[i], 0); // inequalities - blasfeo_dcolex(ni_nl[i], &jac_ineq_p_global[i], 0, index, &tmp_qp_in->d[i], nb[i]+ng[i]); - blasfeo_dvecsc(ni_nl[i], -1.0, &tmp_qp_in->d[i], nb[i]+ng[i]); - blasfeo_daxpy(ni_nl[i], -1.0, &tmp_qp_in->d[i], nb[i]+ng[i], &tmp_qp_in->d[i], 2*(nb[i]+ng[i])+ni_nl[i], - &tmp_qp_in->d[i], 2*(nb[i]+ng[i])+ni_nl[i]); + blasfeo_dcolex(ni_nl[i], &jac_ineq_p_global[i], 0, index, &qp_seed->seed_d[i], nb[i]+ng[i]); + blasfeo_dvecsc(ni_nl[i], -1.0, &qp_seed->seed_d[i], nb[i]+ng[i]); + blasfeo_daxpy(ni_nl[i], -1.0, &qp_seed->seed_d[i], nb[i]+ng[i], &qp_seed->seed_d[i], 2*(nb[i]+ng[i])+ni_nl[i], + &qp_seed->seed_d[i], 2*(nb[i]+ng[i])+ni_nl[i]); } } else @@ -3691,7 +3698,8 @@ void ocp_nlp_common_eval_param_sens(ocp_nlp_config *config, ocp_nlp_dims *dims, } // d_ocp_qp_print(tmp_qp_in->dim, tmp_qp_in); - config->qp_solver->eval_sens(config->qp_solver, dims->qp_solver, tmp_qp_in, tmp_qp_out, + // d_ocp_qp_seed_print(qp_seed->dim, qp_seed); + config->qp_solver->eval_forw_sens(config->qp_solver, dims->qp_solver, mem->qp_in, qp_seed, tmp_qp_out, opts->qp_solver_opts, mem->qp_solver_mem, work->qp_work); // d_ocp_qp_sol_print(tmp_qp_out->dim, tmp_qp_out); @@ -3734,16 +3742,14 @@ void ocp_nlp_common_eval_solution_sens_adj_p(ocp_nlp_config *config, ocp_nlp_dim struct blasfeo_dmat *jac_ineq_p_global = mem->jac_ineq_p_global; struct blasfeo_dmat *jac_dyn_p_global = mem->jac_dyn_p_global; - ocp_qp_in *tmp_qp_in = work->tmp_qp_in; + ocp_qp_seed *qp_seed = work->qp_seed; ocp_qp_out *tmp_qp_out = work->tmp_qp_out; + d_ocp_qp_seed_set_zero(qp_seed); - d_ocp_qp_copy_all(mem->qp_in, tmp_qp_in); - d_ocp_qp_set_rhs_zero(tmp_qp_in); - - /* copy sens_nlp_out to tmp_qp_in */ + /* copy sens_nlp_out to qp_seed */ for (i = 0; i <= N; i++) { - blasfeo_dveccp(nv[i], sens_nlp_out->ux + i, 0, tmp_qp_in->rqz + i, 0); + blasfeo_dveccp(nv[i], sens_nlp_out->ux + i, 0, qp_seed->seed_g + i, 0); // NOTE: noone needs sensitivities in adj dir pi, lam, t wrt. p // if (i < N) // blasfeo_dveccp(nx[i + 1], sens_nlp_out->pi + i, 0, tmp_qp_in->b + i, 0); @@ -3751,7 +3757,7 @@ void ocp_nlp_common_eval_solution_sens_adj_p(ocp_nlp_config *config, ocp_nlp_dim // blasfeo_dveccp(2 * ni[i], sens_nlp_out->t + i, ?); } - config->qp_solver->eval_adj_sens(config->qp_solver, dims->qp_solver, tmp_qp_in, tmp_qp_out, + config->qp_solver->eval_adj_sens(config->qp_solver, dims->qp_solver, mem->qp_in, qp_seed, tmp_qp_out, opts->qp_solver_opts, mem->qp_solver_mem, work->qp_work); if (!strcmp("p_global", field)) diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index c8b9e6aa43..cc52c24881 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -483,6 +483,9 @@ typedef struct ocp_nlp_workspace ocp_qp_res *qp_res; ocp_qp_res_ws *qp_res_ws; + // qp seed (for solution sensitivities) + ocp_qp_seed *qp_seed; + // for globalization: -> move to module?! ocp_nlp_out *tmp_nlp_out; ocp_nlp_out *weight_merit_fun; diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index 6038b51999..6fd0f91ad4 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -278,6 +278,33 @@ double ocp_qp_out_compute_primal_nrm_inf(ocp_qp_out* qp_out) return res; } +/************************************************ + * seed + ************************************************/ + +acados_size_t ocp_qp_seed_calculate_size(ocp_qp_dims *dims) +{ + acados_size_t size = sizeof(ocp_qp_seed); + size += d_ocp_qp_seed_memsize(dims); + return size; +} + + + +ocp_qp_seed *ocp_qp_seed_assign(ocp_qp_dims *dims, void *raw_memory) +{ + char *c_ptr = (char *) raw_memory; + + ocp_qp_seed *qp_seed = (ocp_qp_seed *) c_ptr; + c_ptr += sizeof(ocp_qp_seed); + + d_ocp_qp_seed_create(dims, qp_seed, c_ptr); + c_ptr += d_ocp_qp_seed_memsize(dims); + + assert((char *) raw_memory + ocp_qp_seed_calculate_size(dims) == c_ptr); + + return qp_seed; +} /************************************************ diff --git a/acados/ocp_qp/ocp_qp_common.h b/acados/ocp_qp/ocp_qp_common.h index 5ac123ff70..6c660e2284 100644 --- a/acados/ocp_qp/ocp_qp_common.h +++ b/acados/ocp_qp/ocp_qp_common.h @@ -51,6 +51,7 @@ typedef struct d_ocp_qp ocp_qp_in; typedef struct d_ocp_qp_sol ocp_qp_out; typedef struct d_ocp_qp_res ocp_qp_res; typedef struct d_ocp_qp_res_ws ocp_qp_res_ws; +typedef struct d_ocp_qp_seed ocp_qp_seed; @@ -72,8 +73,8 @@ typedef struct int (*evaluate)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); void (*solver_get)(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); void (*memory_reset)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); - void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); - void (*eval_adj_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); + void (*eval_forw_sens)(void *config, void *qp_in, void *seed, void *qp_out, void *opts, void *mem, void *work); + void (*eval_adj_sens)(void *config, void *qp_in, void *seed, void *qp_out, void *opts, void *mem, void *work); void (*terminate)(void *config, void *mem, void *work); } qp_solver_config; #endif @@ -98,9 +99,11 @@ typedef struct acados_size_t (*workspace_calculate_size)(void *dims, void *opts); int (*condensing)(void *qp_in, void *x_cond_qp_in, void *opts, void *mem, void *work); int (*condense_rhs)(void *qp_in, void *x_cond_qp_in, void *opts, void *mem, void *work); + int (*condense_rhs_seed)(void *qp_in, void *seed, void *xcond_seed, void *opts, void *mem, void *work); int (*condense_lhs)(void *qp_in, void *x_cond_qp_in, void *opts, void *mem, void *work); int (*condense_qp_out)(void *qp_in, void *x_cond_qp_in, void *qp_out, void *p_cond_qp_out, void *opts, void *mem, void *work); int (*expansion)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); + int (*expand_sol_seed)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); } ocp_qp_xcond_config; @@ -172,7 +175,11 @@ void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res // void ocp_qp_res_compute_nrm_inf(ocp_qp_res *qp_res, double res[4]); - +/* seed */ +// +acados_size_t ocp_qp_seed_calculate_size(ocp_qp_dims *dims); +// +ocp_qp_seed *ocp_qp_seed_assign(ocp_qp_dims *dims, void *raw_memory); /* misc */ // diff --git a/acados/ocp_qp/ocp_qp_full_condensing.c b/acados/ocp_qp/ocp_qp_full_condensing.c index 72f229e9c8..b2c76290fe 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing.c +++ b/acados/ocp_qp/ocp_qp_full_condensing.c @@ -316,12 +316,12 @@ acados_size_t ocp_qp_full_condensing_memory_calculate_size(void *dims_, void *op size += sizeof(ocp_qp_full_condensing_memory); size += dense_qp_in_calculate_size(dims->fcond_dims); - size += dense_qp_out_calculate_size(dims->fcond_dims); + size += dense_qp_seed_calculate_size(dims->fcond_dims); size += ocp_qp_in_calculate_size(dims->red_dims); - size += ocp_qp_out_calculate_size(dims->red_dims); + size += ocp_qp_seed_calculate_size(dims->red_dims); size += sizeof(struct d_cond_qp_ws); size += d_cond_qp_ws_memsize(dims->red_dims, opts->hpipm_cond_opts); @@ -374,12 +374,18 @@ void *ocp_qp_full_condensing_memory_assign(void *dims_, void *opts_, void *raw_m mem->fcond_qp_out = dense_qp_out_assign(dims->fcond_dims, c_ptr); c_ptr += dense_qp_out_calculate_size(dims->fcond_dims); + mem->fcond_qp_seed = dense_qp_seed_assign(dims->fcond_dims, c_ptr); + c_ptr += dense_qp_seed_calculate_size(dims->fcond_dims); + mem->red_qp = ocp_qp_in_assign(dims->red_dims, c_ptr); c_ptr += ocp_qp_in_calculate_size(dims->red_dims); mem->red_sol = ocp_qp_out_assign(dims->red_dims, c_ptr); c_ptr += ocp_qp_out_calculate_size(dims->red_dims); + mem->red_seed = ocp_qp_seed_assign(dims->red_dims, c_ptr); + c_ptr += ocp_qp_seed_calculate_size(dims->red_dims); + mem->qp_out_info = (qp_info *) mem->fcond_qp_out->misc; assert((char *) raw_memory + ocp_qp_full_condensing_memory_calculate_size(dims, opts) >= c_ptr); @@ -403,6 +409,11 @@ void ocp_qp_full_condensing_memory_get(void *config_, void *mem_, const char *fi dense_qp_out **ptr = value; *ptr = mem->fcond_qp_out; } + else if(!strcmp(field, "xcond_seed")) + { + dense_qp_seed **ptr = value; + *ptr = mem->fcond_qp_seed; + } else if(!strcmp(field, "qp_out_info")) { qp_info **ptr = value; @@ -557,6 +568,34 @@ int ocp_qp_full_condensing_condense_rhs(void *qp_in_, void *fcond_qp_in_, void * } +int ocp_qp_full_condensing_condense_rhs_seed(void *qp_in_, void *qp_seed, void *dense_seed_, void *opts_, void *mem_, void *work) +{ + ocp_qp_in *qp_in = qp_in_; + dense_qp_seed *dense_seed = dense_seed_; + + ocp_qp_full_condensing_opts *opts = opts_; + ocp_qp_full_condensing_memory *mem = mem_; + + acados_timer timer; + + // start timer + acados_tic(&timer); + + // save pointers to ocp_qp_in in memory (needed for expansion) + mem->ptr_qp_in = qp_in; + mem->ptr_qp_seed = qp_seed; + + // reduce eq constr DOF: residual + d_ocp_qp_reduce_eq_dof_seed(qp_in, qp_seed, mem->red_seed, opts->hpipm_red_opts, mem->hpipm_red_work); + + // convert to fully condensed qp structure + d_cond_qp_cond_seed(mem->red_qp, mem->red_seed, dense_seed, opts->hpipm_cond_opts, mem->hpipm_cond_work); + + // stop timer + mem->time_qp_xcond += acados_toc(&timer); + + return ACADOS_SUCCESS; +} int ocp_qp_full_expansion(void *fcond_qp_out_, void *qp_out_, void *opts_, void *mem_, void *work) @@ -577,9 +616,6 @@ int ocp_qp_full_expansion(void *fcond_qp_out_, void *qp_out_, void *opts_, void // restore solution d_ocp_qp_restore_eq_dof(mem->ptr_qp_in, mem->red_sol, qp_out, opts->hpipm_red_opts, mem->hpipm_red_work); -//d_ocp_qp_sol_print(mem->red_sol->dim, mem->red_sol); -//d_ocp_qp_sol_print(qp_out->dim, qp_out); -//exit(1); // stop timer mem->time_qp_xcond += acados_toc(&timer); @@ -587,6 +623,30 @@ int ocp_qp_full_expansion(void *fcond_qp_out_, void *qp_out_, void *opts_, void } +int ocp_qp_full_condensing_expand_sol_seed(void *fcond_qp_out_, void *qp_out_, void *opts_, void *mem_, void *work) +{ + dense_qp_out *fcond_qp_out = fcond_qp_out_; + ocp_qp_out *qp_out = qp_out_; + ocp_qp_full_condensing_opts *opts = opts_; + ocp_qp_full_condensing_memory *mem = mem_; + + acados_timer timer; + + // start timer + acados_tic(&timer); + + // expand solution + d_cond_qp_expand_sol_seed(mem->red_qp, mem->red_seed, fcond_qp_out, mem->red_sol, opts->hpipm_cond_opts, mem->hpipm_cond_work); + + // restore solution + d_ocp_qp_restore_eq_dof_seed(mem->ptr_qp_in, mem->ptr_qp_seed, mem->red_sol, qp_out, opts->hpipm_red_opts, mem->hpipm_red_work); + + // stop timer + mem->time_qp_xcond += acados_toc(&timer); + + return ACADOS_SUCCESS; +} + void ocp_qp_full_condensing_config_initialize_default(void *config_) { @@ -607,9 +667,11 @@ void ocp_qp_full_condensing_config_initialize_default(void *config_) config->workspace_calculate_size = &ocp_qp_full_condensing_workspace_calculate_size; config->condensing = &ocp_qp_full_condensing; config->condense_rhs = &ocp_qp_full_condensing_condense_rhs; + config->condense_rhs_seed = &ocp_qp_full_condensing_condense_rhs_seed; config->condense_lhs = &ocp_qp_full_condensing_condense_lhs; config->condense_qp_out = &ocp_qp_full_condensing_condense_qp_out; config->expansion = &ocp_qp_full_expansion; + config->expand_sol_seed = &ocp_qp_full_condensing_expand_sol_seed; return; } diff --git a/acados/ocp_qp/ocp_qp_full_condensing.h b/acados/ocp_qp/ocp_qp_full_condensing.h index 39ca81c09a..d9483f6b88 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing.h +++ b/acados/ocp_qp/ocp_qp_full_condensing.h @@ -74,10 +74,13 @@ typedef struct ocp_qp_full_condensing_memory_ // in memory dense_qp_in *fcond_qp_in; dense_qp_out *fcond_qp_out; + dense_qp_seed *fcond_qp_seed; ocp_qp_in *red_qp; // reduced qp ocp_qp_out *red_sol; // reduced qp sol + ocp_qp_seed *red_seed; // only pointer ocp_qp_in *ptr_qp_in; + ocp_qp_seed *ptr_qp_seed; qp_info *qp_out_info; // info in fcond_qp_in double time_qp_xcond; } ocp_qp_full_condensing_memory; diff --git a/acados/ocp_qp/ocp_qp_hpipm.c b/acados/ocp_qp/ocp_qp_hpipm.c index b490901403..72667e929f 100644 --- a/acados/ocp_qp/ocp_qp_hpipm.c +++ b/acados/ocp_qp/ocp_qp_hpipm.c @@ -450,7 +450,7 @@ void ocp_qp_hpipm_solver_get(void *config_, void *qp_in_, void *qp_out_, void *o } -void ocp_qp_hpipm_eval_sens(void *config_, void *param_qp_in_, void *sens_qp_out_, void *opts_, void *mem_, void *work_) +void ocp_qp_hpipm_eval_forw_sens(void *config_, void *param_qp_in_, void *seed, void *sens_qp_out_, void *opts_, void *mem_, void *work_) { ocp_qp_in *param_qp_in = param_qp_in_; ocp_qp_out *sens_qp_out = sens_qp_out_; @@ -458,13 +458,13 @@ void ocp_qp_hpipm_eval_sens(void *config_, void *param_qp_in_, void *sens_qp_out ocp_qp_hpipm_opts *opts = opts_; ocp_qp_hpipm_memory *mem = mem_; - d_ocp_qp_ipm_sens(param_qp_in, sens_qp_out, opts->hpipm_opts, mem->hpipm_workspace); + d_ocp_qp_ipm_sens_frw(param_qp_in, seed, sens_qp_out, opts->hpipm_opts, mem->hpipm_workspace); return; } -void ocp_qp_hpipm_eval_adj_sens(void *config_, void *param_qp_in_, void *sens_qp_out_, void *opts_, void *mem_, void *work_) +void ocp_qp_hpipm_eval_adj_sens(void *config_, void *param_qp_in_, void *seed, void *sens_qp_out_, void *opts_, void *mem_, void *work_) { ocp_qp_in *param_qp_in = param_qp_in_; ocp_qp_out *sens_qp_out = sens_qp_out_; @@ -472,7 +472,7 @@ void ocp_qp_hpipm_eval_adj_sens(void *config_, void *param_qp_in_, void *sens_qp ocp_qp_hpipm_opts *opts = opts_; ocp_qp_hpipm_memory *mem = mem_; - d_ocp_qp_ipm_sens_adj(param_qp_in, sens_qp_out, opts->hpipm_opts, mem->hpipm_workspace); + d_ocp_qp_ipm_sens_adj(param_qp_in, seed, sens_qp_out, opts->hpipm_opts, mem->hpipm_workspace); return; } @@ -504,7 +504,7 @@ void ocp_qp_hpipm_config_initialize_default(void *config_) config->evaluate = &ocp_qp_hpipm; config->solver_get = &ocp_qp_hpipm_solver_get; config->memory_reset = &ocp_qp_hpipm_memory_reset; - config->eval_sens = &ocp_qp_hpipm_eval_sens; + config->eval_forw_sens = &ocp_qp_hpipm_eval_forw_sens; config->eval_adj_sens = &ocp_qp_hpipm_eval_adj_sens; config->terminate = &ocp_qp_hpipm_terminate; diff --git a/acados/ocp_qp/ocp_qp_hpipm.h b/acados/ocp_qp/ocp_qp_hpipm.h index 9c78922ddb..26af6e839e 100644 --- a/acados/ocp_qp/ocp_qp_hpipm.h +++ b/acados/ocp_qp/ocp_qp_hpipm.h @@ -88,8 +88,6 @@ void ocp_qp_hpipm_memory_reset(void *config_, void *qp_in_, void *qp_out_, void // void ocp_qp_hpipm_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); // -void ocp_qp_hpipm_eval_sens(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void ocp_qp_hpipm_config_initialize_default(void *config); diff --git a/acados/ocp_qp/ocp_qp_hpmpc.c b/acados/ocp_qp/ocp_qp_hpmpc.c index d7c88b4d9c..06486381ef 100644 --- a/acados/ocp_qp/ocp_qp_hpmpc.c +++ b/acados/ocp_qp/ocp_qp_hpmpc.c @@ -577,9 +577,9 @@ void ocp_qp_hpmpc_solver_get(void *config_, void *qp_in_, void *qp_out_, void *o exit(1); } -void ocp_qp_hpmpc_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void ocp_qp_hpmpc_eval_forw_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: ocp_qp_hpmpc_eval_sens: not implemented yet\n"); + printf("\nerror: ocp_qp_hpmpc_eval_forw_sens: not implemented yet\n"); exit(1); } @@ -616,7 +616,7 @@ void ocp_qp_hpmpc_config_initialize_default(void *config_) config->workspace_calculate_size = (size_t (*)(void *, void *, void *)) & ocp_qp_hpmpc_workspace_calculate_size; config->evaluate = &ocp_qp_hpmpc; - config->eval_sens = &ocp_qp_hpmpc_eval_sens; + config->eval_forw_sens = &ocp_qp_hpmpc_eval_forw_sens; config->eval_adj_sens = &ocp_qp_hpmpc_eval_adj_sens; config->memory_reset = &ocp_qp_hpmpc_memory_reset; config->solver_get = &ocp_qp_hpmpc_solver_get; diff --git a/acados/ocp_qp/ocp_qp_hpmpc.h b/acados/ocp_qp/ocp_qp_hpmpc.h index a830629b68..ab620f926e 100644 --- a/acados/ocp_qp/ocp_qp_hpmpc.h +++ b/acados/ocp_qp/ocp_qp_hpmpc.h @@ -118,7 +118,7 @@ int ocp_qp_hpmpc(void *config_, void *qp_in, void *qp_out, void *opts_, void *me // void ocp_qp_hpmpc_memory_reset(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_); // -void ocp_qp_hpmpc_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +void ocp_qp_hpmpc_eval_forw_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); // void ocp_qp_hpmpc_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2) // diff --git a/acados/ocp_qp/ocp_qp_ooqp.c b/acados/ocp_qp/ocp_qp_ooqp.c index 1640ada5be..bb603ec932 100644 --- a/acados/ocp_qp/ocp_qp_ooqp.c +++ b/acados/ocp_qp/ocp_qp_ooqp.c @@ -1176,9 +1176,9 @@ void ocp_qp_ooqp_solver_get(void *config_, void *qp_in_, void *qp_out_, void *op } -void ocp_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void ocp_qp_ooqp_eval_forw_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: ocp_qp_ooqp_eval_sens: not implemented yet\n"); + printf("\nerror: ocp_qp_ooqp_eval_forw_sens: not implemented yet\n"); exit(1); } @@ -1215,7 +1215,7 @@ void ocp_qp_ooqp_config_initialize_default(void *config_) config->workspace_calculate_size = (size_t (*)(void *, void *, void *)) & ocp_qp_ooqp_workspace_calculate_size; config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & ocp_qp_ooqp; - config->eval_sens = &ocp_qp_ooqp_eval_sens; + config->eval_forw_sens = &ocp_qp_ooqp_eval_forw_sens; config->eval_adj_sens = &ocp_qp_ooqp_eval_adj_sens; config->memory_reset = &ocp_qp_ooqp_memory_reset; config->solver_get = &ocp_qp_ooqp_solver_get; diff --git a/acados/ocp_qp/ocp_qp_ooqp.h b/acados/ocp_qp/ocp_qp_ooqp.h index 1b33bd4152..6672ac58b1 100644 --- a/acados/ocp_qp/ocp_qp_ooqp.h +++ b/acados/ocp_qp/ocp_qp_ooqp.h @@ -133,8 +133,6 @@ int ocp_qp_ooqp(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_ // void ocp_qp_ooqp_destroy(void *mem_, void *work); // -void ocp_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void ocp_qp_ooqp_memory_reset(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_); // void ocp_qp_ooqp_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); diff --git a/acados/ocp_qp/ocp_qp_osqp.c b/acados/ocp_qp/ocp_qp_osqp.c index d3c341fdad..7df6f585af 100644 --- a/acados/ocp_qp/ocp_qp_osqp.c +++ b/acados/ocp_qp/ocp_qp_osqp.c @@ -1788,13 +1788,13 @@ int ocp_qp_osqp(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *m -void ocp_qp_osqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void ocp_qp_osqp_eval_forw_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: ocp_qp_osqp_eval_sens: not implemented yet\n"); + printf("\nerror: ocp_qp_osqp_eval_forw_sens: not implemented yet\n"); exit(1); } -void ocp_qp_osqp_eval_adj_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void ocp_qp_osqp_eval_adj_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { printf("\nerror: ocp_qp_osqp_eval_adj_sens: not implemented yet\n"); exit(1); @@ -1824,7 +1824,7 @@ void ocp_qp_osqp_config_initialize_default(void *config_) config->workspace_calculate_size = &ocp_qp_osqp_workspace_calculate_size; config->evaluate = &ocp_qp_osqp; config->terminate = &ocp_qp_osqp_terminate; - config->eval_sens = &ocp_qp_osqp_eval_sens; + config->eval_forw_sens = &ocp_qp_osqp_eval_forw_sens; config->eval_adj_sens = &ocp_qp_osqp_eval_adj_sens; config->memory_reset = &ocp_qp_osqp_memory_reset; config->solver_get = &ocp_qp_osqp_solver_get; diff --git a/acados/ocp_qp/ocp_qp_osqp.h b/acados/ocp_qp/ocp_qp_osqp.h index e4158c9e59..6e8ff677f5 100644 --- a/acados/ocp_qp/ocp_qp_osqp.h +++ b/acados/ocp_qp/ocp_qp_osqp.h @@ -92,8 +92,6 @@ acados_size_t ocp_qp_osqp_workspace_calculate_size(void *config, void *dims, voi // int ocp_qp_osqp(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); // -void ocp_qp_osqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void ocp_qp_osqp_memory_reset(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_); // void ocp_qp_osqp_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.c b/acados/ocp_qp/ocp_qp_partial_condensing.c index 0ff4ab68e2..a8b8cc6cbd 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing.c @@ -376,12 +376,12 @@ acados_size_t ocp_qp_partial_condensing_memory_calculate_size(void *dims_, void size += sizeof(ocp_qp_partial_condensing_memory); size += ocp_qp_in_calculate_size(dims->pcond_dims); - size += ocp_qp_out_calculate_size(dims->pcond_dims); + size += ocp_qp_seed_calculate_size(dims->pcond_dims); size += ocp_qp_in_calculate_size(dims->red_dims); - size += ocp_qp_out_calculate_size(dims->red_dims); + size += ocp_qp_seed_calculate_size(dims->red_dims); // hpipm_pcond_work size += sizeof(struct d_part_cond_qp_ws); @@ -435,12 +435,18 @@ void *ocp_qp_partial_condensing_memory_assign(void *dims_, void *opts_, void *ra mem->pcond_qp_out = ocp_qp_out_assign(dims->pcond_dims, c_ptr); c_ptr += ocp_qp_out_calculate_size(dims->pcond_dims); + mem->pcond_qp_seed = ocp_qp_seed_assign(dims->pcond_dims, c_ptr); + c_ptr += ocp_qp_seed_calculate_size(dims->pcond_dims); + mem->red_qp = ocp_qp_in_assign(dims->red_dims, c_ptr); c_ptr += ocp_qp_in_calculate_size(dims->red_dims); mem->red_sol = ocp_qp_out_assign(dims->red_dims, c_ptr); c_ptr += ocp_qp_out_calculate_size(dims->red_dims); + mem->red_seed = ocp_qp_seed_assign(dims->red_dims, c_ptr); + c_ptr += ocp_qp_seed_calculate_size(dims->red_dims); + mem->qp_out_info = (qp_info *) mem->pcond_qp_out->misc; mem->dims = dims; @@ -466,6 +472,11 @@ void ocp_qp_partial_condensing_memory_get(void *config_, void *mem_, const char ocp_qp_out **ptr = value; *ptr = mem->pcond_qp_out; } + else if(!strcmp(field, "xcond_seed")) + { + ocp_qp_seed **ptr = value; + *ptr = mem->pcond_qp_seed; + } else if(!strcmp(field, "qp_out_info")) { qp_info **ptr = value; @@ -614,6 +625,36 @@ int ocp_qp_partial_condensing_condense_rhs(void *qp_in_, void *pcond_qp_in_, voi +int ocp_qp_partial_condensing_condense_rhs_seed(void *qp_in_, void *qp_seed, void *pcond_seed, void *opts_, void *mem_, void *work) +{ + ocp_qp_in *qp_in = qp_in_; + ocp_qp_partial_condensing_opts *opts = opts_; + ocp_qp_partial_condensing_memory *mem = mem_; + + assert(opts->N2 == opts->N2_bkp); + + acados_timer timer; + + // start timer + acados_tic(&timer); + + // save pointers to ocp_qp_in in memory (needed for expansion) + mem->ptr_qp_in = qp_in; + mem->ptr_qp_seed = qp_seed; + + // reduce eq constr DOF: residual + d_ocp_qp_reduce_eq_dof_seed(qp_in, qp_seed, mem->red_seed, opts->hpipm_red_opts, mem->hpipm_red_work); + + // convert to partially condensed qp structure + d_part_cond_qp_cond_seed(mem->red_qp, mem->red_seed, pcond_seed, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); + + // stop timer + mem->time_qp_xcond += acados_toc(&timer); + + return ACADOS_SUCCESS; +} + + int ocp_qp_partial_expansion(void *pcond_qp_out_, void *qp_out_, void *opts_, void *mem_, void *work) { ocp_qp_out *pcond_qp_out = pcond_qp_out_; @@ -630,7 +671,7 @@ int ocp_qp_partial_expansion(void *pcond_qp_out_, void *qp_out_, void *opts_, vo // expand solution // TODO only if N2red_qp, mem->ptr_pcond_qp_in, pcond_qp_out, mem->red_sol, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); + d_part_cond_qp_expand_sol(mem->red_qp, pcond_qp_out, mem->red_sol, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); // restore solution d_ocp_qp_restore_eq_dof(mem->ptr_qp_in, mem->red_sol, qp_out, opts->hpipm_red_opts, mem->hpipm_red_work); @@ -641,6 +682,32 @@ int ocp_qp_partial_expansion(void *pcond_qp_out_, void *qp_out_, void *opts_, vo return ACADOS_SUCCESS; } +int ocp_qp_partial_condensing_expand_sol_seed(void *pcond_qp_out_, void *qp_out_, void *opts_, void *mem_, void *work) +{ + ocp_qp_out *pcond_qp_out = pcond_qp_out_; + ocp_qp_out *qp_out = qp_out_; + ocp_qp_partial_condensing_opts *opts = opts_; + ocp_qp_partial_condensing_memory *mem = mem_; + + assert(opts->N2 == opts->N2_bkp); + + acados_timer timer; + + // start timer + acados_tic(&timer); + + // expand solution + d_part_cond_qp_expand_sol_seed(mem->red_qp, mem->red_seed, pcond_qp_out, mem->red_sol, opts->hpipm_pcond_opts, mem->hpipm_pcond_work); + + // restore solution + d_ocp_qp_restore_eq_dof_seed(mem->ptr_qp_in, mem->ptr_qp_seed, mem->red_sol, qp_out, opts->hpipm_red_opts, mem->hpipm_red_work); + + // stop timer + mem->time_qp_xcond += acados_toc(&timer); + + return ACADOS_SUCCESS; +} + void ocp_qp_partial_condensing_config_initialize_default(void *config_) @@ -663,8 +730,10 @@ void ocp_qp_partial_condensing_config_initialize_default(void *config_) config->condensing = &ocp_qp_partial_condensing; config->condense_lhs = &ocp_qp_partial_condensing_condense_lhs; config->condense_rhs = &ocp_qp_partial_condensing_condense_rhs; + config->condense_rhs_seed = &ocp_qp_partial_condensing_condense_rhs_seed; config->condense_qp_out = &ocp_qp_partial_condensing_condense_qp_out; config->expansion = &ocp_qp_partial_expansion; + config->expand_sol_seed = &ocp_qp_partial_condensing_expand_sol_seed; return; } diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.h b/acados/ocp_qp/ocp_qp_partial_condensing.h index 581fb3c77a..b5687ad50d 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.h +++ b/acados/ocp_qp/ocp_qp_partial_condensing.h @@ -76,11 +76,14 @@ typedef struct ocp_qp_partial_condensing_memory_ // in memory ocp_qp_in *pcond_qp_in; ocp_qp_out *pcond_qp_out; + ocp_qp_seed *pcond_qp_seed; ocp_qp_in *red_qp; // reduced qp ocp_qp_out *red_sol; // reduced qp sol + ocp_qp_seed *red_seed; // only pointer ocp_qp_in *ptr_qp_in; ocp_qp_in *ptr_pcond_qp_in; + ocp_qp_seed *ptr_qp_seed; qp_info *qp_out_info; // info in pcond_qp_in ocp_qp_partial_condensing_dims *dims; double time_qp_xcond; diff --git a/acados/ocp_qp/ocp_qp_qpdunes.c b/acados/ocp_qp/ocp_qp_qpdunes.c index 87c82ec7d3..a36c729963 100644 --- a/acados/ocp_qp/ocp_qp_qpdunes.c +++ b/acados/ocp_qp/ocp_qp_qpdunes.c @@ -925,13 +925,13 @@ int ocp_qp_qpdunes(void *config_, ocp_qp_in *in, ocp_qp_out *out, void *opts_, v -void ocp_qp_qpdunes_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void ocp_qp_qpdunes_eval_forw_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { - printf("\nerror: ocp_qp_qpdunes_eval_sens: not implemented yet\n"); + printf("\nerror: ocp_qp_qpdunes_eval_forw_sens: not implemented yet\n"); exit(1); } -void ocp_qp_qpdunes_eval_adj_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_) +void ocp_qp_qpdunes_eval_adj_sens(void *config_, void *qp_in, void *seed, void *qp_out, void *opts_, void *mem_, void *work_) { printf("\nerror: ocp_qp_qpdunes_eval_adj_sens: not implemented yet\n"); exit(1); @@ -971,7 +971,7 @@ void ocp_qp_qpdunes_config_initialize_default(void *config_) config->workspace_calculate_size = (acados_size_t (*)(void *, void *, void *)) & ocp_qp_qpdunes_workspace_calculate_size; config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & ocp_qp_qpdunes; - config->eval_sens = &ocp_qp_qpdunes_eval_sens; + config->eval_forw_sens = &ocp_qp_qpdunes_eval_forw_sens; config->eval_adj_sens = &ocp_qp_qpdunes_eval_adj_sens; config->solver_get = &ocp_qp_qpdunes_solver_get; config->terminate = &ocp_qp_qpdunes_terminate; diff --git a/acados/ocp_qp/ocp_qp_qpdunes.h b/acados/ocp_qp/ocp_qp_qpdunes.h index ecbc6da63f..60e3288ba6 100644 --- a/acados/ocp_qp/ocp_qp_qpdunes.h +++ b/acados/ocp_qp/ocp_qp_qpdunes.h @@ -108,8 +108,6 @@ int ocp_qp_qpdunes(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *op // void ocp_qp_qpdunes_free_memory(void *mem_); // -void ocp_qp_qpdunes_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); -// void ocp_qp_qpdunes_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2); // void ocp_qp_qpdunes_config_initialize_default(void *config_); diff --git a/acados/ocp_qp/ocp_qp_xcond_solver.c b/acados/ocp_qp/ocp_qp_xcond_solver.c index bcc7926277..9150a0eb0c 100644 --- a/acados/ocp_qp/ocp_qp_xcond_solver.c +++ b/acados/ocp_qp/ocp_qp_xcond_solver.c @@ -371,6 +371,7 @@ void *ocp_qp_xcond_solver_memory_assign(void *config_, ocp_qp_xcond_solver_dims xcond->memory_get(xcond, mem->xcond_memory, "xcond_qp_in", &mem->xcond_qp_in); xcond->memory_get(xcond, mem->xcond_memory, "xcond_qp_out", &mem->xcond_qp_out); + xcond->memory_get(xcond, mem->xcond_memory, "xcond_seed", &mem->xcond_seed); assert((char *) raw_memory + ocp_qp_xcond_solver_memory_calculate_size(config_, dims, opts_) >= c_ptr); @@ -656,7 +657,7 @@ int ocp_qp_xcond_condense_rhs_and_solve(void *config_, ocp_qp_xcond_solver_dims -void ocp_qp_xcond_solver_eval_sens(void *config_, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_out *sens_qp_out, +void ocp_qp_xcond_solver_eval_forw_sens(void *config_, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_seed *seed, ocp_qp_out *sens_qp_out, void *opts_, void *mem_, void *work_) { ocp_qp_xcond_solver_config *config = config_; @@ -672,18 +673,18 @@ void ocp_qp_xcond_solver_eval_sens(void *config_, ocp_qp_xcond_solver_dims *dims cast_workspace(config_, dims, opts, memory, work); // condensing - xcond->condense_rhs(param_qp_in, memory->xcond_qp_in, opts->xcond_opts, memory->xcond_memory, work->xcond_work); + xcond->condense_rhs_seed(param_qp_in, seed, memory->xcond_seed, opts->xcond_opts, memory->xcond_memory, work->xcond_work); // qp evaluate sensitivity - qp_solver->eval_sens(qp_solver, memory->xcond_qp_in, memory->xcond_qp_out, opts->qp_solver_opts, memory->solver_memory, work->qp_solver_work); + qp_solver->eval_forw_sens(qp_solver, memory->xcond_qp_in, memory->xcond_seed, memory->xcond_qp_out, opts->qp_solver_opts, memory->solver_memory, work->qp_solver_work); // expansion - xcond->expansion(memory->xcond_qp_out, sens_qp_out, opts->xcond_opts, memory->xcond_memory, work->xcond_work); + xcond->expand_sol_seed(memory->xcond_qp_out, sens_qp_out, opts->xcond_opts, memory->xcond_memory, work->xcond_work); return; } -void ocp_qp_xcond_solver_eval_adj_sens(void *config_, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_out *sens_qp_out, +void ocp_qp_xcond_solver_eval_adj_sens(void *config_, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_seed *seed, ocp_qp_out *sens_qp_out, void *opts_, void *mem_, void *work_) { ocp_qp_xcond_solver_config *config = config_; @@ -699,13 +700,13 @@ void ocp_qp_xcond_solver_eval_adj_sens(void *config_, ocp_qp_xcond_solver_dims * cast_workspace(config_, dims, opts, memory, work); // condensing - xcond->condense_rhs(param_qp_in, memory->xcond_qp_in, opts->xcond_opts, memory->xcond_memory, work->xcond_work); + xcond->condense_rhs_seed(param_qp_in, seed, memory->xcond_seed, opts->xcond_opts, memory->xcond_memory, work->xcond_work); // qp evaluate sensitivity - qp_solver->eval_adj_sens(qp_solver, memory->xcond_qp_in, memory->xcond_qp_out, opts->qp_solver_opts, memory->solver_memory, work->qp_solver_work); + qp_solver->eval_adj_sens(qp_solver, memory->xcond_qp_in, memory->xcond_seed, memory->xcond_qp_out, opts->qp_solver_opts, memory->solver_memory, work->qp_solver_work); // expansion - xcond->expansion(memory->xcond_qp_out, sens_qp_out, opts->xcond_opts, memory->xcond_memory, work->xcond_work); + xcond->expand_sol_seed(memory->xcond_qp_out, sens_qp_out, opts->xcond_opts, memory->xcond_memory, work->xcond_work); return; } @@ -748,7 +749,7 @@ void ocp_qp_xcond_solver_config_initialize_default(void *config_) config->evaluate = &ocp_qp_xcond_solve; config->condense_lhs = &ocp_qp_xcond_condense_lhs; config->condense_rhs_and_solve = &ocp_qp_xcond_condense_rhs_and_solve; - config->eval_sens = &ocp_qp_xcond_solver_eval_sens; + config->eval_forw_sens = &ocp_qp_xcond_solver_eval_forw_sens; config->eval_adj_sens = &ocp_qp_xcond_solver_eval_adj_sens; config->terminate = &ocp_qp_xcond_solver_terminate; diff --git a/acados/ocp_qp/ocp_qp_xcond_solver.h b/acados/ocp_qp/ocp_qp_xcond_solver.h index a0e20c1fb6..771548c5b1 100644 --- a/acados/ocp_qp/ocp_qp_xcond_solver.h +++ b/acados/ocp_qp/ocp_qp_xcond_solver.h @@ -65,6 +65,7 @@ typedef struct ocp_qp_xcond_solver_memory_ void *solver_memory; void *xcond_qp_in; void *xcond_qp_out; + void *xcond_seed; } ocp_qp_xcond_solver_memory; @@ -98,8 +99,8 @@ typedef struct int (*evaluate)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work); int (*condense_lhs)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work); int (*condense_rhs_and_solve)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work); - void (*eval_sens)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_out *sens_qp_out, void *opts, void *mem, void *work); - void (*eval_adj_sens)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_out *sens_qp_out, void *opts, void *mem, void *work); + void (*eval_forw_sens)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_seed *seed, ocp_qp_out *sens_qp_out, void *opts, void *mem, void *work); + void (*eval_adj_sens)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_seed *seed, ocp_qp_out *sens_qp_out, void *opts, void *mem, void *work); void (*terminate)(void *config, void *mem, void *work); qp_solver_config *qp_solver; // either ocp_qp_solver or dense_solver ocp_qp_xcond_config *xcond; diff --git a/examples/acados_python/chain_mass/solution_sensitivity_example.py b/examples/acados_python/chain_mass/solution_sensitivity_example.py index 5f68ad44e1..0475524575 100644 --- a/examples/acados_python/chain_mass/solution_sensitivity_example.py +++ b/examples/acados_python/chain_mass/solution_sensitivity_example.py @@ -518,10 +518,15 @@ def main_parametric(qp_solver_ric_alg: int = 0, # print(f"{sens_u_=}") # print(f"{sens_x_=}") - test_tol = 1e-5 + test_tol = 1e-9 diff_sens_adj_vs_ref = np.max(np.abs(sens_adj_ref.ravel() - sens_adj)) if diff_sens_adj_vs_ref > test_tol: raise_test_failure_message(f"diff_sens_adj_vs_ref = {diff_sens_adj_vs_ref} should be < {test_tol}") + else: + print(f"Success: diff_sens_adj_vs_ref = {diff_sens_adj_vs_ref} < {test_tol}") + + # assert not all zero + assert np.max(np.abs(sens_adj)) > 1.0 sens_u.append(sens_u_[:, p_idx]) @@ -537,7 +542,12 @@ def main_parametric(qp_solver_ric_alg: int = 0, print(f"i {i} {timings_solve_params_adj[i]*1e3:.5f} \t {timings_solve_params[i]*1e3:.5f} \t {timings_solve_params_adj_uforw[i]*1e3:.5f} \t {timings_solve_params_adj_all_primals[i]*1e3:.5f}") # check wrt forward - print(np.abs(sens_adj- out_dict['sens_u'])) + diff_sens_u_vs_via_adj = np.max(np.abs(sens_adj- out_dict['sens_u'])) + if diff_sens_u_vs_via_adj > test_tol: + raise_test_failure_message(f"diff_sens_u_vs_via_adj = {diff_sens_u_vs_via_adj} should be < {test_tol}") + else: + print(f"Success: diff_sens_u_vs_via_adj = {diff_sens_u_vs_via_adj} < {test_tol}") + # assert np.allclose(sens_adj, out_dict['sens_u']) if generate_plots: diff --git a/external/hpipm b/external/hpipm index 185517cf53..945645e8c0 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 185517cf539598cf173b45fd4d4ea2a74eed3f38 +Subproject commit 945645e8c0ac473f3983fb3c7f22dbae50d864cc