mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-02-26 07:43:51 +08:00
dragonpilot beta3
date: 2023-07-26T22:20:36 commit: c6d842c412052be1985b63d683c63be9dcb2b0eb
This commit is contained in:
dragonpilot
@@ -152,7 +152,7 @@ ocp_cython_o: ocp_cython_c
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
-I /usr/local/pyenv/versions/3.8.10/lib/python3.8/site-packages/numpy/core/include \
|
||||
-I /usr/local/pyenv/versions/3.11.4/lib/python3.11/site-packages/numpy/core/include \
|
||||
acados_ocp_solver_pyx.c \
|
||||
|
||||
ocp_cython: ocp_cython_o
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@@ -0,0 +1,278 @@
|
||||
/*
|
||||
* 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>
|
||||
|
||||
// acados
|
||||
#include "acados_c/external_function_interface.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#include "acados/sim/sim_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/print.h"
|
||||
|
||||
|
||||
// example specific
|
||||
#include "lat_model/lat_model.h"
|
||||
#include "acados_sim_solver_lat.h"
|
||||
|
||||
|
||||
// ** solver data **
|
||||
|
||||
sim_solver_capsule * lat_acados_sim_solver_create_capsule()
|
||||
{
|
||||
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
|
||||
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
|
||||
|
||||
return capsule;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
|
||||
{
|
||||
free(capsule);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_sim_create(sim_solver_capsule * capsule)
|
||||
{
|
||||
// initialize
|
||||
const int nx = LAT_NX;
|
||||
const int nu = LAT_NU;
|
||||
const int nz = LAT_NZ;
|
||||
const int np = LAT_NP;
|
||||
bool tmp_bool;
|
||||
|
||||
|
||||
double Tsim = 0.009765625;
|
||||
|
||||
|
||||
// explicit ode
|
||||
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
|
||||
capsule->sim_forw_vde_casadi->casadi_fun = &lat_expl_vde_forw;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_in = &lat_expl_vde_forw_n_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_out = &lat_expl_vde_forw_n_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &lat_expl_vde_forw_sparsity_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &lat_expl_vde_forw_sparsity_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_work = &lat_expl_vde_forw_work;
|
||||
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
|
||||
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_fun = &lat_expl_ode_fun;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &lat_expl_ode_fun_n_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &lat_expl_ode_fun_n_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &lat_expl_ode_fun_sparsity_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &lat_expl_ode_fun_sparsity_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_work = &lat_expl_ode_fun_work;
|
||||
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
|
||||
|
||||
|
||||
|
||||
// sim plan & config
|
||||
sim_solver_plan_t plan;
|
||||
plan.sim_solver = ERK;
|
||||
|
||||
// create correct config based on plan
|
||||
sim_config * lat_sim_config = sim_config_create(plan);
|
||||
capsule->acados_sim_config = lat_sim_config;
|
||||
|
||||
// sim dims
|
||||
void *lat_sim_dims = sim_dims_create(lat_sim_config);
|
||||
capsule->acados_sim_dims = lat_sim_dims;
|
||||
sim_dims_set(lat_sim_config, lat_sim_dims, "nx", &nx);
|
||||
sim_dims_set(lat_sim_config, lat_sim_dims, "nu", &nu);
|
||||
sim_dims_set(lat_sim_config, lat_sim_dims, "nz", &nz);
|
||||
|
||||
|
||||
// sim opts
|
||||
sim_opts *lat_sim_opts = sim_opts_create(lat_sim_config, lat_sim_dims);
|
||||
capsule->acados_sim_opts = lat_sim_opts;
|
||||
int tmp_int = 3;
|
||||
sim_opts_set(lat_sim_config, lat_sim_opts, "newton_iter", &tmp_int);
|
||||
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
|
||||
sim_opts_set(lat_sim_config, lat_sim_opts, "collocation_type", &collocation_type);
|
||||
|
||||
|
||||
tmp_int = 4;
|
||||
sim_opts_set(lat_sim_config, lat_sim_opts, "num_stages", &tmp_int);
|
||||
tmp_int = 1;
|
||||
sim_opts_set(lat_sim_config, lat_sim_opts, "num_steps", &tmp_int);
|
||||
tmp_bool = 0;
|
||||
sim_opts_set(lat_sim_config, lat_sim_opts, "jac_reuse", &tmp_bool);
|
||||
|
||||
|
||||
// sim in / out
|
||||
sim_in *lat_sim_in = sim_in_create(lat_sim_config, lat_sim_dims);
|
||||
capsule->acados_sim_in = lat_sim_in;
|
||||
sim_out *lat_sim_out = sim_out_create(lat_sim_config, lat_sim_dims);
|
||||
capsule->acados_sim_out = lat_sim_out;
|
||||
|
||||
sim_in_set(lat_sim_config, lat_sim_dims,
|
||||
lat_sim_in, "T", &Tsim);
|
||||
|
||||
// model functions
|
||||
lat_sim_config->model_set(lat_sim_in->model,
|
||||
"expl_vde_for", capsule->sim_forw_vde_casadi);
|
||||
lat_sim_config->model_set(lat_sim_in->model,
|
||||
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
|
||||
|
||||
// sim solver
|
||||
sim_solver *lat_sim_solver = sim_solver_create(lat_sim_config,
|
||||
lat_sim_dims, lat_sim_opts);
|
||||
capsule->acados_sim_solver = lat_sim_solver;
|
||||
|
||||
|
||||
/* initialize parameter values */
|
||||
double* p = calloc(np, sizeof(double));
|
||||
|
||||
|
||||
lat_acados_sim_update_params(capsule, p, np);
|
||||
free(p);
|
||||
|
||||
|
||||
/* initialize input */
|
||||
// x
|
||||
double x0[4];
|
||||
for (int ii = 0; ii < 4; ii++)
|
||||
x0[ii] = 0.0;
|
||||
|
||||
sim_in_set(lat_sim_config, lat_sim_dims,
|
||||
lat_sim_in, "x", x0);
|
||||
|
||||
|
||||
// u
|
||||
double u0[1];
|
||||
for (int ii = 0; ii < 1; ii++)
|
||||
u0[ii] = 0.0;
|
||||
|
||||
sim_in_set(lat_sim_config, lat_sim_dims,
|
||||
lat_sim_in, "u", u0);
|
||||
|
||||
// S_forw
|
||||
double S_forw[20];
|
||||
for (int ii = 0; ii < 20; ii++)
|
||||
S_forw[ii] = 0.0;
|
||||
for (int ii = 0; ii < 4; ii++)
|
||||
S_forw[ii + ii * 4 ] = 1.0;
|
||||
|
||||
|
||||
sim_in_set(lat_sim_config, lat_sim_dims,
|
||||
lat_sim_in, "S_forw", S_forw);
|
||||
|
||||
int status = sim_precompute(lat_sim_solver, lat_sim_in, lat_sim_out);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_sim_solve(sim_solver_capsule *capsule)
|
||||
{
|
||||
// integrate dynamics using acados sim_solver
|
||||
int status = sim_solve(capsule->acados_sim_solver,
|
||||
capsule->acados_sim_in, capsule->acados_sim_out);
|
||||
if (status != 0)
|
||||
printf("error in lat_acados_sim_solve()! Exiting.\n");
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_sim_free(sim_solver_capsule *capsule)
|
||||
{
|
||||
// free memory
|
||||
sim_solver_destroy(capsule->acados_sim_solver);
|
||||
sim_in_destroy(capsule->acados_sim_in);
|
||||
sim_out_destroy(capsule->acados_sim_out);
|
||||
sim_opts_destroy(capsule->acados_sim_opts);
|
||||
sim_dims_destroy(capsule->acados_sim_dims);
|
||||
sim_config_destroy(capsule->acados_sim_config);
|
||||
|
||||
// free external function
|
||||
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
|
||||
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
|
||||
{
|
||||
int status = 0;
|
||||
int casadi_np = LAT_NP;
|
||||
|
||||
if (casadi_np != np) {
|
||||
printf("lat_acados_sim_update_params: trying to set %i parameters for external functions."
|
||||
" External function has %i parameters. Exiting.\n", np, casadi_np);
|
||||
exit(1);
|
||||
}
|
||||
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
|
||||
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
/* getters pointers to C objects*/
|
||||
sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_config;
|
||||
};
|
||||
|
||||
sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_in;
|
||||
};
|
||||
|
||||
sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_out;
|
||||
};
|
||||
|
||||
void * lat_acados_get_sim_dims(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_dims;
|
||||
};
|
||||
|
||||
sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_opts;
|
||||
};
|
||||
|
||||
sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_solver;
|
||||
};
|
||||
|
||||
@@ -0,0 +1,960 @@
|
||||
/*
|
||||
* 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 "lat_model/lat_model.h"
|
||||
|
||||
|
||||
|
||||
|
||||
#include "lat_cost/lat_cost_y_fun.h"
|
||||
#include "lat_cost/lat_cost_y_0_fun.h"
|
||||
#include "lat_cost/lat_cost_y_e_fun.h"
|
||||
|
||||
#include "acados_solver_lat.h"
|
||||
|
||||
#define NX LAT_NX
|
||||
#define NZ LAT_NZ
|
||||
#define NU LAT_NU
|
||||
#define NP LAT_NP
|
||||
#define NBX LAT_NBX
|
||||
#define NBX0 LAT_NBX0
|
||||
#define NBU LAT_NBU
|
||||
#define NSBX LAT_NSBX
|
||||
#define NSBU LAT_NSBU
|
||||
#define NSH LAT_NSH
|
||||
#define NSG LAT_NSG
|
||||
#define NSPHI LAT_NSPHI
|
||||
#define NSHN LAT_NSHN
|
||||
#define NSGN LAT_NSGN
|
||||
#define NSPHIN LAT_NSPHIN
|
||||
#define NSBXN LAT_NSBXN
|
||||
#define NS LAT_NS
|
||||
#define NSN LAT_NSN
|
||||
#define NG LAT_NG
|
||||
#define NBXN LAT_NBXN
|
||||
#define NGN LAT_NGN
|
||||
#define NY0 LAT_NY0
|
||||
#define NY LAT_NY
|
||||
#define NYN LAT_NYN
|
||||
// #define N LAT_N
|
||||
#define NH LAT_NH
|
||||
#define NPHI LAT_NPHI
|
||||
#define NHN LAT_NHN
|
||||
#define NPHIN LAT_NPHIN
|
||||
#define NR LAT_NR
|
||||
|
||||
|
||||
// ** solver data **
|
||||
|
||||
lat_solver_capsule * lat_acados_create_capsule(void)
|
||||
{
|
||||
void* capsule_mem = malloc(sizeof(lat_solver_capsule));
|
||||
lat_solver_capsule *capsule = (lat_solver_capsule *) capsule_mem;
|
||||
|
||||
return capsule;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_free_capsule(lat_solver_capsule *capsule)
|
||||
{
|
||||
free(capsule);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_create(lat_solver_capsule* capsule)
|
||||
{
|
||||
int N_shooting_intervals = LAT_N;
|
||||
double* new_time_steps = NULL; // NULL -> don't alter the code generated time-steps
|
||||
return lat_acados_create_with_discretization(capsule, N_shooting_intervals, new_time_steps);
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_update_time_steps(lat_solver_capsule* capsule, int N, double* new_time_steps)
|
||||
{
|
||||
if (N != capsule->nlp_solver_plan->N) {
|
||||
fprintf(stderr, "lat_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 lat_acados_create: step 1
|
||||
*/
|
||||
void lat_acados_create_1_set_plan(ocp_nlp_plan_t* nlp_solver_plan, const int N)
|
||||
{
|
||||
assert(N == nlp_solver_plan->N);
|
||||
|
||||
/************************************************
|
||||
* plan
|
||||
************************************************/
|
||||
nlp_solver_plan->nlp_solver = SQP_RTI;
|
||||
|
||||
nlp_solver_plan->ocp_qp_solver_plan.qp_solver = PARTIAL_CONDENSING_HPIPM;
|
||||
|
||||
nlp_solver_plan->nlp_cost[0] = NONLINEAR_LS;
|
||||
for (int i = 1; i < N; i++)
|
||||
nlp_solver_plan->nlp_cost[i] = NONLINEAR_LS;
|
||||
|
||||
nlp_solver_plan->nlp_cost[N] = NONLINEAR_LS;
|
||||
|
||||
for (int i = 0; i < N; i++)
|
||||
{
|
||||
nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL;
|
||||
nlp_solver_plan->sim_solver_plan[i].sim_solver = ERK;
|
||||
}
|
||||
|
||||
for (int i = 0; i < N; i++)
|
||||
{nlp_solver_plan->nlp_constraints[i] = BGH;
|
||||
}
|
||||
nlp_solver_plan->nlp_constraints[N] = BGH;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Internal function for lat_acados_create: step 2
|
||||
*/
|
||||
ocp_nlp_dims* lat_acados_create_2_create_and_set_dimensions(lat_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] = 4;
|
||||
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] = 0;
|
||||
|
||||
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]);
|
||||
}
|
||||
ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]);
|
||||
for (int i = 1; i < N; i++)
|
||||
ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]);
|
||||
|
||||
for (int i = 0; i < N; i++)
|
||||
{
|
||||
}
|
||||
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]);
|
||||
ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]);
|
||||
free(intNp1mem);
|
||||
return nlp_dims;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Internal function for lat_acados_create: step 3
|
||||
*/
|
||||
void lat_acados_create_3_create_and_set_functions(lat_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__ , 2); \
|
||||
}while(false)
|
||||
|
||||
|
||||
|
||||
|
||||
// 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], lat_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], lat_expl_ode_fun);
|
||||
}
|
||||
|
||||
|
||||
// nonlinear least squares function
|
||||
MAP_CASADI_FNC(cost_y_0_fun, lat_cost_y_0_fun);
|
||||
MAP_CASADI_FNC(cost_y_0_fun_jac_ut_xt, lat_cost_y_0_fun_jac_ut_xt);
|
||||
MAP_CASADI_FNC(cost_y_0_hess, lat_cost_y_0_hess);
|
||||
// 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], lat_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], lat_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], lat_cost_y_hess);
|
||||
}
|
||||
// nonlinear least square function
|
||||
MAP_CASADI_FNC(cost_y_e_fun, lat_cost_y_e_fun);
|
||||
MAP_CASADI_FNC(cost_y_e_fun_jac_ut_xt, lat_cost_y_e_fun_jac_ut_xt);
|
||||
MAP_CASADI_FNC(cost_y_e_hess, lat_cost_y_e_hess);
|
||||
|
||||
#undef MAP_CASADI_FNC
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Internal function for lat_acados_create: step 4
|
||||
*/
|
||||
void lat_acados_create_4_set_default_parameters(lat_solver_capsule* capsule) {
|
||||
const int N = capsule->nlp_solver_plan->N;
|
||||
// initialize parameters to nominal value
|
||||
double* p = calloc(NP, sizeof(double));
|
||||
|
||||
for (int i = 0; i <= N; i++) {
|
||||
lat_acados_update_params(capsule, i, p, NP);
|
||||
}
|
||||
free(p);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Internal function for lat_acados_create: step 5
|
||||
*/
|
||||
void lat_acados_create_5_set_nlp_in(lat_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
|
||||
|
||||
|
||||
if (new_time_steps) {
|
||||
lat_acados_update_time_steps(capsule, N, new_time_steps);
|
||||
} else {// time_steps are different
|
||||
double* time_steps = malloc(N*sizeof(double));
|
||||
time_steps[0] = 0.009765625;
|
||||
time_steps[1] = 0.029296875;
|
||||
time_steps[2] = 0.048828125;
|
||||
time_steps[3] = 0.068359375;
|
||||
time_steps[4] = 0.087890625;
|
||||
time_steps[5] = 0.107421875;
|
||||
time_steps[6] = 0.126953125;
|
||||
time_steps[7] = 0.146484375;
|
||||
time_steps[8] = 0.166015625;
|
||||
time_steps[9] = 0.185546875;
|
||||
time_steps[10] = 0.205078125;
|
||||
time_steps[11] = 0.224609375;
|
||||
time_steps[12] = 0.244140625;
|
||||
time_steps[13] = 0.263671875;
|
||||
time_steps[14] = 0.283203125;
|
||||
time_steps[15] = 0.302734375;
|
||||
time_steps[16] = 0.322265625;
|
||||
time_steps[17] = 0.341796875;
|
||||
time_steps[18] = 0.361328125;
|
||||
time_steps[19] = 0.380859375;
|
||||
time_steps[20] = 0.400390625;
|
||||
time_steps[21] = 0.419921875;
|
||||
time_steps[22] = 0.439453125;
|
||||
time_steps[23] = 0.458984375;
|
||||
time_steps[24] = 0.478515625;
|
||||
time_steps[25] = 0.498046875;
|
||||
time_steps[26] = 0.517578125;
|
||||
time_steps[27] = 0.537109375;
|
||||
time_steps[28] = 0.556640625;
|
||||
time_steps[29] = 0.576171875;
|
||||
time_steps[30] = 0.595703125;
|
||||
time_steps[31] = 0.615234375;
|
||||
lat_acados_update_time_steps(capsule, N, time_steps);
|
||||
free(time_steps);
|
||||
}
|
||||
|
||||
/**** Dynamics ****/
|
||||
for (int i = 0; i < N; i++)
|
||||
{
|
||||
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]);
|
||||
|
||||
}
|
||||
|
||||
/**** Cost ****/
|
||||
double* W_0 = calloc(NY0*NY0, sizeof(double));
|
||||
// change only the non-zero elements:
|
||||
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:
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0);
|
||||
free(yref_0);
|
||||
double* W = calloc(NY*NY, sizeof(double));
|
||||
// change only the non-zero elements:
|
||||
|
||||
double* yref = calloc(NY, sizeof(double));
|
||||
// change only the non-zero elements:
|
||||
|
||||
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);
|
||||
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);
|
||||
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]);
|
||||
}
|
||||
|
||||
// terminal cost
|
||||
double* yref_e = calloc(NYN, sizeof(double));
|
||||
// change only the non-zero elements:
|
||||
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:
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e);
|
||||
free(W_e);
|
||||
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);
|
||||
|
||||
|
||||
|
||||
/**** Constraints ****/
|
||||
|
||||
// bounds for initial stage
|
||||
// x0
|
||||
int* idxbx0 = malloc(NBX0 * sizeof(int));
|
||||
idxbx0[0] = 0;
|
||||
idxbx0[1] = 1;
|
||||
idxbx0[2] = 2;
|
||||
idxbx0[3] = 3;
|
||||
|
||||
double* lubx0 = calloc(2*NBX0, sizeof(double));
|
||||
double* lbx0 = lubx0;
|
||||
double* ubx0 = lubx0 + NBX0;
|
||||
// change only the non-zero elements:
|
||||
|
||||
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);
|
||||
// idxbxe_0
|
||||
int* idxbxe_0 = malloc(4 * sizeof(int));
|
||||
|
||||
idxbxe_0[0] = 0;
|
||||
idxbxe_0[1] = 1;
|
||||
idxbxe_0[2] = 2;
|
||||
idxbxe_0[3] = 3;
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0);
|
||||
free(idxbxe_0);
|
||||
|
||||
/* constraints that are the same for initial and intermediate */
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// x
|
||||
int* idxbx = malloc(NBX * sizeof(int));
|
||||
|
||||
idxbx[0] = 2;
|
||||
idxbx[1] = 3;
|
||||
double* lubx = calloc(2*NBX, sizeof(double));
|
||||
double* lbx = lubx;
|
||||
double* ubx = lubx + NBX;
|
||||
|
||||
lbx[0] = -1.5707963267948966;
|
||||
ubx[0] = 1.5707963267948966;
|
||||
lbx[1] = -0.8726646259971648;
|
||||
ubx[1] = 0.8726646259971648;
|
||||
|
||||
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);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* terminal constraints */
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Internal function for lat_acados_create: step 6
|
||||
*/
|
||||
void lat_acados_create_6_set_opts(lat_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
|
||||
************************************************/
|
||||
|
||||
|
||||
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization", "fixed_step");int full_step_dual = 0;
|
||||
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "full_step_dual", &full_step_dual);
|
||||
|
||||
// set collocation type (relevant for implicit integrators)
|
||||
sim_collocation_type collocation_type = GAUSS_LEGENDRE;
|
||||
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
|
||||
// all sim_method_num_steps are identical
|
||||
int sim_method_num_steps = 1;
|
||||
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);
|
||||
|
||||
// set up sim_method_num_stages
|
||||
// all sim_method_num_stages are identical
|
||||
int sim_method_num_stages = 4;
|
||||
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);
|
||||
|
||||
int newton_iter_val = 3;
|
||||
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
|
||||
bool tmp_bool = (bool) 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);
|
||||
|
||||
double nlp_solver_step_length = 1;
|
||||
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "step_length", &nlp_solver_step_length);
|
||||
|
||||
double levenberg_marquardt = 0;
|
||||
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt);
|
||||
|
||||
/* options QP solver */
|
||||
int qp_solver_cond_N;
|
||||
|
||||
const int qp_solver_cond_N_ori = 1;
|
||||
qp_solver_cond_N = N < qp_solver_cond_N_ori ? N : qp_solver_cond_N_ori; // use the minimum value here
|
||||
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N);
|
||||
// set HPIPM mode: should be done before setting other QP solver options
|
||||
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_hpipm_mode", "BALANCE");
|
||||
|
||||
|
||||
|
||||
int qp_solver_iter_max = 1;
|
||||
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max);
|
||||
|
||||
int print_level = 0;
|
||||
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level);
|
||||
|
||||
|
||||
int ext_cost_num_hess = 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Internal function for lat_acados_create: step 7
|
||||
*/
|
||||
void lat_acados_create_7_set_nlp_out(lat_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;
|
||||
|
||||
// initialize with x0
|
||||
|
||||
|
||||
|
||||
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 lat_acados_create: step 8
|
||||
*/
|
||||
//void lat_acados_create_8_create_solver(lat_solver_capsule* capsule)
|
||||
//{
|
||||
// capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts);
|
||||
//}
|
||||
|
||||
/**
|
||||
* Internal function for lat_acados_create: step 9
|
||||
*/
|
||||
int lat_acados_create_9_precompute(lat_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 lat_acados_create_with_discretization(lat_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 != LAT_N && !new_time_steps) {
|
||||
fprintf(stderr, "lat_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, LAT_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);
|
||||
lat_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 = lat_acados_create_2_create_and_set_dimensions(capsule);
|
||||
lat_acados_create_3_create_and_set_functions(capsule);
|
||||
|
||||
// 4) set default parameters in functions
|
||||
lat_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);
|
||||
lat_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);
|
||||
lat_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);
|
||||
lat_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);
|
||||
//lat_acados_create_8_create_solver(capsule);
|
||||
|
||||
// 9) do precomputations
|
||||
int status = lat_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 lat_acados_update_qp_solver_cond_N(lat_solver_capsule* capsule, int qp_solver_cond_N)
|
||||
{
|
||||
// 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 lat_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 = lat_acados_create_9_precompute(capsule);
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_reset(lat_solver_capsule* capsule)
|
||||
{
|
||||
|
||||
// set initialization to all zeros
|
||||
|
||||
const int N = capsule->nlp_solver_plan->N;
|
||||
ocp_nlp_config* nlp_config = capsule->nlp_config;
|
||||
ocp_nlp_dims* nlp_dims = capsule->nlp_dims;
|
||||
ocp_nlp_out* nlp_out = capsule->nlp_out;
|
||||
ocp_nlp_in* nlp_in = capsule->nlp_in;
|
||||
ocp_nlp_solver* nlp_solver = capsule->nlp_solver;
|
||||
|
||||
int nx, nu, nv, ns, nz, ni, dim;
|
||||
|
||||
double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NPHIN+NGN, sizeof(double));
|
||||
|
||||
for(int i=0; i<N+1; i++)
|
||||
{
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", buffer);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", buffer);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "sl", buffer);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "su", buffer);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "lam", buffer);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "t", buffer);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "z", buffer);
|
||||
if (i<N)
|
||||
{
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "pi", buffer);
|
||||
}
|
||||
}
|
||||
// get qp_status: if NaN -> reset memory
|
||||
int qp_status;
|
||||
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "qp_status", &qp_status);
|
||||
if (qp_status == 3)
|
||||
{
|
||||
// printf("\nin reset qp_status %d -> resetting QP memory\n", qp_status);
|
||||
ocp_nlp_solver_reset_qp_memory(nlp_solver, nlp_in, nlp_out);
|
||||
}
|
||||
|
||||
free(buffer);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
int lat_acados_update_params(lat_solver_capsule* capsule, int stage, double *p, int np)
|
||||
{
|
||||
int solver_status = 0;
|
||||
|
||||
int casadi_np = 2;
|
||||
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);
|
||||
}
|
||||
const int N = capsule->nlp_solver_plan->N;
|
||||
if (stage < N && stage >= 0)
|
||||
{
|
||||
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);
|
||||
|
||||
|
||||
// constraints
|
||||
|
||||
|
||||
// cost
|
||||
if (stage == 0)
|
||||
{
|
||||
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);
|
||||
}
|
||||
else // 0 < stage < N
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
else // stage == N
|
||||
{
|
||||
// terminal shooting node has no dynamics
|
||||
// cost
|
||||
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);
|
||||
// constraints
|
||||
|
||||
}
|
||||
|
||||
|
||||
return solver_status;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int lat_acados_solve(lat_solver_capsule* capsule)
|
||||
{
|
||||
// solve NLP
|
||||
int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
|
||||
|
||||
return solver_status;
|
||||
}
|
||||
|
||||
|
||||
int lat_acados_free(lat_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
|
||||
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]);
|
||||
}
|
||||
free(capsule->forw_vde_casadi);
|
||||
free(capsule->expl_ode_fun);
|
||||
|
||||
// cost
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
|
||||
// constraints
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
ocp_nlp_in *lat_acados_get_nlp_in(lat_solver_capsule* capsule) { return capsule->nlp_in; }
|
||||
ocp_nlp_out *lat_acados_get_nlp_out(lat_solver_capsule* capsule) { return capsule->nlp_out; }
|
||||
ocp_nlp_out *lat_acados_get_sens_out(lat_solver_capsule* capsule) { return capsule->sens_out; }
|
||||
ocp_nlp_solver *lat_acados_get_nlp_solver(lat_solver_capsule* capsule) { return capsule->nlp_solver; }
|
||||
ocp_nlp_config *lat_acados_get_nlp_config(lat_solver_capsule* capsule) { return capsule->nlp_config; }
|
||||
void *lat_acados_get_nlp_opts(lat_solver_capsule* capsule) { return capsule->nlp_opts; }
|
||||
ocp_nlp_dims *lat_acados_get_nlp_dims(lat_solver_capsule* capsule) { return capsule->nlp_dims; }
|
||||
ocp_nlp_plan_t *lat_acados_get_nlp_plan(lat_solver_capsule* capsule) { return capsule->nlp_solver_plan; }
|
||||
|
||||
|
||||
void lat_acados_print_stats(lat_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);
|
||||
|
||||
|
||||
double stat[1200];
|
||||
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "statistics", stat);
|
||||
|
||||
int nrow = sqp_iter+1 < stat_m ? sqp_iter+1 : stat_m;
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,264 @@
|
||||
/*
|
||||
* 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.;
|
||||
*/
|
||||
|
||||
#define S_FUNCTION_NAME acados_solver_sfunction_lat
|
||||
#define S_FUNCTION_LEVEL 2
|
||||
|
||||
#define MDL_START
|
||||
|
||||
// acados
|
||||
// #include "acados/utils/print.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
// example specific
|
||||
#include "lat_model/lat_model.h"
|
||||
#include "acados_solver_lat.h"
|
||||
|
||||
#include "simstruc.h"
|
||||
|
||||
#define SAMPLINGTIME 0.009765625
|
||||
|
||||
static void mdlInitializeSizes (SimStruct *S)
|
||||
{
|
||||
// specify the number of continuous and discrete states
|
||||
ssSetNumContStates(S, 0);
|
||||
ssSetNumDiscStates(S, 0);// specify the number of input ports
|
||||
if ( !ssSetNumInputPorts(S, 8) )
|
||||
return;
|
||||
|
||||
// specify the number of output ports
|
||||
if ( !ssSetNumOutputPorts(S, 6) )
|
||||
return;
|
||||
|
||||
// specify dimension information for the input ports
|
||||
// lbx_0
|
||||
ssSetInputPortVectorDimension(S, 0, 4);
|
||||
// ubx_0
|
||||
ssSetInputPortVectorDimension(S, 1, 4);
|
||||
// parameters
|
||||
ssSetInputPortVectorDimension(S, 2, (32+1) * 2);
|
||||
// y_ref_0
|
||||
ssSetInputPortVectorDimension(S, 3, 5);
|
||||
// y_ref
|
||||
ssSetInputPortVectorDimension(S, 4, 155);
|
||||
// y_ref_e
|
||||
ssSetInputPortVectorDimension(S, 5, 3);
|
||||
// lbx
|
||||
ssSetInputPortVectorDimension(S, 6, 62);
|
||||
// ubx
|
||||
ssSetInputPortVectorDimension(S, 7, 62);/* specify dimension information for the OUTPUT ports */
|
||||
ssSetOutputPortVectorDimension(S, 0, 1 );
|
||||
ssSetOutputPortVectorDimension(S, 1, 1 );
|
||||
ssSetOutputPortVectorDimension(S, 2, 1 );
|
||||
ssSetOutputPortVectorDimension(S, 3, 4 ); // state at shooting node 1
|
||||
ssSetOutputPortVectorDimension(S, 4, 1);
|
||||
ssSetOutputPortVectorDimension(S, 5, 1 );
|
||||
|
||||
// specify the direct feedthrough status
|
||||
// should be set to 1 for all inputs used in mdlOutputs
|
||||
ssSetInputPortDirectFeedThrough(S, 0, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 1, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 2, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 3, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 4, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 5, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 6, 1);
|
||||
ssSetInputPortDirectFeedThrough(S, 7, 1);
|
||||
|
||||
// one sample time
|
||||
ssSetNumSampleTimes(S, 1);
|
||||
}
|
||||
|
||||
|
||||
#if defined(MATLAB_MEX_FILE)
|
||||
|
||||
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
|
||||
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
|
||||
|
||||
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
#endif /* MATLAB_MEX_FILE */
|
||||
|
||||
|
||||
static void mdlInitializeSampleTimes(SimStruct *S)
|
||||
{
|
||||
ssSetSampleTime(S, 0, SAMPLINGTIME);
|
||||
ssSetOffsetTime(S, 0, 0.0);
|
||||
}
|
||||
|
||||
|
||||
static void mdlStart(SimStruct *S)
|
||||
{
|
||||
lat_solver_capsule *capsule = lat_acados_create_capsule();
|
||||
lat_acados_create(capsule);
|
||||
|
||||
ssSetUserData(S, (void*)capsule);
|
||||
}
|
||||
|
||||
|
||||
static void mdlOutputs(SimStruct *S, int_T tid)
|
||||
{
|
||||
lat_solver_capsule *capsule = ssGetUserData(S);
|
||||
ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(capsule);
|
||||
ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(capsule);
|
||||
ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(capsule);
|
||||
ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(capsule);
|
||||
|
||||
InputRealPtrsType in_sign;
|
||||
|
||||
// local buffer
|
||||
real_t buffer[5];
|
||||
|
||||
/* go through inputs */
|
||||
// lbx_0
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 0);
|
||||
for (int i = 0; i < 4; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
|
||||
// ubx_0
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 1);
|
||||
for (int i = 0; i < 4; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
|
||||
// parameters - stage-variant !!!
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 2);
|
||||
|
||||
// update value of parameters
|
||||
for (int ii = 0; ii <= 32; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < 2; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*2+jj]);
|
||||
lat_acados_update_params(capsule, ii, buffer, 2);
|
||||
}
|
||||
|
||||
|
||||
// y_ref_0
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 3);
|
||||
|
||||
for (int i = 0; i < 5; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
|
||||
|
||||
|
||||
// y_ref - for stages 1 to N-1
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 4);
|
||||
|
||||
for (int ii = 1; ii < 32; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < 5; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*5+jj]);
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
|
||||
}
|
||||
|
||||
|
||||
// y_ref_e
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 5);
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 32, "yref", (void *) buffer);
|
||||
// lbx
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
|
||||
for (int ii = 1; ii < 32; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < 2; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
|
||||
}
|
||||
// ubx
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 7);
|
||||
for (int ii = 1; ii < 32; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < 2; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*2+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
|
||||
}
|
||||
|
||||
/* call solver */
|
||||
int rti_phase = 0;
|
||||
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
|
||||
int acados_status = lat_acados_solve(capsule);
|
||||
|
||||
|
||||
/* set outputs */
|
||||
// assign pointers to output signals
|
||||
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
|
||||
int tmp_int;
|
||||
out_u0 = ssGetOutputPortRealSignal(S, 0);
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
|
||||
|
||||
|
||||
out_status = ssGetOutputPortRealSignal(S, 1);
|
||||
*out_status = (real_t) acados_status;
|
||||
out_KKT_res = ssGetOutputPortRealSignal(S, 2);
|
||||
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
|
||||
out_x1 = ssGetOutputPortRealSignal(S, 3);
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
|
||||
out_cpu_time = ssGetOutputPortRealSignal(S, 4);
|
||||
// get solution time
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
|
||||
out_sqp_iter = ssGetOutputPortRealSignal(S, 5);
|
||||
// get sqp iter
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
|
||||
*out_sqp_iter = (real_t) tmp_int;
|
||||
|
||||
}
|
||||
|
||||
static void mdlTerminate(SimStruct *S)
|
||||
{
|
||||
lat_solver_capsule *capsule = ssGetUserData(S);
|
||||
|
||||
lat_acados_free(capsule);
|
||||
lat_acados_free_capsule(capsule);
|
||||
}
|
||||
|
||||
|
||||
#ifdef MATLAB_MEX_FILE
|
||||
#include "simulink.c"
|
||||
#else
|
||||
#include "cg_sfun.h"
|
||||
#endif
|
||||
@@ -0,0 +1,163 @@
|
||||
/* This file was automatically generated by CasADi 3.6.3.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) lat_cost_y_0_fun_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
|
||||
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
|
||||
/* lat_cost_y_0_fun:(i0[4],i1,i2[2])->(o0[5]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2;
|
||||
a0=arg[0]? arg[0][1] : 0;
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
a0=arg[2]? arg[2][0] : 0;
|
||||
a1=10.;
|
||||
a1=(a0+a1);
|
||||
a2=arg[0]? arg[0][2] : 0;
|
||||
a2=(a1*a2);
|
||||
if (res[0]!=0) res[0][1]=a2;
|
||||
a2=arg[0]? arg[0][3] : 0;
|
||||
a2=(a1*a2);
|
||||
if (res[0]!=0) res[0][2]=a2;
|
||||
a2=arg[1]? arg[1][0] : 0;
|
||||
a1=(a1*a2);
|
||||
if (res[0]!=0) res[0][3]=a1;
|
||||
a1=1.0000000000000001e-01;
|
||||
a0=(a0+a1);
|
||||
a2=(a2/a0);
|
||||
if (res[0]!=0) res[0][4]=a2;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_n_in(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_0_fun_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 3;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
@@ -0,0 +1,174 @@
|
||||
/* This file was automatically generated by CasADi 3.6.3.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) lat_cost_y_0_fun_jac_ut_xt_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
|
||||
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s4[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
|
||||
|
||||
/* lat_cost_y_0_fun_jac_ut_xt:(i0[4],i1,i2[2])->(o0[5],o1[5x5,5nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3;
|
||||
a0=arg[0]? arg[0][1] : 0;
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
a0=arg[2]? arg[2][0] : 0;
|
||||
a1=10.;
|
||||
a1=(a0+a1);
|
||||
a2=arg[0]? arg[0][2] : 0;
|
||||
a2=(a1*a2);
|
||||
if (res[0]!=0) res[0][1]=a2;
|
||||
a2=arg[0]? arg[0][3] : 0;
|
||||
a2=(a1*a2);
|
||||
if (res[0]!=0) res[0][2]=a2;
|
||||
a2=arg[1]? arg[1][0] : 0;
|
||||
a3=(a1*a2);
|
||||
if (res[0]!=0) res[0][3]=a3;
|
||||
a3=1.0000000000000001e-01;
|
||||
a0=(a0+a3);
|
||||
a2=(a2/a0);
|
||||
if (res[0]!=0) res[0][4]=a2;
|
||||
a2=1.;
|
||||
if (res[1]!=0) res[1][0]=a2;
|
||||
if (res[1]!=0) res[1][1]=a1;
|
||||
if (res[1]!=0) res[1][2]=a1;
|
||||
if (res[1]!=0) res[1][3]=a1;
|
||||
a0=(1./a0);
|
||||
if (res[1]!=0) res[1][4]=a0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_0_fun_jac_ut_xt_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_in(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_fun_jac_ut_xt_n_out(void) { return 2;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_0_fun_jac_ut_xt_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_jac_ut_xt_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_fun_jac_ut_xt_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
case 1: return "o1";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_jac_ut_xt_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_fun_jac_ut_xt_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
case 1: return casadi_s4;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 3;
|
||||
if (sz_res) *sz_res = 2;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
@@ -0,0 +1,148 @@
|
||||
/* This file was automatically generated by CasADi 3.6.3.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) lat_cost_y_0_hess_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
|
||||
static const casadi_int casadi_s4[8] = {5, 5, 0, 0, 0, 0, 0, 0};
|
||||
|
||||
/* lat_cost_y_0_hess:(i0[4],i1,i2[5],i3[2])->(o0[5x5,0nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_0_hess_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_hess_n_in(void) { return 4;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_0_hess_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_0_hess_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_hess_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_0_hess_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_hess_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_0_hess_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s4;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_0_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
@@ -0,0 +1,156 @@
|
||||
/* This file was automatically generated by CasADi 3.6.3.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) lat_cost_y_e_fun_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s1[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
|
||||
static const casadi_int casadi_s3[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
|
||||
/* lat_cost_y_e_fun:(i0[4],i1[],i2[2])->(o0[3]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1;
|
||||
a0=arg[0]? arg[0][1] : 0;
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
a0=arg[2]? arg[2][0] : 0;
|
||||
a1=10.;
|
||||
a0=(a0+a1);
|
||||
a1=arg[0]? arg[0][2] : 0;
|
||||
a1=(a0*a1);
|
||||
if (res[0]!=0) res[0][1]=a1;
|
||||
a1=arg[0]? arg[0][3] : 0;
|
||||
a0=(a0*a1);
|
||||
if (res[0]!=0) res[0][2]=a0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_n_in(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_e_fun_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 3;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
@@ -0,0 +1,164 @@
|
||||
/* This file was automatically generated by CasADi 3.6.3.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) lat_cost_y_e_fun_jac_ut_xt_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s1[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
|
||||
static const casadi_int casadi_s3[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s4[9] = {4, 3, 0, 1, 2, 3, 1, 2, 3};
|
||||
|
||||
/* lat_cost_y_e_fun_jac_ut_xt:(i0[4],i1[],i2[2])->(o0[3],o1[4x3,3nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1;
|
||||
a0=arg[0]? arg[0][1] : 0;
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
a0=arg[2]? arg[2][0] : 0;
|
||||
a1=10.;
|
||||
a0=(a0+a1);
|
||||
a1=arg[0]? arg[0][2] : 0;
|
||||
a1=(a0*a1);
|
||||
if (res[0]!=0) res[0][1]=a1;
|
||||
a1=arg[0]? arg[0][3] : 0;
|
||||
a1=(a0*a1);
|
||||
if (res[0]!=0) res[0][2]=a1;
|
||||
a1=1.;
|
||||
if (res[1]!=0) res[1][0]=a1;
|
||||
if (res[1]!=0) res[1][1]=a0;
|
||||
if (res[1]!=0) res[1][2]=a0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_e_fun_jac_ut_xt_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_in(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_fun_jac_ut_xt_n_out(void) { return 2;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_e_fun_jac_ut_xt_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_jac_ut_xt_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_fun_jac_ut_xt_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
case 1: return "o1";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_jac_ut_xt_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_fun_jac_ut_xt_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
case 1: return casadi_s4;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 3;
|
||||
if (sz_res) *sz_res = 2;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
@@ -0,0 +1,148 @@
|
||||
/* This file was automatically generated by CasADi 3.6.3.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) lat_cost_y_e_hess_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s1[3] = {0, 0, 0};
|
||||
static const casadi_int casadi_s2[7] = {3, 1, 0, 3, 0, 1, 2};
|
||||
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
|
||||
static const casadi_int casadi_s4[7] = {4, 4, 0, 0, 0, 0, 0};
|
||||
|
||||
/* lat_cost_y_e_hess:(i0[4],i1[],i2[3],i3[2])->(o0[4x4,0nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_e_hess_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_hess_n_in(void) { return 4;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_e_hess_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_e_hess_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_hess_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_e_hess_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_hess_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_e_hess_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s4;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_e_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
@@ -0,0 +1,163 @@
|
||||
/* This file was automatically generated by CasADi 3.6.3.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) lat_cost_y_fun_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
|
||||
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
|
||||
/* lat_cost_y_fun:(i0[4],i1,i2[2])->(o0[5]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2;
|
||||
a0=arg[0]? arg[0][1] : 0;
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
a0=arg[2]? arg[2][0] : 0;
|
||||
a1=10.;
|
||||
a1=(a0+a1);
|
||||
a2=arg[0]? arg[0][2] : 0;
|
||||
a2=(a1*a2);
|
||||
if (res[0]!=0) res[0][1]=a2;
|
||||
a2=arg[0]? arg[0][3] : 0;
|
||||
a2=(a1*a2);
|
||||
if (res[0]!=0) res[0][2]=a2;
|
||||
a2=arg[1]? arg[1][0] : 0;
|
||||
a1=(a1*a2);
|
||||
if (res[0]!=0) res[0][3]=a1;
|
||||
a1=1.0000000000000001e-01;
|
||||
a0=(a0+a1);
|
||||
a2=(a2/a0);
|
||||
if (res[0]!=0) res[0][4]=a2;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_n_in(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_fun_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 3;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
@@ -0,0 +1,174 @@
|
||||
/* This file was automatically generated by CasADi 3.6.3.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) lat_cost_y_fun_jac_ut_xt_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
|
||||
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s4[13] = {5, 5, 0, 1, 2, 3, 4, 5, 2, 3, 4, 0, 0};
|
||||
|
||||
/* lat_cost_y_fun_jac_ut_xt:(i0[4],i1,i2[2])->(o0[5],o1[5x5,5nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3;
|
||||
a0=arg[0]? arg[0][1] : 0;
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
a0=arg[2]? arg[2][0] : 0;
|
||||
a1=10.;
|
||||
a1=(a0+a1);
|
||||
a2=arg[0]? arg[0][2] : 0;
|
||||
a2=(a1*a2);
|
||||
if (res[0]!=0) res[0][1]=a2;
|
||||
a2=arg[0]? arg[0][3] : 0;
|
||||
a2=(a1*a2);
|
||||
if (res[0]!=0) res[0][2]=a2;
|
||||
a2=arg[1]? arg[1][0] : 0;
|
||||
a3=(a1*a2);
|
||||
if (res[0]!=0) res[0][3]=a3;
|
||||
a3=1.0000000000000001e-01;
|
||||
a0=(a0+a3);
|
||||
a2=(a2/a0);
|
||||
if (res[0]!=0) res[0][4]=a2;
|
||||
a2=1.;
|
||||
if (res[1]!=0) res[1][0]=a2;
|
||||
if (res[1]!=0) res[1][1]=a1;
|
||||
if (res[1]!=0) res[1][2]=a1;
|
||||
if (res[1]!=0) res[1][3]=a1;
|
||||
a0=(1./a0);
|
||||
if (res[1]!=0) res[1][4]=a0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_fun_jac_ut_xt_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_in(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_fun_jac_ut_xt_n_out(void) { return 2;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_fun_jac_ut_xt_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_jac_ut_xt_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_fun_jac_ut_xt_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
case 1: return "o1";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_jac_ut_xt_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_fun_jac_ut_xt_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
case 1: return casadi_s4;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_fun_jac_ut_xt_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 3;
|
||||
if (sz_res) *sz_res = 2;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
@@ -0,0 +1,148 @@
|
||||
/* This file was automatically generated by CasADi 3.6.3.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) lat_cost_y_hess_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
#define casadi_s4 CASADI_PREFIX(s4)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
|
||||
static const casadi_int casadi_s4[8] = {5, 5, 0, 0, 0, 0, 0, 0};
|
||||
|
||||
/* lat_cost_y_hess:(i0[4],i1,i2[5],i3[2])->(o0[5x5,0nz]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_hess(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_hess_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_hess_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_hess_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_hess_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_hess_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_hess_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_cost_y_hess_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_hess_n_in(void) { return 4;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_cost_y_hess_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real lat_cost_y_hess_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_hess_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_cost_y_hess_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_hess_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
case 3: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_cost_y_hess_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s4;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_cost_y_hess_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
@@ -0,0 +1,164 @@
|
||||
/* This file was automatically generated by CasADi 3.6.3.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) lat_expl_ode_fun_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
|
||||
|
||||
/* lat_expl_ode_fun:(i0[4],i1,i2[2])->(o0[4]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4, a5;
|
||||
a0=arg[2]? arg[2][0] : 0;
|
||||
a1=arg[0]? arg[0][2] : 0;
|
||||
a2=cos(a1);
|
||||
a2=(a0*a2);
|
||||
a3=arg[2]? arg[2][1] : 0;
|
||||
a4=sin(a1);
|
||||
a4=(a3*a4);
|
||||
a5=arg[0]? arg[0][3] : 0;
|
||||
a4=(a4*a5);
|
||||
a2=(a2-a4);
|
||||
if (res[0]!=0) res[0][0]=a2;
|
||||
a2=sin(a1);
|
||||
a0=(a0*a2);
|
||||
a1=cos(a1);
|
||||
a3=(a3*a1);
|
||||
a3=(a3*a5);
|
||||
a0=(a0+a3);
|
||||
if (res[0]!=0) res[0][1]=a0;
|
||||
if (res[0]!=0) res[0][2]=a5;
|
||||
a5=arg[1]? arg[1][0] : 0;
|
||||
if (res[0]!=0) res[0][3]=a5;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_ode_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_ode_fun_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_ode_fun_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_expl_ode_fun_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_ode_fun_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_expl_ode_fun_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_expl_ode_fun_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_expl_ode_fun_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_expl_ode_fun_n_in(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_expl_ode_fun_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real lat_expl_ode_fun_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_expl_ode_fun_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_expl_ode_fun_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_ode_fun_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_ode_fun_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_ode_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 3;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
@@ -0,0 +1,186 @@
|
||||
/* This file was automatically generated by CasADi 3.6.3.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) lat_expl_vde_adj_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s1[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s2[6] = {2, 1, 0, 2, 0, 1};
|
||||
static const casadi_int casadi_s3[9] = {5, 1, 0, 5, 0, 1, 2, 3, 4};
|
||||
|
||||
/* lat_expl_vde_adj:(i0[4],i1[4],i2,i3[2])->(o0[5]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a2, a3, a4, a5, a6, a7;
|
||||
a0=0.;
|
||||
if (res[0]!=0) res[0][0]=a0;
|
||||
if (res[0]!=0) res[0][1]=a0;
|
||||
a0=arg[0]? arg[0][2] : 0;
|
||||
a1=cos(a0);
|
||||
a2=arg[3]? arg[3][0] : 0;
|
||||
a3=arg[1]? arg[1][1] : 0;
|
||||
a4=(a2*a3);
|
||||
a1=(a1*a4);
|
||||
a4=sin(a0);
|
||||
a5=arg[3]? arg[3][1] : 0;
|
||||
a6=arg[0]? arg[0][3] : 0;
|
||||
a7=(a6*a3);
|
||||
a7=(a5*a7);
|
||||
a4=(a4*a7);
|
||||
a1=(a1-a4);
|
||||
a4=cos(a0);
|
||||
a7=arg[1]? arg[1][0] : 0;
|
||||
a6=(a6*a7);
|
||||
a6=(a5*a6);
|
||||
a4=(a4*a6);
|
||||
a1=(a1-a4);
|
||||
a4=sin(a0);
|
||||
a2=(a2*a7);
|
||||
a4=(a4*a2);
|
||||
a1=(a1-a4);
|
||||
if (res[0]!=0) res[0][2]=a1;
|
||||
a1=arg[1]? arg[1][2] : 0;
|
||||
a4=cos(a0);
|
||||
a4=(a5*a4);
|
||||
a4=(a4*a3);
|
||||
a1=(a1+a4);
|
||||
a0=sin(a0);
|
||||
a5=(a5*a0);
|
||||
a5=(a5*a7);
|
||||
a1=(a1-a5);
|
||||
if (res[0]!=0) res[0][3]=a1;
|
||||
a1=arg[1]? arg[1][3] : 0;
|
||||
if (res[0]!=0) res[0][4]=a1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_vde_adj(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_vde_adj_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_vde_adj_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_expl_vde_adj_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_vde_adj_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_expl_vde_adj_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_expl_vde_adj_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_expl_vde_adj_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_expl_vde_adj_n_in(void) { return 4;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_expl_vde_adj_n_out(void) { return 1;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real lat_expl_vde_adj_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_expl_vde_adj_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_expl_vde_adj_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_vde_adj_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s0;
|
||||
case 2: return casadi_s1;
|
||||
case 3: return casadi_s2;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_vde_adj_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_vde_adj_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 4;
|
||||
if (sz_res) *sz_res = 1;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
@@ -0,0 +1,299 @@
|
||||
/* This file was automatically generated by CasADi 3.6.3.
|
||||
* It consists of:
|
||||
* 1) content generated by CasADi runtime: not copyrighted
|
||||
* 2) template code copied from CasADi source: permissively licensed (MIT-0)
|
||||
* 3) user code: owned by the user
|
||||
*
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* How to prefix internal symbols */
|
||||
#ifdef CASADI_CODEGEN_PREFIX
|
||||
#define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID)
|
||||
#define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID
|
||||
#define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID)
|
||||
#else
|
||||
#define CASADI_PREFIX(ID) lat_expl_vde_forw_ ## ID
|
||||
#endif
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#ifndef casadi_real
|
||||
#define casadi_real double
|
||||
#endif
|
||||
|
||||
#ifndef casadi_int
|
||||
#define casadi_int int
|
||||
#endif
|
||||
|
||||
/* Add prefix to internal symbols */
|
||||
#define casadi_f0 CASADI_PREFIX(f0)
|
||||
#define casadi_s0 CASADI_PREFIX(s0)
|
||||
#define casadi_s1 CASADI_PREFIX(s1)
|
||||
#define casadi_s2 CASADI_PREFIX(s2)
|
||||
#define casadi_s3 CASADI_PREFIX(s3)
|
||||
|
||||
/* Symbol visibility in DLLs */
|
||||
#ifndef CASADI_SYMBOL_EXPORT
|
||||
#if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__)
|
||||
#if defined(STATIC_LINKED)
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT __declspec(dllexport)
|
||||
#endif
|
||||
#elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY)
|
||||
#define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default")))
|
||||
#else
|
||||
#define CASADI_SYMBOL_EXPORT
|
||||
#endif
|
||||
#endif
|
||||
|
||||
static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s1[23] = {4, 4, 0, 4, 8, 12, 16, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3};
|
||||
static const casadi_int casadi_s2[5] = {1, 1, 0, 1, 0};
|
||||
static const casadi_int casadi_s3[6] = {2, 1, 0, 2, 0, 1};
|
||||
|
||||
/* lat_expl_vde_forw:(i0[4],i1[4x4],i2[4],i3,i4[2])->(o0[4],o1[4x4],o2[4]) */
|
||||
static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
|
||||
casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a2, a3, a4, a5, a6, a7, a8, a9;
|
||||
a0=arg[4]? arg[4][0] : 0;
|
||||
a1=arg[0]? arg[0][2] : 0;
|
||||
a2=cos(a1);
|
||||
a2=(a0*a2);
|
||||
a3=arg[4]? arg[4][1] : 0;
|
||||
a4=sin(a1);
|
||||
a4=(a3*a4);
|
||||
a5=arg[0]? arg[0][3] : 0;
|
||||
a6=(a4*a5);
|
||||
a2=(a2-a6);
|
||||
if (res[0]!=0) res[0][0]=a2;
|
||||
a2=sin(a1);
|
||||
a2=(a0*a2);
|
||||
a6=cos(a1);
|
||||
a6=(a3*a6);
|
||||
a7=(a6*a5);
|
||||
a2=(a2+a7);
|
||||
if (res[0]!=0) res[0][1]=a2;
|
||||
if (res[0]!=0) res[0][2]=a5;
|
||||
a2=arg[3]? arg[3][0] : 0;
|
||||
if (res[0]!=0) res[0][3]=a2;
|
||||
a2=sin(a1);
|
||||
a7=arg[1]? arg[1][2] : 0;
|
||||
a8=(a2*a7);
|
||||
a8=(a0*a8);
|
||||
a9=cos(a1);
|
||||
a10=(a9*a7);
|
||||
a10=(a3*a10);
|
||||
a10=(a5*a10);
|
||||
a11=arg[1]? arg[1][3] : 0;
|
||||
a12=(a4*a11);
|
||||
a10=(a10+a12);
|
||||
a8=(a8+a10);
|
||||
a8=(-a8);
|
||||
if (res[1]!=0) res[1][0]=a8;
|
||||
a8=cos(a1);
|
||||
a10=(a8*a7);
|
||||
a10=(a0*a10);
|
||||
a12=(a6*a11);
|
||||
a13=sin(a1);
|
||||
a7=(a13*a7);
|
||||
a7=(a3*a7);
|
||||
a7=(a5*a7);
|
||||
a12=(a12-a7);
|
||||
a10=(a10+a12);
|
||||
if (res[1]!=0) res[1][1]=a10;
|
||||
if (res[1]!=0) res[1][2]=a11;
|
||||
a11=0.;
|
||||
if (res[1]!=0) res[1][3]=a11;
|
||||
a10=arg[1]? arg[1][6] : 0;
|
||||
a12=(a2*a10);
|
||||
a12=(a0*a12);
|
||||
a7=(a9*a10);
|
||||
a7=(a3*a7);
|
||||
a7=(a5*a7);
|
||||
a14=arg[1]? arg[1][7] : 0;
|
||||
a15=(a4*a14);
|
||||
a7=(a7+a15);
|
||||
a12=(a12+a7);
|
||||
a12=(-a12);
|
||||
if (res[1]!=0) res[1][4]=a12;
|
||||
a12=(a8*a10);
|
||||
a12=(a0*a12);
|
||||
a7=(a6*a14);
|
||||
a10=(a13*a10);
|
||||
a10=(a3*a10);
|
||||
a10=(a5*a10);
|
||||
a7=(a7-a10);
|
||||
a12=(a12+a7);
|
||||
if (res[1]!=0) res[1][5]=a12;
|
||||
if (res[1]!=0) res[1][6]=a14;
|
||||
if (res[1]!=0) res[1][7]=a11;
|
||||
a14=arg[1]? arg[1][10] : 0;
|
||||
a12=(a2*a14);
|
||||
a12=(a0*a12);
|
||||
a7=(a9*a14);
|
||||
a7=(a3*a7);
|
||||
a7=(a5*a7);
|
||||
a10=arg[1]? arg[1][11] : 0;
|
||||
a15=(a4*a10);
|
||||
a7=(a7+a15);
|
||||
a12=(a12+a7);
|
||||
a12=(-a12);
|
||||
if (res[1]!=0) res[1][8]=a12;
|
||||
a12=(a8*a14);
|
||||
a12=(a0*a12);
|
||||
a7=(a6*a10);
|
||||
a14=(a13*a14);
|
||||
a14=(a3*a14);
|
||||
a14=(a5*a14);
|
||||
a7=(a7-a14);
|
||||
a12=(a12+a7);
|
||||
if (res[1]!=0) res[1][9]=a12;
|
||||
if (res[1]!=0) res[1][10]=a10;
|
||||
if (res[1]!=0) res[1][11]=a11;
|
||||
a10=arg[1]? arg[1][14] : 0;
|
||||
a2=(a2*a10);
|
||||
a2=(a0*a2);
|
||||
a9=(a9*a10);
|
||||
a9=(a3*a9);
|
||||
a9=(a5*a9);
|
||||
a12=arg[1]? arg[1][15] : 0;
|
||||
a7=(a4*a12);
|
||||
a9=(a9+a7);
|
||||
a2=(a2+a9);
|
||||
a2=(-a2);
|
||||
if (res[1]!=0) res[1][12]=a2;
|
||||
a8=(a8*a10);
|
||||
a8=(a0*a8);
|
||||
a2=(a6*a12);
|
||||
a13=(a13*a10);
|
||||
a13=(a3*a13);
|
||||
a13=(a5*a13);
|
||||
a2=(a2-a13);
|
||||
a8=(a8+a2);
|
||||
if (res[1]!=0) res[1][13]=a8;
|
||||
if (res[1]!=0) res[1][14]=a12;
|
||||
if (res[1]!=0) res[1][15]=a11;
|
||||
a11=sin(a1);
|
||||
a12=arg[2]? arg[2][2] : 0;
|
||||
a11=(a11*a12);
|
||||
a11=(a0*a11);
|
||||
a8=cos(a1);
|
||||
a8=(a8*a12);
|
||||
a8=(a3*a8);
|
||||
a8=(a5*a8);
|
||||
a2=arg[2]? arg[2][3] : 0;
|
||||
a4=(a4*a2);
|
||||
a8=(a8+a4);
|
||||
a11=(a11+a8);
|
||||
a11=(-a11);
|
||||
if (res[2]!=0) res[2][0]=a11;
|
||||
a11=cos(a1);
|
||||
a11=(a11*a12);
|
||||
a0=(a0*a11);
|
||||
a6=(a6*a2);
|
||||
a1=sin(a1);
|
||||
a1=(a1*a12);
|
||||
a3=(a3*a1);
|
||||
a5=(a5*a3);
|
||||
a6=(a6-a5);
|
||||
a0=(a0+a6);
|
||||
if (res[2]!=0) res[2][1]=a0;
|
||||
if (res[2]!=0) res[2][2]=a2;
|
||||
a2=1.;
|
||||
if (res[2]!=0) res[2][3]=a2;
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_vde_forw(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){
|
||||
return casadi_f0(arg, res, iw, w, mem);
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_vde_forw_alloc_mem(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_vde_forw_init_mem(int mem) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_expl_vde_forw_free_mem(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_vde_forw_checkout(void) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_expl_vde_forw_release(int mem) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_expl_vde_forw_incref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT void lat_expl_vde_forw_decref(void) {
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_expl_vde_forw_n_in(void) { return 5;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_int lat_expl_vde_forw_n_out(void) { return 3;}
|
||||
|
||||
CASADI_SYMBOL_EXPORT casadi_real lat_expl_vde_forw_default_in(casadi_int i) {
|
||||
switch (i) {
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_expl_vde_forw_name_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "i0";
|
||||
case 1: return "i1";
|
||||
case 2: return "i2";
|
||||
case 3: return "i3";
|
||||
case 4: return "i4";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const char* lat_expl_vde_forw_name_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return "o0";
|
||||
case 1: return "o1";
|
||||
case 2: return "o2";
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_vde_forw_sparsity_in(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s0;
|
||||
case 3: return casadi_s2;
|
||||
case 4: return casadi_s3;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT const casadi_int* lat_expl_vde_forw_sparsity_out(casadi_int i) {
|
||||
switch (i) {
|
||||
case 0: return casadi_s0;
|
||||
case 1: return casadi_s1;
|
||||
case 2: return casadi_s0;
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
CASADI_SYMBOL_EXPORT int lat_expl_vde_forw_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) {
|
||||
if (sz_arg) *sz_arg = 5;
|
||||
if (sz_res) *sz_res = 3;
|
||||
if (sz_iw) *sz_iw = 0;
|
||||
if (sz_w) *sz_w = 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
@@ -0,0 +1,216 @@
|
||||
/*
|
||||
* 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>
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados/utils/math.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
#include "acados_solver_lat.h"
|
||||
|
||||
#define NX LAT_NX
|
||||
#define NZ LAT_NZ
|
||||
#define NU LAT_NU
|
||||
#define NP LAT_NP
|
||||
#define NBX LAT_NBX
|
||||
#define NBX0 LAT_NBX0
|
||||
#define NBU LAT_NBU
|
||||
#define NSBX LAT_NSBX
|
||||
#define NSBU LAT_NSBU
|
||||
#define NSH LAT_NSH
|
||||
#define NSG LAT_NSG
|
||||
#define NSPHI LAT_NSPHI
|
||||
#define NSHN LAT_NSHN
|
||||
#define NSGN LAT_NSGN
|
||||
#define NSPHIN LAT_NSPHIN
|
||||
#define NSBXN LAT_NSBXN
|
||||
#define NS LAT_NS
|
||||
#define NSN LAT_NSN
|
||||
#define NG LAT_NG
|
||||
#define NBXN LAT_NBXN
|
||||
#define NGN LAT_NGN
|
||||
#define NY0 LAT_NY0
|
||||
#define NY LAT_NY
|
||||
#define NYN LAT_NYN
|
||||
#define NH LAT_NH
|
||||
#define NPHI LAT_NPHI
|
||||
#define NHN LAT_NHN
|
||||
#define NPHIN LAT_NPHIN
|
||||
#define NR LAT_NR
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
|
||||
lat_solver_capsule *acados_ocp_capsule = lat_acados_create_capsule();
|
||||
// there is an opportunity to change the number of shooting intervals in C without new code generation
|
||||
int N = LAT_N;
|
||||
// allocate the array and fill it accordingly
|
||||
double* new_time_steps = NULL;
|
||||
int status = lat_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps);
|
||||
|
||||
if (status)
|
||||
{
|
||||
printf("lat_acados_create() returned status %d. Exiting.\n", status);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
ocp_nlp_config *nlp_config = lat_acados_get_nlp_config(acados_ocp_capsule);
|
||||
ocp_nlp_dims *nlp_dims = lat_acados_get_nlp_dims(acados_ocp_capsule);
|
||||
ocp_nlp_in *nlp_in = lat_acados_get_nlp_in(acados_ocp_capsule);
|
||||
ocp_nlp_out *nlp_out = lat_acados_get_nlp_out(acados_ocp_capsule);
|
||||
ocp_nlp_solver *nlp_solver = lat_acados_get_nlp_solver(acados_ocp_capsule);
|
||||
void *nlp_opts = lat_acados_get_nlp_opts(acados_ocp_capsule);
|
||||
|
||||
// initial condition
|
||||
int idxbx0[NBX0];
|
||||
idxbx0[0] = 0;
|
||||
idxbx0[1] = 1;
|
||||
idxbx0[2] = 2;
|
||||
idxbx0[3] = 3;
|
||||
|
||||
double lbx0[NBX0];
|
||||
double ubx0[NBX0];
|
||||
lbx0[0] = 0;
|
||||
ubx0[0] = 0;
|
||||
lbx0[1] = 0;
|
||||
ubx0[1] = 0;
|
||||
lbx0[2] = 0;
|
||||
ubx0[2] = 0;
|
||||
lbx0[3] = 0;
|
||||
ubx0[3] = 0;
|
||||
|
||||
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);
|
||||
|
||||
// initialization for state values
|
||||
double x_init[NX];
|
||||
x_init[0] = 0.0;
|
||||
x_init[1] = 0.0;
|
||||
x_init[2] = 0.0;
|
||||
x_init[3] = 0.0;
|
||||
|
||||
// initial value for control input
|
||||
double u0[NU];
|
||||
u0[0] = 0.0;
|
||||
// set parameters
|
||||
double p[NP];
|
||||
p[0] = 0;
|
||||
p[1] = 0;
|
||||
|
||||
for (int ii = 0; ii <= N; ii++)
|
||||
{
|
||||
lat_acados_update_params(acados_ocp_capsule, ii, p, NP);
|
||||
}
|
||||
|
||||
|
||||
// prepare evaluation
|
||||
int NTIMINGS = 1;
|
||||
double min_time = 1e12;
|
||||
double kkt_norm_inf;
|
||||
double elapsed_time;
|
||||
int sqp_iter;
|
||||
|
||||
double xtraj[NX * (N+1)];
|
||||
double utraj[NU * N];
|
||||
|
||||
|
||||
// solve ocp in loop
|
||||
int rti_phase = 0;
|
||||
|
||||
for (int ii = 0; ii < NTIMINGS; ii++)
|
||||
{
|
||||
// initialize solution
|
||||
for (int i = 0; i < N; i++)
|
||||
{
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
|
||||
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", x_init);
|
||||
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
|
||||
status = lat_acados_solve(acados_ocp_capsule);
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
|
||||
min_time = MIN(elapsed_time, min_time);
|
||||
}
|
||||
|
||||
/* print solution and statistics */
|
||||
for (int ii = 0; ii <= nlp_dims->N; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]);
|
||||
for (int ii = 0; ii < nlp_dims->N; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]);
|
||||
|
||||
printf("\n--- xtraj ---\n");
|
||||
d_print_exp_tran_mat( NX, N+1, xtraj, NX);
|
||||
printf("\n--- utraj ---\n");
|
||||
d_print_exp_tran_mat( NU, N, utraj, NU );
|
||||
// ocp_nlp_out_print(nlp_solver->dims, nlp_out);
|
||||
|
||||
printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
|
||||
|
||||
if (status == ACADOS_SUCCESS)
|
||||
{
|
||||
printf("lat_acados_solve(): SUCCESS!\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("lat_acados_solve() failed with status %d.\n", status);
|
||||
}
|
||||
|
||||
// get solution
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
|
||||
|
||||
lat_acados_print_stats(acados_ocp_capsule);
|
||||
|
||||
printf("\nSolver info:\n");
|
||||
printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n",
|
||||
sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf);
|
||||
|
||||
// free solver
|
||||
status = lat_acados_free(acados_ocp_capsule);
|
||||
if (status) {
|
||||
printf("lat_acados_free() returned status %d. \n", status);
|
||||
}
|
||||
// free solver capsule
|
||||
status = lat_acados_free_capsule(acados_ocp_capsule);
|
||||
if (status) {
|
||||
printf("lat_acados_free_capsule() returned status %d. \n", status);
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
@@ -0,0 +1,128 @@
|
||||
/*
|
||||
* 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>
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados/utils/math.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_sim_solver_lat.h"
|
||||
|
||||
#define NX LAT_NX
|
||||
#define NZ LAT_NZ
|
||||
#define NU LAT_NU
|
||||
#define NP LAT_NP
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
int status = 0;
|
||||
sim_solver_capsule *capsule = lat_acados_sim_solver_create_capsule();
|
||||
status = lat_acados_sim_create(capsule);
|
||||
|
||||
if (status)
|
||||
{
|
||||
printf("acados_create() returned status %d. Exiting.\n", status);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
sim_config *acados_sim_config = lat_acados_get_sim_config(capsule);
|
||||
sim_in *acados_sim_in = lat_acados_get_sim_in(capsule);
|
||||
sim_out *acados_sim_out = lat_acados_get_sim_out(capsule);
|
||||
void *acados_sim_dims = lat_acados_get_sim_dims(capsule);
|
||||
|
||||
// initial condition
|
||||
double x_current[NX];
|
||||
x_current[0] = 0.0;
|
||||
x_current[1] = 0.0;
|
||||
x_current[2] = 0.0;
|
||||
x_current[3] = 0.0;
|
||||
|
||||
|
||||
x_current[0] = 0;
|
||||
x_current[1] = 0;
|
||||
x_current[2] = 0;
|
||||
x_current[3] = 0;
|
||||
|
||||
|
||||
|
||||
|
||||
// initial value for control input
|
||||
double u0[NU];
|
||||
u0[0] = 0.0;
|
||||
// set parameters
|
||||
double p[NP];
|
||||
p[0] = 0;
|
||||
p[1] = 0;
|
||||
|
||||
lat_acados_sim_update_params(capsule, p, NP);
|
||||
|
||||
|
||||
int n_sim_steps = 3;
|
||||
// solve ocp in loop
|
||||
for (int ii = 0; ii < n_sim_steps; ii++)
|
||||
{
|
||||
sim_in_set(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_in, "x", x_current);
|
||||
status = lat_acados_sim_solve(capsule);
|
||||
|
||||
if (status != ACADOS_SUCCESS)
|
||||
{
|
||||
printf("acados_solve() failed with status %d.\n", status);
|
||||
}
|
||||
|
||||
sim_out_get(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_out, "x", x_current);
|
||||
|
||||
printf("\nx_current, %d\n", ii);
|
||||
for (int jj = 0; jj < NX; jj++)
|
||||
{
|
||||
printf("%e\n", x_current[jj]);
|
||||
}
|
||||
}
|
||||
|
||||
printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
|
||||
|
||||
// free solver
|
||||
status = lat_acados_sim_free(capsule);
|
||||
if (status) {
|
||||
printf("lat_acados_sim_free() returned status %d. \n", status);
|
||||
}
|
||||
|
||||
lat_acados_sim_solver_free_capsule(capsule);
|
||||
|
||||
return status;
|
||||
}
|
||||
Reference in New Issue
Block a user