2434 lines
96 KiB
C
2434 lines
96 KiB
C
/*
|
|
* 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 <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <assert.h>
|
|
// 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.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_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_z[stage].set_param(capsule->impl_dae_fun_jac_x_xdot_z+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 %}
|
|
}
|
|
|