/* * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl * * 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.; */ // standard #include #include #include // acados // #include "acados/utils/print.h" #include "acados_c/ocp_nlp_interface.h" #include "acados_c/external_function_interface.h" // example specific #include "{{ model.name }}_model/{{ model.name }}_model.h" {% if constraints.constr_type == "BGP" and dims.nphi %} #include "{{ model.name }}_constraints/{{ model.name }}_phi_constraint.h" {% endif %} {% if constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %} #include "{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.h" {% endif %} {% if constraints.constr_type == "BGH" and dims.nh > 0 %} #include "{{ model.name }}_constraints/{{ model.name }}_h_constraint.h" {% endif %} {% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} #include "{{ model.name }}_constraints/{{ model.name }}_h_e_constraint.h" {% endif %} {%- if cost.cost_type == "NONLINEAR_LS" %} #include "{{ model.name }}_cost/{{ model.name }}_cost_y_fun.h" {%- elif cost.cost_type == "EXTERNAL" %} #include "{{ model.name }}_cost/{{ model.name }}_external_cost.h" {%- endif %} {%- if cost.cost_type_0 == "NONLINEAR_LS" %} #include "{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.h" {%- elif cost.cost_type_0 == "EXTERNAL" %} #include "{{ model.name }}_cost/{{ model.name }}_external_cost_0.h" {%- endif %} {%- if cost.cost_type_e == "NONLINEAR_LS" %} #include "{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.h" {%- elif cost.cost_type_e == "EXTERNAL" %} #include "{{ model.name }}_cost/{{ model.name }}_external_cost_e.h" {%- endif %} #include "acados_solver_{{ model.name }}.h" #define NX {{ model.name | upper }}_NX #define NZ {{ model.name | upper }}_NZ #define NU {{ model.name | upper }}_NU #define NP {{ model.name | upper }}_NP #define NBX {{ model.name | upper }}_NBX #define NBX0 {{ model.name | upper }}_NBX0 #define NBU {{ model.name | upper }}_NBU #define NSBX {{ model.name | upper }}_NSBX #define NSBU {{ model.name | upper }}_NSBU #define NSH {{ model.name | upper }}_NSH #define NSG {{ model.name | upper }}_NSG #define NSPHI {{ model.name | upper }}_NSPHI #define NSHN {{ model.name | upper }}_NSHN #define NSGN {{ model.name | upper }}_NSGN #define NSPHIN {{ model.name | upper }}_NSPHIN #define NSBXN {{ model.name | upper }}_NSBXN #define NS {{ model.name | upper }}_NS #define NSN {{ model.name | upper }}_NSN #define NG {{ model.name | upper }}_NG #define NBXN {{ model.name | upper }}_NBXN #define NGN {{ model.name | upper }}_NGN #define NY0 {{ model.name | upper }}_NY0 #define NY {{ model.name | upper }}_NY #define NYN {{ model.name | upper }}_NYN // #define N {{ model.name | upper }}_N #define NH {{ model.name | upper }}_NH #define NPHI {{ model.name | upper }}_NPHI #define NHN {{ model.name | upper }}_NHN #define NPHIN {{ model.name | upper }}_NPHIN #define NR {{ model.name | upper }}_NR // ** solver data ** {{ model.name }}_solver_capsule * {{ model.name }}_acados_create_capsule(void) { void* capsule_mem = malloc(sizeof({{ model.name }}_solver_capsule)); {{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) capsule_mem; return capsule; } int {{ model.name }}_acados_free_capsule({{ model.name }}_solver_capsule *capsule) { free(capsule); return 0; } int {{ model.name }}_acados_create({{ model.name }}_solver_capsule* capsule) { int N_shooting_intervals = {{ model.name | upper }}_N; double* new_time_steps = NULL; // NULL -> don't alter the code generated time-steps return {{ model.name }}_acados_create_with_discretization(capsule, N_shooting_intervals, new_time_steps); } int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule* capsule, int N, double* new_time_steps) { 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 " \ "time steps (= %d)!\n" \ "Please recreate with new discretization and provide a new vector of time_stamps!\n", N, capsule->nlp_solver_plan->N); return 1; } ocp_nlp_config * nlp_config = capsule->nlp_config; ocp_nlp_dims * nlp_dims = capsule->nlp_dims; ocp_nlp_in * nlp_in = capsule->nlp_in; for (int i = 0; i < N; i++) { ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &new_time_steps[i]); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &new_time_steps[i]); } return 0; } /** * Internal function for {{ model.name }}_acados_create: step 1 */ void {{ model.name }}_acados_create_1_set_plan(ocp_nlp_plan_t* nlp_solver_plan, const int N) { assert(N == nlp_solver_plan->N); /************************************************ * plan ************************************************/ {%- if solver_options.nlp_solver_type == "SQP" %} nlp_solver_plan->nlp_solver = SQP; {%- else %} nlp_solver_plan->nlp_solver = SQP_RTI; {%- endif %} nlp_solver_plan->ocp_qp_solver_plan.qp_solver = {{ solver_options.qp_solver }}; 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 }}; nlp_solver_plan->nlp_cost[N] = {{ cost.cost_type_e }}; for (int i = 0; i < N; i++) { {%- if solver_options.integrator_type == "DISCRETE" %} nlp_solver_plan->nlp_dynamics[i] = DISCRETE_MODEL; // discrete dynamics does not need sim solver option, this field is ignored nlp_solver_plan->sim_solver_plan[i].sim_solver = INVALID_SIM_SOLVER; {%- else %} nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL; nlp_solver_plan->sim_solver_plan[i].sim_solver = {{ solver_options.integrator_type }}; {%- endif %} } for (int i = 0; i < N; i++) { {%- if constraints.constr_type == "BGP" %} nlp_solver_plan->nlp_constraints[i] = BGP; {%- else -%} nlp_solver_plan->nlp_constraints[i] = BGH; {%- endif %} } {%- if constraints.constr_type_e == "BGP" %} nlp_solver_plan->nlp_constraints[N] = BGP; {%- else %} nlp_solver_plan->nlp_constraints[N] = BGH; {%- endif %} {%- if solver_options.hessian_approx == "EXACT" %} {%- if solver_options.regularize_method == "NO_REGULARIZE" %} nlp_solver_plan->regularization = NO_REGULARIZE; {%- elif solver_options.regularize_method == "MIRROR" %} nlp_solver_plan->regularization = MIRROR; {%- elif solver_options.regularize_method == "PROJECT" %} nlp_solver_plan->regularization = PROJECT; {%- elif solver_options.regularize_method == "PROJECT_REDUC_HESS" %} nlp_solver_plan->regularization = PROJECT_REDUC_HESS; {%- elif solver_options.regularize_method == "CONVEXIFY" %} nlp_solver_plan->regularization = CONVEXIFY; {%- endif %} {%- endif %} } /** * Internal function for {{ model.name }}_acados_create: step 2 */ ocp_nlp_dims* {{ model.name }}_acados_create_2_create_and_set_dimensions({{ model.name }}_solver_capsule* capsule) { ocp_nlp_plan_t* nlp_solver_plan = capsule->nlp_solver_plan; const int N = nlp_solver_plan->N; ocp_nlp_config* nlp_config = capsule->nlp_config; /************************************************ * dimensions ************************************************/ #define NINTNP1MEMS 17 int* intNp1mem = (int*)malloc( (N+1)*sizeof(int)*NINTNP1MEMS ); int* nx = intNp1mem + (N+1)*0; int* nu = intNp1mem + (N+1)*1; int* nbx = intNp1mem + (N+1)*2; int* nbu = intNp1mem + (N+1)*3; int* nsbx = intNp1mem + (N+1)*4; int* nsbu = intNp1mem + (N+1)*5; int* nsg = intNp1mem + (N+1)*6; int* nsh = intNp1mem + (N+1)*7; int* nsphi = intNp1mem + (N+1)*8; int* ns = intNp1mem + (N+1)*9; int* ng = intNp1mem + (N+1)*10; int* nh = intNp1mem + (N+1)*11; int* nphi = intNp1mem + (N+1)*12; int* nz = intNp1mem + (N+1)*13; int* ny = intNp1mem + (N+1)*14; int* nr = intNp1mem + (N+1)*15; int* nbxe = intNp1mem + (N+1)*16; for (int i = 0; i < N+1; i++) { // common nx[i] = NX; nu[i] = NU; nz[i] = NZ; ns[i] = NS; // cost ny[i] = NY; // constraints nbx[i] = NBX; nbu[i] = NBU; nsbx[i] = NSBX; nsbu[i] = NSBU; nsg[i] = NSG; nsh[i] = NSH; nsphi[i] = NSPHI; ng[i] = NG; nh[i] = NH; nphi[i] = NPHI; nr[i] = NR; nbxe[i] = 0; } // for initial state nbx[0] = NBX0; nsbx[0] = 0; ns[0] = NS - NSBX; nbxe[0] = {{ dims.nbxe_0 }}; ny[0] = NY0; // terminal - common nu[N] = 0; nz[N] = 0; ns[N] = NSN; // cost ny[N] = NYN; // constraint nbx[N] = NBXN; nbu[N] = 0; ng[N] = NGN; nh[N] = NHN; nphi[N] = NPHIN; nr[N] = {{ dims.nr_e }}; nsbx[N] = NSBXN; nsbu[N] = 0; nsg[N] = NSGN; nsh[N] = NSHN; nsphi[N] = NSPHIN; /* create and set ocp_nlp_dims */ ocp_nlp_dims * nlp_dims = ocp_nlp_dims_create(nlp_config); ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nx", nx); ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nu", nu); ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nz", nz); ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "ns", ns); for (int i = 0; i <= N; i++) { ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbx", &nbx[i]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbu", &nbu[i]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbx", &nsbx[i]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbu", &nsbu[i]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "ng", &ng[i]); 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" %} 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" %} for (int i = 1; i < N; i++) ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]); {%- endif %} for (int i = 0; i < N; i++) { {%- if constraints.constr_type == "BGH" and dims.nh > 0 %} ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nh", &nh[i]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsh", &nsh[i]); {%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %} ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nr", &nr[i]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nphi", &nphi[i]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsphi", &nsphi[i]); {%- endif %} } {%- if constraints.constr_type_e == "BGH" %} ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsh", &nsh[N]); {%- elif constraints.constr_type_e == "BGP" %} ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nr", &nr[N]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nphi", &nphi[N]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsphi", &nsphi[N]); {%- endif %} {%- if cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" %} ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]); {%- endif %} free(intNp1mem); {%- if solver_options.integrator_type == "GNSF" -%} // GNSF specific dimensions int gnsf_nx1 = {{ dims.gnsf_nx1 }}; int gnsf_nz1 = {{ dims.gnsf_nz1 }}; int gnsf_nout = {{ dims.gnsf_nout }}; int gnsf_ny = {{ dims.gnsf_ny }}; int gnsf_nuhat = {{ dims.gnsf_nuhat }}; for (int i = 0; i < N; i++) { if (nlp_solver_plan->sim_solver_plan[i].sim_solver == GNSF) { ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nx1", &gnsf_nx1); ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nz1", &gnsf_nz1); ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nout", &gnsf_nout); ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_ny", &gnsf_ny); ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nuhat", &gnsf_nuhat); } } {%- endif %} return nlp_dims; } /** * Internal function for {{ model.name }}_acados_create: step 3 */ void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_solver_capsule* capsule) { const int N = capsule->nlp_solver_plan->N; ocp_nlp_config* nlp_config = capsule->nlp_config; /************************************************ * external functions ************************************************/ #define MAP_CASADI_FNC(__CAPSULE_FNC__, __MODEL_BASE_FNC__) do{ \ capsule->__CAPSULE_FNC__.casadi_fun = & __MODEL_BASE_FNC__ ;\ capsule->__CAPSULE_FNC__.casadi_n_in = & __MODEL_BASE_FNC__ ## _n_in; \ capsule->__CAPSULE_FNC__.casadi_n_out = & __MODEL_BASE_FNC__ ## _n_out; \ capsule->__CAPSULE_FNC__.casadi_sparsity_in = & __MODEL_BASE_FNC__ ## _sparsity_in; \ capsule->__CAPSULE_FNC__.casadi_sparsity_out = & __MODEL_BASE_FNC__ ## _sparsity_out; \ capsule->__CAPSULE_FNC__.casadi_work = & __MODEL_BASE_FNC__ ## _work; \ external_function_param_casadi_create(&capsule->__CAPSULE_FNC__ , {{ dims.np }}); \ }while(false) {% if constraints.constr_type == "BGP" %} // constraints.constr_type == "BGP" capsule->phi_constraint = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { // nonlinear part of convex-composite constraint MAP_CASADI_FNC(phi_constraint[i], {{ model.name }}_phi_constraint); } {%- endif %} {%- if constraints.constr_type_e == "BGP" %} // constraints.constr_type_e == "BGP" // nonlinear part of convex-composite constraint MAP_CASADI_FNC(phi_e_constraint, {{ model.name }}_phi_e_constraint); {%- endif %} {%- if constraints.constr_type == "BGH" and dims.nh > 0 %} // constraints.constr_type == "BGH" and dims.nh > 0 capsule->nl_constr_h_fun_jac = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(nl_constr_h_fun_jac[i], {{ model.name }}_constr_h_fun_jac_uxt_zt); } capsule->nl_constr_h_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(nl_constr_h_fun[i], {{ model.name }}_constr_h_fun); } {% if solver_options.hessian_approx == "EXACT" %} capsule->nl_constr_h_fun_jac_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(nl_constr_h_fun_jac_hess[i], {{ model.name }}_constr_h_fun_jac_uxt_zt_hess); } {% endif %} {% endif %} {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} MAP_CASADI_FNC(nl_constr_h_e_fun_jac, {{ model.name }}_constr_h_e_fun_jac_uxt_zt); MAP_CASADI_FNC(nl_constr_h_e_fun, {{ model.name }}_constr_h_e_fun); {%- if solver_options.hessian_approx == "EXACT" %} MAP_CASADI_FNC(nl_constr_h_e_fun_jac_hess, {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess); {% endif %} {%- endif %} {% if solver_options.integrator_type == "ERK" %} // explicit ode capsule->forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(forw_vde_casadi[i], {{ model.name }}_expl_vde_forw); } capsule->expl_ode_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(expl_ode_fun[i], {{ model.name }}_expl_ode_fun); } {%- if solver_options.hessian_approx == "EXACT" %} capsule->hess_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(hess_vde_casadi[i], {{ model.name }}_expl_ode_hess); } {%- endif %} {% elif solver_options.integrator_type == "IRK" %} // implicit dae capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun); } capsule->impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_z[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_z); } capsule->impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(impl_dae_jac_x_xdot_u_z[i], {{ model.name }}_impl_dae_jac_x_xdot_u_z); } {%- if solver_options.hessian_approx == "EXACT" %} capsule->impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(impl_dae_hess[i], {{ model.name }}_impl_dae_hess); } {%- endif %} {% elif solver_options.integrator_type == "LIFTED_IRK" %} // external functions (implicit model) capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun); } capsule->impl_dae_fun_jac_x_xdot_u = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_u[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_u); } {% elif solver_options.integrator_type == "GNSF" %} {% if model.gnsf.purely_linear != 1 %} capsule->gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(gnsf_phi_fun[i], {{ model.name }}_gnsf_phi_fun); } capsule->gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(gnsf_phi_fun_jac_y[i], {{ model.name }}_gnsf_phi_fun_jac_y); } capsule->gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(gnsf_phi_jac_y_uhat[i], {{ model.name }}_gnsf_phi_jac_y_uhat); } {% if model.gnsf.nontrivial_f_LO == 1 %} capsule->gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(gnsf_f_lo_jac_x1_x1dot_u_z[i], {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz); } {%- endif %} {%- endif %} capsule->gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(gnsf_get_matrices_fun[i], {{ model.name }}_gnsf_get_matrices_fun); } {% elif solver_options.integrator_type == "DISCRETE" %} // discrete dynamics capsule->discr_dyn_phi_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); for (int i = 0; i < N; i++) { {%- if model.dyn_ext_fun_type == "casadi" %} MAP_CASADI_FNC(discr_dyn_phi_fun[i], {{ model.name }}_dyn_disc_phi_fun); {%- else %} capsule->discr_dyn_phi_fun[i].fun = &{{ model.dyn_disc_fun }}; external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun[i], {{ dims.np }}); {%- endif %} } capsule->discr_dyn_phi_fun_jac_ut_xt = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); for (int i = 0; i < N; i++) { {%- if model.dyn_ext_fun_type == "casadi" %} MAP_CASADI_FNC(discr_dyn_phi_fun_jac_ut_xt[i], {{ model.name }}_dyn_disc_phi_fun_jac); {%- else %} capsule->discr_dyn_phi_fun_jac_ut_xt[i].fun = &{{ model.dyn_disc_fun_jac }}; external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt[i], {{ dims.np }}); {%- endif %} } {%- if solver_options.hessian_approx == "EXACT" %} capsule->discr_dyn_phi_fun_jac_ut_xt_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); for (int i = 0; i < N; i++) { {%- if model.dyn_ext_fun_type == "casadi" %} MAP_CASADI_FNC(discr_dyn_phi_fun_jac_ut_xt_hess[i], {{ model.name }}_dyn_disc_phi_fun_jac_hess); {%- else %} capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].fun = &{{ model.dyn_disc_fun_jac_hess }}; external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i], {{ dims.np }}); {%- endif %} } {%- endif %} {%- endif %} {%- if cost.cost_type_0 == "NONLINEAR_LS" %} // nonlinear least squares function MAP_CASADI_FNC(cost_y_0_fun, {{ model.name }}_cost_y_0_fun); MAP_CASADI_FNC(cost_y_0_fun_jac_ut_xt, {{ model.name }}_cost_y_0_fun_jac_ut_xt); MAP_CASADI_FNC(cost_y_0_hess, {{ model.name }}_cost_y_0_hess); {%- elif cost.cost_type_0 == "EXTERNAL" %} // external cost {%- if cost.cost_ext_fun_type_0 == "casadi" %} MAP_CASADI_FNC(ext_cost_0_fun, {{ model.name }}_cost_ext_cost_0_fun); {%- else %} capsule->ext_cost_0_fun.fun = &{{ cost.cost_function_ext_cost_0 }}; external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun, {{ dims.np }}); {%- endif %} // external cost {%- if cost.cost_ext_fun_type_0 == "casadi" %} MAP_CASADI_FNC(ext_cost_0_fun_jac, {{ model.name }}_cost_ext_cost_0_fun_jac); {%- else %} capsule->ext_cost_0_fun_jac.fun = &{{ cost.cost_function_ext_cost_0 }}; external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac, {{ dims.np }}); {%- endif %} // external cost {%- if cost.cost_ext_fun_type_0 == "casadi" %} MAP_CASADI_FNC(ext_cost_0_fun_jac_hess, {{ model.name }}_cost_ext_cost_0_fun_jac_hess); {%- else %} capsule->ext_cost_0_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_0 }}; external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac_hess, {{ dims.np }}); {%- endif %} {%- endif %} {%- if cost.cost_type == "NONLINEAR_LS" %} // nonlinear least squares cost capsule->cost_y_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N-1; i++) { MAP_CASADI_FNC(cost_y_fun[i], {{ model.name }}_cost_y_fun); } capsule->cost_y_fun_jac_ut_xt = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N-1; i++) { MAP_CASADI_FNC(cost_y_fun_jac_ut_xt[i], {{ model.name }}_cost_y_fun_jac_ut_xt); } capsule->cost_y_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N-1; i++) { MAP_CASADI_FNC(cost_y_hess[i], {{ model.name }}_cost_y_hess); } {%- elif cost.cost_type == "EXTERNAL" %} // external cost capsule->ext_cost_fun = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N); for (int i = 0; i < N-1; i++) { {%- if cost.cost_ext_fun_type == "casadi" %} MAP_CASADI_FNC(ext_cost_fun[i], {{ model.name }}_cost_ext_cost_fun); {%- else %} capsule->ext_cost_fun[i].fun = &{{ cost.cost_function_ext_cost }}; external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun[i], {{ dims.np }}); {%- endif %} } capsule->ext_cost_fun_jac = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N); for (int i = 0; i < N-1; i++) { {%- if cost.cost_ext_fun_type == "casadi" %} MAP_CASADI_FNC(ext_cost_fun_jac[i], {{ model.name }}_cost_ext_cost_fun_jac); {%- else %} capsule->ext_cost_fun_jac[i].fun = &{{ cost.cost_function_ext_cost }}; external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac[i], {{ dims.np }}); {%- endif %} } capsule->ext_cost_fun_jac_hess = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N); for (int i = 0; i < N-1; i++) { {%- if cost.cost_ext_fun_type == "casadi" %} MAP_CASADI_FNC(ext_cost_fun_jac_hess[i], {{ model.name }}_cost_ext_cost_fun_jac_hess); {%- else %} capsule->ext_cost_fun_jac_hess[i].fun = &{{ cost.cost_function_ext_cost }}; external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac_hess[i], {{ dims.np }}); {%- endif %} } {%- endif %} {%- if cost.cost_type_e == "NONLINEAR_LS" %} // nonlinear least square function MAP_CASADI_FNC(cost_y_e_fun, {{ model.name }}_cost_y_e_fun); MAP_CASADI_FNC(cost_y_e_fun_jac_ut_xt, {{ model.name }}_cost_y_e_fun_jac_ut_xt); MAP_CASADI_FNC(cost_y_e_hess, {{ model.name }}_cost_y_e_hess); {%- elif cost.cost_type_e == "EXTERNAL" %} // external cost - function {%- if cost.cost_ext_fun_type_e == "casadi" %} MAP_CASADI_FNC(ext_cost_e_fun, {{ model.name }}_cost_ext_cost_e_fun); {% else %} capsule->ext_cost_e_fun.fun = &{{ cost.cost_function_ext_cost_e }}; external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun, {{ dims.np }}); {%- endif %} // external cost - jacobian {%- if cost.cost_ext_fun_type_e == "casadi" %} MAP_CASADI_FNC(ext_cost_e_fun_jac, {{ model.name }}_cost_ext_cost_e_fun_jac); {%- else %} capsule->ext_cost_e_fun_jac.fun = &{{ cost.cost_function_ext_cost_e }}; external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun_jac, {{ dims.np }}); {%- endif %} // external cost - hessian {%- if cost.cost_ext_fun_type_e == "casadi" %} MAP_CASADI_FNC(ext_cost_e_fun_jac_hess, {{ model.name }}_cost_ext_cost_e_fun_jac_hess); {%- else %} capsule->ext_cost_e_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_e }}; external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun_jac_hess, {{ dims.np }}); {%- endif %} {%- endif %} #undef MAP_CASADI_FNC } /** * Internal function for {{ model.name }}_acados_create: step 4 */ void {{ model.name }}_acados_create_4_set_default_parameters({{ model.name }}_solver_capsule* capsule) { {%- if dims.np > 0 %} const int N = capsule->nlp_solver_plan->N; // initialize parameters to nominal value double* p = calloc(NP, sizeof(double)); {%- for item in parameter_values %} {%- if item != 0 %} p[{{ loop.index0 }}] = {{ item }}; {%- endif %} {%- endfor %} for (int i = 0; i <= N; i++) { {{ model.name }}_acados_update_params(capsule, i, p, NP); } free(p); {%- else %} // no parameters defined {%- endif %}{# if dims.np #} } /** * Internal function for {{ model.name }}_acados_create: step 5 */ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule* capsule, const int N, double* new_time_steps) { assert(N == capsule->nlp_solver_plan->N); ocp_nlp_config* nlp_config = capsule->nlp_config; ocp_nlp_dims* nlp_dims = capsule->nlp_dims; /************************************************ * nlp_in ************************************************/ // ocp_nlp_in * nlp_in = ocp_nlp_in_create(nlp_config, nlp_dims); // capsule->nlp_in = nlp_in; ocp_nlp_in * nlp_in = capsule->nlp_in; // set up time_steps {% set all_equal = true -%} {%- set val = solver_options.time_steps[0] %} {%- for j in range(start=1, end=dims.N) %} {%- if val != solver_options.time_steps[j] %} {%- set_global all_equal = false %} {%- break %} {%- endif %} {%- endfor %} if (new_time_steps) { {{ model.name }}_acados_update_time_steps(capsule, N, new_time_steps); } else { {%- if all_equal == true -%} // all time_steps are identical double time_step = {{ solver_options.time_steps[0] }}; for (int i = 0; i < N; i++) { ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_step); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &time_step); } {%- else -%} // time_steps are different double* time_steps = malloc(N*sizeof(double)); {%- for j in range(end=dims.N) %} time_steps[{{ j }}] = {{ solver_options.time_steps[j] }}; {%- endfor %} {{ model.name }}_acados_update_time_steps(capsule, N, time_steps); free(time_steps); {%- endif %} } /**** Dynamics ****/ for (int i = 0; i < N; i++) { {%- if solver_options.integrator_type == "ERK" %} ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_vde_forw", &capsule->forw_vde_casadi[i]); ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_ode_fun", &capsule->expl_ode_fun[i]); {%- if solver_options.hessian_approx == "EXACT" %} ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_ode_hess", &capsule->hess_vde_casadi[i]); {%- endif %} {% elif solver_options.integrator_type == "IRK" %} ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun", &capsule->impl_dae_fun[i]); ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun_jac_x_xdot_z", &capsule->impl_dae_fun_jac_x_xdot_z[i]); ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_jac_x_xdot_u", &capsule->impl_dae_jac_x_xdot_u_z[i]); {%- if solver_options.hessian_approx == "EXACT" %} ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_hess", &capsule->impl_dae_hess[i]); {%- endif %} {% elif solver_options.integrator_type == "LIFTED_IRK" %} ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun", &capsule->impl_dae_fun[i]); ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun_jac_x_xdot_u", &capsule->impl_dae_fun_jac_x_xdot_u[i]); {% elif solver_options.integrator_type == "GNSF" %} {% if model.gnsf.purely_linear != 1 %} ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun", &capsule->gnsf_phi_fun[i]); ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun_jac_y", &capsule->gnsf_phi_fun_jac_y[i]); ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_jac_y_uhat", &capsule->gnsf_phi_jac_y_uhat[i]); {% if model.gnsf.nontrivial_f_LO == 1 %} ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "f_lo_jac_x1_x1dot_u_z", &capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i]); {%- endif %} {%- endif %} ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "gnsf_get_matrices_fun", &capsule->gnsf_get_matrices_fun[i]); {% elif solver_options.integrator_type == "DISCRETE" %} ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "disc_dyn_fun", &capsule->discr_dyn_phi_fun[i]); ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "disc_dyn_fun_jac", &capsule->discr_dyn_phi_fun_jac_ut_xt[i]); {%- if solver_options.hessian_approx == "EXACT" %} ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "disc_dyn_fun_jac_hess", &capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i]); {%- endif %} {%- endif %} } /**** Cost ****/ {%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %} {%- if dims.ny_0 > 0 %} double* W_0 = calloc(NY0*NY0, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny_0) %} {%- for k in range(end=dims.ny_0) %} {%- if cost.W_0[j][k] != 0 %} W_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.W_0[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0); free(W_0); double* yref_0 = calloc(NY0, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny_0) %} {%- if cost.yref_0[j] != 0 %} yref_0[{{ j }}] = {{ cost.yref_0[j] }}; {%- endif %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0); free(yref_0); {%- endif %} {%- endif %} {%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %} {%- if dims.ny > 0 %} double* W = calloc(NY*NY, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny) %} {%- for k in range(end=dims.ny) %} {%- if cost.W[j][k] != 0 %} W[{{ j }}+(NY) * {{ k }}] = {{ cost.W[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} double* yref = calloc(NY, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny) %} {%- if cost.yref[j] != 0 %} yref[{{ j }}] = {{ cost.yref[j] }}; {%- endif %} {%- endfor %} for (int i = 1; i < N; i++) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref); } free(W); free(yref); {%- endif %} {%- endif %} {%- if cost.cost_type_0 == "LINEAR_LS" %} double* Vx_0 = calloc(NY0*NX, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny_0) %} {%- for k in range(end=dims.nx) %} {%- if cost.Vx_0[j][k] != 0 %} Vx_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vx_0[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0); free(Vx_0); {%- if dims.ny_0 > 0 and dims.nu > 0 %} double* Vu_0 = calloc(NY0*NU, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny_0) %} {%- for k in range(end=dims.nu) %} {%- if cost.Vu_0[j][k] != 0 %} Vu_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vu_0[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vu", Vu_0); free(Vu_0); {%- endif %} {%- if dims.ny_0 > 0 and dims.nz > 0 %} double* Vz_0 = calloc(NY0*NZ, sizeof(double)); // change only the non-zero elements: {% for j in range(end=dims.ny_0) %} {%- for k in range(end=dims.nz) %} {%- if cost.Vz_0[j][k] != 0 %} Vz_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vz_0[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vz", Vz_0); free(Vz_0); {%- endif %} {%- endif %}{# LINEAR LS #} {%- if cost.cost_type == "LINEAR_LS" %} double* Vx = calloc(NY*NX, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny) %} {%- for k in range(end=dims.nx) %} {%- if cost.Vx[j][k] != 0 %} Vx[{{ j }}+(NY) * {{ k }}] = {{ cost.Vx[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} for (int i = 1; i < N; i++) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vx", Vx); } free(Vx); {% if dims.ny > 0 and dims.nu > 0 %} double* Vu = calloc(NY*NU, sizeof(double)); // change only the non-zero elements: {% for j in range(end=dims.ny) %} {%- for k in range(end=dims.nu) %} {%- if cost.Vu[j][k] != 0 %} Vu[{{ j }}+(NY) * {{ k }}] = {{ cost.Vu[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} for (int i = 1; i < N; i++) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vu", Vu); } free(Vu); {%- endif %} {%- if dims.ny > 0 and dims.nz > 0 %} double* Vz = calloc(NY*NZ, sizeof(double)); // change only the non-zero elements: {% for j in range(end=dims.ny) %} {%- for k in range(end=dims.nz) %} {%- if cost.Vz[j][k] != 0 %} Vz[{{ j }}+(NY) * {{ k }}] = {{ cost.Vz[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} for (int i = 1; i < N; i++) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vz", Vz); } free(Vz); {%- endif %} {%- endif %}{# LINEAR LS #} {%- if cost.cost_type_0 == "NONLINEAR_LS" %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun", &capsule->cost_y_0_fun); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun_jac", &capsule->cost_y_0_fun_jac_ut_xt); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_hess", &capsule->cost_y_0_hess); {%- elif cost.cost_type_0 == "EXTERNAL" %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun", &capsule->ext_cost_0_fun); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun_jac", &capsule->ext_cost_0_fun_jac); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun_jac_hess", &capsule->ext_cost_0_fun_jac_hess); {%- endif %} {%- if cost.cost_type == "NONLINEAR_LS" %} for (int i = 1; i < N; i++) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun", &capsule->cost_y_fun[i-1]); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun_jac", &capsule->cost_y_fun_jac_ut_xt[i-1]); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_hess", &capsule->cost_y_hess[i-1]); } {%- elif cost.cost_type == "EXTERNAL" %} for (int i = 1; i < N; i++) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "ext_cost_fun", &capsule->ext_cost_fun[i-1]); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "ext_cost_fun_jac", &capsule->ext_cost_fun_jac[i-1]); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "ext_cost_fun_jac_hess", &capsule->ext_cost_fun_jac_hess[i-1]); } {%- endif %} {%- if dims.ns > 0 %} double* zlumem = calloc(4*NS, sizeof(double)); double* Zl = zlumem+NS*0; double* Zu = zlumem+NS*1; double* zl = zlumem+NS*2; double* zu = zlumem+NS*3; // change only the non-zero elements: {%- for j in range(end=dims.ns) %} {%- if cost.Zl[j] != 0 %} Zl[{{ j }}] = {{ cost.Zl[j] }}; {%- endif %} {%- endfor %} {%- for j in range(end=dims.ns) %} {%- if cost.Zu[j] != 0 %} Zu[{{ j }}] = {{ cost.Zu[j] }}; {%- endif %} {%- endfor %} {%- for j in range(end=dims.ns) %} {%- if cost.zl[j] != 0 %} zl[{{ j }}] = {{ cost.zl[j] }}; {%- endif %} {%- endfor %} {%- for j in range(end=dims.ns) %} {%- if cost.zu[j] != 0 %} zu[{{ j }}] = {{ cost.zu[j] }}; {%- endif %} {%- endfor %} for (int i = 0; i < N; i++) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Zl", Zl); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Zu", Zu); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zl", zl); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zu", zu); } free(zlumem); {%- endif %} // terminal cost {%- if cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "NONLINEAR_LS" %} {%- if dims.ny_e > 0 %} double* yref_e = calloc(NYN, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny_e) %} {%- if cost.yref_e[j] != 0 %} yref_e[{{ j }}] = {{ cost.yref_e[j] }}; {%- endif %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e); free(yref_e); double* W_e = calloc(NYN*NYN, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny_e) %} {%- for k in range(end=dims.ny_e) %} {%- if cost.W_e[j][k] != 0 %} W_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.W_e[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); free(W_e); {%- if cost.cost_type_e == "LINEAR_LS" %} double* Vx_e = calloc(NYN*NX, sizeof(double)); // change only the non-zero elements: {% for j in range(end=dims.ny_e) %} {%- for k in range(end=dims.nx) %} {%- if cost.Vx_e[j][k] != 0 %} Vx_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.Vx_e[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); free(Vx_e); {%- endif %} {%- if cost.cost_type_e == "NONLINEAR_LS" %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess); {%- endif %} {%- endif %}{# ny_e > 0 #} {%- elif cost.cost_type_e == "EXTERNAL" %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun", &capsule->ext_cost_e_fun); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac", &capsule->ext_cost_e_fun_jac); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac_hess", &capsule->ext_cost_e_fun_jac_hess); {%- endif %} {% if dims.ns_e > 0 %} double* zluemem = calloc(4*NSN, sizeof(double)); double* Zl_e = zluemem+NSN*0; double* Zu_e = zluemem+NSN*1; double* zl_e = zluemem+NSN*2; double* zu_e = zluemem+NSN*3; // change only the non-zero elements: {% for j in range(end=dims.ns_e) %} {%- if cost.Zl_e[j] != 0 %} Zl_e[{{ j }}] = {{ cost.Zl_e[j] }}; {%- endif %} {%- endfor %} {% for j in range(end=dims.ns_e) %} {%- if cost.Zu_e[j] != 0 %} Zu_e[{{ j }}] = {{ cost.Zu_e[j] }}; {%- endif %} {%- endfor %} {% for j in range(end=dims.ns_e) %} {%- if cost.zl_e[j] != 0 %} zl_e[{{ j }}] = {{ cost.zl_e[j] }}; {%- endif %} {%- endfor %} {% for j in range(end=dims.ns_e) %} {%- if cost.zu_e[j] != 0 %} zu_e[{{ j }}] = {{ cost.zu_e[j] }}; {%- endif %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Zl", Zl_e); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Zu", Zu_e); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "zl", zl_e); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "zu", zu_e); free(zluemem); {%- endif %} /**** Constraints ****/ // bounds for initial stage {%- if dims.nbx_0 > 0 %} // x0 int* idxbx0 = malloc(NBX0 * sizeof(int)); {%- for i in range(end=dims.nbx_0) %} idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }}; {%- endfor %} double* lubx0 = calloc(2*NBX0, sizeof(double)); double* lbx0 = lubx0; double* ubx0 = lubx0 + NBX0; // change only the non-zero elements: {%- for i in range(end=dims.nbx_0) %} {%- if constraints.lbx_0[i] != 0 %} lbx0[{{ i }}] = {{ constraints.lbx_0[i] }}; {%- endif %} {%- if constraints.ubx_0[i] != 0 %} ubx0[{{ i }}] = {{ constraints.ubx_0[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); free(idxbx0); free(lubx0); {%- endif %} {%- if dims.nbxe_0 > 0 %} // idxbxe_0 int* idxbxe_0 = malloc({{ dims.nbxe_0 }} * sizeof(int)); {% 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); free(idxbxe_0); {%- endif %} /* 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); // soft bounds on x int* idxsbx = malloc(NSBX * sizeof(int)); {%- for i in range(end=dims.nsbx) %} idxsbx[{{ i }}] = {{ constraints.idxsbx[i] }}; {%- endfor %} double* lusbx = calloc(2*NSBX, sizeof(double)); double* lsbx = lusbx; double* usbx = lusbx + NSBX; {%- for i in range(end=dims.nsbx) %} {%- if constraints.lsbx[i] != 0 %} lsbx[{{ i }}] = {{ constraints.lsbx[i] }}; {%- endif %} {%- if constraints.usbx[i] != 0 %} usbx[{{ i }}] = {{ constraints.usbx[i] }}; {%- endif %} {%- endfor %} 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); } free(idxsbx); free(lusbx); {%- endif %} {%- if dims.nbu > 0 %} // u int* idxbu = malloc(NBU * sizeof(int)); {% for i in range(end=dims.nbu) %} idxbu[{{ i }}] = {{ constraints.idxbu[i] }}; {%- endfor %} double* lubu = calloc(2*NBU, sizeof(double)); double* lbu = lubu; double* ubu = lubu + NBU; {% for i in range(end=dims.nbu) %} {%- if constraints.lbu[i] != 0 %} lbu[{{ i }}] = {{ constraints.lbu[i] }}; {%- endif %} {%- if constraints.ubu[i] != 0 %} ubu[{{ i }}] = {{ constraints.ubu[i] }}; {%- endif %} {%- endfor %} 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); } free(idxbu); free(lubu); {%- endif %} {%- if dims.nsbu > 0 %} // set up soft bounds for u int* idxsbu = malloc(NSBU * sizeof(int)); {% for i in range(end=dims.nsbu) %} idxsbu[{{ i }}] = {{ constraints.idxsbu[i] }}; {%- endfor %} double* lusbu = calloc(2*NSBU, sizeof(double)); double* lsbu = lusbu; double* usbu = lusbu + NSBU; {% for i in range(end=dims.nsbu) %} {%- if constraints.lsbu[i] != 0 %} lsbu[{{ i }}] = {{ constraints.lsbu[i] }}; {%- endif %} {%- if constraints.usbu[i] != 0 %} usbu[{{ i }}] = {{ constraints.usbu[i] }}; {%- endif %} {%- 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); } free(idxsbu); free(lusbu); {%- endif %} {% if dims.nsg > 0 %} // set up soft bounds for general linear constraints int* idxsg = malloc(NSG * sizeof(int)); {% for i in range(end=dims.nsg) %} idxsg[{{ i }}] = {{ constraints.idxsg[i] }}; {%- endfor %} double* lusg = calloc(2*NSG, sizeof(double)); double* lsg = lusg; double* usg = lusg + NSG; {% for i in range(end=dims.nsg) %} {%- if constraints.lsg[i] != 0 %} lsg[{{ i }}] = {{ constraints.lsg[i] }}; {%- endif %} {%- if constraints.usg[i] != 0 %} usg[{{ i }}] = {{ constraints.usg[i] }}; {%- endif %} {%- endfor %} 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); } free(idxsg); free(lusg); {%- endif %} {% if dims.nsh > 0 %} // set up soft bounds for nonlinear constraints int* idxsh = malloc(NSH * sizeof(int)); {% for i in range(end=dims.nsh) %} idxsh[{{ i }}] = {{ constraints.idxsh[i] }}; {%- endfor %} double* lush = calloc(2*NSH, sizeof(double)); double* lsh = lush; double* ush = lush + NSH; {% for i in range(end=dims.nsh) %} {%- if constraints.lsh[i] != 0 %} lsh[{{ i }}] = {{ constraints.lsh[i] }}; {%- endif %} {%- if constraints.ush[i] != 0 %} ush[{{ i }}] = {{ constraints.ush[i] }}; {%- endif %} {%- endfor %} for (int i = 0; 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); } free(idxsh); free(lush); {%- endif %} {% if dims.nsphi > 0 %} // set up soft bounds for convex-over-nonlinear constraints int* idxsphi = malloc(NSPHI * sizeof(int)); {% for i in range(end=dims.nsphi) %} idxsphi[{{ i }}] = {{ constraints.idxsphi[i] }}; {%- endfor %} double* lusphi = calloc(2*NSPHI, sizeof(double)); double* lsphi = lusphi; double* usphi = lusphi + NSPHI; {% for i in range(end=dims.nsphi) %} {%- if constraints.lsphi[i] != 0 %} lsphi[{{ i }}] = {{ constraints.lsphi[i] }}; {%- endif %} {%- if constraints.usphi[i] != 0 %} usphi[{{ i }}] = {{ constraints.usphi[i] }}; {%- endif %} {%- endfor %} for (int i = 0; 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); } free(idxsphi); free(lusphi); {%- endif %} {% if dims.nbx > 0 %} // x int* idxbx = malloc(NBX * sizeof(int)); {% for i in range(end=dims.nbx) %} idxbx[{{ i }}] = {{ constraints.idxbx[i] }}; {%- endfor %} double* lubx = calloc(2*NBX, sizeof(double)); double* lbx = lubx; double* ubx = lubx + NBX; {% for i in range(end=dims.nbx) %} {%- if constraints.lbx[i] != 0 %} lbx[{{ i }}] = {{ constraints.lbx[i] }}; {%- endif %} {%- if constraints.ubx[i] != 0 %} ubx[{{ i }}] = {{ constraints.ubx[i] }}; {%- endif %} {%- endfor %} 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); } free(idxbx); free(lubx); {%- endif %} {% if dims.ng > 0 %} // set up general constraints for stage 0 to N-1 double* D = calloc(NG*NU, sizeof(double)); double* C = calloc(NG*NX, sizeof(double)); double* lug = calloc(2*NG, sizeof(double)); double* lg = lug; double* ug = lug + NG; {% for j in range(end=dims.ng) -%} {% for k in range(end=dims.nu) %} {%- if constraints.D[j][k] != 0 %} D[{{ j }}+NG * {{ k }}] = {{ constraints.D[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} {% for j in range(end=dims.ng) -%} {% for k in range(end=dims.nx) %} {%- if constraints.C[j][k] != 0 %} C[{{ j }}+NG * {{ k }}] = {{ constraints.C[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} {% for i in range(end=dims.ng) %} {%- if constraints.lg[i] != 0 %} lg[{{ i }}] = {{ constraints.lg[i] }}; {%- endif %} {%- endfor %} {% for i in range(end=dims.ng) %} {%- if constraints.ug[i] != 0 %} ug[{{ i }}] = {{ constraints.ug[i] }}; {%- endif %} {%- endfor %} 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); } free(D); free(C); free(lug); {%- endif %} {% if dims.nh > 0 %} // set up nonlinear constraints for stage 0 to N-1 double* luh = calloc(2*NH, sizeof(double)); double* lh = luh; double* uh = luh + NH; {% for i in range(end=dims.nh) %} {%- if constraints.lh[i] != 0 %} lh[{{ i }}] = {{ constraints.lh[i] }}; {%- endif %} {%- endfor %} {% for i in range(end=dims.nh) %} {%- if constraints.uh[i] != 0 %} uh[{{ i }}] = {{ constraints.uh[i] }}; {%- endif %} {%- endfor %} for (int i = 0; i < N; i++) { // nonlinear constraints for stages 0 to N-1 ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun_jac", &capsule->nl_constr_h_fun_jac[i]); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun", &capsule->nl_constr_h_fun[i]); {% if solver_options.hessian_approx == "EXACT" %} ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_fun_jac_hess[i]); {% 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 %} {% if dims.nphi > 0 and constraints.constr_type == "BGP" %} // set up convex-over-nonlinear constraints for stage 0 to N-1 double* luphi = calloc(2*NPHI, sizeof(double)); double* lphi = luphi; double* uphi = luphi + NPHI; {% for i in range(end=dims.nphi) %} {%- if constraints.lphi[i] != 0 %} lphi[{{ i }}] = {{ constraints.lphi[i] }}; {%- endif %} {%- endfor %} {% for i in range(end=dims.nphi) %} {%- if constraints.uphi[i] != 0 %} uphi[{{ i }}] = {{ constraints.uphi[i] }}; {%- endif %} {%- endfor %} for (int i = 0; i < N; i++) { ocp_nlp_constraints_model_set(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[i]); 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); } free(luphi); {%- endif %} /* terminal constraints */ {% if dims.nbx_e > 0 %} // set up bounds for last stage // x int* idxbx_e = malloc(NBXN * sizeof(int)); {% for i in range(end=dims.nbx_e) %} idxbx_e[{{ i }}] = {{ constraints.idxbx_e[i] }}; {%- endfor %} double* lubx_e = calloc(2*NBXN, sizeof(double)); double* lbx_e = lubx_e; double* ubx_e = lubx_e + NBXN; {% for i in range(end=dims.nbx_e) %} {%- if constraints.lbx_e[i] != 0 %} lbx_e[{{ i }}] = {{ constraints.lbx_e[i] }}; {%- endif %} {%- if constraints.ubx_e[i] != 0 %} 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); free(idxbx_e); free(lubx_e); {%- endif %} {% if dims.nsg_e > 0 %} // set up soft bounds for general linear constraints int* idxsg_e = calloc(NSGN, sizeof(int)); {% for i in range(end=dims.nsg_e) %} idxsg_e[{{ i }}] = {{ constraints.idxsg_e[i] }}; {%- endfor %} double* lusg_e = calloc(2*NSGN, sizeof(double)); double* lsg_e = lusg_e; double* usg_e = lusg_e + NSGN; {% for i in range(end=dims.nsg_e) %} {%- if constraints.lsg_e[i] != 0 %} lsg_e[{{ i }}] = {{ constraints.lsg_e[i] }}; {%- endif %} {%- if constraints.usg_e[i] != 0 %} usg_e[{{ i }}] = {{ constraints.usg_e[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); free(idxsg_e); free(lusg_e); {%- endif %} {% if dims.nsh_e > 0 %} // set up soft bounds for nonlinear constraints int* idxsh_e = malloc(NSHN * sizeof(int)); {% for i in range(end=dims.nsh_e) %} idxsh_e[{{ i }}] = {{ constraints.idxsh_e[i] }}; {%- endfor %} double* lush_e = calloc(2*NSHN, sizeof(double)); double* lsh_e = lush_e; double* ush_e = lush_e + NSHN; {% for i in range(end=dims.nsh_e) %} {%- if constraints.lsh_e[i] != 0 %} lsh_e[{{ i }}] = {{ constraints.lsh_e[i] }}; {%- endif %} {%- if constraints.ush_e[i] != 0 %} ush_e[{{ i }}] = {{ constraints.ush_e[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); free(idxsh_e); free(lush_e); {%- endif %} {% if dims.nsphi_e > 0 %} // set up soft bounds for convex-over-nonlinear constraints int* idxsphi_e = malloc(NSPHIN * sizeof(int)); {% for i in range(end=dims.nsphi_e) %} idxsphi_e[{{ i }}] = {{ constraints.idxsphi_e[i] }}; {%- endfor %} double* lusphi_e = calloc(2*NSPHIN, sizeof(double)); double* lsphi_e = lusphi_e; double* usphi_e = lusphi_e + NSPHIN; {% for i in range(end=dims.nsphi_e) %} {%- if constraints.lsphi_e[i] != 0 %} lsphi_e[{{ i }}] = {{ constraints.lsphi_e[i] }}; {%- endif %} {%- if constraints.usphi_e[i] != 0 %} usphi_e[{{ i }}] = {{ constraints.usphi_e[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); free(idxsphi_e); free(lusphi_e); {%- endif %} {% if dims.nsbx_e > 0 %} // soft bounds on x int* idxsbx_e = malloc(NSBXN * sizeof(int)); {% for i in range(end=dims.nsbx_e) %} idxsbx_e[{{ i }}] = {{ constraints.idxsbx_e[i] }}; {%- endfor %} double* lusbx_e = calloc(2*NSBXN, sizeof(double)); double* lsbx_e = lusbx_e; double* usbx_e = lusbx_e + NSBXN; {% for i in range(end=dims.nsbx_e) %} {%- if constraints.lsbx_e[i] != 0 %} lsbx_e[{{ i }}] = {{ constraints.lsbx_e[i] }}; {%- endif %} {%- if constraints.usbx_e[i] != 0 %} usbx_e[{{ i }}] = {{ constraints.usbx_e[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); free(idxsbx_e); free(lusbx_e); {% endif %} {% if dims.ng_e > 0 %} // set up general constraints for last stage double* C_e = calloc(NGN*NX, sizeof(double)); double* lug_e = calloc(2*NGN, sizeof(double)); double* lg_e = lug_e; double* ug_e = lug_e + NGN; {% for j in range(end=dims.ng) %} {%- for k in range(end=dims.nx) %} {%- if constraints.C_e[j][k] != 0 %} C_e[{{ j }}+NG * {{ k }}] = {{ constraints.C_e[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} {% for i in range(end=dims.ng_e) %} {%- if constraints.lg_e[i] != 0 %} lg_e[{{ i }}] = {{ constraints.lg_e[i] }}; {%- endif %} {%- if constraints.ug_e[i] != 0 %} ug_e[{{ i }}] = {{ constraints.ug_e[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); free(C_e); free(lug_e); {%- endif %} {% if dims.nh_e > 0 %} // set up nonlinear constraints for last stage double* luh_e = calloc(2*NHN, sizeof(double)); double* lh_e = luh_e; double* uh_e = luh_e + NHN; {% for i in range(end=dims.nh_e) %} {%- if constraints.lh_e[i] != 0 %} lh_e[{{ i }}] = {{ constraints.lh_e[i] }}; {%- endif %} {%- endfor %} {% for i in range(end=dims.nh_e) %} {%- if constraints.uh_e[i] != 0 %} uh_e[{{ i }}] = {{ constraints.uh_e[i] }}; {%- endif %} {%- endfor %} ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac", &capsule->nl_constr_h_e_fun_jac); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun", &capsule->nl_constr_h_e_fun); {% if solver_options.hessian_approx == "EXACT" %} ocp_nlp_constraints_model_set(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, N, "lh", lh_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", uh_e); free(luh_e); {%- endif %} {% if dims.nphi_e > 0 and constraints.constr_type_e == "BGP" %} // set up convex-over-nonlinear constraints for last stage double* luphi_e = calloc(2*NPHIN, sizeof(double)); double* lphi_e = luphi_e; double* uphi_e = luphi_e + NPHIN; {% for i in range(end=dims.nphi_e) %} {%- if constraints.lphi_e[i] != 0 %} lphi_e[{{ i }}] = {{ constraints.lphi_e[i] }}; {%- endif %} {%- if constraints.uphi_e[i] != 0 %} uphi_e[{{ i }}] = {{ constraints.uphi_e[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, N, "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_e_constraint); free(luphi_e); {% endif %} } /** * Internal function for {{ model.name }}_acados_create: step 6 */ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule* capsule) { const int N = capsule->nlp_solver_plan->N; ocp_nlp_config* nlp_config = capsule->nlp_config; ocp_nlp_dims* nlp_dims = capsule->nlp_dims; void *nlp_opts = capsule->nlp_opts; /************************************************ * opts ************************************************/ {% if solver_options.hessian_approx == "EXACT" %} bool nlp_solver_exact_hessian = true; // TODO: this if should not be needed! however, calling the setter with false leads to weird behavior. Investigate! if (nlp_solver_exact_hessian) { ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess", &nlp_solver_exact_hessian); } int exact_hess_dyn = {{ solver_options.exact_hess_dyn }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_dyn", &exact_hess_dyn); int exact_hess_cost = {{ solver_options.exact_hess_cost }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_cost", &exact_hess_cost); int exact_hess_constr = {{ solver_options.exact_hess_constr }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_constr", &exact_hess_constr); {%- endif -%} {%- if solver_options.globalization == "FIXED_STEP" %} ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization", "fixed_step"); {%- elif solver_options.globalization == "MERIT_BACKTRACKING" %} ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization", "merit_backtracking"); double alpha_min = {{ solver_options.alpha_min }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "alpha_min", &alpha_min); double alpha_reduction = {{ solver_options.alpha_reduction }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "alpha_reduction", &alpha_reduction); int line_search_use_sufficient_descent = {{ solver_options.line_search_use_sufficient_descent }}; ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "line_search_use_sufficient_descent", &line_search_use_sufficient_descent); int globalization_use_SOC = {{ solver_options.globalization_use_SOC }}; ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_use_SOC", &globalization_use_SOC); double eps_sufficient_descent = {{ solver_options.eps_sufficient_descent }}; ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "eps_sufficient_descent", &eps_sufficient_descent); {%- endif -%} int full_step_dual = {{ solver_options.full_step_dual }}; ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "full_step_dual", &full_step_dual); {%- if dims.nz > 0 %} // TODO: these options are lower level -> should be encapsulated! maybe through hessian approx option. bool output_z_val = true; bool sens_algebraic_val = true; for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_output_z", &output_z_val); for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_sens_algebraic", &sens_algebraic_val); {%- endif %} {%- if solver_options.integrator_type != "DISCRETE" %} // set collocation type (relevant for implicit integrators) sim_collocation_type collocation_type = {{ solver_options.collocation_type }}; for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_collocation_type", &collocation_type); // set up sim_method_num_steps {%- set all_equal = true %} {%- set val = solver_options.sim_method_num_steps[0] %} {%- for j in range(start=1, end=dims.N) %} {%- if val != solver_options.sim_method_num_steps[j] %} {%- set_global all_equal = false %} {%- break %} {%- endif %} {%- endfor %} {%- if all_equal == true %} // all sim_method_num_steps are identical int sim_method_num_steps = {{ solver_options.sim_method_num_steps[0] }}; for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps); {%- else %} // sim_method_num_steps are different int* sim_method_num_steps = malloc(N*sizeof(int)); {%- for j in range(end=dims.N) %} sim_method_num_steps[{{ j }}] = {{ solver_options.sim_method_num_steps[j] }}; {%- endfor %} for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps[i]); free(sim_method_num_steps); {%- endif %} // set up sim_method_num_stages {%- set all_equal = true %} {%- set val = solver_options.sim_method_num_stages[0] %} {%- for j in range(start=1, end=dims.N) %} {%- if val != solver_options.sim_method_num_stages[j] %} {%- set_global all_equal = false %} {%- break %} {%- endif %} {%- endfor %} {%- if all_equal == true %} // all sim_method_num_stages are identical int sim_method_num_stages = {{ solver_options.sim_method_num_stages[0] }}; for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages); {%- else %} int* sim_method_num_stages = malloc(N*sizeof(int)); {%- for j in range(end=dims.N) %} sim_method_num_stages[{{ j }}] = {{ solver_options.sim_method_num_stages[j] }}; {%- endfor %} for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages[i]); free(sim_method_num_stages); {%- endif %} int newton_iter_val = {{ solver_options.sim_method_newton_iter }}; for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_iter", &newton_iter_val); // set up sim_method_jac_reuse {%- set all_equal = true %} {%- set val = solver_options.sim_method_jac_reuse[0] %} {%- for j in range(start=1, end=dims.N) %} {%- if val != solver_options.sim_method_jac_reuse[j] %} {%- set_global all_equal = false %} {%- break %} {%- endif %} {%- endfor %} {%- if all_equal == true %} bool tmp_bool = (bool) {{ solver_options.sim_method_jac_reuse[0] }}; for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool); {%- else %} bool* sim_method_jac_reuse = malloc(N*sizeof(bool)); {%- for j in range(end=dims.N) %} sim_method_jac_reuse[{{ j }}] = (bool){{ solver_options.sim_method_jac_reuse[j] }}; {%- endfor %} for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &sim_method_jac_reuse[i]); free(sim_method_jac_reuse); {%- endif %} {%- endif %} double nlp_solver_step_length = {{ solver_options.nlp_solver_step_length }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "step_length", &nlp_solver_step_length); {%- if solver_options.nlp_solver_warm_start_first_qp %} int nlp_solver_warm_start_first_qp = {{ solver_options.nlp_solver_warm_start_first_qp }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "warm_start_first_qp", &nlp_solver_warm_start_first_qp); {%- endif %} double levenberg_marquardt = {{ solver_options.levenberg_marquardt }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt); /* options QP solver */ {%- if solver_options.qp_solver is starting_with("PARTIAL_CONDENSING") %} int qp_solver_cond_N; {% if solver_options.qp_solver_cond_N -%} const int qp_solver_cond_N_ori = {{ solver_options.qp_solver_cond_N }}; qp_solver_cond_N = N < qp_solver_cond_N_ori ? N : qp_solver_cond_N_ori; // use the minimum value here {%- else %} // NOTE: there is no condensing happening here! qp_solver_cond_N = N; {%- endif %} ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N); {%- endif %} {%- if solver_options.qp_solver is containing("HPIPM") %} // set HPIPM mode: should be done before setting other QP solver options ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_hpipm_mode", "{{ solver_options.hpipm_mode }}"); {%- endif %} {% if solver_options.nlp_solver_type == "SQP" %} // set SQP specific options double nlp_solver_tol_stat = {{ solver_options.nlp_solver_tol_stat }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_stat", &nlp_solver_tol_stat); double nlp_solver_tol_eq = {{ solver_options.nlp_solver_tol_eq }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_eq", &nlp_solver_tol_eq); double nlp_solver_tol_ineq = {{ solver_options.nlp_solver_tol_ineq }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_ineq", &nlp_solver_tol_ineq); double nlp_solver_tol_comp = {{ solver_options.nlp_solver_tol_comp }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_comp", &nlp_solver_tol_comp); int nlp_solver_max_iter = {{ solver_options.nlp_solver_max_iter }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "max_iter", &nlp_solver_max_iter); int initialize_t_slacks = {{ solver_options.initialize_t_slacks }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "initialize_t_slacks", &initialize_t_slacks); {%- endif %} int qp_solver_iter_max = {{ solver_options.qp_solver_iter_max }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max); {# NOTE: qp_solver tolerances must be set after NLP ones, since the setter for NLP tolerances sets the QP tolerances to the sam values. #} {%- if solver_options.qp_solver_tol_stat %} double qp_solver_tol_stat = {{ solver_options.qp_solver_tol_stat }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_stat", &qp_solver_tol_stat); {%- endif -%} {%- if solver_options.qp_solver_tol_eq %} double qp_solver_tol_eq = {{ solver_options.qp_solver_tol_eq }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_eq", &qp_solver_tol_eq); {%- endif -%} {%- if solver_options.qp_solver_tol_ineq %} double qp_solver_tol_ineq = {{ solver_options.qp_solver_tol_ineq }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_ineq", &qp_solver_tol_ineq); {%- endif -%} {%- if solver_options.qp_solver_tol_comp %} double qp_solver_tol_comp = {{ solver_options.qp_solver_tol_comp }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_comp", &qp_solver_tol_comp); {%- endif -%} {%- if solver_options.qp_solver_warm_start %} int qp_solver_warm_start = {{ solver_options.qp_solver_warm_start }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_warm_start", &qp_solver_warm_start); {%- endif -%} int print_level = {{ solver_options.print_level }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level); int ext_cost_num_hess = {{ solver_options.ext_cost_num_hess }}; {%- if cost.cost_type == "EXTERNAL" %} 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); } {%- endif %} {%- if cost.cost_type_e == "EXTERNAL" %} ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, N, "cost_numerical_hessian", &ext_cost_num_hess); {%- endif %} } /** * Internal function for {{ model.name }}_acados_create: step 7 */ void {{ model.name }}_acados_create_7_set_nlp_out({{ model.name }}_solver_capsule* capsule) { const int N = capsule->nlp_solver_plan->N; ocp_nlp_config* nlp_config = capsule->nlp_config; ocp_nlp_dims* nlp_dims = capsule->nlp_dims; ocp_nlp_out* nlp_out = capsule->nlp_out; // initialize primal solution double* xu0 = calloc(NX+NU, sizeof(double)); double* x0 = xu0; {% if dims.nbx_0 == dims.nx %} // initialize with x0 {% for item in constraints.lbx_0 %} {%- if item != 0 %} x0[{{ loop.index0 }}] = {{ item }}; {%- endif %} {%- endfor %} {% else %} // initialize with zeros {%- endif %} double* u0 = xu0 + NX; for (int i = 0; i < N; i++) { // x0 ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, 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, N, "x", x0); free(xu0); } /** * Internal function for {{ model.name }}_acados_create: step 8 */ //void {{ model.name }}_acados_create_8_create_solver({{ model.name }}_solver_capsule* capsule) //{ // capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts); //} /** * Internal function for {{ model.name }}_acados_create: step 9 */ int {{ model.name }}_acados_create_9_precompute({{ model.name }}_solver_capsule* capsule) { int status = ocp_nlp_precompute(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); if (status != ACADOS_SUCCESS) { printf("\nocp_nlp_precompute failed!\n\n"); exit(1); } return status; } int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule* capsule, int N, double* new_time_steps) { // If N does not match the number of shooting intervals used for code generation, new_time_steps must be given. if (N != {{ model.name | upper }}_N && !new_time_steps) { fprintf(stderr, "{{ model.name }}_acados_create_with_discretization: new_time_steps is NULL " \ "but the number of shooting intervals (= %d) differs from the number of " \ "shooting intervals (= %d) during code generation! Please provide a new vector of time_stamps!\n", \ N, {{ model.name | upper }}_N); return 1; } // number of expected runtime parameters capsule->nlp_np = NP; // 1) create and set nlp_solver_plan; create nlp_config capsule->nlp_solver_plan = ocp_nlp_plan_create(N); {{ model.name }}_acados_create_1_set_plan(capsule->nlp_solver_plan, N); capsule->nlp_config = ocp_nlp_config_create(*capsule->nlp_solver_plan); // 3) create and set dimensions capsule->nlp_dims = {{ model.name }}_acados_create_2_create_and_set_dimensions(capsule); {{ model.name }}_acados_create_3_create_and_set_functions(capsule); // 4) set default parameters in functions {{ model.name }}_acados_create_4_set_default_parameters(capsule); // 5) create and set nlp_in capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims); {{ model.name }}_acados_create_5_set_nlp_in(capsule, N, new_time_steps); // 6) create and set nlp_opts capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims); {{ model.name }}_acados_create_6_set_opts(capsule); // 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_create_7_set_nlp_out(capsule); // 8) create solver capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts); //{{ model.name }}_acados_create_8_create_solver(capsule); // 9) do precomputations int status = {{ model.name }}_acados_create_9_precompute(capsule); return status; } /** * This function is for updating an already initialized solver with a different number of qp_cond_N. It is useful for code reuse after code export. */ 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") %} // 1) destroy solver ocp_nlp_solver_destroy(capsule->nlp_solver); // 2) set new value for "qp_cond_N" const int N = capsule->nlp_solver_plan->N; if(qp_solver_cond_N > N) printf("Warning: qp_solver_cond_N = %d > N = %d\n", qp_solver_cond_N, N); ocp_nlp_solver_opts_set(capsule->nlp_config, capsule->nlp_opts, "qp_cond_N", &qp_solver_cond_N); // 3) continue with the remaining steps from {{ model.name }}_acados_create_with_discretization(...): // -> 8) create solver capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts); // -> 9) do precomputations int status = {{ model.name }}_acados_create_9_precompute(capsule); return status; {%- else %} printf("\nacados_update_qp_solver_cond_N() failed, since no partial condensing solver is used!\n\n"); // Todo: what is an adequate behavior here? exit(1); return -1; {%- endif %} } int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule) { // set initialization to all zeros {# TODO: use guess values / initial state value from json instead?! #} const int N = capsule->nlp_solver_plan->N; 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; ocp_nlp_solver* nlp_solver = capsule->nlp_solver; int nx, nu, nv, ns, nz, ni, dim; double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NPHIN+NGN, sizeof(double)); for(int i=0; i reset memory int qp_status; ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "qp_status", &qp_status); if (qp_status == 3) { // printf("\nin reset qp_status %d -> resetting QP memory\n", qp_status); ocp_nlp_solver_reset_qp_memory(nlp_solver, nlp_in, nlp_out); } {%- endif %} free(buffer); return 0; } int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsule, int stage, double *p, int np) { int solver_status = 0; int casadi_np = {{ dims.np }}; if (casadi_np != np) { printf("acados_update_params: trying to set %i parameters for external functions." " External function has %i parameters. Exiting.\n", np, casadi_np); exit(1); } {%- if dims.np > 0 %} const int N = capsule->nlp_solver_plan->N; if (stage < N && stage >= 0) { {%- if solver_options.integrator_type == "IRK" %} capsule->impl_dae_fun[stage].set_param(capsule->impl_dae_fun+stage, p); capsule->impl_dae_fun_jac_x_xdot_z[stage].set_param(capsule->impl_dae_fun_jac_x_xdot_z+stage, p); capsule->impl_dae_jac_x_xdot_u_z[stage].set_param(capsule->impl_dae_jac_x_xdot_u_z+stage, p); {%- if solver_options.hessian_approx == "EXACT" %} capsule->impl_dae_hess[stage].set_param(capsule->impl_dae_hess+stage, p); {%- endif %} {% elif solver_options.integrator_type == "LIFTED_IRK" %} capsule->impl_dae_fun[stage].set_param(capsule->impl_dae_fun+stage, p); capsule->impl_dae_fun_jac_x_xdot_u[stage].set_param(capsule->impl_dae_fun_jac_x_xdot_u+stage, p); {% elif solver_options.integrator_type == "ERK" %} capsule->forw_vde_casadi[stage].set_param(capsule->forw_vde_casadi+stage, p); capsule->expl_ode_fun[stage].set_param(capsule->expl_ode_fun+stage, p); {%- if solver_options.hessian_approx == "EXACT" %} capsule->hess_vde_casadi[stage].set_param(capsule->hess_vde_casadi+stage, p); {%- endif %} {% elif solver_options.integrator_type == "GNSF" %} {% if model.gnsf.purely_linear != 1 %} capsule->gnsf_phi_fun[stage].set_param(capsule->gnsf_phi_fun+stage, p); capsule->gnsf_phi_fun_jac_y[stage].set_param(capsule->gnsf_phi_fun_jac_y+stage, p); capsule->gnsf_phi_jac_y_uhat[stage].set_param(capsule->gnsf_phi_jac_y_uhat+stage, p); {% if model.gnsf.nontrivial_f_LO == 1 %} capsule->gnsf_f_lo_jac_x1_x1dot_u_z[stage].set_param(capsule->gnsf_f_lo_jac_x1_x1dot_u_z+stage, p); {%- endif %} {%- endif %} {% elif solver_options.integrator_type == "DISCRETE" %} capsule->discr_dyn_phi_fun[stage].set_param(capsule->discr_dyn_phi_fun+stage, p); capsule->discr_dyn_phi_fun_jac_ut_xt[stage].set_param(capsule->discr_dyn_phi_fun_jac_ut_xt+stage, p); {%- if solver_options.hessian_approx == "EXACT" %} capsule->discr_dyn_phi_fun_jac_ut_xt_hess[stage].set_param(capsule->discr_dyn_phi_fun_jac_ut_xt_hess+stage, p); {% endif %} {%- endif %}{# integrator_type #} // constraints {% if constraints.constr_type == "BGP" %} capsule->phi_constraint[stage].set_param(capsule->phi_constraint+stage, p); {% elif constraints.constr_type == "BGH" and dims.nh > 0 %} capsule->nl_constr_h_fun_jac[stage].set_param(capsule->nl_constr_h_fun_jac+stage, p); capsule->nl_constr_h_fun[stage].set_param(capsule->nl_constr_h_fun+stage, p); {%- if solver_options.hessian_approx == "EXACT" %} capsule->nl_constr_h_fun_jac_hess[stage].set_param(capsule->nl_constr_h_fun_jac_hess+stage, p); {%- endif %} {%- endif %} // cost if (stage == 0) { {%- if cost.cost_type_0 == "NONLINEAR_LS" %} capsule->cost_y_0_fun.set_param(&capsule->cost_y_0_fun, p); capsule->cost_y_0_fun_jac_ut_xt.set_param(&capsule->cost_y_0_fun_jac_ut_xt, p); capsule->cost_y_0_hess.set_param(&capsule->cost_y_0_hess, p); {%- elif cost.cost_type_0 == "EXTERNAL" %} capsule->ext_cost_0_fun.set_param(&capsule->ext_cost_0_fun, p); capsule->ext_cost_0_fun_jac.set_param(&capsule->ext_cost_0_fun_jac, p); capsule->ext_cost_0_fun_jac_hess.set_param(&capsule->ext_cost_0_fun_jac_hess, p); {% endif %} } else // 0 < stage < N { {%- if cost.cost_type == "NONLINEAR_LS" %} capsule->cost_y_fun[stage-1].set_param(capsule->cost_y_fun+stage-1, p); capsule->cost_y_fun_jac_ut_xt[stage-1].set_param(capsule->cost_y_fun_jac_ut_xt+stage-1, p); capsule->cost_y_hess[stage-1].set_param(capsule->cost_y_hess+stage-1, p); {%- elif cost.cost_type == "EXTERNAL" %} capsule->ext_cost_fun[stage-1].set_param(capsule->ext_cost_fun+stage-1, p); capsule->ext_cost_fun_jac[stage-1].set_param(capsule->ext_cost_fun_jac+stage-1, p); capsule->ext_cost_fun_jac_hess[stage-1].set_param(capsule->ext_cost_fun_jac_hess+stage-1, p); {%- endif %} } } else // stage == N { // terminal shooting node has no dynamics // cost {%- if cost.cost_type_e == "NONLINEAR_LS" %} capsule->cost_y_e_fun.set_param(&capsule->cost_y_e_fun, p); capsule->cost_y_e_fun_jac_ut_xt.set_param(&capsule->cost_y_e_fun_jac_ut_xt, p); capsule->cost_y_e_hess.set_param(&capsule->cost_y_e_hess, p); {%- elif cost.cost_type_e == "EXTERNAL" %} capsule->ext_cost_e_fun.set_param(&capsule->ext_cost_e_fun, p); capsule->ext_cost_e_fun_jac.set_param(&capsule->ext_cost_e_fun_jac, p); capsule->ext_cost_e_fun_jac_hess.set_param(&capsule->ext_cost_e_fun_jac_hess, p); {% endif %} // constraints {% if constraints.constr_type_e == "BGP" %} capsule->phi_e_constraint.set_param(&capsule->phi_e_constraint, p); {% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} capsule->nl_constr_h_e_fun_jac.set_param(&capsule->nl_constr_h_e_fun_jac, p); capsule->nl_constr_h_e_fun.set_param(&capsule->nl_constr_h_e_fun, p); {%- if solver_options.hessian_approx == "EXACT" %} capsule->nl_constr_h_e_fun_jac_hess.set_param(&capsule->nl_constr_h_e_fun_jac_hess, p); {%- endif %} {% endif %} } {% endif %}{# if dims.np #} return solver_status; } int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule* capsule) { // solve NLP int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); return solver_status; } int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) { // before destroying, keep some info const int N = capsule->nlp_solver_plan->N; // free memory ocp_nlp_solver_opts_destroy(capsule->nlp_opts); ocp_nlp_in_destroy(capsule->nlp_in); ocp_nlp_out_destroy(capsule->nlp_out); ocp_nlp_out_destroy(capsule->sens_out); ocp_nlp_solver_destroy(capsule->nlp_solver); ocp_nlp_dims_destroy(capsule->nlp_dims); ocp_nlp_config_destroy(capsule->nlp_config); ocp_nlp_plan_destroy(capsule->nlp_solver_plan); /* free external function */ // dynamics {%- if solver_options.integrator_type == "IRK" %} for (int i = 0; i < N; i++) { external_function_param_casadi_free(&capsule->impl_dae_fun[i]); external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]); external_function_param_casadi_free(&capsule->impl_dae_jac_x_xdot_u_z[i]); {%- if solver_options.hessian_approx == "EXACT" %} external_function_param_casadi_free(&capsule->impl_dae_hess[i]); {%- endif %} } free(capsule->impl_dae_fun); free(capsule->impl_dae_fun_jac_x_xdot_z); free(capsule->impl_dae_jac_x_xdot_u_z); {%- if solver_options.hessian_approx == "EXACT" %} free(capsule->impl_dae_hess); {%- endif %} {%- elif solver_options.integrator_type == "LIFTED_IRK" %} for (int i = 0; i < N; i++) { external_function_param_casadi_free(&capsule->impl_dae_fun[i]); external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_u[i]); } free(capsule->impl_dae_fun); free(capsule->impl_dae_fun_jac_x_xdot_u); {%- elif solver_options.integrator_type == "ERK" %} for (int i = 0; i < N; i++) { external_function_param_casadi_free(&capsule->forw_vde_casadi[i]); external_function_param_casadi_free(&capsule->expl_ode_fun[i]); {%- if solver_options.hessian_approx == "EXACT" %} external_function_param_casadi_free(&capsule->hess_vde_casadi[i]); {%- endif %} } free(capsule->forw_vde_casadi); free(capsule->expl_ode_fun); {%- if solver_options.hessian_approx == "EXACT" %} free(capsule->hess_vde_casadi); {%- endif %} {%- elif solver_options.integrator_type == "GNSF" %} for (int i = 0; i < N; i++) { {% if model.gnsf.purely_linear != 1 %} external_function_param_casadi_free(&capsule->gnsf_phi_fun[i]); external_function_param_casadi_free(&capsule->gnsf_phi_fun_jac_y[i]); external_function_param_casadi_free(&capsule->gnsf_phi_jac_y_uhat[i]); {% if model.gnsf.nontrivial_f_LO == 1 %} external_function_param_casadi_free(&capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i]); {%- endif %} {%- endif %} external_function_param_casadi_free(&capsule->gnsf_get_matrices_fun[i]); } {% if model.gnsf.purely_linear != 1 %} free(capsule->gnsf_phi_fun); free(capsule->gnsf_phi_fun_jac_y); free(capsule->gnsf_phi_jac_y_uhat); {% if model.gnsf.nontrivial_f_LO == 1 %} free(capsule->gnsf_f_lo_jac_x1_x1dot_u_z); {%- endif %} {%- endif %} free(capsule->gnsf_get_matrices_fun); {%- elif solver_options.integrator_type == "DISCRETE" %} for (int i = 0; i < N; i++) { external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun[i]); external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun_jac_ut_xt[i]); {%- if solver_options.hessian_approx == "EXACT" %} external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i]); {%- endif %} } free(capsule->discr_dyn_phi_fun); free(capsule->discr_dyn_phi_fun_jac_ut_xt); {%- if solver_options.hessian_approx == "EXACT" %} free(capsule->discr_dyn_phi_fun_jac_ut_xt_hess); {%- endif %} {%- endif %} // cost {%- if cost.cost_type_0 == "NONLINEAR_LS" %} external_function_param_casadi_free(&capsule->cost_y_0_fun); external_function_param_casadi_free(&capsule->cost_y_0_fun_jac_ut_xt); external_function_param_casadi_free(&capsule->cost_y_0_hess); {%- elif cost.cost_type_0 == "EXTERNAL" %} external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun); external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun_jac); external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun_jac_hess); {%- endif %} {%- if cost.cost_type == "NONLINEAR_LS" %} for (int i = 0; i < N - 1; i++) { external_function_param_casadi_free(&capsule->cost_y_fun[i]); external_function_param_casadi_free(&capsule->cost_y_fun_jac_ut_xt[i]); external_function_param_casadi_free(&capsule->cost_y_hess[i]); } free(capsule->cost_y_fun); free(capsule->cost_y_fun_jac_ut_xt); free(capsule->cost_y_hess); {%- elif cost.cost_type == "EXTERNAL" %} for (int i = 0; i < N - 1; i++) { external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun[i]); external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun_jac[i]); external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun_jac_hess[i]); } free(capsule->ext_cost_fun); free(capsule->ext_cost_fun_jac); free(capsule->ext_cost_fun_jac_hess); {%- endif %} {%- if cost.cost_type_e == "NONLINEAR_LS" %} external_function_param_casadi_free(&capsule->cost_y_e_fun); external_function_param_casadi_free(&capsule->cost_y_e_fun_jac_ut_xt); external_function_param_casadi_free(&capsule->cost_y_e_hess); {%- elif cost.cost_type_e == "EXTERNAL" %} external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun); external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun_jac); external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun_jac_hess); {%- endif %} // constraints {%- if constraints.constr_type == "BGH" and dims.nh > 0 %} for (int i = 0; i < N; i++) { external_function_param_casadi_free(&capsule->nl_constr_h_fun_jac[i]); external_function_param_casadi_free(&capsule->nl_constr_h_fun[i]); } {%- if solver_options.hessian_approx == "EXACT" %} for (int i = 0; i < N; i++) { external_function_param_casadi_free(&capsule->nl_constr_h_fun_jac_hess[i]); } {%- endif %} free(capsule->nl_constr_h_fun_jac); free(capsule->nl_constr_h_fun); {%- if solver_options.hessian_approx == "EXACT" %} free(capsule->nl_constr_h_fun_jac_hess); {%- endif %} {%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %} for (int i = 0; i < N; i++) { external_function_param_casadi_free(&capsule->phi_constraint[i]); } free(capsule->phi_constraint); {%- endif %} {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} external_function_param_casadi_free(&capsule->nl_constr_h_e_fun_jac); external_function_param_casadi_free(&capsule->nl_constr_h_e_fun); {%- if solver_options.hessian_approx == "EXACT" %} external_function_param_casadi_free(&capsule->nl_constr_h_e_fun_jac_hess); {%- endif %} {%- elif constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %} external_function_param_casadi_free(&capsule->phi_e_constraint); {%- endif %} return 0; } ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_in; } ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_out; } ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule* capsule) { return capsule->sens_out; } ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver; } ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_config; } void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_opts; } ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_dims; } ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver_plan; } void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule* capsule) { int sqp_iter, stat_m, stat_n, tmp_int; ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "sqp_iter", &sqp_iter); ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_n", &stat_n); ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_m", &stat_m); {% set stat_n_max = 12 %} double stat[{{ solver_options.nlp_solver_max_iter * stat_n_max }}]; ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "statistics", stat); int nrow = sqp_iter+1 < stat_m ? sqp_iter+1 : stat_m; {%- if solver_options.nlp_solver_type == "SQP" %} printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha\n"); for (int i = 0; i < nrow; i++) { for (int j = 0; j < stat_n + 1; j++) { if (j == 0 || j == 5 || j == 6) { tmp_int = (int) stat[i + j * nrow]; printf("%d\t", tmp_int); } else { printf("%e\t", stat[i + j * nrow]); } } printf("\n"); } {% else %} printf("iter\tqp_stat\tqp_iter\n"); for (int i = 0; i < nrow; i++) { for (int j = 0; j < stat_n + 1; j++) { tmp_int = (int) stat[i + j * nrow]; printf("%d\t", tmp_int); } printf("\n"); } {%- endif %} }