forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Prebuilt Release @ ab07000
This commit is contained in:
IQ.Lvbs history cleanup
1
third_party/.gitignore
vendored
Normal file
1
third_party/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
*.pyc
|
||||
5
third_party/acados/.gitignore
vendored
Normal file
5
third_party/acados/.gitignore
vendored
Normal file
@@ -0,0 +1,5 @@
|
||||
acados_repo/
|
||||
lib
|
||||
!x86_64/
|
||||
!larch64/
|
||||
!aarch64/
|
||||
1
third_party/acados/aarch64
vendored
Symbolic link
1
third_party/acados/aarch64
vendored
Symbolic link
@@ -0,0 +1 @@
|
||||
larch64/
|
||||
6
third_party/acados/acados_template/.gitignore
vendored
Normal file
6
third_party/acados/acados_template/.gitignore
vendored
Normal file
@@ -0,0 +1,6 @@
|
||||
__pycache__/
|
||||
|
||||
# Cython intermediates
|
||||
*_pyx.c
|
||||
*_pyx.o
|
||||
*_pyx.so
|
||||
40
third_party/acados/acados_template/__init__.py
vendored
Normal file
40
third_party/acados/acados_template/__init__.py
vendored
Normal file
@@ -0,0 +1,40 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
from .acados_model import AcadosModel
|
||||
from .acados_ocp import AcadosOcp, AcadosOcpConstraints, AcadosOcpCost, AcadosOcpDims, AcadosOcpOptions
|
||||
from .acados_sim import AcadosSim, AcadosSimDims, AcadosSimOpts
|
||||
from .acados_ocp_solver import AcadosOcpSolver, get_simulink_default_opts, ocp_get_default_cmake_builder
|
||||
from .acados_sim_solver import AcadosSimSolver, sim_get_default_cmake_builder
|
||||
from .utils import print_casadi_expression, get_acados_path, get_python_interface_path, \
|
||||
get_tera_exec_path, get_tera, check_casadi_version, acados_dae_model_json_dump, \
|
||||
casadi_length, make_object_json_dumpable, J_to_idx, get_default_simulink_opts
|
||||
|
||||
from .zoro_description import ZoroDescription, process_zoro_description
|
||||
834
third_party/acados/acados_template/acados_layout.json
vendored
Normal file
834
third_party/acados/acados_template/acados_layout.json
vendored
Normal file
@@ -0,0 +1,834 @@
|
||||
{
|
||||
"code_export_directory": [
|
||||
"str"
|
||||
],
|
||||
"acados_include_path": [
|
||||
"str"
|
||||
],
|
||||
"cython_include_dirs": [
|
||||
"list"
|
||||
],
|
||||
"json_file": [
|
||||
"str"
|
||||
],
|
||||
"shared_lib_ext": [
|
||||
"str"
|
||||
],
|
||||
"model": {
|
||||
"name" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_ext_fun_type" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_generic_source" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_impl_dae_fun" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_impl_dae_fun_jac" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_impl_dae_jac" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_disc_fun_jac_hess" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_disc_fun_jac" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_disc_fun" : [
|
||||
"str"
|
||||
],
|
||||
"gnsf" : {
|
||||
"nontrivial_f_LO": [
|
||||
"int"
|
||||
],
|
||||
"purely_linear": [
|
||||
"int"
|
||||
]
|
||||
}
|
||||
},
|
||||
"parameter_values": [
|
||||
"ndarray",
|
||||
[
|
||||
"np"
|
||||
]
|
||||
],
|
||||
"acados_lib_path": [
|
||||
"str"
|
||||
],
|
||||
"problem_class": [
|
||||
"str"
|
||||
],
|
||||
"constraints": {
|
||||
"constr_type": [
|
||||
"str"
|
||||
],
|
||||
"constr_type_e": [
|
||||
"str"
|
||||
],
|
||||
"lbx": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx"
|
||||
]
|
||||
],
|
||||
"lbu": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbu"
|
||||
]
|
||||
],
|
||||
"ubx": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx"
|
||||
]
|
||||
],
|
||||
"ubu": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbu"
|
||||
]
|
||||
],
|
||||
"idxbx": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx"
|
||||
]
|
||||
],
|
||||
"idxbu": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbu"
|
||||
]
|
||||
],
|
||||
"lbx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx_e"
|
||||
]
|
||||
],
|
||||
"ubx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx_e"
|
||||
]
|
||||
],
|
||||
"idxbx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx_e"
|
||||
]
|
||||
],
|
||||
"lbx_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx_0"
|
||||
]
|
||||
],
|
||||
"ubx_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx_0"
|
||||
]
|
||||
],
|
||||
"idxbx_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx_0"
|
||||
]
|
||||
],
|
||||
"idxbxe_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbxe_0"
|
||||
]
|
||||
],
|
||||
"lg": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng"
|
||||
]
|
||||
],
|
||||
"ug": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng"
|
||||
]
|
||||
],
|
||||
"D": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng",
|
||||
"nu"
|
||||
]
|
||||
],
|
||||
"C": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng",
|
||||
"nx"
|
||||
]
|
||||
],
|
||||
"C_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng_e",
|
||||
"nx"
|
||||
]
|
||||
],
|
||||
"lg_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng_e"
|
||||
]
|
||||
],
|
||||
"ug_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng_e"
|
||||
]
|
||||
],
|
||||
"lh": [
|
||||
"ndarray",
|
||||
[
|
||||
"nh"
|
||||
]
|
||||
],
|
||||
"uh": [
|
||||
"ndarray",
|
||||
[
|
||||
"nh"
|
||||
]
|
||||
],
|
||||
"lh_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nh_e"
|
||||
]
|
||||
],
|
||||
"uh_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nh_e"
|
||||
]
|
||||
],
|
||||
"lphi": [
|
||||
"ndarray",
|
||||
[
|
||||
"nphi"
|
||||
]
|
||||
],
|
||||
"uphi": [
|
||||
"ndarray",
|
||||
[
|
||||
"nphi"
|
||||
]
|
||||
],
|
||||
"lphi_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nphi_e"
|
||||
]
|
||||
],
|
||||
"uphi_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nphi_e"
|
||||
]
|
||||
],
|
||||
"lsbx": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbx"
|
||||
]
|
||||
],
|
||||
"usbx": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbx"
|
||||
]
|
||||
],
|
||||
"lsbu": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbu"
|
||||
]
|
||||
],
|
||||
"usbu": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbu"
|
||||
]
|
||||
],
|
||||
"idxsbx": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbx"
|
||||
]
|
||||
],
|
||||
"idxsbu": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbu"
|
||||
]
|
||||
],
|
||||
"lsbx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbx_e"
|
||||
]
|
||||
],
|
||||
"usbx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbx_e"
|
||||
]
|
||||
],
|
||||
"idxsbx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbx_e"
|
||||
]
|
||||
],
|
||||
"lsg": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsg"
|
||||
]
|
||||
],
|
||||
"usg": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsg"
|
||||
]
|
||||
],
|
||||
"idxsg": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsg"
|
||||
]
|
||||
],
|
||||
"lsg_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsg_e"
|
||||
]
|
||||
],
|
||||
"usg_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsg_e"
|
||||
]
|
||||
],
|
||||
"idxsg_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsg_e"
|
||||
]
|
||||
],
|
||||
"lsh": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsh"
|
||||
]
|
||||
],
|
||||
"ush": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsh"
|
||||
]
|
||||
],
|
||||
"idxsh": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsh"
|
||||
]
|
||||
],
|
||||
"lsh_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsh_e"
|
||||
]
|
||||
],
|
||||
"ush_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsh_e"
|
||||
]
|
||||
],
|
||||
"idxsh_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsh_e"
|
||||
]
|
||||
],
|
||||
"lsphi": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsphi"
|
||||
]
|
||||
],
|
||||
"usphi": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsphi"
|
||||
]
|
||||
],
|
||||
"idxsphi": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsphi"
|
||||
]
|
||||
],
|
||||
"lsphi_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsphi_e"
|
||||
]
|
||||
],
|
||||
"usphi_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsphi_e"
|
||||
]
|
||||
],
|
||||
"idxsphi_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsphi_e"
|
||||
]
|
||||
]
|
||||
},
|
||||
"cost": {
|
||||
"cost_type_0": [
|
||||
"str"
|
||||
],
|
||||
"cost_type": [
|
||||
"str"
|
||||
],
|
||||
"cost_type_e": [
|
||||
"str"
|
||||
],
|
||||
"cost_ext_fun_type_0": [
|
||||
"str"
|
||||
],
|
||||
"cost_ext_fun_type": [
|
||||
"str"
|
||||
],
|
||||
"cost_ext_fun_type_e": [
|
||||
"str"
|
||||
],
|
||||
"Vu_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_0",
|
||||
"nu"
|
||||
]
|
||||
],
|
||||
"Vu": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny",
|
||||
"nu"
|
||||
]
|
||||
],
|
||||
"Vx_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_0",
|
||||
"nx"
|
||||
]
|
||||
],
|
||||
"Vx": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny",
|
||||
"nx"
|
||||
]
|
||||
],
|
||||
"Vx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_e",
|
||||
"nx"
|
||||
]
|
||||
],
|
||||
"Vz_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_0",
|
||||
"nz"
|
||||
]
|
||||
],
|
||||
"Vz": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny",
|
||||
"nz"
|
||||
]
|
||||
],
|
||||
"W_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_0",
|
||||
"ny_0"
|
||||
]
|
||||
],
|
||||
"W": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny",
|
||||
"ny"
|
||||
]
|
||||
],
|
||||
"Zl": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns"
|
||||
]
|
||||
],
|
||||
"Zu": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns"
|
||||
]
|
||||
],
|
||||
"zl": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns"
|
||||
]
|
||||
],
|
||||
"zu": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns"
|
||||
]
|
||||
],
|
||||
"W_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_e",
|
||||
"ny_e"
|
||||
]
|
||||
],
|
||||
"yref_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_0"
|
||||
]
|
||||
],
|
||||
"yref": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny"
|
||||
]
|
||||
],
|
||||
"yref_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_e"
|
||||
]
|
||||
],
|
||||
"Zl_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns_e"
|
||||
]
|
||||
],
|
||||
"Zu_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns_e"
|
||||
]
|
||||
],
|
||||
"zl_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns_e"
|
||||
]
|
||||
],
|
||||
"zu_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns_e"
|
||||
]
|
||||
]
|
||||
},
|
||||
"dims": {
|
||||
"N": [
|
||||
"int"
|
||||
],
|
||||
"nbu": [
|
||||
"int"
|
||||
],
|
||||
"nbx": [
|
||||
"int"
|
||||
],
|
||||
"nsbu": [
|
||||
"int"
|
||||
],
|
||||
"nsbx": [
|
||||
"int"
|
||||
],
|
||||
"nsbx_e": [
|
||||
"int"
|
||||
],
|
||||
"nbx_0": [
|
||||
"int"
|
||||
],
|
||||
"nbx_e": [
|
||||
"int"
|
||||
],
|
||||
"nbxe_0": [
|
||||
"int"
|
||||
],
|
||||
"nsg": [
|
||||
"int"
|
||||
],
|
||||
"nsg_e": [
|
||||
"int"
|
||||
],
|
||||
"nsh": [
|
||||
"int"
|
||||
],
|
||||
"nsh_e": [
|
||||
"int"
|
||||
],
|
||||
"nsphi": [
|
||||
"int"
|
||||
],
|
||||
"nsphi_e": [
|
||||
"int"
|
||||
],
|
||||
"ns": [
|
||||
"int"
|
||||
],
|
||||
"ns_e": [
|
||||
"int"
|
||||
],
|
||||
"ng": [
|
||||
"int"
|
||||
],
|
||||
"ng_e": [
|
||||
"int"
|
||||
],
|
||||
"np": [
|
||||
"int"
|
||||
],
|
||||
"nr": [
|
||||
"int"
|
||||
],
|
||||
"nr_e": [
|
||||
"int"
|
||||
],
|
||||
"nh": [
|
||||
"int"
|
||||
],
|
||||
"nh_e": [
|
||||
"int"
|
||||
],
|
||||
"nphi": [
|
||||
"int"
|
||||
],
|
||||
"nphi_e": [
|
||||
"int"
|
||||
],
|
||||
"nu": [
|
||||
"int"
|
||||
],
|
||||
"nx": [
|
||||
"int"
|
||||
],
|
||||
"ny": [
|
||||
"int"
|
||||
],
|
||||
"ny_0": [
|
||||
"int"
|
||||
],
|
||||
"ny_e": [
|
||||
"int"
|
||||
],
|
||||
"nz": [
|
||||
"int"
|
||||
],
|
||||
"gnsf_nx1": [
|
||||
"int"
|
||||
],
|
||||
"gnsf_nz1": [
|
||||
"int"
|
||||
],
|
||||
"gnsf_nuhat": [
|
||||
"int"
|
||||
],
|
||||
"gnsf_ny": [
|
||||
"int"
|
||||
],
|
||||
"gnsf_nout": [
|
||||
"int"
|
||||
]
|
||||
},
|
||||
"solver_options": {
|
||||
"time_steps": [
|
||||
"ndarray",
|
||||
[
|
||||
"N"
|
||||
]
|
||||
],
|
||||
"hessian_approx": [
|
||||
"str"
|
||||
],
|
||||
"hpipm_mode": [
|
||||
"str"
|
||||
],
|
||||
"regularize_method": [
|
||||
"str"
|
||||
],
|
||||
"integrator_type": [
|
||||
"str"
|
||||
],
|
||||
"nlp_solver_type": [
|
||||
"str"
|
||||
],
|
||||
"collocation_type": [
|
||||
"str"
|
||||
],
|
||||
"globalization": [
|
||||
"str"
|
||||
],
|
||||
"nlp_solver_step_length": [
|
||||
"float"
|
||||
],
|
||||
"levenberg_marquardt": [
|
||||
"float"
|
||||
],
|
||||
"qp_solver": [
|
||||
"str"
|
||||
],
|
||||
"tf": [
|
||||
"float"
|
||||
],
|
||||
"Tsim": [
|
||||
"float"
|
||||
],
|
||||
"alpha_min": [
|
||||
"float"
|
||||
],
|
||||
"alpha_reduction": [
|
||||
"float"
|
||||
],
|
||||
"line_search_use_sufficient_descent": [
|
||||
"int"
|
||||
],
|
||||
"globalization_use_SOC": [
|
||||
"int"
|
||||
],
|
||||
"full_step_dual": [
|
||||
"int"
|
||||
],
|
||||
"eps_sufficient_descent": [
|
||||
"float"
|
||||
],
|
||||
"sim_method_num_stages": [
|
||||
"ndarray",
|
||||
[
|
||||
"N"
|
||||
]
|
||||
],
|
||||
"sim_method_num_steps": [
|
||||
"ndarray",
|
||||
[
|
||||
"N"
|
||||
]
|
||||
],
|
||||
"sim_method_newton_iter": [
|
||||
"int"
|
||||
],
|
||||
"sim_method_newton_tol": [
|
||||
"float"
|
||||
],
|
||||
"sim_method_jac_reuse": [
|
||||
"ndarray",
|
||||
[
|
||||
"N"
|
||||
]
|
||||
],
|
||||
"qp_solver_cond_N": [
|
||||
"int"
|
||||
],
|
||||
"qp_solver_warm_start": [
|
||||
"int"
|
||||
],
|
||||
"qp_solver_tol_stat": [
|
||||
"float"
|
||||
],
|
||||
"qp_solver_tol_eq": [
|
||||
"float"
|
||||
],
|
||||
"qp_solver_tol_ineq": [
|
||||
"float"
|
||||
],
|
||||
"qp_solver_tol_comp": [
|
||||
"float"
|
||||
],
|
||||
"qp_solver_iter_max": [
|
||||
"int"
|
||||
],
|
||||
"qp_solver_cond_ric_alg": [
|
||||
"int"
|
||||
],
|
||||
"qp_solver_ric_alg": [
|
||||
"int"
|
||||
],
|
||||
"nlp_solver_tol_stat": [
|
||||
"float"
|
||||
],
|
||||
"nlp_solver_tol_eq": [
|
||||
"float"
|
||||
],
|
||||
"nlp_solver_tol_ineq": [
|
||||
"float"
|
||||
],
|
||||
"nlp_solver_tol_comp": [
|
||||
"float"
|
||||
],
|
||||
"nlp_solver_max_iter": [
|
||||
"int"
|
||||
],
|
||||
"nlp_solver_ext_qp_res": [
|
||||
"int"
|
||||
],
|
||||
"print_level": [
|
||||
"int"
|
||||
],
|
||||
"initialize_t_slacks": [
|
||||
"int"
|
||||
],
|
||||
"exact_hess_cost": [
|
||||
"int"
|
||||
],
|
||||
"exact_hess_constr": [
|
||||
"int"
|
||||
],
|
||||
"exact_hess_dyn": [
|
||||
"int"
|
||||
],
|
||||
"ext_cost_num_hess": [
|
||||
"int"
|
||||
],
|
||||
"ext_fun_compile_flags": [
|
||||
"str"
|
||||
],
|
||||
"model_external_shared_lib_dir": [
|
||||
"str"
|
||||
],
|
||||
"model_external_shared_lib_name": [
|
||||
"str"
|
||||
]
|
||||
}
|
||||
}
|
||||
154
third_party/acados/acados_template/acados_model.py
vendored
Normal file
154
third_party/acados/acados_template/acados_model.py
vendored
Normal file
@@ -0,0 +1,154 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
|
||||
class AcadosModel():
|
||||
"""
|
||||
Class containing all the information to code generate the external CasADi functions
|
||||
that are needed when creating an acados ocp solver or acados integrator.
|
||||
Thus, this class contains:
|
||||
|
||||
a) the :py:attr:`name` of the model,
|
||||
b) all CasADi variables/expressions needed in the CasADi function generation process.
|
||||
"""
|
||||
def __init__(self):
|
||||
## common for OCP and Integrator
|
||||
self.name = None
|
||||
"""
|
||||
The model name is used for code generation. Type: string. Default: :code:`None`
|
||||
"""
|
||||
self.x = None #: CasADi variable describing the state of the system; Default: :code:`None`
|
||||
self.xdot = None #: CasADi variable describing the derivative of the state wrt time; Default: :code:`None`
|
||||
self.u = None #: CasADi variable describing the input of the system; Default: :code:`None`
|
||||
self.z = [] #: CasADi variable describing the algebraic variables of the DAE; Default: :code:`empty`
|
||||
self.p = [] #: CasADi variable describing parameters of the DAE; Default: :code:`empty`
|
||||
# dynamics
|
||||
self.f_impl_expr = None
|
||||
"""
|
||||
CasADi expression for the implicit dynamics :math:`f_\\text{impl}(\dot{x}, x, u, z, p) = 0`.
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'IRK'.
|
||||
Default: :code:`None`
|
||||
"""
|
||||
self.f_expl_expr = None
|
||||
"""
|
||||
CasADi expression for the explicit dynamics :math:`\dot{x} = f_\\text{expl}(x, u, p)`.
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'ERK'.
|
||||
Default: :code:`None`
|
||||
"""
|
||||
self.disc_dyn_expr = None
|
||||
"""
|
||||
CasADi expression for the discrete dynamics :math:`x_{+} = f_\\text{disc}(x, u, p)`.
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'DISCRETE'.
|
||||
Default: :code:`None`
|
||||
"""
|
||||
|
||||
self.dyn_ext_fun_type = 'casadi' #: type of external functions for dynamics module; 'casadi' or 'generic'; Default: 'casadi'
|
||||
self.dyn_generic_source = None #: name of source file for discrete dyanamics; Default: :code:`None`
|
||||
self.dyn_disc_fun_jac_hess = None #: name of function discrete dyanamics + jacobian and hessian; Default: :code:`None`
|
||||
self.dyn_disc_fun_jac = None #: name of function discrete dyanamics + jacobian; Default: :code:`None`
|
||||
self.dyn_disc_fun = None #: name of function discrete dyanamics; Default: :code:`None`
|
||||
|
||||
# for GNSF models
|
||||
self.gnsf = {'nontrivial_f_LO': 1, 'purely_linear': 0}
|
||||
"""
|
||||
dictionary containing information on GNSF structure needed when rendering templates.
|
||||
Contains integers `nontrivial_f_LO`, `purely_linear`.
|
||||
"""
|
||||
|
||||
## for OCP
|
||||
# constraints
|
||||
# BGH(default): lh <= h(x, u) <= uh
|
||||
self.con_h_expr = None #: CasADi expression for the constraint :math:`h`; Default: :code:`None`
|
||||
# BGP(convex over nonlinear): lphi <= phi(r(x, u)) <= uphi
|
||||
self.con_phi_expr = None #: CasADi expression for the constraint phi; Default: :code:`None`
|
||||
self.con_r_expr = None #: CasADi expression for the constraint phi(r); Default: :code:`None`
|
||||
self.con_r_in_phi = None
|
||||
# terminal
|
||||
self.con_h_expr_e = None #: CasADi expression for the terminal constraint :math:`h^e`; Default: :code:`None`
|
||||
self.con_r_expr_e = None #: CasADi expression for the terminal constraint; Default: :code:`None`
|
||||
self.con_phi_expr_e = None #: CasADi expression for the terminal constraint; Default: :code:`None`
|
||||
self.con_r_in_phi_e = None
|
||||
# cost
|
||||
self.cost_y_expr = None #: CasADi expression for nonlinear least squares; Default: :code:`None`
|
||||
self.cost_y_expr_e = None #: CasADi expression for nonlinear least squares, terminal; Default: :code:`None`
|
||||
self.cost_y_expr_0 = None #: CasADi expression for nonlinear least squares, initial; Default: :code:`None`
|
||||
self.cost_expr_ext_cost = None #: CasADi expression for external cost; Default: :code:`None`
|
||||
self.cost_expr_ext_cost_e = None #: CasADi expression for external cost, terminal; Default: :code:`None`
|
||||
self.cost_expr_ext_cost_0 = None #: CasADi expression for external cost, initial; Default: :code:`None`
|
||||
self.cost_expr_ext_cost_custom_hess = None #: CasADi expression for custom hessian (only for external cost); Default: :code:`None`
|
||||
self.cost_expr_ext_cost_custom_hess_e = None #: CasADi expression for custom hessian (only for external cost), terminal; Default: :code:`None`
|
||||
self.cost_expr_ext_cost_custom_hess_0 = None #: CasADi expression for custom hessian (only for external cost), initial; Default: :code:`None`
|
||||
|
||||
## CONVEX_OVER_NONLINEAR convex-over-nonlinear cost: psi(y(x, u, p) - y_ref; p)
|
||||
self.cost_psi_expr_0 = None
|
||||
"""
|
||||
CasADi expression for the outer loss function :math:`\psi(r, p)`, initial; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_psi_expr = None
|
||||
"""
|
||||
CasADi expression for the outer loss function :math:`\psi(r, p)`; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_psi_expr_e = None
|
||||
"""
|
||||
CasADi expression for the outer loss function :math:`\psi(r, p)`, terminal; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_r_in_psi_expr_0 = None
|
||||
"""
|
||||
CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`, initial; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_r_in_psi_expr = None
|
||||
"""
|
||||
CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_r_in_psi_expr_e = None
|
||||
"""
|
||||
CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`, terminal; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_conl_custom_outer_hess_0 = None
|
||||
"""
|
||||
CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), initial; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_conl_custom_outer_hess = None
|
||||
"""
|
||||
CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost); Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_conl_custom_outer_hess_e = None
|
||||
"""
|
||||
CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), terminal; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
3166
third_party/acados/acados_template/acados_ocp.py
vendored
Normal file
3166
third_party/acados/acados_template/acados_ocp.py
vendored
Normal file
File diff suppressed because it is too large
Load Diff
1990
third_party/acados/acados_template/acados_ocp_solver.py
vendored
Normal file
1990
third_party/acados/acados_template/acados_ocp_solver.py
vendored
Normal file
File diff suppressed because it is too large
Load Diff
366
third_party/acados/acados_template/acados_sim.py
vendored
Normal file
366
third_party/acados/acados_template/acados_sim.py
vendored
Normal file
@@ -0,0 +1,366 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
import numpy as np
|
||||
import os
|
||||
from .acados_model import AcadosModel
|
||||
from .utils import get_acados_path, get_lib_ext
|
||||
|
||||
class AcadosSimDims:
|
||||
"""
|
||||
Class containing the dimensions of the model to be simulated.
|
||||
"""
|
||||
def __init__(self):
|
||||
self.__nx = None
|
||||
self.__nu = None
|
||||
self.__nz = 0
|
||||
self.__np = 0
|
||||
|
||||
@property
|
||||
def nx(self):
|
||||
""":math:`n_x` - number of states. Type: int > 0"""
|
||||
return self.__nx
|
||||
|
||||
@property
|
||||
def nz(self):
|
||||
""":math:`n_z` - number of algebraic variables. Type: int >= 0"""
|
||||
return self.__nz
|
||||
|
||||
@property
|
||||
def nu(self):
|
||||
""":math:`n_u` - number of inputs. Type: int >= 0"""
|
||||
return self.__nu
|
||||
|
||||
@property
|
||||
def np(self):
|
||||
""":math:`n_p` - number of parameters. Type: int >= 0"""
|
||||
return self.__np
|
||||
|
||||
@nx.setter
|
||||
def nx(self, nx):
|
||||
if isinstance(nx, int) and nx > 0:
|
||||
self.__nx = nx
|
||||
else:
|
||||
raise Exception('Invalid nx value, expected positive integer.')
|
||||
|
||||
@nz.setter
|
||||
def nz(self, nz):
|
||||
if isinstance(nz, int) and nz > -1:
|
||||
self.__nz = nz
|
||||
else:
|
||||
raise Exception('Invalid nz value, expected nonnegative integer.')
|
||||
|
||||
@nu.setter
|
||||
def nu(self, nu):
|
||||
if isinstance(nu, int) and nu > -1:
|
||||
self.__nu = nu
|
||||
else:
|
||||
raise Exception('Invalid nu value, expected nonnegative integer.')
|
||||
|
||||
@np.setter
|
||||
def np(self, np):
|
||||
if isinstance(np, int) and np > -1:
|
||||
self.__np = np
|
||||
else:
|
||||
raise Exception('Invalid np value, expected nonnegative integer.')
|
||||
|
||||
def set(self, attr, value):
|
||||
setattr(self, attr, value)
|
||||
|
||||
|
||||
class AcadosSimOpts:
|
||||
"""
|
||||
class containing the solver options
|
||||
"""
|
||||
def __init__(self):
|
||||
self.__integrator_type = 'ERK'
|
||||
self.__collocation_type = 'GAUSS_LEGENDRE'
|
||||
self.__Tsim = None
|
||||
# ints
|
||||
self.__sim_method_num_stages = 1
|
||||
self.__sim_method_num_steps = 1
|
||||
self.__sim_method_newton_iter = 3
|
||||
# doubles
|
||||
self.__sim_method_newton_tol = 0.0
|
||||
# bools
|
||||
self.__sens_forw = True
|
||||
self.__sens_adj = False
|
||||
self.__sens_algebraic = False
|
||||
self.__sens_hess = False
|
||||
self.__output_z = True
|
||||
self.__sim_method_jac_reuse = 0
|
||||
self.__ext_fun_compile_flags = '-O2'
|
||||
|
||||
@property
|
||||
def integrator_type(self):
|
||||
"""Integrator type. Default: 'ERK'."""
|
||||
return self.__integrator_type
|
||||
|
||||
@property
|
||||
def num_stages(self):
|
||||
"""Number of stages in the integrator. Default: 1"""
|
||||
return self.__sim_method_num_stages
|
||||
|
||||
@property
|
||||
def num_steps(self):
|
||||
"""Number of steps in the integrator. Default: 1"""
|
||||
return self.__sim_method_num_steps
|
||||
|
||||
@property
|
||||
def newton_iter(self):
|
||||
"""Number of Newton iterations in simulation method. Default: 3"""
|
||||
return self.__sim_method_newton_iter
|
||||
|
||||
@property
|
||||
def newton_tol(self):
|
||||
"""
|
||||
Tolerance for Newton system solved in implicit integrator (IRK, GNSF).
|
||||
0.0 means this is not used and exactly newton_iter iterations are carried out.
|
||||
Default: 0.0
|
||||
"""
|
||||
return self.__sim_method_newton_tol
|
||||
|
||||
@property
|
||||
def sens_forw(self):
|
||||
"""Boolean determining if forward sensitivities are computed. Default: True"""
|
||||
return self.__sens_forw
|
||||
|
||||
@property
|
||||
def sens_adj(self):
|
||||
"""Boolean determining if adjoint sensitivities are computed. Default: False"""
|
||||
return self.__sens_adj
|
||||
|
||||
@property
|
||||
def sens_algebraic(self):
|
||||
"""Boolean determining if sensitivities wrt algebraic variables are computed. Default: False"""
|
||||
return self.__sens_algebraic
|
||||
|
||||
@property
|
||||
def sens_hess(self):
|
||||
"""Boolean determining if hessians are computed. Default: False"""
|
||||
return self.__sens_hess
|
||||
|
||||
@property
|
||||
def output_z(self):
|
||||
"""Boolean determining if values for algebraic variables (corresponding to start of simulation interval) are computed. Default: True"""
|
||||
return self.__output_z
|
||||
|
||||
@property
|
||||
def sim_method_jac_reuse(self):
|
||||
"""Integer determining if jacobians are reused (0 or 1). Default: 0"""
|
||||
return self.__sim_method_jac_reuse
|
||||
|
||||
@property
|
||||
def T(self):
|
||||
"""Time horizon"""
|
||||
return self.__Tsim
|
||||
|
||||
@property
|
||||
def collocation_type(self):
|
||||
"""Collocation type: relevant for implicit integrators
|
||||
-- string in {GAUSS_RADAU_IIA, GAUSS_LEGENDRE}
|
||||
|
||||
Default: GAUSS_LEGENDRE
|
||||
"""
|
||||
return self.__collocation_type
|
||||
|
||||
@property
|
||||
def ext_fun_compile_flags(self):
|
||||
"""
|
||||
String with compiler flags for external function compilation.
|
||||
Default: '-O2'.
|
||||
"""
|
||||
return self.__ext_fun_compile_flags
|
||||
|
||||
@ext_fun_compile_flags.setter
|
||||
def ext_fun_compile_flags(self, ext_fun_compile_flags):
|
||||
if isinstance(ext_fun_compile_flags, str):
|
||||
self.__ext_fun_compile_flags = ext_fun_compile_flags
|
||||
else:
|
||||
raise Exception('Invalid ext_fun_compile_flags, expected a string.\n')
|
||||
|
||||
@integrator_type.setter
|
||||
def integrator_type(self, integrator_type):
|
||||
integrator_types = ('ERK', 'IRK', 'GNSF')
|
||||
if integrator_type in integrator_types:
|
||||
self.__integrator_type = integrator_type
|
||||
else:
|
||||
raise Exception('Invalid integrator_type value. Possible values are:\n\n' \
|
||||
+ ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\n')
|
||||
|
||||
@collocation_type.setter
|
||||
def collocation_type(self, collocation_type):
|
||||
collocation_types = ('GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE')
|
||||
if collocation_type in collocation_types:
|
||||
self.__collocation_type = collocation_type
|
||||
else:
|
||||
raise Exception('Invalid collocation_type value. Possible values are:\n\n' \
|
||||
+ ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\n')
|
||||
|
||||
@T.setter
|
||||
def T(self, T):
|
||||
self.__Tsim = T
|
||||
|
||||
@num_stages.setter
|
||||
def num_stages(self, num_stages):
|
||||
if isinstance(num_stages, int):
|
||||
self.__sim_method_num_stages = num_stages
|
||||
else:
|
||||
raise Exception('Invalid num_stages value. num_stages must be an integer.')
|
||||
|
||||
@num_steps.setter
|
||||
def num_steps(self, num_steps):
|
||||
if isinstance(num_steps, int):
|
||||
self.__sim_method_num_steps = num_steps
|
||||
else:
|
||||
raise Exception('Invalid num_steps value. num_steps must be an integer.')
|
||||
|
||||
@newton_iter.setter
|
||||
def newton_iter(self, newton_iter):
|
||||
if isinstance(newton_iter, int):
|
||||
self.__sim_method_newton_iter = newton_iter
|
||||
else:
|
||||
raise Exception('Invalid newton_iter value. newton_iter must be an integer.')
|
||||
|
||||
@newton_tol.setter
|
||||
def newton_tol(self, newton_tol):
|
||||
if isinstance(newton_tol, float):
|
||||
self.__sim_method_newton_tol = newton_tol
|
||||
else:
|
||||
raise Exception('Invalid newton_tol value. newton_tol must be an float.')
|
||||
|
||||
@sens_forw.setter
|
||||
def sens_forw(self, sens_forw):
|
||||
if sens_forw in (True, False):
|
||||
self.__sens_forw = sens_forw
|
||||
else:
|
||||
raise Exception('Invalid sens_forw value. sens_forw must be a Boolean.')
|
||||
|
||||
@sens_adj.setter
|
||||
def sens_adj(self, sens_adj):
|
||||
if sens_adj in (True, False):
|
||||
self.__sens_adj = sens_adj
|
||||
else:
|
||||
raise Exception('Invalid sens_adj value. sens_adj must be a Boolean.')
|
||||
|
||||
@sens_hess.setter
|
||||
def sens_hess(self, sens_hess):
|
||||
if sens_hess in (True, False):
|
||||
self.__sens_hess = sens_hess
|
||||
else:
|
||||
raise Exception('Invalid sens_hess value. sens_hess must be a Boolean.')
|
||||
|
||||
@sens_algebraic.setter
|
||||
def sens_algebraic(self, sens_algebraic):
|
||||
if sens_algebraic in (True, False):
|
||||
self.__sens_algebraic = sens_algebraic
|
||||
else:
|
||||
raise Exception('Invalid sens_algebraic value. sens_algebraic must be a Boolean.')
|
||||
|
||||
@output_z.setter
|
||||
def output_z(self, output_z):
|
||||
if output_z in (True, False):
|
||||
self.__output_z = output_z
|
||||
else:
|
||||
raise Exception('Invalid output_z value. output_z must be a Boolean.')
|
||||
|
||||
@sim_method_jac_reuse.setter
|
||||
def sim_method_jac_reuse(self, sim_method_jac_reuse):
|
||||
if sim_method_jac_reuse in (0, 1):
|
||||
self.__sim_method_jac_reuse = sim_method_jac_reuse
|
||||
else:
|
||||
raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be 0 or 1.')
|
||||
|
||||
class AcadosSim:
|
||||
"""
|
||||
The class has the following properties that can be modified to formulate a specific simulation problem, see below:
|
||||
|
||||
:param acados_path: string with the path to acados. It is used to generate the include and lib paths.
|
||||
|
||||
- :py:attr:`dims` of type :py:class:`acados_template.acados_ocp.AcadosSimDims` - are automatically detected from model
|
||||
- :py:attr:`model` of type :py:class:`acados_template.acados_model.AcadosModel`
|
||||
- :py:attr:`solver_options` of type :py:class:`acados_template.acados_sim.AcadosSimOpts`
|
||||
|
||||
- :py:attr:`acados_include_path` (set automatically)
|
||||
- :py:attr:`shared_lib_ext` (set automatically)
|
||||
- :py:attr:`acados_lib_path` (set automatically)
|
||||
- :py:attr:`parameter_values` - used to initialize the parameters (can be changed)
|
||||
|
||||
"""
|
||||
def __init__(self, acados_path=''):
|
||||
if acados_path == '':
|
||||
acados_path = get_acados_path()
|
||||
self.dims = AcadosSimDims()
|
||||
"""Dimension definitions, automatically detected from :py:attr:`model`. Type :py:class:`acados_template.acados_sim.AcadosSimDims`"""
|
||||
self.model = AcadosModel()
|
||||
"""Model definitions, type :py:class:`acados_template.acados_model.AcadosModel`"""
|
||||
self.solver_options = AcadosSimOpts()
|
||||
"""Solver Options, type :py:class:`acados_template.acados_sim.AcadosSimOpts`"""
|
||||
|
||||
self.acados_include_path = os.path.join(acados_path, 'include').replace(os.sep, '/') # the replace part is important on Windows for CMake
|
||||
"""Path to acados include directory (set automatically), type: `string`"""
|
||||
self.acados_lib_path = os.path.join(acados_path, 'lib').replace(os.sep, '/') # the replace part is important on Windows for CMake
|
||||
"""Path to where acados library is located (set automatically), type: `string`"""
|
||||
|
||||
self.code_export_directory = 'c_generated_code'
|
||||
"""Path to where code will be exported. Default: `c_generated_code`."""
|
||||
self.shared_lib_ext = get_lib_ext()
|
||||
|
||||
# get cython paths
|
||||
from sysconfig import get_paths
|
||||
self.cython_include_dirs = [np.get_include(), get_paths()['include']]
|
||||
|
||||
self.__parameter_values = np.array([])
|
||||
self.__problem_class = 'SIM'
|
||||
|
||||
@property
|
||||
def parameter_values(self):
|
||||
""":math:`p` - initial values for parameter - can be updated"""
|
||||
return self.__parameter_values
|
||||
|
||||
@parameter_values.setter
|
||||
def parameter_values(self, parameter_values):
|
||||
if isinstance(parameter_values, np.ndarray):
|
||||
self.__parameter_values = parameter_values
|
||||
else:
|
||||
raise Exception('Invalid parameter_values value. ' +
|
||||
f'Expected numpy array, got {type(parameter_values)}.')
|
||||
|
||||
def set(self, attr, value):
|
||||
# tokenize string
|
||||
tokens = attr.split('_', 1)
|
||||
if len(tokens) > 1:
|
||||
setter_to_call = getattr(getattr(self, tokens[0]), 'set')
|
||||
else:
|
||||
setter_to_call = getattr(self, 'set')
|
||||
|
||||
setter_to_call(tokens[1], value)
|
||||
|
||||
return
|
||||
53
third_party/acados/acados_template/acados_sim_layout.json
vendored
Normal file
53
third_party/acados/acados_template/acados_sim_layout.json
vendored
Normal file
@@ -0,0 +1,53 @@
|
||||
{
|
||||
"acados_include_path": [
|
||||
"str"
|
||||
],
|
||||
"model": {
|
||||
"name" : [
|
||||
"str"
|
||||
]
|
||||
},
|
||||
"acados_lib_path": [
|
||||
"str"
|
||||
],
|
||||
"dims": {
|
||||
"np": [
|
||||
"int"
|
||||
],
|
||||
"nu": [
|
||||
"int"
|
||||
],
|
||||
"nx": [
|
||||
"int"
|
||||
],
|
||||
"nz": [
|
||||
"int"
|
||||
]
|
||||
},
|
||||
"solver_options": {
|
||||
"integrator_type": [
|
||||
"str"
|
||||
],
|
||||
"collocation_type": [
|
||||
"str"
|
||||
],
|
||||
"Tsim": [
|
||||
"float"
|
||||
],
|
||||
"sim_method_num_stages": [
|
||||
"int"
|
||||
],
|
||||
"sim_method_num_steps": [
|
||||
"int"
|
||||
],
|
||||
"sim_method_newton_iter": [
|
||||
"int"
|
||||
],
|
||||
"sim_method_newton_tol": [
|
||||
"float"
|
||||
],
|
||||
"ext_fun_compile_flags": [
|
||||
"str"
|
||||
]
|
||||
}
|
||||
}
|
||||
558
third_party/acados/acados_template/acados_sim_solver.py
vendored
Normal file
558
third_party/acados/acados_template/acados_sim_solver.py
vendored
Normal file
@@ -0,0 +1,558 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
import sys
|
||||
import os
|
||||
import json
|
||||
import importlib
|
||||
|
||||
import numpy as np
|
||||
|
||||
from subprocess import DEVNULL, call, STDOUT
|
||||
|
||||
from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_bool, byref
|
||||
from copy import deepcopy
|
||||
|
||||
from .casadi_function_generation import generate_c_code_implicit_ode, generate_c_code_gnsf, generate_c_code_explicit_ode
|
||||
from .acados_sim import AcadosSim
|
||||
from .acados_ocp import AcadosOcp
|
||||
from .utils import is_column, render_template, format_class_dict, make_object_json_dumpable,\
|
||||
make_model_consistent, set_up_imported_gnsf_model, get_python_interface_path, get_lib_ext,\
|
||||
casadi_length, is_empty, check_casadi_version
|
||||
from .builders import CMakeBuilder
|
||||
from .gnsf.detect_gnsf_structure import detect_gnsf_structure
|
||||
|
||||
|
||||
|
||||
def make_sim_dims_consistent(acados_sim: AcadosSim):
|
||||
dims = acados_sim.dims
|
||||
model = acados_sim.model
|
||||
# nx
|
||||
if is_column(model.x):
|
||||
dims.nx = casadi_length(model.x)
|
||||
else:
|
||||
raise Exception('model.x should be column vector!')
|
||||
|
||||
# nu
|
||||
if is_empty(model.u):
|
||||
dims.nu = 0
|
||||
else:
|
||||
dims.nu = casadi_length(model.u)
|
||||
|
||||
# nz
|
||||
if is_empty(model.z):
|
||||
dims.nz = 0
|
||||
else:
|
||||
dims.nz = casadi_length(model.z)
|
||||
|
||||
# np
|
||||
if is_empty(model.p):
|
||||
dims.np = 0
|
||||
else:
|
||||
dims.np = casadi_length(model.p)
|
||||
if acados_sim.parameter_values.shape[0] != dims.np:
|
||||
raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \
|
||||
f'\nGot np = {dims.np}, acados_sim.parameter_values.shape = {acados_sim.parameter_values.shape[0]}\n')
|
||||
|
||||
|
||||
def get_sim_layout():
|
||||
python_interface_path = get_python_interface_path()
|
||||
abs_path = os.path.join(python_interface_path, 'acados_sim_layout.json')
|
||||
with open(abs_path, 'r') as f:
|
||||
sim_layout = json.load(f)
|
||||
return sim_layout
|
||||
|
||||
|
||||
def sim_formulation_json_dump(acados_sim: AcadosSim, json_file='acados_sim.json'):
|
||||
# Load acados_sim structure description
|
||||
sim_layout = get_sim_layout()
|
||||
|
||||
# Copy input sim object dictionary
|
||||
sim_dict = dict(deepcopy(acados_sim).__dict__)
|
||||
|
||||
for key, v in sim_layout.items():
|
||||
# skip non dict attributes
|
||||
if not isinstance(v, dict): continue
|
||||
# Copy sim object attributes dictionaries
|
||||
sim_dict[key]=dict(getattr(acados_sim, key).__dict__)
|
||||
|
||||
sim_json = format_class_dict(sim_dict)
|
||||
|
||||
with open(json_file, 'w') as f:
|
||||
json.dump(sim_json, f, default=make_object_json_dumpable, indent=4, sort_keys=True)
|
||||
|
||||
|
||||
def sim_get_default_cmake_builder() -> CMakeBuilder:
|
||||
"""
|
||||
If :py:class:`~acados_template.acados_sim_solver.AcadosSimSolver` is used with `CMake` this function returns a good first setting.
|
||||
:return: default :py:class:`~acados_template.builders.CMakeBuilder`
|
||||
"""
|
||||
cmake_builder = CMakeBuilder()
|
||||
cmake_builder.options_on = ['BUILD_ACADOS_SIM_SOLVER_LIB']
|
||||
return cmake_builder
|
||||
|
||||
|
||||
def sim_render_templates(json_file, model_name: str, code_export_dir, cmake_options: CMakeBuilder = None):
|
||||
# setting up loader and environment
|
||||
json_path = os.path.join(os.getcwd(), json_file)
|
||||
|
||||
if not os.path.exists(json_path):
|
||||
raise Exception(f"{json_path} not found!")
|
||||
|
||||
# Render templates
|
||||
in_file = 'acados_sim_solver.in.c'
|
||||
out_file = f'acados_sim_solver_{model_name}.c'
|
||||
render_template(in_file, out_file, code_export_dir, json_path)
|
||||
|
||||
in_file = 'acados_sim_solver.in.h'
|
||||
out_file = f'acados_sim_solver_{model_name}.h'
|
||||
render_template(in_file, out_file, code_export_dir, json_path)
|
||||
|
||||
in_file = 'acados_sim_solver.in.pxd'
|
||||
out_file = f'acados_sim_solver.pxd'
|
||||
render_template(in_file, out_file, code_export_dir, json_path)
|
||||
|
||||
# Builder
|
||||
if cmake_options is not None:
|
||||
in_file = 'CMakeLists.in.txt'
|
||||
out_file = 'CMakeLists.txt'
|
||||
render_template(in_file, out_file, code_export_dir, json_path)
|
||||
else:
|
||||
in_file = 'Makefile.in'
|
||||
out_file = 'Makefile'
|
||||
render_template(in_file, out_file, code_export_dir, json_path)
|
||||
|
||||
in_file = 'main_sim.in.c'
|
||||
out_file = f'main_sim_{model_name}.c'
|
||||
render_template(in_file, out_file, code_export_dir, json_path)
|
||||
|
||||
# folder model
|
||||
model_dir = os.path.join(code_export_dir, model_name + '_model')
|
||||
|
||||
in_file = 'model.in.h'
|
||||
out_file = f'{model_name}_model.h'
|
||||
render_template(in_file, out_file, model_dir, json_path)
|
||||
|
||||
|
||||
def sim_generate_external_functions(acados_sim: AcadosSim):
|
||||
model = acados_sim.model
|
||||
model = make_model_consistent(model)
|
||||
|
||||
integrator_type = acados_sim.solver_options.integrator_type
|
||||
|
||||
opts = dict(generate_hess = acados_sim.solver_options.sens_hess,
|
||||
code_export_directory = acados_sim.code_export_directory)
|
||||
|
||||
# create code_export_dir, model_dir
|
||||
code_export_dir = acados_sim.code_export_directory
|
||||
opts['code_export_directory'] = code_export_dir
|
||||
model_dir = os.path.join(code_export_dir, model.name + '_model')
|
||||
if not os.path.exists(model_dir):
|
||||
os.makedirs(model_dir)
|
||||
|
||||
# generate external functions
|
||||
check_casadi_version()
|
||||
if integrator_type == 'ERK':
|
||||
generate_c_code_explicit_ode(model, opts)
|
||||
elif integrator_type == 'IRK':
|
||||
generate_c_code_implicit_ode(model, opts)
|
||||
elif integrator_type == 'GNSF':
|
||||
generate_c_code_gnsf(model, opts)
|
||||
|
||||
|
||||
class AcadosSimSolver:
|
||||
"""
|
||||
Class to interact with the acados integrator C object.
|
||||
|
||||
:param acados_sim: type :py:class:`~acados_template.acados_ocp.AcadosOcp` (takes values to generate an instance :py:class:`~acados_template.acados_sim.AcadosSim`) or :py:class:`~acados_template.acados_sim.AcadosSim`
|
||||
:param json_file: Default: 'acados_sim.json'
|
||||
:param build: Default: True
|
||||
:param cmake_builder: type :py:class:`~acados_template.utils.CMakeBuilder` generate a `CMakeLists.txt` and use
|
||||
the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with
|
||||
`MS Visual Studio`); default: `None`
|
||||
"""
|
||||
if sys.platform=="win32":
|
||||
from ctypes import wintypes
|
||||
from ctypes import WinDLL
|
||||
dlclose = WinDLL('kernel32', use_last_error=True).FreeLibrary
|
||||
dlclose.argtypes = [wintypes.HMODULE]
|
||||
else:
|
||||
dlclose = CDLL(None).dlclose
|
||||
dlclose.argtypes = [c_void_p]
|
||||
|
||||
|
||||
@classmethod
|
||||
def generate(cls, acados_sim: AcadosSim, json_file='acados_sim.json', cmake_builder: CMakeBuilder = None):
|
||||
"""
|
||||
Generates the code for an acados sim solver, given the description in acados_sim
|
||||
"""
|
||||
|
||||
acados_sim.code_export_directory = os.path.abspath(acados_sim.code_export_directory)
|
||||
|
||||
# make dims consistent
|
||||
make_sim_dims_consistent(acados_sim)
|
||||
|
||||
# module dependent post processing
|
||||
if acados_sim.solver_options.integrator_type == 'GNSF':
|
||||
if acados_sim.solver_options.sens_hess == True:
|
||||
raise Exception("AcadosSimSolver: GNSF does not support sens_hess = True.")
|
||||
if 'gnsf_model' in acados_sim.__dict__:
|
||||
set_up_imported_gnsf_model(acados_sim)
|
||||
else:
|
||||
detect_gnsf_structure(acados_sim)
|
||||
|
||||
# generate external functions
|
||||
sim_generate_external_functions(acados_sim)
|
||||
|
||||
# dump to json
|
||||
sim_formulation_json_dump(acados_sim, json_file)
|
||||
|
||||
# render templates
|
||||
sim_render_templates(json_file, acados_sim.model.name, acados_sim.code_export_directory, cmake_builder)
|
||||
|
||||
|
||||
@classmethod
|
||||
def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None, verbose: bool = True):
|
||||
# Compile solver
|
||||
cwd = os.getcwd()
|
||||
os.chdir(code_export_dir)
|
||||
if with_cython:
|
||||
call(
|
||||
['make', 'clean_sim_cython'],
|
||||
stdout=None if verbose else DEVNULL,
|
||||
stderr=None if verbose else STDOUT
|
||||
)
|
||||
call(
|
||||
['make', 'sim_cython'],
|
||||
stdout=None if verbose else DEVNULL,
|
||||
stderr=None if verbose else STDOUT
|
||||
)
|
||||
else:
|
||||
if cmake_builder is not None:
|
||||
cmake_builder.exec(code_export_dir, verbose=verbose)
|
||||
else:
|
||||
call(
|
||||
['make', 'sim_shared_lib'],
|
||||
stdout=None if verbose else DEVNULL,
|
||||
stderr=None if verbose else STDOUT
|
||||
)
|
||||
os.chdir(cwd)
|
||||
|
||||
|
||||
@classmethod
|
||||
def create_cython_solver(cls, json_file):
|
||||
"""
|
||||
"""
|
||||
with open(json_file, 'r') as f:
|
||||
acados_sim_json = json.load(f)
|
||||
code_export_directory = acados_sim_json['code_export_directory']
|
||||
|
||||
importlib.invalidate_caches()
|
||||
rel_code_export_directory = os.path.relpath(code_export_directory)
|
||||
acados_sim_solver_pyx = importlib.import_module(f'{rel_code_export_directory}.acados_sim_solver_pyx')
|
||||
|
||||
AcadosSimSolverCython = getattr(acados_sim_solver_pyx, 'AcadosSimSolverCython')
|
||||
return AcadosSimSolverCython(acados_sim_json['model']['name'])
|
||||
|
||||
def __init__(self, acados_sim, json_file='acados_sim.json', generate=True, build=True, cmake_builder: CMakeBuilder = None, verbose: bool = True):
|
||||
|
||||
self.solver_created = False
|
||||
self.acados_sim = acados_sim
|
||||
model_name = acados_sim.model.name
|
||||
self.model_name = model_name
|
||||
|
||||
code_export_dir = os.path.abspath(acados_sim.code_export_directory)
|
||||
|
||||
# reuse existing json and casadi functions, when creating integrator from ocp
|
||||
if generate and not isinstance(acados_sim, AcadosOcp):
|
||||
self.generate(acados_sim, json_file=json_file, cmake_builder=cmake_builder)
|
||||
|
||||
if build:
|
||||
self.build(code_export_dir, cmake_builder=cmake_builder, verbose=True)
|
||||
|
||||
# prepare library loading
|
||||
lib_prefix = 'lib'
|
||||
lib_ext = get_lib_ext()
|
||||
if os.name == 'nt':
|
||||
lib_prefix = ''
|
||||
|
||||
# Load acados library to avoid unloading the library.
|
||||
# This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed.
|
||||
# Unloading a library which uses OpenMP results in a segfault (on any platform?).
|
||||
# see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp]
|
||||
# or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html]
|
||||
libacados_name = f'{lib_prefix}acados{lib_ext}'
|
||||
libacados_filepath = os.path.join(acados_sim.acados_lib_path, libacados_name)
|
||||
self.__acados_lib = CDLL(libacados_filepath)
|
||||
# find out if acados was compiled with OpenMP
|
||||
try:
|
||||
self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None
|
||||
except AttributeError as e:
|
||||
self.__acados_lib_uses_omp = False
|
||||
if self.__acados_lib_uses_omp:
|
||||
print('acados was compiled with OpenMP.')
|
||||
else:
|
||||
print('acados was compiled without OpenMP.')
|
||||
libacados_sim_solver_name = f'{lib_prefix}acados_sim_solver_{self.model_name}{lib_ext}'
|
||||
self.shared_lib_name = os.path.join(code_export_dir, libacados_sim_solver_name)
|
||||
|
||||
# get shared_lib
|
||||
self.shared_lib = CDLL(self.shared_lib_name)
|
||||
|
||||
# create capsule
|
||||
getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule").restype = c_void_p
|
||||
self.capsule = getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule")()
|
||||
|
||||
# create solver
|
||||
getattr(self.shared_lib, f"{model_name}_acados_sim_create").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_sim_create").restype = c_int
|
||||
assert getattr(self.shared_lib, f"{model_name}_acados_sim_create")(self.capsule)==0
|
||||
self.solver_created = True
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").restype = c_void_p
|
||||
self.sim_opts = getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").restype = c_void_p
|
||||
self.sim_dims = getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").restype = c_void_p
|
||||
self.sim_config = getattr(self.shared_lib, f"{model_name}_acados_get_sim_config")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").restype = c_void_p
|
||||
self.sim_out = getattr(self.shared_lib, f"{model_name}_acados_get_sim_out")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").restype = c_void_p
|
||||
self.sim_in = getattr(self.shared_lib, f"{model_name}_acados_get_sim_in")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").restype = c_void_p
|
||||
self.sim_solver = getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver")(self.capsule)
|
||||
|
||||
self.gettable_vectors = ['x', 'u', 'z', 'S_adj']
|
||||
self.gettable_matrices = ['S_forw', 'Sx', 'Su', 'S_hess', 'S_algebraic']
|
||||
self.gettable_scalars = ['CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la']
|
||||
|
||||
|
||||
def simulate(self, x=None, u=None, z=None, p=None):
|
||||
"""
|
||||
Simulate the system forward for the given x, u, z, p and return x_next.
|
||||
Wrapper around `solve()` taking care of setting/getting inputs/outputs.
|
||||
"""
|
||||
if x is not None:
|
||||
self.set('x', x)
|
||||
if u is not None:
|
||||
self.set('u', u)
|
||||
if z is not None:
|
||||
self.set('z', z)
|
||||
if p is not None:
|
||||
self.set('p', p)
|
||||
|
||||
status = self.solve()
|
||||
|
||||
if status == 2:
|
||||
print("Warning: acados_sim_solver reached maximum iterations.")
|
||||
elif status != 0:
|
||||
raise Exception(f'acados_sim_solver for model {self.model_name} returned status {status}.')
|
||||
|
||||
x_next = self.get('x')
|
||||
return x_next
|
||||
|
||||
|
||||
def solve(self):
|
||||
"""
|
||||
Solve the simulation problem with current input.
|
||||
"""
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").restype = c_int
|
||||
|
||||
status = getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve")(self.capsule)
|
||||
return status
|
||||
|
||||
|
||||
def get(self, field_):
|
||||
"""
|
||||
Get the last solution of the solver.
|
||||
|
||||
:param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic', 'CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la']
|
||||
"""
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
if field_ in self.gettable_vectors:
|
||||
# get dims
|
||||
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
|
||||
dims_data = cast(dims.ctypes.data, POINTER(c_int))
|
||||
|
||||
self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data)
|
||||
|
||||
# allocate array
|
||||
out = np.ascontiguousarray(np.zeros((dims[0],)), dtype=np.float64)
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
|
||||
self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data)
|
||||
|
||||
elif field_ in self.gettable_matrices:
|
||||
# get dims
|
||||
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
|
||||
dims_data = cast(dims.ctypes.data, POINTER(c_int))
|
||||
|
||||
self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data)
|
||||
|
||||
out = np.zeros((dims[0], dims[1]), order='F')
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
|
||||
self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data)
|
||||
|
||||
elif field_ in self.gettable_scalars:
|
||||
scalar = c_double()
|
||||
scalar_data = byref(scalar)
|
||||
self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, scalar_data)
|
||||
|
||||
out = scalar.value
|
||||
else:
|
||||
raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \
|
||||
f' available fields are {", ".join(self.gettable_vectors+self.gettable_matrices)}, {", ".join(self.gettable_scalars)}')
|
||||
|
||||
return out
|
||||
|
||||
|
||||
|
||||
def set(self, field_: str, value_):
|
||||
"""
|
||||
Set numerical data inside the solver.
|
||||
|
||||
:param field: string in ['x', 'u', 'p', 'xdot', 'z', 'seed_adj', 'T']
|
||||
:param value: the value with appropriate size.
|
||||
"""
|
||||
settable = ['x', 'u', 'p', 'xdot', 'z', 'seed_adj', 'T'] # S_forw
|
||||
|
||||
# TODO: check and throw error here. then remove checks in Cython for speed
|
||||
# cast value_ to avoid conversion issues
|
||||
if isinstance(value_, (float, int)):
|
||||
value_ = np.array([value_])
|
||||
|
||||
value_ = value_.astype(float)
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
value_data_p = cast((value_data), c_void_p)
|
||||
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
# treat parameters separately
|
||||
if field_ == 'p':
|
||||
model_name = self.acados_sim.model.name
|
||||
getattr(self.shared_lib, f"{model_name}_acados_sim_update_params").argtypes = [c_void_p, POINTER(c_double), c_int]
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
getattr(self.shared_lib, f"{model_name}_acados_sim_update_params")(self.capsule, value_data, value_.shape[0])
|
||||
return
|
||||
else:
|
||||
# dimension check
|
||||
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
|
||||
dims_data = cast(dims.ctypes.data, POINTER(c_int))
|
||||
|
||||
self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data)
|
||||
|
||||
value_ = np.ravel(value_, order='F')
|
||||
|
||||
value_shape = value_.shape
|
||||
if len(value_shape) == 1:
|
||||
value_shape = (value_shape[0], 0)
|
||||
|
||||
if value_shape != tuple(dims):
|
||||
raise Exception(f'AcadosSimSolver.set(): mismatching dimension' \
|
||||
f' for field "{field_}" with dimension {tuple(dims)} (you have {value_shape}).')
|
||||
|
||||
# set
|
||||
if field_ in ['xdot', 'z']:
|
||||
self.shared_lib.sim_solver_set.argtypes = [c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.sim_solver_set(self.sim_solver, field, value_data_p)
|
||||
elif field_ in settable:
|
||||
self.shared_lib.sim_in_set.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, value_data_p)
|
||||
else:
|
||||
raise Exception(f'AcadosSimSolver.set(): Unknown field {field_},' \
|
||||
f' available fields are {", ".join(settable)}')
|
||||
|
||||
return
|
||||
|
||||
|
||||
def options_set(self, field_: str, value_: bool):
|
||||
"""
|
||||
Set solver options
|
||||
|
||||
:param field: string in ['sens_forw', 'sens_adj', 'sens_hess']
|
||||
:param value: Boolean
|
||||
"""
|
||||
fields = ['sens_forw', 'sens_adj', 'sens_hess']
|
||||
if field_ not in fields:
|
||||
raise Exception(f"field {field_} not supported. Supported values are {', '.join(fields)}.\n")
|
||||
|
||||
field = field_.encode('utf-8')
|
||||
value_ctypes = c_bool(value_)
|
||||
|
||||
if not isinstance(value_, bool):
|
||||
raise TypeError("options_set: expected boolean for value")
|
||||
|
||||
# only allow setting
|
||||
if getattr(self.acados_sim.solver_options, field_) or value_ == False:
|
||||
self.shared_lib.sim_opts_set.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_bool)]
|
||||
self.shared_lib.sim_opts_set(self.sim_config, self.sim_opts, field, value_ctypes)
|
||||
else:
|
||||
raise RuntimeError(f"Cannot set option {field_} to True, because it was False in original solver options.\n")
|
||||
|
||||
return
|
||||
|
||||
|
||||
def __del__(self):
|
||||
|
||||
if self.solver_created:
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").restype = c_int
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_free")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").restype = c_int
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule")(self.capsule)
|
||||
|
||||
try:
|
||||
self.dlclose(self.shared_lib._handle)
|
||||
except:
|
||||
print(f"WARNING: acados Python interface could not close shared_lib handle of AcadosSimSolver {self.model_name}.\n",
|
||||
"Attempting to create a new one with the same name will likely result in the old one being used!")
|
||||
pass
|
||||
130
third_party/acados/acados_template/builders.py
vendored
Normal file
130
third_party/acados/acados_template/builders.py
vendored
Normal file
@@ -0,0 +1,130 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
import os
|
||||
import sys
|
||||
from subprocess import DEVNULL, call, STDOUT
|
||||
|
||||
|
||||
class CMakeBuilder:
|
||||
"""
|
||||
Class to work with the `CMake` build system.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._source_dir = None # private source directory, this is set to code_export_dir
|
||||
self.build_dir = 'build'
|
||||
self._build_dir = None # private build directory, usually rendered to abspath(build_dir)
|
||||
self.generator = None
|
||||
"""Defines the generator, options can be found via `cmake --help` under 'Generator'. Type: string. Linux default 'Unix Makefiles', Windows 'Visual Studio 15 2017 Win64'; default value: `None`."""
|
||||
# set something for Windows
|
||||
if os.name == 'nt':
|
||||
self.generator = 'Visual Studio 15 2017 Win64'
|
||||
self.build_targets = None
|
||||
"""A comma-separated list of the build targets, if `None` then all targets will be build; type: List of strings; default: `None`."""
|
||||
self.options_on = None
|
||||
"""List of strings as CMake options which are translated to '-D Opt[0]=ON -D Opt[1]=ON ...'; default: `None`."""
|
||||
|
||||
# Generate the command string for handling the cmake command.
|
||||
def get_cmd1_cmake(self):
|
||||
defines_str = ''
|
||||
if self.options_on is not None:
|
||||
defines_arr = [f' -D{opt}=ON' for opt in self.options_on]
|
||||
defines_str = ' '.join(defines_arr)
|
||||
generator_str = ''
|
||||
if self.generator is not None:
|
||||
generator_str = f' -G"{self.generator}"'
|
||||
return f'cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX="{self._source_dir}"{defines_str}{generator_str} -Wdev -S"{self._source_dir}" -B"{self._build_dir}"'
|
||||
|
||||
# Generate the command string for handling the build.
|
||||
def get_cmd2_build(self):
|
||||
import multiprocessing
|
||||
cmd = f'cmake --build "{self._build_dir}" --config Release -j{multiprocessing.cpu_count()}'
|
||||
if self.build_targets is not None:
|
||||
cmd += f' -t {self.build_targets}'
|
||||
return cmd
|
||||
|
||||
# Generate the command string for handling the install command.
|
||||
def get_cmd3_install(self):
|
||||
return f'cmake --install "{self._build_dir}"'
|
||||
|
||||
def exec(self, code_export_directory, verbose=True):
|
||||
"""
|
||||
Execute the compilation using `CMake` with the given settings.
|
||||
:param code_export_directory: must be the absolute path to the directory where the code was exported to
|
||||
"""
|
||||
if(os.path.isabs(code_export_directory) is False):
|
||||
print(f'(W) the code export directory "{code_export_directory}" is not an absolute path!')
|
||||
self._source_dir = code_export_directory
|
||||
self._build_dir = os.path.abspath(self.build_dir)
|
||||
try:
|
||||
os.mkdir(self._build_dir)
|
||||
except FileExistsError as e:
|
||||
pass
|
||||
|
||||
try:
|
||||
os.chdir(self._build_dir)
|
||||
cmd_str = self.get_cmd1_cmake()
|
||||
print(f'call("{cmd_str})"')
|
||||
retcode = call(
|
||||
cmd_str,
|
||||
shell=True,
|
||||
stdout=None if verbose else DEVNULL,
|
||||
stderr=None if verbose else STDOUT
|
||||
)
|
||||
if retcode != 0:
|
||||
raise RuntimeError(f'CMake command "{cmd_str}" was terminated by signal {retcode}')
|
||||
cmd_str = self.get_cmd2_build()
|
||||
print(f'call("{cmd_str}")')
|
||||
retcode = call(
|
||||
cmd_str,
|
||||
shell=True,
|
||||
stdout=None if verbose else DEVNULL,
|
||||
stderr=None if verbose else STDOUT
|
||||
)
|
||||
if retcode != 0:
|
||||
raise RuntimeError(f'Build command "{cmd_str}" was terminated by signal {retcode}')
|
||||
cmd_str = self.get_cmd3_install()
|
||||
print(f'call("{cmd_str}")')
|
||||
retcode = call(
|
||||
cmd_str,
|
||||
shell=True,
|
||||
stdout=None if verbose else DEVNULL,
|
||||
stderr=None if verbose else STDOUT
|
||||
)
|
||||
if retcode != 0:
|
||||
raise RuntimeError(f'Install command "{cmd_str}" was terminated by signal {retcode}')
|
||||
except OSError as e:
|
||||
print("Execution failed:", e, file=sys.stderr)
|
||||
except Exception as e:
|
||||
print("Execution failed:", e, file=sys.stderr)
|
||||
exit(1)
|
||||
397
third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt
vendored
Normal file
397
third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt
vendored
Normal file
@@ -0,0 +1,397 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
{%- if solver_options.qp_solver %}
|
||||
{%- set qp_solver = solver_options.qp_solver %}
|
||||
{%- else %}
|
||||
{%- set qp_solver = "FULL_CONDENSING_HPIPM" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if solver_options.hessian_approx %}
|
||||
{%- set hessian_approx = solver_options.hessian_approx %}
|
||||
{%- elif solver_options.sens_hess %}
|
||||
{%- set hessian_approx = "EXACT" %}
|
||||
{%- else %}
|
||||
{%- set hessian_approx = "GAUSS_NEWTON" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constraints.constr_type %}
|
||||
{%- set constr_type = constraints.constr_type %}
|
||||
{%- else %}
|
||||
{%- set constr_type = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constraints.constr_type_e %}
|
||||
{%- set constr_type_e = constraints.constr_type_e %}
|
||||
{%- else %}
|
||||
{%- set constr_type_e = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type %}
|
||||
{%- set cost_type = cost.cost_type %}
|
||||
{%- else %}
|
||||
{%- set cost_type = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type_e %}
|
||||
{%- set cost_type_e = cost.cost_type_e %}
|
||||
{%- else %}
|
||||
{%- set cost_type_e = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type_0 %}
|
||||
{%- set cost_type_0 = cost.cost_type_0 %}
|
||||
{%- else %}
|
||||
{%- set cost_type_0 = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nh %}
|
||||
{%- set dims_nh = dims.nh %}
|
||||
{%- else %}
|
||||
{%- set dims_nh = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nphi %}
|
||||
{%- set dims_nphi = dims.nphi %}
|
||||
{%- else %}
|
||||
{%- set dims_nphi = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nh_e %}
|
||||
{%- set dims_nh_e = dims.nh_e %}
|
||||
{%- else %}
|
||||
{%- set dims_nh_e = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nphi_e %}
|
||||
{%- set dims_nphi_e = dims.nphi_e %}
|
||||
{%- else %}
|
||||
{%- set dims_nphi_e = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if solver_options.model_external_shared_lib_dir %}
|
||||
{%- set model_external_shared_lib_dir = solver_options.model_external_shared_lib_dir %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if solver_options.model_external_shared_lib_name %}
|
||||
{%- set model_external_shared_lib_name = solver_options.model_external_shared_lib_name %}
|
||||
{%- endif %}
|
||||
|
||||
{#- control operator #}
|
||||
{%- if os and os == "pc" %}
|
||||
{%- set control = "&" %}
|
||||
{%- else %}
|
||||
{%- set control = ";" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if acados_link_libs and os and os == "pc" %}{# acados linking libraries and flags #}
|
||||
{%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp -%}
|
||||
{%- set openmp_flag = acados_link_libs.openmp %}
|
||||
{%- else %}
|
||||
{%- set openmp_flag = " " %}
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
{%- set link_libs = "-lqpOASES_e" %}
|
||||
{%- elif qp_solver == "FULL_CONDENSING_DAQP" %}
|
||||
{%- set link_libs = "-ldaqp" %}
|
||||
{%- else %}
|
||||
{%- set link_libs = "" %}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
cmake_minimum_required(VERSION 3.13)
|
||||
|
||||
project({{ model.name }})
|
||||
|
||||
# build options.
|
||||
option(BUILD_ACADOS_SOLVER_LIB "Should the solver library acados_solver_{{ model.name }} be build?" OFF)
|
||||
option(BUILD_ACADOS_OCP_SOLVER_LIB "Should the OCP solver library acados_ocp_solver_{{ model.name }} be build?" OFF)
|
||||
option(BUILD_EXAMPLE "Should the example main_{{ model.name }} be build?" OFF)
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
option(BUILD_SIM_EXAMPLE "Should the simulation example main_sim_{{ model.name }} be build?" OFF)
|
||||
option(BUILD_ACADOS_SIM_SOLVER_LIB "Should the simulation solver library acados_sim_solver_{{ model.name }} be build?" OFF)
|
||||
{%- endif %}
|
||||
|
||||
|
||||
if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" AND CMAKE_SYSTEM_NAME MATCHES "Windows")
|
||||
# MinGW, change to .lib such that mex recognizes it
|
||||
set(CMAKE_SHARED_LIBRARY_SUFFIX ".lib")
|
||||
set(CMAKE_SHARED_LIBRARY_PREFIX "")
|
||||
endif()
|
||||
|
||||
|
||||
# object target names
|
||||
set(MODEL_OBJ model_{{ model.name }})
|
||||
set(OCP_OBJ ocp_{{ model.name }})
|
||||
set(SIM_OBJ sim_{{ model.name }})
|
||||
|
||||
# model
|
||||
set(MODEL_SRC
|
||||
{%- if model.dyn_ext_fun_type == "casadi" %}
|
||||
{%- if solver_options.integrator_type == "ERK" %}
|
||||
{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c
|
||||
{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c
|
||||
{{ model.name }}_model/{{ model.name }}_expl_vde_adj.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c
|
||||
{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c
|
||||
{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c
|
||||
{%- elif solver_options.integrator_type == "DISCRETE" %}
|
||||
{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c
|
||||
{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c
|
||||
{%- endif %}
|
||||
{%- endif -%}
|
||||
{%- else %}
|
||||
{{ model.name }}_model/{{ model.dyn_generic_source }}
|
||||
{%- endif %}
|
||||
)
|
||||
add_library(${MODEL_OBJ} OBJECT ${MODEL_SRC} )
|
||||
|
||||
# optimal control problem - mostly CasADi exports
|
||||
if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMPLE})
|
||||
set(OCP_SRC
|
||||
{%- if constr_type == "BGP" and dims_nphi > 0 %}
|
||||
{{ model.name }}_constraints/{{ model.name }}_phi_constraint.c
|
||||
{%- endif %}
|
||||
{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
|
||||
{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c
|
||||
{%- endif %}
|
||||
|
||||
{%- if constr_type == "BGH" and dims_nh > 0 %}
|
||||
{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c
|
||||
{{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
|
||||
{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
|
||||
{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost_type_0 == "NONLINEAR_LS" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
|
||||
{%- elif cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun_jac_hess.c
|
||||
{%- elif cost_type_0 == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type_0 == "casadi" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
|
||||
{%- else %}
|
||||
{{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost_type == "NONLINEAR_LS" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
|
||||
{%- elif cost_type == "CONVEX_OVER_NONLINEAR" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_conl_cost_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_conl_cost_fun_jac_hess.c
|
||||
{%- elif cost_type == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type == "casadi" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
|
||||
{%- elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
|
||||
{{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost_type_e == "NONLINEAR_LS" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
|
||||
{%- elif cost_type_e == "CONVEX_OVER_NONLINEAR" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun_jac_hess.c
|
||||
{%- elif cost_type_e == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type_e == "casadi" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
|
||||
{%- elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
|
||||
{{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
acados_solver_{{ model.name }}.c)
|
||||
add_library(${OCP_OBJ} OBJECT ${OCP_SRC})
|
||||
endif()
|
||||
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
# for sim solver
|
||||
if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_EXAMPLE}
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
OR ${BUILD_SIM_EXAMPLE} OR ${BUILD_ACADOS_SIM_SOLVER_LIB}
|
||||
{%- endif -%}
|
||||
)
|
||||
set(SIM_SRC acados_sim_solver_{{ model.name }}.c)
|
||||
add_library(${SIM_OBJ} OBJECT ${SIM_SRC})
|
||||
endif()
|
||||
{%- endif %}
|
||||
|
||||
# for target example
|
||||
set(EX_SRC main_{{ model.name }}.c)
|
||||
set(EX_EXE main_{{ model.name }})
|
||||
|
||||
{%- if model_external_shared_lib_dir and model_external_shared_lib_name %}
|
||||
set(EXTERNAL_DIR {{ model_external_shared_lib_dir | replace(from="\", to="/") }})
|
||||
set(EXTERNAL_LIB {{ model_external_shared_lib_name }})
|
||||
{%- else %}
|
||||
set(EXTERNAL_DIR)
|
||||
set(EXTERNAL_LIB)
|
||||
{%- endif %}
|
||||
|
||||
# set some search paths for preprocessor and linker
|
||||
set(ACADOS_INCLUDE_PATH {{ acados_include_path | replace(from="\", to="/") }} CACHE PATH "Define the path which contains the include directory for acados.")
|
||||
set(ACADOS_LIB_PATH {{ acados_lib_path | replace(from="\", to="/") }} CACHE PATH "Define the path which contains the lib directory for acados.")
|
||||
|
||||
# c-compiler flags for debugging
|
||||
set(CMAKE_C_FLAGS_DEBUG "-O0 -ggdb")
|
||||
|
||||
set(CMAKE_C_FLAGS "-fPIC -std=c99 {{ openmp_flag }}
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" -%}
|
||||
-DACADOS_WITH_QPOASES
|
||||
{%- endif -%}
|
||||
{%- if qp_solver == "FULL_CONDENSING_DAQP" -%}
|
||||
-DACADOS_WITH_DAQP
|
||||
{%- endif -%}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" -%}
|
||||
-DACADOS_WITH_OSQP
|
||||
{%- endif -%}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" -%}
|
||||
-DACADOS_WITH_QPDUNES
|
||||
{%- endif -%}
|
||||
")
|
||||
#-fno-diagnostics-show-line-numbers -g
|
||||
|
||||
include_directories(
|
||||
${ACADOS_INCLUDE_PATH}
|
||||
${ACADOS_INCLUDE_PATH}/acados
|
||||
${ACADOS_INCLUDE_PATH}/blasfeo/include
|
||||
${ACADOS_INCLUDE_PATH}/hpipm/include
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
${ACADOS_INCLUDE_PATH}/qpOASES_e/
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "FULL_CONDENSING_DAQP" %}
|
||||
${ACADOS_INCLUDE_PATH}/daqp/include
|
||||
{%- endif %}
|
||||
)
|
||||
|
||||
# linker flags
|
||||
link_directories(${ACADOS_LIB_PATH})
|
||||
|
||||
# link to libraries
|
||||
if(UNIX)
|
||||
link_libraries(acados hpipm blasfeo m {{ link_libs }})
|
||||
else()
|
||||
link_libraries(acados hpipm blasfeo {{ link_libs }})
|
||||
endif()
|
||||
|
||||
# the targets
|
||||
|
||||
# bundled_shared_lib
|
||||
if(${BUILD_ACADOS_SOLVER_LIB})
|
||||
set(LIB_ACADOS_SOLVER acados_solver_{{ model.name }})
|
||||
add_library(${LIB_ACADOS_SOLVER} SHARED $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${OCP_OBJ}>
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
$<TARGET_OBJECTS:${SIM_OBJ}>
|
||||
{%- endif -%}
|
||||
)
|
||||
install(TARGETS ${LIB_ACADOS_SOLVER} DESTINATION ${CMAKE_INSTALL_PREFIX})
|
||||
endif(${BUILD_ACADOS_SOLVER_LIB})
|
||||
|
||||
# ocp_shared_lib
|
||||
if(${BUILD_ACADOS_OCP_SOLVER_LIB})
|
||||
set(LIB_ACADOS_OCP_SOLVER acados_ocp_solver_{{ model.name }})
|
||||
add_library(${LIB_ACADOS_OCP_SOLVER} SHARED $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${OCP_OBJ}>)
|
||||
# Specify libraries or flags to use when linking a given target and/or its dependents.
|
||||
target_link_libraries(${LIB_ACADOS_OCP_SOLVER} PRIVATE ${EXTERNAL_LIB})
|
||||
target_link_directories(${LIB_ACADOS_OCP_SOLVER} PRIVATE ${EXTERNAL_DIR})
|
||||
install(TARGETS ${LIB_ACADOS_OCP_SOLVER} DESTINATION ${CMAKE_INSTALL_PREFIX})
|
||||
endif(${BUILD_ACADOS_OCP_SOLVER_LIB})
|
||||
|
||||
# example
|
||||
if(${BUILD_EXAMPLE})
|
||||
add_executable(${EX_EXE} ${EX_SRC} $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${OCP_OBJ}>
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
$<TARGET_OBJECTS:${SIM_OBJ}>
|
||||
{%- endif -%}
|
||||
)
|
||||
install(TARGETS ${EX_EXE} DESTINATION ${CMAKE_INSTALL_PREFIX})
|
||||
endif(${BUILD_EXAMPLE})
|
||||
|
||||
{% if solver_options.integrator_type != "DISCRETE" -%}
|
||||
# example_sim
|
||||
if(${BUILD_SIM_EXAMPLE})
|
||||
set(EX_SIM_SRC main_sim_{{ model.name }}.c)
|
||||
set(EX_SIM_EXE main_sim_{{ model.name }})
|
||||
add_executable(${EX_SIM_EXE} ${EX_SIM_SRC} $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${SIM_OBJ}>)
|
||||
install(TARGETS ${EX_SIM_EXE} DESTINATION ${CMAKE_INSTALL_PREFIX})
|
||||
endif(${BUILD_SIM_EXAMPLE})
|
||||
|
||||
# sim_shared_lib
|
||||
if(${BUILD_ACADOS_SIM_SOLVER_LIB})
|
||||
set(LIB_ACADOS_SIM_SOLVER acados_sim_solver_{{ model.name }})
|
||||
add_library(${LIB_ACADOS_SIM_SOLVER} SHARED $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${SIM_OBJ}>)
|
||||
install(TARGETS ${LIB_ACADOS_SIM_SOLVER} DESTINATION ${CMAKE_INSTALL_PREFIX})
|
||||
endif(${BUILD_ACADOS_SIM_SOLVER_LIB})
|
||||
{%- endif %}
|
||||
|
||||
468
third_party/acados/acados_template/c_templates_tera/Makefile.in
vendored
Normal file
468
third_party/acados/acados_template/c_templates_tera/Makefile.in
vendored
Normal file
@@ -0,0 +1,468 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
{%- if solver_options.qp_solver %}
|
||||
{%- set qp_solver = solver_options.qp_solver %}
|
||||
{%- else %}
|
||||
{%- set qp_solver = "FULL_CONDENSING_HPIPM" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if solver_options.hessian_approx %}
|
||||
{%- set hessian_approx = solver_options.hessian_approx %}
|
||||
{%- elif solver_options.sens_hess %}
|
||||
{%- set hessian_approx = "EXACT" %}
|
||||
{%- else %}
|
||||
{%- set hessian_approx = "GAUSS_NEWTON" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constraints.constr_type %}
|
||||
{%- set constr_type = constraints.constr_type %}
|
||||
{%- else %}
|
||||
{%- set constr_type = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constraints.constr_type_e %}
|
||||
{%- set constr_type_e = constraints.constr_type_e %}
|
||||
{%- else %}
|
||||
{%- set constr_type_e = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type %}
|
||||
{%- set cost_type = cost.cost_type %}
|
||||
{%- else %}
|
||||
{%- set cost_type = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type_e %}
|
||||
{%- set cost_type_e = cost.cost_type_e %}
|
||||
{%- else %}
|
||||
{%- set cost_type_e = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type_0 %}
|
||||
{%- set cost_type_0 = cost.cost_type_0 %}
|
||||
{%- else %}
|
||||
{%- set cost_type_0 = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nh %}
|
||||
{%- set dims_nh = dims.nh %}
|
||||
{%- else %}
|
||||
{%- set dims_nh = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nphi %}
|
||||
{%- set dims_nphi = dims.nphi %}
|
||||
{%- else %}
|
||||
{%- set dims_nphi = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nh_e %}
|
||||
{%- set dims_nh_e = dims.nh_e %}
|
||||
{%- else %}
|
||||
{%- set dims_nh_e = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nphi_e %}
|
||||
{%- set dims_nphi_e = dims.nphi_e %}
|
||||
{%- else %}
|
||||
{%- set dims_nphi_e = 0 %}
|
||||
{%- endif %}
|
||||
{%- if solver_options.model_external_shared_lib_dir %}
|
||||
{%- set model_external_shared_lib_dir = solver_options.model_external_shared_lib_dir %}
|
||||
{%- endif %}
|
||||
{%- if solver_options.model_external_shared_lib_name %}
|
||||
{%- set model_external_shared_lib_name = solver_options.model_external_shared_lib_name %}
|
||||
{%- endif %}
|
||||
|
||||
{# control operator #}
|
||||
{%- if os and os == "pc" %}
|
||||
{%- set control = "&" %}
|
||||
{%- else %}
|
||||
{%- set control = ";" %}
|
||||
{%- endif %}
|
||||
|
||||
{# acados linking libraries and flags #}
|
||||
{%- if acados_link_libs and os and os == "pc" %}
|
||||
{%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp -%}
|
||||
{%- set openmp_flag = acados_link_libs.openmp %}
|
||||
{%- else %}
|
||||
{%- set openmp_flag = " " %}
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
{%- set link_libs = "-lqpOASES_e" %}
|
||||
{%- elif qp_solver == "FULL_CONDENSING_DAQP" %}
|
||||
{%- set link_libs = "-ldaqp" %}
|
||||
{%- else %}
|
||||
{%- set link_libs = "" %}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
# define sources and use make's implicit rules to generate object files (*.o)
|
||||
|
||||
# model
|
||||
MODEL_SRC=
|
||||
{%- if model.dyn_ext_fun_type == "casadi" %}
|
||||
{%- if solver_options.integrator_type == "ERK" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_adj.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c
|
||||
{%- elif solver_options.integrator_type == "DISCRETE" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- else %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_generic_source }}
|
||||
{%- endif %}
|
||||
MODEL_OBJ := $(MODEL_SRC:.c=.o)
|
||||
|
||||
# optimal control problem - mostly CasADi exports
|
||||
OCP_SRC=
|
||||
{%- if constr_type == "BGP" and dims_nphi > 0 %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.c
|
||||
{%- endif %}
|
||||
{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c
|
||||
{%- endif %}
|
||||
|
||||
{%- if constr_type == "BGH" and dims_nh > 0 %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost_type_0 == "NONLINEAR_LS" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
|
||||
{%- elif cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun_jac_hess.c
|
||||
{%- elif cost_type_0 == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type_0 == "casadi" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
|
||||
{%- else %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost_type == "NONLINEAR_LS" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
|
||||
{%- elif cost_type == "CONVEX_OVER_NONLINEAR" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_fun_jac_hess.c
|
||||
{%- elif cost_type == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type == "casadi" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
|
||||
{%- elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost_type_e == "NONLINEAR_LS" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
|
||||
{%- elif cost_type_e == "CONVEX_OVER_NONLINEAR" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun_jac_hess.c
|
||||
{%- elif cost_type_e == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type_e == "casadi" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
|
||||
{%- elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if solver_options.custom_update_filename %}
|
||||
{%- if solver_options.custom_update_filename != "" %}
|
||||
OCP_SRC+= {{ solver_options.custom_update_filename }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
OCP_SRC+= acados_solver_{{ model.name }}.c
|
||||
OCP_OBJ := $(OCP_SRC:.c=.o)
|
||||
|
||||
# for sim solver
|
||||
SIM_SRC= acados_sim_solver_{{ model.name }}.c
|
||||
SIM_OBJ := $(SIM_SRC:.c=.o)
|
||||
|
||||
# for target example
|
||||
EX_SRC= main_{{ model.name }}.c
|
||||
EX_OBJ := $(EX_SRC:.c=.o)
|
||||
EX_EXE := $(EX_SRC:.c=)
|
||||
|
||||
# for target example_sim
|
||||
EX_SIM_SRC= main_sim_{{ model.name }}.c
|
||||
EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
|
||||
EX_SIM_EXE := $(EX_SIM_SRC:.c=)
|
||||
|
||||
# combine model, sim and ocp object files
|
||||
OBJ=
|
||||
OBJ+= $(MODEL_OBJ)
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
OBJ+= $(SIM_OBJ)
|
||||
{%- endif %}
|
||||
OBJ+= $(OCP_OBJ)
|
||||
|
||||
EXTERNAL_DIR=
|
||||
EXTERNAL_LIB=
|
||||
|
||||
{%- if model_external_shared_lib_dir and model_external_shared_lib_name %}
|
||||
EXTERNAL_DIR+= {{ model_external_shared_lib_dir }}
|
||||
EXTERNAL_LIB+= {{ model_external_shared_lib_name }}
|
||||
{%- endif %}
|
||||
|
||||
INCLUDE_PATH = {{ acados_include_path }}
|
||||
LIB_PATH = {{ acados_lib_path }}
|
||||
|
||||
# preprocessor flags for make's implicit rules
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
CPPFLAGS += -DACADOS_WITH_QPOASES
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "FULL_CONDENSING_DAQP" %}
|
||||
CPPFLAGS += -DACADOS_WITH_DAQP
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
|
||||
CPPFLAGS += -DACADOS_WITH_OSQP
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %}
|
||||
CPPFLAGS += -DACADOS_WITH_QPDUNES
|
||||
{%- endif %}
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/acados
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
CPPFLAGS+= -I $(INCLUDE_PATH)/qpOASES_e/
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "FULL_CONDENSING_DAQP" %}
|
||||
CPPFLAGS+= -I $(INCLUDE_PATH)/daqp/include
|
||||
{%- endif %}
|
||||
|
||||
{# c-compiler flags #}
|
||||
# define the c-compiler flags for make's implicit rules
|
||||
CFLAGS = -fPIC -std=c99 {{ openmp_flag }} {{ solver_options.ext_fun_compile_flags }}#-fno-diagnostics-show-line-numbers -g
|
||||
# # Debugging
|
||||
# CFLAGS += -g3
|
||||
|
||||
# linker flags
|
||||
LDFLAGS+= -L$(LIB_PATH)
|
||||
|
||||
# link to libraries
|
||||
LDLIBS+= -lacados
|
||||
LDLIBS+= -lhpipm
|
||||
LDLIBS+= -lblasfeo
|
||||
LDLIBS+= -lm
|
||||
LDLIBS+= {{ link_libs }}
|
||||
|
||||
# libraries
|
||||
LIBACADOS_SOLVER=libacados_solver_{{ model.name }}{{ shared_lib_ext }}
|
||||
LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }}
|
||||
LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c={{ shared_lib_ext }})
|
||||
|
||||
# virtual targets
|
||||
.PHONY : all clean
|
||||
|
||||
#all: clean example_sim example shared_lib
|
||||
{% if solver_options.integrator_type == "DISCRETE" -%}
|
||||
all: clean example
|
||||
shared_lib: ocp_shared_lib
|
||||
{%- else %}
|
||||
all: clean example_sim example
|
||||
shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
|
||||
{%- endif %}
|
||||
|
||||
# some linker targets
|
||||
example: $(EX_OBJ) $(OBJ)
|
||||
$(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
|
||||
$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
{% if solver_options.integrator_type != "DISCRETE" -%}
|
||||
bundled_shared_lib: $(OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
|
||||
{%- endif %}
|
||||
|
||||
ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \
|
||||
-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB)
|
||||
|
||||
sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
|
||||
# Cython targets
|
||||
ocp_cython_c: ocp_shared_lib
|
||||
cython \
|
||||
-o acados_ocp_solver_pyx.c \
|
||||
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
|
||||
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
|
||||
-I {{ code_export_directory }} \
|
||||
|
||||
ocp_cython_o: ocp_cython_c
|
||||
$(CC) $(ACADOS_FLAGS) -c -O2 \
|
||||
-fPIC \
|
||||
-o acados_ocp_solver_pyx.o \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
{%- for path in cython_include_dirs %}
|
||||
-I {{ path }} \
|
||||
{%- endfor %}
|
||||
acados_ocp_solver_pyx.c \
|
||||
|
||||
ocp_cython: ocp_cython_o
|
||||
$(CC) $(ACADOS_FLAGS) -shared \
|
||||
-o acados_ocp_solver_pyx{{ shared_lib_ext }} \
|
||||
-Wl,-rpath=$(LIB_PATH) \
|
||||
acados_ocp_solver_pyx.o \
|
||||
$(abspath .)/libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} \
|
||||
$(LDFLAGS) $(LDLIBS)
|
||||
|
||||
# Sim Cython targets
|
||||
sim_cython_c: sim_shared_lib
|
||||
cython \
|
||||
-o acados_sim_solver_pyx.c \
|
||||
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
|
||||
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \
|
||||
-I {{ code_export_directory }} \
|
||||
|
||||
sim_cython_o: sim_cython_c
|
||||
$(CC) $(ACADOS_FLAGS) -c -O2 \
|
||||
-fPIC \
|
||||
-o acados_sim_solver_pyx.o \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
{%- for path in cython_include_dirs %}
|
||||
-I {{ path }} \
|
||||
{%- endfor %}
|
||||
acados_sim_solver_pyx.c \
|
||||
|
||||
sim_cython: sim_cython_o
|
||||
$(CC) $(ACADOS_FLAGS) -shared \
|
||||
-o acados_sim_solver_pyx{{ shared_lib_ext }} \
|
||||
-Wl,-rpath=$(LIB_PATH) \
|
||||
acados_sim_solver_pyx.o \
|
||||
$(abspath .)/libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }} \
|
||||
$(LDFLAGS) $(LDLIBS)
|
||||
|
||||
{%- if os and os == "pc" %}
|
||||
|
||||
clean:
|
||||
del \Q *.o 2>nul
|
||||
del \Q *{{ shared_lib_ext }} 2>nul
|
||||
del \Q main_{{ model.name }} 2>nul
|
||||
|
||||
clean_ocp_shared_lib:
|
||||
del \Q libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} 2>nul
|
||||
del \Q acados_solver_{{ model.name }}.o 2>nul
|
||||
|
||||
clean_ocp_cython:
|
||||
del \Q libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} 2>nul
|
||||
del \Q acados_solver_{{ model.name }}.o 2>nul
|
||||
del \Q acados_ocp_solver_pyx{{ shared_lib_ext }} 2>nul
|
||||
del \Q acados_ocp_solver_pyx.o 2>nul
|
||||
|
||||
clean_sim_cython:
|
||||
del \Q libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }} 2>nul
|
||||
del \Q acados_sim_solver_{{ model.name }}.o 2>nul
|
||||
del \Q acados_sim_solver_pyx{{ shared_lib_ext }} 2>nul
|
||||
del \Q acados_sim_solver_pyx.o 2>nul
|
||||
|
||||
{%- else %}
|
||||
|
||||
clean:
|
||||
$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
|
||||
$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
|
||||
$(RM) $(EX_EXE) $(EX_SIM_EXE)
|
||||
|
||||
clean_ocp_shared_lib:
|
||||
$(RM) $(LIBACADOS_OCP_SOLVER)
|
||||
$(RM) $(OCP_OBJ)
|
||||
|
||||
clean_ocp_cython:
|
||||
$(RM) libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }}
|
||||
$(RM) acados_solver_{{ model.name }}.o
|
||||
$(RM) acados_ocp_solver_pyx{{ shared_lib_ext }}
|
||||
$(RM) acados_ocp_solver_pyx.o
|
||||
|
||||
clean_sim_cython:
|
||||
$(RM) libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }}
|
||||
$(RM) acados_sim_solver_{{ model.name }}.o
|
||||
$(RM) acados_sim_solver_pyx{{ shared_lib_ext }}
|
||||
$(RM) acados_sim_solver_pyx.o
|
||||
|
||||
{%- endif %}
|
||||
103
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m
vendored
Normal file
103
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m
vendored
Normal file
@@ -0,0 +1,103 @@
|
||||
%
|
||||
% 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.;
|
||||
|
||||
%
|
||||
|
||||
function make_main_mex_{{ model.name }}()
|
||||
|
||||
opts.output_dir = pwd;
|
||||
|
||||
% get acados folder
|
||||
acados_folder = getenv('ACADOS_INSTALL_DIR');
|
||||
|
||||
% set paths
|
||||
acados_include = ['-I' fullfile(acados_folder, 'include')];
|
||||
template_lib_include = ['-l' 'acados_solver_{{ model.name }}'];
|
||||
template_lib_path = ['-L' fullfile(pwd)];
|
||||
|
||||
acados_lib_path = ['-L' fullfile(acados_folder, 'lib')];
|
||||
external_include = ['-I', fullfile(acados_folder, 'external')];
|
||||
blasfeo_include = ['-I', fullfile(acados_folder, 'external', 'blasfeo', 'include')];
|
||||
hpipm_include = ['-I', fullfile(acados_folder, 'external', 'hpipm', 'include')];
|
||||
|
||||
mex_names = { ...
|
||||
'main_mex_{{ model.name }}' ...
|
||||
};
|
||||
|
||||
mex_files = cell(length(mex_names), 1);
|
||||
for k=1:length(mex_names)
|
||||
mex_files{k} = fullfile([mex_names{k}, '.c']);
|
||||
end
|
||||
|
||||
%% octave C flags
|
||||
if is_octave()
|
||||
if ~exist(fullfile(opts.output_dir, 'cflags_octave.txt'), 'file')
|
||||
diary(fullfile(opts.output_dir, 'cflags_octave.txt'));
|
||||
diary on
|
||||
mkoctfile -p CFLAGS
|
||||
diary off
|
||||
input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r');
|
||||
cflags_tmp = fscanf(input_file, '%[^\n]s');
|
||||
fclose(input_file);
|
||||
if ~ismac()
|
||||
cflags_tmp = [cflags_tmp, ' -std=c99 -fopenmp'];
|
||||
else
|
||||
cflags_tmp = [cflags_tmp, ' -std=c99'];
|
||||
end
|
||||
input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'w');
|
||||
fprintf(input_file, '%s', cflags_tmp);
|
||||
fclose(input_file);
|
||||
end
|
||||
% read cflags from file
|
||||
input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r');
|
||||
cflags_tmp = fscanf(input_file, '%[^\n]s');
|
||||
fclose(input_file);
|
||||
setenv('CFLAGS', cflags_tmp);
|
||||
end
|
||||
|
||||
%% compile mex
|
||||
for ii=1:length(mex_files)
|
||||
disp(['compiling ', mex_files{ii}])
|
||||
if is_octave()
|
||||
% mkoctfile -p CFLAGS
|
||||
mex(acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
|
||||
acados_lib_path, template_lib_path, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii})
|
||||
else
|
||||
if ismac()
|
||||
FLAGS = 'CFLAGS=$CFLAGS -std=c99';
|
||||
else
|
||||
FLAGS = 'CFLAGS=$CFLAGS -std=c99 -fopenmp';
|
||||
end
|
||||
mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
|
||||
acados_lib_path, template_lib_path, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii})
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
127
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m
vendored
Normal file
127
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m
vendored
Normal file
@@ -0,0 +1,127 @@
|
||||
%
|
||||
% 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.;
|
||||
|
||||
%
|
||||
|
||||
function make_mex_{{ model.name }}()
|
||||
|
||||
opts.output_dir = pwd;
|
||||
|
||||
% get acados folder
|
||||
acados_folder = getenv('ACADOS_INSTALL_DIR');
|
||||
|
||||
% set paths
|
||||
acados_include = ['-I' fullfile(acados_folder, 'include')];
|
||||
template_lib_include = ['-l' 'acados_ocp_solver_{{ model.name }}'];
|
||||
template_lib_path = ['-L' fullfile(pwd)];
|
||||
|
||||
acados_lib_path = ['-L' fullfile(acados_folder, 'lib')];
|
||||
external_include = ['-I', fullfile(acados_folder, 'external')];
|
||||
blasfeo_include = ['-I', fullfile(acados_folder, 'external', 'blasfeo', 'include')];
|
||||
hpipm_include = ['-I', fullfile(acados_folder, 'external', 'hpipm', 'include')];
|
||||
|
||||
% load linking information of compiled acados
|
||||
link_libs_core_filename = fullfile(acados_folder, 'lib', 'link_libs.json');
|
||||
addpath(fullfile(acados_folder, 'external', 'jsonlab'));
|
||||
link_libs = loadjson(link_libs_core_filename);
|
||||
|
||||
% add necessary link instructs
|
||||
acados_lib_extra = {};
|
||||
lib_names = fieldnames(link_libs);
|
||||
for idx = 1 : numel(lib_names)
|
||||
lib_name = lib_names{idx};
|
||||
link_arg = link_libs.(lib_name);
|
||||
if ~isempty(link_arg)
|
||||
acados_lib_extra = [acados_lib_extra, link_arg];
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
mex_include = ['-I', fullfile(acados_folder, 'interfaces', 'acados_matlab_octave')];
|
||||
|
||||
mex_names = { ...
|
||||
'acados_mex_create_{{ model.name }}' ...
|
||||
'acados_mex_free_{{ model.name }}' ...
|
||||
'acados_mex_solve_{{ model.name }}' ...
|
||||
'acados_mex_set_{{ model.name }}' ...
|
||||
};
|
||||
|
||||
mex_files = cell(length(mex_names), 1);
|
||||
for k=1:length(mex_names)
|
||||
mex_files{k} = fullfile([mex_names{k}, '.c']);
|
||||
end
|
||||
|
||||
%% octave C flags
|
||||
if is_octave()
|
||||
if ~exist(fullfile(opts.output_dir, 'cflags_octave.txt'), 'file')
|
||||
diary(fullfile(opts.output_dir, 'cflags_octave.txt'));
|
||||
diary on
|
||||
mkoctfile -p CFLAGS
|
||||
diary off
|
||||
input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r');
|
||||
cflags_tmp = fscanf(input_file, '%[^\n]s');
|
||||
fclose(input_file);
|
||||
if ~ismac()
|
||||
cflags_tmp = [cflags_tmp, ' -std=c99 -fopenmp'];
|
||||
else
|
||||
cflags_tmp = [cflags_tmp, ' -std=c99'];
|
||||
end
|
||||
input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'w');
|
||||
fprintf(input_file, '%s', cflags_tmp);
|
||||
fclose(input_file);
|
||||
end
|
||||
% read cflags from file
|
||||
input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r');
|
||||
cflags_tmp = fscanf(input_file, '%[^\n]s');
|
||||
fclose(input_file);
|
||||
setenv('CFLAGS', cflags_tmp);
|
||||
end
|
||||
|
||||
%% compile mex
|
||||
for ii=1:length(mex_files)
|
||||
disp(['compiling ', mex_files{ii}])
|
||||
if is_octave()
|
||||
% mkoctfile -p CFLAGS
|
||||
mex(acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
|
||||
template_lib_path, mex_include, acados_lib_path, '-lacados', '-lhpipm', '-lblasfeo',...
|
||||
acados_lib_extra{:}, mex_files{ii})
|
||||
else
|
||||
if ismac()
|
||||
FLAGS = 'CFLAGS=$CFLAGS -std=c99';
|
||||
else
|
||||
FLAGS = 'CFLAGS=$CFLAGS -std=c99 -fopenmp';
|
||||
end
|
||||
mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
|
||||
template_lib_path, mex_include, acados_lib_path, '-lacados', '-lhpipm', '-lblasfeo',...
|
||||
acados_lib_extra{:}, mex_files{ii})
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
432
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m
vendored
Normal file
432
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m
vendored
Normal file
@@ -0,0 +1,432 @@
|
||||
%
|
||||
% 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.;
|
||||
|
||||
%
|
||||
|
||||
SOURCES = { ...
|
||||
{%- if solver_options.integrator_type == 'ERK' %}
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c', ...
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c',...
|
||||
{%- if solver_options.hessian_approx == 'EXACT' %}
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c',...
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c', ...
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c', ...
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c', ...
|
||||
{%- if solver_options.hessian_approx == 'EXACT' %}
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c',...
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c',...
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c',...
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c',...
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c',...
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c',...
|
||||
{%- elif solver_options.integrator_type == "DISCRETE" %}
|
||||
'{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c',...
|
||||
'{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c',...
|
||||
{%- if solver_options.hessian_approx == "EXACT" %}
|
||||
'{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c',...
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost.cost_type_0 == "NONLINEAR_LS" %}
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c',...
|
||||
{%- elif cost.cost_type_0 == "EXTERNAL" %}
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c',...
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type == "NONLINEAR_LS" %}
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_fun.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_hess.c',...
|
||||
{%- elif cost.cost_type == "EXTERNAL" %}
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c',...
|
||||
{%- endif %}
|
||||
{%- if cost.cost_type_e == "NONLINEAR_LS" %}
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c',...
|
||||
{%- elif cost.cost_type_e == "EXTERNAL" %}
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c',...
|
||||
{%- endif %}
|
||||
{%- if constraints.constr_type == "BGH" and dims.nh > 0 %}
|
||||
'{{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c', ...
|
||||
'{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c', ...
|
||||
'{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c', ...
|
||||
{%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %}
|
||||
'{{ model.name }}_constraints/{{ model.name }}_phi_constraint.c', ...
|
||||
{%- endif %}
|
||||
{%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
|
||||
'{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c', ...
|
||||
'{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c', ...
|
||||
'{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c', ...
|
||||
{%- elif constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %}
|
||||
'{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c', ...
|
||||
{%- endif %}
|
||||
'acados_solver_sfunction_{{ model.name }}.c', ...
|
||||
'acados_solver_{{ model.name }}.c'
|
||||
};
|
||||
|
||||
INC_PATH = '{{ acados_include_path }}';
|
||||
|
||||
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
|
||||
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
|
||||
['-I', fullfile(INC_PATH, 'acados')], ...
|
||||
['-I', fullfile(INC_PATH)]};
|
||||
|
||||
{% if solver_options.qp_solver is containing("QPOASES") %}
|
||||
INCS{end+1} = ['-I', fullfile(INC_PATH, 'qpOASES_e')];
|
||||
{% endif %}
|
||||
|
||||
CFLAGS = 'CFLAGS=$CFLAGS';
|
||||
LDFLAGS = 'LDFLAGS=$LDFLAGS';
|
||||
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
|
||||
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
|
||||
|
||||
{% if solver_options.qp_solver is containing("QPOASES") %}
|
||||
CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPOASES ' ];
|
||||
COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPOASES ' ];
|
||||
{%- elif solver_options.qp_solver is containing("OSQP") %}
|
||||
CFLAGS = [ CFLAGS, ' -DACADOS_WITH_OSQP ' ];
|
||||
COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_OSQP ' ];
|
||||
{%- elif solver_options.qp_solver is containing("QPDUNES") %}
|
||||
CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPDUNES ' ];
|
||||
COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPDUNES ' ];
|
||||
{%- elif solver_options.qp_solver is containing("DAQP") %}
|
||||
CFLAGS = [ CFLAGS, ' -DACADOS_WITH_DAQP' ];
|
||||
COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_DAQP' ];
|
||||
{%- elif solver_options.qp_solver is containing("HPMPC") %}
|
||||
CFLAGS = [ CFLAGS, ' -DACADOS_WITH_HPMPC ' ];
|
||||
COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_HPMPC ' ];
|
||||
{% endif %}
|
||||
|
||||
LIB_PATH = ['-L', fullfile('{{ acados_lib_path }}')];
|
||||
|
||||
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
|
||||
|
||||
% acados linking libraries and flags
|
||||
{%- if acados_link_libs and os and os == "pc" %}
|
||||
LDFLAGS = [LDFLAGS ' {{ acados_link_libs.openmp }}'];
|
||||
COMPFLAGS = [COMPFLAGS ' {{ acados_link_libs.openmp }}'];
|
||||
LIBS{end+1} = '{{ acados_link_libs.qpoases }}';
|
||||
LIBS{end+1} = '{{ acados_link_libs.hpmpc }}';
|
||||
LIBS{end+1} = '{{ acados_link_libs.osqp }}';
|
||||
{%- else %}
|
||||
{% if solver_options.qp_solver is containing("QPOASES") %}
|
||||
LIBS{end+1} = '-lqpOASES_e';
|
||||
{% endif %}
|
||||
{% if solver_options.qp_solver is containing("DAQP") %}
|
||||
LIBS{end+1} = '-ldaqp';
|
||||
{% endif %}
|
||||
{%- endif %}
|
||||
|
||||
|
||||
try
|
||||
% mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
|
||||
mex('-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
|
||||
LIB_PATH, LIBS{:}, SOURCES{:}, ...
|
||||
'-output', 'acados_solver_sfunction_{{ model.name }}' );
|
||||
catch exception
|
||||
disp('make_sfun failed with the following exception:')
|
||||
disp(exception);
|
||||
disp('Try adding -v to the mex command above to get more information.')
|
||||
keyboard
|
||||
end
|
||||
|
||||
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_{{ model.name }}', '.', ...
|
||||
eval('mexext')] );
|
||||
|
||||
|
||||
%% print note on usage of s-function, and create I/O port names vectors
|
||||
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
|
||||
input_note = 'Inputs are:\n';
|
||||
i_in = 1;
|
||||
|
||||
global sfun_input_names
|
||||
sfun_input_names = {};
|
||||
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
|
||||
' size [{{ dims.nbx_0 }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lbx_0 [{{ dims.nbx_0 }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
|
||||
' size [{{ dims.nbx_0 }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'ubx_0 [{{ dims.nbx_0 }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N,',...
|
||||
' size [{{ (dims.N+1)*dims.np }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'parameter_traj [{{ (dims.N+1)*dims.np }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %}
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [{{ dims.ny_0 }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'y_ref_0 [{{ dims.ny_0 }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %}
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
|
||||
' size [{{ (dims.N-1) * dims.ny }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'y_ref [{{ (dims.N-1) * dims.ny }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %}
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [{{ dims.ny_e }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'y_ref_e [{{ dims.ny_e }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lbx [{{ (dims.N-1) * dims.nbx }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'ubx [{{ (dims.N-1) * dims.nbx }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lbx_e (lbx at shooting node N), size [{{ dims.nbx_e }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lbx_e [{{ dims.nbx_e }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx_e (ubx at shooting node N), size [{{ dims.nbx_e }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'ubx_e [{{ dims.nbx_e }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lbu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lbu [{{ dims.N*dims.nbu }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif -%}
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'ubu [{{ dims.N*dims.nbu }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif -%}
|
||||
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lg for shooting nodes 0 to N-1, size [{{ dims.N*dims.ng }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lg [{{ dims.N*dims.ng }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') ug for shooting nodes 0 to N-1, size [{{ dims.N*dims.ng }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'ug [{{ dims.N*dims.ng }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lh for shooting nodes 0 to N-1, size [{{ dims.N*dims.nh }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lh [{{ dims.N*dims.nh }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') uh for shooting nodes 0 to N-1, size [{{ dims.N*dims.nh }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'uh [{{ dims.N*dims.nh }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lh_e, size [{{ dims.nh_e }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lh_e [{{ dims.nh_e }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
{%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') uh_e, size [{{ dims.nh_e }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'uh_e [{{ dims.nh_e }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') cost_W_0 in column-major format, size [{{ dims.ny_0 * dims.ny_0 }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'cost_W_0 [{{ dims.ny_0 * dims.ny_0 }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') cost_W in column-major format, that is set for all intermediate shooting nodes: 1 to N-1, size [{{ dims.ny * dims.ny }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'cost_W [{{ dims.ny * dims.ny }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') cost_W_e in column-major format, size [{{ dims.ny_e * dims.ny_e }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'cost_W_e [{{ dims.ny_e * dims.ny_e }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.reset_solver %} {#- reset_solver #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') reset_solver determines if iterate is set to all zeros before other initializations (x_init, u_init) are set and before solver is called, size [1]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'reset_solver [1]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.x_init %} {#- x_init #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') initialization of x for all shooting nodes, size [{{ dims.nx * (dims.N+1) }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'x_init [{{ dims.nx * (dims.N+1) }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.u_init %} {#- u_init #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') initialization of u for shooting nodes 0 to N-1, size [{{ dims.nu * (dims.N) }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'u_init [{{ dims.nu * (dims.N) }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
fprintf(input_note)
|
||||
|
||||
disp(' ')
|
||||
|
||||
output_note = 'Outputs are:\n';
|
||||
i_out = 0;
|
||||
|
||||
global sfun_output_names
|
||||
sfun_output_names = {};
|
||||
|
||||
{%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [{{ dims.nu }}]\n ');
|
||||
sfun_output_names = [sfun_output_names; 'u0 [{{ dims.nu }}]'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.utraj == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') utraj, control input concatenated for nodes 0 to N-1, size [{{ dims.nu * dims.N }}]\n ');
|
||||
sfun_output_names = [sfun_output_names; 'utraj [{{ dims.nu * dims.N }}]'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.xtraj == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') xtraj, state concatenated for nodes 0 to N, size [{{ dims.nx * (dims.N + 1) }}]\n ');
|
||||
sfun_output_names = [sfun_output_names; 'xtraj [{{ dims.nx * (dims.N + 1) }}]'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.solver_status == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
|
||||
sfun_output_names = [sfun_output_names; 'solver_status'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.cost_value == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') cost function value\n ');
|
||||
sfun_output_names = [sfun_output_names; 'cost_value'];
|
||||
{%- endif %}
|
||||
|
||||
|
||||
{%- if simulink_opts.outputs.KKT_residual == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
|
||||
sfun_output_names = [sfun_output_names; 'KKT_residual'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.KKT_residuals == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') KKT residuals, size [4] (stat, eq, ineq, comp)\n ');
|
||||
sfun_output_names = [sfun_output_names; 'KKT_residuals [4]'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
|
||||
sfun_output_names = [sfun_output_names; 'x1'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
|
||||
sfun_output_names = [sfun_output_names; 'CPU_time'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_sim == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') CPU time integrator\n ');
|
||||
sfun_output_names = [sfun_output_names; 'CPU_time_sim'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_qp == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') CPU time QP solution\n ');
|
||||
sfun_output_names = [sfun_output_names; 'CPU_time_qp'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_lin == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') CPU time linearization (including integrator)\n ');
|
||||
sfun_output_names = [sfun_output_names; 'CPU_time_lin'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.sqp_iter == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
|
||||
sfun_output_names = [sfun_output_names; 'sqp_iter'];
|
||||
{%- endif %}
|
||||
|
||||
fprintf(output_note)
|
||||
|
||||
% The mask drawing command is:
|
||||
% ---
|
||||
% global sfun_input_names sfun_output_names
|
||||
% for i = 1:length(sfun_input_names)
|
||||
% port_label('input', i, sfun_input_names{i})
|
||||
% end
|
||||
% for i = 1:length(sfun_output_names)
|
||||
% port_label('output', i, sfun_output_names{i})
|
||||
% end
|
||||
% ---
|
||||
% It can be used by copying it in sfunction/Mask/Edit mask/Icon drawing commands
|
||||
% (you can access it wirth ctrl+M on the s-function)
|
||||
137
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m
vendored
Normal file
137
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m
vendored
Normal file
@@ -0,0 +1,137 @@
|
||||
%
|
||||
% 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.;
|
||||
|
||||
%
|
||||
|
||||
SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ...
|
||||
'acados_sim_solver_{{ model.name }}.c ', ...
|
||||
{%- if solver_options.integrator_type == 'ERK' %}
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c ',...
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c ',...
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_vde_adj.c ',...
|
||||
{%- if solver_options.hessian_approx == 'EXACT' %}
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c ',...
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c ', ...
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c ', ...
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c ', ...
|
||||
{%- if solver_options.hessian_approx == 'EXACT' %}
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c ',...
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{%- if model.gnsf.purely_linear != 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c ',...
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c ',...
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c ',...
|
||||
{%- if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c ',...
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c ',...
|
||||
{%- endif %}
|
||||
];
|
||||
|
||||
INC_PATH = '{{ acados_include_path }}';
|
||||
|
||||
INCS = [ ' -I', fullfile(INC_PATH, 'blasfeo', 'include'), ...
|
||||
' -I', fullfile(INC_PATH, 'hpipm', 'include'), ...
|
||||
' -I', INC_PATH, ' -I', fullfile(INC_PATH, 'acados'), ' '];
|
||||
|
||||
CFLAGS = ' -O';
|
||||
|
||||
LIB_PATH = '{{ acados_lib_path }}';
|
||||
|
||||
LIBS = '-lacados -lblasfeo -lhpipm';
|
||||
|
||||
try
|
||||
% eval( [ 'mex -v -output acados_sim_solver_sfunction_{{ model.name }} ', ...
|
||||
eval( [ 'mex -output acados_sim_solver_sfunction_{{ model.name }} ', ...
|
||||
CFLAGS, INCS, ' ', SOURCES, ' -L', LIB_PATH, ' ', LIBS ]);
|
||||
|
||||
catch exception
|
||||
disp('make_sfun failed with the following exception:')
|
||||
disp(exception);
|
||||
disp('Try adding -v to the mex command above to get more information.')
|
||||
keyboard
|
||||
end
|
||||
|
||||
|
||||
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_sim_solver_sfunction_{{ model.name }}', '.', ...
|
||||
eval('mexext')] );
|
||||
|
||||
|
||||
global sfun_sim_input_names
|
||||
sfun_sim_input_names = {};
|
||||
|
||||
%% print note on usage of s-function
|
||||
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
|
||||
input_note = 'Inputs are:\n1) x0, initial state, size [{{ dims.nx }}]\n ';
|
||||
i_in = 2;
|
||||
sfun_sim_input_names = [sfun_sim_input_names; 'x0 [{{ dims.nx }}]'];
|
||||
|
||||
{%- if dims.nu > 0 %}
|
||||
input_note = strcat(input_note, num2str(i_in), ') u, size [{{ dims.nu }}]\n ');
|
||||
i_in = i_in + 1;
|
||||
sfun_sim_input_names = [sfun_sim_input_names; 'u [{{ dims.nu }}]'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.np > 0 %}
|
||||
input_note = strcat(input_note, num2str(i_in), ') parameters, size [{{ dims.np }}]\n ');
|
||||
i_in = i_in + 1;
|
||||
sfun_sim_input_names = [sfun_sim_input_names; 'p [{{ dims.np }}]'];
|
||||
{%- endif %}
|
||||
|
||||
|
||||
fprintf(input_note)
|
||||
|
||||
disp(' ')
|
||||
|
||||
global sfun_sim_output_names
|
||||
sfun_sim_output_names = {};
|
||||
|
||||
output_note = strcat('Outputs are:\n', ...
|
||||
'1) x1 - simulated state, size [{{ dims.nx }}]\n');
|
||||
sfun_sim_output_names = [sfun_sim_output_names; 'x1 [{{ dims.nx }}]'];
|
||||
|
||||
fprintf(output_note)
|
||||
|
||||
|
||||
% The mask drawing command is:
|
||||
% ---
|
||||
% global sfun_sim_input_names sfun_sim_output_names
|
||||
% for i = 1:length(sfun_sim_input_names)
|
||||
% port_label('input', i, sfun_sim_input_names{i})
|
||||
% end
|
||||
% for i = 1:length(sfun_sim_output_names)
|
||||
% port_label('output', i, sfun_sim_output_names{i})
|
||||
% end
|
||||
% ---
|
||||
% It can be used by copying it in sfunction/Mask/Edit mask/Icon drawing commands
|
||||
% (you can access it wirth ctrl+M on the s-function)
|
||||
270
third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m
vendored
Normal file
270
third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m
vendored
Normal file
@@ -0,0 +1,270 @@
|
||||
%
|
||||
% 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.;
|
||||
|
||||
%
|
||||
|
||||
classdef {{ model.name }}_mex_solver < handle
|
||||
|
||||
properties
|
||||
C_ocp
|
||||
C_ocp_ext_fun
|
||||
cost_ext_fun_type
|
||||
cost_ext_fun_type_e
|
||||
N
|
||||
name
|
||||
code_gen_dir
|
||||
end % properties
|
||||
|
||||
|
||||
|
||||
methods
|
||||
|
||||
% constructor
|
||||
function obj = {{ model.name }}_mex_solver()
|
||||
make_mex_{{ model.name }}();
|
||||
[obj.C_ocp, obj.C_ocp_ext_fun] = acados_mex_create_{{ model.name }}();
|
||||
% to have path to destructor when changing directory
|
||||
addpath('.')
|
||||
obj.cost_ext_fun_type = '{{ cost.cost_ext_fun_type }}';
|
||||
obj.cost_ext_fun_type_e = '{{ cost.cost_ext_fun_type_e }}';
|
||||
obj.N = {{ dims.N }};
|
||||
obj.name = '{{ model.name }}';
|
||||
obj.code_gen_dir = pwd();
|
||||
end
|
||||
|
||||
% destructor
|
||||
function delete(obj)
|
||||
disp("delete template...");
|
||||
return_dir = pwd();
|
||||
cd(obj.code_gen_dir);
|
||||
if ~isempty(obj.C_ocp)
|
||||
acados_mex_free_{{ model.name }}(obj.C_ocp);
|
||||
end
|
||||
cd(return_dir);
|
||||
disp("done.");
|
||||
end
|
||||
|
||||
% solve
|
||||
function solve(obj)
|
||||
acados_mex_solve_{{ model.name }}(obj.C_ocp);
|
||||
end
|
||||
|
||||
% set -- borrowed from MEX interface
|
||||
function set(varargin)
|
||||
obj = varargin{1};
|
||||
field = varargin{2};
|
||||
value = varargin{3};
|
||||
if ~isa(field, 'char')
|
||||
error('field must be a char vector, use '' ''');
|
||||
end
|
||||
if nargin==3
|
||||
acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value);
|
||||
elseif nargin==4
|
||||
stage = varargin{4};
|
||||
acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value, stage);
|
||||
else
|
||||
disp('acados_ocp.set: wrong number of input arguments (2 or 3 allowed)');
|
||||
end
|
||||
end
|
||||
|
||||
function value = get_cost(obj)
|
||||
value = ocp_get_cost(obj.C_ocp);
|
||||
end
|
||||
|
||||
% get -- borrowed from MEX interface
|
||||
function value = get(varargin)
|
||||
% usage:
|
||||
% obj.get(field, value, [stage])
|
||||
obj = varargin{1};
|
||||
field = varargin{2};
|
||||
if any(strfind('sens', field))
|
||||
error('field sens* (sensitivities of optimal solution) not yet supported for templated MEX.')
|
||||
end
|
||||
if ~isa(field, 'char')
|
||||
error('field must be a char vector, use '' ''');
|
||||
end
|
||||
|
||||
if nargin==2
|
||||
value = ocp_get(obj.C_ocp, field);
|
||||
elseif nargin==3
|
||||
stage = varargin{3};
|
||||
value = ocp_get(obj.C_ocp, field, stage);
|
||||
else
|
||||
disp('acados_ocp.get: wrong number of input arguments (1 or 2 allowed)');
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
function [] = store_iterate(varargin)
|
||||
%%% Stores the current iterate of the ocp solver in a json file.
|
||||
%%% param1: filename: if not set, use model_name + timestamp + '.json'
|
||||
%%% param2: overwrite: if false and filename exists add timestamp to filename
|
||||
|
||||
obj = varargin{1};
|
||||
filename = '';
|
||||
overwrite = false;
|
||||
|
||||
if nargin>=2
|
||||
filename = varargin{2};
|
||||
if ~isa(filename, 'char')
|
||||
error('filename must be a char vector, use '' ''');
|
||||
end
|
||||
end
|
||||
|
||||
if nargin==3
|
||||
overwrite = varargin{3};
|
||||
end
|
||||
|
||||
if nargin > 3
|
||||
disp('acados_ocp.get: wrong number of input arguments (1 or 2 allowed)');
|
||||
end
|
||||
|
||||
if strcmp(filename,'')
|
||||
filename = [obj.name '_iterate.json'];
|
||||
end
|
||||
if ~overwrite
|
||||
% append timestamp
|
||||
if exist(filename, 'file')
|
||||
filename = filename(1:end-5);
|
||||
filename = [filename '_' datestr(now,'yyyy-mm-dd-HH:MM:SS') '.json'];
|
||||
end
|
||||
end
|
||||
filename = fullfile(pwd, filename);
|
||||
|
||||
% get iterate:
|
||||
solution = struct();
|
||||
for i=0:obj.N
|
||||
solution.(['x_' num2str(i)]) = obj.get('x', i);
|
||||
solution.(['lam_' num2str(i)]) = obj.get('lam', i);
|
||||
solution.(['t_' num2str(i)]) = obj.get('t', i);
|
||||
solution.(['sl_' num2str(i)]) = obj.get('sl', i);
|
||||
solution.(['su_' num2str(i)]) = obj.get('su', i);
|
||||
end
|
||||
for i=0:obj.N-1
|
||||
solution.(['z_' num2str(i)]) = obj.get('z', i);
|
||||
solution.(['u_' num2str(i)]) = obj.get('u', i);
|
||||
solution.(['pi_' num2str(i)]) = obj.get('pi', i);
|
||||
end
|
||||
|
||||
acados_folder = getenv('ACADOS_INSTALL_DIR');
|
||||
addpath(fullfile(acados_folder, 'external', 'jsonlab'));
|
||||
savejson('', solution, filename);
|
||||
|
||||
json_string = savejson('', solution, 'ForceRootName', 0);
|
||||
|
||||
fid = fopen(filename, 'w');
|
||||
if fid == -1, error('store_iterate: Cannot create JSON file'); end
|
||||
fwrite(fid, json_string, 'char');
|
||||
fclose(fid);
|
||||
|
||||
disp(['stored current iterate in ' filename]);
|
||||
end
|
||||
|
||||
|
||||
function [] = load_iterate(obj, filename)
|
||||
%%% Loads the iterate stored in json file with filename into the ocp solver.
|
||||
acados_folder = getenv('ACADOS_INSTALL_DIR');
|
||||
addpath(fullfile(acados_folder, 'external', 'jsonlab'));
|
||||
filename = fullfile(pwd, filename);
|
||||
|
||||
if ~exist(filename, 'file')
|
||||
error(['load_iterate: failed, file does not exist: ' filename])
|
||||
end
|
||||
|
||||
solution = loadjson(filename);
|
||||
keys = fieldnames(solution);
|
||||
|
||||
for k = 1:numel(keys)
|
||||
key = keys{k};
|
||||
key_parts = strsplit(key, '_');
|
||||
field = key_parts{1};
|
||||
stage = key_parts{2};
|
||||
|
||||
val = solution.(key);
|
||||
|
||||
% check if array is empty (can happen for z)
|
||||
if numel(val) > 0
|
||||
obj.set(field, val, str2num(stage))
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
% print
|
||||
function print(varargin)
|
||||
if nargin < 2
|
||||
field = 'stat';
|
||||
else
|
||||
field = varargin{2};
|
||||
end
|
||||
|
||||
obj = varargin{1};
|
||||
|
||||
if strcmp(field, 'stat')
|
||||
stat = obj.get('stat');
|
||||
{%- if solver_options.nlp_solver_type == "SQP" %}
|
||||
fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha');
|
||||
if size(stat,2)>8
|
||||
fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp');
|
||||
end
|
||||
fprintf('\n');
|
||||
for jj=1:size(stat,1)
|
||||
fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d\t%e', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7), stat(jj, 8));
|
||||
if size(stat,2)>8
|
||||
fprintf('\t%e\t%e\t%e\t%e', stat(jj,9), stat(jj,10), stat(jj,11), stat(jj,12));
|
||||
end
|
||||
fprintf('\n');
|
||||
end
|
||||
fprintf('\n');
|
||||
{%- else %}
|
||||
fprintf('\niter\tqp_status\tqp_iter');
|
||||
if size(stat,2)>3
|
||||
fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp');
|
||||
end
|
||||
fprintf('\n');
|
||||
for jj=1:size(stat,1)
|
||||
fprintf('%d\t%d\t\t%d', stat(jj,1), stat(jj,2), stat(jj,3));
|
||||
if size(stat,2)>3
|
||||
fprintf('\t%e\t%e\t%e\t%e', stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7));
|
||||
end
|
||||
fprintf('\n');
|
||||
end
|
||||
{% endif %}
|
||||
|
||||
else
|
||||
fprintf('unsupported field in function print of acados_ocp.print, got %s', field);
|
||||
keyboard
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
end % methods
|
||||
|
||||
end % class
|
||||
|
||||
708
third_party/acados/acados_template/casadi_function_generation.py
vendored
Normal file
708
third_party/acados/acados_template/casadi_function_generation.py
vendored
Normal file
@@ -0,0 +1,708 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
import os
|
||||
import casadi as ca
|
||||
from .utils import is_empty, casadi_length
|
||||
|
||||
|
||||
def get_casadi_symbol(x):
|
||||
if isinstance(x, ca.MX):
|
||||
return ca.MX.sym
|
||||
elif isinstance(x, ca.SX):
|
||||
return ca.SX.sym
|
||||
else:
|
||||
raise TypeError("Expected casadi SX or MX.")
|
||||
|
||||
################
|
||||
# Dynamics
|
||||
################
|
||||
|
||||
|
||||
def generate_c_code_discrete_dynamics( model, opts ):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
# load model
|
||||
x = model.x
|
||||
u = model.u
|
||||
p = model.p
|
||||
phi = model.disc_dyn_expr
|
||||
model_name = model.name
|
||||
nx = casadi_length(x)
|
||||
|
||||
symbol = get_casadi_symbol(x)
|
||||
# assume nx1 = nx !!!
|
||||
lam = symbol('lam', nx, 1)
|
||||
|
||||
# generate jacobians
|
||||
ux = ca.vertcat(u,x)
|
||||
jac_ux = ca.jacobian(phi, ux)
|
||||
# generate adjoint
|
||||
adj_ux = ca.jtimes(phi, ux, lam, True)
|
||||
# generate hessian
|
||||
hess_ux = ca.jacobian(adj_ux, ux)
|
||||
|
||||
# change directory
|
||||
cwd = os.getcwd()
|
||||
model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model'))
|
||||
if not os.path.exists(model_dir):
|
||||
os.makedirs(model_dir)
|
||||
os.chdir(model_dir)
|
||||
|
||||
# set up & generate ca.Functions
|
||||
fun_name = model_name + '_dyn_disc_phi_fun'
|
||||
phi_fun = ca.Function(fun_name, [x, u, p], [phi])
|
||||
phi_fun.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_dyn_disc_phi_fun_jac'
|
||||
phi_fun_jac_ut_xt = ca.Function(fun_name, [x, u, p], [phi, jac_ux.T])
|
||||
phi_fun_jac_ut_xt.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_dyn_disc_phi_fun_jac_hess'
|
||||
phi_fun_jac_ut_xt_hess = ca.Function(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux])
|
||||
phi_fun_jac_ut_xt_hess.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
os.chdir(cwd)
|
||||
return
|
||||
|
||||
|
||||
|
||||
def generate_c_code_explicit_ode( model, opts ):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
generate_hess = opts["generate_hess"]
|
||||
|
||||
# load model
|
||||
x = model.x
|
||||
u = model.u
|
||||
p = model.p
|
||||
f_expl = model.f_expl_expr
|
||||
model_name = model.name
|
||||
|
||||
## get model dimensions
|
||||
nx = x.size()[0]
|
||||
nu = u.size()[0]
|
||||
|
||||
symbol = get_casadi_symbol(x)
|
||||
|
||||
## set up functions to be exported
|
||||
Sx = symbol('Sx', nx, nx)
|
||||
Sp = symbol('Sp', nx, nu)
|
||||
lambdaX = symbol('lambdaX', nx, 1)
|
||||
|
||||
fun_name = model_name + '_expl_ode_fun'
|
||||
|
||||
## Set up functions
|
||||
expl_ode_fun = ca.Function(fun_name, [x, u, p], [f_expl])
|
||||
|
||||
vdeX = ca.jtimes(f_expl,x,Sx)
|
||||
vdeP = ca.jacobian(f_expl,u) + ca.jtimes(f_expl,x,Sp)
|
||||
|
||||
fun_name = model_name + '_expl_vde_forw'
|
||||
|
||||
expl_vde_forw = ca.Function(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP])
|
||||
|
||||
adj = ca.jtimes(f_expl, ca.vertcat(x, u), lambdaX, True)
|
||||
|
||||
fun_name = model_name + '_expl_vde_adj'
|
||||
expl_vde_adj = ca.Function(fun_name, [x, lambdaX, u, p], [adj])
|
||||
|
||||
if generate_hess:
|
||||
S_forw = ca.vertcat(ca.horzcat(Sx, Sp), ca.horzcat(ca.DM.zeros(nu,nx), ca.DM.eye(nu)))
|
||||
hess = ca.mtimes(ca.transpose(S_forw),ca.jtimes(adj, ca.vertcat(x,u), S_forw))
|
||||
hess2 = []
|
||||
for j in range(nx+nu):
|
||||
for i in range(j,nx+nu):
|
||||
hess2 = ca.vertcat(hess2, hess[i,j])
|
||||
|
||||
fun_name = model_name + '_expl_ode_hess'
|
||||
expl_ode_hess = ca.Function(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2])
|
||||
|
||||
# change directory
|
||||
cwd = os.getcwd()
|
||||
model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model'))
|
||||
if not os.path.exists(model_dir):
|
||||
os.makedirs(model_dir)
|
||||
os.chdir(model_dir)
|
||||
|
||||
# generate C code
|
||||
fun_name = model_name + '_expl_ode_fun'
|
||||
expl_ode_fun.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_expl_vde_forw'
|
||||
expl_vde_forw.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_expl_vde_adj'
|
||||
expl_vde_adj.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
if generate_hess:
|
||||
fun_name = model_name + '_expl_ode_hess'
|
||||
expl_ode_hess.generate(fun_name, casadi_codegen_opts)
|
||||
os.chdir(cwd)
|
||||
|
||||
return
|
||||
|
||||
|
||||
def generate_c_code_implicit_ode( model, opts ):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
# load model
|
||||
x = model.x
|
||||
xdot = model.xdot
|
||||
u = model.u
|
||||
z = model.z
|
||||
p = model.p
|
||||
f_impl = model.f_impl_expr
|
||||
model_name = model.name
|
||||
|
||||
# get model dimensions
|
||||
nx = casadi_length(x)
|
||||
nz = casadi_length(z)
|
||||
|
||||
# generate jacobians
|
||||
jac_x = ca.jacobian(f_impl, x)
|
||||
jac_xdot = ca.jacobian(f_impl, xdot)
|
||||
jac_u = ca.jacobian(f_impl, u)
|
||||
jac_z = ca.jacobian(f_impl, z)
|
||||
|
||||
# Set up functions
|
||||
p = model.p
|
||||
fun_name = model_name + '_impl_dae_fun'
|
||||
impl_dae_fun = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl])
|
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z'
|
||||
impl_dae_fun_jac_x_xdot_z = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z])
|
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z'
|
||||
impl_dae_fun_jac_x_xdot_u_z = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z])
|
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u'
|
||||
impl_dae_fun_jac_x_xdot_u = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u])
|
||||
|
||||
fun_name = model_name + '_impl_dae_jac_x_xdot_u_z'
|
||||
impl_dae_jac_x_xdot_u_z = ca.Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z])
|
||||
|
||||
if opts["generate_hess"]:
|
||||
x_xdot_z_u = ca.vertcat(x, xdot, z, u)
|
||||
symbol = get_casadi_symbol(x)
|
||||
multiplier = symbol('multiplier', nx + nz)
|
||||
ADJ = ca.jtimes(f_impl, x_xdot_z_u, multiplier, True)
|
||||
HESS = ca.jacobian(ADJ, x_xdot_z_u)
|
||||
fun_name = model_name + '_impl_dae_hess'
|
||||
impl_dae_hess = ca.Function(fun_name, [x, xdot, u, z, multiplier, p], [HESS])
|
||||
|
||||
# change directory
|
||||
cwd = os.getcwd()
|
||||
model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model'))
|
||||
if not os.path.exists(model_dir):
|
||||
os.makedirs(model_dir)
|
||||
os.chdir(model_dir)
|
||||
|
||||
# generate C code
|
||||
fun_name = model_name + '_impl_dae_fun'
|
||||
impl_dae_fun.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z'
|
||||
impl_dae_fun_jac_x_xdot_z.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_impl_dae_jac_x_xdot_u_z'
|
||||
impl_dae_jac_x_xdot_u_z.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z'
|
||||
impl_dae_fun_jac_x_xdot_u_z.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u'
|
||||
impl_dae_fun_jac_x_xdot_u.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
if opts["generate_hess"]:
|
||||
fun_name = model_name + '_impl_dae_hess'
|
||||
impl_dae_hess.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
os.chdir(cwd)
|
||||
return
|
||||
|
||||
|
||||
def generate_c_code_gnsf( model, opts ):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
model_name = model.name
|
||||
|
||||
# set up directory
|
||||
cwd = os.getcwd()
|
||||
model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model'))
|
||||
if not os.path.exists(model_dir):
|
||||
os.makedirs(model_dir)
|
||||
os.chdir(model_dir)
|
||||
|
||||
# obtain gnsf dimensions
|
||||
get_matrices_fun = model.get_matrices_fun
|
||||
phi_fun = model.phi_fun
|
||||
|
||||
size_gnsf_A = get_matrices_fun.size_out(0)
|
||||
gnsf_nx1 = size_gnsf_A[1]
|
||||
gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
|
||||
gnsf_nuhat = max(phi_fun.size_in(1))
|
||||
gnsf_ny = max(phi_fun.size_in(0))
|
||||
gnsf_nout = max(phi_fun.size_out(0))
|
||||
|
||||
# set up expressions
|
||||
# if the model uses ca.MX because of cost/constraints
|
||||
# the DAE can be exported as ca.SX -> detect GNSF in Matlab
|
||||
# -> evaluated ca.SX GNSF functions with ca.MX.
|
||||
u = model.u
|
||||
symbol = get_casadi_symbol(u)
|
||||
|
||||
y = symbol("y", gnsf_ny, 1)
|
||||
uhat = symbol("uhat", gnsf_nuhat, 1)
|
||||
p = model.p
|
||||
x1 = symbol("gnsf_x1", gnsf_nx1, 1)
|
||||
x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1)
|
||||
z1 = symbol("gnsf_z1", gnsf_nz1, 1)
|
||||
dummy = symbol("gnsf_dummy", 1, 1)
|
||||
empty_var = symbol("gnsf_empty_var", 0, 0)
|
||||
|
||||
## generate C code
|
||||
fun_name = model_name + '_gnsf_phi_fun'
|
||||
phi_fun_ = ca.Function(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)])
|
||||
phi_fun_.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_gnsf_phi_fun_jac_y'
|
||||
phi_fun_jac_y = model.phi_fun_jac_y
|
||||
phi_fun_jac_y_ = ca.Function(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p))
|
||||
phi_fun_jac_y_.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_gnsf_phi_jac_y_uhat'
|
||||
phi_jac_y_uhat = model.phi_jac_y_uhat
|
||||
phi_jac_y_uhat_ = ca.Function(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p))
|
||||
phi_jac_y_uhat_.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_gnsf_f_lo_fun_jac_x1k1uz'
|
||||
f_lo_fun_jac_x1k1uz = model.f_lo_fun_jac_x1k1uz
|
||||
f_lo_fun_jac_x1k1uz_eval = f_lo_fun_jac_x1k1uz(x1, x1dot, z1, u, p)
|
||||
|
||||
# avoid codegeneration issue
|
||||
if not isinstance(f_lo_fun_jac_x1k1uz_eval, tuple) and is_empty(f_lo_fun_jac_x1k1uz_eval):
|
||||
f_lo_fun_jac_x1k1uz_eval = [empty_var]
|
||||
|
||||
f_lo_fun_jac_x1k1uz_ = ca.Function(fun_name, [x1, x1dot, z1, u, p],
|
||||
f_lo_fun_jac_x1k1uz_eval)
|
||||
f_lo_fun_jac_x1k1uz_.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_gnsf_get_matrices_fun'
|
||||
get_matrices_fun_ = ca.Function(fun_name, [dummy], get_matrices_fun(1))
|
||||
get_matrices_fun_.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
# remove fields for json dump
|
||||
del model.phi_fun
|
||||
del model.phi_fun_jac_y
|
||||
del model.phi_jac_y_uhat
|
||||
del model.f_lo_fun_jac_x1k1uz
|
||||
del model.get_matrices_fun
|
||||
|
||||
os.chdir(cwd)
|
||||
|
||||
return
|
||||
|
||||
|
||||
################
|
||||
# Cost
|
||||
################
|
||||
|
||||
def generate_c_code_external_cost(model, stage_type, opts):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
x = model.x
|
||||
p = model.p
|
||||
u = model.u
|
||||
z = model.z
|
||||
symbol = get_casadi_symbol(x)
|
||||
|
||||
if stage_type == 'terminal':
|
||||
suffix_name = "_cost_ext_cost_e_fun"
|
||||
suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess"
|
||||
suffix_name_jac = "_cost_ext_cost_e_fun_jac"
|
||||
ext_cost = model.cost_expr_ext_cost_e
|
||||
custom_hess = model.cost_expr_ext_cost_custom_hess_e
|
||||
# Last stage cannot depend on u and z
|
||||
u = symbol("u", 0, 0)
|
||||
z = symbol("z", 0, 0)
|
||||
|
||||
elif stage_type == 'path':
|
||||
suffix_name = "_cost_ext_cost_fun"
|
||||
suffix_name_hess = "_cost_ext_cost_fun_jac_hess"
|
||||
suffix_name_jac = "_cost_ext_cost_fun_jac"
|
||||
ext_cost = model.cost_expr_ext_cost
|
||||
custom_hess = model.cost_expr_ext_cost_custom_hess
|
||||
|
||||
elif stage_type == 'initial':
|
||||
suffix_name = "_cost_ext_cost_0_fun"
|
||||
suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess"
|
||||
suffix_name_jac = "_cost_ext_cost_0_fun_jac"
|
||||
ext_cost = model.cost_expr_ext_cost_0
|
||||
custom_hess = model.cost_expr_ext_cost_custom_hess_0
|
||||
|
||||
nunx = x.shape[0] + u.shape[0]
|
||||
|
||||
# set up functions to be exported
|
||||
fun_name = model.name + suffix_name
|
||||
fun_name_hess = model.name + suffix_name_hess
|
||||
fun_name_jac = model.name + suffix_name_jac
|
||||
|
||||
# generate expression for full gradient and Hessian
|
||||
hess_uxz, grad_uxz = ca.hessian(ext_cost, ca.vertcat(u, x, z))
|
||||
|
||||
hess_ux = hess_uxz[:nunx, :nunx]
|
||||
hess_z = hess_uxz[nunx:, nunx:]
|
||||
hess_z_ux = hess_uxz[nunx:, :nunx]
|
||||
|
||||
if custom_hess is not None:
|
||||
hess_ux = custom_hess
|
||||
|
||||
ext_cost_fun = ca.Function(fun_name, [x, u, z, p], [ext_cost])
|
||||
|
||||
ext_cost_fun_jac_hess = ca.Function(
|
||||
fun_name_hess, [x, u, z, p], [ext_cost, grad_uxz, hess_ux, hess_z, hess_z_ux]
|
||||
)
|
||||
ext_cost_fun_jac = ca.Function(
|
||||
fun_name_jac, [x, u, z, p], [ext_cost, grad_uxz]
|
||||
)
|
||||
|
||||
# change directory
|
||||
cwd = os.getcwd()
|
||||
cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost'))
|
||||
if not os.path.exists(cost_dir):
|
||||
os.makedirs(cost_dir)
|
||||
os.chdir(cost_dir)
|
||||
|
||||
ext_cost_fun.generate(fun_name, casadi_codegen_opts)
|
||||
ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_codegen_opts)
|
||||
ext_cost_fun_jac.generate(fun_name_jac, casadi_codegen_opts)
|
||||
|
||||
os.chdir(cwd)
|
||||
return
|
||||
|
||||
|
||||
def generate_c_code_nls_cost( model, cost_name, stage_type, opts ):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
x = model.x
|
||||
z = model.z
|
||||
p = model.p
|
||||
u = model.u
|
||||
|
||||
symbol = get_casadi_symbol(x)
|
||||
|
||||
if stage_type == 'terminal':
|
||||
middle_name = '_cost_y_e'
|
||||
u = symbol('u', 0, 0)
|
||||
y_expr = model.cost_y_expr_e
|
||||
|
||||
elif stage_type == 'initial':
|
||||
middle_name = '_cost_y_0'
|
||||
y_expr = model.cost_y_expr_0
|
||||
|
||||
elif stage_type == 'path':
|
||||
middle_name = '_cost_y'
|
||||
y_expr = model.cost_y_expr
|
||||
|
||||
# change directory
|
||||
cwd = os.getcwd()
|
||||
cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost'))
|
||||
if not os.path.exists(cost_dir):
|
||||
os.makedirs(cost_dir)
|
||||
os.chdir(cost_dir)
|
||||
|
||||
# set up expressions
|
||||
cost_jac_expr = ca.transpose(ca.jacobian(y_expr, ca.vertcat(u, x)))
|
||||
dy_dz = ca.jacobian(y_expr, z)
|
||||
ny = casadi_length(y_expr)
|
||||
|
||||
y = symbol('y', ny, 1)
|
||||
|
||||
y_adj = ca.jtimes(y_expr, ca.vertcat(u, x), y, True)
|
||||
y_hess = ca.jacobian(y_adj, ca.vertcat(u, x))
|
||||
|
||||
## generate C code
|
||||
suffix_name = '_fun'
|
||||
fun_name = cost_name + middle_name + suffix_name
|
||||
y_fun = ca.Function( fun_name, [x, u, z, p], [ y_expr ])
|
||||
y_fun.generate( fun_name, casadi_codegen_opts )
|
||||
|
||||
suffix_name = '_fun_jac_ut_xt'
|
||||
fun_name = cost_name + middle_name + suffix_name
|
||||
y_fun_jac_ut_xt = ca.Function(fun_name, [x, u, z, p], [ y_expr, cost_jac_expr, dy_dz ])
|
||||
y_fun_jac_ut_xt.generate( fun_name, casadi_codegen_opts )
|
||||
|
||||
suffix_name = '_hess'
|
||||
fun_name = cost_name + middle_name + suffix_name
|
||||
y_hess = ca.Function(fun_name, [x, u, z, y, p], [ y_hess ])
|
||||
y_hess.generate( fun_name, casadi_codegen_opts )
|
||||
|
||||
os.chdir(cwd)
|
||||
|
||||
return
|
||||
|
||||
|
||||
|
||||
def generate_c_code_conl_cost(model, cost_name, stage_type, opts):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
x = model.x
|
||||
z = model.z
|
||||
p = model.p
|
||||
|
||||
symbol = get_casadi_symbol(x)
|
||||
|
||||
if stage_type == 'terminal':
|
||||
u = symbol('u', 0, 0)
|
||||
|
||||
yref = model.cost_r_in_psi_expr_e
|
||||
inner_expr = model.cost_y_expr_e - yref
|
||||
outer_expr = model.cost_psi_expr_e
|
||||
res_expr = model.cost_r_in_psi_expr_e
|
||||
|
||||
suffix_name_fun = '_conl_cost_e_fun'
|
||||
suffix_name_fun_jac_hess = '_conl_cost_e_fun_jac_hess'
|
||||
|
||||
custom_hess = model.cost_conl_custom_outer_hess_e
|
||||
|
||||
elif stage_type == 'initial':
|
||||
u = model.u
|
||||
|
||||
yref = model.cost_r_in_psi_expr_0
|
||||
inner_expr = model.cost_y_expr_0 - yref
|
||||
outer_expr = model.cost_psi_expr_0
|
||||
res_expr = model.cost_r_in_psi_expr_0
|
||||
|
||||
suffix_name_fun = '_conl_cost_0_fun'
|
||||
suffix_name_fun_jac_hess = '_conl_cost_0_fun_jac_hess'
|
||||
|
||||
custom_hess = model.cost_conl_custom_outer_hess_0
|
||||
|
||||
elif stage_type == 'path':
|
||||
u = model.u
|
||||
|
||||
yref = model.cost_r_in_psi_expr
|
||||
inner_expr = model.cost_y_expr - yref
|
||||
outer_expr = model.cost_psi_expr
|
||||
res_expr = model.cost_r_in_psi_expr
|
||||
|
||||
suffix_name_fun = '_conl_cost_fun'
|
||||
suffix_name_fun_jac_hess = '_conl_cost_fun_jac_hess'
|
||||
|
||||
custom_hess = model.cost_conl_custom_outer_hess
|
||||
|
||||
# set up function names
|
||||
fun_name_cost_fun = model.name + suffix_name_fun
|
||||
fun_name_cost_fun_jac_hess = model.name + suffix_name_fun_jac_hess
|
||||
|
||||
# set up functions to be exported
|
||||
outer_loss_fun = ca.Function('psi', [res_expr, p], [outer_expr])
|
||||
cost_expr = outer_loss_fun(inner_expr, p)
|
||||
|
||||
outer_loss_grad_fun = ca.Function('outer_loss_grad', [res_expr, p], [ca.jacobian(outer_expr, res_expr).T])
|
||||
|
||||
if custom_hess is None:
|
||||
outer_hess_fun = ca.Function('inner_hess', [res_expr, p], [ca.hessian(outer_loss_fun(res_expr, p), res_expr)[0]])
|
||||
else:
|
||||
outer_hess_fun = ca.Function('inner_hess', [res_expr, p], [custom_hess])
|
||||
|
||||
Jt_ux_expr = ca.jacobian(inner_expr, ca.vertcat(u, x)).T
|
||||
Jt_z_expr = ca.jacobian(inner_expr, z).T
|
||||
|
||||
cost_fun = ca.Function(
|
||||
fun_name_cost_fun,
|
||||
[x, u, z, yref, p],
|
||||
[cost_expr])
|
||||
|
||||
cost_fun_jac_hess = ca.Function(
|
||||
fun_name_cost_fun_jac_hess,
|
||||
[x, u, z, yref, p],
|
||||
[cost_expr, outer_loss_grad_fun(inner_expr, p), Jt_ux_expr, Jt_z_expr, outer_hess_fun(inner_expr, p)]
|
||||
)
|
||||
# change directory
|
||||
cwd = os.getcwd()
|
||||
cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost'))
|
||||
if not os.path.exists(cost_dir):
|
||||
os.makedirs(cost_dir)
|
||||
os.chdir(cost_dir)
|
||||
|
||||
# generate C code
|
||||
cost_fun.generate(fun_name_cost_fun, casadi_codegen_opts)
|
||||
cost_fun_jac_hess.generate(fun_name_cost_fun_jac_hess, casadi_codegen_opts)
|
||||
|
||||
os.chdir(cwd)
|
||||
|
||||
return
|
||||
|
||||
|
||||
################
|
||||
# Constraints
|
||||
################
|
||||
def generate_c_code_constraint( model, con_name, is_terminal, opts ):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
# load constraint variables and expression
|
||||
x = model.x
|
||||
p = model.p
|
||||
|
||||
symbol = get_casadi_symbol(x)
|
||||
|
||||
if is_terminal:
|
||||
con_h_expr = model.con_h_expr_e
|
||||
con_phi_expr = model.con_phi_expr_e
|
||||
# create dummy u, z
|
||||
u = symbol('u', 0, 0)
|
||||
z = symbol('z', 0, 0)
|
||||
else:
|
||||
con_h_expr = model.con_h_expr
|
||||
con_phi_expr = model.con_phi_expr
|
||||
u = model.u
|
||||
z = model.z
|
||||
|
||||
if (not is_empty(con_h_expr)) and (not is_empty(con_phi_expr)):
|
||||
raise Exception("acados: you can either have constraint_h, or constraint_phi, not both.")
|
||||
|
||||
if (is_empty(con_h_expr) and is_empty(con_phi_expr)):
|
||||
# both empty -> nothing to generate
|
||||
return
|
||||
|
||||
if is_empty(con_h_expr):
|
||||
constr_type = 'BGP'
|
||||
else:
|
||||
constr_type = 'BGH'
|
||||
|
||||
if is_empty(p):
|
||||
p = symbol('p', 0, 0)
|
||||
|
||||
if is_empty(z):
|
||||
z = symbol('z', 0, 0)
|
||||
|
||||
if not (is_empty(con_h_expr)) and opts['generate_hess']:
|
||||
# multipliers for hessian
|
||||
nh = casadi_length(con_h_expr)
|
||||
lam_h = symbol('lam_h', nh, 1)
|
||||
|
||||
# set up & change directory
|
||||
cwd = os.getcwd()
|
||||
constraints_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_constraints'))
|
||||
if not os.path.exists(constraints_dir):
|
||||
os.makedirs(constraints_dir)
|
||||
os.chdir(constraints_dir)
|
||||
|
||||
# export casadi functions
|
||||
if constr_type == 'BGH':
|
||||
if is_terminal:
|
||||
fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt'
|
||||
else:
|
||||
fun_name = con_name + '_constr_h_fun_jac_uxt_zt'
|
||||
|
||||
jac_ux_t = ca.transpose(ca.jacobian(con_h_expr, ca.vertcat(u,x)))
|
||||
jac_z_t = ca.jacobian(con_h_expr, z)
|
||||
constraint_fun_jac_tran = ca.Function(fun_name, [x, u, z, p], \
|
||||
[con_h_expr, jac_ux_t, jac_z_t])
|
||||
|
||||
constraint_fun_jac_tran.generate(fun_name, casadi_codegen_opts)
|
||||
if opts['generate_hess']:
|
||||
|
||||
if is_terminal:
|
||||
fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt_hess'
|
||||
else:
|
||||
fun_name = con_name + '_constr_h_fun_jac_uxt_zt_hess'
|
||||
|
||||
# adjoint
|
||||
adj_ux = ca.jtimes(con_h_expr, ca.vertcat(u, x), lam_h, True)
|
||||
# hessian
|
||||
hess_ux = ca.jacobian(adj_ux, ca.vertcat(u, x))
|
||||
|
||||
adj_z = ca.jtimes(con_h_expr, z, lam_h, True)
|
||||
hess_z = ca.jacobian(adj_z, z)
|
||||
|
||||
# set up functions
|
||||
constraint_fun_jac_tran_hess = \
|
||||
ca.Function(fun_name, [x, u, lam_h, z, p], \
|
||||
[con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z])
|
||||
|
||||
# generate C code
|
||||
constraint_fun_jac_tran_hess.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
if is_terminal:
|
||||
fun_name = con_name + '_constr_h_e_fun'
|
||||
else:
|
||||
fun_name = con_name + '_constr_h_fun'
|
||||
h_fun = ca.Function(fun_name, [x, u, z, p], [con_h_expr])
|
||||
h_fun.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
else: # BGP constraint
|
||||
if is_terminal:
|
||||
fun_name = con_name + '_phi_e_constraint'
|
||||
r = model.con_r_in_phi_e
|
||||
con_r_expr = model.con_r_expr_e
|
||||
else:
|
||||
fun_name = con_name + '_phi_constraint'
|
||||
r = model.con_r_in_phi
|
||||
con_r_expr = model.con_r_expr
|
||||
|
||||
nphi = casadi_length(con_phi_expr)
|
||||
con_phi_expr_x_u_z = ca.substitute(con_phi_expr, r, con_r_expr)
|
||||
phi_jac_u = ca.jacobian(con_phi_expr_x_u_z, u)
|
||||
phi_jac_x = ca.jacobian(con_phi_expr_x_u_z, x)
|
||||
phi_jac_z = ca.jacobian(con_phi_expr_x_u_z, z)
|
||||
|
||||
hess = ca.hessian(con_phi_expr[0], r)[0]
|
||||
for i in range(1, nphi):
|
||||
hess = ca.vertcat(hess, ca.hessian(con_phi_expr[i], r)[0])
|
||||
|
||||
r_jac_u = ca.jacobian(con_r_expr, u)
|
||||
r_jac_x = ca.jacobian(con_r_expr, x)
|
||||
|
||||
constraint_phi = \
|
||||
ca.Function(fun_name, [x, u, z, p], \
|
||||
[con_phi_expr_x_u_z, \
|
||||
ca.vertcat(ca.transpose(phi_jac_u), ca.transpose(phi_jac_x)), \
|
||||
ca.transpose(phi_jac_z), \
|
||||
hess,
|
||||
ca.vertcat(ca.transpose(r_jac_u), ca.transpose(r_jac_x))])
|
||||
|
||||
constraint_phi.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
# change directory back
|
||||
os.chdir(cwd)
|
||||
|
||||
return
|
||||
|
||||
216
third_party/acados/acados_template/gnsf/check_reformulation.py
vendored
Normal file
216
third_party/acados/acados_template/gnsf/check_reformulation.py
vendored
Normal file
@@ -0,0 +1,216 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
from ..utils import casadi_length
|
||||
from casadi import *
|
||||
import numpy as np
|
||||
|
||||
|
||||
def check_reformulation(model, gnsf, print_info):
|
||||
|
||||
## Description:
|
||||
# this function takes the implicit ODE/ index-1 DAE and a gnsf structure
|
||||
# to evaluate both models at num_eval random points x0, x0dot, z0, u0
|
||||
# if for all points the relative error is <= TOL, the function will return::
|
||||
# 1, otherwise it will give an error.
|
||||
|
||||
TOL = 1e-14
|
||||
num_eval = 10
|
||||
|
||||
# get dimensions
|
||||
nx = gnsf["nx"]
|
||||
nu = gnsf["nu"]
|
||||
nz = gnsf["nz"]
|
||||
nx1 = gnsf["nx1"]
|
||||
nx2 = gnsf["nx2"]
|
||||
nz1 = gnsf["nz1"]
|
||||
nz2 = gnsf["nz2"]
|
||||
n_out = gnsf["n_out"]
|
||||
|
||||
# get model matrices
|
||||
A = gnsf["A"]
|
||||
B = gnsf["B"]
|
||||
C = gnsf["C"]
|
||||
E = gnsf["E"]
|
||||
c = gnsf["c"]
|
||||
|
||||
L_x = gnsf["L_x"]
|
||||
L_xdot = gnsf["L_xdot"]
|
||||
L_z = gnsf["L_z"]
|
||||
L_u = gnsf["L_u"]
|
||||
|
||||
A_LO = gnsf["A_LO"]
|
||||
E_LO = gnsf["E_LO"]
|
||||
B_LO = gnsf["B_LO"]
|
||||
c_LO = gnsf["c_LO"]
|
||||
|
||||
I_x1 = range(nx1)
|
||||
I_x2 = range(nx1, nx)
|
||||
|
||||
I_z1 = range(nz1)
|
||||
I_z2 = range(nz1, nz)
|
||||
|
||||
idx_perm_f = gnsf["idx_perm_f"]
|
||||
|
||||
# get casadi variables
|
||||
x = gnsf["x"]
|
||||
xdot = gnsf["xdot"]
|
||||
z = gnsf["z"]
|
||||
u = gnsf["u"]
|
||||
y = gnsf["y"]
|
||||
uhat = gnsf["uhat"]
|
||||
p = gnsf["p"]
|
||||
|
||||
# create functions
|
||||
impl_dae_fun = Function("impl_dae_fun", [x, xdot, u, z, p], [model.f_impl_expr])
|
||||
phi_fun = Function("phi_fun", [y, uhat, p], [gnsf["phi_expr"]])
|
||||
f_lo_fun = Function(
|
||||
"f_lo_fun", [x[range(nx1)], xdot[range(nx1)], z, u, p], [gnsf["f_lo_expr"]]
|
||||
)
|
||||
|
||||
# print(gnsf)
|
||||
# print(gnsf["n_out"])
|
||||
|
||||
for i_check in range(num_eval):
|
||||
# generate random values
|
||||
x0 = np.random.rand(nx, 1)
|
||||
x0dot = np.random.rand(nx, 1)
|
||||
z0 = np.random.rand(nz, 1)
|
||||
u0 = np.random.rand(nu, 1)
|
||||
|
||||
if gnsf["ny"] > 0:
|
||||
y0 = L_x @ x0[I_x1] + L_xdot @ x0dot[I_x1] + L_z @ z0[I_z1]
|
||||
else:
|
||||
y0 = []
|
||||
if gnsf["nuhat"] > 0:
|
||||
uhat0 = L_u @ u0
|
||||
else:
|
||||
uhat0 = []
|
||||
|
||||
# eval functions
|
||||
p0 = np.random.rand(gnsf["np"], 1)
|
||||
f_impl_val = impl_dae_fun(x0, x0dot, u0, z0, p0).full()
|
||||
phi_val = phi_fun(y0, uhat0, p0)
|
||||
f_lo_val = f_lo_fun(x0[I_x1], x0dot[I_x1], z0[I_z1], u0, p0)
|
||||
|
||||
f_impl_val = f_impl_val[idx_perm_f]
|
||||
# eval gnsf
|
||||
if n_out > 0:
|
||||
C_phi = C @ phi_val
|
||||
else:
|
||||
C_phi = np.zeros((nx1 + nz1, 1))
|
||||
try:
|
||||
gnsf_val1 = (
|
||||
A @ x0[I_x1] + B @ u0 + C_phi + c - E @ vertcat(x0dot[I_x1], z0[I_z1])
|
||||
)
|
||||
# gnsf_1 = (A @ x[I_x1] + B @ u + C_phi + c - E @ vertcat(xdot[I_x1], z[I_z1]))
|
||||
except:
|
||||
import pdb
|
||||
|
||||
pdb.set_trace()
|
||||
|
||||
if nx2 > 0: # eval LOS:
|
||||
gnsf_val2 = (
|
||||
A_LO @ x0[I_x2]
|
||||
+ B_LO @ u0
|
||||
+ c_LO
|
||||
+ f_lo_val
|
||||
- E_LO @ vertcat(x0dot[I_x2], z0[I_z2])
|
||||
)
|
||||
gnsf_val = vertcat(gnsf_val1, gnsf_val2).full()
|
||||
else:
|
||||
gnsf_val = gnsf_val1.full()
|
||||
# compute error and check
|
||||
rel_error = np.linalg.norm(f_impl_val - gnsf_val) / np.linalg.norm(f_impl_val)
|
||||
|
||||
if rel_error > TOL:
|
||||
print("transcription failed rel_error > TOL")
|
||||
print("you are in debug mode now: import pdb; pdb.set_trace()")
|
||||
abs_error = gnsf_val - f_impl_val
|
||||
# T = table(f_impl_val, gnsf_val, abs_error)
|
||||
# print(T)
|
||||
print("abs_error:", abs_error)
|
||||
# error('transcription failed rel_error > TOL')
|
||||
# check = 0
|
||||
import pdb
|
||||
|
||||
pdb.set_trace()
|
||||
if print_info:
|
||||
print(" ")
|
||||
print("model reformulation checked: relative error <= TOL = ", str(TOL))
|
||||
print(" ")
|
||||
check = 1
|
||||
## helpful for debugging:
|
||||
# # use in calling function and compare
|
||||
# # compare f_impl(i) with gnsf_val1(i)
|
||||
#
|
||||
|
||||
# nx = gnsf['nx']
|
||||
# nu = gnsf['nu']
|
||||
# nz = gnsf['nz']
|
||||
# nx1 = gnsf['nx1']
|
||||
# nx2 = gnsf['nx2']
|
||||
#
|
||||
# A = gnsf['A']
|
||||
# B = gnsf['B']
|
||||
# C = gnsf['C']
|
||||
# E = gnsf['E']
|
||||
# c = gnsf['c']
|
||||
#
|
||||
# L_x = gnsf['L_x']
|
||||
# L_z = gnsf['L_z']
|
||||
# L_xdot = gnsf['L_xdot']
|
||||
# L_u = gnsf['L_u']
|
||||
#
|
||||
# A_LO = gnsf['A_LO']
|
||||
#
|
||||
# x0 = rand(nx, 1)
|
||||
# x0dot = rand(nx, 1)
|
||||
# z0 = rand(nz, 1)
|
||||
# u0 = rand(nu, 1)
|
||||
# I_x1 = range(nx1)
|
||||
# I_x2 = nx1+range(nx)
|
||||
#
|
||||
# y0 = L_x @ x0[I_x1] + L_xdot @ x0dot[I_x1] + L_z @ z0
|
||||
# uhat0 = L_u @ u0
|
||||
#
|
||||
# gnsf_val1 = (A @ x[I_x1] + B @ u + # C @ phi_current + c) - E @ [xdot[I_x1] z]
|
||||
# gnsf_val1 = gnsf_val1.simplify()
|
||||
#
|
||||
# # gnsf_val2 = A_LO @ x[I_x2] + gnsf['f_lo_fun'](x[I_x1], xdot[I_x1], z, u) - xdot[I_x2]
|
||||
# gnsf_val2 = A_LO @ x[I_x2] + gnsf['f_lo_fun'](x[I_x1], xdot[I_x1], z, u) - xdot[I_x2]
|
||||
#
|
||||
#
|
||||
# gnsf_val = [gnsf_val1 gnsf_val2]
|
||||
# gnsf_val = gnsf_val.simplify()
|
||||
# dyn_expr_f = dyn_expr_f.simplify()
|
||||
# import pdb; pdb.set_trace()
|
||||
|
||||
return check
|
||||
278
third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py
vendored
Normal file
278
third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py
vendored
Normal file
@@ -0,0 +1,278 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
from casadi import *
|
||||
from .check_reformulation import check_reformulation
|
||||
from .determine_input_nonlinearity_function import determine_input_nonlinearity_function
|
||||
from ..utils import casadi_length, print_casadi_expression
|
||||
|
||||
|
||||
def detect_affine_terms_reduce_nonlinearity(gnsf, acados_ocp, print_info):
|
||||
|
||||
## Description
|
||||
# this function takes a gnsf structure with trivial model matrices (A, B,
|
||||
# E, c are zeros, and C is eye).
|
||||
# It detects all affine linear terms and sets up an equivalent model in the
|
||||
# GNSF structure, where all affine linear terms are modeled through the
|
||||
# matrices A, B, E, c and the linear output system (LOS) is empty.
|
||||
# NOTE: model is just taken as an argument to check equivalence of the
|
||||
# models within the function.
|
||||
|
||||
model = acados_ocp.model
|
||||
if print_info:
|
||||
print(" ")
|
||||
print("====================================================================")
|
||||
print(" ")
|
||||
print("============ Detect affine-linear dependencies ==================")
|
||||
print(" ")
|
||||
print("====================================================================")
|
||||
print(" ")
|
||||
# symbolics
|
||||
x = gnsf["x"]
|
||||
xdot = gnsf["xdot"]
|
||||
u = gnsf["u"]
|
||||
z = gnsf["z"]
|
||||
|
||||
# dimensions
|
||||
nx = gnsf["nx"]
|
||||
nu = gnsf["nu"]
|
||||
nz = gnsf["nz"]
|
||||
|
||||
ny_old = gnsf["ny"]
|
||||
nuhat_old = gnsf["nuhat"]
|
||||
|
||||
## Represent all affine dependencies through the model matrices A, B, E, c
|
||||
## determine A
|
||||
n_nodes_current = n_nodes(gnsf["phi_expr"])
|
||||
|
||||
for ii in range(casadi_length(gnsf["phi_expr"])):
|
||||
fii = gnsf["phi_expr"][ii]
|
||||
for ix in range(nx):
|
||||
var = x[ix]
|
||||
varname = var.name
|
||||
# symbolic jacobian of fii w.r.t. xi
|
||||
jac_fii_xi = jacobian(fii, var)
|
||||
if jac_fii_xi.is_constant():
|
||||
# jacobian value
|
||||
jac_fii_xi_fun = Function("jac_fii_xi_fun", [x[1]], [jac_fii_xi])
|
||||
# x[1] as input just to have a scalar input and call the function as follows:
|
||||
gnsf["A"][ii, ix] = jac_fii_xi_fun(0).full()
|
||||
else:
|
||||
gnsf["A"][ii, ix] = 0
|
||||
if print_info:
|
||||
print(
|
||||
"phi(",
|
||||
str(ii),
|
||||
") is nonlinear in x(",
|
||||
str(ix),
|
||||
") = ",
|
||||
varname,
|
||||
)
|
||||
print(fii)
|
||||
print("-----------------------------------------------------")
|
||||
f_next = gnsf["phi_expr"] - gnsf["A"] @ x
|
||||
f_next = simplify(f_next)
|
||||
n_nodes_next = n_nodes(f_next)
|
||||
|
||||
if print_info:
|
||||
print("\n")
|
||||
print(f"determined matrix A:")
|
||||
print(gnsf["A"])
|
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes")
|
||||
# assert(n_nodes_current >= n_nodes_next,'n_nodes_current >= n_nodes_next FAILED')
|
||||
gnsf["phi_expr"] = f_next
|
||||
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
## determine B
|
||||
n_nodes_current = n_nodes(gnsf["phi_expr"])
|
||||
|
||||
for ii in range(casadi_length(gnsf["phi_expr"])):
|
||||
fii = gnsf["phi_expr"][ii]
|
||||
for iu in range(nu):
|
||||
var = u[iu]
|
||||
varname = var.name
|
||||
# symbolic jacobian of fii w.r.t. ui
|
||||
jac_fii_ui = jacobian(fii, var)
|
||||
if jac_fii_ui.is_constant(): # i.e. hessian is structural zero:
|
||||
# jacobian value
|
||||
jac_fii_ui_fun = Function("jac_fii_ui_fun", [x[1]], [jac_fii_ui])
|
||||
gnsf["B"][ii, iu] = jac_fii_ui_fun(0).full()
|
||||
else:
|
||||
gnsf["B"][ii, iu] = 0
|
||||
if print_info:
|
||||
print(f"phi({ii}) is nonlinear in u(", str(iu), ") = ", varname)
|
||||
print(fii)
|
||||
print("-----------------------------------------------------")
|
||||
f_next = gnsf["phi_expr"] - gnsf["B"] @ u
|
||||
f_next = simplify(f_next)
|
||||
n_nodes_next = n_nodes(f_next)
|
||||
|
||||
if print_info:
|
||||
print("\n")
|
||||
print(f"determined matrix B:")
|
||||
print(gnsf["B"])
|
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes")
|
||||
|
||||
gnsf["phi_expr"] = f_next
|
||||
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
## determine E
|
||||
n_nodes_current = n_nodes(gnsf["phi_expr"])
|
||||
k = vertcat(xdot, z)
|
||||
|
||||
for ii in range(casadi_length(gnsf["phi_expr"])):
|
||||
fii = gnsf["phi_expr"][ii]
|
||||
for ik in range(casadi_length(k)):
|
||||
# symbolic jacobian of fii w.r.t. ui
|
||||
var = k[ik]
|
||||
varname = var.name
|
||||
jac_fii_ki = jacobian(fii, var)
|
||||
if jac_fii_ki.is_constant():
|
||||
# jacobian value
|
||||
jac_fii_ki_fun = Function("jac_fii_ki_fun", [x[1]], [jac_fii_ki])
|
||||
gnsf["E"][ii, ik] = -jac_fii_ki_fun(0).full()
|
||||
else:
|
||||
gnsf["E"][ii, ik] = 0
|
||||
if print_info:
|
||||
print(f"phi( {ii}) is nonlinear in xdot_z({ik}) = ", varname)
|
||||
print(fii)
|
||||
print("-----------------------------------------------------")
|
||||
f_next = gnsf["phi_expr"] + gnsf["E"] @ k
|
||||
f_next = simplify(f_next)
|
||||
n_nodes_next = n_nodes(f_next)
|
||||
|
||||
if print_info:
|
||||
print("\n")
|
||||
print(f"determined matrix E:")
|
||||
print(gnsf["E"])
|
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes")
|
||||
|
||||
gnsf["phi_expr"] = f_next
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
## determine constant term c
|
||||
|
||||
n_nodes_current = n_nodes(gnsf["phi_expr"])
|
||||
for ii in range(casadi_length(gnsf["phi_expr"])):
|
||||
fii = gnsf["phi_expr"][ii]
|
||||
if fii.is_constant():
|
||||
# function value goes into c
|
||||
fii_fun = Function("fii_fun", [x[1]], [fii])
|
||||
gnsf["c"][ii] = fii_fun(0).full()
|
||||
else:
|
||||
gnsf["c"][ii] = 0
|
||||
if print_info:
|
||||
print(f"phi(", str(ii), ") is NOT constant")
|
||||
print(fii)
|
||||
print("-----------------------------------------------------")
|
||||
gnsf["phi_expr"] = gnsf["phi_expr"] - gnsf["c"]
|
||||
gnsf["phi_expr"] = simplify(gnsf["phi_expr"])
|
||||
n_nodes_next = n_nodes(gnsf["phi_expr"])
|
||||
|
||||
if print_info:
|
||||
print("\n")
|
||||
print(f"determined vector c:")
|
||||
print(gnsf["c"])
|
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes")
|
||||
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
## determine nonlinearity & corresponding matrix C
|
||||
## Reduce dimension of phi
|
||||
n_nodes_current = n_nodes(gnsf["phi_expr"])
|
||||
ind_non_zero = []
|
||||
for ii in range(casadi_length(gnsf["phi_expr"])):
|
||||
fii = gnsf["phi_expr"][ii]
|
||||
fii = simplify(fii)
|
||||
if not fii.is_zero():
|
||||
ind_non_zero = list(set.union(set(ind_non_zero), set([ii])))
|
||||
gnsf["phi_expr"] = gnsf["phi_expr"][ind_non_zero]
|
||||
|
||||
# C
|
||||
gnsf["C"] = np.zeros((nx + nz, len(ind_non_zero)))
|
||||
for ii in range(len(ind_non_zero)):
|
||||
gnsf["C"][ind_non_zero[ii], ii] = 1
|
||||
gnsf = determine_input_nonlinearity_function(gnsf)
|
||||
n_nodes_next = n_nodes(gnsf["phi_expr"])
|
||||
|
||||
if print_info:
|
||||
print(" ")
|
||||
print("determined matrix C:")
|
||||
print(gnsf["C"])
|
||||
print(
|
||||
"---------------------------------------------------------------------------------"
|
||||
)
|
||||
print(
|
||||
"------------- Success: Affine linear terms detected -----------------------------"
|
||||
)
|
||||
print(
|
||||
"---------------------------------------------------------------------------------"
|
||||
)
|
||||
print(
|
||||
f'reduced nonlinearity dimension n_out from {nx+nz} to {gnsf["n_out"]}'
|
||||
)
|
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes")
|
||||
print(" ")
|
||||
print("phi now reads as:")
|
||||
print_casadi_expression(gnsf["phi_expr"])
|
||||
|
||||
## determine input of nonlinearity function
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
gnsf["ny"] = casadi_length(gnsf["y"])
|
||||
gnsf["nuhat"] = casadi_length(gnsf["uhat"])
|
||||
|
||||
if print_info:
|
||||
print(
|
||||
"-----------------------------------------------------------------------------------"
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
f"reduced input ny of phi from ",
|
||||
str(ny_old),
|
||||
" to ",
|
||||
str(gnsf["ny"]),
|
||||
)
|
||||
print(
|
||||
f"reduced input nuhat of phi from ",
|
||||
str(nuhat_old),
|
||||
" to ",
|
||||
str(gnsf["nuhat"]),
|
||||
)
|
||||
print(
|
||||
"-----------------------------------------------------------------------------------"
|
||||
)
|
||||
|
||||
# if print_info:
|
||||
# print(f"gnsf: {gnsf}")
|
||||
|
||||
return gnsf
|
||||
240
third_party/acados/acados_template/gnsf/detect_gnsf_structure.py
vendored
Normal file
240
third_party/acados/acados_template/gnsf/detect_gnsf_structure.py
vendored
Normal file
@@ -0,0 +1,240 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
|
||||
|
||||
from casadi import Function, jacobian, SX, vertcat, horzcat
|
||||
|
||||
from .determine_trivial_gnsf_transcription import determine_trivial_gnsf_transcription
|
||||
from .detect_affine_terms_reduce_nonlinearity import (
|
||||
detect_affine_terms_reduce_nonlinearity,
|
||||
)
|
||||
from .reformulate_with_LOS import reformulate_with_LOS
|
||||
from .reformulate_with_invertible_E_mat import reformulate_with_invertible_E_mat
|
||||
from .structure_detection_print_summary import structure_detection_print_summary
|
||||
from .check_reformulation import check_reformulation
|
||||
|
||||
|
||||
def detect_gnsf_structure(acados_ocp, transcribe_opts=None):
|
||||
|
||||
## Description
|
||||
# This function takes a CasADi implicit ODE or index-1 DAE model "model"
|
||||
# consisting of a CasADi expression f_impl in the symbolic CasADi
|
||||
# variables x, xdot, u, z, (and possibly parameters p), which are also part
|
||||
# of the model, as well as a model name.
|
||||
# It will create a struct "gnsf" containing all information needed to use
|
||||
# it with the gnsf integrator in acados.
|
||||
# Additionally it will create the struct "reordered_model" which contains
|
||||
# the permuted state vector and permuted f_impl, in which additionally some
|
||||
# functions, which were made part of the linear output system of the gnsf,
|
||||
# have changed signs.
|
||||
|
||||
# Options: transcribe_opts is a Matlab struct consisting of booleans:
|
||||
# print_info: if extensive information on how the model is processed
|
||||
# is printed to the console.
|
||||
# generate_gnsf_model: if the neccessary C functions to simulate the gnsf
|
||||
# model with the acados implementation of the GNSF exploiting
|
||||
# integrator should be generated.
|
||||
# generate_gnsf_model: if the neccessary C functions to simulate the
|
||||
# reordered model with the acados implementation of the IRK
|
||||
# integrator should be generated.
|
||||
# check_E_invertibility: if the transcription method should check if the
|
||||
# assumption that the main blocks of the matrix gnsf.E are invertible
|
||||
# holds. If not, the method will try to reformulate the gnsf model
|
||||
# with a different model, such that the assumption holds.
|
||||
|
||||
# acados_root_dir = getenv('ACADOS_INSTALL_DIR')
|
||||
|
||||
## load transcribe_opts
|
||||
if transcribe_opts is None:
|
||||
print("WARNING: GNSF structure detection called without transcribe_opts")
|
||||
print(" using default settings")
|
||||
print("")
|
||||
transcribe_opts = dict()
|
||||
|
||||
if "print_info" in transcribe_opts:
|
||||
print_info = transcribe_opts["print_info"]
|
||||
else:
|
||||
print_info = 1
|
||||
print("print_info option was not set - default is true")
|
||||
|
||||
if "detect_LOS" in transcribe_opts:
|
||||
detect_LOS = transcribe_opts["detect_LOS"]
|
||||
else:
|
||||
detect_LOS = 1
|
||||
if print_info:
|
||||
print("detect_LOS option was not set - default is true")
|
||||
|
||||
if "check_E_invertibility" in transcribe_opts:
|
||||
check_E_invertibility = transcribe_opts["check_E_invertibility"]
|
||||
else:
|
||||
check_E_invertibility = 1
|
||||
if print_info:
|
||||
print("check_E_invertibility option was not set - default is true")
|
||||
|
||||
## Reformulate implicit index-1 DAE into GNSF form
|
||||
# (Generalized nonlinear static feedback)
|
||||
gnsf = determine_trivial_gnsf_transcription(acados_ocp, print_info)
|
||||
gnsf = detect_affine_terms_reduce_nonlinearity(gnsf, acados_ocp, print_info)
|
||||
|
||||
if detect_LOS:
|
||||
gnsf = reformulate_with_LOS(acados_ocp, gnsf, print_info)
|
||||
|
||||
if check_E_invertibility:
|
||||
gnsf = reformulate_with_invertible_E_mat(gnsf, acados_ocp, print_info)
|
||||
|
||||
# detect purely linear model
|
||||
if gnsf["nx1"] == 0 and gnsf["nz1"] == 0 and gnsf["nontrivial_f_LO"] == 0:
|
||||
gnsf["purely_linear"] = 1
|
||||
else:
|
||||
gnsf["purely_linear"] = 0
|
||||
|
||||
structure_detection_print_summary(gnsf, acados_ocp)
|
||||
check_reformulation(acados_ocp.model, gnsf, print_info)
|
||||
|
||||
## copy relevant fields from gnsf to model
|
||||
acados_ocp.model.get_matrices_fun = Function()
|
||||
dummy = acados_ocp.model.x[0]
|
||||
model_name = acados_ocp.model.name
|
||||
|
||||
get_matrices_fun = Function(
|
||||
f"{model_name}_gnsf_get_matrices_fun",
|
||||
[dummy],
|
||||
[
|
||||
gnsf["A"],
|
||||
gnsf["B"],
|
||||
gnsf["C"],
|
||||
gnsf["E"],
|
||||
gnsf["L_x"],
|
||||
gnsf["L_xdot"],
|
||||
gnsf["L_z"],
|
||||
gnsf["L_u"],
|
||||
gnsf["A_LO"],
|
||||
gnsf["c"],
|
||||
gnsf["E_LO"],
|
||||
gnsf["B_LO"],
|
||||
gnsf["nontrivial_f_LO"],
|
||||
gnsf["purely_linear"],
|
||||
gnsf["ipiv_x"] + 1,
|
||||
gnsf["ipiv_z"] + 1,
|
||||
gnsf["c_LO"],
|
||||
],
|
||||
)
|
||||
|
||||
phi = gnsf["phi_expr"]
|
||||
y = gnsf["y"]
|
||||
uhat = gnsf["uhat"]
|
||||
p = gnsf["p"]
|
||||
|
||||
jac_phi_y = jacobian(phi, y)
|
||||
jac_phi_uhat = jacobian(phi, uhat)
|
||||
|
||||
phi_fun = Function(f"{model_name}_gnsf_phi_fun", [y, uhat, p], [phi])
|
||||
acados_ocp.model.phi_fun = phi_fun
|
||||
acados_ocp.model.phi_fun_jac_y = Function(
|
||||
f"{model_name}_gnsf_phi_fun_jac_y", [y, uhat, p], [phi, jac_phi_y]
|
||||
)
|
||||
acados_ocp.model.phi_jac_y_uhat = Function(
|
||||
f"{model_name}_gnsf_phi_jac_y_uhat", [y, uhat, p], [jac_phi_y, jac_phi_uhat]
|
||||
)
|
||||
|
||||
x1 = acados_ocp.model.x[gnsf["idx_perm_x"][: gnsf["nx1"]]]
|
||||
x1dot = acados_ocp.model.xdot[gnsf["idx_perm_x"][: gnsf["nx1"]]]
|
||||
if gnsf["nz1"] > 0:
|
||||
z1 = acados_ocp.model.z[gnsf["idx_perm_z"][: gnsf["nz1"]]]
|
||||
else:
|
||||
z1 = SX.sym("z1", 0, 0)
|
||||
f_lo = gnsf["f_lo_expr"]
|
||||
u = acados_ocp.model.u
|
||||
acados_ocp.model.f_lo_fun_jac_x1k1uz = Function(
|
||||
f"{model_name}_gnsf_f_lo_fun_jac_x1k1uz",
|
||||
[x1, x1dot, z1, u, p],
|
||||
[
|
||||
f_lo,
|
||||
horzcat(
|
||||
jacobian(f_lo, x1),
|
||||
jacobian(f_lo, x1dot),
|
||||
jacobian(f_lo, u),
|
||||
jacobian(f_lo, z1),
|
||||
),
|
||||
],
|
||||
)
|
||||
|
||||
acados_ocp.model.get_matrices_fun = get_matrices_fun
|
||||
|
||||
size_gnsf_A = gnsf["A"].shape
|
||||
acados_ocp.dims.gnsf_nx1 = size_gnsf_A[1]
|
||||
acados_ocp.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
|
||||
acados_ocp.dims.gnsf_nuhat = max(phi_fun.size_in(1))
|
||||
acados_ocp.dims.gnsf_ny = max(phi_fun.size_in(0))
|
||||
acados_ocp.dims.gnsf_nout = max(phi_fun.size_out(0))
|
||||
|
||||
# # dim
|
||||
# model['dim_gnsf_nx1'] = gnsf['nx1']
|
||||
# model['dim_gnsf_nx2'] = gnsf['nx2']
|
||||
# model['dim_gnsf_nz1'] = gnsf['nz1']
|
||||
# model['dim_gnsf_nz2'] = gnsf['nz2']
|
||||
# model['dim_gnsf_nuhat'] = gnsf['nuhat']
|
||||
# model['dim_gnsf_ny'] = gnsf['ny']
|
||||
# model['dim_gnsf_nout'] = gnsf['n_out']
|
||||
|
||||
# # sym
|
||||
# model['sym_gnsf_y'] = gnsf['y']
|
||||
# model['sym_gnsf_uhat'] = gnsf['uhat']
|
||||
|
||||
# # data
|
||||
# model['dyn_gnsf_A'] = gnsf['A']
|
||||
# model['dyn_gnsf_A_LO'] = gnsf['A_LO']
|
||||
# model['dyn_gnsf_B'] = gnsf['B']
|
||||
# model['dyn_gnsf_B_LO'] = gnsf['B_LO']
|
||||
# model['dyn_gnsf_E'] = gnsf['E']
|
||||
# model['dyn_gnsf_E_LO'] = gnsf['E_LO']
|
||||
# model['dyn_gnsf_C'] = gnsf['C']
|
||||
# model['dyn_gnsf_c'] = gnsf['c']
|
||||
# model['dyn_gnsf_c_LO'] = gnsf['c_LO']
|
||||
# model['dyn_gnsf_L_x'] = gnsf['L_x']
|
||||
# model['dyn_gnsf_L_xdot'] = gnsf['L_xdot']
|
||||
# model['dyn_gnsf_L_z'] = gnsf['L_z']
|
||||
# model['dyn_gnsf_L_u'] = gnsf['L_u']
|
||||
# model['dyn_gnsf_idx_perm_x'] = gnsf['idx_perm_x']
|
||||
# model['dyn_gnsf_ipiv_x'] = gnsf['ipiv_x']
|
||||
# model['dyn_gnsf_idx_perm_z'] = gnsf['idx_perm_z']
|
||||
# model['dyn_gnsf_ipiv_z'] = gnsf['ipiv_z']
|
||||
# model['dyn_gnsf_idx_perm_f'] = gnsf['idx_perm_f']
|
||||
# model['dyn_gnsf_ipiv_f'] = gnsf['ipiv_f']
|
||||
|
||||
# # flags
|
||||
# model['dyn_gnsf_nontrivial_f_LO'] = gnsf['nontrivial_f_LO']
|
||||
# model['dyn_gnsf_purely_linear'] = gnsf['purely_linear']
|
||||
|
||||
# # casadi expr
|
||||
# model['dyn_gnsf_expr_phi'] = gnsf['phi_expr']
|
||||
# model['dyn_gnsf_expr_f_lo'] = gnsf['f_lo_expr']
|
||||
|
||||
return acados_ocp
|
||||
110
third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py
vendored
Normal file
110
third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py
vendored
Normal file
@@ -0,0 +1,110 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
|
||||
|
||||
from casadi import *
|
||||
from ..utils import casadi_length, is_empty
|
||||
|
||||
|
||||
def determine_input_nonlinearity_function(gnsf):
|
||||
|
||||
## Description
|
||||
# this function takes a structure gnsf and updates the matrices L_x,
|
||||
# L_xdot, L_z, L_u and CasADi vectors y, uhat of this structure as follows:
|
||||
|
||||
# given a CasADi expression phi_expr, which may depend on the variables
|
||||
# (x1, x1dot, z, u), this function determines a vector y (uhat) consisting
|
||||
# of all components of (x1, x1dot, z) (respectively u) that enter phi_expr.
|
||||
# Additionally matrices L_x, L_xdot, L_z, L_u are determined such that
|
||||
# y = L_x * x + L_xdot * xdot + L_z * z
|
||||
# uhat = L_u * u
|
||||
# Furthermore the dimensions ny, nuhat, n_out are updated
|
||||
|
||||
## y
|
||||
y = SX.sym('y', 0, 0)
|
||||
# components of x1
|
||||
for ii in range(gnsf["nx1"]):
|
||||
if which_depends(gnsf["phi_expr"], gnsf["x"][ii])[0]:
|
||||
y = vertcat(y, gnsf["x"][ii])
|
||||
# else:
|
||||
# x[ii] is not part of y
|
||||
# components of x1dot
|
||||
for ii in range(gnsf["nx1"]):
|
||||
if which_depends(gnsf["phi_expr"], gnsf["xdot"][ii])[0]:
|
||||
print(gnsf["phi_expr"], "depends on", gnsf["xdot"][ii])
|
||||
y = vertcat(y, gnsf["xdot"][ii])
|
||||
# else:
|
||||
# xdot[ii] is not part of y
|
||||
# components of z
|
||||
for ii in range(gnsf["nz1"]):
|
||||
if which_depends(gnsf["phi_expr"], gnsf["z"][ii])[0]:
|
||||
y = vertcat(y, gnsf["z"][ii])
|
||||
# else:
|
||||
# z[ii] is not part of y
|
||||
## uhat
|
||||
uhat = SX.sym('uhat', 0, 0)
|
||||
# components of u
|
||||
for ii in range(gnsf["nu"]):
|
||||
if which_depends(gnsf["phi_expr"], gnsf["u"][ii])[0]:
|
||||
uhat = vertcat(uhat, gnsf["u"][ii])
|
||||
# else:
|
||||
# u[ii] is not part of uhat
|
||||
## generate gnsf['phi_expr_fun']
|
||||
# linear input matrices
|
||||
if is_empty(y):
|
||||
gnsf["L_x"] = []
|
||||
gnsf["L_xdot"] = []
|
||||
gnsf["L_u"] = []
|
||||
gnsf["L_z"] = []
|
||||
else:
|
||||
dummy = SX.sym("dummy_input", 0)
|
||||
L_x_fun = Function(
|
||||
"L_x_fun", [dummy], [jacobian(y, gnsf["x"][range(gnsf["nx1"])])]
|
||||
)
|
||||
L_xdot_fun = Function(
|
||||
"L_xdot_fun", [dummy], [jacobian(y, gnsf["xdot"][range(gnsf["nx1"])])]
|
||||
)
|
||||
L_z_fun = Function(
|
||||
"L_z_fun", [dummy], [jacobian(y, gnsf["z"][range(gnsf["nz1"])])]
|
||||
)
|
||||
L_u_fun = Function("L_u_fun", [dummy], [jacobian(uhat, gnsf["u"])])
|
||||
|
||||
gnsf["L_x"] = L_x_fun(0).full()
|
||||
gnsf["L_xdot"] = L_xdot_fun(0).full()
|
||||
gnsf["L_u"] = L_u_fun(0).full()
|
||||
gnsf["L_z"] = L_z_fun(0).full()
|
||||
gnsf["y"] = y
|
||||
gnsf["uhat"] = uhat
|
||||
|
||||
gnsf["ny"] = casadi_length(y)
|
||||
gnsf["nuhat"] = casadi_length(uhat)
|
||||
gnsf["n_out"] = casadi_length(gnsf["phi_expr"])
|
||||
|
||||
return gnsf
|
||||
155
third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py
vendored
Normal file
155
third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py
vendored
Normal file
@@ -0,0 +1,155 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
from casadi import *
|
||||
import numpy as np
|
||||
from ..utils import casadi_length, idx_perm_to_ipiv
|
||||
from .determine_input_nonlinearity_function import determine_input_nonlinearity_function
|
||||
from .check_reformulation import check_reformulation
|
||||
|
||||
|
||||
def determine_trivial_gnsf_transcription(acados_ocp, print_info):
|
||||
## Description
|
||||
# this function takes a model of an implicit ODE/ index-1 DAE and sets up
|
||||
# an equivalent model in the GNSF structure, with empty linear output
|
||||
# system and trivial model matrices, i.e. A, B, E, c are zeros, and C is
|
||||
# eye. - no structure is exploited
|
||||
|
||||
model = acados_ocp.model
|
||||
# initial print
|
||||
print("*****************************************************************")
|
||||
print(" ")
|
||||
print(f"****** Restructuring {model.name} model ***********")
|
||||
print(" ")
|
||||
print("*****************************************************************")
|
||||
|
||||
# load model
|
||||
f_impl_expr = model.f_impl_expr
|
||||
|
||||
model_name_prefix = model.name
|
||||
|
||||
# x
|
||||
x = model.x
|
||||
nx = acados_ocp.dims.nx
|
||||
# check type
|
||||
if isinstance(x[0], SX):
|
||||
isSX = True
|
||||
else:
|
||||
print("GNSF detection only works for SX CasADi type!!!")
|
||||
import pdb
|
||||
|
||||
pdb.set_trace()
|
||||
# xdot
|
||||
xdot = model.xdot
|
||||
# u
|
||||
nu = acados_ocp.dims.nu
|
||||
if nu == 0:
|
||||
u = SX.sym("u", 0, 0)
|
||||
else:
|
||||
u = model.u
|
||||
|
||||
nz = acados_ocp.dims.nz
|
||||
if nz == 0:
|
||||
z = SX.sym("z", 0, 0)
|
||||
else:
|
||||
z = model.z
|
||||
|
||||
p = model.p
|
||||
nparam = acados_ocp.dims.np
|
||||
|
||||
# avoid SX of size 0x1
|
||||
if casadi_length(u) == 0:
|
||||
u = SX.sym("u", 0, 0)
|
||||
nu = 0
|
||||
## initialize gnsf struct
|
||||
# dimensions
|
||||
gnsf = {"nx": nx, "nu": nu, "nz": nz, "np": nparam}
|
||||
gnsf["nx1"] = nx
|
||||
gnsf["nx2"] = 0
|
||||
gnsf["nz1"] = nz
|
||||
gnsf["nz2"] = 0
|
||||
gnsf["nuhat"] = nu
|
||||
gnsf["ny"] = 2 * nx + nz
|
||||
|
||||
gnsf["phi_expr"] = f_impl_expr
|
||||
gnsf["A"] = np.zeros((nx + nz, nx))
|
||||
gnsf["B"] = np.zeros((nx + nz, nu))
|
||||
gnsf["E"] = np.zeros((nx + nz, nx + nz))
|
||||
gnsf["c"] = np.zeros((nx + nz, 1))
|
||||
gnsf["C"] = np.eye(nx + nz)
|
||||
gnsf["name"] = model_name_prefix
|
||||
|
||||
gnsf["x"] = x
|
||||
gnsf["xdot"] = xdot
|
||||
gnsf["z"] = z
|
||||
gnsf["u"] = u
|
||||
gnsf["p"] = p
|
||||
|
||||
gnsf = determine_input_nonlinearity_function(gnsf)
|
||||
|
||||
gnsf["A_LO"] = []
|
||||
gnsf["E_LO"] = []
|
||||
gnsf["B_LO"] = []
|
||||
gnsf["c_LO"] = []
|
||||
gnsf["f_lo_expr"] = []
|
||||
|
||||
# permutation
|
||||
gnsf["idx_perm_x"] = range(nx) # matlab-style)
|
||||
gnsf["ipiv_x"] = idx_perm_to_ipiv(gnsf["idx_perm_x"]) # blasfeo-style
|
||||
gnsf["idx_perm_z"] = range(nz)
|
||||
gnsf["ipiv_z"] = idx_perm_to_ipiv(gnsf["idx_perm_z"])
|
||||
gnsf["idx_perm_f"] = range((nx + nz))
|
||||
gnsf["ipiv_f"] = idx_perm_to_ipiv(gnsf["idx_perm_f"])
|
||||
|
||||
gnsf["nontrivial_f_LO"] = 0
|
||||
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
if print_info:
|
||||
print(f"Success: Set up equivalent GNSF model with trivial matrices")
|
||||
print(" ")
|
||||
if print_info:
|
||||
print(
|
||||
"-----------------------------------------------------------------------------------"
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
"reduced input ny of phi from ",
|
||||
str(2 * nx + nz),
|
||||
" to ",
|
||||
str(gnsf["ny"]),
|
||||
)
|
||||
print(
|
||||
"reduced input nuhat of phi from ", str(nu), " to ", str(gnsf["nuhat"])
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
"-----------------------------------------------------------------------------------"
|
||||
)
|
||||
return gnsf
|
||||
43
third_party/acados/acados_template/gnsf/matlab to python.md
vendored
Normal file
43
third_party/acados/acados_template/gnsf/matlab to python.md
vendored
Normal file
@@ -0,0 +1,43 @@
|
||||
# matlab to python
|
||||
|
||||
% -> #
|
||||
|
||||
; ->
|
||||
|
||||
from casadi import *
|
||||
->
|
||||
from casadi import *
|
||||
|
||||
|
||||
print\('(.*)'\)
|
||||
print('$1')
|
||||
|
||||
print\(\['(.*)'\]\)
|
||||
print(f'$1')
|
||||
|
||||
keyboard
|
||||
import pdb; pdb.set_trace()
|
||||
|
||||
|
||||
range((([^))]*))
|
||||
range($1)
|
||||
|
||||
\s*end
|
||||
->
|
||||
nothing
|
||||
|
||||
|
||||
if (.*)
|
||||
if $1:
|
||||
|
||||
else
|
||||
else:
|
||||
|
||||
num2str
|
||||
str
|
||||
|
||||
for ([a-z_]*) =
|
||||
for $1 in
|
||||
|
||||
length\(
|
||||
len(
|
||||
394
third_party/acados/acados_template/gnsf/reformulate_with_LOS.py
vendored
Normal file
394
third_party/acados/acados_template/gnsf/reformulate_with_LOS.py
vendored
Normal file
@@ -0,0 +1,394 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
|
||||
|
||||
from .determine_input_nonlinearity_function import determine_input_nonlinearity_function
|
||||
from .check_reformulation import check_reformulation
|
||||
from casadi import *
|
||||
from ..utils import casadi_length, idx_perm_to_ipiv, is_empty
|
||||
|
||||
|
||||
def reformulate_with_LOS(acados_ocp, gnsf, print_info):
|
||||
|
||||
## Description:
|
||||
# This function takes an intitial transcription of the implicit ODE model
|
||||
# "model" into "gnsf" and reformulates "gnsf" with a linear output system
|
||||
# (LOS), containing as many states of the model as possible.
|
||||
# Therefore it might be that the state vector and the implicit function
|
||||
# vector have to be reordered. This reordered model is part of the output,
|
||||
# namely reordered_model.
|
||||
|
||||
## import CasADi and load models
|
||||
model = acados_ocp.model
|
||||
|
||||
# symbolics
|
||||
x = gnsf["x"]
|
||||
xdot = gnsf["xdot"]
|
||||
u = gnsf["u"]
|
||||
z = gnsf["z"]
|
||||
|
||||
# dimensions
|
||||
nx = gnsf["nx"]
|
||||
nz = gnsf["nz"]
|
||||
|
||||
# get model matrices
|
||||
A = gnsf["A"]
|
||||
B = gnsf["B"]
|
||||
C = gnsf["C"]
|
||||
E = gnsf["E"]
|
||||
c = gnsf["c"]
|
||||
|
||||
A_LO = gnsf["A_LO"]
|
||||
|
||||
y = gnsf["y"]
|
||||
|
||||
phi_old = gnsf["phi_expr"]
|
||||
|
||||
if print_info:
|
||||
print(" ")
|
||||
print("=================================================================")
|
||||
print(" ")
|
||||
print("================ Detect Linear Output System ===============")
|
||||
print(" ")
|
||||
print("=================================================================")
|
||||
print(" ")
|
||||
## build initial I_x1 and I_x2_candidates
|
||||
# I_xrange( all components of x for which either xii or xdot_ii enters y):
|
||||
# I_LOS_candidates: the remaining components
|
||||
|
||||
I_nsf_components = set()
|
||||
I_LOS_candidates = set()
|
||||
|
||||
if gnsf["ny"] > 0:
|
||||
for ii in range(nx):
|
||||
if which_depends(y, x[ii])[0] or which_depends(y, xdot[ii])[0]:
|
||||
# i.e. xii or xiidot are part of y, and enter phi_expr
|
||||
if print_info:
|
||||
print(f"x_{ii} is part of x1")
|
||||
I_nsf_components = set.union(I_nsf_components, set([ii]))
|
||||
else:
|
||||
# i.e. neither xii nor xiidot are part of y, i.e. enter phi_expr
|
||||
I_LOS_candidates = set.union(I_LOS_candidates, set([ii]))
|
||||
if print_info:
|
||||
print(" ")
|
||||
for ii in range(nz):
|
||||
if which_depends(y, z[ii])[0]:
|
||||
# i.e. xii or xiidot are part of y, and enter phi_expr
|
||||
if print_info:
|
||||
print(f"z_{ii} is part of x1")
|
||||
I_nsf_components = set.union(I_nsf_components, set([ii + nx]))
|
||||
else:
|
||||
# i.e. neither xii nor xiidot are part of y, i.e. enter phi_expr
|
||||
I_LOS_candidates = set.union(I_LOS_candidates, set([ii + nx]))
|
||||
else:
|
||||
I_LOS_candidates = set(range((nx + nz)))
|
||||
if print_info:
|
||||
print(" ")
|
||||
print(f"I_LOS_candidates {I_LOS_candidates}")
|
||||
new_nsf_components = I_nsf_components
|
||||
I_nsf_eq = set([])
|
||||
unsorted_dyn = set(range(nx + nz))
|
||||
xdot_z = vertcat(xdot, z)
|
||||
|
||||
## determine components of Linear Output System
|
||||
# determine maximal index set I_x2
|
||||
# such that the components x(I_x2) can be written as a LOS
|
||||
Eq_map = []
|
||||
while True:
|
||||
## find equations corresponding to new_nsf_components
|
||||
for ii in new_nsf_components:
|
||||
current_var = xdot_z[ii]
|
||||
var_name = current_var.name
|
||||
|
||||
# print( unsorted_dyn)
|
||||
# print("np.nonzero(E[:,ii])[0]",np.nonzero(E[:,ii])[0])
|
||||
I_eq = set.intersection(set(np.nonzero(E[:, ii])[0]), unsorted_dyn)
|
||||
if len(I_eq) == 1:
|
||||
i_eq = I_eq.pop()
|
||||
if print_info:
|
||||
print(f"component {i_eq} is associated with state {ii}")
|
||||
elif len(I_eq) > 1: # x_ii_dot occurs in more than 1 eq linearly
|
||||
# find the equation with least linear dependencies on
|
||||
# I_LOS_cancidates
|
||||
number_of_eq = 0
|
||||
candidate_dependencies = np.zeros(len(I_eq), 1)
|
||||
I_x2_candidates = set.intersection(I_LOS_candidates, set(range(nx)))
|
||||
for eq in I_eq:
|
||||
depending_candidates = set.union(
|
||||
np.nonzero(E[eq, I_LOS_candidates])[0],
|
||||
np.nonzero(A[eq, I_x2_candidates])[0],
|
||||
)
|
||||
candidate_dependencies[number_of_eq] = +len(depending_candidates)
|
||||
number_of_eq += 1
|
||||
number_of_eq = np.argmin(candidate_dependencies)
|
||||
i_eq = I_eq[number_of_eq]
|
||||
else: ## x_ii_dot does not occur linearly in any of the unsorted dynamics
|
||||
for j in unsorted_dyn:
|
||||
phi_eq_j = gnsf["phi_expr"][np.nonzero(C[j, :])[0]]
|
||||
if which_depends(phi_eq_j, xdot_z(ii))[0]:
|
||||
I_eq = set.union(I_eq, j)
|
||||
if is_empty(I_eq):
|
||||
I_eq = unsorted_dyn
|
||||
# find the equation with least linear dependencies on I_LOS_cancidates
|
||||
number_of_eq = 0
|
||||
candidate_dependencies = np.zeros(len(I_eq), 1)
|
||||
I_x2_candidates = set.intersection(I_LOS_candidates, set(range(nx)))
|
||||
for eq in I_eq:
|
||||
depending_candidates = set.union(
|
||||
np.nonzero(E[eq, I_LOS_candidates])[0],
|
||||
np.nonzero(A[eq, I_x2_candidates])[0],
|
||||
)
|
||||
candidate_dependencies[number_of_eq] = +len(depending_candidates)
|
||||
number_of_eq += 1
|
||||
number_of_eq = np.argmin(candidate_dependencies)
|
||||
i_eq = I_eq[number_of_eq]
|
||||
## add 1 * [xdot,z](ii) to both sides of i_eq
|
||||
if print_info:
|
||||
print(
|
||||
"adding 1 * ",
|
||||
var_name,
|
||||
" to both sides of equation ",
|
||||
i_eq,
|
||||
".",
|
||||
)
|
||||
gnsf["E"][i_eq, ii] = 1
|
||||
i_phi = np.nonzero(gnsf["C"][i_eq, :])
|
||||
if is_empty(i_phi):
|
||||
i_phi = len(gnsf["phi_expr"]) + 1
|
||||
gnsf["C"][i_eq, i_phi] = 1 # add column to C with 1 entry
|
||||
gnsf["phi_expr"] = vertcat(gnsf["phi_expr"], 0)
|
||||
gnsf["phi_expr"][i_phi] = (
|
||||
gnsf["phi_expr"](i_phi)
|
||||
+ gnsf["E"][i_eq, ii] / gnsf["C"][i_eq, i_phi] * xdot_z[ii]
|
||||
)
|
||||
if print_info:
|
||||
print(
|
||||
"detected equation ",
|
||||
i_eq,
|
||||
" to correspond to variable ",
|
||||
var_name,
|
||||
)
|
||||
I_nsf_eq = set.union(I_nsf_eq, {i_eq})
|
||||
# remove i_eq from unsorted_dyn
|
||||
unsorted_dyn.remove(i_eq)
|
||||
Eq_map.append([ii, i_eq])
|
||||
|
||||
## add components to I_x1
|
||||
for eq in I_nsf_eq:
|
||||
I_linear_dependence = set.union(
|
||||
set(np.nonzero(A[eq, :])[0]), set(np.nonzero(E[eq, :])[0])
|
||||
)
|
||||
I_nsf_components = set.union(I_linear_dependence, I_nsf_components)
|
||||
# I_nsf_components = I_nsf_components[:]
|
||||
|
||||
new_nsf_components = set.intersection(I_LOS_candidates, I_nsf_components)
|
||||
if is_empty(new_nsf_components):
|
||||
if print_info:
|
||||
print("new_nsf_components is empty")
|
||||
break
|
||||
# remove new_nsf_components from candidates
|
||||
I_LOS_candidates = set.difference(I_LOS_candidates, new_nsf_components)
|
||||
if not is_empty(Eq_map):
|
||||
# [~, new_eq_order] = sort(Eq_map(1,:))
|
||||
# I_nsf_eq = Eq_map(2, new_eq_order )
|
||||
for count, m in enumerate(Eq_map):
|
||||
m.append(count)
|
||||
sorted(Eq_map, key=lambda x: x[1])
|
||||
new_eq_order = [m[2] for m in Eq_map]
|
||||
Eq_map = [Eq_map[i] for i in new_eq_order]
|
||||
I_nsf_eq = [m[1] for m in Eq_map]
|
||||
|
||||
else:
|
||||
I_nsf_eq = []
|
||||
|
||||
I_LOS_components = I_LOS_candidates
|
||||
I_LOS_eq = sorted(set.difference(set(range(nx + nz)), I_nsf_eq))
|
||||
I_nsf_eq = sorted(I_nsf_eq)
|
||||
|
||||
I_x1 = set.intersection(I_nsf_components, set(range(nx)))
|
||||
I_z1 = set.intersection(I_nsf_components, set(range(nx, nx + nz)))
|
||||
I_z1 = set([i - nx for i in I_z1])
|
||||
|
||||
I_x2 = set.intersection(I_LOS_components, set(range(nx)))
|
||||
I_z2 = set.intersection(I_LOS_components, set(range(nx, nx + nz)))
|
||||
I_z2 = set([i - nx for i in I_z2])
|
||||
|
||||
if print_info:
|
||||
print(f"I_x1 {I_x1}, I_x2 {I_x2}")
|
||||
|
||||
## permute x, xdot
|
||||
if is_empty(I_x1):
|
||||
x1 = []
|
||||
x1dot = []
|
||||
else:
|
||||
x1 = x[list(I_x1)]
|
||||
x1dot = xdot[list(I_x1)]
|
||||
if is_empty(I_x2):
|
||||
x2 = []
|
||||
x2dot = []
|
||||
else:
|
||||
x2 = x[list(I_x2)]
|
||||
x2dot = xdot[list(I_x2)]
|
||||
if is_empty(I_z1):
|
||||
z1 = []
|
||||
else:
|
||||
z1 = z(I_z1)
|
||||
if is_empty(I_z2):
|
||||
z2 = []
|
||||
else:
|
||||
z2 = z[list(I_z2)]
|
||||
|
||||
I_x1 = sorted(I_x1)
|
||||
I_x2 = sorted(I_x2)
|
||||
I_z1 = sorted(I_z1)
|
||||
I_z2 = sorted(I_z2)
|
||||
gnsf["xdot"] = vertcat(x1dot, x2dot)
|
||||
gnsf["x"] = vertcat(x1, x2)
|
||||
gnsf["z"] = vertcat(z1, z2)
|
||||
gnsf["nx1"] = len(I_x1)
|
||||
gnsf["nx2"] = len(I_x2)
|
||||
gnsf["nz1"] = len(I_z1)
|
||||
gnsf["nz2"] = len(I_z2)
|
||||
|
||||
# store permutations
|
||||
gnsf["idx_perm_x"] = I_x1 + I_x2
|
||||
gnsf["ipiv_x"] = idx_perm_to_ipiv(gnsf["idx_perm_x"])
|
||||
gnsf["idx_perm_z"] = I_z1 + I_z2
|
||||
gnsf["ipiv_z"] = idx_perm_to_ipiv(gnsf["idx_perm_z"])
|
||||
gnsf["idx_perm_f"] = I_nsf_eq + I_LOS_eq
|
||||
gnsf["ipiv_f"] = idx_perm_to_ipiv(gnsf["idx_perm_f"])
|
||||
|
||||
f_LO = SX.sym("f_LO", 0, 0)
|
||||
|
||||
## rewrite I_LOS_eq as LOS
|
||||
if gnsf["n_out"] == 0:
|
||||
C_phi = np.zeros(gnsf["nx"] + gnsf["nz"], 1)
|
||||
else:
|
||||
C_phi = C @ phi_old
|
||||
if gnsf["nx1"] == 0:
|
||||
Ax1 = np.zeros(gnsf["nx"] + gnsf["nz"], 1)
|
||||
else:
|
||||
Ax1 = A[:, sorted(I_x1)] @ x1
|
||||
if gnsf["nx1"] + gnsf["nz1"] == 0:
|
||||
lhs_nsf = np.zeros(gnsf["nx"] + gnsf["nz"], 1)
|
||||
else:
|
||||
lhs_nsf = E[:, sorted(I_nsf_components)] @ vertcat(x1, z1)
|
||||
n_LO = len(I_LOS_eq)
|
||||
B_LO = np.zeros((n_LO, gnsf["nu"]))
|
||||
A_LO = np.zeros((gnsf["nx2"] + gnsf["nz2"], gnsf["nx2"]))
|
||||
E_LO = np.zeros((n_LO, n_LO))
|
||||
c_LO = np.zeros((n_LO, 1))
|
||||
|
||||
I_LOS_eq = list(I_LOS_eq)
|
||||
for eq in I_LOS_eq:
|
||||
i_LO = I_LOS_eq.index(eq)
|
||||
f_LO = vertcat(f_LO, Ax1[eq] + C_phi[eq] - lhs_nsf[eq])
|
||||
print(f"eq {eq} I_LOS_components {I_LOS_components}, i_LO {i_LO}, f_LO {f_LO}")
|
||||
E_LO[i_LO, :] = E[eq, sorted(I_LOS_components)]
|
||||
A_LO[i_LO, :] = A[eq, I_x2]
|
||||
c_LO[i_LO, :] = c[eq]
|
||||
B_LO[i_LO, :] = B[eq, :]
|
||||
if casadi_length(f_LO) == 0:
|
||||
f_LO = SX.zeros((gnsf["nx2"] + gnsf["nz2"], 1))
|
||||
f_LO = simplify(f_LO)
|
||||
gnsf["A_LO"] = A_LO
|
||||
gnsf["E_LO"] = E_LO
|
||||
gnsf["B_LO"] = B_LO
|
||||
gnsf["c_LO"] = c_LO
|
||||
gnsf["f_lo_expr"] = f_LO
|
||||
|
||||
## remove I_LOS_eq from NSF type system
|
||||
gnsf["A"] = gnsf["A"][np.ix_(sorted(I_nsf_eq), sorted(I_x1))]
|
||||
gnsf["B"] = gnsf["B"][sorted(I_nsf_eq), :]
|
||||
gnsf["C"] = gnsf["C"][sorted(I_nsf_eq), :]
|
||||
gnsf["E"] = gnsf["E"][np.ix_(sorted(I_nsf_eq), sorted(I_nsf_components))]
|
||||
gnsf["c"] = gnsf["c"][sorted(I_nsf_eq), :]
|
||||
|
||||
## reduce phi, C
|
||||
I_nonzero = []
|
||||
for ii in range(gnsf["C"].shape[1]): # n_colums of C:
|
||||
print(f"ii {ii}")
|
||||
if not all(gnsf["C"][:, ii] == 0): # if column ~= 0
|
||||
I_nonzero.append(ii)
|
||||
gnsf["C"] = gnsf["C"][:, I_nonzero]
|
||||
gnsf["phi_expr"] = gnsf["phi_expr"][I_nonzero]
|
||||
|
||||
gnsf = determine_input_nonlinearity_function(gnsf)
|
||||
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
gnsf["nontrivial_f_LO"] = 0
|
||||
if not is_empty(gnsf["f_lo_expr"]):
|
||||
for ii in range(casadi_length(gnsf["f_lo_expr"])):
|
||||
fii = gnsf["f_lo_expr"][ii]
|
||||
if not fii.is_zero():
|
||||
gnsf["nontrivial_f_LO"] = 1
|
||||
if not gnsf["nontrivial_f_LO"] and print_info:
|
||||
print("f_LO is fully trivial (== 0)")
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
if print_info:
|
||||
print("")
|
||||
print(
|
||||
"---------------------------------------------------------------------------------"
|
||||
)
|
||||
print(
|
||||
"------------- Success: Linear Output System (LOS) detected ----------------------"
|
||||
)
|
||||
print(
|
||||
"---------------------------------------------------------------------------------"
|
||||
)
|
||||
print("")
|
||||
print(
|
||||
"==>> moved ",
|
||||
gnsf["nx2"],
|
||||
"differential states and ",
|
||||
gnsf["nz2"],
|
||||
" algebraic variables to the Linear Output System",
|
||||
)
|
||||
print(
|
||||
"==>> recuced output dimension of phi from ",
|
||||
casadi_length(phi_old),
|
||||
" to ",
|
||||
casadi_length(gnsf["phi_expr"]),
|
||||
)
|
||||
print(" ")
|
||||
print("Matrices defining the LOS read as")
|
||||
print(" ")
|
||||
print("E_LO =")
|
||||
print(gnsf["E_LO"])
|
||||
print("A_LO =")
|
||||
print(gnsf["A_LO"])
|
||||
print("B_LO =")
|
||||
print(gnsf["B_LO"])
|
||||
print("c_LO =")
|
||||
print(gnsf["c_LO"])
|
||||
|
||||
return gnsf
|
||||
167
third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py
vendored
Normal file
167
third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py
vendored
Normal file
@@ -0,0 +1,167 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
|
||||
|
||||
from casadi import *
|
||||
from .determine_input_nonlinearity_function import determine_input_nonlinearity_function
|
||||
from .check_reformulation import check_reformulation
|
||||
|
||||
|
||||
def reformulate_with_invertible_E_mat(gnsf, model, print_info):
|
||||
## Description
|
||||
# this function checks that the necessary condition to apply the gnsf
|
||||
# structure exploiting integrator to a model, namely that the matrices E11,
|
||||
# E22 are invertible holds.
|
||||
# if this is not the case, it will make these matrices invertible and add:
|
||||
# corresponding terms, to the term C * phi, such that the obtained model is
|
||||
# still equivalent
|
||||
|
||||
# check invertibility of E11, E22 and reformulate if needed:
|
||||
ind_11 = range(gnsf["nx1"])
|
||||
ind_22 = range(gnsf["nx1"], gnsf["nx1"] + gnsf["nz1"])
|
||||
|
||||
if print_info:
|
||||
print(" ")
|
||||
print("----------------------------------------------------")
|
||||
print("checking rank of E11 and E22")
|
||||
print("----------------------------------------------------")
|
||||
## check if E11, E22 are invertible:
|
||||
z_check = False
|
||||
if gnsf["nz1"] > 0:
|
||||
z_check = (
|
||||
np.linalg.matrix_rank(gnsf["E"][np.ix_(ind_22, ind_22)]) != gnsf["nz1"]
|
||||
)
|
||||
|
||||
if (
|
||||
np.linalg.matrix_rank(gnsf["E"][np.ix_(ind_11, ind_11)]) != gnsf["nx1"]
|
||||
or z_check
|
||||
):
|
||||
# print warning (always)
|
||||
print(f"the rank of E11 or E22 is not full after the reformulation")
|
||||
print("")
|
||||
print(
|
||||
f"the script will try to reformulate the model with an invertible matrix instead"
|
||||
)
|
||||
print(
|
||||
f"NOTE: this feature is based on a heuristic, it should be used with care!!!"
|
||||
)
|
||||
|
||||
## load models
|
||||
xdot = gnsf["xdot"]
|
||||
z = gnsf["z"]
|
||||
|
||||
# # GNSF
|
||||
# get dimensions
|
||||
nx1 = gnsf["nx1"]
|
||||
x1dot = xdot[range(nx1)]
|
||||
|
||||
k = vertcat(x1dot, z)
|
||||
for i in [1, 2]:
|
||||
if i == 1:
|
||||
ind = range(gnsf["nx1"])
|
||||
else:
|
||||
ind = range(gnsf["nx1"], gnsf["nx1"] + gnsf["nz1"])
|
||||
mat = gnsf["E"][np.ix_(ind, ind)]
|
||||
import pdb
|
||||
|
||||
pdb.set_trace()
|
||||
while np.linalg.matrix_rank(mat) < len(ind):
|
||||
# import pdb; pdb.set_trace()
|
||||
if print_info:
|
||||
print(" ")
|
||||
print(f"the rank of E", str(i), str(i), " is not full")
|
||||
print(
|
||||
f"the algorithm will try to reformulate the model with an invertible matrix instead"
|
||||
)
|
||||
print(
|
||||
f"NOTE: this feature is not super stable and might need more testing!!!!!!"
|
||||
)
|
||||
for sub_max in ind:
|
||||
sub_ind = range(min(ind), sub_max)
|
||||
# regard the submatrix mat(sub_ind, sub_ind)
|
||||
sub_mat = gnsf["E"][sub_ind, sub_ind]
|
||||
if np.linalg.matrix_rank(sub_mat) < len(sub_ind):
|
||||
# reformulate the model by adding a 1 to last diagonal
|
||||
# element and changing rhs respectively.
|
||||
gnsf["E"][sub_max, sub_max] = gnsf["E"][sub_max, sub_max] + 1
|
||||
# this means adding the term 1 * k(sub_max) to the sub_max
|
||||
# row of the l.h.s
|
||||
if len(np.nonzero(gnsf["C"][sub_max, :])[0]) == 0:
|
||||
# if isempty(find(gnsf['C'](sub_max,:), 1)):
|
||||
# add new nonlinearity entry
|
||||
gnsf["C"][sub_max, gnsf["n_out"] + 1] = 1
|
||||
gnsf["phi_expr"] = vertcat(gnsf["phi_expr"], k[sub_max])
|
||||
else:
|
||||
ind_f = np.nonzero(gnsf["C"][sub_max, :])[0]
|
||||
if len(ind_f) != 1:
|
||||
raise Exception("C is assumed to be a selection matrix")
|
||||
else:
|
||||
ind_f = ind_f[0]
|
||||
# add term to corresponding nonlinearity entry
|
||||
# note: herbey we assume that C is a selection matrix,
|
||||
# i.e. gnsf['phi_expr'](ind_f) is only entering one equation
|
||||
|
||||
gnsf["phi_expr"][ind_f] = (
|
||||
gnsf["phi_expr"][ind_f]
|
||||
+ k[sub_max] / gnsf["C"][sub_max, ind_f]
|
||||
)
|
||||
gnsf = determine_input_nonlinearity_function(gnsf)
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
print("successfully reformulated the model with invertible matrices E11, E22")
|
||||
else:
|
||||
if print_info:
|
||||
print(" ")
|
||||
print(
|
||||
"the rank of both E11 and E22 is naturally full after the reformulation "
|
||||
)
|
||||
print("==> model reformulation finished")
|
||||
print(" ")
|
||||
if (gnsf['nx2'] > 0 or gnsf['nz2'] > 0) and det(gnsf["E_LO"]) == 0:
|
||||
print(
|
||||
"_______________________________________________________________________________________________________"
|
||||
)
|
||||
print(" ")
|
||||
print("TAKE CARE ")
|
||||
print("E_LO matrix is NOT regular after automatic transcription!")
|
||||
print("->> this means the model CANNOT be used with the gnsf integrator")
|
||||
print(
|
||||
"->> it probably means that one entry (of xdot or z) that was moved to the linear output type system"
|
||||
)
|
||||
print(" does not appear in the model at all (zero column in E_LO)")
|
||||
print(" OR: the columns of E_LO are linearly dependent ")
|
||||
print(" ")
|
||||
print(
|
||||
" SOLUTIONs: a) go through your model & check equations the method wanted to move to LOS"
|
||||
)
|
||||
print(" b) deactivate the detect_LOS option")
|
||||
print(
|
||||
"_______________________________________________________________________________________________________"
|
||||
)
|
||||
return gnsf
|
||||
174
third_party/acados/acados_template/gnsf/structure_detection_print_summary.py
vendored
Normal file
174
third_party/acados/acados_template/gnsf/structure_detection_print_summary.py
vendored
Normal file
@@ -0,0 +1,174 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
|
||||
|
||||
from casadi import n_nodes
|
||||
import numpy as np
|
||||
|
||||
|
||||
def structure_detection_print_summary(gnsf, acados_ocp):
|
||||
|
||||
## Description
|
||||
# this function prints the most important info after determining a GNSF
|
||||
# reformulation of the implicit model "initial_model" into "gnsf", which is
|
||||
# equivalent to the "reordered_model".
|
||||
model = acados_ocp.model
|
||||
# # GNSF
|
||||
# get dimensions
|
||||
nx = gnsf["nx"]
|
||||
nu = gnsf["nu"]
|
||||
nz = gnsf["nz"]
|
||||
|
||||
nx1 = gnsf["nx1"]
|
||||
nx2 = gnsf["nx2"]
|
||||
|
||||
nz1 = gnsf["nz1"]
|
||||
nz2 = gnsf["nz2"]
|
||||
|
||||
# np = gnsf['np']
|
||||
n_out = gnsf["n_out"]
|
||||
ny = gnsf["ny"]
|
||||
nuhat = gnsf["nuhat"]
|
||||
|
||||
#
|
||||
f_impl_expr = model.f_impl_expr
|
||||
n_nodes_initial = n_nodes(model.f_impl_expr)
|
||||
# x_old = model.x
|
||||
# f_impl_old = model.f_impl_expr
|
||||
|
||||
x = gnsf["x"]
|
||||
z = gnsf["z"]
|
||||
|
||||
phi_current = gnsf["phi_expr"]
|
||||
|
||||
## PRINT SUMMARY -- STRUCHTRE DETECTION
|
||||
print(" ")
|
||||
print(
|
||||
"*********************************************************************************************"
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
"****************** SUCCESS: GNSF STRUCTURE DETECTION COMPLETE !!! ***************"
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
"*********************************************************************************************"
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
f"========================= STRUCTURE DETECTION SUMMARY ===================================="
|
||||
)
|
||||
print(" ")
|
||||
print("-------- Nonlinear Static Feedback type system --------")
|
||||
print(" ")
|
||||
print(" successfully transcribed dynamic system model into GNSF structure ")
|
||||
print(" ")
|
||||
print(
|
||||
"reduced dimension of nonlinearity phi from ",
|
||||
str(nx + nz),
|
||||
" to ",
|
||||
str(gnsf["n_out"]),
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
"reduced input dimension of nonlinearity phi from ",
|
||||
2 * nx + nz + nu,
|
||||
" to ",
|
||||
gnsf["ny"] + gnsf["nuhat"],
|
||||
)
|
||||
print(" ")
|
||||
print(f"reduced number of nodes in CasADi expression of nonlinearity phi from {n_nodes_initial} to {n_nodes(phi_current)}\n")
|
||||
print("----------- Linear Output System (LOS) ---------------")
|
||||
if nx2 + nz2 > 0:
|
||||
print(" ")
|
||||
print(f"introduced Linear Output System of size ", str(nx2 + nz2))
|
||||
print(" ")
|
||||
if nx2 > 0:
|
||||
print("consisting of the states:")
|
||||
print(" ")
|
||||
print(x[range(nx1, nx)])
|
||||
print(" ")
|
||||
if nz2 > 0:
|
||||
print("and algebraic variables:")
|
||||
print(" ")
|
||||
print(z[range(nz1, nz)])
|
||||
print(" ")
|
||||
if gnsf["purely_linear"] == 1:
|
||||
print(" ")
|
||||
print("Model is fully linear!")
|
||||
print(" ")
|
||||
if not all(gnsf["idx_perm_x"] == np.array(range(nx))):
|
||||
print(" ")
|
||||
print(
|
||||
"--------------------------------------------------------------------------------------------------"
|
||||
)
|
||||
print(
|
||||
"NOTE: permuted differential state vector x, such that x_gnsf = x(idx_perm_x) with idx_perm_x ="
|
||||
)
|
||||
print(" ")
|
||||
print(gnsf["idx_perm_x"])
|
||||
if nz != 0 and not all(gnsf["idx_perm_z"] == np.array(range(nz))):
|
||||
print(" ")
|
||||
print(
|
||||
"--------------------------------------------------------------------------------------------------"
|
||||
)
|
||||
print(
|
||||
"NOTE: permuted algebraic state vector z, such that z_gnsf = z(idx_perm_z) with idx_perm_z ="
|
||||
)
|
||||
print(" ")
|
||||
print(gnsf["idx_perm_z"])
|
||||
if not all(gnsf["idx_perm_f"] == np.array(range(nx + nz))):
|
||||
print(" ")
|
||||
print(
|
||||
"--------------------------------------------------------------------------------------------------"
|
||||
)
|
||||
print(
|
||||
"NOTE: permuted rhs expression vector f, such that f_gnsf = f(idx_perm_f) with idx_perm_f ="
|
||||
)
|
||||
print(" ")
|
||||
print(gnsf["idx_perm_f"])
|
||||
## print GNSF dimensions
|
||||
print(
|
||||
"--------------------------------------------------------------------------------------------------------"
|
||||
)
|
||||
print(" ")
|
||||
print("The dimensions of the GNSF reformulated model read as:")
|
||||
print(" ")
|
||||
# T_dim = table(nx, nu, nz, np, nx1, nz1, n_out, ny, nuhat)
|
||||
# print( T_dim )
|
||||
print(f"nx ", {nx})
|
||||
print(f"nu ", {nu})
|
||||
print(f"nz ", {nz})
|
||||
# print(f"np ", {np})
|
||||
print(f"nx1 ", {nx1})
|
||||
print(f"nz1 ", {nz1})
|
||||
print(f"n_out ", {n_out})
|
||||
print(f"ny ", {ny})
|
||||
print(f"nuhat ", {nuhat})
|
||||
44
third_party/acados/acados_template/simulink_default_opts.json
vendored
Normal file
44
third_party/acados/acados_template/simulink_default_opts.json
vendored
Normal file
@@ -0,0 +1,44 @@
|
||||
{
|
||||
"outputs": {
|
||||
"u0": 1,
|
||||
"utraj": 0,
|
||||
"xtraj": 0,
|
||||
"solver_status": 1,
|
||||
"cost_value": 0,
|
||||
"KKT_residual": 1,
|
||||
"KKT_residuals": 0,
|
||||
"x1": 1,
|
||||
"CPU_time": 1,
|
||||
"CPU_time_sim": 0,
|
||||
"CPU_time_qp": 0,
|
||||
"CPU_time_lin": 0,
|
||||
"sqp_iter": 1
|
||||
},
|
||||
"inputs": {
|
||||
"lbx_0": 1,
|
||||
"ubx_0": 1,
|
||||
"parameter_traj": 1,
|
||||
"y_ref_0": 1,
|
||||
"y_ref": 1,
|
||||
"y_ref_e": 1,
|
||||
"lbx": 1,
|
||||
"ubx": 1,
|
||||
"lbx_e": 1,
|
||||
"ubx_e": 1,
|
||||
"lbu": 1,
|
||||
"ubu": 1,
|
||||
"lg": 1,
|
||||
"ug": 1,
|
||||
"lh": 1,
|
||||
"uh": 1,
|
||||
"lh_e": 1,
|
||||
"uh_e": 1,
|
||||
"cost_W_0": 0,
|
||||
"cost_W": 0,
|
||||
"cost_W_e": 0,
|
||||
"reset_solver": 0,
|
||||
"x_init": 0,
|
||||
"u_init": 0
|
||||
},
|
||||
"samplingtime": "t0"
|
||||
}
|
||||
442
third_party/acados/acados_template/utils.py
vendored
Normal file
442
third_party/acados/acados_template/utils.py
vendored
Normal file
@@ -0,0 +1,442 @@
|
||||
#
|
||||
# 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.;
|
||||
#
|
||||
|
||||
import os, sys, json
|
||||
import urllib.request
|
||||
import shutil
|
||||
import numpy as np
|
||||
from casadi import SX, MX, DM, Function, CasadiMeta
|
||||
|
||||
ALLOWED_CASADI_VERSIONS = ('3.5.6', '3.5.5', '3.5.4', '3.5.3', '3.5.2', '3.5.1', '3.4.5', '3.4.0')
|
||||
|
||||
TERA_VERSION = "0.0.34"
|
||||
|
||||
PLATFORM2TERA = {
|
||||
"linux": "linux",
|
||||
"darwin": "osx",
|
||||
"win32": "windows"
|
||||
}
|
||||
|
||||
|
||||
def get_acados_path():
|
||||
ACADOS_PATH = os.environ.get('ACADOS_SOURCE_DIR')
|
||||
if not ACADOS_PATH:
|
||||
acados_template_path = os.path.dirname(os.path.abspath(__file__))
|
||||
vendored_acados_path = os.path.realpath(os.path.join(acados_template_path, '..'))
|
||||
upstream_acados_path = os.path.realpath(os.path.join(acados_template_path, '..', '..', '..'))
|
||||
if os.path.isfile(os.path.join(vendored_acados_path, 'acados_template', 'acados_layout.json')):
|
||||
ACADOS_PATH = vendored_acados_path
|
||||
else:
|
||||
ACADOS_PATH = upstream_acados_path
|
||||
msg = 'Warning: Did not find environment variable ACADOS_SOURCE_DIR, '
|
||||
msg += 'guessed ACADOS_PATH to be {}.\n'.format(ACADOS_PATH)
|
||||
msg += 'Please export ACADOS_SOURCE_DIR to avoid this warning.'
|
||||
print(msg)
|
||||
return ACADOS_PATH
|
||||
|
||||
|
||||
def get_python_interface_path():
|
||||
ACADOS_PYTHON_INTERFACE_PATH = os.environ.get('ACADOS_PYTHON_INTERFACE_PATH')
|
||||
if not ACADOS_PYTHON_INTERFACE_PATH:
|
||||
vendored_interface_path = os.path.dirname(os.path.abspath(__file__))
|
||||
if os.path.isfile(os.path.join(vendored_interface_path, 'acados_layout.json')):
|
||||
ACADOS_PYTHON_INTERFACE_PATH = vendored_interface_path
|
||||
else:
|
||||
acados_path = get_acados_path()
|
||||
ACADOS_PYTHON_INTERFACE_PATH = os.path.join(acados_path, 'interfaces', 'acados_template', 'acados_template')
|
||||
return ACADOS_PYTHON_INTERFACE_PATH
|
||||
|
||||
|
||||
def get_tera_exec_path():
|
||||
TERA_PATH = os.environ.get('TERA_PATH')
|
||||
if not TERA_PATH:
|
||||
TERA_PATH = os.path.join(get_acados_path(), 'bin', 't_renderer')
|
||||
if os.name == 'nt':
|
||||
TERA_PATH += '.exe'
|
||||
return TERA_PATH
|
||||
|
||||
|
||||
def check_casadi_version():
|
||||
casadi_version = CasadiMeta.version()
|
||||
if casadi_version in ALLOWED_CASADI_VERSIONS:
|
||||
return
|
||||
else:
|
||||
msg = 'Warning: Please note that the following versions of CasADi are '
|
||||
msg += 'officially supported: {}.\n '.format(" or ".join(ALLOWED_CASADI_VERSIONS))
|
||||
msg += 'If there is an incompatibility with the CasADi generated code, '
|
||||
msg += 'please consider changing your CasADi version.\n'
|
||||
msg += 'Version {} currently in use.'.format(casadi_version)
|
||||
print(msg)
|
||||
|
||||
|
||||
def is_column(x):
|
||||
if isinstance(x, np.ndarray):
|
||||
if x.ndim == 1:
|
||||
return True
|
||||
elif x.ndim == 2 and x.shape[1] == 1:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
elif isinstance(x, (MX, SX, DM)):
|
||||
if x.shape[1] == 1:
|
||||
return True
|
||||
elif x.shape[0] == 0 and x.shape[1] == 0:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
elif x == None or x == []:
|
||||
return False
|
||||
else:
|
||||
raise Exception("is_column expects one of the following types: np.ndarray, casadi.MX, casadi.SX."
|
||||
+ " Got: " + str(type(x)))
|
||||
|
||||
|
||||
def is_empty(x):
|
||||
if isinstance(x, (MX, SX, DM)):
|
||||
return x.is_empty()
|
||||
elif isinstance(x, np.ndarray):
|
||||
if np.prod(x.shape) == 0:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
elif x == None:
|
||||
return True
|
||||
elif isinstance(x, (set, list)):
|
||||
if len(x)==0:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
else:
|
||||
raise Exception("is_empty expects one of the following types: casadi.MX, casadi.SX, "
|
||||
+ "None, numpy array empty list, set. Got: " + str(type(x)))
|
||||
|
||||
|
||||
def casadi_length(x):
|
||||
if isinstance(x, (MX, SX, DM)):
|
||||
return int(np.prod(x.shape))
|
||||
else:
|
||||
raise Exception("casadi_length expects one of the following types: casadi.MX, casadi.SX."
|
||||
+ " Got: " + str(type(x)))
|
||||
|
||||
|
||||
def make_model_consistent(model):
|
||||
x = model.x
|
||||
xdot = model.xdot
|
||||
u = model.u
|
||||
z = model.z
|
||||
p = model.p
|
||||
|
||||
if isinstance(x, MX):
|
||||
symbol = MX.sym
|
||||
elif isinstance(x, SX):
|
||||
symbol = SX.sym
|
||||
else:
|
||||
raise Exception("model.x must be casadi.SX or casadi.MX, got {}".format(type(x)))
|
||||
|
||||
if is_empty(p):
|
||||
model.p = symbol('p', 0, 0)
|
||||
|
||||
if is_empty(z):
|
||||
model.z = symbol('z', 0, 0)
|
||||
|
||||
return model
|
||||
|
||||
def get_lib_ext():
|
||||
lib_ext = '.so'
|
||||
if sys.platform == 'darwin':
|
||||
lib_ext = '.dylib'
|
||||
elif os.name == 'nt':
|
||||
lib_ext = ''
|
||||
|
||||
return lib_ext
|
||||
|
||||
def get_tera():
|
||||
tera_path = get_tera_exec_path()
|
||||
acados_path = get_acados_path()
|
||||
|
||||
if os.path.exists(tera_path) and os.access(tera_path, os.X_OK):
|
||||
return tera_path
|
||||
|
||||
repo_url = "https://github.com/acados/tera_renderer/releases"
|
||||
url = "{}/download/v{}/t_renderer-v{}-{}".format(
|
||||
repo_url, TERA_VERSION, TERA_VERSION, PLATFORM2TERA[sys.platform])
|
||||
|
||||
manual_install = 'For manual installation follow these instructions:\n'
|
||||
manual_install += '1 Download binaries from {}\n'.format(url)
|
||||
manual_install += '2 Copy them in {}/bin\n'.format(acados_path)
|
||||
manual_install += '3 Strip the version and platform from the binaries: '
|
||||
manual_install += 'as t_renderer-v0.0.34-X -> t_renderer)\n'
|
||||
manual_install += '4 Enable execution privilege on the file "t_renderer" with:\n'
|
||||
manual_install += '"chmod +x {}"\n\n'.format(tera_path)
|
||||
|
||||
msg = "\n"
|
||||
msg += 'Tera template render executable not found, '
|
||||
msg += 'while looking in path:\n{}\n'.format(tera_path)
|
||||
msg += 'In order to be able to render the templates, '
|
||||
msg += 'you need to download the tera renderer binaries from:\n'
|
||||
msg += '{}\n\n'.format(repo_url)
|
||||
msg += 'Do you wish to set up Tera renderer automatically?\n'
|
||||
msg += 'y/N? (press y to download tera or any key for manual installation)\n'
|
||||
|
||||
if input(msg) == 'y':
|
||||
print("Dowloading {}".format(url))
|
||||
with urllib.request.urlopen(url) as response, open(tera_path, 'wb') as out_file:
|
||||
shutil.copyfileobj(response, out_file)
|
||||
print("Successfully downloaded t_renderer.")
|
||||
os.chmod(tera_path, 0o755)
|
||||
return tera_path
|
||||
|
||||
msg_cancel = "\nYou cancelled automatic download.\n\n"
|
||||
msg_cancel += manual_install
|
||||
msg_cancel += "Once installed re-run your script.\n\n"
|
||||
print(msg_cancel)
|
||||
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
def render_template(in_file, out_file, output_dir, json_path, template_glob=None):
|
||||
|
||||
acados_path = os.path.dirname(os.path.abspath(__file__))
|
||||
if template_glob is None:
|
||||
template_glob = os.path.join(acados_path, 'c_templates_tera', '**', '*')
|
||||
cwd = os.getcwd()
|
||||
|
||||
if not os.path.exists(output_dir):
|
||||
os.makedirs(output_dir)
|
||||
os.chdir(output_dir)
|
||||
|
||||
tera_path = get_tera()
|
||||
|
||||
# call tera as system cmd
|
||||
os_cmd = f"{tera_path} '{template_glob}' '{in_file}' '{json_path}' '{out_file}'"
|
||||
# Windows cmd.exe can not cope with '...', so use "..." instead:
|
||||
if os.name == 'nt':
|
||||
os_cmd = os_cmd.replace('\'', '\"')
|
||||
|
||||
status = os.system(os_cmd)
|
||||
if (status != 0):
|
||||
raise Exception(f'Rendering of {in_file} failed!\n\nAttempted to execute OS command:\n{os_cmd}\n\n')
|
||||
|
||||
os.chdir(cwd)
|
||||
|
||||
|
||||
## Conversion functions
|
||||
def make_object_json_dumpable(input):
|
||||
if isinstance(input, (np.ndarray)):
|
||||
return input.tolist()
|
||||
elif isinstance(input, (SX)):
|
||||
return input.serialize()
|
||||
elif isinstance(input, (MX)):
|
||||
# NOTE: MX expressions can not be serialized, only Functions.
|
||||
return input.__str__()
|
||||
elif isinstance(input, (DM)):
|
||||
return input.full()
|
||||
else:
|
||||
raise TypeError(f"Cannot make input of type {type(input)} dumpable.")
|
||||
|
||||
|
||||
def format_class_dict(d):
|
||||
"""
|
||||
removes the __ artifact from class to dict conversion
|
||||
"""
|
||||
out = {}
|
||||
for k, v in d.items():
|
||||
if isinstance(v, dict):
|
||||
v = format_class_dict(v)
|
||||
|
||||
out_key = k.split('__', 1)[-1]
|
||||
out[k.replace(k, out_key)] = v
|
||||
return out
|
||||
|
||||
|
||||
def get_ocp_nlp_layout() -> dict:
|
||||
python_interface_path = get_python_interface_path()
|
||||
abs_path = os.path.join(python_interface_path, 'acados_layout.json')
|
||||
with open(abs_path, 'r') as f:
|
||||
ocp_nlp_layout = json.load(f)
|
||||
return ocp_nlp_layout
|
||||
|
||||
|
||||
def get_default_simulink_opts() -> dict:
|
||||
python_interface_path = get_python_interface_path()
|
||||
abs_path = os.path.join(python_interface_path, 'simulink_default_opts.json')
|
||||
with open(abs_path, 'r') as f:
|
||||
simulink_opts = json.load(f)
|
||||
return simulink_opts
|
||||
|
||||
|
||||
def J_to_idx(J):
|
||||
nrows = J.shape[0]
|
||||
idx = np.zeros((nrows, ))
|
||||
for i in range(nrows):
|
||||
this_idx = np.nonzero(J[i,:])[0]
|
||||
if len(this_idx) != 1:
|
||||
raise Exception('Invalid J matrix structure detected, ' \
|
||||
'must contain one nonzero element per row.')
|
||||
if this_idx.size > 0 and J[i,this_idx[0]] != 1:
|
||||
raise Exception('J matrices can only contain 1s.')
|
||||
idx[i] = this_idx[0]
|
||||
return idx
|
||||
|
||||
|
||||
def J_to_idx_slack(J):
|
||||
nrows = J.shape[0]
|
||||
ncol = J.shape[1]
|
||||
idx = np.zeros((ncol, ))
|
||||
i_idx = 0
|
||||
for i in range(nrows):
|
||||
this_idx = np.nonzero(J[i,:])[0]
|
||||
if len(this_idx) == 1:
|
||||
idx[i_idx] = i
|
||||
i_idx = i_idx + 1
|
||||
elif len(this_idx) > 1:
|
||||
raise Exception('J_to_idx_slack: Invalid J matrix. ' \
|
||||
'Found more than one nonzero in row ' + str(i))
|
||||
if this_idx.size > 0 and J[i,this_idx[0]] != 1:
|
||||
raise Exception('J_to_idx_slack: J matrices can only contain 1s, ' \
|
||||
'got J(' + str(i) + ', ' + str(this_idx[0]) + ') = ' + str(J[i,this_idx[0]]) )
|
||||
if not i_idx == ncol:
|
||||
raise Exception('J_to_idx_slack: J must contain a 1 in every column!')
|
||||
return idx
|
||||
|
||||
|
||||
def acados_dae_model_json_dump(model):
|
||||
|
||||
# load model
|
||||
x = model.x
|
||||
xdot = model.xdot
|
||||
u = model.u
|
||||
z = model.z
|
||||
p = model.p
|
||||
|
||||
f_impl = model.f_impl_expr
|
||||
model_name = model.name
|
||||
|
||||
# create struct with impl_dae_fun, casadi_version
|
||||
fun_name = model_name + '_impl_dae_fun'
|
||||
impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl])
|
||||
|
||||
casadi_version = CasadiMeta.version()
|
||||
str_impl_dae_fun = impl_dae_fun.serialize()
|
||||
|
||||
dae_dict = {"str_impl_dae_fun": str_impl_dae_fun, "casadi_version": casadi_version}
|
||||
|
||||
# dump
|
||||
json_file = model_name + '_acados_dae.json'
|
||||
with open(json_file, 'w') as f:
|
||||
json.dump(dae_dict, f, default=make_object_json_dumpable, indent=4, sort_keys=True)
|
||||
print("dumped ", model_name, " dae to file:", json_file, "\n")
|
||||
|
||||
|
||||
def set_up_imported_gnsf_model(acados_ocp):
|
||||
|
||||
gnsf = acados_ocp.gnsf_model
|
||||
|
||||
# check CasADi version
|
||||
# dump_casadi_version = gnsf['casadi_version']
|
||||
# casadi_version = CasadiMeta.version()
|
||||
|
||||
# if not casadi_version == dump_casadi_version:
|
||||
# print("WARNING: GNSF model was dumped with another CasADi version.\n"
|
||||
# + "This might yield errors. Please use the same version for compatibility, serialize version: "
|
||||
# + dump_casadi_version + " current Python CasADi verison: " + casadi_version)
|
||||
# input("Press any key to attempt to continue...")
|
||||
|
||||
# load model
|
||||
phi_fun = Function.deserialize(gnsf['phi_fun'])
|
||||
phi_fun_jac_y = Function.deserialize(gnsf['phi_fun_jac_y'])
|
||||
phi_jac_y_uhat = Function.deserialize(gnsf['phi_jac_y_uhat'])
|
||||
get_matrices_fun = Function.deserialize(gnsf['get_matrices_fun'])
|
||||
|
||||
# obtain gnsf dimensions
|
||||
size_gnsf_A = get_matrices_fun.size_out(0)
|
||||
acados_ocp.dims.gnsf_nx1 = size_gnsf_A[1]
|
||||
acados_ocp.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
|
||||
acados_ocp.dims.gnsf_nuhat = max(phi_fun.size_in(1))
|
||||
acados_ocp.dims.gnsf_ny = max(phi_fun.size_in(0))
|
||||
acados_ocp.dims.gnsf_nout = max(phi_fun.size_out(0))
|
||||
|
||||
# save gnsf functions in model
|
||||
acados_ocp.model.phi_fun = phi_fun
|
||||
acados_ocp.model.phi_fun_jac_y = phi_fun_jac_y
|
||||
acados_ocp.model.phi_jac_y_uhat = phi_jac_y_uhat
|
||||
acados_ocp.model.get_matrices_fun = get_matrices_fun
|
||||
|
||||
# get_matrices_fun = Function([model_name,'_gnsf_get_matrices_fun'], {dummy},...
|
||||
# {A, B, C, E, L_x, L_xdot, L_z, L_u, A_LO, c, E_LO, B_LO,...
|
||||
# nontrivial_f_LO, purely_linear, ipiv_x, ipiv_z, c_LO});
|
||||
get_matrices_out = get_matrices_fun(0)
|
||||
acados_ocp.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12])
|
||||
acados_ocp.model.gnsf['purely_linear'] = int(get_matrices_out[13])
|
||||
|
||||
if "f_lo_fun_jac_x1k1uz" in gnsf:
|
||||
f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz'])
|
||||
acados_ocp.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz
|
||||
else:
|
||||
dummy_var_x1 = SX.sym('dummy_var_x1', acados_ocp.dims.gnsf_nx1)
|
||||
dummy_var_x1dot = SX.sym('dummy_var_x1dot', acados_ocp.dims.gnsf_nx1)
|
||||
dummy_var_z1 = SX.sym('dummy_var_z1', acados_ocp.dims.gnsf_nz1)
|
||||
dummy_var_u = SX.sym('dummy_var_z1', acados_ocp.dims.nu)
|
||||
dummy_var_p = SX.sym('dummy_var_z1', acados_ocp.dims.np)
|
||||
empty_var = SX.sym('empty_var', 0, 0)
|
||||
|
||||
empty_fun = Function('empty_fun', \
|
||||
[dummy_var_x1, dummy_var_x1dot, dummy_var_z1, dummy_var_u, dummy_var_p],
|
||||
[empty_var])
|
||||
acados_ocp.model.f_lo_fun_jac_x1k1uz = empty_fun
|
||||
|
||||
del acados_ocp.gnsf_model
|
||||
|
||||
|
||||
def idx_perm_to_ipiv(idx_perm):
|
||||
n = len(idx_perm)
|
||||
vec = list(range(n))
|
||||
ipiv = np.zeros(n)
|
||||
|
||||
print(n, idx_perm)
|
||||
# import pdb; pdb.set_trace()
|
||||
for ii in range(n):
|
||||
idx0 = idx_perm[ii]
|
||||
for jj in range(ii,n):
|
||||
if vec[jj]==idx0:
|
||||
idx1 = jj
|
||||
break
|
||||
tmp = vec[ii]
|
||||
vec[ii] = vec[idx1]
|
||||
vec[idx1] = tmp
|
||||
ipiv[ii] = idx1
|
||||
|
||||
ipiv = ipiv-1 # C 0-based indexing
|
||||
return ipiv
|
||||
|
||||
|
||||
def print_casadi_expression(f):
|
||||
for ii in range(casadi_length(f)):
|
||||
print(f[ii,:])
|
||||
78
third_party/acados/acados_template/zoro_description.py
vendored
Normal file
78
third_party/acados/acados_template/zoro_description.py
vendored
Normal file
@@ -0,0 +1,78 @@
|
||||
# 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.;
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass
|
||||
class ZoroDescription:
|
||||
"""
|
||||
Zero-Order Robust Optimization scheme.
|
||||
|
||||
For advanced users.
|
||||
"""
|
||||
backoff_scaling_gamma: float = 1.0
|
||||
fdbk_K_mat: np.ndarray = None
|
||||
unc_jac_G_mat: np.ndarray = None # default: an identity matrix
|
||||
P0_mat: np.ndarray = None
|
||||
W_mat: np.ndarray = None
|
||||
idx_lbx_t: list = field(default_factory=list)
|
||||
idx_ubx_t: list = field(default_factory=list)
|
||||
idx_lbx_e_t: list = field(default_factory=list)
|
||||
idx_ubx_e_t: list = field(default_factory=list)
|
||||
idx_lbu_t: list = field(default_factory=list)
|
||||
idx_ubu_t: list = field(default_factory=list)
|
||||
idx_lg_t: list = field(default_factory=list)
|
||||
idx_ug_t: list = field(default_factory=list)
|
||||
idx_lg_e_t: list = field(default_factory=list)
|
||||
idx_ug_e_t: list = field(default_factory=list)
|
||||
idx_lh_t: list = field(default_factory=list)
|
||||
idx_uh_t: list = field(default_factory=list)
|
||||
idx_lh_e_t: list = field(default_factory=list)
|
||||
idx_uh_e_t: list = field(default_factory=list)
|
||||
|
||||
def process_zoro_description(zoro_description: ZoroDescription):
|
||||
zoro_description.nw, _ = zoro_description.W_mat.shape
|
||||
if zoro_description.unc_jac_G_mat is None:
|
||||
zoro_description.unc_jac_G_mat = np.eye(zoro_description.nw)
|
||||
zoro_description.nlbx_t = len(zoro_description.idx_lbx_t)
|
||||
zoro_description.nubx_t = len(zoro_description.idx_ubx_t)
|
||||
zoro_description.nlbx_e_t = len(zoro_description.idx_lbx_e_t)
|
||||
zoro_description.nubx_e_t = len(zoro_description.idx_ubx_e_t)
|
||||
zoro_description.nlbu_t = len(zoro_description.idx_lbu_t)
|
||||
zoro_description.nubu_t = len(zoro_description.idx_ubu_t)
|
||||
zoro_description.nlg_t = len(zoro_description.idx_lg_t)
|
||||
zoro_description.nug_t = len(zoro_description.idx_ug_t)
|
||||
zoro_description.nlg_e_t = len(zoro_description.idx_lg_e_t)
|
||||
zoro_description.nug_e_t = len(zoro_description.idx_ug_e_t)
|
||||
zoro_description.nlh_t = len(zoro_description.idx_lh_t)
|
||||
zoro_description.nuh_t = len(zoro_description.idx_uh_t)
|
||||
zoro_description.nlh_e_t = len(zoro_description.idx_lh_e_t)
|
||||
zoro_description.nuh_e_t = len(zoro_description.idx_uh_e_t)
|
||||
return zoro_description.__dict__
|
||||
62
third_party/acados/build.sh
vendored
Executable file
62
third_party/acados/build.sh
vendored
Executable file
@@ -0,0 +1,62 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
|
||||
|
||||
ARCHNAME="x86_64"
|
||||
BLAS_TARGET="X64_AUTOMATIC"
|
||||
if [ -f /TICI ]; then
|
||||
ARCHNAME="larch64"
|
||||
BLAS_TARGET="ARMV8A_ARM_CORTEX_A57"
|
||||
fi
|
||||
|
||||
ACADOS_FLAGS="-DACADOS_WITH_QPOASES=ON -UBLASFEO_TARGET -DBLASFEO_TARGET=$BLAS_TARGET"
|
||||
|
||||
if [[ "$OSTYPE" == "darwin"* ]]; then
|
||||
ACADOS_FLAGS="$ACADOS_FLAGS -DCMAKE_OSX_ARCHITECTURES=arm64;x86_64 -DCMAKE_MACOSX_RPATH=1"
|
||||
ARCHNAME="Darwin"
|
||||
fi
|
||||
|
||||
if [ ! -d acados_repo/ ]; then
|
||||
git clone https://github.com/acados/acados.git $DIR/acados_repo
|
||||
# git clone https://github.com/commaai/acados.git $DIR/acados_repo
|
||||
fi
|
||||
cd acados_repo
|
||||
git fetch --all
|
||||
git checkout 8af9b0ad180940ef611884574a0b27a43504311d # v0.2.2
|
||||
git submodule update --depth=1 --recursive --init
|
||||
|
||||
# build
|
||||
mkdir -p build
|
||||
cd build
|
||||
cmake $ACADOS_FLAGS ..
|
||||
make -j20 install
|
||||
|
||||
INSTALL_DIR="$DIR/$ARCHNAME"
|
||||
rm -rf $INSTALL_DIR
|
||||
mkdir -p $INSTALL_DIR
|
||||
|
||||
rm $DIR/acados_repo/lib/*.json
|
||||
|
||||
rm -rf $DIR/include $DIR/acados_template
|
||||
cp -r $DIR/acados_repo/include $DIR
|
||||
cp -r $DIR/acados_repo/lib $INSTALL_DIR
|
||||
cp -r $DIR/acados_repo/interfaces/acados_template/acados_template $DIR/
|
||||
#pip3 install -e $DIR/acados/interfaces/acados_template
|
||||
|
||||
# skip macOS - sed is different :/
|
||||
if [[ "$OSTYPE" != "darwin"* ]]; then
|
||||
# strip future_fstrings to avoid having to install the compatibility package
|
||||
find $DIR/acados_template/ -type f -exec sed -i '/future.fstrings/d' {} +
|
||||
fi
|
||||
|
||||
# build tera
|
||||
cd $DIR/acados_repo/interfaces/acados_template/tera_renderer/
|
||||
if [[ "$OSTYPE" == "darwin"* ]]; then
|
||||
cargo build --verbose --release --target aarch64-apple-darwin
|
||||
cargo build --verbose --release --target x86_64-apple-darwin
|
||||
lipo -create -output target/release/t_renderer target/x86_64-apple-darwin/release/t_renderer target/aarch64-apple-darwin/release/t_renderer
|
||||
else
|
||||
cargo build --verbose --release
|
||||
fi
|
||||
cp target/release/t_renderer $INSTALL_DIR/
|
||||
BIN
third_party/acados/larch64/lib/libacados.so
vendored
Normal file
BIN
third_party/acados/larch64/lib/libacados.so
vendored
Normal file
Binary file not shown.
BIN
third_party/acados/larch64/lib/libblasfeo.so
vendored
Normal file
BIN
third_party/acados/larch64/lib/libblasfeo.so
vendored
Normal file
Binary file not shown.
BIN
third_party/acados/larch64/lib/libhpipm.so
vendored
Normal file
BIN
third_party/acados/larch64/lib/libhpipm.so
vendored
Normal file
Binary file not shown.
1
third_party/acados/larch64/lib/libqpOASES_e.so
vendored
Symbolic link
1
third_party/acados/larch64/lib/libqpOASES_e.so
vendored
Symbolic link
@@ -0,0 +1 @@
|
||||
libqpOASES_e.so.3.1
|
||||
BIN
third_party/acados/larch64/lib/libqpOASES_e.so.3.1
vendored
Normal file
BIN
third_party/acados/larch64/lib/libqpOASES_e.so.3.1
vendored
Normal file
Binary file not shown.
BIN
third_party/acados/larch64/t_renderer
vendored
Executable file
BIN
third_party/acados/larch64/t_renderer
vendored
Executable file
Binary file not shown.
1
third_party/bootstrap/.gitignore
vendored
Normal file
1
third_party/bootstrap/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
/icons/
|
||||
1
third_party/bootstrap/bootstrap-icons.svg
vendored
Normal file
1
third_party/bootstrap/bootstrap-icons.svg
vendored
Normal file
File diff suppressed because one or more lines are too long
|
After Width: | Height: | Size: 1.0 MiB |
14
third_party/bootstrap/pull.sh
vendored
Executable file
14
third_party/bootstrap/pull.sh
vendored
Executable file
@@ -0,0 +1,14 @@
|
||||
#!/bin/bash
|
||||
set -e
|
||||
|
||||
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
|
||||
cd $DIR
|
||||
|
||||
if [ ! -d icons/ ]; then
|
||||
git clone https://github.com/twbs/icons/
|
||||
fi
|
||||
|
||||
cd icons
|
||||
git fetch --all
|
||||
git checkout d5aa187483a1b0b186f87adcfa8576350d970d98
|
||||
cp bootstrap-icons.svg ../
|
||||
1
third_party/libyuv/.gitignore
vendored
Normal file
1
third_party/libyuv/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
libyuv/
|
||||
29
third_party/libyuv/LICENSE
vendored
Normal file
29
third_party/libyuv/LICENSE
vendored
Normal file
@@ -0,0 +1,29 @@
|
||||
Copyright 2011 The LibYuv Project Authors. All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are
|
||||
met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
|
||||
* 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.
|
||||
|
||||
* Neither the name of Google nor the names of its contributors may
|
||||
be used to endorse or promote products derived from this software
|
||||
without specific prior written permission.
|
||||
|
||||
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.
|
||||
1
third_party/libyuv/aarch64
vendored
Symbolic link
1
third_party/libyuv/aarch64
vendored
Symbolic link
@@ -0,0 +1 @@
|
||||
larch64/
|
||||
39
third_party/libyuv/build.sh
vendored
Executable file
39
third_party/libyuv/build.sh
vendored
Executable file
@@ -0,0 +1,39 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
|
||||
|
||||
ARCHNAME=$(uname -m)
|
||||
if [ -f /TICI ]; then
|
||||
ARCHNAME="larch64"
|
||||
fi
|
||||
|
||||
if [[ "$OSTYPE" == "darwin"* ]]; then
|
||||
ARCHNAME="Darwin"
|
||||
fi
|
||||
|
||||
cd $DIR
|
||||
if [ ! -d libyuv ]; then
|
||||
git clone --single-branch https://chromium.googlesource.com/libyuv/libyuv
|
||||
fi
|
||||
|
||||
cd libyuv
|
||||
git checkout 4a14cb2e81235ecd656e799aecaaf139db8ce4a2
|
||||
|
||||
# build
|
||||
cmake .
|
||||
make -j$(nproc)
|
||||
|
||||
INSTALL_DIR="$DIR/$ARCHNAME"
|
||||
rm -rf $INSTALL_DIR
|
||||
mkdir -p $INSTALL_DIR
|
||||
|
||||
rm -rf $DIR/include
|
||||
mkdir -p $INSTALL_DIR/lib
|
||||
cp $DIR/libyuv/libyuv.a $INSTALL_DIR/lib
|
||||
cp -r $DIR/libyuv/include $DIR
|
||||
|
||||
## To create universal binary on Darwin:
|
||||
## ```
|
||||
## lipo -create -output Darwin/libyuv.a path-to-x64/libyuv.a path-to-arm64/libyuv.a
|
||||
## ```
|
||||
16
third_party/libyuv/include/libyuv/version.h
vendored
Normal file
16
third_party/libyuv/include/libyuv/version.h
vendored
Normal file
@@ -0,0 +1,16 @@
|
||||
/*
|
||||
* Copyright 2012 The LibYuv Project Authors. All rights reserved.
|
||||
*
|
||||
* Use of this source code is governed by a BSD-style license
|
||||
* that can be found in the LICENSE file in the root of the source
|
||||
* tree. An additional intellectual property rights grant can be found
|
||||
* in the file PATENTS. All contributing project authors may
|
||||
* be found in the AUTHORS file in the root of the source tree.
|
||||
*/
|
||||
|
||||
#ifndef INCLUDE_LIBYUV_VERSION_H_
|
||||
#define INCLUDE_LIBYUV_VERSION_H_
|
||||
|
||||
#define LIBYUV_VERSION 1622
|
||||
|
||||
#endif // INCLUDE_LIBYUV_VERSION_H_
|
||||
BIN
third_party/mapd_pfeiferj/mapd
vendored
Executable file
BIN
third_party/mapd_pfeiferj/mapd
vendored
Executable file
Binary file not shown.
3
third_party/raylib/.gitignore
vendored
Normal file
3
third_party/raylib/.gitignore
vendored
Normal file
@@ -0,0 +1,3 @@
|
||||
/raylib_repo/
|
||||
/raylib_python_repo/
|
||||
/wheel/
|
||||
90
third_party/raylib/build.sh
vendored
Executable file
90
third_party/raylib/build.sh
vendored
Executable file
@@ -0,0 +1,90 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
SUDO=""
|
||||
|
||||
# Use sudo if not root
|
||||
if [[ ! $(id -u) -eq 0 ]]; then
|
||||
if [[ -z $(which sudo) ]]; then
|
||||
echo "Please install sudo or run as root"
|
||||
exit 1
|
||||
fi
|
||||
SUDO="sudo"
|
||||
fi
|
||||
|
||||
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
|
||||
cd $DIR
|
||||
|
||||
RAYLIB_PLATFORM="PLATFORM_DESKTOP"
|
||||
|
||||
ARCHNAME=$(uname -m)
|
||||
if [ -f /TICI ]; then
|
||||
ARCHNAME="larch64"
|
||||
RAYLIB_PLATFORM="PLATFORM_COMMA"
|
||||
elif [[ "$OSTYPE" == "linux"* ]]; then
|
||||
# required dependencies on Linux PC
|
||||
$SUDO apt install \
|
||||
libxcursor-dev \
|
||||
libxi-dev \
|
||||
libxinerama-dev \
|
||||
libxrandr-dev
|
||||
fi
|
||||
|
||||
if [[ "$OSTYPE" == "darwin"* ]]; then
|
||||
ARCHNAME="Darwin"
|
||||
fi
|
||||
|
||||
INSTALL_DIR="$DIR/$ARCHNAME"
|
||||
rm -rf $INSTALL_DIR
|
||||
mkdir -p $INSTALL_DIR
|
||||
|
||||
INSTALL_H_DIR="$DIR/include"
|
||||
rm -rf $INSTALL_H_DIR
|
||||
mkdir -p $INSTALL_H_DIR
|
||||
|
||||
if [ ! -d raylib_repo ]; then
|
||||
git clone -b master --no-tags https://github.com/commaai/raylib.git raylib_repo
|
||||
fi
|
||||
|
||||
cd raylib_repo
|
||||
|
||||
COMMIT=${1:-3425bd9d1fb292ede4d80f97a1f4f258f614cffc}
|
||||
git fetch origin $COMMIT
|
||||
git reset --hard $COMMIT
|
||||
git clean -xdff .
|
||||
|
||||
cd src
|
||||
|
||||
make -j$(nproc) PLATFORM=$RAYLIB_PLATFORM RAYLIB_RELEASE_PATH=$INSTALL_DIR
|
||||
cp raylib.h raymath.h rlgl.h $INSTALL_H_DIR/
|
||||
echo "raylib development files installed/updated in $INSTALL_H_DIR"
|
||||
|
||||
# this commit needs to be in line with raylib
|
||||
set -x
|
||||
RAYGUI_COMMIT="76b36b597edb70ffaf96f046076adc20d67e7827"
|
||||
curl -fsSLo $INSTALL_H_DIR/raygui.h https://raw.githubusercontent.com/raysan5/raygui/$RAYGUI_COMMIT/src/raygui.h
|
||||
|
||||
if [ -f /TICI ]; then
|
||||
|
||||
# Building the python bindings
|
||||
cd $DIR
|
||||
|
||||
if [ ! -d raylib_python_repo ]; then
|
||||
git clone -b master --no-tags https://github.com/commaai/raylib-python-cffi.git raylib_python_repo
|
||||
fi
|
||||
|
||||
cd raylib_python_repo
|
||||
|
||||
BINDINGS_COMMIT="a0710d95af3c12fd7f4b639589be9a13dad93cb6"
|
||||
git fetch origin $BINDINGS_COMMIT
|
||||
git reset --hard $BINDINGS_COMMIT
|
||||
git clean -xdff .
|
||||
|
||||
RAYLIB_PLATFORM=$RAYLIB_PLATFORM RAYLIB_INCLUDE_PATH=$INSTALL_H_DIR RAYLIB_LIB_PATH=$INSTALL_DIR python setup.py bdist_wheel
|
||||
cd $DIR
|
||||
|
||||
rm -rf wheel
|
||||
mkdir wheel
|
||||
cp raylib_python_repo/dist/*.whl wheel/
|
||||
|
||||
fi
|
||||
Reference in New Issue
Block a user