IQ.Lvbs/IQ.Pilot
8
7
Fork 2
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Prebuilt Release @ 27f668a

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-09-03 18:23:24 -05:00
commit b073c5182b
2554 changed files with 679696 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
iqpilot/third_party/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
*.pyc

5
iqpilot/third_party/acados/.gitignore vendored Normal file
View File

@@ -0,0 +1,5 @@
acados_repo/
lib
!x86_64/
!larch64/
!aarch64/

1
iqpilot/third_party/acados/aarch64 vendored Symbolic link
View File

@@ -0,0 +1 @@
larch64/

6
iqpilot/third_party/acados/acados_template/.gitignore vendored Normal file
View File

@@ -0,0 +1,6 @@
__pycache__/
# Cython intermediates
*_pyx.c
*_pyx.o
*_pyx.so

40
iqpilot/third_party/acados/acados_template/__init__.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/acados_layout.json vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/acados_model.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/acados_ocp.py vendored Normal file
View File

File diff suppressed because it is too large Load Diff

1990
iqpilot/third_party/acados/acados_template/acados_ocp_solver.py vendored Normal file
View File

File diff suppressed because it is too large Load Diff

366
iqpilot/third_party/acados/acados_template/acados_sim.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/acados_sim_layout.json vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/acados_sim_solver.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/builders.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/c_templates_tera/Makefile.in vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/casadi_function_generation.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/gnsf/check_reformulation.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/gnsf/detect_gnsf_structure.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/gnsf/matlab to python.md vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/gnsf/reformulate_with_LOS.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/gnsf/structure_detection_print_summary.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/simulink_default_opts.json vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/utils.py vendored Normal file
View 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
iqpilot/third_party/acados/acados_template/zoro_description.py vendored Normal file
View 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
iqpilot/third_party/acados/build.sh vendored Executable file
View 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
iqpilot/third_party/acados/larch64/lib/libacados.so vendored Normal file
View File

Binary file not shown.

BIN
iqpilot/third_party/acados/larch64/lib/libblasfeo.so vendored Normal file
View File

Binary file not shown.

BIN
iqpilot/third_party/acados/larch64/lib/libhpipm.so vendored Normal file
View File

Binary file not shown.

1
iqpilot/third_party/acados/larch64/lib/libqpOASES_e.so vendored Symbolic link
View File

@@ -0,0 +1 @@
libqpOASES_e.so.3.1

BIN
iqpilot/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 vendored Normal file
View File

Binary file not shown.

BIN
iqpilot/third_party/acados/larch64/t_renderer vendored Executable file
View File

Binary file not shown.

1
iqpilot/third_party/bootstrap/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
/icons/

1
iqpilot/third_party/bootstrap/bootstrap-icons.svg vendored Normal file
View File

File diff suppressed because one or more lines are too long
Side by Side

After

Width:  |  Height:  |  Size: 1.0 MiB

14
iqpilot/third_party/bootstrap/pull.sh vendored Executable file
View 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
iqpilot/third_party/libyuv/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
libyuv/

29
iqpilot/third_party/libyuv/LICENSE vendored Normal file
View 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
iqpilot/third_party/libyuv/aarch64 vendored Symbolic link
View File

@@ -0,0 +1 @@
larch64/

39
iqpilot/third_party/libyuv/build.sh vendored Executable file
View 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
iqpilot/third_party/libyuv/include/libyuv/version.h vendored Normal file
View 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
iqpilot/third_party/mapd_pfeiferj/mapd vendored Executable file
View File

Binary file not shown.

3
iqpilot/third_party/raylib/.gitignore vendored Normal file
View File

@@ -0,0 +1,3 @@
/raylib_repo/
/raylib_python_repo/
/wheel/

90
iqpilot/third_party/raylib/build.sh vendored Executable file
View 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