mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-09-14 19:43:53 +08:00
dragonpilot 2023-02-07T02:56:39 for EON/C2
version: dragonpilot v0.9.1 beta for EON/C2 date: 2023-02-07T02:56:39 dp-dev(priv2) master commit: 459790d03b790e5f3a8cff18710ac4d44841a16a
This commit is contained in:
Dragonpilot Team
committed by
Comma Device
Comma Device
parent
1f2e3aa8b6
commit
8555e48fc9
@@ -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,103 @@
|
||||
/*
|
||||
* 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.;
|
||||
*/
|
||||
|
||||
#ifndef ACADOS_SIM_lat_H_
|
||||
#define ACADOS_SIM_lat_H_
|
||||
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#define LAT_NX 4
|
||||
#define LAT_NZ 0
|
||||
#define LAT_NU 1
|
||||
#define LAT_NP 2
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
// ** capsule for solver data **
|
||||
typedef struct sim_solver_capsule
|
||||
{
|
||||
// acados objects
|
||||
sim_in *acados_sim_in;
|
||||
sim_out *acados_sim_out;
|
||||
sim_solver *acados_sim_solver;
|
||||
sim_opts *acados_sim_opts;
|
||||
sim_config *acados_sim_config;
|
||||
void *acados_sim_dims;
|
||||
|
||||
/* external functions */
|
||||
// ERK
|
||||
external_function_param_casadi * sim_forw_vde_casadi;
|
||||
external_function_param_casadi * sim_expl_ode_fun_casadi;
|
||||
external_function_param_casadi * sim_expl_ode_hess;
|
||||
|
||||
// IRK
|
||||
external_function_param_casadi * sim_impl_dae_fun;
|
||||
external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
|
||||
external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
|
||||
external_function_param_casadi * sim_impl_dae_hess;
|
||||
|
||||
// GNSF
|
||||
external_function_param_casadi * sim_gnsf_phi_fun;
|
||||
external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
|
||||
external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
|
||||
external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
|
||||
external_function_param_casadi * sim_gnsf_get_matrices_fun;
|
||||
|
||||
} sim_solver_capsule;
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT int lat_acados_sim_create(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solve(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int lat_acados_sim_free(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int lat_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_config * lat_acados_get_sim_config(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_in * lat_acados_get_sim_in(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_out * lat_acados_get_sim_out(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT void * lat_acados_get_sim_dims(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_opts * lat_acados_get_sim_opts(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_solver * lat_acados_get_sim_solver(sim_solver_capsule *capsule);
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_solver_capsule * lat_acados_sim_solver_create_capsule(void);
|
||||
ACADOS_SYMBOL_EXPORT int lat_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SIM_lat_H_
|
||||
+264
@@ -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, (16+1) * 2);
|
||||
// y_ref_0
|
||||
ssSetInputPortVectorDimension(S, 3, 5);
|
||||
// y_ref
|
||||
ssSetInputPortVectorDimension(S, 4, 75);
|
||||
// y_ref_e
|
||||
ssSetInputPortVectorDimension(S, 5, 3);
|
||||
// lbx
|
||||
ssSetInputPortVectorDimension(S, 6, 30);
|
||||
// ubx
|
||||
ssSetInputPortVectorDimension(S, 7, 30);/* 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 <= 16; 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 < 16; 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, 16, "yref", (void *) buffer);
|
||||
// lbx
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, 6);
|
||||
for (int ii = 1; ii < 16; 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 < 16; 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,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;
|
||||
}
|
||||
@@ -0,0 +1,125 @@
|
||||
%
|
||||
% 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.;
|
||||
%
|
||||
|
||||
SOURCES = { ...
|
||||
'lat_model/lat_expl_ode_fun.c', ...
|
||||
'lat_model/lat_expl_vde_forw.c',...
|
||||
'lat_cost/lat_cost_y_0_fun.c',...
|
||||
'lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',...
|
||||
'lat_cost/lat_cost_y_0_hess.c',...
|
||||
'lat_cost/lat_cost_y_fun.c',...
|
||||
'lat_cost/lat_cost_y_fun_jac_ut_xt.c',...
|
||||
'lat_cost/lat_cost_y_hess.c',...
|
||||
'lat_cost/lat_cost_y_e_fun.c',...
|
||||
'lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',...
|
||||
'lat_cost/lat_cost_y_e_hess.c',...
|
||||
'acados_solver_sfunction_lat.c', ...
|
||||
'acados_solver_lat.c'
|
||||
};
|
||||
|
||||
INC_PATH = '/data/openpilot/third_party/acados/include/acados/include';
|
||||
|
||||
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
|
||||
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
|
||||
['-I', fullfile(INC_PATH, 'acados')], ...
|
||||
['-I', fullfile(INC_PATH)]};
|
||||
|
||||
|
||||
|
||||
CFLAGS = 'CFLAGS=$CFLAGS';
|
||||
LDFLAGS = 'LDFLAGS=$LDFLAGS';
|
||||
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
|
||||
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
|
||||
|
||||
|
||||
|
||||
LIB_PATH = ['-L', fullfile('/data/openpilot/third_party/acados/include/acados/lib')];
|
||||
|
||||
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
|
||||
|
||||
% acados linking libraries and flags
|
||||
|
||||
|
||||
mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
|
||||
LIB_PATH, LIBS{:}, SOURCES{:}, ...
|
||||
'-output', 'acados_solver_sfunction_lat' );
|
||||
|
||||
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_lat', '.', ...
|
||||
eval('mexext')] );
|
||||
|
||||
|
||||
%% print note on usage of s-function
|
||||
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
|
||||
input_note = 'Inputs are:\n';
|
||||
i_in = 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
|
||||
' size [4]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
|
||||
' size [4]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
|
||||
' size [34]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [5]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
|
||||
' size [75]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [3]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [30]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [30]\n ');
|
||||
i_in = i_in + 1;
|
||||
|
||||
fprintf(input_note)
|
||||
|
||||
disp(' ')
|
||||
|
||||
output_note = 'Outputs are:\n';
|
||||
i_out = 0;
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [1]\n ');
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
|
||||
|
||||
fprintf(output_note)
|
||||
Reference in New Issue
Block a user