mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-07-20 20:32:06 +08:00
lp-dp 2023-09-18T04:13:07 for EON/C2
version: lp-dp v0.9.5 for EON/C2 date: 2023-09-18T04:13:07 commit: 5d47b8da43a2550bbebcfbb6636f1296b5433e07
This commit is contained in:
Vehicle Researcher
Binary file not shown.
Binary file not shown.
+1
-1
@@ -1 +1 @@
|
||||
#define COMMA_VERSION "2023.09.13"
|
||||
#define COMMA_VERSION "2023.09.18"
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -603,7 +603,7 @@ class Controls:
|
||||
# Check if openpilot is engaged and actuators are enabled
|
||||
self.enabled = self.state in ENABLED_STATES
|
||||
self.active = self.state in ACTIVE_STATES
|
||||
if self.active or self._dp_alka_active:
|
||||
if self.active or (self._dp_alka and self._dp_alka_active):
|
||||
self.current_alert_types.append(ET.WARNING)
|
||||
|
||||
def state_control(self, CS):
|
||||
@@ -634,7 +634,7 @@ class Controls:
|
||||
(not standstill or self.joystick_mode)
|
||||
CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
|
||||
|
||||
if self._dp_alka_active and not standstill and CS.cruiseState.available:
|
||||
if (self._dp_alka and self._dp_alka_active) and not standstill and CS.cruiseState.available:
|
||||
if self.sm['liveCalibration'].calStatus != log.LiveCalibrationData.Status.calibrated:
|
||||
pass
|
||||
elif CS.steerFaultTemporary or CS.steerFaultPermanent:
|
||||
@@ -871,7 +871,7 @@ class Controls:
|
||||
dat = messaging.new_message('controlsStateExt')
|
||||
dat.valid = CS.canValid
|
||||
controlsStateExt = dat.controlsStateExt
|
||||
controlsStateExt.alkaActive = CC.latActive and self._dp_alka_active
|
||||
controlsStateExt.alkaActive = CC.latActive and (self._dp_alka and self._dp_alka_active)
|
||||
self.pm.send('controlsStateExt', dat)
|
||||
|
||||
# carState
|
||||
|
||||
@@ -0,0 +1,101 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#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_vde_adj_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_
|
||||
@@ -0,0 +1,69 @@
|
||||
/*
|
||||
* 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 lat_Y_0_COST
|
||||
#define lat_Y_0_COST
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
int lat_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int lat_cost_y_0_fun_work(int *, int *, int *, int *);
|
||||
const int *lat_cost_y_0_fun_sparsity_in(int);
|
||||
const int *lat_cost_y_0_fun_sparsity_out(int);
|
||||
int lat_cost_y_0_fun_n_in(void);
|
||||
int lat_cost_y_0_fun_n_out(void);
|
||||
|
||||
int lat_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int lat_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
|
||||
const int *lat_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
|
||||
const int *lat_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
|
||||
int lat_cost_y_0_fun_jac_ut_xt_n_in(void);
|
||||
int lat_cost_y_0_fun_jac_ut_xt_n_out(void);
|
||||
|
||||
int lat_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int lat_cost_y_0_hess_work(int *, int *, int *, int *);
|
||||
const int *lat_cost_y_0_hess_sparsity_in(int);
|
||||
const int *lat_cost_y_0_hess_sparsity_out(int);
|
||||
int lat_cost_y_0_hess_n_in(void);
|
||||
int lat_cost_y_0_hess_n_out(void);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // lat_Y_0_COST
|
||||
@@ -0,0 +1,69 @@
|
||||
/*
|
||||
* 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 lat_Y_E_COST
|
||||
#define lat_Y_E_COST
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
int lat_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int lat_cost_y_e_fun_work(int *, int *, int *, int *);
|
||||
const int *lat_cost_y_e_fun_sparsity_in(int);
|
||||
const int *lat_cost_y_e_fun_sparsity_out(int);
|
||||
int lat_cost_y_e_fun_n_in(void);
|
||||
int lat_cost_y_e_fun_n_out(void);
|
||||
|
||||
int lat_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int lat_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
|
||||
const int *lat_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
|
||||
const int *lat_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
|
||||
int lat_cost_y_e_fun_jac_ut_xt_n_in(void);
|
||||
int lat_cost_y_e_fun_jac_ut_xt_n_out(void);
|
||||
|
||||
int lat_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int lat_cost_y_e_hess_work(int *, int *, int *, int *);
|
||||
const int *lat_cost_y_e_hess_sparsity_in(int);
|
||||
const int *lat_cost_y_e_hess_sparsity_out(int);
|
||||
int lat_cost_y_e_hess_n_in(void);
|
||||
int lat_cost_y_e_hess_n_out(void);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // lat_Y_E_COST
|
||||
@@ -0,0 +1,69 @@
|
||||
/*
|
||||
* 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 lat_Y_COST
|
||||
#define lat_Y_COST
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
int lat_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int lat_cost_y_fun_work(int *, int *, int *, int *);
|
||||
const int *lat_cost_y_fun_sparsity_in(int);
|
||||
const int *lat_cost_y_fun_sparsity_out(int);
|
||||
int lat_cost_y_fun_n_in(void);
|
||||
int lat_cost_y_fun_n_out(void);
|
||||
|
||||
int lat_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int lat_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
|
||||
const int *lat_cost_y_fun_jac_ut_xt_sparsity_in(int);
|
||||
const int *lat_cost_y_fun_jac_ut_xt_sparsity_out(int);
|
||||
int lat_cost_y_fun_jac_ut_xt_n_in(void);
|
||||
int lat_cost_y_fun_jac_ut_xt_n_out(void);
|
||||
|
||||
int lat_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int lat_cost_y_hess_work(int *, int *, int *, int *);
|
||||
const int *lat_cost_y_hess_sparsity_in(int);
|
||||
const int *lat_cost_y_hess_sparsity_out(int);
|
||||
int lat_cost_y_hess_n_in(void);
|
||||
int lat_cost_y_hess_n_out(void);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // lat_Y_COST
|
||||
@@ -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';
|
||||
|
||||
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/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)
|
||||
@@ -43,7 +43,7 @@ class LateralPlanner:
|
||||
self.LP = LanePlanner()
|
||||
# dp // mapd - for vision turn controller
|
||||
self._d_path_w_lines_xyz = np.zeros((TRAJECTORY_SIZE, 3))
|
||||
self._dp_lat_lane_priority_mode_speed_based = int(self.params.get("self.dp_lat_lane_priority_mode_speed_based", encoding="utf-8")) if self._dp_lat_lane_priority_mode else 0
|
||||
self._dp_lat_lane_priority_mode_speed_based = int(self.params.get("dp_lat_lane_priority_mode_speed_based", encoding="utf-8")) if self._dp_lat_lane_priority_mode else 0
|
||||
self.param_read_counter = 0
|
||||
|
||||
# Vehicle model parameters used to calculate lateral movement of car
|
||||
|
||||
@@ -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';
|
||||
|
||||
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/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 [3]\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 [45]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [2]\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)
|
||||
@@ -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.;
|
||||
%
|
||||
|
||||
SOURCES = { ...
|
||||
'long_model/long_expl_ode_fun.c', ...
|
||||
'long_model/long_expl_vde_forw.c',...
|
||||
'long_cost/long_cost_y_0_fun.c',...
|
||||
'long_cost/long_cost_y_0_fun_jac_ut_xt.c',...
|
||||
'long_cost/long_cost_y_0_hess.c',...
|
||||
'long_cost/long_cost_y_fun.c',...
|
||||
'long_cost/long_cost_y_fun_jac_ut_xt.c',...
|
||||
'long_cost/long_cost_y_hess.c',...
|
||||
'long_cost/long_cost_y_e_fun.c',...
|
||||
'long_cost/long_cost_y_e_fun_jac_ut_xt.c',...
|
||||
'long_cost/long_cost_y_e_hess.c',...
|
||||
'long_constraints/long_constr_h_fun.c', ...
|
||||
'long_constraints/long_constr_h_fun_jac_uxt_zt_hess.c', ...
|
||||
'long_constraints/long_constr_h_fun_jac_uxt_zt.c', ...
|
||||
'acados_solver_sfunction_long.c', ...
|
||||
'acados_solver_long.c'
|
||||
};
|
||||
|
||||
INC_PATH = '/data/openpilot/third_party/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/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_long' );
|
||||
|
||||
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_long', '.', ...
|
||||
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 [3]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
|
||||
' size [3]\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 [65]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [6]\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 [66]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [5]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') lh, size [4]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') uh, size [4]\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)
|
||||
+101
@@ -0,0 +1,101 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#ifndef ACADOS_SIM_long_H_
|
||||
#define ACADOS_SIM_long_H_
|
||||
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#define LONG_NX 3
|
||||
#define LONG_NZ 0
|
||||
#define LONG_NU 1
|
||||
#define LONG_NP 6
|
||||
|
||||
#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_vde_adj_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 long_acados_sim_create(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_solve(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_free(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_config * long_acados_get_sim_config(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_in * long_acados_get_sim_in(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_out * long_acados_get_sim_out(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT void * long_acados_get_sim_dims(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_opts * long_acados_get_sim_opts(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_solver * long_acados_get_sim_solver(sim_solver_capsule *capsule);
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_solver_capsule * long_acados_sim_solver_create_capsule(void);
|
||||
ACADOS_SYMBOL_EXPORT int long_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SIM_long_H_
|
||||
+63
@@ -0,0 +1,63 @@
|
||||
/*
|
||||
* 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 long_H_CONSTRAINT
|
||||
#define long_H_CONSTRAINT
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
int long_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *);
|
||||
const int *long_constr_h_fun_jac_uxt_zt_sparsity_in(int);
|
||||
const int *long_constr_h_fun_jac_uxt_zt_sparsity_out(int);
|
||||
int long_constr_h_fun_jac_uxt_zt_n_in(void);
|
||||
int long_constr_h_fun_jac_uxt_zt_n_out(void);
|
||||
|
||||
int long_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_constr_h_fun_work(int *, int *, int *, int *);
|
||||
const int *long_constr_h_fun_sparsity_in(int);
|
||||
const int *long_constr_h_fun_sparsity_out(int);
|
||||
int long_constr_h_fun_n_in(void);
|
||||
int long_constr_h_fun_n_out(void);
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // long_H_CONSTRAINT
|
||||
+69
@@ -0,0 +1,69 @@
|
||||
/*
|
||||
* 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 long_Y_0_COST
|
||||
#define long_Y_0_COST
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
int long_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_0_fun_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_0_fun_sparsity_in(int);
|
||||
const int *long_cost_y_0_fun_sparsity_out(int);
|
||||
int long_cost_y_0_fun_n_in(void);
|
||||
int long_cost_y_0_fun_n_out(void);
|
||||
|
||||
int long_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
|
||||
const int *long_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
|
||||
int long_cost_y_0_fun_jac_ut_xt_n_in(void);
|
||||
int long_cost_y_0_fun_jac_ut_xt_n_out(void);
|
||||
|
||||
int long_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_0_hess_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_0_hess_sparsity_in(int);
|
||||
const int *long_cost_y_0_hess_sparsity_out(int);
|
||||
int long_cost_y_0_hess_n_in(void);
|
||||
int long_cost_y_0_hess_n_out(void);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // long_Y_0_COST
|
||||
+69
@@ -0,0 +1,69 @@
|
||||
/*
|
||||
* 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 long_Y_E_COST
|
||||
#define long_Y_E_COST
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
int long_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_e_fun_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_e_fun_sparsity_in(int);
|
||||
const int *long_cost_y_e_fun_sparsity_out(int);
|
||||
int long_cost_y_e_fun_n_in(void);
|
||||
int long_cost_y_e_fun_n_out(void);
|
||||
|
||||
int long_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
|
||||
const int *long_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
|
||||
int long_cost_y_e_fun_jac_ut_xt_n_in(void);
|
||||
int long_cost_y_e_fun_jac_ut_xt_n_out(void);
|
||||
|
||||
int long_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_e_hess_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_e_hess_sparsity_in(int);
|
||||
const int *long_cost_y_e_hess_sparsity_out(int);
|
||||
int long_cost_y_e_hess_n_in(void);
|
||||
int long_cost_y_e_hess_n_out(void);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // long_Y_E_COST
|
||||
+69
@@ -0,0 +1,69 @@
|
||||
/*
|
||||
* 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 long_Y_COST
|
||||
#define long_Y_COST
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
int long_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_fun_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_fun_sparsity_in(int);
|
||||
const int *long_cost_y_fun_sparsity_out(int);
|
||||
int long_cost_y_fun_n_in(void);
|
||||
int long_cost_y_fun_n_out(void);
|
||||
|
||||
int long_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_fun_jac_ut_xt_sparsity_in(int);
|
||||
const int *long_cost_y_fun_jac_ut_xt_sparsity_out(int);
|
||||
int long_cost_y_fun_jac_ut_xt_n_in(void);
|
||||
int long_cost_y_fun_jac_ut_xt_n_out(void);
|
||||
|
||||
int long_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int long_cost_y_hess_work(int *, int *, int *, int *);
|
||||
const int *long_cost_y_hess_sparsity_in(int);
|
||||
const int *long_cost_y_hess_sparsity_out(int);
|
||||
int long_cost_y_hess_n_in(void);
|
||||
int long_cost_y_hess_n_out(void);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // long_Y_COST
|
||||
@@ -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.;
|
||||
%
|
||||
|
||||
SOURCES = { ...
|
||||
'long_model/long_expl_ode_fun.c', ...
|
||||
'long_model/long_expl_vde_forw.c',...
|
||||
'long_cost/long_cost_y_0_fun.c',...
|
||||
'long_cost/long_cost_y_0_fun_jac_ut_xt.c',...
|
||||
'long_cost/long_cost_y_0_hess.c',...
|
||||
'long_cost/long_cost_y_fun.c',...
|
||||
'long_cost/long_cost_y_fun_jac_ut_xt.c',...
|
||||
'long_cost/long_cost_y_hess.c',...
|
||||
'long_cost/long_cost_y_e_fun.c',...
|
||||
'long_cost/long_cost_y_e_fun_jac_ut_xt.c',...
|
||||
'long_cost/long_cost_y_e_hess.c',...
|
||||
'long_constraints/long_constr_h_fun.c', ...
|
||||
'long_constraints/long_constr_h_fun_jac_uxt_zt_hess.c', ...
|
||||
'long_constraints/long_constr_h_fun_jac_uxt_zt.c', ...
|
||||
'acados_solver_sfunction_long.c', ...
|
||||
'acados_solver_long.c'
|
||||
};
|
||||
|
||||
INC_PATH = '/data/openpilot/third_party/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/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_long' );
|
||||
|
||||
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_long', '.', ...
|
||||
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 [3]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
|
||||
' size [3]\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 [78]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [6]\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 [66]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [5]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') lh, size [4]\n ');
|
||||
i_in = i_in + 1;
|
||||
input_note = strcat(input_note, num2str(i_in), ') uh, size [4]\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)
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_6221041922176669826);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3875401196843302167);
|
||||
void car_H_mod_fun(double *state, double *out_9089536759471748437);
|
||||
void car_f_fun(double *state, double dt, double *out_3779678567620275028);
|
||||
void car_F_fun(double *state, double dt, double *out_1785761162142041567);
|
||||
void car_h_25(double *state, double *unused, double *out_4996244158397258433);
|
||||
void car_H_25(double *state, double *unused, double *out_5210786231112538818);
|
||||
void car_h_24(double *state, double *unused, double *out_1972001985963987938);
|
||||
void car_H_24(double *state, double *unused, double *out_6660386036005494697);
|
||||
void car_h_30(double *state, double *unused, double *out_4721050096112752544);
|
||||
void car_H_30(double *state, double *unused, double *out_6319267501105396043);
|
||||
void car_h_26(double *state, double *unused, double *out_4705150565156322630);
|
||||
void car_H_26(double *state, double *unused, double *out_1469282912238482594);
|
||||
void car_h_27(double *state, double *unused, double *out_5429109263587212);
|
||||
void car_H_27(double *state, double *unused, double *out_8494030812905820954);
|
||||
void car_h_29(double *state, double *unused, double *out_280623171548093101);
|
||||
void car_H_29(double *state, double *unused, double *out_5809036156791003859);
|
||||
void car_h_28(double *state, double *unused, double *out_1485357213837934172);
|
||||
void car_H_28(double *state, double *unused, double *out_3156951516864649055);
|
||||
void car_h_31(double *state, double *unused, double *out_897940860117886569);
|
||||
void car_H_31(double *state, double *unused, double *out_5241432192989499246);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_671583855790742447);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5648898715511339411);
|
||||
void car_H_mod_fun(double *state, double *out_1463661415422686295);
|
||||
void car_f_fun(double *state, double dt, double *out_4065214630836186202);
|
||||
void car_F_fun(double *state, double dt, double *out_8116097140535481129);
|
||||
void car_h_25(double *state, double *unused, double *out_4769400438462954391);
|
||||
void car_H_25(double *state, double *unused, double *out_4920414855801952303);
|
||||
void car_h_24(double *state, double *unused, double *out_5467276253789859636);
|
||||
void car_H_24(double *state, double *unused, double *out_1655156950789565798);
|
||||
void car_h_30(double *state, double *unused, double *out_8362735586557612018);
|
||||
void car_H_30(double *state, double *unused, double *out_2402081897294703676);
|
||||
void car_h_26(double *state, double *unused, double *out_4441278616070982763);
|
||||
void car_H_26(double *state, double *unused, double *out_8661918174676008527);
|
||||
void car_h_27(double *state, double *unused, double *out_4099496445352121952);
|
||||
void car_H_27(double *state, double *unused, double *out_178487826110760459);
|
||||
void car_h_29(double *state, double *unused, double *out_3361062318896766373);
|
||||
void car_H_29(double *state, double *unused, double *out_1891850552980311492);
|
||||
void car_h_28(double *state, double *unused, double *out_2514929989030348327);
|
||||
void car_H_28(double *state, double *unused, double *out_6974249570049842066);
|
||||
void car_h_31(double *state, double *unused, double *out_141268967578377915);
|
||||
void car_H_31(double *state, double *unused, double *out_4889768893924991875);
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
void car_set_mass(double x);
|
||||
void car_set_rotational_inertia(double x);
|
||||
|
||||
@@ -5,18 +5,18 @@ void gnss_update_6(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void gnss_update_20(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void gnss_err_fun(double *nom_x, double *delta_x, double *out_7450363419333197724);
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_6032626064256685771);
|
||||
void gnss_H_mod_fun(double *state, double *out_5362512971153695335);
|
||||
void gnss_f_fun(double *state, double dt, double *out_5166670299033539086);
|
||||
void gnss_F_fun(double *state, double dt, double *out_1872100173248322506);
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_8967874647147044523);
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_7893943660228887170);
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_9116871330935910653);
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_5950134878048585736);
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_2263061303225699817);
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_2633963512929241755);
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_2263061303225699817);
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_2633963512929241755);
|
||||
void gnss_err_fun(double *nom_x, double *delta_x, double *out_3729148486180488546);
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_5677138656413877520);
|
||||
void gnss_H_mod_fun(double *state, double *out_237374271348491673);
|
||||
void gnss_f_fun(double *state, double dt, double *out_5929499374247199262);
|
||||
void gnss_F_fun(double *state, double dt, double *out_847881920946397673);
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_4769849476023598544);
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_747127776806928814);
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_5861460590680520073);
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_8385843719838436302);
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_5467876944101765304);
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_4135749185661226665);
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_5467876944101765304);
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_4135749185661226665);
|
||||
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
Binary file not shown.
@@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub
|
||||
void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_H(double *in_vec, double *out_8494622585832445872);
|
||||
void live_err_fun(double *nom_x, double *delta_x, double *out_2249411990467774485);
|
||||
void live_inv_err_fun(double *nom_x, double *true_x, double *out_2859626302290428272);
|
||||
void live_H_mod_fun(double *state, double *out_1508708626145229465);
|
||||
void live_f_fun(double *state, double dt, double *out_3025328284235069091);
|
||||
void live_F_fun(double *state, double dt, double *out_2866880934425822829);
|
||||
void live_h_4(double *state, double *unused, double *out_195412779303848081);
|
||||
void live_H_4(double *state, double *unused, double *out_8454023741651631723);
|
||||
void live_h_9(double *state, double *unused, double *out_1505666915611342749);
|
||||
void live_H_9(double *state, double *unused, double *out_8695213388281222368);
|
||||
void live_h_10(double *state, double *unused, double *out_4353819279872822444);
|
||||
void live_H_10(double *state, double *unused, double *out_8953681816626175346);
|
||||
void live_h_12(double *state, double *unused, double *out_5544595070917867689);
|
||||
void live_H_12(double *state, double *unused, double *out_4973263924025958098);
|
||||
void live_h_35(double *state, double *unused, double *out_3473632359961004563);
|
||||
void live_H_35(double *state, double *unused, double *out_6626058274685312517);
|
||||
void live_h_32(double *state, double *unused, double *out_7617791596832061829);
|
||||
void live_H_32(double *state, double *unused, double *out_555957272717473204);
|
||||
void live_h_13(double *state, double *unused, double *out_8348130591745782147);
|
||||
void live_H_13(double *state, double *unused, double *out_1808664940182043499);
|
||||
void live_h_14(double *state, double *unused, double *out_1505666915611342749);
|
||||
void live_H_14(double *state, double *unused, double *out_8695213388281222368);
|
||||
void live_h_33(double *state, double *unused, double *out_3527010921757156150);
|
||||
void live_H_33(double *state, double *unused, double *out_3475501270046454913);
|
||||
void live_H(double *in_vec, double *out_5647844793781907598);
|
||||
void live_err_fun(double *nom_x, double *delta_x, double *out_7240336443609568449);
|
||||
void live_inv_err_fun(double *nom_x, double *true_x, double *out_5067105285576238600);
|
||||
void live_H_mod_fun(double *state, double *out_1707446900005187655);
|
||||
void live_f_fun(double *state, double dt, double *out_8886202847586122156);
|
||||
void live_F_fun(double *state, double dt, double *out_8123954071177754895);
|
||||
void live_h_4(double *state, double *unused, double *out_5003732951226513629);
|
||||
void live_H_4(double *state, double *unused, double *out_7002519190470052957);
|
||||
void live_h_9(double *state, double *unused, double *out_2706973776430170011);
|
||||
void live_H_9(double *state, double *unused, double *out_7243708837099643602);
|
||||
void live_h_10(double *state, double *unused, double *out_5958541671058094074);
|
||||
void live_H_10(double *state, double *unused, double *out_6806706378750062726);
|
||||
void live_h_12(double *state, double *unused, double *out_3703959340398270602);
|
||||
void live_H_12(double *state, double *unused, double *out_6424768475207536864);
|
||||
void live_h_35(double *state, double *unused, double *out_1455318101445775547);
|
||||
void live_H_35(double *state, double *unused, double *out_8077562825866891283);
|
||||
void live_h_32(double *state, double *unused, double *out_6206442455067868074);
|
||||
void live_H_32(double *state, double *unused, double *out_6136724488533882482);
|
||||
void live_h_13(double *state, double *unused, double *out_5313389083226060905);
|
||||
void live_H_13(double *state, double *unused, double *out_8424484780051135399);
|
||||
void live_h_14(double *state, double *unused, double *out_2706973776430170011);
|
||||
void live_H_14(double *state, double *unused, double *out_7243708837099643602);
|
||||
void live_h_33(double *state, double *unused, double *out_2007778731318175820);
|
||||
void live_H_33(double *state, double *unused, double *out_4927005821228033679);
|
||||
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Vendored
BIN
Binary file not shown.
BIN
Binary file not shown.
Vendored
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
third_party/acados/acados_template/gnsf/__pycache__/reformulate_with_invertible_E_mat.cpython-38.pyc
Vendored
BIN
Binary file not shown.
third_party/acados/acados_template/gnsf/__pycache__/structure_detection_print_summary.cpython-38.pyc
Vendored
BIN
Binary file not shown.
Reference in New Issue
Block a user