brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

openpilot v0.9.6 release

Browse Source 
date: 2024-01-12T10:13:37
master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6
This commit is contained in:
FrogAi
2024-01-12 22:39:28 -07:00
commit 08e9fb1edc
1881 changed files with 653708 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
third_party/.gitignore vendored Normal file
View File

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

6
third_party/SConscript vendored Normal file
View File

@@ -0,0 +1,6 @@
Import('env')
env.Library('json11', ['json11/json11.cpp'], CCFLAGS=env['CCFLAGS'] + ['-Wno-unqualified-std-cast-call'])
env.Append(CPPPATH=[Dir('json11')])
env.Library('kaitai', ['kaitai/kaitaistream.cpp'], CPPDEFINES=['KS_STR_ENCODING_NONE'])

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

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

40
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
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
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'.
"""

3167
third_party/acados/acados_template/acados_ocp.py vendored Normal file
View File

File diff suppressed because it is too large Load Diff

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

File diff suppressed because it is too large Load Diff

795
third_party/acados/acados_template/acados_ocp_solver_pyx.pyx vendored Normal file
View File

@@ -0,0 +1,795 @@
# -*- coding: future_fstrings -*-
#
# 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.;
#
# cython: language_level=3
# cython: profile=False
# distutils: language=c
cimport cython
from libc cimport string
cimport acados_solver_common
# TODO: make this import more clear? it is not a general solver, but problem specific.
cimport acados_solver
cimport numpy as cnp
import os
from datetime import datetime
import numpy as np
cdef class AcadosOcpSolverCython:
"""
Class to interact with the acados ocp solver C object.
"""
cdef acados_solver.nlp_solver_capsule *capsule
cdef void *nlp_opts
cdef acados_solver_common.ocp_nlp_dims *nlp_dims
cdef acados_solver_common.ocp_nlp_config *nlp_config
cdef acados_solver_common.ocp_nlp_out *nlp_out
cdef acados_solver_common.ocp_nlp_out *sens_out
cdef acados_solver_common.ocp_nlp_in *nlp_in
cdef acados_solver_common.ocp_nlp_solver *nlp_solver
cdef bint solver_created
cdef str model_name
cdef int N
cdef str nlp_solver_type
def __cinit__(self, model_name, nlp_solver_type, N):
self.solver_created = False
self.N = N
self.model_name = model_name
self.nlp_solver_type = nlp_solver_type
# create capsule
self.capsule = acados_solver.acados_create_capsule()
# create solver
assert acados_solver.acados_create(self.capsule) == 0
self.solver_created = True
# get pointers solver
self.__get_pointers_solver()
def __get_pointers_solver(self):
"""
Private function to get the pointers for solver
"""
# get pointers solver
self.nlp_opts = acados_solver.acados_get_nlp_opts(self.capsule)
self.nlp_dims = acados_solver.acados_get_nlp_dims(self.capsule)
self.nlp_config = acados_solver.acados_get_nlp_config(self.capsule)
self.nlp_out = acados_solver.acados_get_nlp_out(self.capsule)
self.sens_out = acados_solver.acados_get_sens_out(self.capsule)
self.nlp_in = acados_solver.acados_get_nlp_in(self.capsule)
self.nlp_solver = acados_solver.acados_get_nlp_solver(self.capsule)
def solve_for_x0(self, x0_bar):
"""
Wrapper around `solve()` which sets initial state constraint, solves the OCP, and returns u0.
"""
self.set(0, "lbx", x0_bar)
self.set(0, "ubx", x0_bar)
status = self.solve()
if status == 2:
print("Warning: acados_ocp_solver reached maximum iterations.")
elif status != 0:
raise Exception(f'acados acados_ocp_solver returned status {status}')
u0 = self.get(0, "u")
return u0
def solve(self):
"""
Solve the ocp with current input.
"""
return acados_solver.acados_solve(self.capsule)
def reset(self, reset_qp_solver_mem=1):
"""
Sets current iterate to all zeros.
"""
return acados_solver.acados_reset(self.capsule, reset_qp_solver_mem)
def custom_update(self, data_):
"""
A custom function that can be implemented by a user to be called between solver calls.
By default this does nothing.
The idea is to have a convenient wrapper to do complex updates of parameters and numerical data efficiently in C,
in a function that is compiled into the solver library and can be conveniently used in the Python environment.
"""
data_len = len(data_)
cdef cnp.ndarray[cnp.float64_t, ndim=1] data = np.ascontiguousarray(data_, dtype=np.float64)
return acados_solver.acados_custom_update(self.capsule, <double *> data.data, data_len)
def set_new_time_steps(self, new_time_steps):
"""
Set new time steps.
Recreates the solver if N changes.
:param new_time_steps: 1 dimensional np array of new time steps for the solver
.. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
do not require a new code export and compilation.
"""
raise NotImplementedError("AcadosOcpSolverCython: does not support set_new_time_steps() since it is only a prototyping feature")
# # unlikely but still possible
# if not self.solver_created:
# raise Exception('Solver was not yet created!')
# ## check if time steps really changed in value
# # get time steps
# cdef cnp.ndarray[cnp.float64_t, ndim=1] old_time_steps = np.ascontiguousarray(np.zeros((self.N,)), dtype=np.float64)
# assert acados_solver.acados_get_time_steps(self.capsule, self.N, <double *> old_time_steps.data)
# if np.array_equal(old_time_steps, new_time_steps):
# return
# N = new_time_steps.size
# cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(new_time_steps, dtype=np.float64)
# # check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
# if len(old_time_steps) == N:
# assert acados_solver.acados_update_time_steps(self.capsule, N, <double *> value.data) == 0
# else: # recreate the solver with the new time steps
# self.solver_created = False
# # delete old memory (analog to __del__)
# acados_solver.acados_free(self.capsule)
# # create solver with new time steps
# assert acados_solver.acados_create_with_discretization(self.capsule, N, <double *> value.data) == 0
# self.solver_created = True
# # get pointers solver
# self.__get_pointers_solver()
# # store time_steps, N
# self.time_steps = new_time_steps
# self.N = N
def update_qp_solver_cond_N(self, qp_solver_cond_N: int):
"""
Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver.
This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or
the influence of a different `qp_solver_cond_N` is studied without code export and compilation.
:param qp_solver_cond_N: new number of condensing stages for the solver
.. note:: This function can only be used in combination with a partial condensing QP solver.
.. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be
necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically
`qp_solver_cond_N < N`.
"""
raise NotImplementedError("AcadosOcpSolverCython: does not support update_qp_solver_cond_N() since it is only a prototyping feature")
# # unlikely but still possible
# if not self.solver_created:
# raise Exception('Solver was not yet created!')
# if self.N < qp_solver_cond_N:
# raise Exception('Setting qp_solver_cond_N to be larger than N does not work!')
# if self.qp_solver_cond_N != qp_solver_cond_N:
# self.solver_created = False
# # recreate the solver
# acados_solver.acados_update_qp_solver_cond_N(self.capsule, qp_solver_cond_N)
# # store the new value
# self.qp_solver_cond_N = qp_solver_cond_N
# self.solver_created = True
# # get pointers solver
# self.__get_pointers_solver()
def eval_param_sens(self, index, stage=0, field="ex"):
"""
Calculate the sensitivity of the curent solution with respect to the initial state component of index
:param index: integer corresponding to initial state index in range(nx)
"""
field_ = field
field = field_.encode('utf-8')
# checks
if not isinstance(index, int):
raise Exception('AcadosOcpSolverCython.eval_param_sens(): index must be Integer.')
cdef int nx = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8'))
if index < 0 or index > nx:
raise Exception(f'AcadosOcpSolverCython.eval_param_sens(): index must be in [0, nx-1], got: {index}.')
# actual eval_param
acados_solver_common.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out)
return
def get(self, int stage, str field_):
"""
Get the last solution of the solver:
:param stage: integer corresponding to shooting node
:param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',]
.. note:: regarding lam, t: \n
the inequalities are internally organized in the following order: \n
[ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
.. note:: pi: multipliers for dynamics equality constraints \n
lam: multipliers for inequalities \n
t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
sl: slack variables of soft lower inequality constraints \n
su: slack variables of soft upper inequality constraints \n
"""
out_fields = ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su']
field = field_.encode('utf-8')
if field_ not in out_fields:
raise Exception('AcadosOcpSolverCython.get(): {} is an invalid argument.\
\n Possible values are {}.'.format(field_, out_fields))
if stage < 0 or stage > self.N:
raise Exception('AcadosOcpSolverCython.get(): stage index must be in [0, N], got: {}.'.format(self.N))
if stage == self.N and field_ == 'pi':
raise Exception('AcadosOcpSolverCython.get(): field {} does not exist at final stage {}.'\
.format(field_, stage))
cdef int dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config,
self.nlp_dims, self.nlp_out, stage, field)
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((dims,))
acados_solver_common.ocp_nlp_out_get(self.nlp_config, \
self.nlp_dims, self.nlp_out, stage, field, <void *> out.data)
return out
def print_statistics(self):
"""
prints statistics of previous solver run as a table:
- iter: iteration number
- res_stat: stationarity residual
- res_eq: residual wrt equality constraints (dynamics)
- res_ineq: residual wrt inequality constraints (constraints)
- res_comp: residual wrt complementarity conditions
- qp_stat: status of QP solver
- qp_iter: number of QP iterations
- qp_res_stat: stationarity residual of the last QP solution
- qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution
- qp_res_ineq: residual wrt inequality constraints (constraints) of the last QP solution
- qp_res_comp: residual wrt complementarity conditions of the last QP solution
"""
acados_solver.acados_print_stats(self.capsule)
def store_iterate(self, filename='', overwrite=False):
"""
Stores the current iterate of the ocp solver in a json file.
:param filename: if not set, use model_name + timestamp + '.json'
:param overwrite: if false and filename exists add timestamp to filename
"""
import json
if filename == '':
filename += self.model_name + '_' + 'iterate' + '.json'
if not overwrite:
# append timestamp
if os.path.isfile(filename):
filename = filename[:-5]
filename += datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f') + '.json'
# get iterate:
solution = dict()
lN = len(str(self.N+1))
for i in range(self.N+1):
i_string = f'{i:0{lN}d}'
solution['x_'+i_string] = self.get(i,'x')
solution['u_'+i_string] = self.get(i,'u')
solution['z_'+i_string] = self.get(i,'z')
solution['lam_'+i_string] = self.get(i,'lam')
solution['t_'+i_string] = self.get(i, 't')
solution['sl_'+i_string] = self.get(i, 'sl')
solution['su_'+i_string] = self.get(i, 'su')
if i < self.N:
solution['pi_'+i_string] = self.get(i,'pi')
for k in list(solution.keys()):
if len(solution[k]) == 0:
del solution[k]
# save
with open(filename, 'w') as f:
json.dump(solution, f, default=lambda x: x.tolist(), indent=4, sort_keys=True)
print("stored current iterate in ", os.path.join(os.getcwd(), filename))
def load_iterate(self, filename):
"""
Loads the iterate stored in json file with filename into the ocp solver.
"""
import json
if not os.path.isfile(filename):
raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename))
with open(filename, 'r') as f:
solution = json.load(f)
for key in solution.keys():
(field, stage) = key.split('_')
self.set(int(stage), field, np.array(solution[key]))
def get_stats(self, field_):
"""
Get the information of the last solver call.
:param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
Available fileds:
- time_tot: total CPU time previous call
- time_lin: CPU time for linearization
- time_sim: CPU time for integrator
- time_sim_ad: CPU time for integrator contribution of external function calls
- time_sim_la: CPU time for integrator contribution of linear algebra
- time_qp: CPU time qp solution
- time_qp_solver_call: CPU time inside qp solver (without converting the QP)
- time_qp_xcond: time_glob: CPU time globalization
- time_solution_sensitivities: CPU time for previous call to eval_param_sens
- time_reg: CPU time regularization
- sqp_iter: number of SQP iterations
- qp_iter: vector of QP iterations for last SQP call
- statistics: table with info about last iteration
- stat_m: number of rows in statistics matrix
- stat_n: number of columns in statistics matrix
- residuals: residuals of last iterate
- alpha: step sizes of SQP iterations
"""
double_fields = ['time_tot',
'time_lin',
'time_sim',
'time_sim_ad',
'time_sim_la',
'time_qp',
'time_qp_solver_call',
'time_qp_xcond',
'time_glob',
'time_solution_sensitivities',
'time_reg'
]
fields = double_fields + [
'sqp_iter',
'qp_iter',
'statistics',
'stat_m',
'stat_n',
'residuals',
'alpha',
]
field = field_.encode('utf-8')
if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
return self.__get_stat_int(field)
elif field_ in double_fields:
return self.__get_stat_double(field)
elif field_ == 'statistics':
sqp_iter = self.get_stats("sqp_iter")
stat_m = self.get_stats("stat_m")
stat_n = self.get_stats("stat_n")
min_size = min([stat_m, sqp_iter+1])
return self.__get_stat_matrix(field, stat_n+1, min_size)
elif field_ == 'qp_iter':
full_stats = self.get_stats('statistics')
if self.nlp_solver_type == 'SQP':
return full_stats[6, :]
elif self.nlp_solver_type == 'SQP_RTI':
return full_stats[2, :]
elif field_ == 'alpha':
full_stats = self.get_stats('statistics')
if self.nlp_solver_type == 'SQP':
return full_stats[7, :]
else: # self.nlp_solver_type == 'SQP_RTI':
raise Exception("alpha values are not available for SQP_RTI")
elif field_ == 'residuals':
return self.get_residuals()
else:
raise NotImplementedError("TODO!")
def __get_stat_int(self, field):
cdef int out
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &out)
return out
def __get_stat_double(self, field):
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((1,))
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out.data)
return out
def __get_stat_matrix(self, field, n, m):
cdef cnp.ndarray[cnp.float64_t, ndim=2] out_mat = np.ascontiguousarray(np.zeros((n, m)), dtype=np.float64)
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out_mat.data)
return out_mat
def get_cost(self):
"""
Returns the cost value of the current solution.
"""
# compute cost internally
acados_solver_common.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out)
# create output
cdef double out
# call getter
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, "cost_value", <void *> &out)
return out
def get_residuals(self, recompute=False):
"""
Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
"""
# compute residuals if RTI
if self.nlp_solver_type == 'SQP_RTI' or recompute:
acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
# create output array
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((4,), dtype=np.float64))
cdef double double_value
field = "res_stat".encode('utf-8')
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
out[0] = double_value
field = "res_eq".encode('utf-8')
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
out[1] = double_value
field = "res_ineq".encode('utf-8')
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
out[2] = double_value
field = "res_comp".encode('utf-8')
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
out[3] = double_value
return out
# Note: this function should not be used anymore, better use cost_set, constraints_set
def set(self, int stage, str field_, value_):
"""
Set numerical data inside the solver.
:param stage: integer corresponding to shooting node
:param field: string in ['x', 'u', 'pi', 'lam', 't', 'p']
.. note:: regarding lam, t: \n
the inequalities are internally organized in the following order: \n
[ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
.. note:: pi: multipliers for dynamics equality constraints \n
lam: multipliers for inequalities \n
t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
sl: slack variables of soft lower inequality constraints \n
su: slack variables of soft upper inequality constraints \n
"""
if not isinstance(value_, np.ndarray):
raise Exception(f"set: value must be numpy array, got {type(value_)}.")
cost_fields = ['y_ref', 'yref']
constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
mem_fields = ['xdot_guess', 'z_guess']
field = field_.encode('utf-8')
cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(value_, dtype=np.float64)
# treat parameters separately
if field_ == 'p':
assert acados_solver.acados_update_params(self.capsule, stage, <double *> value.data, value.shape[0]) == 0
else:
if field_ not in constraints_fields + cost_fields + out_fields:
raise Exception("AcadosOcpSolverCython.set(): {} is not a valid argument.\
\nPossible values are {}.".format(field, \
constraints_fields + cost_fields + out_fields + ['p']))
dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config,
self.nlp_dims, self.nlp_out, stage, field)
if value_.shape[0] != dims:
msg = 'AcadosOcpSolverCython.set(): mismatching dimension for field "{}" '.format(field_)
msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0])
raise Exception(msg)
if field_ in constraints_fields:
acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config,
self.nlp_dims, self.nlp_in, stage, field, <void *> value.data)
elif field_ in cost_fields:
acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config,
self.nlp_dims, self.nlp_in, stage, field, <void *> value.data)
elif field_ in out_fields:
acados_solver_common.ocp_nlp_out_set(self.nlp_config,
self.nlp_dims, self.nlp_out, stage, field, <void *> value.data)
elif field_ in mem_fields:
acados_solver_common.ocp_nlp_set(self.nlp_config, \
self.nlp_solver, stage, field, <void *> value.data)
if field_ == 'z':
field = 'z_guess'.encode('utf-8')
acados_solver_common.ocp_nlp_set(self.nlp_config, \
self.nlp_solver, stage, field, <void *> value.data)
return
def cost_set(self, int stage, str field_, value_):
"""
Set numerical data in the cost module of the solver.
:param stage: integer corresponding to shooting node
:param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess'
:param value: of appropriate size
"""
if not isinstance(value_, np.ndarray):
raise Exception(f"cost_set: value must be numpy array, got {type(value_)}.")
field = field_.encode('utf-8')
cdef int dims[2]
acados_solver_common.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \
self.nlp_dims, self.nlp_out, stage, field, &dims[0])
cdef double[::1,:] value
value_shape = value_.shape
if len(value_shape) == 1:
value_shape = (value_shape[0], 0)
value = np.asfortranarray(value_[None,:])
elif len(value_shape) == 2:
# Get elements in column major order
value = np.asfortranarray(value_)
if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
raise Exception('AcadosOcpSolverCython.cost_set(): mismatching dimension' +
f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, \
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
def constraints_set(self, int stage, str field_, value_):
"""
Set numerical data in the constraint module of the solver.
:param stage: integer corresponding to shooting node
:param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D']
:param value: of appropriate size
"""
if not isinstance(value_, np.ndarray):
raise Exception(f"constraints_set: value must be numpy array, got {type(value_)}.")
field = field_.encode('utf-8')
cdef int dims[2]
acados_solver_common.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
self.nlp_dims, self.nlp_out, stage, field, &dims[0])
cdef double[::1,:] value
value_shape = value_.shape
if len(value_shape) == 1:
value_shape = (value_shape[0], 0)
value = np.asfortranarray(value_[None,:])
elif len(value_shape) == 2:
# Get elements in column major order
value = np.asfortranarray(value_)
if value_shape != tuple(dims):
raise Exception(f'AcadosOcpSolverCython.constraints_set(): mismatching dimension' +
f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
return
def get_from_qp_in(self, int stage, str field_):
"""
Get numerical data from the dynamics module of the solver:
:param stage: integer corresponding to shooting node
:param field: string, e.g. 'A'
"""
field = field_.encode('utf-8')
# get dims
cdef int[2] dims
acados_solver_common.ocp_nlp_qp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0])
# create output data
cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F')
# call getter
acados_solver_common.ocp_nlp_get_at_stage(self.nlp_config, self.nlp_dims, self.nlp_solver, stage, field, <void *> out.data)
return out
def options_set(self, str field_, value_):
"""
Set options of the solver.
:param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0'
:param value: of type int, float, string
- qp_tol_stat: QP solver tolerance stationarity
- qp_tol_eq: QP solver tolerance equalities
- qp_tol_ineq: QP solver tolerance inequalities
- qp_tol_comp: QP solver tolerance complementarity
- qp_tau_min: for HPIPM QP solvers: minimum value of barrier parameter in HPIPM
- qp_mu0: for HPIPM QP solvers: initial value for complementarity slackness
- warm_start_first_qp: indicates if first QP in SQP is warm_started
"""
int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'warm_start_first_qp']
double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent',
'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0']
string_fields = ['globalization']
# encode
field = field_.encode('utf-8')
cdef int int_value
cdef double double_value
cdef unsigned char[::1] string_value
# check field availability and type
if field_ in int_fields:
if not isinstance(value_, int):
raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_)))
if field_ == 'rti_phase':
if value_ < 0 or value_ > 2:
raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
'take only values 0, 1, 2 for SQP-RTI-type solvers')
if self.nlp_solver_type != 'SQP_RTI' and value_ > 0:
raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
'take only value 0 for SQP-type solvers')
int_value = value_
acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &int_value)
elif field_ in double_fields:
if not isinstance(value_, float):
raise Exception('solver option {} must be of type float. You have {}.'.format(field_, type(value_)))
double_value = value_
acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &double_value)
elif field_ in string_fields:
if not isinstance(value_, bytes):
raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_)))
string_value = value_.encode('utf-8')
acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &string_value[0])
else:
raise Exception('AcadosOcpSolverCython.options_set() does not support field {}.'\
'\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
def set_params_sparse(self, int stage, idx_values_, param_values_):
"""
set parameters of the solvers external function partially:
Pseudo: solver.param[idx_values_] = param_values_;
Parameters:
:param stage_: integer corresponding to shooting node
:param idx_values_: 0 based integer array corresponding to parameter indices to be set
:param param_values_: new parameter values as numpy array
"""
if not isinstance(param_values_, np.ndarray):
raise Exception('param_values_ must be np.array.')
if param_values_.shape[0] != len(idx_values_):
raise Exception(f'param_values_ and idx_values_ must be of the same size.' +
f' Got sizes idx {param_values_.shape[0]}, param_values {len(idx_values_)}.')
# n_update = c_int(len(param_values_))
# param_data = cast(param_values_.ctypes.data, POINTER(c_double))
# c_idx_values = np.ascontiguousarray(idx_values_, dtype=np.intc)
# idx_data = cast(c_idx_values.ctypes.data, POINTER(c_int))
# getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse").argtypes = \
# [c_void_p, c_int, POINTER(c_int), POINTER(c_double), c_int]
# getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse").restype = c_int
# getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse") \
# (self.capsule, stage, idx_data, param_data, n_update)
cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(param_values_, dtype=np.float64)
# cdef cnp.ndarray[cnp.intc, ndim=1] idx = np.ascontiguousarray(idx_values_, dtype=np.intc)
# NOTE: this does throw an error somehow:
# ValueError: Buffer dtype mismatch, expected 'int object' but got 'int'
# cdef cnp.ndarray[cnp.int, ndim=1] idx = np.ascontiguousarray(idx_values_, dtype=np.intc)
cdef cnp.ndarray[cnp.int32_t, ndim=1] idx = np.ascontiguousarray(idx_values_, dtype=np.int32)
cdef int n_update = value.shape[0]
# print(f"in set_params_sparse Cython n_update {n_update}")
assert acados_solver.acados_update_params_sparse(self.capsule, stage, <int *> idx.data, <double *> value.data, n_update) == 0
return
def __del__(self):
if self.solver_created:
acados_solver.acados_free(self.capsule)
acados_solver.acados_free_capsule(self.capsule)

367
third_party/acados/acados_template/acados_sim.py vendored Normal file
View File

@@ -0,0 +1,367 @@
# -*- coding: future_fstrings -*-
#
# Copyright (c) The acados authors.
#
# This file is part of acados.
#
# The 2-Clause BSD License
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.;
#
import numpy as np
import os
from .acados_model import AcadosModel
from .utils import get_acados_path, get_lib_ext
class AcadosSimDims:
"""
Class containing the dimensions of the model to be simulated.
"""
def __init__(self):
self.__nx = None
self.__nu = None
self.__nz = 0
self.__np = 0
@property
def nx(self):
""":math:`n_x` - number of states. Type: int > 0"""
return self.__nx
@property
def nz(self):
""":math:`n_z` - number of algebraic variables. Type: int >= 0"""
return self.__nz
@property
def nu(self):
""":math:`n_u` - number of inputs. Type: int >= 0"""
return self.__nu
@property
def np(self):
""":math:`n_p` - number of parameters. Type: int >= 0"""
return self.__np
@nx.setter
def nx(self, nx):
if isinstance(nx, int) and nx > 0:
self.__nx = nx
else:
raise Exception('Invalid nx value, expected positive integer.')
@nz.setter
def nz(self, nz):
if isinstance(nz, int) and nz > -1:
self.__nz = nz
else:
raise Exception('Invalid nz value, expected nonnegative integer.')
@nu.setter
def nu(self, nu):
if isinstance(nu, int) and nu > -1:
self.__nu = nu
else:
raise Exception('Invalid nu value, expected nonnegative integer.')
@np.setter
def np(self, np):
if isinstance(np, int) and np > -1:
self.__np = np
else:
raise Exception('Invalid np value, expected nonnegative integer.')
def set(self, attr, value):
setattr(self, attr, value)
class AcadosSimOpts:
"""
class containing the solver options
"""
def __init__(self):
self.__integrator_type = 'ERK'
self.__collocation_type = 'GAUSS_LEGENDRE'
self.__Tsim = None
# ints
self.__sim_method_num_stages = 1
self.__sim_method_num_steps = 1
self.__sim_method_newton_iter = 3
# doubles
self.__sim_method_newton_tol = 0.0
# bools
self.__sens_forw = True
self.__sens_adj = False
self.__sens_algebraic = False
self.__sens_hess = False
self.__output_z = True
self.__sim_method_jac_reuse = 0
self.__ext_fun_compile_flags = '-O2'
@property
def integrator_type(self):
"""Integrator type. Default: 'ERK'."""
return self.__integrator_type
@property
def num_stages(self):
"""Number of stages in the integrator. Default: 1"""
return self.__sim_method_num_stages
@property
def num_steps(self):
"""Number of steps in the integrator. Default: 1"""
return self.__sim_method_num_steps
@property
def newton_iter(self):
"""Number of Newton iterations in simulation method. Default: 3"""
return self.__sim_method_newton_iter
@property
def newton_tol(self):
"""
Tolerance for Newton system solved in implicit integrator (IRK, GNSF).
0.0 means this is not used and exactly newton_iter iterations are carried out.
Default: 0.0
"""
return self.__sim_method_newton_tol
@property
def sens_forw(self):
"""Boolean determining if forward sensitivities are computed. Default: True"""
return self.__sens_forw
@property
def sens_adj(self):
"""Boolean determining if adjoint sensitivities are computed. Default: False"""
return self.__sens_adj
@property
def sens_algebraic(self):
"""Boolean determining if sensitivities wrt algebraic variables are computed. Default: False"""
return self.__sens_algebraic
@property
def sens_hess(self):
"""Boolean determining if hessians are computed. Default: False"""
return self.__sens_hess
@property
def output_z(self):
"""Boolean determining if values for algebraic variables (corresponding to start of simulation interval) are computed. Default: True"""
return self.__output_z
@property
def sim_method_jac_reuse(self):
"""Integer determining if jacobians are reused (0 or 1). Default: 0"""
return self.__sim_method_jac_reuse
@property
def T(self):
"""Time horizon"""
return self.__Tsim
@property
def collocation_type(self):
"""Collocation type: relevant for implicit integrators
-- string in {GAUSS_RADAU_IIA, GAUSS_LEGENDRE}
Default: GAUSS_LEGENDRE
"""
return self.__collocation_type
@property
def ext_fun_compile_flags(self):
"""
String with compiler flags for external function compilation.
Default: '-O2'.
"""
return self.__ext_fun_compile_flags
@ext_fun_compile_flags.setter
def ext_fun_compile_flags(self, ext_fun_compile_flags):
if isinstance(ext_fun_compile_flags, str):
self.__ext_fun_compile_flags = ext_fun_compile_flags
else:
raise Exception('Invalid ext_fun_compile_flags, expected a string.\n')
@integrator_type.setter
def integrator_type(self, integrator_type):
integrator_types = ('ERK', 'IRK', 'GNSF')
if integrator_type in integrator_types:
self.__integrator_type = integrator_type
else:
raise Exception('Invalid integrator_type value. Possible values are:\n\n' \
+ ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\n')
@collocation_type.setter
def collocation_type(self, collocation_type):
collocation_types = ('GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE')
if collocation_type in collocation_types:
self.__collocation_type = collocation_type
else:
raise Exception('Invalid collocation_type value. Possible values are:\n\n' \
+ ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\n')
@T.setter
def T(self, T):
self.__Tsim = T
@num_stages.setter
def num_stages(self, num_stages):
if isinstance(num_stages, int):
self.__sim_method_num_stages = num_stages
else:
raise Exception('Invalid num_stages value. num_stages must be an integer.')
@num_steps.setter
def num_steps(self, num_steps):
if isinstance(num_steps, int):
self.__sim_method_num_steps = num_steps
else:
raise Exception('Invalid num_steps value. num_steps must be an integer.')
@newton_iter.setter
def newton_iter(self, newton_iter):
if isinstance(newton_iter, int):
self.__sim_method_newton_iter = newton_iter
else:
raise Exception('Invalid newton_iter value. newton_iter must be an integer.')
@newton_tol.setter
def newton_tol(self, newton_tol):
if isinstance(newton_tol, float):
self.__sim_method_newton_tol = newton_tol
else:
raise Exception('Invalid newton_tol value. newton_tol must be an float.')
@sens_forw.setter
def sens_forw(self, sens_forw):
if sens_forw in (True, False):
self.__sens_forw = sens_forw
else:
raise Exception('Invalid sens_forw value. sens_forw must be a Boolean.')
@sens_adj.setter
def sens_adj(self, sens_adj):
if sens_adj in (True, False):
self.__sens_adj = sens_adj
else:
raise Exception('Invalid sens_adj value. sens_adj must be a Boolean.')
@sens_hess.setter
def sens_hess(self, sens_hess):
if sens_hess in (True, False):
self.__sens_hess = sens_hess
else:
raise Exception('Invalid sens_hess value. sens_hess must be a Boolean.')
@sens_algebraic.setter
def sens_algebraic(self, sens_algebraic):
if sens_algebraic in (True, False):
self.__sens_algebraic = sens_algebraic
else:
raise Exception('Invalid sens_algebraic value. sens_algebraic must be a Boolean.')
@output_z.setter
def output_z(self, output_z):
if output_z in (True, False):
self.__output_z = output_z
else:
raise Exception('Invalid output_z value. output_z must be a Boolean.')
@sim_method_jac_reuse.setter
def sim_method_jac_reuse(self, sim_method_jac_reuse):
if sim_method_jac_reuse in (0, 1):
self.__sim_method_jac_reuse = sim_method_jac_reuse
else:
raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be 0 or 1.')
class AcadosSim:
"""
The class has the following properties that can be modified to formulate a specific simulation problem, see below:
:param acados_path: string with the path to acados. It is used to generate the include and lib paths.
- :py:attr:`dims` of type :py:class:`acados_template.acados_ocp.AcadosSimDims` - are automatically detected from model
- :py:attr:`model` of type :py:class:`acados_template.acados_model.AcadosModel`
- :py:attr:`solver_options` of type :py:class:`acados_template.acados_sim.AcadosSimOpts`
- :py:attr:`acados_include_path` (set automatically)
- :py:attr:`shared_lib_ext` (set automatically)
- :py:attr:`acados_lib_path` (set automatically)
- :py:attr:`parameter_values` - used to initialize the parameters (can be changed)
"""
def __init__(self, acados_path=''):
if acados_path == '':
acados_path = get_acados_path()
self.dims = AcadosSimDims()
"""Dimension definitions, automatically detected from :py:attr:`model`. Type :py:class:`acados_template.acados_sim.AcadosSimDims`"""
self.model = AcadosModel()
"""Model definitions, type :py:class:`acados_template.acados_model.AcadosModel`"""
self.solver_options = AcadosSimOpts()
"""Solver Options, type :py:class:`acados_template.acados_sim.AcadosSimOpts`"""
self.acados_include_path = os.path.join(acados_path, 'include').replace(os.sep, '/') # the replace part is important on Windows for CMake
"""Path to acados include directory (set automatically), type: `string`"""
self.acados_lib_path = os.path.join(acados_path, 'lib').replace(os.sep, '/') # the replace part is important on Windows for CMake
"""Path to where acados library is located (set automatically), type: `string`"""
self.code_export_directory = 'c_generated_code'
"""Path to where code will be exported. Default: `c_generated_code`."""
self.shared_lib_ext = get_lib_ext()
# get cython paths
from sysconfig import get_paths
self.cython_include_dirs = [np.get_include(), get_paths()['include']]
self.__parameter_values = np.array([])
self.__problem_class = 'SIM'
@property
def parameter_values(self):
""":math:`p` - initial values for parameter - can be updated"""
return self.__parameter_values
@parameter_values.setter
def parameter_values(self, parameter_values):
if isinstance(parameter_values, np.ndarray):
self.__parameter_values = parameter_values
else:
raise Exception('Invalid parameter_values value. ' +
f'Expected numpy array, got {type(parameter_values)}.')
def set(self, attr, value):
# tokenize string
tokens = attr.split('_', 1)
if len(tokens) > 1:
setter_to_call = getattr(getattr(self, tokens[0]), 'set')
else:
setter_to_call = getattr(self, 'set')
setter_to_call(tokens[1], value)
return

53
third_party/acados/acados_template/acados_sim_layout.json vendored Normal file
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"
]
}
}

559
third_party/acados/acados_template/acados_sim_solver.py vendored Normal file
View File

@@ -0,0 +1,559 @@
# -*- coding: future_fstrings -*-
#
# 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

64
third_party/acados/acados_template/acados_sim_solver_common.pxd vendored Normal file
View File

@@ -0,0 +1,64 @@
# -*- coding: future_fstrings -*-
#
# 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.;
#
cdef extern from "acados/sim/sim_common.h":
ctypedef struct sim_config:
pass
ctypedef struct sim_opts:
pass
ctypedef struct sim_in:
pass
ctypedef struct sim_out:
pass
cdef extern from "acados_c/sim_interface.h":
ctypedef struct sim_plan:
pass
ctypedef struct sim_solver:
pass
# out
void sim_out_get(sim_config *config, void *dims, sim_out *out, const char *field, void *value)
int sim_dims_get_from_attr(sim_config *config, void *dims, const char *field, void *dims_data)
# opts
void sim_opts_set(sim_config *config, void *opts_, const char *field, void *value)
# get/set
void sim_in_set(sim_config *config, void *dims, sim_in *sim_in, const char *field, void *value)
void sim_solver_set(sim_solver *solver, const char *field, void *value)

256
third_party/acados/acados_template/acados_sim_solver_pyx.pyx vendored Normal file
View File

@@ -0,0 +1,256 @@
# -*- coding: future_fstrings -*-
#
# 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.;
#
# cython: language_level=3
# cython: profile=False
# distutils: language=c
cimport cython
from libc cimport string
# from libc cimport bool as bool_t
cimport acados_sim_solver_common
cimport acados_sim_solver
cimport numpy as cnp
import os
from datetime import datetime
import numpy as np
cdef class AcadosSimSolverCython:
"""
Class to interact with the acados sim solver C object.
"""
cdef acados_sim_solver.sim_solver_capsule *capsule
cdef void *sim_dims
cdef acados_sim_solver_common.sim_opts *sim_opts
cdef acados_sim_solver_common.sim_config *sim_config
cdef acados_sim_solver_common.sim_out *sim_out
cdef acados_sim_solver_common.sim_in *sim_in
cdef acados_sim_solver_common.sim_solver *sim_solver
cdef bint solver_created
cdef str model_name
cdef str sim_solver_type
cdef list gettable_vectors
cdef list gettable_matrices
cdef list gettable_scalars
def __cinit__(self, model_name):
self.solver_created = False
self.model_name = model_name
# create capsule
self.capsule = acados_sim_solver.acados_sim_solver_create_capsule()
# create solver
assert acados_sim_solver.acados_sim_create(self.capsule) == 0
self.solver_created = True
# get pointers solver
self.__get_pointers_solver()
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 __get_pointers_solver(self):
"""
Private function to get the pointers for solver
"""
# get pointers solver
self.sim_opts = acados_sim_solver.acados_get_sim_opts(self.capsule)
self.sim_dims = acados_sim_solver.acados_get_sim_dims(self.capsule)
self.sim_config = acados_sim_solver.acados_get_sim_config(self.capsule)
self.sim_out = acados_sim_solver.acados_get_sim_out(self.capsule)
self.sim_in = acados_sim_solver.acados_get_sim_in(self.capsule)
self.sim_solver = acados_sim_solver.acados_get_sim_solver(self.capsule)
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 sim with current input.
"""
return acados_sim_solver.acados_sim_solve(self.capsule)
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:
return self.__get_vector(field)
elif field_ in self.gettable_matrices:
return self.__get_matrix(field)
elif field_ in self.gettable_scalars:
return self.__get_scalar(field)
else:
raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \
f' available fields are {", ".join(self.gettable.keys())}')
def __get_scalar(self, field):
cdef double scalar
acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> &scalar)
return scalar
def __get_vector(self, field):
cdef int[2] dims
acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0])
# cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((dims[0],), dtype=np.float64))
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((dims[0]),)
acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> out.data)
return out
def __get_matrix(self, field):
cdef int[2] dims
acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0])
cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F', dtype=np.float64)
acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> out.data)
return out
def set(self, field_: str, value_):
"""
Set numerical data inside the solver.
:param field: string in ['p', 'seed_adj', 'T', 'x', 'u', 'xdot', 'z']
:param value: the value with appropriate size.
"""
settable = ['seed_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw
# cast value_ to avoid conversion issues
if isinstance(value_, (float, int)):
value_ = np.array([value_])
# if len(value_.shape) > 1:
# raise RuntimeError('AcadosSimSolverCython.set(): value_ should be 1 dimensional')
cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(value_, dtype=np.float64).flatten()
field = field_.encode('utf-8')
cdef int[2] dims
# treat parameters separately
if field_ == 'p':
assert acados_sim_solver.acados_sim_update_params(self.capsule, <double *> value.data, value.shape[0]) == 0
return
else:
acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0])
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'AcadosSimSolverCython.set(): mismatching dimension' \
f' for field "{field_}" with dimension {tuple(dims)} (you have {value_shape}).')
# set
if field_ in ['xdot', 'z']:
acados_sim_solver_common.sim_solver_set(self.sim_solver, field, <void *> value.data)
elif field_ in settable:
acados_sim_solver_common.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, <void *> value.data)
else:
raise Exception(f'AcadosSimSolverCython.set(): Unknown field {field_},' \
f' available fields are {", ".join(settable)}')
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')
if not isinstance(value_, bool):
raise TypeError("options_set: expected boolean for value")
cdef bint bool_value = value_
acados_sim_solver_common.sim_opts_set(self.sim_config, self.sim_opts, field, <void *> &bool_value)
# TODO: only allow setting
# if getattr(self.acados_sim.solver_options, field_) or value_ == False:
# acados_sim_solver_common.sim_opts_set(self.sim_config, self.sim_opts, field, <void *> &bool_value)
# 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:
acados_sim_solver.acados_sim_free(self.capsule)
acados_sim_solver.acados_sim_solver_free_capsule(self.capsule)

100
third_party/acados/acados_template/acados_solver_common.pxd vendored Normal file
View File

@@ -0,0 +1,100 @@
# -*- coding: future_fstrings -*-
#
# 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.;
#
cdef extern from "acados/ocp_nlp/ocp_nlp_common.h":
ctypedef struct ocp_nlp_config:
pass
ctypedef struct ocp_nlp_dims:
pass
ctypedef struct ocp_nlp_in:
pass
ctypedef struct ocp_nlp_out:
pass
cdef extern from "acados_c/ocp_nlp_interface.h":
ctypedef enum ocp_nlp_solver_t:
pass
ctypedef enum ocp_nlp_cost_t:
pass
ctypedef enum ocp_nlp_dynamics_t:
pass
ctypedef enum ocp_nlp_constraints_t:
pass
ctypedef enum ocp_nlp_reg_t:
pass
ctypedef struct ocp_nlp_plan:
pass
ctypedef struct ocp_nlp_solver:
pass
int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in_,
int start_stage, const char *field, void *value)
int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims,
ocp_nlp_in *in_, int stage, const char *field, void *value)
# out
void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
int stage, const char *field, void *value)
void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
int stage, const char *field, void *value)
void ocp_nlp_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_solver *solver,
int stage, const char *field, void *value)
int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
int stage, const char *field)
void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
int stage, const char *field, int *dims_out)
void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
int stage, const char *field, int *dims_out)
void ocp_nlp_qp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
int stage, const char *field, int *dims_out)
# opts
void ocp_nlp_solver_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void* value)
# solver
void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out)
void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out)
void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in_, ocp_nlp_out *nlp_out)
# get/set
void ocp_nlp_get(ocp_nlp_config *config, ocp_nlp_solver *solver, const char *field, void *return_value_)
void ocp_nlp_set(ocp_nlp_config *config, ocp_nlp_solver *solver, int stage, const char *field, void *value)

131
third_party/acados/acados_template/builders.py vendored Normal file
View File

@@ -0,0 +1,131 @@
# -*- coding: future_fstrings -*-
#
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
#
# This file is part of acados.
#
# The 2-Clause BSD License
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.;
#
import os
import sys
from subprocess import DEVNULL, call, STDOUT
class CMakeBuilder:
"""
Class to work with the `CMake` build system.
"""
def __init__(self):
self._source_dir = None # private source directory, this is set to code_export_dir
self.build_dir = 'build'
self._build_dir = None # private build directory, usually rendered to abspath(build_dir)
self.generator = None
"""Defines the generator, options can be found via `cmake --help` under 'Generator'. Type: string. Linux default 'Unix Makefiles', Windows 'Visual Studio 15 2017 Win64'; default value: `None`."""
# set something for Windows
if os.name == 'nt':
self.generator = 'Visual Studio 15 2017 Win64'
self.build_targets = None
"""A comma-separated list of the build targets, if `None` then all targets will be build; type: List of strings; default: `None`."""
self.options_on = None
"""List of strings as CMake options which are translated to '-D Opt[0]=ON -D Opt[1]=ON ...'; default: `None`."""
# Generate the command string for handling the cmake command.
def get_cmd1_cmake(self):
defines_str = ''
if self.options_on is not None:
defines_arr = [f' -D{opt}=ON' for opt in self.options_on]
defines_str = ' '.join(defines_arr)
generator_str = ''
if self.generator is not None:
generator_str = f' -G"{self.generator}"'
return f'cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX="{self._source_dir}"{defines_str}{generator_str} -Wdev -S"{self._source_dir}" -B"{self._build_dir}"'
# Generate the command string for handling the build.
def get_cmd2_build(self):
import multiprocessing
cmd = f'cmake --build "{self._build_dir}" --config Release -j{multiprocessing.cpu_count()}'
if self.build_targets is not None:
cmd += f' -t {self.build_targets}'
return cmd
# Generate the command string for handling the install command.
def get_cmd3_install(self):
return f'cmake --install "{self._build_dir}"'
def exec(self, code_export_directory, verbose=True):
"""
Execute the compilation using `CMake` with the given settings.
:param code_export_directory: must be the absolute path to the directory where the code was exported to
"""
if(os.path.isabs(code_export_directory) is False):
print(f'(W) the code export directory "{code_export_directory}" is not an absolute path!')
self._source_dir = code_export_directory
self._build_dir = os.path.abspath(self.build_dir)
try:
os.mkdir(self._build_dir)
except FileExistsError as e:
pass
try:
os.chdir(self._build_dir)
cmd_str = self.get_cmd1_cmake()
print(f'call("{cmd_str})"')
retcode = call(
cmd_str,
shell=True,
stdout=None if verbose else DEVNULL,
stderr=None if verbose else STDOUT
)
if retcode != 0:
raise RuntimeError(f'CMake command "{cmd_str}" was terminated by signal {retcode}')
cmd_str = self.get_cmd2_build()
print(f'call("{cmd_str}")')
retcode = call(
cmd_str,
shell=True,
stdout=None if verbose else DEVNULL,
stderr=None if verbose else STDOUT
)
if retcode != 0:
raise RuntimeError(f'Build command "{cmd_str}" was terminated by signal {retcode}')
cmd_str = self.get_cmd3_install()
print(f'call("{cmd_str}")')
retcode = call(
cmd_str,
shell=True,
stdout=None if verbose else DEVNULL,
stderr=None if verbose else STDOUT
)
if retcode != 0:
raise RuntimeError(f'Install command "{cmd_str}" was terminated by signal {retcode}')
except OSError as e:
print("Execution failed:", e, file=sys.stderr)
except Exception as e:
print("Execution failed:", e, file=sys.stderr)
exit(1)

397
third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt vendored Normal file
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
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 %}

526
third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c vendored Normal file
View File

@@ -0,0 +1,526 @@
/*
* 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.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 %}
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados_c/external_function_interface.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#include "acados/sim/sim_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/print.h"
// example specific
#include "{{ model.name }}_model/{{ model.name }}_model.h"
#include "acados_sim_solver_{{ model.name }}.h"
// ** solver data **
sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule()
{
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
return capsule;
}
int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
{
free(capsule);
return 0;
}
int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
{
// initialize
const int nx = {{ model.name | upper }}_NX;
const int nu = {{ model.name | upper }}_NU;
const int nz = {{ model.name | upper }}_NZ;
const int np = {{ model.name | upper }}_NP;
bool tmp_bool;
{#// double Tsim = {{ solver_options.tf / dims.N }};#}
double Tsim = {{ solver_options.Tsim }};
{% if solver_options.integrator_type == "IRK" %}
capsule->sim_impl_dae_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
{%- if model.dyn_ext_fun_type == "casadi" %}
// external functions (implicit model)
capsule->sim_impl_dae_fun->casadi_fun = &{{ model.name }}_impl_dae_fun;
capsule->sim_impl_dae_fun->casadi_work = &{{ model.name }}_impl_dae_fun_work;
capsule->sim_impl_dae_fun->casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in;
capsule->sim_impl_dae_fun->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out;
capsule->sim_impl_dae_fun->casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in;
capsule->sim_impl_dae_fun->casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out;
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_fun, np);
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z;
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_work;
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in;
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out;
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in;
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out;
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np);
// external_function_param_{{ model.dyn_ext_fun_type }} impl_dae_jac_x_xdot_u_z;
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_fun = &{{ model.name }}_impl_dae_jac_x_xdot_u_z;
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_work = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_work;
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in;
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out;
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in;
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out;
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np);
{%- else %}
capsule->sim_impl_dae_fun->fun = &{{ model.dyn_impl_dae_fun }};
capsule->sim_impl_dae_fun_jac_x_xdot_z->fun = &{{ model.dyn_impl_dae_fun_jac }};
capsule->sim_impl_dae_jac_x_xdot_u_z->fun = &{{ model.dyn_impl_dae_jac }};
{%- endif %}
{%- if hessian_approx == "EXACT" %}
capsule->sim_impl_dae_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
// external_function_param_{{ model.dyn_ext_fun_type }} impl_dae_jac_x_xdot_u_z;
capsule->sim_impl_dae_hess->casadi_fun = &{{ model.name }}_impl_dae_hess;
capsule->sim_impl_dae_hess->casadi_work = &{{ model.name }}_impl_dae_hess_work;
capsule->sim_impl_dae_hess->casadi_sparsity_in = &{{ model.name }}_impl_dae_hess_sparsity_in;
capsule->sim_impl_dae_hess->casadi_sparsity_out = &{{ model.name }}_impl_dae_hess_sparsity_out;
capsule->sim_impl_dae_hess->casadi_n_in = &{{ model.name }}_impl_dae_hess_n_in;
capsule->sim_impl_dae_hess->casadi_n_out = &{{ model.name }}_impl_dae_hess_n_out;
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_hess, np);
{%- endif %}
{% elif solver_options.integrator_type == "ERK" %}
// explicit ode
capsule->sim_forw_vde_casadi = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
capsule->sim_vde_adj_casadi = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
capsule->sim_expl_ode_fun_casadi = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
capsule->sim_forw_vde_casadi->casadi_fun = &{{ model.name }}_expl_vde_forw;
capsule->sim_forw_vde_casadi->casadi_n_in = &{{ model.name }}_expl_vde_forw_n_in;
capsule->sim_forw_vde_casadi->casadi_n_out = &{{ model.name }}_expl_vde_forw_n_out;
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &{{ model.name }}_expl_vde_forw_sparsity_in;
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &{{ model.name }}_expl_vde_forw_sparsity_out;
capsule->sim_forw_vde_casadi->casadi_work = &{{ model.name }}_expl_vde_forw_work;
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_vde_adj_casadi->casadi_fun = &{{ model.name }}_expl_vde_adj;
capsule->sim_vde_adj_casadi->casadi_n_in = &{{ model.name }}_expl_vde_adj_n_in;
capsule->sim_vde_adj_casadi->casadi_n_out = &{{ model.name }}_expl_vde_adj_n_out;
capsule->sim_vde_adj_casadi->casadi_sparsity_in = &{{ model.name }}_expl_vde_adj_sparsity_in;
capsule->sim_vde_adj_casadi->casadi_sparsity_out = &{{ model.name }}_expl_vde_adj_sparsity_out;
capsule->sim_vde_adj_casadi->casadi_work = &{{ model.name }}_expl_vde_adj_work;
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_vde_adj_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &{{ model.name }}_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &{{ model.name }}_expl_ode_fun_n_in;
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &{{ model.name }}_expl_ode_fun_n_out;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &{{ model.name }}_expl_ode_fun_sparsity_in;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &{{ model.name }}_expl_ode_fun_sparsity_out;
capsule->sim_expl_ode_fun_casadi->casadi_work = &{{ model.name }}_expl_ode_fun_work;
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_expl_ode_fun_casadi, np);
{%- if hessian_approx == "EXACT" %}
capsule->sim_expl_ode_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
// external_function_param_{{ model.dyn_ext_fun_type }} impl_dae_jac_x_xdot_u_z;
capsule->sim_expl_ode_hess->casadi_fun = &{{ model.name }}_expl_ode_hess;
capsule->sim_expl_ode_hess->casadi_work = &{{ model.name }}_expl_ode_hess_work;
capsule->sim_expl_ode_hess->casadi_sparsity_in = &{{ model.name }}_expl_ode_hess_sparsity_in;
capsule->sim_expl_ode_hess->casadi_sparsity_out = &{{ model.name }}_expl_ode_hess_sparsity_out;
capsule->sim_expl_ode_hess->casadi_n_in = &{{ model.name }}_expl_ode_hess_n_in;
capsule->sim_expl_ode_hess->casadi_n_out = &{{ model.name }}_expl_ode_hess_n_out;
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_expl_ode_hess, np);
{%- endif %}
{% elif solver_options.integrator_type == "GNSF" -%}
{% if model.gnsf.purely_linear != 1 %}
capsule->sim_gnsf_phi_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
{% if model.gnsf.nontrivial_f_LO == 1 %}
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
{%- endif %}
{%- endif %}
capsule->sim_gnsf_get_matrices_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
{% if model.gnsf.purely_linear != 1 %}
capsule->sim_gnsf_phi_fun->casadi_fun = &{{ model.name }}_gnsf_phi_fun;
capsule->sim_gnsf_phi_fun->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in;
capsule->sim_gnsf_phi_fun->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out;
capsule->sim_gnsf_phi_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_sparsity_in;
capsule->sim_gnsf_phi_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_sparsity_out;
capsule->sim_gnsf_phi_fun->casadi_work = &{{ model.name }}_gnsf_phi_fun_work;
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_phi_fun, np);
capsule->sim_gnsf_phi_fun_jac_y->casadi_fun = &{{ model.name }}_gnsf_phi_fun_jac_y;
capsule->sim_gnsf_phi_fun_jac_y->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_jac_y_n_in;
capsule->sim_gnsf_phi_fun_jac_y->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_jac_y_n_out;
capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in;
capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out;
capsule->sim_gnsf_phi_fun_jac_y->casadi_work = &{{ model.name }}_gnsf_phi_fun_jac_y_work;
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_phi_fun_jac_y, np);
capsule->sim_gnsf_phi_jac_y_uhat->casadi_fun = &{{ model.name }}_gnsf_phi_jac_y_uhat;
capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_in;
capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_out;
capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in;
capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out;
capsule->sim_gnsf_phi_jac_y_uhat->casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work;
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_phi_jac_y_uhat, np);
{% if model.gnsf.nontrivial_f_LO == 1 %}
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out;
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in;
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out;
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work;
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np);
{%- endif %}
{%- endif %}
capsule->sim_gnsf_get_matrices_fun->casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
capsule->sim_gnsf_get_matrices_fun->casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
capsule->sim_gnsf_get_matrices_fun->casadi_n_out = &{{ model.name }}_gnsf_get_matrices_fun_n_out;
capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_in;
capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_out;
capsule->sim_gnsf_get_matrices_fun->casadi_work = &{{ model.name }}_gnsf_get_matrices_fun_work;
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_get_matrices_fun, np);
{% endif %}
// sim plan & config
sim_solver_plan_t plan;
plan.sim_solver = {{ solver_options.integrator_type }};
// create correct config based on plan
sim_config * {{ model.name }}_sim_config = sim_config_create(plan);
capsule->acados_sim_config = {{ model.name }}_sim_config;
// sim dims
void *{{ model.name }}_sim_dims = sim_dims_create({{ model.name }}_sim_config);
capsule->acados_sim_dims = {{ model.name }}_sim_dims;
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nx", &nx);
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nu", &nu);
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nz", &nz);
{% if solver_options.integrator_type == "GNSF" %}
int gnsf_nx1 = {{ dims.gnsf_nx1 }};
int gnsf_nz1 = {{ dims.gnsf_nz1 }};
int gnsf_nout = {{ dims.gnsf_nout }};
int gnsf_ny = {{ dims.gnsf_ny }};
int gnsf_nuhat = {{ dims.gnsf_nuhat }};
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nx1", &gnsf_nx1);
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nz1", &gnsf_nz1);
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nout", &gnsf_nout);
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "ny", &gnsf_ny);
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nuhat", &gnsf_nuhat);
{% endif %}
// sim opts
sim_opts *{{ model.name }}_sim_opts = sim_opts_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims);
capsule->acados_sim_opts = {{ model.name }}_sim_opts;
int tmp_int = {{ solver_options.sim_method_newton_iter }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "newton_iter", &tmp_int);
double tmp_double = {{ solver_options.sim_method_newton_tol }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "newton_tol", &tmp_double);
sim_collocation_type collocation_type = {{ solver_options.collocation_type }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "collocation_type", &collocation_type);
{% if problem_class == "SIM" %}
tmp_int = {{ solver_options.sim_method_num_stages }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_stages", &tmp_int);
tmp_int = {{ solver_options.sim_method_num_steps }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_steps", &tmp_int);
// options that are not available to AcadosOcpSolver
// (in OCP they will be determined by other options, like exact_hessian)
tmp_bool = {{ solver_options.sens_forw }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_forw", &tmp_bool);
tmp_bool = {{ solver_options.sens_adj }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_adj", &tmp_bool);
tmp_bool = {{ solver_options.sens_algebraic }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_algebraic", &tmp_bool);
tmp_bool = {{ solver_options.sens_hess }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_hess", &tmp_bool);
tmp_bool = {{ solver_options.output_z }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "output_z", &tmp_bool);
{% else %} {# num_stages and num_steps of first shooting interval are used #}
tmp_int = {{ solver_options.sim_method_num_stages[0] }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_stages", &tmp_int);
tmp_int = {{ solver_options.sim_method_num_steps[0] }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_steps", &tmp_int);
tmp_bool = {{ solver_options.sim_method_jac_reuse[0] }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "jac_reuse", &tmp_bool);
{% endif %}
// sim in / out
sim_in *{{ model.name }}_sim_in = sim_in_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims);
capsule->acados_sim_in = {{ model.name }}_sim_in;
sim_out *{{ model.name }}_sim_out = sim_out_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims);
capsule->acados_sim_out = {{ model.name }}_sim_out;
sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
{{ model.name }}_sim_in, "T", &Tsim);
// model functions
{%- if solver_options.integrator_type == "IRK" %}
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
"impl_ode_fun", capsule->sim_impl_dae_fun);
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
"impl_ode_fun_jac_x_xdot", capsule->sim_impl_dae_fun_jac_x_xdot_z);
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
"impl_ode_jac_x_xdot_u", capsule->sim_impl_dae_jac_x_xdot_u_z);
{%- if hessian_approx == "EXACT" %}
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
"impl_dae_hess", capsule->sim_impl_dae_hess);
{%- endif %}
{%- elif solver_options.integrator_type == "ERK" %}
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
"expl_vde_forw", capsule->sim_forw_vde_casadi);
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
"expl_vde_adj", capsule->sim_vde_adj_casadi);
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
{%- if hessian_approx == "EXACT" %}
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
"expl_ode_hess", capsule->sim_expl_ode_hess);
{%- endif %}
{%- elif solver_options.integrator_type == "GNSF" %}
{% if model.gnsf.purely_linear != 1 %}
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
"phi_fun", capsule->sim_gnsf_phi_fun);
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
"phi_fun_jac_y", capsule->sim_gnsf_phi_fun_jac_y);
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
"phi_jac_y_uhat", capsule->sim_gnsf_phi_jac_y_uhat);
{% if model.gnsf.nontrivial_f_LO == 1 %}
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
"f_lo_jac_x1_x1dot_u_z", capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
{%- endif %}
{%- endif %}
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
"gnsf_get_matrices_fun", capsule->sim_gnsf_get_matrices_fun);
{%- endif %}
// sim solver
sim_solver *{{ model.name }}_sim_solver = sim_solver_create({{ model.name }}_sim_config,
{{ model.name }}_sim_dims, {{ model.name }}_sim_opts);
capsule->acados_sim_solver = {{ model.name }}_sim_solver;
{% if dims.np > 0 %}
/* initialize parameter values */
double* p = calloc(np, sizeof(double));
{% for item in parameter_values %}
{%- if item != 0 %}
p[{{ loop.index0 }}] = {{ item }};
{%- endif %}
{%- endfor %}
{{ model.name }}_acados_sim_update_params(capsule, p, np);
free(p);
{% endif %}{# if dims.np #}
/* initialize input */
// x
double x0[{{ dims.nx }}];
for (int ii = 0; ii < {{ dims.nx }}; ii++)
x0[ii] = 0.0;
sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
{{ model.name }}_sim_in, "x", x0);
// u
double u0[{{ dims.nu }}];
for (int ii = 0; ii < {{ dims.nu }}; ii++)
u0[ii] = 0.0;
sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
{{ model.name }}_sim_in, "u", u0);
// S_forw
double S_forw[{{ dims.nx * (dims.nx + dims.nu) }}];
for (int ii = 0; ii < {{ dims.nx * (dims.nx + dims.nu) }}; ii++)
S_forw[ii] = 0.0;
for (int ii = 0; ii < {{ dims.nx }}; ii++)
S_forw[ii + ii * {{ dims.nx }} ] = 1.0;
sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
{{ model.name }}_sim_in, "S_forw", S_forw);
int status = sim_precompute({{ model.name }}_sim_solver, {{ model.name }}_sim_in, {{ model.name }}_sim_out);
return status;
}
int {{ model.name }}_acados_sim_solve(sim_solver_capsule *capsule)
{
// integrate dynamics using acados sim_solver
int status = sim_solve(capsule->acados_sim_solver,
capsule->acados_sim_in, capsule->acados_sim_out);
if (status != 0)
printf("error in {{ model.name }}_acados_sim_solve()! Exiting.\n");
return status;
}
int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule)
{
// free memory
sim_solver_destroy(capsule->acados_sim_solver);
sim_in_destroy(capsule->acados_sim_in);
sim_out_destroy(capsule->acados_sim_out);
sim_opts_destroy(capsule->acados_sim_opts);
sim_dims_destroy(capsule->acados_sim_dims);
sim_config_destroy(capsule->acados_sim_config);
// free external function
{%- if solver_options.integrator_type == "IRK" %}
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_fun);
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_fun_jac_x_xdot_z);
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_jac_x_xdot_u_z);
{%- if hessian_approx == "EXACT" %}
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_hess);
{%- endif %}
{%- elif solver_options.integrator_type == "ERK" %}
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_forw_vde_casadi);
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_vde_adj_casadi);
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_expl_ode_fun_casadi);
{%- if hessian_approx == "EXACT" %}
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_expl_ode_hess);
{%- endif %}
{%- elif solver_options.integrator_type == "GNSF" %}
{% if model.gnsf.purely_linear != 1 %}
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_phi_fun);
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_phi_fun_jac_y);
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_phi_jac_y_uhat);
{% if model.gnsf.nontrivial_f_LO == 1 %}
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
{%- endif %}
{%- endif %}
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_get_matrices_fun);
{% endif %}
return 0;
}
int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
{
int status = 0;
int casadi_np = {{ model.name | upper }}_NP;
if (casadi_np != np) {
printf("{{ model.name }}_acados_sim_update_params: trying to set %i parameters for external functions."
" External function has %i parameters. Exiting.\n", np, casadi_np);
exit(1);
}
{%- if solver_options.integrator_type == "ERK" %}
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
capsule->sim_vde_adj_casadi[0].set_param(capsule->sim_vde_adj_casadi, p);
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
{%- if hessian_approx == "EXACT" %}
capsule->sim_expl_ode_hess[0].set_param(capsule->sim_expl_ode_hess, p);
{%- endif %}
{%- elif solver_options.integrator_type == "IRK" %}
capsule->sim_impl_dae_fun[0].set_param(capsule->sim_impl_dae_fun, p);
capsule->sim_impl_dae_fun_jac_x_xdot_z[0].set_param(capsule->sim_impl_dae_fun_jac_x_xdot_z, p);
capsule->sim_impl_dae_jac_x_xdot_u_z[0].set_param(capsule->sim_impl_dae_jac_x_xdot_u_z, p);
{%- if hessian_approx == "EXACT" %}
capsule->sim_impl_dae_hess[0].set_param(capsule->sim_impl_dae_hess, p);
{%- endif %}
{%- elif solver_options.integrator_type == "GNSF" %}
{% if model.gnsf.purely_linear != 1 %}
capsule->sim_gnsf_phi_fun[0].set_param(capsule->sim_gnsf_phi_fun, p);
capsule->sim_gnsf_phi_fun_jac_y[0].set_param(capsule->sim_gnsf_phi_fun_jac_y, p);
capsule->sim_gnsf_phi_jac_y_uhat[0].set_param(capsule->sim_gnsf_phi_jac_y_uhat, p);
{% if model.gnsf.nontrivial_f_LO == 1 %}
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p);
{%- endif %}
{%- endif %}
capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p);
{% endif %}
return status;
}
/* getters pointers to C objects*/
sim_config * {{ model.name }}_acados_get_sim_config(sim_solver_capsule *capsule)
{
return capsule->acados_sim_config;
};
sim_in * {{ model.name }}_acados_get_sim_in(sim_solver_capsule *capsule)
{
return capsule->acados_sim_in;
};
sim_out * {{ model.name }}_acados_get_sim_out(sim_solver_capsule *capsule)
{
return capsule->acados_sim_out;
};
void * {{ model.name }}_acados_get_sim_dims(sim_solver_capsule *capsule)
{
return capsule->acados_sim_dims;
};
sim_opts * {{ model.name }}_acados_get_sim_opts(sim_solver_capsule *capsule)
{
return capsule->acados_sim_opts;
};
sim_solver * {{ model.name }}_acados_get_sim_solver(sim_solver_capsule *capsule)
{
return capsule->acados_sim_solver;
};

101
third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h vendored Normal file
View File

@@ -0,0 +1,101 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_{{ model.name }}_H_
#define ACADOS_SIM_{{ model.name }}_H_
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#define {{ model.name | upper }}_NX {{ dims.nx }}
#define {{ model.name | upper }}_NZ {{ dims.nz }}
#define {{ model.name | upper }}_NU {{ dims.nu }}
#define {{ model.name | upper }}_NP {{ dims.np }}
#ifdef __cplusplus
extern "C" {
#endif
// ** capsule for solver data **
typedef struct sim_solver_capsule
{
// acados objects
sim_in *acados_sim_in;
sim_out *acados_sim_out;
sim_solver *acados_sim_solver;
sim_opts *acados_sim_opts;
sim_config *acados_sim_config;
void *acados_sim_dims;
/* external functions */
// ERK
external_function_param_{{ model.dyn_ext_fun_type }} * sim_forw_vde_casadi;
external_function_param_{{ model.dyn_ext_fun_type }} * sim_vde_adj_casadi;
external_function_param_{{ model.dyn_ext_fun_type }} * sim_expl_ode_fun_casadi;
external_function_param_{{ model.dyn_ext_fun_type }} * sim_expl_ode_hess;
// IRK
external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_fun;
external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_fun_jac_x_xdot_z;
external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_jac_x_xdot_u_z;
external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_hess;
// GNSF
external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_fun;
external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_fun_jac_y;
external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_jac_y_uhat;
external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_get_matrices_fun;
} sim_solver_capsule;
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_create(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_solve(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
ACADOS_SYMBOL_EXPORT sim_config * {{ model.name }}_acados_get_sim_config(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_in * {{ model.name }}_acados_get_sim_in(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_out * {{ model.name }}_acados_get_sim_out(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT void * {{ model.name }}_acados_get_sim_dims(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_opts * {{ model.name }}_acados_get_sim_opts(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver * {{ model.name }}_acados_get_sim_solver(sim_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule(void);
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_SIM_{{ model.name }}_H_

51
third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.pxd vendored Normal file
View File

@@ -0,0 +1,51 @@
#
# 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.;
#
cimport acados_sim_solver_common
cdef extern from "acados_sim_solver_{{ model.name }}.h":
ctypedef struct sim_solver_capsule "sim_solver_capsule":
pass
sim_solver_capsule * acados_sim_solver_create_capsule "{{ model.name }}_acados_sim_solver_create_capsule"()
int acados_sim_solver_free_capsule "{{ model.name }}_acados_sim_solver_free_capsule"(sim_solver_capsule *capsule)
int acados_sim_create "{{ model.name }}_acados_sim_create"(sim_solver_capsule * capsule)
int acados_sim_solve "{{ model.name }}_acados_sim_solve"(sim_solver_capsule * capsule)
int acados_sim_free "{{ model.name }}_acados_sim_free"(sim_solver_capsule * capsule)
int acados_sim_update_params "{{ model.name }}_acados_sim_update_params"(sim_solver_capsule * capsule, double *value, int np_)
# int acados_sim_update_params_sparse "{{ model.name }}_acados_sim_update_params_sparse"(sim_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
acados_sim_solver_common.sim_in *acados_get_sim_in "{{ model.name }}_acados_get_sim_in"(sim_solver_capsule * capsule)
acados_sim_solver_common.sim_out *acados_get_sim_out "{{ model.name }}_acados_get_sim_out"(sim_solver_capsule * capsule)
acados_sim_solver_common.sim_solver *acados_get_sim_solver "{{ model.name }}_acados_get_sim_solver"(sim_solver_capsule * capsule)
acados_sim_solver_common.sim_config *acados_get_sim_config "{{ model.name }}_acados_get_sim_config"(sim_solver_capsule * capsule)
acados_sim_solver_common.sim_opts *acados_get_sim_opts "{{ model.name }}_acados_get_sim_opts"(sim_solver_capsule * capsule)
void *acados_get_sim_dims "{{ model.name }}_acados_get_sim_dims"(sim_solver_capsule * capsule)

2741
third_party/acados/acados_template/c_templates_tera/acados_solver.in.c vendored Normal file
View File

File diff suppressed because it is too large Load Diff

241
third_party/acados/acados_template/c_templates_tera/acados_solver.in.h vendored Normal file
View File

@@ -0,0 +1,241 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SOLVER_{{ model.name }}_H_
#define ACADOS_SOLVER_{{ model.name }}_H_
#include "acados/utils/types.h"
#include "acados_c/ocp_nlp_interface.h"
#include "acados_c/external_function_interface.h"
#define {{ model.name | upper }}_NX {{ dims.nx }}
#define {{ model.name | upper }}_NZ {{ dims.nz }}
#define {{ model.name | upper }}_NU {{ dims.nu }}
#define {{ model.name | upper }}_NP {{ dims.np }}
#define {{ model.name | upper }}_NBX {{ dims.nbx }}
#define {{ model.name | upper }}_NBX0 {{ dims.nbx_0 }}
#define {{ model.name | upper }}_NBU {{ dims.nbu }}
#define {{ model.name | upper }}_NSBX {{ dims.nsbx }}
#define {{ model.name | upper }}_NSBU {{ dims.nsbu }}
#define {{ model.name | upper }}_NSH {{ dims.nsh }}
#define {{ model.name | upper }}_NSG {{ dims.nsg }}
#define {{ model.name | upper }}_NSPHI {{ dims.nsphi }}
#define {{ model.name | upper }}_NSHN {{ dims.nsh_e }}
#define {{ model.name | upper }}_NSGN {{ dims.nsg_e }}
#define {{ model.name | upper }}_NSPHIN {{ dims.nsphi_e }}
#define {{ model.name | upper }}_NSBXN {{ dims.nsbx_e }}
#define {{ model.name | upper }}_NS {{ dims.ns }}
#define {{ model.name | upper }}_NSN {{ dims.ns_e }}
#define {{ model.name | upper }}_NG {{ dims.ng }}
#define {{ model.name | upper }}_NBXN {{ dims.nbx_e }}
#define {{ model.name | upper }}_NGN {{ dims.ng_e }}
#define {{ model.name | upper }}_NY0 {{ dims.ny_0 }}
#define {{ model.name | upper }}_NY {{ dims.ny }}
#define {{ model.name | upper }}_NYN {{ dims.ny_e }}
#define {{ model.name | upper }}_N {{ dims.N }}
#define {{ model.name | upper }}_NH {{ dims.nh }}
#define {{ model.name | upper }}_NPHI {{ dims.nphi }}
#define {{ model.name | upper }}_NHN {{ dims.nh_e }}
#define {{ model.name | upper }}_NPHIN {{ dims.nphi_e }}
#define {{ model.name | upper }}_NR {{ dims.nr }}
#ifdef __cplusplus
extern "C" {
#endif
{%- if not solver_options.custom_update_filename %}
{%- set custom_update_filename = "" %}
{% else %}
{%- set custom_update_filename = solver_options.custom_update_filename %}
{%- endif %}
// ** capsule for solver data **
typedef struct {{ model.name }}_solver_capsule
{
// acados objects
ocp_nlp_in *nlp_in;
ocp_nlp_out *nlp_out;
ocp_nlp_out *sens_out;
ocp_nlp_solver *nlp_solver;
void *nlp_opts;
ocp_nlp_plan_t *nlp_solver_plan;
ocp_nlp_config *nlp_config;
ocp_nlp_dims *nlp_dims;
// number of expected runtime parameters
unsigned int nlp_np;
/* external functions */
// dynamics
{% if solver_options.integrator_type == "ERK" %}
external_function_param_casadi *forw_vde_casadi;
external_function_param_casadi *expl_ode_fun;
{% if solver_options.hessian_approx == "EXACT" %}
external_function_param_casadi *hess_vde_casadi;
{%- endif %}
{% elif solver_options.integrator_type == "IRK" %}
external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun;
external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun_jac_x_xdot_z;
external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_jac_x_xdot_u_z;
{% if solver_options.hessian_approx == "EXACT" %}
external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_hess;
{%- endif %}
{% elif solver_options.integrator_type == "LIFTED_IRK" %}
external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun;
external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun_jac_x_xdot_u;
{% elif solver_options.integrator_type == "GNSF" %}
external_function_param_casadi *gnsf_phi_fun;
external_function_param_casadi *gnsf_phi_fun_jac_y;
external_function_param_casadi *gnsf_phi_jac_y_uhat;
external_function_param_casadi *gnsf_f_lo_jac_x1_x1dot_u_z;
external_function_param_casadi *gnsf_get_matrices_fun;
{% elif solver_options.integrator_type == "DISCRETE" %}
external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun;
external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt;
{%- if solver_options.hessian_approx == "EXACT" %}
external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt_hess;
{%- endif %}
{%- endif %}
// cost
{% if cost.cost_type == "NONLINEAR_LS" %}
external_function_param_casadi *cost_y_fun;
external_function_param_casadi *cost_y_fun_jac_ut_xt;
external_function_param_casadi *cost_y_hess;
{% elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %}
external_function_param_casadi *conl_cost_fun;
external_function_param_casadi *conl_cost_fun_jac_hess;
{%- elif cost.cost_type == "EXTERNAL" %}
external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun;
external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac;
external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac_hess;
{% endif %}
{% if cost.cost_type_0 == "NONLINEAR_LS" %}
external_function_param_casadi cost_y_0_fun;
external_function_param_casadi cost_y_0_fun_jac_ut_xt;
external_function_param_casadi cost_y_0_hess;
{% elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
external_function_param_casadi conl_cost_0_fun;
external_function_param_casadi conl_cost_0_fun_jac_hess;
{% elif cost.cost_type_0 == "EXTERNAL" %}
external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun;
external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac;
external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac_hess;
{%- endif %}
{% if cost.cost_type_e == "NONLINEAR_LS" %}
external_function_param_casadi cost_y_e_fun;
external_function_param_casadi cost_y_e_fun_jac_ut_xt;
external_function_param_casadi cost_y_e_hess;
{% elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %}
external_function_param_casadi conl_cost_e_fun;
external_function_param_casadi conl_cost_e_fun_jac_hess;
{% elif cost.cost_type_e == "EXTERNAL" %}
external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun;
external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac;
external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac_hess;
{%- endif %}
// constraints
{%- if constraints.constr_type == "BGP" %}
external_function_param_casadi *phi_constraint;
{% elif constraints.constr_type == "BGH" and dims.nh > 0 %}
external_function_param_casadi *nl_constr_h_fun_jac;
external_function_param_casadi *nl_constr_h_fun;
{%- if solver_options.hessian_approx == "EXACT" %}
external_function_param_casadi *nl_constr_h_fun_jac_hess;
{%- endif %}
{%- endif %}
{% if constraints.constr_type_e == "BGP" %}
external_function_param_casadi phi_e_constraint;
{% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
external_function_param_casadi nl_constr_h_e_fun_jac;
external_function_param_casadi nl_constr_h_e_fun;
{%- if solver_options.hessian_approx == "EXACT" %}
external_function_param_casadi nl_constr_h_e_fun_jac_hess;
{%- endif %}
{%- endif %}
{%- if custom_update_filename != "" %}
void * custom_update_memory;
{%- endif %}
} {{ model.name }}_solver_capsule;
ACADOS_SYMBOL_EXPORT {{ model.name }}_solver_capsule * {{ model.name }}_acados_create_capsule(void);
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free_capsule({{ model.name }}_solver_capsule *capsule);
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_create({{ model.name }}_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule, int reset_qp_solver_mem);
/**
* Generic version of {{ model.name }}_acados_create which allows to use a different number of shooting intervals than
* the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code
* generation, the time-steps from code generation is used.
*/
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule * capsule, int n_time_steps, double* new_time_steps);
/**
* Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the
* nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0.
*/
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps);
/**
* This function is used for updating an already initialized solver with a different number of qp_cond_N.
*/
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule * capsule, int qp_solver_cond_N);
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *value, int np);
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_params_sparse({{ model.name }}_solver_capsule * capsule, int stage, int *idx, double *p, int n_update);
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_custom_update({{ model.name }}_solver_capsule* capsule, double* data, int data_len);
ACADOS_SYMBOL_EXPORT ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule);
ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_SOLVER_{{ model.name }}_H_

62
third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd vendored Normal file
View File

@@ -0,0 +1,62 @@
#
# 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.;
#
cimport acados_solver_common
cdef extern from "acados_solver_{{ model.name }}.h":
ctypedef struct nlp_solver_capsule "{{ model.name }}_solver_capsule":
pass
nlp_solver_capsule * acados_create_capsule "{{ model.name }}_acados_create_capsule"()
int acados_free_capsule "{{ model.name }}_acados_free_capsule"(nlp_solver_capsule *capsule)
int acados_create "{{ model.name }}_acados_create"(nlp_solver_capsule * capsule)
int acados_create_with_discretization "{{ model.name }}_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps)
int acados_update_time_steps "{{ model.name }}_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps)
int acados_update_qp_solver_cond_N "{{ model.name }}_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
int acados_update_params "{{ model.name }}_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
int acados_update_params_sparse "{{ model.name }}_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
int acados_solve "{{ model.name }}_acados_solve"(nlp_solver_capsule * capsule)
int acados_reset "{{ model.name }}_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem)
int acados_free "{{ model.name }}_acados_free"(nlp_solver_capsule * capsule)
void acados_print_stats "{{ model.name }}_acados_print_stats"(nlp_solver_capsule * capsule)
int acados_custom_update "{{ model.name }}_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len)
acados_solver_common.ocp_nlp_in *acados_get_nlp_in "{{ model.name }}_acados_get_nlp_in"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_out *acados_get_nlp_out "{{ model.name }}_acados_get_nlp_out"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_out *acados_get_sens_out "{{ model.name }}_acados_get_sens_out"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "{{ model.name }}_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_config *acados_get_nlp_config "{{ model.name }}_acados_get_nlp_config"(nlp_solver_capsule * capsule)
void *acados_get_nlp_opts "{{ model.name }}_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "{{ model.name }}_acados_get_nlp_dims"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "{{ model.name }}_acados_get_nlp_plan"(nlp_solver_capsule * capsule)

110
third_party/acados/acados_template/c_templates_tera/constraints.in.h 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.;
*/
#ifndef {{ model.name }}_CONSTRAINTS
#define {{ model.name }}_CONSTRAINTS
#ifdef __cplusplus
extern "C" {
#endif
{% if dims.nphi > 0 %}
int {{ model.name }}_phi_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_phi_constraint_work(int *, int *, int *, int *);
const int *{{ model.name }}_phi_constraint_sparsity_in(int);
const int *{{ model.name }}_phi_constraint_sparsity_out(int);
int {{ model.name }}_phi_constraint_n_in(void);
int {{ model.name }}_phi_constraint_n_out(void);
{% endif %}
{% if dims.nphi_e > 0 %}
int {{ model.name }}_phi_e_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_phi_e_constraint_work(int *, int *, int *, int *);
const int *{{ model.name }}_phi_e_constraint_sparsity_in(int);
const int *{{ model.name }}_phi_e_constraint_sparsity_out(int);
int {{ model.name }}_phi_e_constraint_n_in(void);
int {{ model.name }}_phi_e_constraint_n_out(void);
{% endif %}
{% if dims.nh > 0 %}
int {{ model.name }}_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *);
const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in(int);
const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out(int);
int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(void);
int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(void);
int {{ model.name }}_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_constr_h_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_constr_h_fun_sparsity_in(int);
const int *{{ model.name }}_constr_h_fun_sparsity_out(int);
int {{ model.name }}_constr_h_fun_n_in(void);
int {{ model.name }}_constr_h_fun_n_out(void);
{% if solver_options.hessian_approx == "EXACT" -%}
int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in(int);
const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out(int);
int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(void);
int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(void);
{% endif %}
{% endif %}
{% if dims.nh_e > 0 %}
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_work(int *, int *, int *, int *);
const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_in(int);
const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_out(int);
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in(void);
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out(void);
int {{ model.name }}_constr_h_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_constr_h_e_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_constr_h_e_fun_sparsity_in(int);
const int *{{ model.name }}_constr_h_e_fun_sparsity_out(int);
int {{ model.name }}_constr_h_e_fun_n_in(void);
int {{ model.name }}_constr_h_e_fun_n_out(void);
{% if solver_options.hessian_approx == "EXACT" -%}
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_in(int);
const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_out(int);
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in(void);
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out(void);
{% endif %}
{% endif %}
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // {{ model.name }}_CONSTRAINTS

238
third_party/acados/acados_template/c_templates_tera/cost.in.h vendored Normal file
View File

@@ -0,0 +1,238 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef {{ model.name }}_COST
#define {{ model.name }}_COST
#ifdef __cplusplus
extern "C" {
#endif
// Cost at initial shooting node
{% if cost.cost_type_0 == "NONLINEAR_LS" %}
int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int);
const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int);
int {{ model.name }}_cost_y_0_fun_n_in(void);
int {{ model.name }}_cost_y_0_fun_n_out(void);
int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(void);
int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(void);
int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int);
const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int);
int {{ model.name }}_cost_y_0_hess_n_in(void);
int {{ model.name }}_cost_y_0_hess_n_out(void);
{% elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
int {{ model.name }}_conl_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_conl_cost_0_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_conl_cost_0_fun_sparsity_in(int);
const int *{{ model.name }}_conl_cost_0_fun_sparsity_out(int);
int {{ model.name }}_conl_cost_0_fun_n_in(void);
int {{ model.name }}_conl_cost_0_fun_n_out(void);
int {{ model.name }}_conl_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_conl_cost_0_fun_jac_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_conl_cost_0_fun_jac_hess_sparsity_in(int);
const int *{{ model.name }}_conl_cost_0_fun_jac_hess_sparsity_out(int);
int {{ model.name }}_conl_cost_0_fun_jac_hess_n_in(void);
int {{ model.name }}_conl_cost_0_fun_jac_hess_n_out(void);
{% elif cost.cost_type_0 == "EXTERNAL" %}
{%- if cost.cost_ext_fun_type_0 == "casadi" %}
int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int);
const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int);
int {{ model.name }}_cost_ext_cost_0_fun_n_in(void);
int {{ model.name }}_cost_ext_cost_0_fun_n_out(void);
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int);
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int);
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(void);
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(void);
int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int);
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int);
int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(void);
int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(void);
{%- else %}
int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *);
{%- endif %}
{% endif %}
// Cost at path shooting node
{% if cost.cost_type == "NONLINEAR_LS" %}
int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_y_fun_sparsity_in(int);
const int *{{ model.name }}_cost_y_fun_sparsity_out(int);
int {{ model.name }}_cost_y_fun_n_in(void);
int {{ model.name }}_cost_y_fun_n_out(void);
int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int);
const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int);
int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(void);
int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(void);
int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_y_hess_sparsity_in(int);
const int *{{ model.name }}_cost_y_hess_sparsity_out(int);
int {{ model.name }}_cost_y_hess_n_in(void);
int {{ model.name }}_cost_y_hess_n_out(void);
{% elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %}
int {{ model.name }}_conl_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_conl_cost_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_conl_cost_fun_sparsity_in(int);
const int *{{ model.name }}_conl_cost_fun_sparsity_out(int);
int {{ model.name }}_conl_cost_fun_n_in(void);
int {{ model.name }}_conl_cost_fun_n_out(void);
int {{ model.name }}_conl_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_conl_cost_fun_jac_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_conl_cost_fun_jac_hess_sparsity_in(int);
const int *{{ model.name }}_conl_cost_fun_jac_hess_sparsity_out(int);
int {{ model.name }}_conl_cost_fun_jac_hess_n_in(void);
int {{ model.name }}_conl_cost_fun_jac_hess_n_out(void);
{% elif cost.cost_type == "EXTERNAL" %}
{%- if cost.cost_ext_fun_type == "casadi" %}
int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int);
const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int);
int {{ model.name }}_cost_ext_cost_fun_n_in(void);
int {{ model.name }}_cost_ext_cost_fun_n_out(void);
int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int);
const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int);
int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(void);
int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(void);
int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int);
const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int);
int {{ model.name }}_cost_ext_cost_fun_jac_n_in(void);
int {{ model.name }}_cost_ext_cost_fun_jac_n_out(void);
{%- else %}
int {{ cost.cost_function_ext_cost }}(void **, void **, void *);
{%- endif %}
{% endif %}
// Cost at terminal shooting node
{% if cost.cost_type_e == "NONLINEAR_LS" %}
int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int);
const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int);
int {{ model.name }}_cost_y_e_fun_n_in(void);
int {{ model.name }}_cost_y_e_fun_n_out(void);
int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(void);
int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(void);
int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int);
const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int);
int {{ model.name }}_cost_y_e_hess_n_in(void);
int {{ model.name }}_cost_y_e_hess_n_out(void);
{% elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %}
int {{ model.name }}_conl_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_conl_cost_e_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_conl_cost_e_fun_sparsity_in(int);
const int *{{ model.name }}_conl_cost_e_fun_sparsity_out(int);
int {{ model.name }}_conl_cost_e_fun_n_in(void);
int {{ model.name }}_conl_cost_e_fun_n_out(void);
int {{ model.name }}_conl_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_conl_cost_e_fun_jac_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_conl_cost_e_fun_jac_hess_sparsity_in(int);
const int *{{ model.name }}_conl_cost_e_fun_jac_hess_sparsity_out(int);
int {{ model.name }}_conl_cost_e_fun_jac_hess_n_in(void);
int {{ model.name }}_conl_cost_e_fun_jac_hess_n_out(void);
{% elif cost.cost_type_e == "EXTERNAL" %}
{%- if cost.cost_ext_fun_type_e == "casadi" %}
int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int);
const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int);
int {{ model.name }}_cost_ext_cost_e_fun_n_in(void);
int {{ model.name }}_cost_ext_cost_e_fun_n_out(void);
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int);
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int);
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(void);
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(void);
int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *);
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int);
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int);
int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(void);
int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(void);
{%- else %}
int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *);
{%- endif %}
{% endif %}
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // {{ model.name }}_COST

226
third_party/acados/acados_template/c_templates_tera/main.in.c vendored Normal file
View File

@@ -0,0 +1,226 @@
/*
* 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 not solver_options.custom_update_filename %}
{%- set custom_update_filename = "" %}
{% else %}
{%- set custom_update_filename = solver_options.custom_update_filename %}
{%- endif %}
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados/utils/print.h"
#include "acados/utils/math.h"
#include "acados_c/ocp_nlp_interface.h"
#include "acados_c/external_function_interface.h"
#include "acados_solver_{{ model.name }}.h"
// blasfeo
#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
#define NX {{ model.name | upper }}_NX
#define NZ {{ model.name | upper }}_NZ
#define NU {{ model.name | upper }}_NU
#define NP {{ model.name | upper }}_NP
#define NBX {{ model.name | upper }}_NBX
#define NBX0 {{ model.name | upper }}_NBX0
#define NBU {{ model.name | upper }}_NBU
#define NSBX {{ model.name | upper }}_NSBX
#define NSBU {{ model.name | upper }}_NSBU
#define NSH {{ model.name | upper }}_NSH
#define NSG {{ model.name | upper }}_NSG
#define NSPHI {{ model.name | upper }}_NSPHI
#define NSHN {{ model.name | upper }}_NSHN
#define NSGN {{ model.name | upper }}_NSGN
#define NSPHIN {{ model.name | upper }}_NSPHIN
#define NSBXN {{ model.name | upper }}_NSBXN
#define NS {{ model.name | upper }}_NS
#define NSN {{ model.name | upper }}_NSN
#define NG {{ model.name | upper }}_NG
#define NBXN {{ model.name | upper }}_NBXN
#define NGN {{ model.name | upper }}_NGN
#define NY0 {{ model.name | upper }}_NY0
#define NY {{ model.name | upper }}_NY
#define NYN {{ model.name | upper }}_NYN
#define NH {{ model.name | upper }}_NH
#define NPHI {{ model.name | upper }}_NPHI
#define NHN {{ model.name | upper }}_NHN
#define NPHIN {{ model.name | upper }}_NPHIN
#define NR {{ model.name | upper }}_NR
int main()
{
{{ model.name }}_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
// there is an opportunity to change the number of shooting intervals in C without new code generation
int N = {{ model.name | upper }}_N;
// allocate the array and fill it accordingly
double* new_time_steps = NULL;
int status = {{ model.name }}_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps);
if (status)
{
printf("{{ model.name }}_acados_create() returned status %d. Exiting.\n", status);
exit(1);
}
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule);
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule);
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule);
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(acados_ocp_capsule);
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(acados_ocp_capsule);
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule);
// initial condition
int idxbx0[NBX0];
{%- for i in range(end=dims.nbx_0) %}
idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }};
{%- endfor %}
double lbx0[NBX0];
double ubx0[NBX0];
{%- for i in range(end=dims.nbx_0) %}
lbx0[{{ i }}] = {{ constraints.lbx_0[i] }};
ubx0[{{ i }}] = {{ constraints.ubx_0[i] }};
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
// initialization for state values
double x_init[NX];
{%- for i in range(end=dims.nx) %}
x_init[{{ i }}] = 0.0;
{%- endfor %}
// initial value for control input
double u0[NU];
{%- for i in range(end=dims.nu) %}
u0[{{ i }}] = 0.0;
{%- endfor %}
{%- if dims.np > 0 %}
// set parameters
double p[NP];
{%- for item in parameter_values %}
p[{{ loop.index0 }}] = {{ item }};
{%- endfor %}
for (int ii = 0; ii <= N; ii++)
{
{{ model.name }}_acados_update_params(acados_ocp_capsule, ii, p, NP);
}
{% endif %}{# if np > 0 #}
// prepare evaluation
int NTIMINGS = 1;
double min_time = 1e12;
double kkt_norm_inf;
double elapsed_time;
int sqp_iter;
double xtraj[NX * (N+1)];
double utraj[NU * N];
// solve ocp in loop
int rti_phase = 0;
for (int ii = 0; ii < NTIMINGS; ii++)
{
// initialize solution
for (int i = 0; i < N; i++)
{
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
}
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x_init);
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
status = {{ model.name }}_acados_solve(acados_ocp_capsule);
ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
min_time = MIN(elapsed_time, min_time);
}
/* print solution and statistics */
for (int ii = 0; ii <= nlp_dims->N; ii++)
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]);
for (int ii = 0; ii < nlp_dims->N; ii++)
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]);
printf("\n--- xtraj ---\n");
d_print_exp_tran_mat( NX, N+1, xtraj, NX);
printf("\n--- utraj ---\n");
d_print_exp_tran_mat( NU, N, utraj, NU );
// ocp_nlp_out_print(nlp_solver->dims, nlp_out);
printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
if (status == ACADOS_SUCCESS)
{
printf("{{ model.name }}_acados_solve(): SUCCESS!\n");
}
else
{
printf("{{ model.name }}_acados_solve() failed with status %d.\n", status);
}
{%- if custom_update_filename != "" %}
{{ model.name }}_acados_custom_update(acados_ocp_capsule, xtraj, NX*(N+1));
{%- endif %}
// get solution
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
{{ model.name }}_acados_print_stats(acados_ocp_capsule);
printf("\nSolver info:\n");
printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n",
sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf);
// free solver
status = {{ model.name }}_acados_free(acados_ocp_capsule);
if (status) {
printf("{{ model.name }}_acados_free() returned status %d. \n", status);
}
// free solver capsule
status = {{ model.name }}_acados_free_capsule(acados_ocp_capsule);
if (status) {
printf("{{ model.name }}_acados_free_capsule() returned status %d. \n", status);
}
return status;
}

132
third_party/acados/acados_template/c_templates_tera/main_sim.in.c vendored Normal file
View File

@@ -0,0 +1,132 @@
/*
* 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.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados/utils/print.h"
#include "acados/utils/math.h"
#include "acados_c/sim_interface.h"
#include "acados_sim_solver_{{ model.name }}.h"
#define NX {{ model.name | upper }}_NX
#define NZ {{ model.name | upper }}_NZ
#define NU {{ model.name | upper }}_NU
#define NP {{ model.name | upper }}_NP
int main()
{
int status = 0;
sim_solver_capsule *capsule = {{ model.name }}_acados_sim_solver_create_capsule();
status = {{ model.name }}_acados_sim_create(capsule);
if (status)
{
printf("acados_create() returned status %d. Exiting.\n", status);
exit(1);
}
sim_config *acados_sim_config = {{ model.name }}_acados_get_sim_config(capsule);
sim_in *acados_sim_in = {{ model.name }}_acados_get_sim_in(capsule);
sim_out *acados_sim_out = {{ model.name }}_acados_get_sim_out(capsule);
void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule);
// initial condition
double x_current[NX];
{%- for i in range(end=dims.nx) %}
x_current[{{ i }}] = 0.0;
{%- endfor %}
{% if constraints.lbx_0 %}
{%- for i in range(end=dims.nbx_0) %}
x_current[{{ constraints.idxbx_0[i] }}] = {{ constraints.lbx_0[i] }};
{%- endfor %}
{% if dims.nbx_0 != dims.nx %}
printf("main_sim: NOTE: initial state not fully defined via lbx_0, using 0.0 for indices that are not in idxbx_0.");
{%- endif %}
{% else %}
printf("main_sim: initial state not defined, should be in lbx_0, using zero vector.");
{%- endif %}
// initial value for control input
double u0[NU];
{%- for i in range(end=dims.nu) %}
u0[{{ i }}] = 0.0;
{%- endfor %}
{%- if dims.np > 0 %}
// set parameters
double p[NP];
{%- for item in parameter_values %}
p[{{ loop.index0 }}] = {{ item }};
{%- endfor %}
{{ model.name }}_acados_sim_update_params(capsule, p, NP);
{% endif %}{# if np > 0 #}
int n_sim_steps = 3;
// solve ocp in loop
for (int ii = 0; ii < n_sim_steps; ii++)
{
sim_in_set(acados_sim_config, acados_sim_dims,
acados_sim_in, "x", x_current);
status = {{ model.name }}_acados_sim_solve(capsule);
if (status != ACADOS_SUCCESS)
{
printf("acados_solve() failed with status %d.\n", status);
}
sim_out_get(acados_sim_config, acados_sim_dims,
acados_sim_out, "x", x_current);
printf("\nx_current, %d\n", ii);
for (int jj = 0; jj < NX; jj++)
{
printf("%e\n", x_current[jj]);
}
}
printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
// free solver
status = {{ model.name }}_acados_sim_free(capsule);
if (status) {
printf("{{ model.name }}_acados_sim_free() returned status %d. \n", status);
}
{{ model.name }}_acados_sim_solver_free_capsule(capsule);
return status;
}

384
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c vendored Normal file
View File

@@ -0,0 +1,384 @@
/*
* 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.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
// acados
#include "acados/utils/print.h"
#include "acados_c/ocp_nlp_interface.h"
#include "acados_solver_{{ model.name }}.h"
// mex
#include "mex.h"
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
long long *l_ptr;
int status = 0;
// create solver
{{ model.name }}_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
status = {{ model.name }}_acados_create(acados_ocp_capsule);
if (status)
{
mexPrintf("{{ model.name }}_acados_create() returned status %d.\n", status);
}
mexPrintf("{{ model.name }}_acados_create() -> success!\n");
// get pointers to nlp solver related objects
ocp_nlp_plan_t *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule);
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule);
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule);
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(acados_ocp_capsule);
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(acados_ocp_capsule);
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule);
// mexPrintf("acados: got pointer to objectes!\n");
// field names of output struct
#define FIELDS_OCP 9
#define FIELDS_EXT_FUN 25
#define MAX_FIELDS 25
char *fieldnames[MAX_FIELDS];
for (int i = 0; i < MAX_FIELDS; i++)
{
fieldnames[i] = (char*) mxMalloc(50);
}
memcpy(fieldnames[0],"config",sizeof("config"));
memcpy(fieldnames[1],"dims",sizeof("dims"));
memcpy(fieldnames[2],"opts",sizeof("opts"));
memcpy(fieldnames[3],"in",sizeof("in"));
memcpy(fieldnames[4],"out",sizeof("out"));
memcpy(fieldnames[5],"solver",sizeof("solver"));
memcpy(fieldnames[6],"sens_out",sizeof("sens_out"));
memcpy(fieldnames[7],"plan",sizeof("plan"));
memcpy(fieldnames[8],"capsule",sizeof("capsule"));
// create output struct - C_ocp
plhs[0] = mxCreateStructMatrix(1, 1, 9, (const char **) fieldnames);
// MEX: config, dims, opts, in, out, solver, sens_out, plan
// plan
mxArray *plan_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(plan_mat);
l_ptr[0] = (long long) nlp_plan;
mxSetField(plhs[0], 0, "plan", plan_mat);
// config
mxArray *config_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(config_mat);
l_ptr[0] = (long long) nlp_config;
mxSetField(plhs[0], 0, "config", config_mat);
// dims
mxArray *dims_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(dims_mat);
l_ptr[0] = (long long) nlp_dims;
mxSetField(plhs[0], 0, "dims", dims_mat);
// opts
mxArray *opts_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(opts_mat);
l_ptr[0] = (long long) nlp_opts;
mxSetField(plhs[0], 0, "opts", opts_mat);
// in
mxArray *in_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(in_mat);
l_ptr[0] = (long long) nlp_in;
mxSetField(plhs[0], 0, "in", in_mat);
// out
mxArray *out_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(out_mat);
l_ptr[0] = (long long) nlp_out;
mxSetField(plhs[0], 0, "out", out_mat);
// solver
mxArray *solver_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(solver_mat);
l_ptr[0] = (long long) nlp_solver;
mxSetField(plhs[0], 0, "solver", solver_mat);
// TODO: sens_out not actually implemented in templates..
// sens_out
mxArray *sens_out_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(sens_out_mat);
l_ptr[0] = (long long) 1;
mxSetField(plhs[0], 0, "sens_out", sens_out_mat);
// capsule
mxArray *capsule_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(capsule_mat);
l_ptr[0] = (long long) acados_ocp_capsule;
mxSetField(plhs[0], 0, "capsule", capsule_mat);
/* store external function pointers */
// dyn
memcpy(fieldnames[0],"expl_ode_fun",sizeof("expl_ode_fun"));
memcpy(fieldnames[1],"forw_vde",sizeof("forw_vde"));
memcpy(fieldnames[2],"hess_vde",sizeof("hess_vde"));
memcpy(fieldnames[3],"impl_dae_fun",sizeof("impl_dae_fun"));
memcpy(fieldnames[4],"impl_dae_fun_jac_x_xdot_z",sizeof("impl_dae_fun_jac_x_xdot_z"));
memcpy(fieldnames[5],"impl_dae_jac_x_xdot_u_z",sizeof("impl_dae_jac_x_xdot_u_z"));
memcpy(fieldnames[6],"impl_dae_hess",sizeof("impl_dae_hess"));
memcpy(fieldnames[7],"gnsf_phi_fun",sizeof("gnsf_phi_fun"));
memcpy(fieldnames[8],"gnsf_phi_fun_jac_y",sizeof("gnsf_phi_fun_jac_y"));
memcpy(fieldnames[9],"gnsf_phi_jac_y_uhat",sizeof("gnsf_phi_jac_y_uhat"));
memcpy(fieldnames[10],"gnsf_f_lo_jac_x1_x1dot_u_z",sizeof("gnsf_f_lo_jac_x1_x1dot_u_z"));
memcpy(fieldnames[11],"gnsf_get_matrices_fun",sizeof("gnsf_get_matrices_fun"));
memcpy(fieldnames[12],"disc_phi_fun",sizeof("disc_phi_fun"));
memcpy(fieldnames[13],"disc_phi_fun_jac",sizeof("disc_phi_fun_jac"));
memcpy(fieldnames[14],"disc_phi_fun_jac_hess",sizeof("disc_phi_fun_jac_hess"));
// cost
memcpy(fieldnames[15],"cost_y_fun",sizeof("cost_y_fun"));
memcpy(fieldnames[16],"cost_y_fun_jac_ut_xt",sizeof("cost_y_fun_jac_ut_xt"));
memcpy(fieldnames[17],"cost_y_hess",sizeof("cost_y_hess"));
memcpy(fieldnames[18],"ext_cost_fun",sizeof("ext_cost_fun"));
memcpy(fieldnames[19],"ext_cost_fun_jac",sizeof("ext_cost_fun_jac"));
memcpy(fieldnames[20],"ext_cost_fun_jac_hess",sizeof("ext_cost_fun_jac_hess"));
// constraints
memcpy(fieldnames[21],"phi_constraint",sizeof("phi_constraint"));
memcpy(fieldnames[22],"nl_constr_h_fun_jac",sizeof("nl_constr_h_fun_jac"));
memcpy(fieldnames[23],"nl_constr_h_fun",sizeof("nl_constr_h_fun"));
memcpy(fieldnames[24],"nl_constr_h_fun_jac_hess",sizeof("nl_constr_h_fun_jac_hess"));
// create output struct - C_ocp_ext_fun
plhs[1] = mxCreateStructMatrix(1, 1, FIELDS_EXT_FUN, (const char **) fieldnames);
for (int i = 0; i < FIELDS_EXT_FUN; i++)
{
mxFree( fieldnames[i] );
}
/* dynamics */
mxArray *expl_ode_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *forw_vde_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *hess_vde_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *impl_dae_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *impl_dae_fun_jac_x_xdot_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *impl_dae_jac_x_xdot_u_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *impl_dae_hess_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *gnsf_phi_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *gnsf_phi_fun_jac_y_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *gnsf_phi_jac_y_uhat_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *gnsf_f_lo_jac_x1_x1dot_u_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *gnsf_get_matrices_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *disc_phi_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *disc_phi_fun_jac_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
mxArray *disc_phi_fun_jac_hess_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
{% if solver_options.integrator_type == "ERK" %}
{# TODO: remove _casadi from these names.. #}
l_ptr = mxGetData(forw_vde_mat);
l_ptr[0] = (long long) acados_ocp_capsule->forw_vde_casadi;
l_ptr = mxGetData(expl_ode_fun_mat);
l_ptr[0] = (long long) acados_ocp_capsule->expl_ode_fun;
{% if solver_options.hessian_approx == "EXACT" %}
l_ptr = mxGetData(hess_vde_mat);
l_ptr[0] = (long long) acados_ocp_capsule->hess_vde_casadi;
{%- endif %}
{% elif solver_options.integrator_type == "IRK" %}
l_ptr = mxGetData(impl_dae_fun_mat);
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_fun;
l_ptr = mxGetData(impl_dae_fun_jac_x_xdot_z_mat);
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_fun_jac_x_xdot_z;
l_ptr = mxGetData(impl_dae_jac_x_xdot_u_z_mat);
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_jac_x_xdot_u_z;
{% if solver_options.hessian_approx == "EXACT" %}
l_ptr = mxGetData(impl_dae_hess_mat);
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_hess;
{%- endif %}
{% elif solver_options.integrator_type == "GNSF" %}
{% if model.gnsf.purely_linear != 1 %}
l_ptr = mxGetData(gnsf_phi_fun_mat);
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun;
l_ptr = mxGetData(gnsf_phi_fun_jac_y_mat);
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun_jac_y;
l_ptr = mxGetData(gnsf_phi_jac_y_uhat_mat);
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_jac_y_uhat;
{% if model.gnsf.nontrivial_f_LO == 1 %}
l_ptr = mxGetData(gnsf_f_lo_jac_x1_x1dot_u_z_mat);
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_f_lo_jac_x1_x1dot_u_z;
{%- endif %}
{%- endif %}
l_ptr = mxGetData(gnsf_get_matrices_fun_mat);
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_get_matrices_fun;
{% elif solver_options.integrator_type == "DISCRETE" %}
l_ptr = mxGetData(disc_phi_fun_mat);
l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun;
l_ptr = mxGetData(disc_phi_fun_jac_mat);
l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun_jac_ut_xt;
{% if solver_options.hessian_approx == "EXACT" %}
l_ptr = mxGetData(disc_phi_fun_jac_hess_mat);
l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun_jac_ut_xt_hess;
{%- endif %}
{%- endif %}
mxSetField(plhs[1], 0, "expl_ode_fun", expl_ode_fun_mat);
mxSetField(plhs[1], 0, "forw_vde", forw_vde_mat);
mxSetField(plhs[1], 0, "hess_vde", hess_vde_mat);
mxSetField(plhs[1], 0, "gnsf_phi_fun", gnsf_phi_fun_mat);
mxSetField(plhs[1], 0, "gnsf_phi_fun_jac_y", gnsf_phi_fun_jac_y_mat);
mxSetField(plhs[1], 0, "gnsf_phi_jac_y_uhat", gnsf_phi_jac_y_uhat_mat);
mxSetField(plhs[1], 0, "gnsf_f_lo_jac_x1_x1dot_u_z", gnsf_f_lo_jac_x1_x1dot_u_z_mat);
mxSetField(plhs[1], 0, "gnsf_get_matrices_fun", gnsf_get_matrices_fun_mat);
mxSetField(plhs[1], 0, "impl_dae_fun", impl_dae_fun_mat);
mxSetField(plhs[1], 0, "impl_dae_fun_jac_x_xdot_z", impl_dae_fun_jac_x_xdot_z_mat);
mxSetField(plhs[1], 0, "impl_dae_jac_x_xdot_u_z", impl_dae_jac_x_xdot_u_z_mat);
mxSetField(plhs[1], 0, "impl_dae_hess", impl_dae_hess_mat);
mxSetField(plhs[1], 0, "disc_phi_fun", disc_phi_fun_mat);
mxSetField(plhs[1], 0, "disc_phi_fun_jac", disc_phi_fun_jac_mat);
mxSetField(plhs[1], 0, "disc_phi_fun_jac_hess", disc_phi_fun_jac_hess_mat);
/* constaints */
mxArray *phi_constraint_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(phi_constraint_mat);
{%- if constraints.constr_type == "BGP" %}
l_ptr[0] = (long long) acados_ocp_capsule->phi_constraint;
{% endif %}
{% if constraints.constr_type_e == "BGP" %}
l_ptr[1] = (long long) &acados_ocp_capsule->phi_e_constraint;
{% endif %}
mxSetField(plhs[1], 0, "phi_constraint", phi_constraint_mat);
mxArray *nl_constr_h_fun_jac_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(nl_constr_h_fun_jac_mat);
{% if constraints.constr_type == "BGH" and dims.nh > 0 %}
l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun_jac;
{% endif %}
{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun_jac;
{%- endif %}
mxSetField(plhs[1], 0, "nl_constr_h_fun_jac", nl_constr_h_fun_jac_mat);
mxArray *nl_constr_h_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(nl_constr_h_fun_mat);
{% if constraints.constr_type == "BGH" and dims.nh > 0 %}
l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun;
{% endif %}
{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun;
{%- endif %}
mxSetField(plhs[1], 0, "nl_constr_h_fun", nl_constr_h_fun_mat);
mxArray *nl_constr_h_fun_jac_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(nl_constr_h_fun_jac_hess_mat);
{% if constraints.constr_type == "BGH" and dims.nh > 0 and solver_options.hessian_approx == "EXACT" %}
l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun_jac_hess;
{% endif %}
{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 and solver_options.hessian_approx == "EXACT" %}
l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun_jac_hess;
{%- endif %}
mxSetField(plhs[1], 0, "nl_constr_h_fun_jac_hess", nl_constr_h_fun_jac_hess_mat);
/* cost */
mxArray *cost_y_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(cost_y_fun_mat);
{% if cost.cost_type == "NONLINEAR_LS" %}
l_ptr[0] = (long long) acados_ocp_capsule->cost_y_fun;
{% endif %}
{% if cost.cost_type_e == "NONLINEAR_LS" %}
l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_fun;
{%- endif %}
mxSetField(plhs[1], 0, "cost_y_fun", cost_y_fun_mat);
mxArray *cost_y_fun_jac_ut_xt_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(cost_y_fun_jac_ut_xt_mat);
{% if cost.cost_type == "NONLINEAR_LS" %}
l_ptr[0] = (long long) acados_ocp_capsule->cost_y_fun_jac_ut_xt;
{% endif %}
{% if cost.cost_type_e == "NONLINEAR_LS" %}
l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_fun_jac_ut_xt;
{%- endif %}
mxSetField(plhs[1], 0, "cost_y_fun_jac_ut_xt", cost_y_fun_jac_ut_xt_mat);
mxArray *cost_y_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(cost_y_hess_mat);
{% if cost.cost_type == "NONLINEAR_LS" %}
l_ptr[0] = (long long) acados_ocp_capsule->cost_y_hess;
{% endif %}
{% if cost.cost_type_e == "NONLINEAR_LS" %}
l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_hess;
{%- endif %}
mxSetField(plhs[1], 0, "cost_y_hess", cost_y_hess_mat);
mxArray *ext_cost_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(ext_cost_fun_mat);
{% if cost.cost_type == "EXTERNAL" %}
l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun;
{% endif -%}
{% if cost.cost_type_e == "EXTERNAL" %}
l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun;
{%- endif %}
mxSetField(plhs[1], 0, "ext_cost_fun", ext_cost_fun_mat);
mxArray *ext_cost_fun_jac_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(ext_cost_fun_jac_mat);
{% if cost.cost_type == "EXTERNAL" %}
l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun_jac;
{% endif -%}
{% if cost.cost_type_e == "EXTERNAL" %}
l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun_jac;
{%- endif %}
mxSetField(plhs[1], 0, "ext_cost_fun_jac", ext_cost_fun_jac_mat);
mxArray *ext_cost_fun_jac_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
l_ptr = mxGetData(ext_cost_fun_jac_hess_mat);
{% if cost.cost_type == "EXTERNAL" %}
l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun_jac_hess;
{% endif -%}
{% if cost.cost_type_e == "EXTERNAL" %}
l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun_jac_hess;
{%- endif %}
mxSetField(plhs[1], 0, "ext_cost_fun_jac_hess", ext_cost_fun_jac_hess_mat);
return;
}

67
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_free.in.c vendored Normal file
View File

@@ -0,0 +1,67 @@
/*
* 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.;
*/
// system
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
// acados
#include "acados_solver_{{ model.name }}.h"
// mex
#include "mex.h"
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int status = 0;
long long *ptr;
// mexPrintf("\nin mex_acados_free\n");
const mxArray *C_ocp = prhs[0];
// capsule
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
status = {{ model.name }}_acados_free(capsule);
if (status)
{
mexPrintf("{{ model.name }}_acados_free() returned status %d.\n", status);
}
status = {{ model.name }}_acados_free_capsule(capsule);
if (status)
{
mexPrintf("{{ model.name }}_acados_free_capsule() returned status %d.\n", status);
}
return;
}

632
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c vendored Normal file
View File

@@ -0,0 +1,632 @@
/*
* 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.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
// acados
#include "acados/utils/print.h"
#include "acados_c/ocp_nlp_interface.h"
#include "acados_solver_{{ model.name }}.h"
// mex
#include "mex.h"
#include "mex_macros.h"
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
long long *ptr;
int acados_size;
mxArray *mex_field;
char fun_name[20] = "ocp_set";
char buffer [500]; // for error messages
/* RHS */
int min_nrhs = 6;
// C ocp
const mxArray *C_ocp = prhs[2];
// capsule
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
// plan
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "plan" ) );
ocp_nlp_plan_t *plan = (ocp_nlp_plan_t *) ptr[0];
// config
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "config" ) );
ocp_nlp_config *config = (ocp_nlp_config *) ptr[0];
// dims
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "dims" ) );
ocp_nlp_dims *dims = (ocp_nlp_dims *) ptr[0];
// opts
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "opts" ) );
void *opts = (void *) ptr[0];
// in
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "in" ) );
ocp_nlp_in *in = (ocp_nlp_in *) ptr[0];
// out
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "out" ) );
ocp_nlp_out *out = (ocp_nlp_out *) ptr[0];
// solver
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "solver" ) );
ocp_nlp_solver *solver = (ocp_nlp_solver *) ptr[0];
const mxArray *C_ext_fun_pointers = prhs[3];
// field
char *field = mxArrayToString( prhs[4] );
// value
double *value = mxGetPr( prhs[5] );
// for checks
int matlab_size = (int) mxGetNumberOfElements( prhs[5] );
int nrow = (int) mxGetM( prhs[5] );
int ncol = (int) mxGetN( prhs[5] );
int N = dims->N;
int nu = dims->nu[0];
int nx = dims->nx[0];
// stage
int s0, se;
if (nrhs==min_nrhs)
{
s0 = 0;
se = N;
}
else if (nrhs==min_nrhs+1)
{
s0 = mxGetScalar( prhs[6] );
if (s0 > N)
{
sprintf(buffer, "ocp_set: N < specified stage = %d\n", s0);
mexErrMsgTxt(buffer);
}
se = s0 + 1;
}
else
{
sprintf(buffer, "ocp_set: wrong nrhs: %d\n", nrhs);
mexErrMsgTxt(buffer);
}
/* Set value */
// constraints
if (!strcmp(field, "constr_x0"))
{
acados_size = nx;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_constraints_model_set(config, dims, in, 0, "lbx", value);
ocp_nlp_constraints_model_set(config, dims, in, 0, "ubx", value);
}
else if (!strcmp(field, "constr_C"))
{
for (int ii=s0; ii<se; ii++)
{
int ng = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
MEX_DIM_CHECK_MAT(fun_name, "constr_C", nrow, ncol, ng, nx);
ocp_nlp_constraints_model_set(config, dims, in, ii, "C", value);
}
}
else if (!strcmp(field, "constr_lbx"))
{
for (int ii=s0; ii<se; ii++)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lbx");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_constraints_model_set(config, dims, in, ii, "lbx", value);
}
}
else if (!strcmp(field, "constr_ubx"))
{
for (int ii=s0; ii<se; ii++)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ubx");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_constraints_model_set(config, dims, in, ii, "ubx", value);
}
}
else if (!strcmp(field, "constr_lbu"))
{
for (int ii=s0; ii<se; ii++)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lbu");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_constraints_model_set(config, dims, in, ii, "lbu", value);
}
}
else if (!strcmp(field, "constr_ubu"))
{
for (int ii=s0; ii<se; ii++)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ubu");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_constraints_model_set(config, dims, in, ii, "ubu", value);
}
}
else if (!strcmp(field, "constr_D"))
{
for (int ii=s0; ii<se; ii++)
{
int ng = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
MEX_DIM_CHECK_MAT(fun_name, "constr_D", nrow, ncol, ng, nu);
ocp_nlp_constraints_model_set(config, dims, in, ii, "D", value);
}
}
else if (!strcmp(field, "constr_lg"))
{
for (int ii=s0; ii<se; ii++)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lg");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_constraints_model_set(config, dims, in, ii, "lg", value);
}
}
else if (!strcmp(field, "constr_ug"))
{
for (int ii=s0; ii<se; ii++)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_constraints_model_set(config, dims, in, ii, "ug", value);
}
}
else if (!strcmp(field, "constr_lh"))
{
for (int ii=s0; ii<se; ii++)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lh");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_constraints_model_set(config, dims, in, ii, "lh", value);
}
}
else if (!strcmp(field, "constr_uh"))
{
for (int ii=s0; ii<se; ii++)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "uh");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_constraints_model_set(config, dims, in, ii, "uh", value);
}
}
// cost:
else if (!strcmp(field, "cost_y_ref"))
{
for (int ii=s0; ii<se; ii++)
{
if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_cost_model_set(config, dims, in, ii, "y_ref", value);
}
else
{
MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
}
}
}
else if (!strcmp(field, "cost_y_ref_e"))
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, N, "y_ref");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_cost_model_set(config, dims, in, N, "y_ref", value);
}
else if (!strcmp(field, "cost_Vu"))
{
for (int ii=s0; ii<se; ii++)
{
if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
{
int ny = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref");
int nu = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "u");
acados_size = ny * nu;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_cost_model_set(config, dims, in, ii, "Vu", value);
}
else
{
MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
}
}
}
else if (!strcmp(field, "cost_Vx"))
{
for (int ii=s0; ii<se; ii++)
{
if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
{
int ny = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref");
int nx = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "x");
acados_size = ny * nx;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_cost_model_set(config, dims, in, ii, "Vx", value);
}
else
{
MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
}
}
}
else if (!strcmp(field, "cost_W"))
{
for (int ii=s0; ii<se; ii++)
{
if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
{
int ny = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "y_ref");
acados_size = ny * ny;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_cost_model_set(config, dims, in, ii, "W", value);
}
else
{
MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
}
}
}
else if (!strcmp(field, "cost_Z"))
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "cost_Z");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
for (int ii=s0; ii<se; ii++)
{
ocp_nlp_cost_model_set(config, dims, in, ii, "Z", value);
}
}
else if (!strcmp(field, "cost_Zl"))
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "Zl");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
for (int ii=s0; ii<se; ii++)
{
ocp_nlp_cost_model_set(config, dims, in, ii, "Zl", value);
}
}
else if (!strcmp(field, "cost_Zu"))
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "Zu");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
for (int ii=s0; ii<se; ii++)
{
ocp_nlp_cost_model_set(config, dims, in, ii, "Zu", value);
}
}
else if (!strcmp(field, "cost_z"))
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "cost_z");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
for (int ii=s0; ii<se; ii++)
{
ocp_nlp_cost_model_set(config, dims, in, ii, "z", value);
}
}
else if (!strcmp(field, "cost_zl"))
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "zl");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
for (int ii=s0; ii<se; ii++)
{
ocp_nlp_cost_model_set(config, dims, in, ii, "zl", value);
}
}
else if (!strcmp(field, "cost_zu"))
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "zu");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
for (int ii=s0; ii<se; ii++)
{
ocp_nlp_cost_model_set(config, dims, in, ii, "zu", value);
}
}
// constraints TODO
// // NOTE(oj): how is it with Jbx, Jbu, idxb can they be changed?!
// else if (!strcmp(field, "constr_lbx"))
// {
// // bounds at 0 are a special case.
// if (s0==0)
// {
// sprintf(buffer, "%s cannot set %s for stage 0", fun_name, field);
// mexErrMsgTxt(buffer);
// }
// }
// initializations
else if (!strcmp(field, "init_x") || !strcmp(field, "x"))
{
if (nrhs == min_nrhs)
{
acados_size = (N+1) * nx;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
for (int ii=0; ii<=N; ii++)
{
ocp_nlp_out_set(config, dims, out, ii, "x", value+ii*nx);
}
}
else // (nrhs == min_nrhs + 1)
{
acados_size = nx;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_out_set(config, dims, out, s0, "x", value);
}
}
else if (!strcmp(field, "init_u") || !strcmp(field, "u"))
{
if (nrhs==min_nrhs)
{
acados_size = N*nu;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
for (int ii=0; ii<N; ii++)
{
ocp_nlp_out_set(config, dims, out, ii, "u", value+ii*nu);
}
}
else // (nrhs == min_nrhs + 1)
{
acados_size = nu;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_out_set(config, dims, out, s0, "u", value);
}
}
else if (!strcmp(field, "init_z")||!strcmp(field, "z"))
{
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
sim_solver_t type = sim_plan.sim_solver;
if (type == IRK)
{
int nz = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "z");
if (nrhs==min_nrhs)
{
acados_size = N*nz;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
for (int ii=0; ii<N; ii++)
{
ocp_nlp_set(config, solver, ii, "z_guess", value+ii*nz);
}
}
else // (nrhs==min_nrhs+1)
{
acados_size = nz;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_set(config, solver, s0, "z_guess", value);
}
}
else
{
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk")
}
}
else if (!strcmp(field, "init_xdot")||!strcmp(field, "xdot"))
{
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
sim_solver_t type = sim_plan.sim_solver;
if (type == IRK)
{
int nx = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "x");
if (nrhs==min_nrhs)
{
acados_size = N*nx;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
for (int ii=0; ii<N; ii++)
{
ocp_nlp_set(config, solver, ii, "xdot_guess", value+ii*nx);
}
}
else // nrhs==min_nrhs+1)
{
acados_size = nx;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_set(config, solver, s0, "xdot_guess", value);
}
}
else
{
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk")
}
}
else if (!strcmp(field, "init_gnsf_phi")||!strcmp(field, "gnsf_phi"))
{
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
sim_solver_t type = sim_plan.sim_solver;
if (type == GNSF)
{
int nout = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "init_gnsf_phi");
if (nrhs==min_nrhs)
{
acados_size = N*nout;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
for (int ii=0; ii<N; ii++)
{
ocp_nlp_set(config, solver, ii, "gnsf_phi_guess", value+ii*nx);
}
}
else // (nrhs==min_nrhs+1)
{
acados_size = nout;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_set(config, solver, s0, "gnsf_phi_guess", value);
}
}
else
{
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_gnsf_phi", "irk_gnsf")
}
}
else if (!strcmp(field, "init_pi")||!strcmp(field, "pi"))
{
if (nrhs==min_nrhs)
{
acados_size = N*nx;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
for (int ii=0; ii<N; ii++)
{
ocp_nlp_out_set(config, dims, out, ii, "pi", value+ii*nx);
}
}
else // (nrhs==min_nrhs+1)
{
acados_size = nx;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_out_set(config, dims, out, s0, "pi", value);
}
}
else if (!strcmp(field, "init_lam")||!strcmp(field, "lam"))
{
if (nrhs==min_nrhs)
{
MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
}
else //(nrhs==min_nrhs+1)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "lam");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_out_set(config, dims, out, s0, "lam", value);
}
}
else if (!strcmp(field, "init_t")||!strcmp(field, "t"))
{
if (nrhs==min_nrhs)
{
MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
}
else //(nrhs==min_nrhs+1)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "t");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_out_set(config, dims, out, s0, "t", value);
}
}
else if (!strcmp(field, "init_sl")||!strcmp(field, "sl"))
{
if (nrhs==min_nrhs)
{
MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
}
else //(nrhs==min_nrhs+1)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "sl");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_out_set(config, dims, out, s0, field, value);
}
}
else if (!strcmp(field, "init_su")||!strcmp(field, "su"))
{
if (nrhs==min_nrhs)
{
MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
}
else //(nrhs==min_nrhs+1)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "su");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_out_set(config, dims, out, s0, field, value);
}
}
else if (!strcmp(field, "p"))
{
if (nrhs==min_nrhs) // all stages
{
for (int ii=0; ii<=N; ii++)
{
{{ model.name }}_acados_update_params(capsule, ii, value, matlab_size);
}
}
else if (nrhs==min_nrhs+1) // one stage
{
int stage = mxGetScalar( prhs[6] );
{{ model.name }}_acados_update_params(capsule, stage, value, matlab_size);
}
}
else if (!strcmp(field, "nlp_solver_max_iter"))
{
acados_size = 1;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
int nlp_solver_max_iter = (int) value[0];
ocp_nlp_solver_opts_set(config, opts, "max_iter", &nlp_solver_max_iter);
}
else if (!strcmp(field, "rti_phase"))
{
acados_size = 1;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
int rti_phase = (int) value[0];
if (plan->nlp_solver == SQP && rti_phase != 0)
{
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, field, "sqp_rti")
}
ocp_nlp_solver_opts_set(config, opts, "rti_phase", &rti_phase);
}
else if (!strcmp(field, "qp_warm_start"))
{
acados_size = 1;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
int qp_warm_start = (int) value[0];
ocp_nlp_solver_opts_set(config, opts, "qp_warm_start", &qp_warm_start);
}
else if (!strcmp(field, "warm_start_first_qp"))
{
acados_size = 1;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
int warm_start_first_qp = (int) value[0];
ocp_nlp_solver_opts_set(config, opts, "warm_start_first_qp", &warm_start_first_qp);
}
else if (!strcmp(field, "print_level"))
{
acados_size = 1;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
int print_level = (int) value[0];
ocp_nlp_solver_opts_set(config, opts, "print_level", &print_level);
}
else
{
MEX_FIELD_NOT_SUPPORTED_SUGGEST(fun_name, field, "p, constr_x0,\
constr_lbx, constr_ubx, constr_C, constr_D, constr_lg, constr_ug, constr_lh, constr_uh,\
constr_lbu, constr_ubu, cost_y_ref[_e], sl, su, x, xdot, u, pi, lam, z, \
cost_Vu, cost_Vx, cost_Vz, cost_W, cost_Z, cost_Zl, cost_Zu, cost_z,\
cost_zl, cost_zu, init_x, init_u, init_z, init_xdot, init_gnsf_phi,\
init_pi, nlp_solver_max_iter, qp_warm_start, warm_start_first_qp, print_level");
}
return;
}

56
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_solve.in.c vendored Normal file
View File

@@ -0,0 +1,56 @@
/*
* 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.;
*/
// system
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
// acados
#include "acados_solver_{{ model.name }}.h"
// mex
#include "mex.h"
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
// C_ocp
long long *ptr;
const mxArray *C_ocp = prhs[0];
// capsule
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
// solve
{{ model.name }}_acados_solve(capsule);
}

230
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_sim_solver_sfun.in.c vendored Normal file
View File

@@ -0,0 +1,230 @@
/*
* 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.;
*/
#define S_FUNCTION_NAME acados_sim_solver_sfunction_{{ model.name }}
#define S_FUNCTION_LEVEL 2
#define MDL_START
// acados
// #include "acados/utils/print.h"
#include "acados_c/ocp_nlp_interface.h"
#include "acados_c/external_function_interface.h"
// example specific
#include "{{ model.name }}_model/{{ model.name }}_model.h"
#include "acados_sim_solver_{{ model.name }}.h"
#include "simstruc.h"
#define SAMPLINGTIME {{ solver_options.Tsim }}
static void mdlInitializeSizes (SimStruct *S)
{
// specify the number of continuous and discrete states
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
{# compute number of input ports #}
{%- set n_inputs = 1 %} {# x0 #}
{%- if dims.nu > 0 %} {# u0 -#}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif %}
{%- if dims.np > 0 %} {# parameters #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif %}
// specify the number of input ports
if ( !ssSetNumInputPorts(S, {{ n_inputs }}) )
return;
// specify the number of output ports
if ( !ssSetNumOutputPorts(S, 1) )
return;
// specify dimension information for the input ports
{%- set i_input = 0 %}
// x0
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nx }});
{%- if dims.nu > 0 %}
{%- set i_input = i_input + 1 %}
// u0
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nu }});
{%- endif %}
{%- if dims.np > 0 %}
{%- set i_input = i_input + 1 %}
// parameters
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.np }});
{%- endif %}
// specify dimension information for the output ports
ssSetOutputPortVectorDimension(S, 0, {{ dims.nx }} ); // xnext
// specify the direct feedthrough status
// should be set to 1 for all inputs used in mdlOutputs
{%- for i in range(end=n_inputs) %}
ssSetInputPortDirectFeedThrough(S, {{ i }}, 1);
{%- endfor %}
// one sample time
ssSetNumSampleTimes(S, 1);
}
#if defined(MATLAB_MEX_FILE)
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
return;
}
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
return;
}
#endif /* MATLAB_MEX_FILE */
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, SAMPLINGTIME);
ssSetOffsetTime(S, 0, 0.0);
}
static void mdlStart(SimStruct *S)
{
sim_solver_capsule *capsule = {{ model.name }}_acados_sim_solver_create_capsule();
{{ model.name }}_acados_sim_create(capsule);
ssSetUserData(S, (void*)capsule);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
sim_solver_capsule *capsule = ssGetUserData(S);
sim_config *acados_sim_config = {{ model.name }}_acados_get_sim_config(capsule);
sim_in *acados_sim_in = {{ model.name }}_acados_get_sim_in(capsule);
sim_out *acados_sim_out = {{ model.name }}_acados_get_sim_out(capsule);
void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule);
// sim_opts * {{ model.name }}_acados_get_sim_opts(capsule);
// sim_solver * {{ model.name }}_acados_get_sim_solver(capsule);
InputRealPtrsType in_sign;
{% set input_sizes = [dims.nx, dims.nu, dims.np] %}
// local buffer
{%- set buffer_size = input_sizes | sort | last %}
real_t buffer[{{ buffer_size }}];
/* go through inputs */
{%- set i_input = 0 %}
// initial condition
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.nx }}; i++)
buffer[i] = (double)(*in_sign[i]);
sim_in_set(acados_sim_config, acados_sim_dims,
acados_sim_in, "x", buffer);
// ssPrintf("\nin acados sim:\n");
// for (int i = 0; i < {{ dims.nx }}; i++) ssPrintf("x0[%d] = %f\n", i, buffer[i]);
// ssPrintf("\n");
{% if dims.nu > 0 %}
// control input - u
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.nu }}; i++)
buffer[i] = (double)(*in_sign[i]);
sim_in_set(acados_sim_config, acados_sim_dims,
acados_sim_in, "u", buffer);
{%- endif %}
{% if dims.np > 0 %}
// parameters
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.np }}; i++)
buffer[i] = (double)(*in_sign[i]);
// update value of parameters
{{ model.name }}_acados_sim_update_params(capsule, buffer, {{ dims.np }});
{%- endif %}
/* call solver */
int acados_status = {{ model.name }}_acados_sim_solve(capsule);
/* set outputs */
real_t *out_x = ssGetOutputPortRealSignal(S, 0);
// get simulated state
sim_out_get(acados_sim_config, acados_sim_dims, acados_sim_out,
"xn", (void *) out_x);
// ssPrintf("\nacados sim solve: returned %d\n", acados_status);
// for (int i = 0; i < {{ dims.nx }}; i++) ssPrintf("x_sim[%d] = %f\n", i, out_x[i]);
// ssPrintf("\n");
}
static void mdlTerminate(SimStruct *S)
{
sim_solver_capsule *capsule = ssGetUserData(S);
{{ model.name }}_acados_sim_free(capsule);
{{ model.name }}_acados_sim_solver_free_capsule(capsule);
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif

853
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c vendored Normal file
View File

@@ -0,0 +1,853 @@
/*
* 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.;
*/
#define S_FUNCTION_NAME acados_solver_sfunction_{{ model.name }}
#define S_FUNCTION_LEVEL 2
#define MDL_START
// acados
// #include "acados/utils/print.h"
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
// example specific
#include "{{ model.name }}_model/{{ model.name }}_model.h"
#include "acados_solver_{{ model.name }}.h"
#include "simstruc.h"
{% if simulink_opts.samplingtime == "t0" -%}
#define SAMPLINGTIME {{ solver_options.time_steps[0] }}
{%- elif simulink_opts.samplingtime == "-1" -%}
#define SAMPLINGTIME -1
{%- else -%}
{{ throw(message = "simulink_opts.samplingtime must be '-1' or 't0', got val") }}
{%- endif %}
static void mdlInitializeSizes (SimStruct *S)
{
// specify the number of continuous and discrete states
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
int N = {{ model.name | upper }}_N;
{%- for key, val in simulink_opts.inputs -%}
{%- if val != 0 and val != 1 -%}
{{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }}
{%- endif -%}
{%- endfor -%}
{#- compute number of input ports #}
{%- set n_inputs = 0 -%}
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 -%} {#- y_ref_0 -#}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref -%} {#- y_ref -#}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e -%} {#- y_ref_e #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- for key, val in simulink_opts.inputs -%}
{%- if val != 0 and val != 1 -%}
{{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }}
{%- endif -%}
{%- endfor -%}
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
{%- set n_inputs = n_inputs + 1 %}
{%- endif -%}
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
{%- set n_inputs = n_inputs + 1 %}
{%- endif -%}
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if simulink_opts.inputs.reset_solver -%} {#- reset_solver #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if simulink_opts.inputs.x_init -%} {#- x_init #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
{%- if simulink_opts.inputs.u_init -%} {#- u_init #}
{%- set n_inputs = n_inputs + 1 -%}
{%- endif -%}
// specify the number of input ports
if ( !ssSetNumInputPorts(S, {{ n_inputs }}) )
return;
// specify the number of output ports
{%- set_global n_outputs = 0 %}
{%- for key, val in simulink_opts.outputs %}
{%- if val == 1 %}
{%- set_global n_outputs = n_outputs + val %}
{%- elif val != 0 %}
{{ throw(message = "simulink_opts.outputs must be 0 or 1, got val") }}
{%- endif %}
{%- endfor %}
if ( !ssSetNumOutputPorts(S, {{ n_outputs }}) )
return;
// specify dimension information for the input ports
{%- set i_input = -1 %}{# note here i_input is 0-based #}
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #}
{%- set i_input = i_input + 1 %}
// lbx_0
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_0 }});
{%- endif %}
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #}
{%- set i_input = i_input + 1 %}
// ubx_0
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_0 }});
{%- endif %}
{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #}
{%- set i_input = i_input + 1 %}
// parameters
ssSetInputPortVectorDimension(S, {{ i_input }}, (N+1) * {{ dims.np }});
{%- endif %}
{%- if dims.ny > 0 and simulink_opts.inputs.y_ref_0 %}
{%- set i_input = i_input + 1 %}
// y_ref_0
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_0 }});
{%- endif %}
{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %}
{%- set i_input = i_input + 1 %}
// y_ref
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.ny }});
{%- endif %}
{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %}
{%- set i_input = i_input + 1 %}
// y_ref_e
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_e }});
{%- endif %}
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #}
{%- set i_input = i_input + 1 %}
// lbx
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.nbx }});
{%- endif %}
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #}
{%- set i_input = i_input + 1 %}
// ubx
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.nbx }});
{%- endif %}
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #}
{%- set i_input = i_input + 1 %}
// lbx_e
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_e }});
{%- endif %}
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #}
{%- set i_input = i_input + 1 %}
// ubx_e
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_e }});
{%- endif %}
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #}
{%- set i_input = i_input + 1 %}
// lbu
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nbu }});
{%- endif -%}
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #}
{%- set i_input = i_input + 1 %}
// ubu
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nbu }});
{%- endif -%}
{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #}
{%- set i_input = i_input + 1 %}
// lg
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.ng }});
{%- endif -%}
{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #}
{%- set i_input = i_input + 1 %}
// ug
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.ng }});
{%- endif -%}
{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #}
{%- set i_input = i_input + 1 %}
// lh
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nh }});
{%- endif -%}
{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #}
{%- set i_input = i_input + 1 %}
// uh
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nh }});
{%- endif -%}
{%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #}
{%- set i_input = i_input + 1 %}
// lh_e
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh_e }});
{%- endif -%}
{%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #}
{%- set i_input = i_input + 1 %}
// uh_e
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh_e }});
{%- endif -%}
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
{%- set i_input = i_input + 1 %}
// cost_W_0
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_0 * dims.ny_0 }});
{%- endif %}
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
{%- set i_input = i_input + 1 %}
// cost_W
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny * dims.ny }});
{%- endif %}
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
{%- set i_input = i_input + 1 %}
// cost_W_e
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_e * dims.ny_e }});
{%- endif %}
{%- if simulink_opts.inputs.reset_solver -%} {#- reset_solver #}
{%- set i_input = i_input + 1 %}
// reset_solver
ssSetInputPortVectorDimension(S, {{ i_input }}, 1);
{%- endif -%}
{%- if simulink_opts.inputs.x_init -%} {#- x_init #}
{%- set i_input = i_input + 1 %}
// x_init
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nx * (dims.N+1) }});
{%- endif -%}
{%- if simulink_opts.inputs.u_init -%} {#- u_init #}
{%- set i_input = i_input + 1 %}
// u_init
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nu * (dims.N) }});
{%- endif -%}
/* specify dimension information for the OUTPUT ports */
{%- set i_output = -1 %}{# note here i_output is 0-based #}
{%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %}
{%- set i_output = i_output + 1 %}
ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nu }} );
{%- endif %}
{%- if simulink_opts.outputs.utraj == 1 %}
{%- set i_output = i_output + 1 %}
ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nu * dims.N }} );
{%- endif %}
{%- if simulink_opts.outputs.xtraj == 1 %}
{%- set i_output = i_output + 1 %}
ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx * (dims.N+1) }} );
{%- endif %}
{%- if simulink_opts.outputs.solver_status == 1 %}
{%- set i_output = i_output + 1 %}
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
{%- endif %}
{%- if simulink_opts.outputs.cost_value == 1 %}
{%- set i_output = i_output + 1 %}
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
{%- endif %}
{%- if simulink_opts.outputs.KKT_residual == 1 %}
{%- set i_output = i_output + 1 %}
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
{%- endif %}
{%- if simulink_opts.outputs.KKT_residuals == 1 %}
{%- set i_output = i_output + 1 %}
ssSetOutputPortVectorDimension(S, {{ i_output }}, 4 );
{%- endif %}
{%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
{%- set i_output = i_output + 1 %}
ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx }} ); // state at shooting node 1
{%- endif %}
{%- if simulink_opts.outputs.CPU_time == 1 %}
{%- set i_output = i_output + 1 %}
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
{%- endif %}
{%- if simulink_opts.outputs.CPU_time_sim == 1 %}
{%- set i_output = i_output + 1 %}
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
{%- endif %}
{%- if simulink_opts.outputs.CPU_time_qp == 1 %}
{%- set i_output = i_output + 1 %}
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
{%- endif %}
{%- if simulink_opts.outputs.CPU_time_lin == 1 %}
{%- set i_output = i_output + 1 %}
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
{%- endif %}
{%- if simulink_opts.outputs.sqp_iter == 1 %}
{%- set i_output = i_output + 1 %}
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
{%- endif %}
// specify the direct feedthrough status
// should be set to 1 for all inputs used in mdlOutputs
{%- for i in range(end=n_inputs) %}
ssSetInputPortDirectFeedThrough(S, {{ i }}, 1);
{%- endfor %}
// one sample time
ssSetNumSampleTimes(S, 1);
}
#if defined(MATLAB_MEX_FILE)
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
return;
}
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
{
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
return;
}
#endif /* MATLAB_MEX_FILE */
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, SAMPLINGTIME);
ssSetOffsetTime(S, 0, 0.0);
}
static void mdlStart(SimStruct *S)
{
{{ model.name }}_solver_capsule *capsule = {{ model.name }}_acados_create_capsule();
{{ model.name }}_acados_create(capsule);
ssSetUserData(S, (void*)capsule);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
{{ model.name }}_solver_capsule *capsule = ssGetUserData(S);
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule);
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(capsule);
InputRealPtrsType in_sign;
int N = {{ model.name | upper }}_N;
{%- set buffer_sizes = [dims.nbx_0, dims.np, dims.nbx, dims.nbx_e, dims.nbu, dims.ng, dims.nh, dims.ng_e, dims.nh_e] -%}
{%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %} {# y_ref_0 #}
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0)) %}
{%- endif %}
{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %} {# y_ref #}
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny)) %}
{%- endif %}
{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %} {# y_ref_e #}
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_e)) %}
{%- endif %}
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0 * dims.ny_0)) %}
{%- endif %}
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny * dims.ny)) %}
{%- endif %}
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_e * dims.ny_e)) %}
{%- endif %}
// local buffer
{%- set buffer_size = buffer_sizes | sort | last %}
real_t buffer[{{ buffer_size }}];
/* go through inputs */
{%- set i_input = -1 %}
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #}
// lbx_0
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.nbx_0 }}; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
{%- endif %}
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #}
// ubx_0
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.nbx_0 }}; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
{%- endif %}
{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #}
// parameters - stage-variant !!!
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
// update value of parameters
for (int ii = 0; ii <= N; ii++)
{
for (int jj = 0; jj < {{ dims.np }}; jj++)
buffer[jj] = (double)(*in_sign[ii*{{dims.np}}+jj]);
{{ model.name }}_acados_update_params(capsule, ii, buffer, {{ dims.np }});
}
{%- endif %}
{% if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %}
// y_ref_0
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.ny_0 }}; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
{%- endif %}
{% if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %}
// y_ref - for stages 1 to N-1
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int ii = 1; ii < N; ii++)
{
for (int jj = 0; jj < {{ dims.ny }}; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.ny }}+jj]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
}
{%- endif %}
{% if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %}
// y_ref_e
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.ny_e }}; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", (void *) buffer);
{%- endif %}
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #}
// lbx
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int ii = 1; ii < N; ii++)
{
for (int jj = 0; jj < {{ dims.nbx }}; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
}
{%- endif %}
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #}
// ubx
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int ii = 1; ii < N; ii++)
{
for (int jj = 0; jj < {{ dims.nbx }}; jj++)
buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
}
{%- endif %}
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #}
// lbx_e
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.nbx_e }}; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", buffer);
{%- endif %}
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #}
// ubx_e
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.nbx_e }}; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", buffer);
{%- endif %}
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #}
// lbu
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int ii = 0; ii < N; ii++)
{
for (int jj = 0; jj < {{ dims.nbu }}; jj++)
buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbu", (void *) buffer);
}
{%- endif -%}
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #}
// ubu
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int ii = 0; ii < N; ii++)
{
for (int jj = 0; jj < {{ dims.nbu }}; jj++)
buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubu", (void *) buffer);
}
{%- endif -%}
{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #}
// lg
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int ii = 0; ii < N; ii++)
{
for (int jj = 0; jj < {{ dims.ng }}; jj++)
buffer[jj] = (double)(*in_sign[ii*{{ dims.ng }}+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lg", (void *) buffer);
}
{%- endif -%}
{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #}
// ug
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int ii = 0; ii < N; ii++)
{
for (int jj = 0; jj < {{ dims.ng }}; jj++)
buffer[jj] = (double)(*in_sign[ii*{{ dims.ng }}+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ug", (void *) buffer);
}
{%- endif -%}
{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #}
// lh
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int ii = 0; ii < N; ii++)
{
for (int jj = 0; jj < {{ dims.nh }}; jj++)
buffer[jj] = (double)(*in_sign[ii*{{ dims.nh }}+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", (void *) buffer);
}
{%- endif -%}
{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #}
// uh
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int ii = 0; ii < N; ii++)
{
for (int jj = 0; jj < {{ dims.nh }}; jj++)
buffer[jj] = (double)(*in_sign[ii*{{ dims.nh }}+jj]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", (void *) buffer);
}
{%- endif -%}
{%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #}
// lh_e
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.nh_e }}; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", buffer);
{%- endif -%}
{%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #}
// uh_e
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.nh_e }}; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", buffer);
{%- endif -%}
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
// cost_W_0
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.ny_0 * dims.ny_0 }}; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", buffer);
{%- endif %}
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
// cost_W
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.ny * dims.ny }}; i++)
buffer[i] = (double)(*in_sign[i]);
for (int ii = 1; ii < N; ii++)
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "W", buffer);
{%- endif %}
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
// cost_W_e
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int i = 0; i < {{ dims.ny_e * dims.ny_e }}; i++)
buffer[i] = (double)(*in_sign[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", buffer);
{%- endif %}
{%- if simulink_opts.inputs.reset_solver %} {#- reset_solver #}
// reset_solver
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
double reset = (double)(*in_sign[0]);
if (reset)
{
{{ model.name }}_acados_reset(capsule, 1);
}
{%- endif %}
{%- if simulink_opts.inputs.x_init %} {#- x_init #}
// x_init
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int ii = 0; ii < {{ dims.N + 1 }}; ii++)
{
for (int jj = 0; jj < {{ dims.nx }}; jj++)
buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nx }}+jj]);
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "x", (void *) buffer);
}
{%- endif %}
{%- if simulink_opts.inputs.u_init %} {#- u_init #}
// u_init
{%- set i_input = i_input + 1 %}
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
for (int ii = 0; ii < N; ii++)
{
for (int jj = 0; jj < {{ dims.nu }}; jj++)
buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nu }}+jj]);
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "u", (void *) buffer);
}
{%- endif %}
/* call solver */
int rti_phase = 0;
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
int acados_status = {{ model.name }}_acados_solve(capsule);
/* set outputs */
// assign pointers to output signals
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_KKT_residuals, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin, *out_cost_value;
int tmp_int;
{%- set i_output = -1 -%}{# note here i_output is 0-based #}
{%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %}
{%- set i_output = i_output + 1 %}
out_u0 = ssGetOutputPortRealSignal(S, {{ i_output }});
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
{%- endif %}
{%- if simulink_opts.outputs.utraj == 1 %}
{%- set i_output = i_output + 1 %}
out_utraj = ssGetOutputPortRealSignal(S, {{ i_output }});
for (int ii = 0; ii < N; ii++)
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii,
"u", (void *) (out_utraj + ii * {{ dims.nu }}));
{%- endif %}
{% if simulink_opts.outputs.xtraj == 1 %}
{%- set i_output = i_output + 1 %}
out_xtraj = ssGetOutputPortRealSignal(S, {{ i_output }});
for (int ii = 0; ii < {{ dims.N + 1 }}; ii++)
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii,
"x", (void *) (out_xtraj + ii * {{ dims.nx }}));
{%- endif %}
{%- if simulink_opts.outputs.solver_status == 1 %}
{%- set i_output = i_output + 1 %}
out_status = ssGetOutputPortRealSignal(S, {{ i_output }});
*out_status = (real_t) acados_status;
{%- endif %}
{%- if simulink_opts.outputs.cost_value == 1 %}
{%- set i_output = i_output + 1 %}
out_cost_value = ssGetOutputPortRealSignal(S, {{ i_output }});
ocp_nlp_eval_cost(capsule->nlp_solver, nlp_in, nlp_out);
ocp_nlp_get(nlp_config, capsule->nlp_solver, "cost_value", (void *) out_cost_value);
{%- endif %}
{%- if simulink_opts.outputs.KKT_residual == 1 %}
{%- set i_output = i_output + 1 %}
out_KKT_res = ssGetOutputPortRealSignal(S, {{ i_output }});
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
{%- endif %}
{%- if simulink_opts.outputs.KKT_residuals == 1 %}
{%- set i_output = i_output + 1 %}
out_KKT_residuals = ssGetOutputPortRealSignal(S, {{ i_output }});
{%- if solver_options.nlp_solver_type == "SQP_RTI" %}
ocp_nlp_eval_residuals(capsule->nlp_solver, nlp_in, nlp_out);
{%- endif %}
ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_stat", (void *) &out_KKT_residuals[0]);
ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_eq", (void *) &out_KKT_residuals[1]);
ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_ineq", (void *) &out_KKT_residuals[2]);
ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_comp", (void *) &out_KKT_residuals[3]);
{%- endif %}
{%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
{%- set i_output = i_output + 1 %}
out_x1 = ssGetOutputPortRealSignal(S, {{ i_output }});
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
{%- endif %}
{%- if simulink_opts.outputs.CPU_time == 1 %}
{%- set i_output = i_output + 1 %}
out_cpu_time = ssGetOutputPortRealSignal(S, {{ i_output }});
// get solution time
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
{%- endif -%}
{%- if simulink_opts.outputs.CPU_time_sim == 1 %}
{%- set i_output = i_output + 1 %}
out_cpu_time_sim = ssGetOutputPortRealSignal(S, {{ i_output }});
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_sim", (void *) out_cpu_time_sim);
{%- endif -%}
{%- if simulink_opts.outputs.CPU_time_qp == 1 %}
{%- set i_output = i_output + 1 %}
out_cpu_time_qp = ssGetOutputPortRealSignal(S, {{ i_output }});
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_qp", (void *) out_cpu_time_qp);
{%- endif -%}
{%- if simulink_opts.outputs.CPU_time_lin == 1 %}
{%- set i_output = i_output + 1 %}
out_cpu_time_lin = ssGetOutputPortRealSignal(S, {{ i_output }});
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_lin", (void *) out_cpu_time_lin);
{%- endif -%}
{%- if simulink_opts.outputs.sqp_iter == 1 %}
{%- set i_output = i_output + 1 %}
out_sqp_iter = ssGetOutputPortRealSignal(S, {{ i_output }});
// get sqp iter
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
*out_sqp_iter = (real_t) tmp_int;
{%- endif %}
}
static void mdlTerminate(SimStruct *S)
{
{{ model.name }}_solver_capsule *capsule = ssGetUserData(S);
{{ model.name }}_acados_free(capsule);
{{ model.name }}_acados_free_capsule(capsule);
}
#ifdef MATLAB_MEX_FILE
#include "simulink.c"
#else
#include "cg_sfun.h"
#endif

181
third_party/acados/acados_template/c_templates_tera/matlab_templates/main_mex.in.c vendored Normal file
View File

@@ -0,0 +1,181 @@
/*
* 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.;
*/
// standard
#include <stdio.h>
#include <stdlib.h>
// acados
#include "acados/utils/print.h"
#include "acados/utils/math.h"
#include "acados_c/ocp_nlp_interface.h"
#include "acados_solver_{{ model.name }}.h"
// mex
#include "mex.h"
/* auxilary mex */
// prints a matrix in column-major format (exponential notation)
void MEX_print_exp_mat(int m, int n, double *A, int lda)
{
for (int i=0; i<m; i++)
{
for (int j=0; j<n; j++)
{
mexPrintf("%e\t", A[i+lda*j]);
}
mexPrintf("\n");
}
mexPrintf("\n");
}
// prints the transposed of a matrix in column-major format (exponential notation)
void MEX_print_exp_tran_mat(int row, int col, double *A, int lda)
{
for (int j=0; j<col; j++)
{
for (int i=0; i<row; i++)
{
mexPrintf("%e\t", A[i+lda*j]);
}
mexPrintf("\n");
}
mexPrintf("\n");
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
int status = 0;
status = {{ model.name }}_acados_create();
if (status)
{
mexPrintf("{{ model.name }}_acados_create() returned status %d. Exiting.\n", status);
exit(1);
}
// get pointers to nlp solver related objects
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config();
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims();
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in();
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out();
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver();
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts();
// initial condition
int idxbx0[{{ dims.nbx_0 }}];
{% for i in range(end=dims.nbx_0) %}
idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }};
{%- endfor %}
double lbx0[{{ dims.nbx_0 }}];
double ubx0[{{ dims.nbx_0 }}];
{% for i in range(end=dims.nbx_0) %}
lbx0[{{ i }}] = {{ constraints.lbx_0[i] }};
ubx0[{{ i }}] = {{ constraints.ubx_0[i] }};
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
// initialization for state values
double x_init[{{ dims.nx }}];
{%- for i in range(end=dims.nx) %}
x_init[{{ i }}] = 0.0;
{%- endfor %}
// initial value for control input
double u0[{{ dims.nu }}];
{%- for i in range(end=dims.nu) %}
u0[{{ i }}] = 0.0;
{%- endfor %}
// prepare evaluation
int NTIMINGS = 10;
double min_time = 1e12;
double kkt_norm_inf;
double elapsed_time;
int sqp_iter;
double xtraj[{{ dims.nx }} * ({{ dims.N }}+1)];
double utraj[{{ dims.nu }} * ({{ dims.N }})];
// solve ocp in loop
for (int ii = 0; ii < NTIMINGS; ii++)
{
// initialize primal solution
for (int i = 0; i <= nlp_dims->N; i++)
{
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
}
status = {{ model.name }}_acados_solve();
ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
min_time = MIN(elapsed_time, min_time);
}
/* print solution and statistics */
for (int ii = 0; ii <= nlp_dims->N; ii++)
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*{{ dims.nx }}]);
for (int ii = 0; ii < nlp_dims->N; ii++)
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*{{ dims.nu }}]);
mexPrintf("\n--- xtraj ---\n");
MEX_print_exp_tran_mat( {{ dims.nx }}, {{ dims.N }}+1, xtraj, {{ dims.nx }} );
mexPrintf("\n--- utraj ---\n");
MEX_print_exp_tran_mat( {{ dims.nu }}, {{ dims.N }}, utraj, {{ dims.nu }} );
mexPrintf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
if (status == ACADOS_SUCCESS)
mexPrintf("{{ model.name }}_acados_solve(): SUCCESS!\n");
else
mexPrintf("{{ model.name }}_acados_solve() failed with status %d.\n", status);
// get solution
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
mexPrintf("\nSolver info:\n");
mexPrintf(" SQP iterations %2d\n minimum time for 1 solve %f [ms]\n KKT %e\n",
sqp_iter, min_time*1000, kkt_norm_inf);
// free solver
status = {{ model.name }}_acados_free();
if (status)
{
mexPrintf("{{ model.name }}_acados_free() returned status %d.\n", status);
}
return;
}

103
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
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
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
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
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

218
third_party/acados/acados_template/c_templates_tera/model.in.h vendored Normal file
View File

@@ -0,0 +1,218 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef {{ model.name }}_MODEL
#define {{ model.name }}_MODEL
#ifdef __cplusplus
extern "C" {
#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 solver_options.integrator_type == "IRK" or solver_options.integrator_type == "LIFTED_IRK" %}
{% if model.dyn_ext_fun_type == "casadi" %}
// implicit ODE: function
int {{ model.name }}_impl_dae_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_impl_dae_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_impl_dae_fun_sparsity_in(int);
const int *{{ model.name }}_impl_dae_fun_sparsity_out(int);
int {{ model.name }}_impl_dae_fun_n_in(void);
int {{ model.name }}_impl_dae_fun_n_out(void);
// implicit ODE: function + jacobians
int {{ model.name }}_impl_dae_fun_jac_x_xdot_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_work(int *, int *, int *, int *);
const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in(int);
const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out(int);
int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in(void);
int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out(void);
// implicit ODE: jacobians only
int {{ model.name }}_impl_dae_jac_x_xdot_u_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_impl_dae_jac_x_xdot_u_z_work(int *, int *, int *, int *);
const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in(int);
const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out(int);
int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in(void);
int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out(void);
// implicit ODE - for lifted_irk
int {{ model.name }}_impl_dae_fun_jac_x_xdot_u(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_work(int *, int *, int *, int *);
const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_in(int);
const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out(int);
int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in(void);
int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out(void);
{%- if hessian_approx == "EXACT" %}
// implicit ODE - hessian
int {{ model.name }}_impl_dae_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_impl_dae_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_impl_dae_hess_sparsity_in(int);
const int *{{ model.name }}_impl_dae_hess_sparsity_out(int);
int {{ model.name }}_impl_dae_hess_n_in(void);
int {{ model.name }}_impl_dae_hess_n_out(void);
{% endif %}
{% else %}{# ext_fun_type #}
{%- if hessian_approx == "EXACT" %}
int {{ model.dyn_impl_dae_hess }}(void **, void **, void *);
{% endif %}
int {{ model.dyn_impl_dae_fun_jac }}(void **, void **, void *);
int {{ model.dyn_impl_dae_jac }}(void **, void **, void *);
int {{ model.dyn_impl_dae_fun }}(void **, void **, void *);
{% endif %}{# ext_fun_type #}
{% elif solver_options.integrator_type == "GNSF" %}
/* GNSF Functions */
{% if model.gnsf.purely_linear != 1 %}
// phi_fun
int {{ model.name }}_gnsf_phi_fun(const double** arg, double** res, int* iw, double* w, void *mem);
int {{ model.name }}_gnsf_phi_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_gnsf_phi_fun_sparsity_in(int);
const int *{{ model.name }}_gnsf_phi_fun_sparsity_out(int);
int {{ model.name }}_gnsf_phi_fun_n_in(void);
int {{ model.name }}_gnsf_phi_fun_n_out(void);
// phi_fun_jac_y
int {{ model.name }}_gnsf_phi_fun_jac_y(const double** arg, double** res, int* iw, double* w, void *mem);
int {{ model.name }}_gnsf_phi_fun_jac_y_work(int *, int *, int *, int *);
const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in(int);
const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out(int);
int {{ model.name }}_gnsf_phi_fun_jac_y_n_in(void);
int {{ model.name }}_gnsf_phi_fun_jac_y_n_out(void);
// phi_jac_y_uhat
int {{ model.name }}_gnsf_phi_jac_y_uhat(const double** arg, double** res, int* iw, double* w, void *mem);
int {{ model.name }}_gnsf_phi_jac_y_uhat_work(int *, int *, int *, int *);
const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in(int);
const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out(int);
int {{ model.name }}_gnsf_phi_jac_y_uhat_n_in(void);
int {{ model.name }}_gnsf_phi_jac_y_uhat_n_out(void);
{% if model.gnsf.nontrivial_f_LO == 1 %}
// f_lo_fun_jac_x1k1uz
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz(const double** arg, double** res, int* iw, double* w, void *mem);
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work(int *, int *, int *, int *);
const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in(int);
const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out(int);
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in(void);
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out(void);
{%- endif %}
{%- endif %}
// used to import model matrices
int {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem);
int {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int);
const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int);
int {{ model.name }}_gnsf_get_matrices_fun_n_in(void);
int {{ model.name }}_gnsf_get_matrices_fun_n_out(void);
{% elif solver_options.integrator_type == "ERK" %}
/* explicit ODE */
// explicit ODE
int {{ model.name }}_expl_ode_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_expl_ode_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_expl_ode_fun_sparsity_in(int);
const int *{{ model.name }}_expl_ode_fun_sparsity_out(int);
int {{ model.name }}_expl_ode_fun_n_in(void);
int {{ model.name }}_expl_ode_fun_n_out(void);
// explicit forward VDE
int {{ model.name }}_expl_vde_forw(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_expl_vde_forw_work(int *, int *, int *, int *);
const int *{{ model.name }}_expl_vde_forw_sparsity_in(int);
const int *{{ model.name }}_expl_vde_forw_sparsity_out(int);
int {{ model.name }}_expl_vde_forw_n_in(void);
int {{ model.name }}_expl_vde_forw_n_out(void);
// explicit adjoint VDE
int {{ model.name }}_expl_vde_adj(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_expl_vde_adj_work(int *, int *, int *, int *);
const int *{{ model.name }}_expl_vde_adj_sparsity_in(int);
const int *{{ model.name }}_expl_vde_adj_sparsity_out(int);
int {{ model.name }}_expl_vde_adj_n_in(void);
int {{ model.name }}_expl_vde_adj_n_out(void);
{%- if hessian_approx == "EXACT" %}
int {{ model.name }}_expl_ode_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_expl_ode_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_expl_ode_hess_sparsity_in(int);
const int *{{ model.name }}_expl_ode_hess_sparsity_out(int);
int {{ model.name }}_expl_ode_hess_n_in(void);
int {{ model.name }}_expl_ode_hess_n_out(void);
{%- endif %}
{% elif solver_options.integrator_type == "DISCRETE" %}
{% if model.dyn_ext_fun_type == "casadi" %}
int {{ model.name }}_dyn_disc_phi_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_dyn_disc_phi_fun_work(int *, int *, int *, int *);
const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_in(int);
const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_out(int);
int {{ model.name }}_dyn_disc_phi_fun_n_in(void);
int {{ model.name }}_dyn_disc_phi_fun_n_out(void);
int {{ model.name }}_dyn_disc_phi_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_dyn_disc_phi_fun_jac_work(int *, int *, int *, int *);
const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_in(int);
const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_out(int);
int {{ model.name }}_dyn_disc_phi_fun_jac_n_in(void);
int {{ model.name }}_dyn_disc_phi_fun_jac_n_out(void);
{%- if hessian_approx == "EXACT" %}
int {{ model.name }}_dyn_disc_phi_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
int {{ model.name }}_dyn_disc_phi_fun_jac_hess_work(int *, int *, int *, int *);
const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_in(int);
const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_out(int);
int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_in(void);
int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_out(void);
{%- endif %}
{% else %}
{%- if hessian_approx == "EXACT" %}
int {{ model.dyn_disc_fun_jac_hess }}(void **, void **, void *);
{% endif %}
int {{ model.dyn_disc_fun_jac }}(void **, void **, void *);
int {{ model.dyn_disc_fun }}(void **, void **, void *);
{% endif %}
{% endif %}
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // {{ model.name }}_MODEL

708
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

819
third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c vendored Normal file
View File

@@ -0,0 +1,819 @@
/*
* 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.;
*/
// This is a template based custom_update function
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <math.h>
#include "custom_update_function.h"
#include "acados_solver_{{ model.name }}.h"
#include "acados_c/ocp_nlp_interface.h"
#include "acados/utils/mem.h"
#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
#include "blasfeo/include/blasfeo_d_blasfeo_api.h"
typedef struct custom_memory
{
// covariance matrics
struct blasfeo_dmat *uncertainty_matrix_buffer; // shape = (N+1, nx, nx)
// covariance matrix of the additive disturbance
struct blasfeo_dmat W_mat; // shape = (nw, nw)
struct blasfeo_dmat unc_jac_G_mat; // shape = (nx, nw)
struct blasfeo_dmat temp_GW_mat; // shape = (nx, nw)
struct blasfeo_dmat GWG_mat; // shape = (nx, nx)
// sensitivity matrices
struct blasfeo_dmat A_mat; // shape = (nx, nx)
struct blasfeo_dmat B_mat; // shape = (nx, nu)
// matrix in linear constraints
struct blasfeo_dmat Cg_mat; // shape = (ng, nx)
struct blasfeo_dmat Dg_mat; // shape = (ng, nu)
struct blasfeo_dmat Cg_e_mat; // shape = (ng_e, nx)
struct blasfeo_dmat dummy_Dgh_e_mat; // shape = (ngh_e_max, nu)
// matrix in nonlinear constraints
struct blasfeo_dmat Ch_mat; // shape = (nh, nx)
struct blasfeo_dmat Dh_mat; // shape = (nh, nu)
struct blasfeo_dmat Ch_e_mat; // shape = (nh_e, nx)
// feedback gain matrix
struct blasfeo_dmat K_mat; // shape = (nu, nx)
// AK = A - B@K
struct blasfeo_dmat AK_mat; // shape = (nx, nx)
// A@P_k
struct blasfeo_dmat temp_AP_mat; // shape = (nx, nx)
// K@P_k, K@P_k@K^T
struct blasfeo_dmat temp_KP_mat; // shape = (nu, nx)
struct blasfeo_dmat temp_KPK_mat; // shape = (nu, nu)
// C + D @ K, (C + D @ K) @ P_k
struct blasfeo_dmat temp_CaDK_mat; // shape = (ngh_me_max, nx)
struct blasfeo_dmat temp_CaDKmP_mat; // shape = (ngh_me_max, nx)
struct blasfeo_dmat temp_beta_mat; // shape = (ngh_me_max, ngh_me_max)
double *d_A_mat; // shape = (nx, nx)
double *d_B_mat; // shape = (nx, nu)
double *d_Cg_mat; // shape = (ng, nx)
double *d_Dg_mat; // shape = (ng, nu)
double *d_Cg_e_mat; // shape = (ng_e, nx)
double *d_Cgh_mat; // shape = (ng+nh, nx)
double *d_Dgh_mat; // shape = (ng+nh, nu)
double *d_Cgh_e_mat; // shape = (ng_e+nh_e, nx)
double *d_state_vec;
// upper and lower bounds on state variables
double *d_lbx; // shape = (nbx,)
double *d_ubx; // shape = (nbx,)
double *d_lbx_e; // shape = (nbx_e,)
double *d_ubx_e; // shape = (nbx_e,)
// tightened upper and lower bounds on state variables
double *d_lbx_tightened; // shape = (nbx,)
double *d_ubx_tightened; // shape = (nbx,)
double *d_lbx_e_tightened; // shape = (nbx_e,)
double *d_ubx_e_tightened; // shape = (nbx_e,)
// upper and lower bounds on control inputs
double *d_lbu; // shape = (nbu,)
double *d_ubu; // shape = (nbu,)
// tightened upper and lower bounds on control inputs
double *d_lbu_tightened; // shape = (nbu,)
double *d_ubu_tightened; // shape = (nbu,)
// upper and lower bounds on polytopic constraints
double *d_lg; // shape = (ng,)
double *d_ug; // shape = (ng,)
double *d_lg_e; // shape = (ng_e,)
double *d_ug_e; // shape = (ng_e,)
// tightened lower bounds on polytopic constraints
double *d_lg_tightened; // shape = (ng,)
double *d_ug_tightened; // shape = (ng,)
double *d_lg_e_tightened; // shape = (ng_e,)
double *d_ug_e_tightened; // shape = (ng_e,)
// upper and lower bounds on nonlinear constraints
double *d_lh; // shape = (nh,)
double *d_uh; // shape = (nh,)
double *d_lh_e; // shape = (nh_e,)
double *d_uh_e; // shape = (nh_e,)
// tightened upper and lower bounds on nonlinear constraints
double *d_lh_tightened; // shape = (nh,)
double *d_uh_tightened; // shape = (nh,)
double *d_lh_e_tightened; // shape = (nh_e,)
double *d_uh_e_tightened; // shape = (nh_e,)
int *idxbx; // shape = (nbx,)
int *idxbu; // shape = (nbu,)
int *idxbx_e; // shape = (nbx_e,)
void *raw_memory; // Pointer to allocated memory, to be used for freeing
} custom_memory;
static int int_max(int num1, int num2)
{
return (num1 > num2 ) ? num1 : num2;
}
static int custom_memory_calculate_size(ocp_nlp_config *nlp_config, ocp_nlp_dims *nlp_dims)
{
int N = nlp_dims->N;
int nx = {{ dims.nx }};
int nu = {{ dims.nu }};
int nw = {{ zoro_description.nw }};
int ng = {{ dims.ng }};
int nh = {{ dims.nh }};
int nbx = {{ dims.nbx }};
int nbu = {{ dims.nbu }};
int ng_e = {{ dims.ng_e }};
int nh_e = {{ dims.nh_e }};
int ngh_e_max = int_max(ng_e, nh_e);
int ngh_me_max = int_max(ngh_e_max, int_max(ng, nh));
int nbx_e = {{ dims.nbx_e }};
assert({{zoro_description.nlbx_t}} <= nbx);
assert({{zoro_description.nubx_t}} <= nbx);
assert({{zoro_description.nlbu_t}} <= nbu);
assert({{zoro_description.nubu_t}} <= nbu);
assert({{zoro_description.nlg_t}} <= ng);
assert({{zoro_description.nug_t}} <= ng);
assert({{zoro_description.nlh_t}} <= nh);
assert({{zoro_description.nuh_t}} <= nh);
assert({{zoro_description.nlbx_e_t}} <= nbx_e);
assert({{zoro_description.nubx_e_t}} <= nbx_e);
assert({{zoro_description.nlg_e_t}} <= ng_e);
assert({{zoro_description.nug_e_t}} <= ng_e);
assert({{zoro_description.nlh_e_t}} <= nh_e);
assert({{zoro_description.nuh_e_t}} <= nh_e);
acados_size_t size = sizeof(custom_memory);
size += nbx * sizeof(int);
/* blasfeo structs */
size += (N + 1) * sizeof(struct blasfeo_dmat);
/* blasfeo mem: mat */
size += (N + 1) * blasfeo_memsize_dmat(nx, nx); // uncertainty_matrix_buffer
size += blasfeo_memsize_dmat(nw, nw); // W_mat
size += 2 * blasfeo_memsize_dmat(nx, nw); // unc_jac_G_mat, temp_GW_mat
size += 4 * blasfeo_memsize_dmat(nx, nx); // GWG_mat, A_mat, AK_mat, temp_AP_mat
size += blasfeo_memsize_dmat(nx, nu); // B_mat
size += 2 * blasfeo_memsize_dmat(nu, nx); // K_mat, temp_KP_mat
size += blasfeo_memsize_dmat(nu, nu); // temp_KPK_mat
size += blasfeo_memsize_dmat(ng, nx); // Cg_mat
size += blasfeo_memsize_dmat(ng, nu); // Dg_mat
size += blasfeo_memsize_dmat(ng_e, nx); // Cg_e_mat
size += blasfeo_memsize_dmat(ngh_e_max, nu); // dummy_Dgh_e_mat
size += blasfeo_memsize_dmat(nh, nx); // Ch_mat
size += blasfeo_memsize_dmat(nh, nu); // Dh_mat
size += blasfeo_memsize_dmat(nh_e, nx); // Ch_e_mat
size += 2 * blasfeo_memsize_dmat(ngh_me_max, nx); // temp_CaDK_mat, temp_CaDKmP_mat
size += blasfeo_memsize_dmat(ngh_me_max, ngh_me_max); // temp_beta_mat
/* blasfeo mem: vec */
/* Arrays */
size += nx*nx *sizeof(double); // d_A_mat
size += nx*nu *sizeof(double); // d_B_mat
size += (ng + ng_e) * nx * sizeof(double); // d_Cg_mat, d_Cg_e_mat
size += (ng) * nu * sizeof(double); // d_Dg_mat
size += (nh + nh_e + ng + ng_e) * nx * sizeof(double); // d_Cgh_mat, d_Cgh_e_mat
size += (nh + ng) * nu * sizeof(double); // d_Dgh_mat
// d_state_vec
size += nx *sizeof(double);
// constraints and tightened constraints
size += 4 * (nbx + nbu + ng + nh)*sizeof(double);
size += 4 * (nbx_e + ng_e + nh_e)*sizeof(double);
size += (nbx + nbu + nbx_e)*sizeof(int); // idxbx, idxbu, idxbx_e
size += 1 * 8; // initial alignment
make_int_multiple_of(64, &size);
size += 1 * 64;
return size;
}
static custom_memory *custom_memory_assign(ocp_nlp_config *nlp_config, ocp_nlp_dims *nlp_dims, void *raw_memory)
{
int N = nlp_dims->N;
int nx = {{ dims.nx }};
int nu = {{ dims.nu }};
int nw = {{ zoro_description.nw }};
int ng = {{ dims.ng }};
int nh = {{ dims.nh }};
int nbx = {{ dims.nbx }};
int nbu = {{ dims.nbu }};
int ng_e = {{ dims.ng_e }};
int nh_e = {{ dims.nh_e }};
int ngh_e_max = int_max(ng_e, nh_e);
int ngh_me_max = int_max(ngh_e_max, int_max(ng, nh));
int nbx_e = {{ dims.nbx_e }};
char *c_ptr = (char *) raw_memory;
custom_memory *mem = (custom_memory *) c_ptr;
c_ptr += sizeof(custom_memory);
align_char_to(8, &c_ptr);
assign_and_advance_blasfeo_dmat_structs(N+1, &mem->uncertainty_matrix_buffer, &c_ptr);
align_char_to(64, &c_ptr);
for (int ii = 0; ii <= N; ii++)
{
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->uncertainty_matrix_buffer[ii], &c_ptr);
}
// Disturbance Dynamics
assign_and_advance_blasfeo_dmat_mem(nw, nw, &mem->W_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(nx, nw, &mem->unc_jac_G_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(nx, nw, &mem->temp_GW_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->GWG_mat, &c_ptr);
// System Dynamics
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->A_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(nx, nu, &mem->B_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(ng, nx, &mem->Cg_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(ng, nu, &mem->Dg_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(ng_e, nx, &mem->Cg_e_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(ngh_e_max, nu, &mem->dummy_Dgh_e_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(nh, nx, &mem->Ch_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(nh, nu, &mem->Dh_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(nh_e, nx, &mem->Ch_e_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(nu, nx, &mem->K_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->AK_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->temp_AP_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(nu, nx, &mem->temp_KP_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(nu, nu, &mem->temp_KPK_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(ngh_me_max, nx, &mem->temp_CaDK_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(ngh_me_max, nx, &mem->temp_CaDKmP_mat, &c_ptr);
assign_and_advance_blasfeo_dmat_mem(ngh_me_max, ngh_me_max, &mem->temp_beta_mat, &c_ptr);
assign_and_advance_double(nx*nx, &mem->d_A_mat, &c_ptr);
assign_and_advance_double(nx*nu, &mem->d_B_mat, &c_ptr);
assign_and_advance_double(ng*nx, &mem->d_Cg_mat, &c_ptr);
assign_and_advance_double(ng*nu, &mem->d_Dg_mat, &c_ptr);
assign_and_advance_double(ng_e*nx, &mem->d_Cg_e_mat, &c_ptr);
assign_and_advance_double((ng + nh)*nx, &mem->d_Cgh_mat, &c_ptr);
assign_and_advance_double((ng + nh)*nu, &mem->d_Dgh_mat, &c_ptr);
assign_and_advance_double((ng_e + nh_e)*nx, &mem->d_Cgh_e_mat, &c_ptr);
assign_and_advance_double(nx, &mem->d_state_vec, &c_ptr);
assign_and_advance_double(nbx, &mem->d_lbx, &c_ptr);
assign_and_advance_double(nbx, &mem->d_ubx, &c_ptr);
assign_and_advance_double(nbx_e, &mem->d_lbx_e, &c_ptr);
assign_and_advance_double(nbx_e, &mem->d_ubx_e, &c_ptr);
assign_and_advance_double(nbx, &mem->d_lbx_tightened, &c_ptr);
assign_and_advance_double(nbx, &mem->d_ubx_tightened, &c_ptr);
assign_and_advance_double(nbx_e, &mem->d_lbx_e_tightened, &c_ptr);
assign_and_advance_double(nbx_e, &mem->d_ubx_e_tightened, &c_ptr);
assign_and_advance_double(nbu, &mem->d_lbu, &c_ptr);
assign_and_advance_double(nbu, &mem->d_ubu, &c_ptr);
assign_and_advance_double(nbu, &mem->d_lbu_tightened, &c_ptr);
assign_and_advance_double(nbu, &mem->d_ubu_tightened, &c_ptr);
assign_and_advance_double(ng, &mem->d_lg, &c_ptr);
assign_and_advance_double(ng, &mem->d_ug, &c_ptr);
assign_and_advance_double(ng_e, &mem->d_lg_e, &c_ptr);
assign_and_advance_double(ng_e, &mem->d_ug_e, &c_ptr);
assign_and_advance_double(ng, &mem->d_lg_tightened, &c_ptr);
assign_and_advance_double(ng, &mem->d_ug_tightened, &c_ptr);
assign_and_advance_double(ng_e, &mem->d_lg_e_tightened, &c_ptr);
assign_and_advance_double(ng_e, &mem->d_ug_e_tightened, &c_ptr);
assign_and_advance_double(nh, &mem->d_lh, &c_ptr);
assign_and_advance_double(nh, &mem->d_uh, &c_ptr);
assign_and_advance_double(nh_e, &mem->d_lh_e, &c_ptr);
assign_and_advance_double(nh_e, &mem->d_uh_e, &c_ptr);
assign_and_advance_double(nh, &mem->d_lh_tightened, &c_ptr);
assign_and_advance_double(nh, &mem->d_uh_tightened, &c_ptr);
assign_and_advance_double(nh_e, &mem->d_lh_e_tightened, &c_ptr);
assign_and_advance_double(nh_e, &mem->d_uh_e_tightened, &c_ptr);
assign_and_advance_int(nbx, &mem->idxbx, &c_ptr);
assign_and_advance_int(nbu, &mem->idxbu, &c_ptr);
assign_and_advance_int(nbx_e, &mem->idxbx_e, &c_ptr);
assert((char *) raw_memory + custom_memory_calculate_size(nlp_config, nlp_dims) >= c_ptr);
mem->raw_memory = raw_memory;
return mem;
}
static void *custom_memory_create({{ model.name }}_solver_capsule* capsule)
{
printf("\nin custom_memory_create_function\n");
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule);
acados_size_t bytes = custom_memory_calculate_size(nlp_config, nlp_dims);
void *ptr = acados_calloc(1, bytes);
custom_memory *custom_mem = custom_memory_assign(nlp_config, nlp_dims, ptr);
custom_mem->raw_memory = ptr;
return custom_mem;
}
static void custom_val_init_function(ocp_nlp_dims *nlp_dims, ocp_nlp_in *nlp_in, ocp_nlp_solver *nlp_solver, custom_memory *custom_mem)
{
int N = nlp_dims->N;
int nx = {{ dims.nx }};
int nu = {{ dims.nu }};
int nw = {{ zoro_description.nw }};
int ng = {{ dims.ng }};
int nh = {{ dims.nh }};
int nbx = {{ dims.nbx }};
int nbu = {{ dims.nbu }};
int ng_e = {{ dims.ng_e }};
int nh_e = {{ dims.nh_e }};
int ngh_e_max = int_max(ng_e, nh_e);
int nbx_e = {{ dims.nbx_e }};
/* Get the state constraint bounds */
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "idxbx", custom_mem->idxbx);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "idxbx", custom_mem->idxbx_e);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbx", custom_mem->d_lbx);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubx", custom_mem->d_ubx);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "idxbu", custom_mem->idxbu);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbu", custom_mem->d_lbu);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubu", custom_mem->d_ubu);
// Get the Jacobians and the bounds of the linear constraints
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lg", custom_mem->d_lg);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ug", custom_mem->d_ug);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "C", custom_mem->d_Cg_mat);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "D", custom_mem->d_Dg_mat);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "C", custom_mem->d_Cg_e_mat);
blasfeo_pack_dmat(ng, nx, custom_mem->d_Cg_mat, ng, &custom_mem->Cg_mat, 0, 0);
blasfeo_pack_dmat(ng, nu, custom_mem->d_Dg_mat, ng, &custom_mem->Dg_mat, 0, 0);
blasfeo_pack_dmat(ng_e, nx, custom_mem->d_Cg_e_mat, ng_e, &custom_mem->Cg_e_mat, 0, 0);
blasfeo_dgese(ngh_e_max, nu, 0., &custom_mem->dummy_Dgh_e_mat, 0, 0); //fill with zeros
// NOTE: fixed lower and upper bounds of nonlinear constraints
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lh", custom_mem->d_lh);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "uh", custom_mem->d_uh);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e);
/* Initilize tightened constraints*/
// NOTE: tightened constraints are only initialized once
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbx", custom_mem->d_lbx_tightened);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubx", custom_mem->d_ubx_tightened);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e_tightened);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e_tightened);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbu", custom_mem->d_lbu_tightened);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubu", custom_mem->d_ubu_tightened);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lg", custom_mem->d_lg_tightened);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ug", custom_mem->d_ug_tightened);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e_tightened);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e_tightened);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lh", custom_mem->d_lh_tightened);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "uh", custom_mem->d_uh_tightened);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e_tightened);
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e_tightened);
/* Initialize the W matrix */
// blasfeo_dgese(nw, nw, 0., &custom_mem->W_mat, 0, 0);
{%- for ir in range(end=zoro_description.nw) %}
{%- for ic in range(end=zoro_description.nw) %}
blasfeo_dgein1({{zoro_description.W_mat[ir][ic]}}, &custom_mem->W_mat, {{ir}}, {{ic}});
{%- endfor %}
{%- endfor %}
{%- for ir in range(end=dims.nx) %}
{%- for ic in range(end=zoro_description.nw) %}
blasfeo_dgein1({{zoro_description.unc_jac_G_mat[ir][ic]}}, &custom_mem->unc_jac_G_mat, {{ir}}, {{ic}});
{%- endfor %}
{%- endfor %}
// NOTE: if G is changing this is not in init!
// temp_GW_mat = unc_jac_G_mat * W_mat
blasfeo_dgemm_nn(nx, nw, nw, 1.0, &custom_mem->unc_jac_G_mat, 0, 0,
&custom_mem->W_mat, 0, 0, 0.0,
&custom_mem->temp_GW_mat, 0, 0, &custom_mem->temp_GW_mat, 0, 0);
// GWG_mat = temp_GW_mat * unc_jac_G_mat^T
blasfeo_dgemm_nt(nx, nx, nw, 1.0, &custom_mem->temp_GW_mat, 0, 0,
&custom_mem->unc_jac_G_mat, 0, 0, 0.0,
&custom_mem->GWG_mat, 0, 0, &custom_mem->GWG_mat, 0, 0);
/* Initialize the uncertainty_matrix_buffer[0] */
{%- for ir in range(end=dims.nx) %}
{%- for ic in range(end=dims.nx) %}
blasfeo_dgein1({{zoro_description.P0_mat[ir][ic]}}, &custom_mem->uncertainty_matrix_buffer[0], {{ir}}, {{ic}});
{%- endfor %}
{%- endfor %}
/* Initialize the feedback gain matrix */
{%- for ir in range(end=dims.nu) %}
{%- for ic in range(end=dims.nx) %}
blasfeo_dgein1({{zoro_description.fdbk_K_mat[ir][ic]}}, &custom_mem->K_mat, {{ir}}, {{ic}});
{%- endfor %}
{%- endfor %}
}
int custom_update_init_function({{ model.name }}_solver_capsule* capsule)
{
capsule->custom_update_memory = custom_memory_create(capsule);
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule);
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(capsule);
custom_val_init_function(nlp_dims, nlp_in, nlp_solver, capsule->custom_update_memory);
return 1;
}
static void compute_gh_beta(struct blasfeo_dmat* K_mat, struct blasfeo_dmat* C_mat,
struct blasfeo_dmat* D_mat, struct blasfeo_dmat* CaDK_mat,
struct blasfeo_dmat* CaDKmP_mat, struct blasfeo_dmat* beta_mat,
struct blasfeo_dmat* P_mat,
int n_cstr, int nx, int nu)
{
// (C+DK)@P@(C^T+K^TD^T)
// CaDK_mat = C_mat + D_mat @ K_mat
blasfeo_dgemm_nn(n_cstr, nx, nu, 1.0, D_mat, 0, 0,
K_mat, 0, 0, 1.0,
C_mat, 0, 0, CaDK_mat, 0, 0);
// CaDKmP_mat = CaDK_mat @ P_mat
blasfeo_dgemm_nn(n_cstr, nx, nx, 1.0, CaDK_mat, 0, 0,
P_mat, 0, 0, 0.0,
CaDKmP_mat, 0, 0, CaDKmP_mat, 0, 0);
// beta_mat = CaDKmP_mat @ CaDK_mat^T
blasfeo_dgemm_nt(n_cstr, n_cstr, nx, 1.0, CaDKmP_mat, 0, 0,
CaDK_mat, 0, 0, 0.0,
beta_mat, 0, 0, beta_mat, 0, 0);
}
static void compute_KPK(struct blasfeo_dmat* K_mat, struct blasfeo_dmat* temp_KP_mat,
struct blasfeo_dmat* temp_KPK_mat, struct blasfeo_dmat* P_mat,
int nx, int nu)
{
// K @ P_k @ K^T
// temp_KP_mat = K_mat @ P_mat
blasfeo_dgemm_nn(nu, nx, nx, 1.0, K_mat, 0, 0,
P_mat, 0, 0, 0.0,
temp_KP_mat, 0, 0, temp_KP_mat, 0, 0);
// temp_KPK_mat = temp_KP_mat @ K_mat^T
blasfeo_dgemm_nt(nu, nu, nx, 1.0, temp_KP_mat, 0, 0,
K_mat, 0, 0, 0.0,
temp_KPK_mat, 0, 0, temp_KPK_mat, 0, 0);
}
static void compute_next_P_matrix(struct blasfeo_dmat* P_mat, struct blasfeo_dmat* P_next_mat,
struct blasfeo_dmat* A_mat, struct blasfeo_dmat* B_mat,
struct blasfeo_dmat* K_mat, struct blasfeo_dmat* W_mat,
struct blasfeo_dmat* AK_mat, struct blasfeo_dmat* temp_AP_mat, int nx, int nu)
{
// AK_mat = -B@K + A
blasfeo_dgemm_nn(nx, nx, nu, -1.0, B_mat, 0, 0, K_mat, 0, 0,
1.0, A_mat, 0, 0, AK_mat, 0, 0);
// temp_AP_mat = AK_mat @ P_k
blasfeo_dgemm_nn(nx, nx, nx, 1.0, AK_mat, 0, 0,
P_mat, 0, 0, 0.0,
temp_AP_mat, 0, 0, temp_AP_mat, 0, 0);
// P_{k+1} = temp_AP_mat @ AK_mat^T + GWG_mat
blasfeo_dgemm_nt(nx, nx, nx, 1.0, temp_AP_mat, 0, 0,
AK_mat, 0, 0, 1.0,
W_mat, 0, 0, P_next_mat, 0, 0);
}
static void reset_P0_matrix(ocp_nlp_dims *nlp_dims, struct blasfeo_dmat* P_mat, double* data)
{
int nx = nlp_dims->nx[0];
blasfeo_pack_dmat(nx, nx, data, nx, P_mat, 0, 0);
}
static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out, custom_memory *custom_mem)
{
ocp_nlp_config *nlp_config = solver->config;
ocp_nlp_dims *nlp_dims = solver->dims;
int N = nlp_dims->N;
int nx = nlp_dims->nx[0];
int nu = nlp_dims->nu[0];
int nx_sqr = nx*nx;
int nbx = {{ dims.nbx }};
int nbu = {{ dims.nbu }};
int ng = {{ dims.ng }};
int nh = {{ dims.nh }};
int ng_e = {{ dims.ng_e }};
int nh_e = {{ dims.nh_e }};
int nbx_e = {{ dims.nbx_e }};
double backoff_scaling_gamma = {{ zoro_description.backoff_scaling_gamma }};
// First Stage
// NOTE: lbx_0 and ubx_0 should not be tightened.
// NOTE: lg_0 and ug_0 are not tightened.
// NOTE: lh_0 and uh_0 are not tightened.
{%- if zoro_description.nlbu_t + zoro_description.nubu_t > 0 %}
compute_KPK(&custom_mem->K_mat, &custom_mem->temp_KP_mat,
&custom_mem->temp_KPK_mat, &(custom_mem->uncertainty_matrix_buffer[0]), nx, nu);
{%- if zoro_description.nlbu_t > 0 %}
// backoff lbu
{%- for it in zoro_description.idx_lbu_t %}
custom_mem->d_lbu_tightened[{{it}}]
= custom_mem->d_lbu[{{it}}]
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat,
custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}]));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbu", custom_mem->d_lbu_tightened);
{%- endif %}
{%- if zoro_description.nubu_t > 0 %}
// backoff ubu
{%- for it in zoro_description.idx_ubu_t %}
custom_mem->d_ubu_tightened[{{it}}]
= custom_mem->d_ubu[{{it}}]
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat,
custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}]));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubu", custom_mem->d_ubu_tightened);
{%- endif %}
{%- endif %}
// Middle Stages
// constraint tightening: for next stage based on dynamics of ii stage
// P[ii+1] = (A-B@K) @ P[ii] @ (A-B@K).T + G@W@G.T
for (int ii = 0; ii < N-1; ii++)
{
// get and pack: A, B
ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, ii, "A", custom_mem->d_A_mat);
blasfeo_pack_dmat(nx, nx, custom_mem->d_A_mat, nx, &custom_mem->A_mat, 0, 0);
ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, ii, "B", custom_mem->d_B_mat);
blasfeo_pack_dmat(nx, nu, custom_mem->d_B_mat, nx, &custom_mem->B_mat, 0, 0);
compute_next_P_matrix(&(custom_mem->uncertainty_matrix_buffer[ii]),
&(custom_mem->uncertainty_matrix_buffer[ii+1]),
&custom_mem->A_mat, &custom_mem->B_mat,
&custom_mem->K_mat, &custom_mem->GWG_mat,
&custom_mem->AK_mat, &custom_mem->temp_AP_mat, nx, nu);
// state constraints
{%- if zoro_description.nlbx_t + zoro_description.nubx_t> 0 %}
{%- if zoro_description.nlbx_t > 0 %}
// lbx
{%- for it in zoro_description.idx_lbx_t %}
custom_mem->d_lbx_tightened[{{it}}]
= custom_mem->d_lbx[{{it}}]
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1],
custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}]));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbx", custom_mem->d_lbx_tightened);
{%- endif %}
{% if zoro_description.nubx_t > 0 %}
// ubx
{%- for it in zoro_description.idx_ubx_t %}
custom_mem->d_ubx_tightened[{{it}}] = custom_mem->d_ubx[{{it}}]
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1],
custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}]));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubx", custom_mem->d_ubx_tightened);
{%- endif %}
{%- endif %}
{%- if zoro_description.nlbu_t + zoro_description.nubu_t > 0 %}
// input constraints
compute_KPK(&custom_mem->K_mat, &custom_mem->temp_KP_mat,
&custom_mem->temp_KPK_mat, &(custom_mem->uncertainty_matrix_buffer[ii+1]), nx, nu);
{%- if zoro_description.nlbu_t > 0 %}
{%- for it in zoro_description.idx_lbu_t %}
custom_mem->d_lbu_tightened[{{it}}] = custom_mem->d_lbu[{{it}}]
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat,
custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}]));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbu", custom_mem->d_lbu_tightened);
{%- endif %}
{%- if zoro_description.nubu_t > 0 %}
{%- for it in zoro_description.idx_ubu_t %}
custom_mem->d_ubu_tightened[{{it}}] = custom_mem->d_ubu[{{it}}]
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat,
custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}]));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubu", custom_mem->d_ubu_tightened);
{%- endif %}
{%- endif %}
{%- if zoro_description.nlg_t + zoro_description.nug_t > 0 %}
// Linear constraints: g
compute_gh_beta(&custom_mem->K_mat, &custom_mem->Cg_mat,
&custom_mem->Dg_mat, &custom_mem->temp_CaDK_mat,
&custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat,
&custom_mem->uncertainty_matrix_buffer[ii+1], ng, nx, nu);
{%- if zoro_description.nlg_t > 0 %}
{%- for it in zoro_description.idx_lg_t %}
custom_mem->d_lg_tightened[{{it}}]
= custom_mem->d_lg[{{it}}]
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lg", custom_mem->d_lg_tightened);
{%- endif %}
{%- if zoro_description.nug_t > 0 %}
{%- for it in zoro_description.idx_ug_t %}
custom_mem->d_ug_tightened[{{it}}]
= custom_mem->d_ug[{{it}}]
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ug", custom_mem->d_ug_tightened);
{%- endif %}
{%- endif %}
{%- if zoro_description.nlh_t + zoro_description.nuh_t > 0 %}
// nonlinear constraints: h
// Get C_{k+1} and D_{k+1}
ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, ii+1, "C", custom_mem->d_Cgh_mat);
ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, ii+1, "D", custom_mem->d_Dgh_mat);
// NOTE: the d_Cgh_mat is column-major, the first ng rows are the Jacobians of the linear constraints
blasfeo_pack_dmat(nh, nx, custom_mem->d_Cgh_mat+ng, ng+nh, &custom_mem->Ch_mat, 0, 0);
blasfeo_pack_dmat(nh, nu, custom_mem->d_Dgh_mat+ng, ng+nh, &custom_mem->Dh_mat, 0, 0);
compute_gh_beta(&custom_mem->K_mat, &custom_mem->Ch_mat,
&custom_mem->Dh_mat, &custom_mem->temp_CaDK_mat,
&custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat,
&custom_mem->uncertainty_matrix_buffer[ii+1], nh, nx, nu);
{%- if zoro_description.nlh_t > 0 %}
{%- for it in zoro_description.idx_lh_t %}
custom_mem->d_lh_tightened[{{it}}]
= custom_mem->d_lh[{{it}}]
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lh", custom_mem->d_lh_tightened);
{%- endif %}
{%- if zoro_description.nuh_t > 0 %}
{%- for it in zoro_description.idx_uh_t %}
custom_mem->d_uh_tightened[{{it}}] = custom_mem->d_uh[{{it}}]
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "uh", custom_mem->d_uh_tightened);
{%- endif %}
{%- endif %}
}
// Last stage
// get and pack: A, B
ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, N-1, "A", custom_mem->d_A_mat);
blasfeo_pack_dmat(nx, nx, custom_mem->d_A_mat, nx, &custom_mem->A_mat, 0, 0);
ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, N-1, "B", custom_mem->d_B_mat);
blasfeo_pack_dmat(nx, nu, custom_mem->d_B_mat, nx, &custom_mem->B_mat, 0, 0);
// AK_mat = -B*K + A
compute_next_P_matrix(&(custom_mem->uncertainty_matrix_buffer[N-1]),
&(custom_mem->uncertainty_matrix_buffer[N]),
&custom_mem->A_mat, &custom_mem->B_mat,
&custom_mem->K_mat, &custom_mem->GWG_mat,
&custom_mem->AK_mat, &custom_mem->temp_AP_mat, nx, nu);
// state constraints nlbx_e_t
{%- if zoro_description.nlbx_e_t + zoro_description.nubx_e_t> 0 %}
{%- if zoro_description.nlbx_e_t > 0 %}
// lbx_e
{%- for it in zoro_description.idx_lbx_e_t %}
custom_mem->d_lbx_e_tightened[{{it}}]
= custom_mem->d_lbx_e[{{it}}]
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N],
custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}]));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e_tightened);
{%- endif %}
{% if zoro_description.nubx_e_t > 0 %}
// ubx_e
{%- for it in zoro_description.idx_ubx_e_t %}
custom_mem->d_ubx_e_tightened[{{it}}] = custom_mem->d_ubx_e[{{it}}]
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N],
custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}]));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e_tightened);
{%- endif %}
{%- endif %}
{%- if zoro_description.nlg_e_t + zoro_description.nug_e_t > 0 %}
// Linear constraints: g
compute_gh_beta(&custom_mem->K_mat, &custom_mem->Cg_mat,
&custom_mem->dummy_Dgh_e_mat, &custom_mem->temp_CaDK_mat,
&custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat,
&custom_mem->uncertainty_matrix_buffer[N], ng, nx, nu);
{%- if zoro_description.nlg_e_t > 0 %}
{%- for it in zoro_description.idx_lg_e_t %}
custom_mem->d_lg_e_tightened[{{it}}]
= custom_mem->d_lg_e[{{it}}]
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e_tightened);
{%- endif %}
{%- if zoro_description.nug_e_t > 0 %}
{%- for it in zoro_description.idx_ug_e_t %}
custom_mem->d_ug_e_tightened[{{it}}]
= custom_mem->d_ug_e[{{it}}]
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e_tightened);
{%- endif %}
{%- endif %}
{%- if zoro_description.nlh_e_t + zoro_description.nuh_e_t > 0 %}
// nonlinear constraints: h
// Get C_{k+1} and D_{k+1}
ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, N, "C", custom_mem->d_Cgh_mat);
// NOTE: the d_Cgh_mat is column-major, the first ng rows are the Jacobians of the linear constraints
blasfeo_pack_dmat(nh, nx, custom_mem->d_Cgh_mat+ng, ng+nh, &custom_mem->Ch_mat, 0, 0);
compute_gh_beta(&custom_mem->K_mat, &custom_mem->Ch_mat,
&custom_mem->dummy_Dgh_e_mat, &custom_mem->temp_CaDK_mat,
&custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat,
&custom_mem->uncertainty_matrix_buffer[N], nh, nx, nu);
{%- if zoro_description.nlh_e_t > 0 %}
{%- for it in zoro_description.idx_lh_e_t %}
custom_mem->d_lh_e_tightened[{{it}}]
= custom_mem->d_lh_e[{{it}}]
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e_tightened);
{%- endif %}
{%- if zoro_description.nuh_e_t > 0 %}
{%- for it in zoro_description.idx_uh_e_t %}
custom_mem->d_uh_e_tightened[{{it}}] = custom_mem->d_uh_e[{{it}}]
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
{%- endfor %}
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e_tightened);
{%- endif %}
{%- endif %}
}
int custom_update_function({{ model.name }}_solver_capsule* capsule, double* data, int data_len)
{
custom_memory *custom_mem = (custom_memory *) capsule->custom_update_memory;
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule);
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(capsule);
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(capsule);
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(capsule);
if (data_len > 0)
{
reset_P0_matrix(nlp_dims, &custom_mem->uncertainty_matrix_buffer[0], data);
}
uncertainty_propagate_and_update(nlp_solver, nlp_in, nlp_out, custom_mem);
return 1;
}
int custom_update_terminate_function({{ model.name }}_solver_capsule* capsule)
{
custom_memory *mem = capsule->custom_update_memory;
free(mem->raw_memory);
return 1;
}
// useful prints for debugging
/*
printf("A_mat:\n");
blasfeo_print_exp_dmat(nx, nx, &custom_mem->A_mat, 0, 0);
printf("B_mat:\n");
blasfeo_print_exp_dmat(nx, nu, &custom_mem->B_mat, 0, 0);
printf("K_mat:\n");
blasfeo_print_exp_dmat(nu, nx, &custom_mem->K_mat, 0, 0);
printf("AK_mat:\n");
blasfeo_print_exp_dmat(nx, nx, &custom_mem->AK_mat, 0, 0);
printf("temp_AP_mat:\n");
blasfeo_print_exp_dmat(nx, nx, &custom_mem->temp_AP_mat, 0, 0);
printf("W_mat:\n");
blasfeo_print_exp_dmat(nx, nx, &custom_mem->W_mat, 0, 0);
printf("P_k+1:\n");
blasfeo_print_exp_dmat(nx, nx, &(custom_mem->uncertainty_matrix_buffer[ii+1]), 0, 0);*/

44
third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.h vendored Normal file
View File

@@ -0,0 +1,44 @@
/*
* 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.;
*/
#include "acados_solver_{{ model.name }}.h"
// Called at the end of solver creation.
// This is allowed to allocate memory and store the pointer to it into capsule->custom_update_memory.
int custom_update_init_function({{ model.name }}_solver_capsule* capsule);
// Custom update function that can be called between solver calls
int custom_update_function({{ model.name }}_solver_capsule* capsule, double* data, int data_len);
// Called just before destroying the solver.
// Responsible to free allocated memory, stored at capsule->custom_update_memory.
int custom_update_terminate_function({{ model.name }}_solver_capsule* capsule);

1
third_party/acados/acados_template/gnsf/__init__.py vendored Normal file
View File

@@ -0,0 +1 @@

216
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 acados_template.utils import casadi_length
from casadi import *
import numpy as np
def check_reformulation(model, gnsf, print_info):
## Description:
# this function takes the implicit ODE/ index-1 DAE and a gnsf structure
# to evaluate both models at num_eval random points x0, x0dot, z0, u0
# if for all points the relative error is <= TOL, the function will return::
# 1, otherwise it will give an error.
TOL = 1e-14
num_eval = 10
# get dimensions
nx = gnsf["nx"]
nu = gnsf["nu"]
nz = gnsf["nz"]
nx1 = gnsf["nx1"]
nx2 = gnsf["nx2"]
nz1 = gnsf["nz1"]
nz2 = gnsf["nz2"]
n_out = gnsf["n_out"]
# get model matrices
A = gnsf["A"]
B = gnsf["B"]
C = gnsf["C"]
E = gnsf["E"]
c = gnsf["c"]
L_x = gnsf["L_x"]
L_xdot = gnsf["L_xdot"]
L_z = gnsf["L_z"]
L_u = gnsf["L_u"]
A_LO = gnsf["A_LO"]
E_LO = gnsf["E_LO"]
B_LO = gnsf["B_LO"]
c_LO = gnsf["c_LO"]
I_x1 = range(nx1)
I_x2 = range(nx1, nx)
I_z1 = range(nz1)
I_z2 = range(nz1, nz)
idx_perm_f = gnsf["idx_perm_f"]
# get casadi variables
x = gnsf["x"]
xdot = gnsf["xdot"]
z = gnsf["z"]
u = gnsf["u"]
y = gnsf["y"]
uhat = gnsf["uhat"]
p = gnsf["p"]
# create functions
impl_dae_fun = Function("impl_dae_fun", [x, xdot, u, z, p], [model.f_impl_expr])
phi_fun = Function("phi_fun", [y, uhat, p], [gnsf["phi_expr"]])
f_lo_fun = Function(
"f_lo_fun", [x[range(nx1)], xdot[range(nx1)], z, u, p], [gnsf["f_lo_expr"]]
)
# print(gnsf)
# print(gnsf["n_out"])
for i_check in range(num_eval):
# generate random values
x0 = np.random.rand(nx, 1)
x0dot = np.random.rand(nx, 1)
z0 = np.random.rand(nz, 1)
u0 = np.random.rand(nu, 1)
if gnsf["ny"] > 0:
y0 = L_x @ x0[I_x1] + L_xdot @ x0dot[I_x1] + L_z @ z0[I_z1]
else:
y0 = []
if gnsf["nuhat"] > 0:
uhat0 = L_u @ u0
else:
uhat0 = []
# eval functions
p0 = np.random.rand(gnsf["np"], 1)
f_impl_val = impl_dae_fun(x0, x0dot, u0, z0, p0).full()
phi_val = phi_fun(y0, uhat0, p0)
f_lo_val = f_lo_fun(x0[I_x1], x0dot[I_x1], z0[I_z1], u0, p0)
f_impl_val = f_impl_val[idx_perm_f]
# eval gnsf
if n_out > 0:
C_phi = C @ phi_val
else:
C_phi = np.zeros((nx1 + nz1, 1))
try:
gnsf_val1 = (
A @ x0[I_x1] + B @ u0 + C_phi + c - E @ vertcat(x0dot[I_x1], z0[I_z1])
)
# gnsf_1 = (A @ x[I_x1] + B @ u + C_phi + c - E @ vertcat(xdot[I_x1], z[I_z1]))
except:
import pdb
pdb.set_trace()
if nx2 > 0: # eval LOS:
gnsf_val2 = (
A_LO @ x0[I_x2]
+ B_LO @ u0
+ c_LO
+ f_lo_val
- E_LO @ vertcat(x0dot[I_x2], z0[I_z2])
)
gnsf_val = vertcat(gnsf_val1, gnsf_val2).full()
else:
gnsf_val = gnsf_val1.full()
# compute error and check
rel_error = np.linalg.norm(f_impl_val - gnsf_val) / np.linalg.norm(f_impl_val)
if rel_error > TOL:
print("transcription failed rel_error > TOL")
print("you are in debug mode now: import pdb; pdb.set_trace()")
abs_error = gnsf_val - f_impl_val
# T = table(f_impl_val, gnsf_val, abs_error)
# print(T)
print("abs_error:", abs_error)
# error('transcription failed rel_error > TOL')
# check = 0
import pdb
pdb.set_trace()
if print_info:
print(" ")
print("model reformulation checked: relative error <= TOL = ", str(TOL))
print(" ")
check = 1
## helpful for debugging:
# # use in calling function and compare
# # compare f_impl(i) with gnsf_val1(i)
#
# nx = gnsf['nx']
# nu = gnsf['nu']
# nz = gnsf['nz']
# nx1 = gnsf['nx1']
# nx2 = gnsf['nx2']
#
# A = gnsf['A']
# B = gnsf['B']
# C = gnsf['C']
# E = gnsf['E']
# c = gnsf['c']
#
# L_x = gnsf['L_x']
# L_z = gnsf['L_z']
# L_xdot = gnsf['L_xdot']
# L_u = gnsf['L_u']
#
# A_LO = gnsf['A_LO']
#
# x0 = rand(nx, 1)
# x0dot = rand(nx, 1)
# z0 = rand(nz, 1)
# u0 = rand(nu, 1)
# I_x1 = range(nx1)
# I_x2 = nx1+range(nx)
#
# y0 = L_x @ x0[I_x1] + L_xdot @ x0dot[I_x1] + L_z @ z0
# uhat0 = L_u @ u0
#
# gnsf_val1 = (A @ x[I_x1] + B @ u + # C @ phi_current + c) - E @ [xdot[I_x1] z]
# gnsf_val1 = gnsf_val1.simplify()
#
# # gnsf_val2 = A_LO @ x[I_x2] + gnsf['f_lo_fun'](x[I_x1], xdot[I_x1], z, u) - xdot[I_x2]
# gnsf_val2 = A_LO @ x[I_x2] + gnsf['f_lo_fun'](x[I_x1], xdot[I_x1], z, u) - xdot[I_x2]
#
#
# gnsf_val = [gnsf_val1 gnsf_val2]
# gnsf_val = gnsf_val.simplify()
# dyn_expr_f = dyn_expr_f.simplify()
# import pdb; pdb.set_trace()
return check

278
third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py vendored Normal file
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
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
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
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
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
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
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
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
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"
}

435
third_party/acados/acados_template/utils.py vendored Normal file
View File

@@ -0,0 +1,435 @@
# -*- coding: future_fstrings -*-
#
# 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__))
acados_path = os.path.join(acados_template_path, '..','..','..')
ACADOS_PATH = os.path.realpath(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:
acados_path = get_acados_path()
ACADOS_PYTHON_INTERFACE_PATH = os.path.join(acados_path, 'interfaces', 'acados_template', 'acados_template')
return ACADOS_PYTHON_INTERFACE_PATH
def get_tera_exec_path():
TERA_PATH = os.environ.get('TERA_PATH')
if not TERA_PATH:
TERA_PATH = os.path.join(get_acados_path(), 'bin', 't_renderer')
if os.name == 'nt':
TERA_PATH += '.exe'
return TERA_PATH
def check_casadi_version():
casadi_version = CasadiMeta.version()
if casadi_version in ALLOWED_CASADI_VERSIONS:
return
else:
msg = 'Warning: Please note that the following versions of CasADi are '
msg += 'officially supported: {}.\n '.format(" or ".join(ALLOWED_CASADI_VERSIONS))
msg += 'If there is an incompatibility with the CasADi generated code, '
msg += 'please consider changing your CasADi version.\n'
msg += 'Version {} currently in use.'.format(casadi_version)
print(msg)
def is_column(x):
if isinstance(x, np.ndarray):
if x.ndim == 1:
return True
elif x.ndim == 2 and x.shape[1] == 1:
return True
else:
return False
elif isinstance(x, (MX, SX, DM)):
if x.shape[1] == 1:
return True
elif x.shape[0] == 0 and x.shape[1] == 0:
return True
else:
return False
elif x == None or x == []:
return False
else:
raise Exception("is_column expects one of the following types: np.ndarray, casadi.MX, casadi.SX."
+ " Got: " + str(type(x)))
def is_empty(x):
if isinstance(x, (MX, SX, DM)):
return x.is_empty()
elif isinstance(x, np.ndarray):
if np.prod(x.shape) == 0:
return True
else:
return False
elif x == None:
return True
elif isinstance(x, (set, list)):
if len(x)==0:
return True
else:
return False
else:
raise Exception("is_empty expects one of the following types: casadi.MX, casadi.SX, "
+ "None, numpy array empty list, set. Got: " + str(type(x)))
def casadi_length(x):
if isinstance(x, (MX, SX, DM)):
return int(np.prod(x.shape))
else:
raise Exception("casadi_length expects one of the following types: casadi.MX, casadi.SX."
+ " Got: " + str(type(x)))
def make_model_consistent(model):
x = model.x
xdot = model.xdot
u = model.u
z = model.z
p = model.p
if isinstance(x, MX):
symbol = MX.sym
elif isinstance(x, SX):
symbol = SX.sym
else:
raise Exception("model.x must be casadi.SX or casadi.MX, got {}".format(type(x)))
if is_empty(p):
model.p = symbol('p', 0, 0)
if is_empty(z):
model.z = symbol('z', 0, 0)
return model
def get_lib_ext():
lib_ext = '.so'
if sys.platform == 'darwin':
lib_ext = '.dylib'
elif os.name == 'nt':
lib_ext = ''
return lib_ext
def get_tera():
tera_path = get_tera_exec_path()
acados_path = get_acados_path()
if os.path.exists(tera_path) and os.access(tera_path, os.X_OK):
return tera_path
repo_url = "https://github.com/acados/tera_renderer/releases"
url = "{}/download/v{}/t_renderer-v{}-{}".format(
repo_url, TERA_VERSION, TERA_VERSION, PLATFORM2TERA[sys.platform])
manual_install = 'For manual installation follow these instructions:\n'
manual_install += '1 Download binaries from {}\n'.format(url)
manual_install += '2 Copy them in {}/bin\n'.format(acados_path)
manual_install += '3 Strip the version and platform from the binaries: '
manual_install += 'as t_renderer-v0.0.34-X -> t_renderer)\n'
manual_install += '4 Enable execution privilege on the file "t_renderer" with:\n'
manual_install += '"chmod +x {}"\n\n'.format(tera_path)
msg = "\n"
msg += 'Tera template render executable not found, '
msg += 'while looking in path:\n{}\n'.format(tera_path)
msg += 'In order to be able to render the templates, '
msg += 'you need to download the tera renderer binaries from:\n'
msg += '{}\n\n'.format(repo_url)
msg += 'Do you wish to set up Tera renderer automatically?\n'
msg += 'y/N? (press y to download tera or any key for manual installation)\n'
if input(msg) == 'y':
print("Dowloading {}".format(url))
with urllib.request.urlopen(url) as response, open(tera_path, 'wb') as out_file:
shutil.copyfileobj(response, out_file)
print("Successfully downloaded t_renderer.")
os.chmod(tera_path, 0o755)
return tera_path
msg_cancel = "\nYou cancelled automatic download.\n\n"
msg_cancel += manual_install
msg_cancel += "Once installed re-run your script.\n\n"
print(msg_cancel)
sys.exit(1)
def render_template(in_file, out_file, output_dir, json_path, template_glob=None):
acados_path = os.path.dirname(os.path.abspath(__file__))
if template_glob is None:
template_glob = os.path.join(acados_path, 'c_templates_tera', '**', '*')
cwd = os.getcwd()
if not os.path.exists(output_dir):
os.makedirs(output_dir)
os.chdir(output_dir)
tera_path = get_tera()
# call tera as system cmd
os_cmd = f"{tera_path} '{template_glob}' '{in_file}' '{json_path}' '{out_file}'"
# Windows cmd.exe can not cope with '...', so use "..." instead:
if os.name == 'nt':
os_cmd = os_cmd.replace('\'', '\"')
status = os.system(os_cmd)
if (status != 0):
raise Exception(f'Rendering of {in_file} failed!\n\nAttempted to execute OS command:\n{os_cmd}\n\n')
os.chdir(cwd)
## Conversion functions
def make_object_json_dumpable(input):
if isinstance(input, (np.ndarray)):
return input.tolist()
elif isinstance(input, (SX)):
return input.serialize()
elif isinstance(input, (MX)):
# NOTE: MX expressions can not be serialized, only Functions.
return input.__str__()
elif isinstance(input, (DM)):
return input.full()
else:
raise TypeError(f"Cannot make input of type {type(input)} dumpable.")
def format_class_dict(d):
"""
removes the __ artifact from class to dict conversion
"""
out = {}
for k, v in d.items():
if isinstance(v, dict):
v = format_class_dict(v)
out_key = k.split('__', 1)[-1]
out[k.replace(k, out_key)] = v
return out
def get_ocp_nlp_layout() -> dict:
python_interface_path = get_python_interface_path()
abs_path = os.path.join(python_interface_path, 'acados_layout.json')
with open(abs_path, 'r') as f:
ocp_nlp_layout = json.load(f)
return ocp_nlp_layout
def get_default_simulink_opts() -> dict:
python_interface_path = get_python_interface_path()
abs_path = os.path.join(python_interface_path, 'simulink_default_opts.json')
with open(abs_path, 'r') as f:
simulink_opts = json.load(f)
return simulink_opts
def J_to_idx(J):
nrows = J.shape[0]
idx = np.zeros((nrows, ))
for i in range(nrows):
this_idx = np.nonzero(J[i,:])[0]
if len(this_idx) != 1:
raise Exception('Invalid J matrix structure detected, ' \
'must contain one nonzero element per row.')
if this_idx.size > 0 and J[i,this_idx[0]] != 1:
raise Exception('J matrices can only contain 1s.')
idx[i] = this_idx[0]
return idx
def J_to_idx_slack(J):
nrows = J.shape[0]
ncol = J.shape[1]
idx = np.zeros((ncol, ))
i_idx = 0
for i in range(nrows):
this_idx = np.nonzero(J[i,:])[0]
if len(this_idx) == 1:
idx[i_idx] = i
i_idx = i_idx + 1
elif len(this_idx) > 1:
raise Exception('J_to_idx_slack: Invalid J matrix. ' \
'Found more than one nonzero in row ' + str(i))
if this_idx.size > 0 and J[i,this_idx[0]] != 1:
raise Exception('J_to_idx_slack: J matrices can only contain 1s, ' \
'got J(' + str(i) + ', ' + str(this_idx[0]) + ') = ' + str(J[i,this_idx[0]]) )
if not i_idx == ncol:
raise Exception('J_to_idx_slack: J must contain a 1 in every column!')
return idx
def acados_dae_model_json_dump(model):
# load model
x = model.x
xdot = model.xdot
u = model.u
z = model.z
p = model.p
f_impl = model.f_impl_expr
model_name = model.name
# create struct with impl_dae_fun, casadi_version
fun_name = model_name + '_impl_dae_fun'
impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl])
casadi_version = CasadiMeta.version()
str_impl_dae_fun = impl_dae_fun.serialize()
dae_dict = {"str_impl_dae_fun": str_impl_dae_fun, "casadi_version": casadi_version}
# dump
json_file = model_name + '_acados_dae.json'
with open(json_file, 'w') as f:
json.dump(dae_dict, f, default=make_object_json_dumpable, indent=4, sort_keys=True)
print("dumped ", model_name, " dae to file:", json_file, "\n")
def set_up_imported_gnsf_model(acados_ocp):
gnsf = acados_ocp.gnsf_model
# check CasADi version
# dump_casadi_version = gnsf['casadi_version']
# casadi_version = CasadiMeta.version()
# if not casadi_version == dump_casadi_version:
# print("WARNING: GNSF model was dumped with another CasADi version.\n"
# + "This might yield errors. Please use the same version for compatibility, serialize version: "
# + dump_casadi_version + " current Python CasADi verison: " + casadi_version)
# input("Press any key to attempt to continue...")
# load model
phi_fun = Function.deserialize(gnsf['phi_fun'])
phi_fun_jac_y = Function.deserialize(gnsf['phi_fun_jac_y'])
phi_jac_y_uhat = Function.deserialize(gnsf['phi_jac_y_uhat'])
get_matrices_fun = Function.deserialize(gnsf['get_matrices_fun'])
# obtain gnsf dimensions
size_gnsf_A = get_matrices_fun.size_out(0)
acados_ocp.dims.gnsf_nx1 = size_gnsf_A[1]
acados_ocp.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
acados_ocp.dims.gnsf_nuhat = max(phi_fun.size_in(1))
acados_ocp.dims.gnsf_ny = max(phi_fun.size_in(0))
acados_ocp.dims.gnsf_nout = max(phi_fun.size_out(0))
# save gnsf functions in model
acados_ocp.model.phi_fun = phi_fun
acados_ocp.model.phi_fun_jac_y = phi_fun_jac_y
acados_ocp.model.phi_jac_y_uhat = phi_jac_y_uhat
acados_ocp.model.get_matrices_fun = get_matrices_fun
# get_matrices_fun = Function([model_name,'_gnsf_get_matrices_fun'], {dummy},...
# {A, B, C, E, L_x, L_xdot, L_z, L_u, A_LO, c, E_LO, B_LO,...
# nontrivial_f_LO, purely_linear, ipiv_x, ipiv_z, c_LO});
get_matrices_out = get_matrices_fun(0)
acados_ocp.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12])
acados_ocp.model.gnsf['purely_linear'] = int(get_matrices_out[13])
if "f_lo_fun_jac_x1k1uz" in gnsf:
f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz'])
acados_ocp.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz
else:
dummy_var_x1 = SX.sym('dummy_var_x1', acados_ocp.dims.gnsf_nx1)
dummy_var_x1dot = SX.sym('dummy_var_x1dot', acados_ocp.dims.gnsf_nx1)
dummy_var_z1 = SX.sym('dummy_var_z1', acados_ocp.dims.gnsf_nz1)
dummy_var_u = SX.sym('dummy_var_z1', acados_ocp.dims.nu)
dummy_var_p = SX.sym('dummy_var_z1', acados_ocp.dims.np)
empty_var = SX.sym('empty_var', 0, 0)
empty_fun = Function('empty_fun', \
[dummy_var_x1, dummy_var_x1dot, dummy_var_z1, dummy_var_u, dummy_var_p],
[empty_var])
acados_ocp.model.f_lo_fun_jac_x1k1uz = empty_fun
del acados_ocp.gnsf_model
def idx_perm_to_ipiv(idx_perm):
n = len(idx_perm)
vec = list(range(n))
ipiv = np.zeros(n)
print(n, idx_perm)
# import pdb; pdb.set_trace()
for ii in range(n):
idx0 = idx_perm[ii]
for jj in range(ii,n):
if vec[jj]==idx0:
idx1 = jj
break
tmp = vec[ii]
vec[ii] = vec[idx1]
vec[idx1] = tmp
ipiv[ii] = idx1
ipiv = ipiv-1 # C 0-based indexing
return ipiv
def print_casadi_expression(f):
for ii in range(casadi_length(f)):
print(f[ii,:])

78
third_party/acados/acados_template/zoro_description.py vendored Normal file
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__

147
third_party/acados/include/acados/dense_qp/dense_qp_common.h vendored Normal file
View File

@@ -0,0 +1,147 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_DENSE_QP_DENSE_QP_COMMON_H_
#define ACADOS_DENSE_QP_DENSE_QP_COMMON_H_
#ifdef __cplusplus
extern "C" {
#endif
// hpipm
#include "hpipm/include/hpipm_d_dense_qp.h"
#include "hpipm/include/hpipm_d_dense_qp_res.h"
#include "hpipm/include/hpipm_d_dense_qp_sol.h"
// acados
#include "acados/utils/types.h"
typedef struct d_dense_qp_dim dense_qp_dims;
typedef struct d_dense_qp dense_qp_in;
typedef struct d_dense_qp_sol dense_qp_out;
typedef struct d_dense_qp_res dense_qp_res;
typedef struct d_dense_qp_res_ws dense_qp_res_ws;
#ifndef QP_SOLVER_CONFIG_
#define QP_SOLVER_CONFIG_
typedef struct
{
void (*dims_set)(void *config_, void *dims_, const char *field, const int* value);
acados_size_t (*opts_calculate_size)(void *config, void *dims);
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
void (*opts_initialize_default)(void *config, void *dims, void *args);
void (*opts_update)(void *config, void *dims, void *args);
void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *args);
void *(*memory_assign)(void *config, void *dims, void *args, void *raw_memory);
void (*memory_get)(void *config_, void *mem_, const char *field, void* value);
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *args);
int (*evaluate)(void *config, void *qp_in, void *qp_out, void *args, void *mem, void *work);
void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
} qp_solver_config;
#endif
#ifndef QP_INFO_
#define QP_INFO_
typedef struct
{
double solve_QP_time;
double condensing_time;
double interface_time;
double total_time;
int num_iter;
int t_computed;
} qp_info;
#endif
/* config */
//
acados_size_t dense_qp_solver_config_calculate_size();
//
qp_solver_config *dense_qp_solver_config_assign(void *raw_memory);
/* dims */
//
acados_size_t dense_qp_dims_calculate_size();
//
dense_qp_dims *dense_qp_dims_assign(void *raw_memory);
//
void dense_qp_dims_set(void *config_, void *dims_, const char *field, const int* value);
//
/* in */
//
acados_size_t dense_qp_in_calculate_size(dense_qp_dims *dims);
//
dense_qp_in *dense_qp_in_assign(dense_qp_dims *dims, void *raw_memory);
/* out */
//
acados_size_t dense_qp_out_calculate_size(dense_qp_dims *dims);
//
dense_qp_out *dense_qp_out_assign(dense_qp_dims *dims, void *raw_memory);
//
void dense_qp_out_get(dense_qp_out *out, const char *field, void *value);
/* res */
//
acados_size_t dense_qp_res_calculate_size(dense_qp_dims *dims);
//
dense_qp_res *dense_qp_res_assign(dense_qp_dims *dims, void *raw_memory);
//
acados_size_t dense_qp_res_workspace_calculate_size(dense_qp_dims *dims);
//
dense_qp_res_ws *dense_qp_res_workspace_assign(dense_qp_dims *dims, void *raw_memory);
//
void dense_qp_compute_t(dense_qp_in *qp_in, dense_qp_out *qp_out);
//
void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res *qp_res, dense_qp_res_ws *res_ws);
//
void dense_qp_res_compute_nrm_inf(dense_qp_res *qp_res, double res[4]);
/* misc */
//
void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out);
//
void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out);
//
void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out *out);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_DENSE_QP_DENSE_QP_COMMON_H_

110
third_party/acados/include/acados/dense_qp/dense_qp_daqp.h 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.;
*/
#ifndef ACADOS_DENSE_QP_DENSE_QP_DAQP_H_
#define ACADOS_DENSE_QP_DENSE_QP_DAQP_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// daqp
#include "daqp/include/types.h"
// acados
#include "acados/dense_qp/dense_qp_common.h"
#include "acados/utils/types.h"
typedef struct dense_qp_daqp_opts_
{
DAQPSettings* daqp_opts;
int warm_start;
} dense_qp_daqp_opts;
typedef struct dense_qp_daqp_memory_
{
double* lb_tmp;
double* ub_tmp;
int* idxb;
int* idxv_to_idxb;
int* idxs;
int* idxdaqp_to_idxs;
double* Zl;
double* Zu;
double* zl;
double* zu;
double* d_ls;
double* d_us;
double time_qp_solver_call;
int iter;
DAQPWorkspace * daqp_work;
} dense_qp_daqp_memory;
// opts
acados_size_t dense_qp_daqp_opts_calculate_size(void *config, dense_qp_dims *dims);
//
void *dense_qp_daqp_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory);
//
void dense_qp_daqp_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_);
//
void dense_qp_daqp_opts_update(void *config, dense_qp_dims *dims, void *opts_);
//
// memory
acados_size_t dense_qp_daqp_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
//
void *dense_qp_daqp_workspace_assign(void *config, dense_qp_dims *dims, void *raw_memory);
//
acados_size_t dense_qp_daqp_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
//
void *dense_qp_daqp_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory);
//
// functions
int dense_qp_daqp(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_);
//
void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void dense_qp_daqp_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void dense_qp_daqp_config_initialize_default(void *config_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_DENSE_QP_DENSE_QP_DAQP_H_

91
third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h vendored Normal file
View File

@@ -0,0 +1,91 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_
#define ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_
#ifdef __cplusplus
extern "C" {
#endif
// hpipm
#include "hpipm/include/hpipm_d_dense_qp.h"
#include "hpipm/include/hpipm_d_dense_qp_ipm.h"
#include "hpipm/include/hpipm_d_dense_qp_sol.h"
// acados
#include "acados/dense_qp/dense_qp_common.h"
#include "acados/utils/types.h"
typedef struct dense_qp_hpipm_opts_
{
struct d_dense_qp_ipm_arg *hpipm_opts;
} dense_qp_hpipm_opts;
typedef struct dense_qp_hpipm_memory_
{
struct d_dense_qp_ipm_ws *hpipm_workspace;
double time_qp_solver_call;
int iter;
} dense_qp_hpipm_memory;
//
acados_size_t dense_qp_hpipm_opts_calculate_size(void *config, void *dims);
//
void *dense_qp_hpipm_opts_assign(void *config, void *dims, void *raw_memory);
//
void dense_qp_hpipm_opts_initialize_default(void *config, void *dims, void *opts_);
//
void dense_qp_hpipm_opts_update(void *config, void *dims, void *opts_);
//
acados_size_t dense_qp_hpipm_calculate_memory_size(void *dims, void *opts_);
//
void *dense_qp_hpipm_assign_memory(void *dims, void *opts_, void *raw_memory);
//
acados_size_t dense_qp_hpipm_calculate_workspace_size(void *dims, void *opts_);
//
int dense_qp_hpipm(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void dense_qp_hpipm_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void dense_qp_hpipm_config_initialize_default(void *config_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_

125
third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h vendored Normal file
View File

@@ -0,0 +1,125 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_DENSE_QP_DENSE_QP_OOQP_H_
#define ACADOS_DENSE_QP_DENSE_QP_OOQP_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "acados/dense_qp/dense_qp_common.h"
#include "acados/utils/types.h"
enum dense_qp_ooqp_termination_code
{
DENSE_SUCCESSFUL_TERMINATION = 0,
DENSE_NOT_FINISHED,
DENSE_MAX_ITS_EXCEEDED,
DENSE_INFEASIBLE,
DENSE_UNKNOWN
};
typedef struct dense_qp_ooqp_opts_
{
int printLevel;
int useDiagonalWeights; // TODO(dimitris): implement option
int fixHessian;
int fixDynamics;
int fixInequalities;
} dense_qp_ooqp_opts;
typedef struct dense_qp_ooqp_workspace_
{
double *x;
double *gamma;
double *phi;
double *y;
double *z;
double *lambda;
double *pi;
double objectiveValue;
} dense_qp_ooqp_workspace;
typedef struct dense_qp_ooqp_memory_
{
int firstRun;
int nx;
int my;
int mz;
double *c;
double *dQ;
double *xlow;
char *ixlow;
double *xupp;
char *ixupp;
double *dA;
double *bA;
double *dC;
double *clow;
char *iclow;
double *cupp;
char *icupp;
double time_qp_solver_call;
int iter;
} dense_qp_ooqp_memory;
//
acados_size_t dense_qp_ooqp_opts_calculate_size(void *config_, dense_qp_dims *dims);
//
void *dense_qp_ooqp_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory);
//
void dense_qp_ooqp_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_);
//
void dense_qp_ooqp_opts_update(void *config_, dense_qp_dims *dims, void *opts_);
//
acados_size_t dense_qp_ooqp_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_);
//
void *dense_qp_ooqp_memory_assign(void *config_, dense_qp_dims *dims, void *opts_,
void *raw_memory);
//
acados_size_t dense_qp_ooqp_workspace_calculate_size(void *config_, dense_qp_dims *dims, void *opts_);
//
int dense_qp_ooqp(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_,
void *memory_, void *work_);
//
void dense_qp_ooqp_destroy(void *mem_, void *work);
//
void dense_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void dense_qp_ooqp_config_initialize_default(void *config_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_DENSE_QP_DENSE_QP_OOQP_H_

124
third_party/acados/include/acados/dense_qp/dense_qp_qore.h vendored Normal file
View File

@@ -0,0 +1,124 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_DENSE_QP_DENSE_QP_QORE_H_
#define ACADOS_DENSE_QP_DENSE_QP_QORE_H_
#ifdef __cplusplus
extern "C" {
#endif
// qore
#include "qore/QPSOLVER_DENSE/include/qpsolver_dense.h"
// acados
#include "acados/dense_qp/dense_qp_common.h"
#include "acados/utils/types.h"
typedef struct dense_qp_qore_opts_
{
int nsmax; // maximum size of Schur complement
int print_freq; // print frequency,
// prtfreq < 0: disable printing;
// prtfreq == 0: print on each call and include working set changes;
// prtfreq > 0: print on every prtfreq seconds, but do not include working set
// changes;
int warm_start; // warm start with updated matrices H and C
int warm_strategy; // 0: ramp-up from zero homotopy; 1: setup homotopy from the previous
// solution
int hot_start; // hot start with unchanged matrices H and C
int max_iter; // maximum number of iterations
int compute_t; // compute t in qp_out (to have correct residuals in NLP)
} dense_qp_qore_opts;
typedef struct dense_qp_qore_memory_
{
double *H;
double *HH;
double *g;
double *gg;
double *Zl;
double *Zu;
double *zl;
double *zu;
double *A;
double *b;
double *C;
double *CC;
double *Ct;
double *CCt;
double *d_lb0;
double *d_ub0;
double *d_lb;
double *d_ub;
double *d_lg;
double *d_ug;
double *d_ls;
double *d_us;
double *lb;
double *ub;
int *idxb;
int *idxb_stacked;
int *idxs;
double *prim_sol;
double *dual_sol;
QoreProblemDense *QP;
int num_iter;
dense_qp_in *qp_stacked;
double time_qp_solver_call;
int iter;
} dense_qp_qore_memory;
acados_size_t dense_qp_qore_opts_calculate_size(void *config, dense_qp_dims *dims);
//
void *dense_qp_qore_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory);
//
void dense_qp_qore_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_);
//
void dense_qp_qore_opts_update(void *config, dense_qp_dims *dims, void *opts_);
//
acados_size_t dense_qp_qore_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
//
void *dense_qp_qore_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory);
//
acados_size_t dense_qp_qore_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
//
int dense_qp_qore(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_);
//
void dense_qp_qore_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void dense_qp_qore_config_initialize_default(void *config);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_DENSE_QP_DENSE_QP_QORE_H_

126
third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h vendored Normal file
View File

@@ -0,0 +1,126 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_
#define ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/dense_qp/dense_qp_common.h"
#include "acados/utils/types.h"
typedef struct dense_qp_qpoases_opts_
{
double max_cputime; // maximum cpu time in seconds
int max_nwsr; // maximum number of working set recalculations
int warm_start; // warm start with dual_sol in memory
int use_precomputed_cholesky;
int hotstart; // this option requires constant data matrices! (eg linear MPC, inexact schemes
// with frozen sensitivities)
int set_acado_opts; // use same options as in acado code generation
int compute_t; // compute t in qp_out (to have correct residuals in NLP)
double tolerance; // terminationTolerance
} dense_qp_qpoases_opts;
typedef struct dense_qp_qpoases_memory_
{
double *H;
double *HH;
double *R;
double *g;
double *gg;
double *Zl;
double *Zu;
double *zl;
double *zu;
double *A;
double *b;
double *d_lb0;
double *d_ub0;
double *d_lb;
double *d_ub;
double *C;
double *CC;
double *d_lg0;
double *d_ug0;
double *d_lg;
double *d_ug;
double *d_ls;
double *d_us;
int *idxb;
int *idxb_stacked;
int *idxs;
double *prim_sol;
double *dual_sol;
void *QPB; // NOTE(giaf): cast to QProblemB to use
void *QP; // NOTE(giaf): cast to QProblem to use
double cputime; // cputime of qpoases
int nwsr; // performed number of working set recalculations
int first_it; // to be used with hotstart
dense_qp_in *qp_stacked;
double time_qp_solver_call; // equal to cputime
int iter;
} dense_qp_qpoases_memory;
acados_size_t dense_qp_qpoases_opts_calculate_size(void *config, dense_qp_dims *dims);
//
void *dense_qp_qpoases_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory);
//
void dense_qp_qpoases_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_);
//
void dense_qp_qpoases_opts_update(void *config, dense_qp_dims *dims, void *opts_);
//
acados_size_t dense_qp_qpoases__memorycalculate_size(void *config, dense_qp_dims *dims, void *opts_);
//
void *dense_qp_qpoases_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory);
//
acados_size_t dense_qp_qpoases_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
//
int dense_qp_qpoases(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_);
//
void dense_qp_qpoases_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void dense_qp_qpoases_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void dense_qp_qpoases_config_initialize_default(void *config_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_

446
third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h vendored Normal file
View File

@@ -0,0 +1,446 @@
/*
* 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.;
*/
/// \defgroup ocp_nlp ocp_nlp
/// @{
/// @}
/// \defgroup ocp_nlp_solver ocp_nlp_solver
/// @{
/// @}
/// \ingroup ocp_nlp
/// @{
/// \ingroup ocp_nlp_solver
/// @{
/// \defgroup ocp_nlp_common ocp_nlp_common
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
#define ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/ocp_qp/ocp_qp_xcond_solver.h"
#include "acados/sim/sim_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* config
************************************************/
typedef struct ocp_nlp_config
{
int N; // number of stages
// solver-specific implementations of memory management functions
acados_size_t (*opts_calculate_size)(void *config, void *dims);
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
void (*opts_initialize_default)(void *config, void *dims, void *opts_);
void (*opts_update)(void *config, void *dims, void *opts_);
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts_);
void *(*memory_assign)(void *config, void *dims, void *opts_, void *raw_memory);
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts_);
void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
void (*opts_set_at_stage)(void *config_, void *opts_, size_t stage, const char *field, void* value);
// evaluate solver // TODO rename into solve
int (*evaluate)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
void (*eval_param_sens)(void *config, void *dims, void *opts_, void *mem, void *work,
char *field, int stage, int index, void *sens_nlp_out);
// prepare memory
int (*precompute)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
void (*memory_reset_qp_solver)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
// initalize this struct with default values
void (*config_initialize_default)(void *config);
// general getter
void (*get)(void *config_, void *dims, void *mem_, const char *field, void *return_value_);
void (*opts_get)(void *config_, void *dims, void *opts_, const char *field, void *return_value_);
void (*work_get)(void *config_, void *dims, void *work_, const char *field, void *return_value_);
// config structs of submodules
ocp_qp_xcond_solver_config *qp_solver; // TODO rename xcond_solver
ocp_nlp_dynamics_config **dynamics;
ocp_nlp_cost_config **cost;
ocp_nlp_constraints_config **constraints;
ocp_nlp_reg_config *regularize;
} ocp_nlp_config;
//
acados_size_t ocp_nlp_config_calculate_size(int N);
//
ocp_nlp_config *ocp_nlp_config_assign(int N, void *raw_memory);
/************************************************
* dims
************************************************/
/// Structure to store dimensions/number of variables.
typedef struct ocp_nlp_dims
{
void **cost;
void **dynamics;
void **constraints;
ocp_qp_xcond_solver_dims *qp_solver; // xcond solver instead ??
ocp_nlp_reg_dims *regularize;
int *nv; // number of primal variables (states+controls+slacks)
int *nx; // number of differential states
int *nu; // number of inputs
int *ni; // number of two-sided inequality constraints: nb+ng+nh+ns
int *nz; // number of algebraic variables
int *ns; // number of slack variables
int N; // number of shooting nodes
void *raw_memory; // Pointer to allocated memory, to be used for freeing
} ocp_nlp_dims;
//
acados_size_t ocp_nlp_dims_calculate_size(void *config);
//
ocp_nlp_dims *ocp_nlp_dims_assign(void *config, void *raw_memory);
/// Sets the dimension of optimization variables
/// (states, constrols, algebraic variables, slack variables).
///
/// \param config_ The configuration struct.
/// \param dims_ The dimension struct.
/// \param field The type of optimization variables, either nx, nu, nz, or ns.
/// \param value_array Number of variables for each stage.
void ocp_nlp_dims_set_opt_vars(void *config_, void *dims_,
const char *field, const void* value_array);
/// Sets the dimensions of constraints functions for a stage
/// (bounds on states, bounds on controls, equality constraints,
/// inequality constraints).
///
/// \param config_ The configuration struct.
/// \param dims_ The dimension struct.
/// \param stage Stage number.
/// \param field The type of constraint/bound, either nbx, nbu, ng, or nh.
/// \param value_field Number of constraints/bounds for the given stage.
void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage,
const char *field, const void* value_field);
/// Sets the dimensions of the cost terms for a stage.
///
/// \param config_ The configuration struct.
/// \param dims_ The dimension struct.
/// \param stage Stage number.
/// \param field Type of cost term, can be eiter ny.
/// \param value_field Number of cost terms/residuals for the given stage.
void ocp_nlp_dims_set_cost(void *config_, void *dims_, int stage, const char *field,
const void* value_field);
/// Sets the dimensions of the dynamics for a stage.
///
/// \param config_ The configuration struct.
/// \param dims_ The dimension struct.
/// \param stage Stage number.
/// \param field TBD
/// \param value TBD
void ocp_nlp_dims_set_dynamics(void *config_, void *dims_, int stage, const char *field,
const void* value);
/************************************************
* Inputs
************************************************/
/// Struct for storing the inputs of an OCP NLP solver
typedef struct ocp_nlp_in
{
/// Length of sampling intervals/timesteps.
double *Ts;
/// Pointers to cost functions (TBC).
void **cost;
/// Pointers to dynamics functions (TBC).
void **dynamics;
/// Pointers to constraints functions (TBC).
void **constraints;
/// Pointer to allocated memory, to be used for freeing.
void *raw_memory;
} ocp_nlp_in;
//
acados_size_t ocp_nlp_in_calculate_size_self(int N);
//
acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims);
//
ocp_nlp_in *ocp_nlp_in_assign_self(int N, void *raw_memory);
//
ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory);
/************************************************
* out
************************************************/
typedef struct ocp_nlp_out
{
struct blasfeo_dvec *ux; // NOTE: this contains [u; x; s_l; s_u]! - rename to uxs?
struct blasfeo_dvec *z; // algebraic vairables
struct blasfeo_dvec *pi; // multipliers for dynamics
struct blasfeo_dvec *lam; // inequality mulitpliers
struct blasfeo_dvec *t; // slack variables corresponding to evaluation of all inequalities (at the solution)
// NOTE: the inequalities are internally organized in the following order:
// [ lbu lbx lg lh lphi ubu ubx ug uh uphi; lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
double inf_norm_res;
void *raw_memory; // Pointer to allocated memory, to be used for freeing
} ocp_nlp_out;
//
acados_size_t ocp_nlp_out_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims);
//
ocp_nlp_out *ocp_nlp_out_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
void *raw_memory);
/************************************************
* options
************************************************/
/// Globalization types
typedef enum
{
FIXED_STEP,
MERIT_BACKTRACKING,
} ocp_nlp_globalization_t;
typedef struct ocp_nlp_opts
{
ocp_qp_xcond_solver_opts *qp_solver_opts; // xcond solver opts instead ???
void *regularize;
void **dynamics; // dynamics_opts
void **cost; // cost_opts
void **constraints; // constraints_opts
double step_length; // step length in case of FIXED_STEP
double levenberg_marquardt; // LM factor to be added to the hessian before regularization
int reuse_workspace;
int num_threads;
int print_level;
// TODO: move to separate struct?
ocp_nlp_globalization_t globalization;
int full_step_dual;
int line_search_use_sufficient_descent;
int globalization_use_SOC;
double alpha_min;
double alpha_reduction;
double eps_sufficient_descent;
} ocp_nlp_opts;
//
acados_size_t ocp_nlp_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value);
//
void ocp_nlp_opts_set_at_stage(void *config, void *opts, int stage, const char *field, void *value);
/************************************************
* residuals
************************************************/
typedef struct ocp_nlp_res
{
struct blasfeo_dvec *res_stat; // stationarity
struct blasfeo_dvec *res_eq; // dynamics
struct blasfeo_dvec *res_ineq; // inequality constraints
struct blasfeo_dvec *res_comp; // complementarity
struct blasfeo_dvec tmp; // tmp
double inf_norm_res_stat;
double inf_norm_res_eq;
double inf_norm_res_ineq;
double inf_norm_res_comp;
acados_size_t memsize;
} ocp_nlp_res;
//
acados_size_t ocp_nlp_res_calculate_size(ocp_nlp_dims *dims);
//
ocp_nlp_res *ocp_nlp_res_assign(ocp_nlp_dims *dims, void *raw_memory);
//
void ocp_nlp_res_get_inf_norm(ocp_nlp_res *res, double *out);
/************************************************
* memory
************************************************/
typedef struct ocp_nlp_memory
{
// void *qp_solver_mem; // xcond solver mem instead ???
ocp_qp_xcond_solver_memory *qp_solver_mem; // xcond solver mem instead ???
void *regularize_mem;
void **dynamics; // dynamics memory
void **cost; // cost memory
void **constraints; // constraints memory
// residuals
ocp_nlp_res *nlp_res;
// qp in & out
ocp_qp_in *qp_in;
ocp_qp_out *qp_out;
// QP stuff not entering the qp_in struct
struct blasfeo_dmat *dzduxt; // dzdux transposed
struct blasfeo_dvec *z_alg; // z_alg, output algebraic variables
struct blasfeo_dvec *cost_grad;
struct blasfeo_dvec *ineq_fun;
struct blasfeo_dvec *ineq_adj;
struct blasfeo_dvec *dyn_fun;
struct blasfeo_dvec *dyn_adj;
double cost_value;
bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables
struct blasfeo_dvec *sim_guess;
int *sqp_iter; // pointer to iteration number
} ocp_nlp_memory;
//
acados_size_t ocp_nlp_memory_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts);
//
ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
ocp_nlp_opts *opts, void *raw_memory);
/************************************************
* workspace
************************************************/
typedef struct ocp_nlp_workspace
{
void *qp_work;
void **dynamics; // dynamics_workspace
void **cost; // cost_workspace
void **constraints; // constraints_workspace
// for globalization: -> move to module?!
ocp_nlp_out *tmp_nlp_out;
ocp_nlp_out *weight_merit_fun;
struct blasfeo_dvec tmp_nxu;
struct blasfeo_dvec tmp_ni;
struct blasfeo_dvec dxnext_dy;
} ocp_nlp_workspace;
//
acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts);
//
ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
ocp_nlp_opts *opts, ocp_nlp_memory *mem, void *raw_memory);
/************************************************
* function
************************************************/
void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_initialize_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_approximate_qp_vectors_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_embed_initial_value(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_update_variables_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, double alpha);
//
int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work,
int check_early_termination);
//
double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_initialize_t_slacks(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
//
void ocp_nlp_res_compute(ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out,
ocp_nlp_res *res, ocp_nlp_memory *mem);
//
void ocp_nlp_cost_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
/// @}
/// @}
/// @}

238
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h vendored Normal file
View File

@@ -0,0 +1,238 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_constraints
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
#ifdef __cplusplus
extern "C" {
#endif
// acados
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* dims
************************************************/
typedef struct
{
int nx;
int nu;
int nz;
int nb; // nbx + nbu
int nbu; // number of input box constraints
int nbx; // number of state box constraints
int ng; // number of general linear constraints
int nh; // number of nonlinear path constraints
int ns; // nsbu + nsbx + nsg + nsh
int nsbu; // number of softened input bounds
int nsbx; // number of softened state bounds
int nsg; // number of softened general linear constraints
int nsh; // number of softened nonlinear constraints
int nbue; // number of input box constraints which are equality
int nbxe; // number of state box constraints which are equality
int nge; // number of general linear constraints which are equality
int nhe; // number of nonlinear path constraints which are equality
} ocp_nlp_constraints_bgh_dims;
//
acados_size_t ocp_nlp_constraints_bgh_dims_calculate_size(void *config);
//
void *ocp_nlp_constraints_bgh_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_constraints_bgh_dims_get(void *config_, void *dims_, const char *field, int* value);
//
void ocp_nlp_constraints_bgh_dims_set(void *config_, void *dims_,
const char *field, const int* value);
/************************************************
* model
************************************************/
typedef struct
{
int *idxb;
int *idxs;
int *idxe;
struct blasfeo_dvec d; // gathers bounds
struct blasfeo_dmat DCt; // general linear constraint matrix
// lg <= [D, C] * [u; x] <= ug
external_function_generic *nl_constr_h_fun; // nonlinear: lh <= h(x,u) <= uh
external_function_generic *nl_constr_h_fun_jac; // nonlinear: lh <= h(x,u) <= uh
external_function_generic *nl_constr_h_fun_jac_hess; // nonlinear: lh <= h(x,u) <= uh
} ocp_nlp_constraints_bgh_model;
//
acados_size_t ocp_nlp_constraints_bgh_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims, void *raw_memory);
//
int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_,
void *model_, const char *field, void *value);
//
void ocp_nlp_constraints_bgh_model_get(void *config_, void *dims_,
void *model_, const char *field, void *value);
/************************************************
* options
************************************************/
typedef struct
{
int compute_adj;
int compute_hess;
} ocp_nlp_constraints_bgh_opts;
//
acados_size_t ocp_nlp_constraints_bgh_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_constraints_bgh_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_constraints_bgh_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_constraints_bgh_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_constraints_bgh_opts_set(void *config, void *opts, char *field, void *value);
/************************************************
* memory
************************************************/
typedef struct
{
struct blasfeo_dvec fun;
struct blasfeo_dvec adj;
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
struct blasfeo_dvec *lam; // pointer to lam in nlp_out
struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out
struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory
struct blasfeo_dmat *DCt; // pointer to DCt in qp_in
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory
int *idxb; // pointer to idxb[ii] in qp_in
int *idxs_rev; // pointer to idxs_rev[ii] in qp_in
int *idxe; // pointer to idxe[ii] in qp_in
} ocp_nlp_constraints_bgh_memory;
//
acados_size_t ocp_nlp_constraints_bgh_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_constraints_bgh_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_fun_ptr(void *memory_);
//
struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_adj_ptr(void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory);
//
void ocp_nlp_constraints_bgh_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_idxb_ptr(int *idxb, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_idxe_ptr(int *idxe, void *memory_);
/************************************************
* workspace
************************************************/
typedef struct
{
struct blasfeo_dmat tmp_nv_nv;
struct blasfeo_dmat tmp_nz_nh;
struct blasfeo_dmat tmp_nv_nh;
struct blasfeo_dmat tmp_nz_nv;
struct blasfeo_dmat hess_z;
struct blasfeo_dvec tmp_ni;
struct blasfeo_dvec tmp_nh;
} ocp_nlp_constraints_bgh_workspace;
//
acados_size_t ocp_nlp_constraints_bgh_workspace_calculate_size(void *config, void *dims, void *opts);
/* functions */
//
void ocp_nlp_constraints_bgh_config_initialize_default(void *config);
//
void ocp_nlp_constraints_bgh_initialize(void *config, void *dims, void *model, void *opts,
void *mem, void *work);
//
void ocp_nlp_constraints_bgh_update_qp_matrices(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
//
void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
//
void ocp_nlp_constraints_bgh_bounds_update(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
/// @}
/// @}

218
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h vendored Normal file
View File

@@ -0,0 +1,218 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_constraints
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
#ifdef __cplusplus
extern "C" {
#endif
// acados
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/* dims */
typedef struct
{
int nx;
int nu;
int nz;
int nb; // nbx + nbu
int nbu;
int nbx;
int ng; // number of general linear constraints
int nphi; // dimension of convex outer part
int ns; // nsbu + nsbx + nsg + nsphi
int nsbu; // number of softened input bounds
int nsbx; // number of softened state bounds
int nsg; // number of softened general linear constraints
int nsphi; // number of softened nonlinear constraints
int nr; // dimension of nonlinear function in convex_over_nonlinear constraint
int nbue; // number of input box constraints which are equality
int nbxe; // number of state box constraints which are equality
int nge; // number of general linear constraints which are equality
int nphie; // number of nonlinear path constraints which are equality
} ocp_nlp_constraints_bgp_dims;
//
acados_size_t ocp_nlp_constraints_bgp_dims_calculate_size(void *config);
//
void *ocp_nlp_constraints_bgp_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_constraints_bgp_dims_get(void *config_, void *dims_, const char *field, int* value);
/* model */
typedef struct
{
// ocp_nlp_constraints_bgp_dims *dims;
int *idxb;
int *idxs;
int *idxe;
struct blasfeo_dvec d;
struct blasfeo_dmat DCt;
external_function_generic *nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux;
external_function_generic *nl_constr_phi_o_r_fun;
external_function_generic *nl_constr_r_fun_jac;
} ocp_nlp_constraints_bgp_model;
//
acados_size_t ocp_nlp_constraints_bgp_calculate_size(void *config, void *dims);
//
void *ocp_nlp_constraints_bgp_assign(void *config, void *dims, void *raw_memory);
//
int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_,
void *model_, const char *field, void *value);
//
void ocp_nlp_constraints_bgp_model_get(void *config_, void *dims_,
void *model_, const char *field, void *value);
/* options */
typedef struct
{
int compute_adj;
int compute_hess;
} ocp_nlp_constraints_bgp_opts;
//
acados_size_t ocp_nlp_constraints_bgp_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_constraints_bgp_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_constraints_bgp_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_constraints_bgp_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_constraints_bgp_opts_set(void *config, void *opts, char *field, void *value);
/* memory */
typedef struct
{
struct blasfeo_dvec fun;
struct blasfeo_dvec adj;
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
struct blasfeo_dvec *lam; // pointer to lam in nlp_out
struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out
struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory
struct blasfeo_dmat *DCt; // pointer to DCt in qp_in
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory
int *idxb; // pointer to idxb[ii] in qp_in
int *idxs_rev; // pointer to idxs_rev[ii] in qp_in
int *idxe; // pointer to idxe[ii] in qp_in
} ocp_nlp_constraints_bgp_memory;
//
acados_size_t ocp_nlp_constraints_bgp_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_constraints_bgp_memory_assign(void *config, void *dims, void *opts,
void *raw_memory);
//
struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_fun_ptr(void *memory_);
//
struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_adj_ptr(void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory);
//
void ocp_nlp_constraints_bgp_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_idxb_ptr(int *idxb, void *memory_);
//
void ocp_nlp_constraints_bgp_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_);
//
void ocp_nlp_constraints_bgh_memory_set_idxe_ptr(int *idxe, void *memory_);
/* workspace */
typedef struct
{
struct blasfeo_dvec tmp_ni;
struct blasfeo_dmat jac_r_ux_tran;
struct blasfeo_dmat tmp_nr_nphi_nr;
struct blasfeo_dmat tmp_nv_nr;
struct blasfeo_dmat tmp_nv_nphi;
struct blasfeo_dmat tmp_nz_nphi;
} ocp_nlp_constraints_bgp_workspace;
//
acados_size_t ocp_nlp_constraints_bgp_workspace_calculate_size(void *config, void *dims, void *opts);
/* functions */
//
void ocp_nlp_constraints_bgp_config_initialize_default(void *config);
//
void ocp_nlp_constraints_bgp_initialize(void *config, void *dims, void *model,
void *opts, void *mem, void *work);
//
void ocp_nlp_constraints_bgp_update_qp_matrices(void *config_, void *dims,
void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims,
void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_constraints_bgp_bounds_update(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
/// @}
/// @}

108
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h vendored Normal file
View File

@@ -0,0 +1,108 @@
/*
* 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.;
*/
/// \ingroup ocp_nlp
/// @{
/// \defgroup ocp_nlp_constraints ocp_nlp_constraints
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
#ifdef __cplusplus
extern "C" {
#endif
// acados
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* config
************************************************/
typedef struct
{
acados_size_t (*dims_calculate_size)(void *config);
void *(*dims_assign)(void *config, void *raw_memory);
acados_size_t (*model_calculate_size)(void *config, void *dims);
void *(*model_assign)(void *config, void *dims, void *raw_memory);
int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value);
void (*model_get)(void *config_, void *dims_, void *model_, const char *field, void *value);
acados_size_t (*opts_calculate_size)(void *config, void *dims);
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
void (*opts_initialize_default)(void *config, void *dims, void *opts);
void (*opts_update)(void *config, void *dims, void *opts);
void (*opts_set)(void *config, void *opts, char *field, void *value);
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory);
struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory);
void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory);
void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory);
void (*memory_set_lam_ptr)(struct blasfeo_dvec *lam, void *memory);
void (*memory_set_tmp_lam_ptr)(struct blasfeo_dvec *tmp_lam, void *memory);
void (*memory_set_DCt_ptr)(struct blasfeo_dmat *DCt, void *memory);
void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory);
void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory);
void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzduxt, void *memory);
void (*memory_set_idxb_ptr)(int *idxb, void *memory);
void (*memory_set_idxs_rev_ptr)(int *idxs_rev, void *memory);
void (*memory_set_idxe_ptr)(int *idxe, void *memory);
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
void (*initialize)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
void (*update_qp_matrices)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
void (*compute_fun)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
void (*bounds_update)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
void (*config_initialize_default)(void *config);
// dimension setters
void (*dims_set)(void *config_, void *dims_, const char *field, const int *value);
void (*dims_get)(void *config_, void *dims_, const char *field, int* value);
} ocp_nlp_constraints_config;
//
acados_size_t ocp_nlp_constraints_config_calculate_size();
//
ocp_nlp_constraints_config *ocp_nlp_constraints_config_assign(void *raw_memory);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
/// @}
/// @}

107
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h vendored Normal file
View File

@@ -0,0 +1,107 @@
/*
* 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.;
*/
///
/// \defgroup ocp_nlp_cost ocp_nlp_cost
///
/// \addtogroup ocp_nlp_cost ocp_nlp_cost
/// @{
/// \addtogroup ocp_nlp_cost_common ocp_nlp_cost_common
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
#define ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
#ifdef __cplusplus
extern "C" {
#endif
// acados
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* config
************************************************/
typedef struct
{
acados_size_t (*dims_calculate_size)(void *config);
void *(*dims_assign)(void *config, void *raw_memory);
void (*dims_set)(void *config_, void *dims_, const char *field, int *value);
void (*dims_get)(void *config_, void *dims_, const char *field, int *value);
acados_size_t (*model_calculate_size)(void *config, void *dims);
void *(*model_assign)(void *config, void *dims, void *raw_memory);
int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_);
acados_size_t (*opts_calculate_size)(void *config, void *dims);
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
void (*opts_initialize_default)(void *config, void *dims, void *opts);
void (*opts_update)(void *config, void *dims, void *opts);
void (*opts_set)(void *config, void *opts, const char *field, void *value);
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
double *(*memory_get_fun_ptr)(void *memory);
struct blasfeo_dvec *(*memory_get_grad_ptr)(void *memory);
void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory);
void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory);
void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory);
void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzdux, void *memory);
void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory);
void (*memory_set_Z_ptr)(struct blasfeo_dvec *Z, void *memory);
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
// computes the function value, gradient and hessian (approximation) of the cost function
void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
// computes the cost function value (intended for globalization)
void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
void (*config_initialize_default)(void *config);
void (*precompute)(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
} ocp_nlp_cost_config;
//
acados_size_t ocp_nlp_cost_config_calculate_size();
//
ocp_nlp_cost_config *ocp_nlp_cost_config_assign(void *raw_memory);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
/// @}
/// @}

207
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h vendored Normal file
View File

@@ -0,0 +1,207 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_cost ocp_nlp_cost
/// @{
/// \addtogroup ocp_nlp_cost_conl ocp_nlp_cost_conl
/// \brief This module implements convex-over-nonlinear costs of the form
/// \f$\min_{x,u,z} \psi(y(x,u,z,p) - y_{\text{ref}}, p)\f$,
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
#define ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* dims
************************************************/
typedef struct
{
int nx; // number of states
int nz; // number of algebraic variables
int nu; // number of inputs
int ny; // number of outputs
int ns; // number of slacks
} ocp_nlp_cost_conl_dims;
//
acados_size_t ocp_nlp_cost_conl_dims_calculate_size(void *config);
//
void *ocp_nlp_cost_conl_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_cost_conl_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz);
//
void ocp_nlp_cost_conl_dims_set(void *config_, void *dims_, const char *field, int* value);
//
void ocp_nlp_cost_conl_dims_get(void *config_, void *dims_, const char *field, int* value);
/************************************************
* model
************************************************/
typedef struct
{
// slack penalty has the form z^T * s + .5 * s^T * Z * s
external_function_generic *conl_cost_fun;
external_function_generic *conl_cost_fun_jac_hess;
struct blasfeo_dvec y_ref;
struct blasfeo_dvec Z; // diagonal Hessian of slacks as vector
struct blasfeo_dvec z; // gradient of slacks as vector
double scaling;
} ocp_nlp_cost_conl_model;
//
acados_size_t ocp_nlp_cost_conl_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_conl_model_assign(void *config, void *dims, void *raw_memory);
//
int ocp_nlp_cost_conl_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_);
/************************************************
* options
************************************************/
typedef struct
{
bool gauss_newton_hess; // dummy options, we always use a gauss-newton hessian
} ocp_nlp_cost_conl_opts;
//
acados_size_t ocp_nlp_cost_conl_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_conl_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_cost_conl_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_conl_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_conl_opts_set(void *config, void *opts, const char *field, void *value);
/************************************************
* memory
************************************************/
typedef struct
{
struct blasfeo_dvec grad; // gradient of cost function
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dvec *Z; // pointer to Z in qp_in
struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
double fun; ///< value of the cost function
} ocp_nlp_cost_conl_memory;
//
acados_size_t ocp_nlp_cost_conl_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_cost_conl_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
double *ocp_nlp_cost_conl_memory_get_fun_ptr(void *memory_);
//
struct blasfeo_dvec *ocp_nlp_cost_conl_memory_get_grad_ptr(void *memory_);
//
void ocp_nlp_cost_conl_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
//
void ocp_nlp_cost_conl_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
//
void ocp_nlp_cost_conl_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
//
void ocp_nlp_cost_conl_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
//
void ocp_nlp_cost_conl_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
//
void ocp_nlp_cost_conl_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
/************************************************
* workspace
************************************************/
typedef struct
{
struct blasfeo_dmat W; // hessian of outer loss function
struct blasfeo_dmat W_chol; // cholesky factor of hessian of outer loss function
struct blasfeo_dmat Jt_ux; // jacobian of inner residual function
struct blasfeo_dmat Jt_ux_tilde; // jacobian of inner residual function plus gradient contribution of algebraic variables
struct blasfeo_dmat Jt_z; // jacobian of inner residual function wrt algebraic variables
struct blasfeo_dmat tmp_nv_ny;
struct blasfeo_dvec tmp_ny;
struct blasfeo_dvec tmp_2ns;
} ocp_nlp_cost_conl_workspace;
//
acados_size_t ocp_nlp_cost_conl_workspace_calculate_size(void *config, void *dims, void *opts);
/************************************************
* functions
************************************************/
//
void ocp_nlp_cost_conl_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_conl_config_initialize_default(void *config);
//
void ocp_nlp_cost_conl_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
//
void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
/// @}
/// @}
/// @}

187
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h vendored Normal file
View File

@@ -0,0 +1,187 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_
#define ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* dims
************************************************/
typedef struct
{
int nx; // number of states
int nz; // number of algebraic variables
int nu; // number of inputs
int ns; // number of slacks
} ocp_nlp_cost_external_dims;
//
acados_size_t ocp_nlp_cost_external_dims_calculate_size(void *config);
//
void *ocp_nlp_cost_external_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_cost_external_dims_set(void *config_, void *dims_, const char *field, int* value);
//
void ocp_nlp_cost_external_dims_get(void *config_, void *dims_, const char *field, int* value);
/************************************************
* model
************************************************/
typedef struct
{
external_function_generic *ext_cost_fun; // function
external_function_generic *ext_cost_fun_jac_hess; // function, gradient and hessian
external_function_generic *ext_cost_fun_jac; // function, gradient
struct blasfeo_dvec Z;
struct blasfeo_dvec z;
struct blasfeo_dmat numerical_hessian; // custom hessian approximation
double scaling;
} ocp_nlp_cost_external_model;
//
acados_size_t ocp_nlp_cost_external_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_external_model_assign(void *config, void *dims, void *raw_memory);
/************************************************
* options
************************************************/
typedef struct
{
int use_numerical_hessian; // > 0 indicating custom hessian is used instead of CasADi evaluation
} ocp_nlp_cost_external_opts;
//
acados_size_t ocp_nlp_cost_external_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_external_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_cost_external_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_external_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_external_opts_set(void *config, void *opts, const char *field, void *value);
/************************************************
* memory
************************************************/
typedef struct
{
struct blasfeo_dvec grad; // gradient of cost function
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
struct blasfeo_dvec *tmp_ux; // pointer to tmp_ux in nlp_out
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dvec *Z; // pointer to Z in qp_in
struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
double fun; ///< value of the cost function
} ocp_nlp_cost_external_memory;
//
acados_size_t ocp_nlp_cost_external_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_cost_external_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
double *ocp_nlp_cost_external_memory_get_fun_ptr(void *memory_);
//
struct blasfeo_dvec *ocp_nlp_cost_external_memory_get_grad_ptr(void *memory_);
//
void ocp_nlp_cost_external_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
//
void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
//
void ocp_nlp_cost_external_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
//
void ocp_nlp_cost_external_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
//
void ocp_nlp_cost_external_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
//
void ocp_nlp_cost_external_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
/************************************************
* workspace
************************************************/
typedef struct
{
struct blasfeo_dmat tmp_nunx_nunx;
struct blasfeo_dmat tmp_nz_nz;
struct blasfeo_dmat tmp_nz_nunx;
struct blasfeo_dvec tmp_nunxnz;
struct blasfeo_dvec tmp_2ns; // temporary vector of dimension 2*ns
} ocp_nlp_cost_external_workspace;
//
acados_size_t ocp_nlp_cost_external_workspace_calculate_size(void *config, void *dims, void *opts);
/************************************************
* functions
************************************************/
//
void ocp_nlp_cost_external_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_external_config_initialize_default(void *config);
//
void ocp_nlp_cost_external_initialize(void *config_, void *dims, void *model_,
void *opts_, void *mem_, void *work_);
//
void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_external_compute_fun(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_

243
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h vendored Normal file
View File

@@ -0,0 +1,243 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_cost ocp_nlp_cost
/// @{
/// \addtogroup ocp_nlp_cost_ls ocp_nlp_cost_ls
/// \brief This module implements linear-least squares costs of the form
/// \f$\min_{x,u,z} \| V_x x + V_u u + V_z z - y_{\text{ref}}\|_W^2\f$.
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
#define ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
////////////////////////////////////////////////////////////////////////////////
// dims //
////////////////////////////////////////////////////////////////////////////////
typedef struct
{
int nx; // number of states
int nz; // number of algebraic variables
int nu; // number of inputs
int ny; // number of outputs
int ns; // number of slacks
} ocp_nlp_cost_ls_dims;
/// Calculate the size of the ocp_nlp_cost_ls_dims struct
///
/// \param[in] config_ structure containing configuration of ocp_nlp_cost
/// module
/// \param[out] []
/// \return \c size of ocp_nlp_dims struct
acados_size_t ocp_nlp_cost_ls_dims_calculate_size(void *config);
/// Assign memory pointed to by raw_memory to ocp_nlp-cost_ls dims struct
///
/// \param[in] config structure containing configuration of ocp_nlp_cost
/// module
/// \param[in] raw_memory pointer to memory location
/// \param[out] []
/// \return dims
void *ocp_nlp_cost_ls_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_cost_ls_dims_set(void *config_, void *dims_, const char *field, int* value);
//
void ocp_nlp_cost_ls_dims_get(void *config_, void *dims_, const char *field, int* value);
////////////////////////////////////////////////////////////////////////////////
// model //
////////////////////////////////////////////////////////////////////////////////
/// structure containing the data describing the linear least-square cost
typedef struct
{
// slack penalty has the form z^T * s + .5 * s^T * Z * s
struct blasfeo_dmat Cyt; ///< output matrix: Cy * [u,x] = y; in transposed form
struct blasfeo_dmat Vz; ///< Vz in ls cost Vx*x + Vu*u + Vz*z
struct blasfeo_dmat W; ///< ls norm corresponding to this matrix
struct blasfeo_dvec y_ref; ///< yref
struct blasfeo_dvec Z; ///< diagonal Hessian of slacks as vector (lower and upper)
struct blasfeo_dvec z; ///< gradient of slacks as vector (lower and upper)
double scaling;
int W_changed; ///< flag indicating whether W has changed and needs to be refactorized
int Cyt_or_scaling_changed; ///< flag indicating whether Cyt or scaling has changed and Hessian needs to be recomputed
} ocp_nlp_cost_ls_model;
//
acados_size_t ocp_nlp_cost_ls_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_ls_model_assign(void *config, void *dims, void *raw_memory);
//
int ocp_nlp_cost_ls_model_set(void *config_, void *dims_, void *model_,
const char *field, void *value_);
////////////////////////////////////////////////////////////////////////////////
// options //
////////////////////////////////////////////////////////////////////////////////
typedef struct
{
int dummy; // struct can't be void
} ocp_nlp_cost_ls_opts;
//
acados_size_t ocp_nlp_cost_ls_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_ls_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_cost_ls_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_ls_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_ls_opts_set(void *config, void *opts, const char *field, void *value);
////////////////////////////////////////////////////////////////////////////////
// memory //
////////////////////////////////////////////////////////////////////////////////
/// structure containing the memory associated with cost_ls component
/// of the ocp_nlp module
typedef struct
{
struct blasfeo_dmat hess; ///< hessian of cost function
struct blasfeo_dmat W_chol; ///< cholesky factor of weight matrix
struct blasfeo_dvec res; ///< ls residual r(x)
struct blasfeo_dvec grad; ///< gradient of cost function
struct blasfeo_dvec *ux; ///< pointer to ux in nlp_out
struct blasfeo_dvec *tmp_ux; ///< pointer to ux in tmp_nlp_out
struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
struct blasfeo_dmat *RSQrq; ///< pointer to RSQrq in qp_in
struct blasfeo_dvec *Z; ///< pointer to Z in qp_in
double fun; ///< value of the cost function
} ocp_nlp_cost_ls_memory;
//
acados_size_t ocp_nlp_cost_ls_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_cost_ls_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
double *ocp_nlp_cost_ls_memory_get_fun_ptr(void *memory_);
//
struct blasfeo_dvec *ocp_nlp_cost_ls_memory_get_grad_ptr(void *memory_);
//
void ocp_nlp_cost_ls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
//
void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
//
void ocp_nlp_cost_ls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
//
void ocp_nlp_cost_ls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
//
void ocp_nlp_cost_ls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
//
void ocp_nlp_cost_ls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
////////////////////////////////////////////////////////////////////////////////
// workspace //
////////////////////////////////////////////////////////////////////////////////
typedef struct
{
struct blasfeo_dmat tmp_nv_ny; // temporary matrix of dimensions nv, ny
struct blasfeo_dmat Cyt_tilde; // updated Cyt (after z elimination)
struct blasfeo_dmat dzdux_tran; // derivatives of z wrt u and x (tran)
struct blasfeo_dvec tmp_ny; // temporary vector of dimension ny
struct blasfeo_dvec tmp_2ns; // temporary vector of dimension ny
struct blasfeo_dvec tmp_nz; // temporary vector of dimension nz
struct blasfeo_dvec y_ref_tilde; // updated y_ref (after z elimination)
} ocp_nlp_cost_ls_workspace;
//
acados_size_t ocp_nlp_cost_ls_workspace_calculate_size(void *config, void *dims, void *opts);
////////////////////////////////////////////////////////////////////////////////
// functions //
////////////////////////////////////////////////////////////////////////////////
// computations that are done once when solver is created
void ocp_nlp_cost_ls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_ls_config_initialize_default(void *config);
//
void ocp_nlp_cost_ls_initialize(void *config_, void *dims, void *model_, void *opts_,
void *mem_, void *work_);
//
void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims, void *model_,
void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
/// @}
/// @}
/// @}

211
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h vendored Normal file
View File

@@ -0,0 +1,211 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_cost ocp_nlp_cost
/// @{
/// \addtogroup ocp_nlp_cost_nls ocp_nlp_cost_nls
/// \brief This module implements nonlinear-least squares costs of the form
/// \f$\min_{x,u,z} \| y(x,u,z,p) - y_{\text{ref}} \|_W^2\f$,
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
#define ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* dims
************************************************/
typedef struct
{
int nx; // number of states
int nz; // number of algebraic variables
int nu; // number of inputs
int ny; // number of outputs
int ns; // number of slacks
} ocp_nlp_cost_nls_dims;
//
acados_size_t ocp_nlp_cost_nls_dims_calculate_size(void *config);
//
void *ocp_nlp_cost_nls_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_cost_nls_dims_set(void *config_, void *dims_, const char *field, int* value);
//
void ocp_nlp_cost_nls_dims_get(void *config_, void *dims_, const char *field, int* value);
/************************************************
* model
************************************************/
typedef struct
{
// nonliner function nls_y(x,u) replaces Cy * [x,u] in ls_cost
// slack penalty has the form z^T * s + .5 * s^T * Z * s
external_function_generic *nls_y_fun; // evaluation of nls function
external_function_generic *nls_y_fun_jac; // evaluation nls function and jacobian
external_function_generic *nls_y_hess; // hessian*seeds of nls residuals
struct blasfeo_dmat W; //
struct blasfeo_dvec y_ref;
struct blasfeo_dvec Z; // diagonal Hessian of slacks as vector
struct blasfeo_dvec z; // gradient of slacks as vector
double scaling;
int W_changed; ///< flag indicating whether W has changed and needs to be refactorized
} ocp_nlp_cost_nls_model;
//
acados_size_t ocp_nlp_cost_nls_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_nls_model_assign(void *config, void *dims, void *raw_memory);
//
int ocp_nlp_cost_nls_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_);
/************************************************
* options
************************************************/
typedef struct
{
bool gauss_newton_hess; // gauss-newton hessian approximation
} ocp_nlp_cost_nls_opts;
//
acados_size_t ocp_nlp_cost_nls_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_cost_nls_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_cost_nls_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_nls_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_cost_nls_opts_set(void *config, void *opts, const char *field, void *value);
/************************************************
* memory
************************************************/
typedef struct
{
struct blasfeo_dmat W_chol; // cholesky factor of weight matrix
struct blasfeo_dmat Jt; // jacobian of nls fun
struct blasfeo_dvec res; // nls residual r(x)
struct blasfeo_dvec grad; // gradient of cost function
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dvec *Z; // pointer to Z in qp_in
struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
double fun; ///< value of the cost function
} ocp_nlp_cost_nls_memory;
//
acados_size_t ocp_nlp_cost_nls_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_cost_nls_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
double *ocp_nlp_cost_nls_memory_get_fun_ptr(void *memory_);
//
struct blasfeo_dvec *ocp_nlp_cost_nls_memory_get_grad_ptr(void *memory_);
//
void ocp_nlp_cost_nls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
//
void ocp_nlp_cost_nls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
//
void ocp_nlp_cost_nls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
//
void ocp_nlp_cost_nls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
//
void ocp_nlp_cost_nls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
//
void ocp_nlp_cost_nls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
/************************************************
* workspace
************************************************/
typedef struct
{
struct blasfeo_dmat tmp_nv_ny;
struct blasfeo_dmat tmp_nv_nv;
struct blasfeo_dmat Vz;
struct blasfeo_dmat Cyt_tilde;
struct blasfeo_dvec tmp_ny;
struct blasfeo_dvec tmp_2ns;
struct blasfeo_dvec tmp_nz;
} ocp_nlp_cost_nls_workspace;
//
acados_size_t ocp_nlp_cost_nls_workspace_calculate_size(void *config, void *dims, void *opts);
/************************************************
* functions
************************************************/
//
void ocp_nlp_cost_nls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_nls_config_initialize_default(void *config);
//
void ocp_nlp_cost_nls_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
//
void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
//
void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
/// @}
/// @}
/// @}

119
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h vendored Normal file
View File

@@ -0,0 +1,119 @@
/*
* 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.;
*/
/// \ingroup ocp_nlp
/// @{
/// \defgroup ocp_nlp_dynamics ocp_nlp_dynamics
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/sim/sim_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* config
************************************************/
typedef struct
{
void (*config_initialize_default)(void *config);
sim_config *sim_solver;
/* dims */
acados_size_t (*dims_calculate_size)(void *config);
void *(*dims_assign)(void *config, void *raw_memory);
void (*dims_set)(void *config_, void *dims_, const char *field, int *value);
void (*dims_get)(void *config_, void *dims_, const char *field, int* value);
/* model */
acados_size_t (*model_calculate_size)(void *config, void *dims);
void *(*model_assign)(void *config, void *dims, void *raw_memory);
void (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_);
/* opts */
acados_size_t (*opts_calculate_size)(void *config, void *dims);
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
void (*opts_initialize_default)(void *config, void *dims, void *opts);
void (*opts_set)(void *config_, void *opts_, const char *field, void *value);
void (*opts_update)(void *config, void *dims, void *opts);
/* memory */
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
// get shooting node gap x_next(x_n, u_n) - x_{n+1}
struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory_);
struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory_);
void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory_);
void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory_);
void (*memory_set_ux1_ptr)(struct blasfeo_dvec *ux1, void *memory_);
void (*memory_set_tmp_ux1_ptr)(struct blasfeo_dvec *tmp_ux1, void *memory_);
void (*memory_set_pi_ptr)(struct blasfeo_dvec *pi, void *memory_);
void (*memory_set_tmp_pi_ptr)(struct blasfeo_dvec *tmp_pi, void *memory_);
void (*memory_set_BAbt_ptr)(struct blasfeo_dmat *BAbt, void *memory_);
void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory_);
void (*memory_set_dzduxt_ptr)(struct blasfeo_dmat *mat, void *memory_);
void (*memory_set_sim_guess_ptr)(struct blasfeo_dvec *vec, bool *bool_ptr, void *memory_);
void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *vec, void *memory_);
void (*memory_get)(void *config, void *dims, void *mem, const char *field, void* value);
/* workspace */
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
int (*precompute)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
} ocp_nlp_dynamics_config;
//
acados_size_t ocp_nlp_dynamics_config_calculate_size();
//
ocp_nlp_dynamics_config *ocp_nlp_dynamics_config_assign(void *raw_memory);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
/// @}
/// @}

209
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h vendored Normal file
View File

@@ -0,0 +1,209 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_dynamics
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
#include "acados_c/sim_interface.h"
/************************************************
* dims
************************************************/
typedef struct
{
void *sim;
int nx; // number of states at the current stage
int nz; // number of algebraic states at the current stage
int nu; // number of inputs at the current stage
int nx1; // number of states at the next stage
int nu1; // number of inputes at the next stage
} ocp_nlp_dynamics_cont_dims;
//
acados_size_t ocp_nlp_dynamics_cont_dims_calculate_size(void *config);
//
void *ocp_nlp_dynamics_cont_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_dynamics_cont_dims_set(void *config_, void *dims_, const char *field, int* value);
/************************************************
* options
************************************************/
typedef struct
{
void *sim_solver;
int compute_adj;
int compute_hess;
} ocp_nlp_dynamics_cont_opts;
//
acados_size_t ocp_nlp_dynamics_cont_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_dynamics_cont_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_dynamics_cont_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_dynamics_cont_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_dynamics_cont_opts_set(void *config, void *opts, const char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct
{
struct blasfeo_dvec fun;
struct blasfeo_dvec adj;
struct blasfeo_dvec *ux; // pointer to ux in nlp_out at current stage
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out at current stage
struct blasfeo_dvec *ux1; // pointer to ux in nlp_out at next stage
struct blasfeo_dvec *tmp_ux1; // pointer to ux in tmp_nlp_out at next stage
struct blasfeo_dvec *pi; // pointer to pi in nlp_out at current stage
struct blasfeo_dvec *tmp_pi; // pointer to pi in tmp_nlp_out at current stage
struct blasfeo_dmat *BAbt; // pointer to BAbt in qp_in
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dvec *z_alg; // pointer to output z at t = 0
bool *set_sim_guess; // indicate if initialization for integrator is set from outside
struct blasfeo_dvec *sim_guess; // initializations for integrator
// struct blasfeo_dvec *z; // pointer to (input) z in nlp_out at current stage
struct blasfeo_dmat *dzduxt; // pointer to dzdux transposed
void *sim_solver; // sim solver memory
} ocp_nlp_dynamics_cont_memory;
//
acados_size_t ocp_nlp_dynamics_cont_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_dynamics_cont_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_fun_ptr(void *memory);
//
struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_adj_ptr(void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory);
//
void ocp_nlp_dynamics_cont_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory);
/************************************************
* workspace
************************************************/
typedef struct
{
struct blasfeo_dmat hess;
sim_in *sim_in;
sim_out *sim_out;
void *sim_solver; // sim solver workspace
} ocp_nlp_dynamics_cont_workspace;
acados_size_t ocp_nlp_dynamics_cont_workspace_calculate_size(void *config, void *dims, void *opts);
/************************************************
* model
************************************************/
typedef struct
{
void *sim_model;
// double *state_transition; // TODO
double T; // simulation time
} ocp_nlp_dynamics_cont_model;
//
acados_size_t ocp_nlp_dynamics_cont_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_dynamics_cont_model_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_dynamics_cont_model_set(void *config_, void *dims_, void *model_, const char *field, void *value);
/************************************************
* functions
************************************************/
//
void ocp_nlp_dynamics_cont_config_initialize_default(void *config);
//
void ocp_nlp_dynamics_cont_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
//
void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
//
void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
//
int ocp_nlp_dynamics_cont_precompute(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
/// @}
/// @}

192
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h vendored Normal file
View File

@@ -0,0 +1,192 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_dynamics
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
#include "acados/utils/external_function_generic.h"
#include "acados/utils/types.h"
/************************************************
* dims
************************************************/
typedef struct
{
int nx; // number of states at the current stage
int nu; // number of inputs at the current stage
int nx1; // number of states at the next stage
int nu1; // number of inputes at the next stage
} ocp_nlp_dynamics_disc_dims;
//
acados_size_t ocp_nlp_dynamics_disc_dims_calculate_size(void *config);
//
void *ocp_nlp_dynamics_disc_dims_assign(void *config, void *raw_memory);
//
void ocp_nlp_dynamics_disc_dims_set(void *config_, void *dims_, const char *dim, int* value);
/************************************************
* options
************************************************/
typedef struct
{
int compute_adj;
int compute_hess;
} ocp_nlp_dynamics_disc_opts;
//
acados_size_t ocp_nlp_dynamics_disc_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_dynamics_disc_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_dynamics_disc_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_dynamics_disc_opts_update(void *config, void *dims, void *opts);
//
int ocp_nlp_dynamics_disc_precompute(void *config_, void *dims, void *model_, void *opts_,
void *mem_, void *work_);
/************************************************
* memory
************************************************/
typedef struct
{
struct blasfeo_dvec fun;
struct blasfeo_dvec adj;
struct blasfeo_dvec *ux; // pointer to ux in nlp_out at current stage
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out at current stage
struct blasfeo_dvec *ux1; // pointer to ux in nlp_out at next stage
struct blasfeo_dvec *tmp_ux1;// pointer to ux in tmp_nlp_out at next stage
struct blasfeo_dvec *pi; // pointer to pi in nlp_out at current stage
struct blasfeo_dvec *tmp_pi; // pointer to pi in tmp_nlp_out at current stage
struct blasfeo_dmat *BAbt; // pointer to BAbt in qp_in
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
} ocp_nlp_dynamics_disc_memory;
//
acados_size_t ocp_nlp_dynamics_disc_memory_calculate_size(void *config, void *dims, void *opts);
//
void *ocp_nlp_dynamics_disc_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
//
struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_fun_ptr(void *memory);
//
struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_adj_ptr(void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory);
//
void ocp_nlp_dynamics_disc_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory);
/************************************************
* workspace
************************************************/
typedef struct
{
struct blasfeo_dmat tmp_nv_nv;
} ocp_nlp_dynamics_disc_workspace;
acados_size_t ocp_nlp_dynamics_disc_workspace_calculate_size(void *config, void *dims, void *opts);
/************************************************
* model
************************************************/
typedef struct
{
external_function_generic *disc_dyn_fun;
external_function_generic *disc_dyn_fun_jac;
external_function_generic *disc_dyn_fun_jac_hess;
} ocp_nlp_dynamics_disc_model;
//
acados_size_t ocp_nlp_dynamics_disc_model_calculate_size(void *config, void *dims);
//
void *ocp_nlp_dynamics_disc_model_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_dynamics_disc_model_set(void *config_, void *dims_, void *model_, const char *field, void *value);
/************************************************
* functions
************************************************/
//
void ocp_nlp_dynamics_disc_config_initialize_default(void *config);
//
void ocp_nlp_dynamics_disc_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
//
void ocp_nlp_dynamics_disc_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
//
void ocp_nlp_dynamics_disc_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
/// @}
/// @}

122
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h vendored Normal file
View File

@@ -0,0 +1,122 @@
/*
* 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.;
*/
/// \ingroup ocp_nlp
/// @{
/// \defgroup ocp_nlp_reg ocp_nlp_reg
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
#define ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "acados/ocp_qp/ocp_qp_common.h"
/* dims */
//typedef ocp_qp_dims ocp_nlp_reg_dims;
typedef struct
{
int *nx;
int *nu;
int *nbu;
int *nbx;
int *ng;
int N;
} ocp_nlp_reg_dims;
//
acados_size_t ocp_nlp_reg_dims_calculate_size(int N);
//
ocp_nlp_reg_dims *ocp_nlp_reg_dims_assign(int N, void *raw_memory);
//
void ocp_nlp_reg_dims_set(void *config_, ocp_nlp_reg_dims *dims, int stage, char *field, int* value);
/* config */
typedef struct
{
/* dims */
acados_size_t (*dims_calculate_size)(int N);
ocp_nlp_reg_dims *(*dims_assign)(int N, void *raw_memory);
void (*dims_set)(void *config, ocp_nlp_reg_dims *dims, int stage, char *field, int *value);
/* opts */
acados_size_t (*opts_calculate_size)(void);
void *(*opts_assign)(void *raw_memory);
void (*opts_initialize_default)(void *config, ocp_nlp_reg_dims *dims, void *opts);
void (*opts_set)(void *config, ocp_nlp_reg_dims *dims, void *opts, char *field, void* value);
/* memory */
acados_size_t (*memory_calculate_size)(void *config, ocp_nlp_reg_dims *dims, void *opts);
void *(*memory_assign)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
void (*memory_set)(void *config, ocp_nlp_reg_dims *dims, void *memory, char *field, void* value);
void (*memory_set_RSQrq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
void (*memory_set_rq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
void (*memory_set_BAbt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
void (*memory_set_b_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
void (*memory_set_idxb_ptr)(ocp_nlp_reg_dims *dims, int **idxb, void *memory);
void (*memory_set_DCt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
void (*memory_set_ux_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
void (*memory_set_pi_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
void (*memory_set_lam_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
/* functions */
void (*regularize_hessian)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory);
void (*correct_dual_sol)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory);
} ocp_nlp_reg_config;
//
acados_size_t ocp_nlp_reg_config_calculate_size(void);
//
void *ocp_nlp_reg_config_assign(void *raw_memory);
/* regularization help functions */
void acados_reconstruct_A(int dim, double *A, double *V, double *d);
void acados_mirror(int dim, double *A, double *V, double *d, double *e, double epsilon);
void acados_project(int dim, double *A, double *V, double *d, double *e, double epsilon);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
/// @}
/// @}

146
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h vendored Normal file
View File

@@ -0,0 +1,146 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_reg
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
#define ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
/************************************************
* dims
************************************************/
// use the functions in ocp_nlp_reg_common
/************************************************
* options
************************************************/
typedef struct
{
double delta;
double epsilon;
// double gamma; // 0.0
} ocp_nlp_reg_convexify_opts;
//
acados_size_t ocp_nlp_reg_convexify_opts_calculate_size(void);
//
void *ocp_nlp_reg_convexify_opts_assign(void *raw_memory);
//
void ocp_nlp_reg_convexify_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
//
void ocp_nlp_reg_convexify_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct {
double *R;
double *V; // TODO move to workspace
double *d; // TODO move to workspace
double *e; // TODO move to workspace
double *reg_hess; // TODO move to workspace
struct blasfeo_dmat Q_tilde;
struct blasfeo_dmat Q_bar;
struct blasfeo_dmat BAQ;
struct blasfeo_dmat L;
struct blasfeo_dmat delta_eye;
struct blasfeo_dmat St_copy;
struct blasfeo_dmat *original_RSQrq;
struct blasfeo_dmat tmp_RSQ;
struct blasfeo_dvec tmp_nuxM;
struct blasfeo_dvec tmp_nbgM;
// struct blasfeo_dvec grad;
// struct blasfeo_dvec b2;
// giaf's
struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dvec **rq; // pointer to rq in qp_in
struct blasfeo_dmat **BAbt; // pointer to BAbt in qp_in
struct blasfeo_dvec **b; // pointer to b in qp_in
struct blasfeo_dmat **DCt; // pointer to DCt in qp_in
struct blasfeo_dvec **ux; // pointer to ux in qp_out
struct blasfeo_dvec **pi; // pointer to pi in qp_out
struct blasfeo_dvec **lam; // pointer to lam in qp_out
int **idxb; // pointer to idxb in qp_in
} ocp_nlp_reg_convexify_memory;
//
acados_size_t ocp_nlp_reg_convexify_calculate_memory_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
//
void *ocp_nlp_reg_convexify_assign_memory(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
/************************************************
* workspace
************************************************/
// TODO
/************************************************
* functions
************************************************/
//
void ocp_nlp_reg_convexify_config_initialize_default(ocp_nlp_reg_config *config);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
/// @}
/// @}

121
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h vendored Normal file
View File

@@ -0,0 +1,121 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_reg
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
#define ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
/************************************************
* dims
************************************************/
// use the functions in ocp_nlp_reg_common
/************************************************
* options
************************************************/
typedef struct
{
double epsilon;
} ocp_nlp_reg_mirror_opts;
//
acados_size_t ocp_nlp_reg_mirror_opts_calculate_size(void);
//
void *ocp_nlp_reg_mirror_opts_assign(void *raw_memory);
//
void ocp_nlp_reg_mirror_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
//
void ocp_nlp_reg_mirror_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct
{
double *reg_hess; // TODO move to workspace
double *V; // TODO move to workspace
double *d; // TODO move to workspace
double *e; // TODO move to workspace
// giaf's
struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in
} ocp_nlp_reg_mirror_memory;
//
acados_size_t ocp_nlp_reg_mirror_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
//
void *ocp_nlp_reg_mirror_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
/************************************************
* workspace
************************************************/
// TODO
/************************************************
* functions
************************************************/
//
void ocp_nlp_reg_mirror_config_initialize_default(ocp_nlp_reg_config *config);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
/// @}
/// @}

106
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h vendored Normal file
View File

@@ -0,0 +1,106 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_reg
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
#define ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
/************************************************
* dims
************************************************/
// use the functions in ocp_nlp_reg_common
/************************************************
* options
************************************************/
//
acados_size_t ocp_nlp_reg_noreg_opts_calculate_size(void);
//
void *ocp_nlp_reg_noreg_opts_assign(void *raw_memory);
//
void ocp_nlp_reg_noreg_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
//
void ocp_nlp_reg_noreg_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
/************************************************
* memory
************************************************/
//
acados_size_t ocp_nlp_reg_noreg_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
//
void *ocp_nlp_reg_noreg_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
/************************************************
* workspace
************************************************/
// not needed
/************************************************
* functions
************************************************/
//
void ocp_nlp_reg_noreg_config_initialize_default(ocp_nlp_reg_config *config);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
/// @}
/// @}

121
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h vendored Normal file
View File

@@ -0,0 +1,121 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_reg
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
/************************************************
* dims
************************************************/
// use the functions in ocp_nlp_reg_common
/************************************************
* options
************************************************/
typedef struct
{
double epsilon;
} ocp_nlp_reg_project_opts;
//
acados_size_t ocp_nlp_reg_project_opts_calculate_size(void);
//
void *ocp_nlp_reg_project_opts_assign(void *raw_memory);
//
void ocp_nlp_reg_project_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
//
void ocp_nlp_reg_project_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct
{
double *reg_hess; // TODO move to workspace
double *V; // TODO move to workspace
double *d; // TODO move to workspace
double *e; // TODO move to workspace
// giaf's
struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in
} ocp_nlp_reg_project_memory;
//
acados_size_t ocp_nlp_reg_project_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
//
void *ocp_nlp_reg_project_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
/************************************************
* workspace
************************************************/
// TODO
/************************************************
* functions
************************************************/
//
void ocp_nlp_reg_project_config_initialize_default(ocp_nlp_reg_config *config);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
/// @}
/// @}

132
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h vendored Normal file
View File

@@ -0,0 +1,132 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_reg
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
#ifdef __cplusplus
extern "C" {
#endif
// blasfeo
#include "blasfeo/include/blasfeo_common.h"
// acados
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
/************************************************
* dims
************************************************/
// use the functions in ocp_nlp_reg_common
/************************************************
* options
************************************************/
typedef struct
{
double thr_eig;
double min_eig;
double min_pivot;
int pivoting;
} ocp_nlp_reg_project_reduc_hess_opts;
//
acados_size_t ocp_nlp_reg_project_reduc_hess_opts_calculate_size(void);
//
void *ocp_nlp_reg_project_reduc_hess_opts_assign(void *raw_memory);
//
void ocp_nlp_reg_project_reduc_hess_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
//
void ocp_nlp_reg_project_reduc_hess_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct
{
double *reg_hess; // TODO move to workspace
double *V; // TODO move to workspace
double *d; // TODO move to workspace
double *e; // TODO move to workspace
// giaf's
struct blasfeo_dmat L; // TODO move to workspace
struct blasfeo_dmat L2; // TODO move to workspace
struct blasfeo_dmat L3; // TODO move to workspace
struct blasfeo_dmat Ls; // TODO move to workspace
struct blasfeo_dmat P; // TODO move to workspace
struct blasfeo_dmat AL; // TODO move to workspace
struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in
struct blasfeo_dmat **BAbt; // pointer to RSQrq in qp_in
} ocp_nlp_reg_project_reduc_hess_memory;
//
acados_size_t ocp_nlp_reg_project_reduc_hess_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
//
void *ocp_nlp_reg_project_reduc_hess_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
/************************************************
* workspace
************************************************/
// TODO
/************************************************
* functions
************************************************/
//
void ocp_nlp_reg_project_reduc_hess_config_initialize_default(ocp_nlp_reg_config *config);
#ifdef __cplusplus
}
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
/// @}
/// @}

170
third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h vendored Normal file
View File

@@ -0,0 +1,170 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_solver
/// @{
/// \addtogroup ocp_nlp_sqp ocp_nlp_sqp
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_H_
#define ACADOS_OCP_NLP_OCP_NLP_SQP_H_
#ifdef __cplusplus
extern "C" {
#endif
// acados
#include "acados/ocp_nlp/ocp_nlp_common.h"
#include "acados/utils/types.h"
/************************************************
* options
************************************************/
typedef struct
{
ocp_nlp_opts *nlp_opts;
double tol_stat; // exit tolerance on stationarity condition
double tol_eq; // exit tolerance on equality constraints
double tol_ineq; // exit tolerance on inequality constraints
double tol_comp; // exit tolerance on complementarity condition
int max_iter;
int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
int qp_warm_start; // qp_warm_start in all but the first sqp iterations
bool warm_start_first_qp; // to set qp_warm_start in first iteration
int rti_phase; // only phase 0 at the moment
int initialize_t_slacks; // 0-false or 1-true
} ocp_nlp_sqp_opts;
//
acados_size_t ocp_nlp_sqp_opts_calculate_size(void *config, void *dims);
//
void *ocp_nlp_sqp_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_nlp_sqp_opts_initialize_default(void *config, void *dims, void *opts);
//
void ocp_nlp_sqp_opts_update(void *config, void *dims, void *opts);
//
void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* value);
//
void ocp_nlp_sqp_opts_set_at_stage(void *config_, void *opts_, size_t stage, const char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct
{
// nlp memory
ocp_nlp_memory *nlp_mem;
double time_qp_sol;
double time_qp_solver_call;
double time_qp_xcond;
double time_lin;
double time_reg;
double time_tot;
double time_glob;
double time_sim;
double time_sim_la;
double time_sim_ad;
double time_solution_sensitivities;
// statistics
double *stat;
int stat_m;
int stat_n;
int status;
int sqp_iter;
} ocp_nlp_sqp_memory;
//
acados_size_t ocp_nlp_sqp_memory_calculate_size(void *config, void *dims, void *opts_);
//
void *ocp_nlp_sqp_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
//
void ocp_nlp_sqp_memory_reset_qp_solver(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
void *opts_, void *mem_, void *work_);
/************************************************
* workspace
************************************************/
typedef struct
{
ocp_nlp_workspace *nlp_work;
// temp QP in & out (to be used as workspace in param sens)
ocp_qp_in *tmp_qp_in;
ocp_qp_out *tmp_qp_out;
// qp residuals
ocp_qp_res *qp_res;
ocp_qp_res_ws *qp_res_ws;
} ocp_nlp_sqp_workspace;
//
acados_size_t ocp_nlp_sqp_workspace_calculate_size(void *config, void *dims, void *opts_);
/************************************************
* functions
************************************************/
//
int ocp_nlp_sqp(void *config, void *dims, void *nlp_in, void *nlp_out,
void *args, void *mem, void *work_);
//
void ocp_nlp_sqp_config_initialize_default(void *config_);
//
int ocp_nlp_sqp_precompute(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
void *opts_, void *mem_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_SQP_H_
/// @}
/// @}
/// @}

163
third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h vendored Normal file
View File

@@ -0,0 +1,163 @@
/*
* 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.;
*/
/// \addtogroup ocp_nlp
/// @{
/// \addtogroup ocp_nlp_solver
/// @{
/// \addtogroup ocp_nlp_sqp_rti ocp_nlp_sqp_rti
/// @{
#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
#define ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
#ifdef __cplusplus
extern "C" {
#endif
// acados
#include "acados/ocp_nlp/ocp_nlp_common.h"
#include "acados/utils/types.h"
/************************************************
* options
************************************************/
typedef struct
{
ocp_nlp_opts *nlp_opts;
int compute_dual_sol;
int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
int qp_warm_start; // NOTE: this is not actually setting the warm_start! Just for compatibility with sqp.
bool warm_start_first_qp; // to set qp_warm_start in first iteration
int rti_phase; // phase of RTI. Possible values 1 (preparation), 2 (feedback) 0 (both)
} ocp_nlp_sqp_rti_opts;
//
acados_size_t ocp_nlp_sqp_rti_opts_calculate_size(void *config_, void *dims_);
//
void *ocp_nlp_sqp_rti_opts_assign(void *config_, void *dims_, void *raw_memory);
//
void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, void *dims_, void *opts_);
//
void ocp_nlp_sqp_rti_opts_update(void *config_, void *dims_, void *opts_);
//
void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, void* value);
//
void ocp_nlp_sqp_rti_opts_set_at_stage(void *config_, void *opts_, size_t stage,
const char *field, void* value);
/************************************************
* memory
************************************************/
typedef struct
{
// nlp memory
ocp_nlp_memory *nlp_mem;
double time_qp_sol;
double time_qp_solver_call;
double time_qp_xcond;
double time_lin;
double time_reg;
double time_tot;
double time_glob;
double time_solution_sensitivities;
// statistics
double *stat;
int stat_m;
int stat_n;
int status;
} ocp_nlp_sqp_rti_memory;
//
acados_size_t ocp_nlp_sqp_rti_memory_calculate_size(void *config_, void *dims_, void *opts_);
//
void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, void *opts_,
void *raw_memory);
/************************************************
* workspace
************************************************/
typedef struct
{
ocp_nlp_workspace *nlp_work;
// temp QP in & out (to be used as workspace in param sens)
ocp_qp_in *tmp_qp_in;
ocp_qp_out *tmp_qp_out;
// qp residuals
ocp_qp_res *qp_res;
ocp_qp_res_ws *qp_res_ws;
} ocp_nlp_sqp_rti_workspace;
//
acados_size_t ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, void *dims_, void *opts_);
/************************************************
* functions
************************************************/
//
int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
void *opts_, void *mem_, void *work_);
//
void ocp_nlp_sqp_rti_config_initialize_default(void *config_);
//
int ocp_nlp_sqp_rti_precompute(void *config_, void *dims_,
void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
/// @}
/// @}
/// @}

180
third_party/acados/include/acados/ocp_qp/ocp_qp_common.h vendored Normal file
View File

@@ -0,0 +1,180 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_OCP_QP_OCP_QP_COMMON_H_
#define ACADOS_OCP_QP_OCP_QP_COMMON_H_
#ifdef __cplusplus
extern "C" {
#endif
// hpipm
#include "hpipm/include/hpipm_d_ocp_qp.h"
#include "hpipm/include/hpipm_d_ocp_qp_dim.h"
#include "hpipm/include/hpipm_d_ocp_qp_res.h"
#include "hpipm/include/hpipm_d_ocp_qp_sol.h"
// acados
#include "acados/utils/types.h"
typedef struct d_ocp_qp_dim ocp_qp_dims;
typedef struct d_ocp_qp ocp_qp_in;
typedef struct d_ocp_qp_sol ocp_qp_out;
typedef struct d_ocp_qp_res ocp_qp_res;
typedef struct d_ocp_qp_res_ws ocp_qp_res_ws;
#ifndef QP_SOLVER_CONFIG_
#define QP_SOLVER_CONFIG_
typedef struct
{
void (*dims_set)(void *config_, void *dims_, int stage, const char *field, int* value);
acados_size_t (*opts_calculate_size)(void *config, void *dims);
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
void (*opts_initialize_default)(void *config, void *dims, void *opts);
void (*opts_update)(void *config, void *dims, void *opts);
void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
void (*memory_get)(void *config_, void *mem_, const char *field, void* value);
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
int (*evaluate)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
void (*memory_reset)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
} qp_solver_config;
#endif
typedef struct
{
acados_size_t (*dims_calculate_size)(void *config, int N);
void *(*dims_assign)(void *config, int N, void *raw_memory);
void (*dims_set)(void *config, void *dims_, int stage, const char *field, int* value);
void (*dims_get)(void *config, void *dims, const char *field, void* value);
// TODO add config everywhere !!!!!
acados_size_t (*opts_calculate_size)(void *dims);
void *(*opts_assign)(void *dims, void *raw_memory);
void (*opts_initialize_default)(void *dims, void *opts);
void (*opts_update)(void *dims, void *opts);
void (*opts_set)(void *opts_, const char *field, void* value);
acados_size_t (*memory_calculate_size)(void *dims, void *opts);
void *(*memory_assign)(void *dims, void *opts, void *raw_memory);
void (*memory_get)(void *config, void *mem, const char *field, void* value);
acados_size_t (*workspace_calculate_size)(void *dims, void *opts);
int (*condensing)(void *qp_in, void *qp_out, void *opts, void *mem, void *work);
int (*condensing_rhs)(void *qp_in, void *qp_out, void *opts, void *mem, void *work);
int (*expansion)(void *qp_in, void *qp_out, void *opts, void *mem, void *work);
} ocp_qp_xcond_config;
/// Struct containing metrics of the qp solver.
#ifndef QP_INFO_
#define QP_INFO_
typedef struct
{
double solve_QP_time;
double condensing_time;
double interface_time;
double total_time;
int num_iter;
int t_computed;
} qp_info;
#endif
/* config */
//
acados_size_t ocp_qp_solver_config_calculate_size();
//
qp_solver_config *ocp_qp_solver_config_assign(void *raw_memory);
//
acados_size_t ocp_qp_condensing_config_calculate_size();
//
ocp_qp_xcond_config *ocp_qp_condensing_config_assign(void *raw_memory);
/* dims */
//
acados_size_t ocp_qp_dims_calculate_size(int N);
//
ocp_qp_dims *ocp_qp_dims_assign(int N, void *raw_memory);
//
void ocp_qp_dims_set(void *config_, void *dims, int stage, const char *field, int* value);
//
void ocp_qp_dims_get(void *config_, void *dims, int stage, const char *field, int* value);
/* in */
//
acados_size_t ocp_qp_in_calculate_size(ocp_qp_dims *dims);
//
ocp_qp_in *ocp_qp_in_assign(ocp_qp_dims *dims, void *raw_memory);
/* out */
//
acados_size_t ocp_qp_out_calculate_size(ocp_qp_dims *dims);
//
ocp_qp_out *ocp_qp_out_assign(ocp_qp_dims *dims, void *raw_memory);
/* res */
//
acados_size_t ocp_qp_res_calculate_size(ocp_qp_dims *dims);
//
ocp_qp_res *ocp_qp_res_assign(ocp_qp_dims *dims, void *raw_memory);
//
acados_size_t ocp_qp_res_workspace_calculate_size(ocp_qp_dims *dims);
//
ocp_qp_res_ws *ocp_qp_res_workspace_assign(ocp_qp_dims *dims, void *raw_memory);
//
void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res, ocp_qp_res_ws *res_ws);
//
void ocp_qp_res_compute_nrm_inf(ocp_qp_res *qp_res, double res[4]);
/* misc */
//
void ocp_qp_stack_slacks_dims(ocp_qp_dims *in, ocp_qp_dims *out);
//
void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out);
//
void ocp_qp_compute_t(ocp_qp_in *qp_in, ocp_qp_out *qp_out);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_QP_OCP_QP_COMMON_H_

118
third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h vendored Normal file
View File

@@ -0,0 +1,118 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_
#define ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "acados/ocp_qp/ocp_qp_common.h"
typedef struct
{
int N;
int *nx;
int *nu;
int *nb;
int *nc;
double **A;
double **B;
double **b;
double **Q;
double **S;
double **R;
double **q;
double **r;
int **idxb;
double **lb;
double **ub;
double **Cx;
double **Cu;
double **lc;
double **uc;
} colmaj_ocp_qp_in;
typedef struct
{
double **x;
double **u;
double **pi;
double **lam;
} colmaj_ocp_qp_out;
typedef struct
{
double **res_r;
double **res_q;
double **res_ls;
double **res_us;
double **res_b;
double **res_d_lb;
double **res_d_ub;
double **res_d_lg;
double **res_d_ug;
double **res_d_ls;
double **res_d_us;
double **res_m_lb;
double **res_m_ub;
double **res_m_lg;
double **res_m_ug;
double **res_m_ls;
double **res_m_us;
double res_nrm_inf[4];
} colmaj_ocp_qp_res;
//
acados_size_t colmaj_ocp_qp_in_calculate_size(ocp_qp_dims *dims);
//
char *assign_colmaj_ocp_qp_in(ocp_qp_dims *dims, colmaj_ocp_qp_in **qp_in, void *ptr);
//
acados_size_t colmaj_ocp_qp_out_calculate_size(ocp_qp_dims *dims);
//
char *assign_colmaj_ocp_qp_out(ocp_qp_dims *dims, colmaj_ocp_qp_out **qp_out, void *ptr);
//
acados_size_t colmaj_ocp_qp_res_calculate_size(ocp_qp_dims *dims);
//
char *assign_colmaj_ocp_qp_res(ocp_qp_dims *dims, colmaj_ocp_qp_res **qp_res, void *ptr);
//
void convert_colmaj_to_ocp_qp_in(colmaj_ocp_qp_in *cm_qp_in, ocp_qp_in *qp_in);
//
void convert_ocp_qp_out_to_colmaj(ocp_qp_out *qp_out, colmaj_ocp_qp_out *cm_qp_out);
//
void convert_ocp_qp_res_to_colmaj(ocp_qp_res *qp_res, colmaj_ocp_qp_res *cm_qp_res);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_

114
third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h vendored Normal file
View File

@@ -0,0 +1,114 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_
#define ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_
#ifdef __cplusplus
extern "C" {
#endif
// hpipm
#include "hpipm/include/hpipm_d_ocp_qp_red.h"
// acados
#include "acados/dense_qp/dense_qp_common.h"
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/utils/types.h"
typedef struct
{
ocp_qp_dims *orig_dims;
ocp_qp_dims *red_dims; // dims of reduced qp
dense_qp_dims *fcond_dims;
} ocp_qp_full_condensing_dims;
typedef struct ocp_qp_full_condensing_opts_
{
struct d_cond_qp_arg *hpipm_cond_opts;
struct d_ocp_qp_reduce_eq_dof_arg *hpipm_red_opts;
// dense_qp_dims *fcond_dims; // TODO(all): move to dims
int cond_hess; // 0 cond only rhs, 1 cond hess + rhs
int expand_dual_sol; // 0 primal sol only, 1 primal + dual sol
int ric_alg;
int mem_qp_in; // allocate qp_in in memory
} ocp_qp_full_condensing_opts;
typedef struct ocp_qp_full_condensing_memory_
{
struct d_cond_qp_ws *hpipm_cond_work;
struct d_ocp_qp_reduce_eq_dof_ws *hpipm_red_work;
// in memory
dense_qp_in *fcond_qp_in;
dense_qp_out *fcond_qp_out;
ocp_qp_in *red_qp; // reduced qp
ocp_qp_out *red_sol; // reduced qp sol
// only pointer
ocp_qp_in *ptr_qp_in;
qp_info *qp_out_info; // info in fcond_qp_in
double time_qp_xcond;
} ocp_qp_full_condensing_memory;
//
acados_size_t ocp_qp_full_condensing_opts_calculate_size(void *dims);
//
void *ocp_qp_full_condensing_opts_assign(void *dims, void *raw_memory);
//
void ocp_qp_full_condensing_opts_initialize_default(void *dims, void *opts_);
//
void ocp_qp_full_condensing_opts_update(void *dims, void *opts_);
//
void ocp_qp_full_condensing_opts_set(void *opts_, const char *field, void* value);
//
acados_size_t ocp_qp_full_condensing_memory_calculate_size(void *dims, void *opts_);
//
void *ocp_qp_full_condensing_memory_assign(void *dims, void *opts_, void *raw_memory);
//
acados_size_t ocp_qp_full_condensing_workspace_calculate_size(void *dims, void *opts_);
//
int ocp_qp_full_condensing(void *in, void *out, void *opts, void *mem, void *work);
//
int ocp_qp_full_expansion(void *in, void *out, void *opts, void *mem, void *work);
//
void ocp_qp_full_condensing_config_initialize_default(void *config_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_

100
third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h vendored Normal file
View File

@@ -0,0 +1,100 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_OCP_QP_OCP_QP_HPIPM_H_
#define ACADOS_OCP_QP_OCP_QP_HPIPM_H_
#ifdef __cplusplus
extern "C" {
#endif
// hpipm
#include "hpipm/include/hpipm_d_ocp_qp_ipm.h"
// acados
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/utils/types.h"
// struct of arguments to the solver
// TODO(roversch): why not make this a typedef of the underlying struct?
typedef struct ocp_qp_hpipm_opts_
{
struct d_ocp_qp_ipm_arg *hpipm_opts;
} ocp_qp_hpipm_opts;
// TODO(roversch): why not make this a typedef of the underlying struct?
// struct of the solver memory
typedef struct ocp_qp_hpipm_memory_
{
struct d_ocp_qp_ipm_ws *hpipm_workspace;
double time_qp_solver_call;
int iter;
int status;
} ocp_qp_hpipm_memory;
//
acados_size_t ocp_qp_hpipm_opts_calculate_size(void *config, void *dims);
//
void *ocp_qp_hpipm_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_qp_hpipm_opts_initialize_default(void *config, void *dims, void *opts_);
//
void ocp_qp_hpipm_opts_update(void *config, void *dims, void *opts_);
//
void ocp_qp_hpipm_opts_set(void *config_, void *opts_, const char *field, void *value);
//
acados_size_t ocp_qp_hpipm_memory_calculate_size(void *config, void *dims, void *opts_);
//
void *ocp_qp_hpipm_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
//
acados_size_t ocp_qp_hpipm_workspace_calculate_size(void *config, void *dims, void *opts_);
//
int ocp_qp_hpipm(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void ocp_qp_hpipm_memory_reset(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_);
//
void ocp_qp_hpipm_eval_sens(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void ocp_qp_hpipm_config_initialize_default(void *config);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_QP_OCP_QP_HPIPM_H_

127
third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h 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.;
*/
#ifndef ACADOS_OCP_QP_OCP_QP_HPMPC_H_
#define ACADOS_OCP_QP_OCP_QP_HPMPC_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/utils/types.h"
typedef enum hpmpc_options_t_ { HPMPC_DEFAULT_ARGUMENTS } hpmpc_options_t;
typedef struct ocp_qp_hpmpc_opts_
{
double tol;
int max_iter;
double mu0;
double alpha_min;
int warm_start;
int N2; // horizion length of the partially condensed problem
// partial tightening
double sigma_mu;
int N;
int M;
} ocp_qp_hpmpc_opts;
// struct of the solver memory
typedef struct ocp_qp_hpmpc_memory_
{
struct blasfeo_dvec *hpi;
double *stats;
// workspace
void *hpmpc_work; // raw workspace
// partial tightening-specific (init of extra variables)
struct blasfeo_dvec *lam0;
struct blasfeo_dvec *ux0;
struct blasfeo_dvec *pi0;
struct blasfeo_dvec *t0;
// 2. workspace
struct blasfeo_dmat *hsL;
struct blasfeo_dmat *hsric_work_mat;
struct blasfeo_dmat sLxM;
struct blasfeo_dmat sPpM;
struct blasfeo_dvec *hsQx;
struct blasfeo_dvec *hsqx;
struct blasfeo_dvec *hstinv;
struct blasfeo_dvec *hsrq;
struct blasfeo_dvec *hsdux;
struct blasfeo_dvec *hsdlam;
struct blasfeo_dvec *hsdt;
struct blasfeo_dvec *hsdpi;
struct blasfeo_dvec *hslamt;
struct blasfeo_dvec *hsPb;
void *work_ric;
int out_iter;
double time_qp_solver_call;
int iter;
int status;
} ocp_qp_hpmpc_memory;
acados_size_t ocp_qp_hpmpc_opts_calculate_size(void *config_, ocp_qp_dims *dims);
//
void *ocp_qp_hpmpc_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory);
//
void ocp_qp_hpmpc_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_);
//
void ocp_qp_hpmpc_opts_update(void *config_, ocp_qp_dims *dims, void *opts_);
//
acados_size_t ocp_qp_hpmpc_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
//
void *ocp_qp_hpmpc_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory);
//
acados_size_t ocp_qp_hpmpc_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
//
int ocp_qp_hpmpc(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void ocp_qp_hpmpc_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void ocp_qp_hpmpc_config_initialize_default(void *config_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_QP_OCP_QP_HPMPC_H_

144
third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h vendored Normal file
View File

@@ -0,0 +1,144 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_OCP_QP_OCP_QP_OOQP_H_
#define ACADOS_OCP_QP_OCP_QP_OOQP_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/utils/types.h"
enum ocp_qp_ooqp_termination_code
{
SPARSE_SUCCESSFUL_TERMINATION = 0,
SPARSE_NOT_FINISHED,
SPARSE_MAX_ITS_EXCEEDED,
SPARSE_INFEASIBLE,
SPARSE_UNKNOWN
};
typedef struct ocp_qp_ooqp_opts_
{
int printLevel;
int useDiagonalWeights; // TODO(dimitris): implement option
int fixHessian;
int fixHessianSparsity;
int fixDynamics;
int fixDynamicsSparsity;
int fixInequalities;
int fixInequalitiesSparsity;
} ocp_qp_ooqp_opts;
typedef struct ocp_qp_ooqp_workspace_
{
double *x;
double *gamma;
double *phi;
double *y;
double *z;
double *lambda;
double *pi;
double objectiveValue;
int *tmpInt; // temporary vector to sort indicies sparse matrices
double *tmpReal; // temporary vector to sort data of sparse matrices
// int ierr;
} ocp_qp_ooqp_workspace;
typedef struct ocp_qp_ooqp_memory_
{
int firstRun;
double *c;
int nx;
int *irowQ;
int nnzQ;
int *jcolQ;
int *orderQ;
double *dQ;
double *xlow;
char *ixlow;
double *xupp;
char *ixupp;
int *irowA;
int nnzA;
int *jcolA;
int *orderA;
double *dA;
double *bA;
int my;
int *irowC;
int nnzC;
int *jcolC;
int *orderC;
double *dC;
double *clow;
int mz;
char *iclow;
double *cupp;
char *icupp;
int nnz; // max(nnzQ, nnzA, nnzC)
double time_qp_solver_call;
int iter;
int status;
} ocp_qp_ooqp_memory;
//
acados_size_t ocp_qp_ooqp_opts_calculate_size(void *config_, ocp_qp_dims *dims);
//
void *ocp_qp_ooqp_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory);
//
void ocp_qp_ooqp_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_);
//
void ocp_qp_ooqp_opts_update(void *config_, ocp_qp_dims *dims, void *opts_);
//
acados_size_t ocp_qp_ooqp_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
//
void *ocp_qp_ooqp_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory);
//
acados_size_t ocp_qp_ooqp_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
//
int ocp_qp_ooqp(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *memory_,
void *work_);
//
void ocp_qp_ooqp_destroy(void *mem_, void *work);
//
void ocp_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void ocp_qp_ooqp_config_initialize_default(void *config_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_QP_OCP_QP_OOQP_H_

103
third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h 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.;
*/
#ifndef ACADOS_OCP_QP_OCP_QP_OSQP_H_
#define ACADOS_OCP_QP_OCP_QP_OSQP_H_
#ifdef __cplusplus
extern "C" {
#endif
// osqp
#include "osqp/include/types.h"
// acados
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/utils/types.h"
typedef struct ocp_qp_osqp_opts_
{
OSQPSettings *osqp_opts;
} ocp_qp_osqp_opts;
typedef struct ocp_qp_osqp_memory_
{
c_int first_run;
c_float *q;
c_float *l;
c_float *u;
c_int P_nnzmax;
c_int *P_i;
c_int *P_p;
c_float *P_x;
c_int A_nnzmax;
c_int *A_i;
c_int *A_p;
c_float *A_x;
OSQPData *osqp_data;
OSQPWorkspace *osqp_work;
double time_qp_solver_call;
int iter;
int status;
} ocp_qp_osqp_memory;
acados_size_t ocp_qp_osqp_opts_calculate_size(void *config, void *dims);
//
void *ocp_qp_osqp_opts_assign(void *config, void *dims, void *raw_memory);
//
void ocp_qp_osqp_opts_initialize_default(void *config, void *dims, void *opts_);
//
void ocp_qp_osqp_opts_update(void *config, void *dims, void *opts_);
//
acados_size_t ocp_qp_osqp_memory_calculate_size(void *config, void *dims, void *opts_);
//
void *ocp_qp_osqp_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
//
acados_size_t ocp_qp_osqp_workspace_calculate_size(void *config, void *dims, void *opts_);
//
int ocp_qp_osqp(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void ocp_qp_osqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void ocp_qp_osqp_config_initialize_default(void *config);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_QP_OCP_QP_OSQP_H_

120
third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h vendored Normal file
View File

@@ -0,0 +1,120 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_
#define ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_
#ifdef __cplusplus
extern "C" {
#endif
// hpipm
#include "hpipm/include/hpipm_d_ocp_qp_red.h"
// acados
#include "acados/ocp_qp/ocp_qp_common.h"
typedef struct
{
ocp_qp_dims *orig_dims;
ocp_qp_dims *red_dims; // dims of reduced qp
ocp_qp_dims *pcond_dims;
int *block_size;
int N2;
int N2_bkp;
} ocp_qp_partial_condensing_dims;
typedef struct ocp_qp_partial_condensing_opts_
{
struct d_part_cond_qp_arg *hpipm_pcond_opts;
struct d_ocp_qp_reduce_eq_dof_arg *hpipm_red_opts;
// ocp_qp_dims *pcond_dims; // TODO(all): move to dims
// int *block_size;
int N2;
int N2_bkp;
// int expand_dual_sol; // 0 primal sol only, 1 primal + dual sol
int ric_alg;
int mem_qp_in; // allocate qp_in in memory
} ocp_qp_partial_condensing_opts;
typedef struct ocp_qp_partial_condensing_memory_
{
struct d_part_cond_qp_ws *hpipm_pcond_work;
struct d_ocp_qp_reduce_eq_dof_ws *hpipm_red_work;
// in memory
ocp_qp_in *pcond_qp_in;
ocp_qp_out *pcond_qp_out;
ocp_qp_in *red_qp; // reduced qp
ocp_qp_out *red_sol; // reduced qp sol
// only pointer
ocp_qp_in *ptr_qp_in;
ocp_qp_in *ptr_pcond_qp_in;
qp_info *qp_out_info; // info in pcond_qp_in
double time_qp_xcond;
} ocp_qp_partial_condensing_memory;
//
acados_size_t ocp_qp_partial_condensing_opts_calculate_size(void *dims);
//
void *ocp_qp_partial_condensing_opts_assign(void *dims, void *raw_memory);
//
void ocp_qp_partial_condensing_opts_initialize_default(void *dims, void *opts_);
//
void ocp_qp_partial_condensing_opts_update(void *dims, void *opts_);
//
void ocp_qp_partial_condensing_opts_set(void *opts_, const char *field, void* value);
//
acados_size_t ocp_qp_partial_condensing_memory_calculate_size(void *dims, void *opts_);
//
void *ocp_qp_partial_condensing_memory_assign(void *dims, void *opts, void *raw_memory);
//
acados_size_t ocp_qp_partial_condensing_workspace_calculate_size(void *dims, void *opts_);
//
int ocp_qp_partial_condensing(void *in, void *out, void *opts, void *mem, void *work);
//
int ocp_qp_partial_expansion(void *in, void *out, void *opts, void *mem, void *work);
//
void ocp_qp_partial_condensing_config_initialize_default(void *config_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_

119
third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h vendored Normal file
View File

@@ -0,0 +1,119 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_OCP_QP_OCP_QP_QPDUNES_H_
#define ACADOS_OCP_QP_OCP_QP_QPDUNES_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "qpDUNES.h"
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/utils/types.h"
typedef enum qpdunes_options_t_ {
QPDUNES_DEFAULT_ARGUMENTS,
QPDUNES_LINEAR_MPC, // TODO(dimitris): partly implemented
QPDUNES_NONLINEAR_MPC, // TODO(dimitris): not implemented yet
QPDUNES_ACADO_SETTINGS
} qpdunes_options_t;
typedef enum { QPDUNES_WITH_QPOASES, QPDUNES_WITH_CLIPPING } qpdunes_stage_qp_solver_t;
typedef struct ocp_qp_qpdunes_opts_
{
qpOptions_t options;
qpdunes_stage_qp_solver_t stageQpSolver;
int warmstart; // warmstart = 0: all multipliers set to zero, warmstart = 1: use previous mult.
bool isLinearMPC;
} ocp_qp_qpdunes_opts;
typedef struct ocp_qp_qpdunes_memory_
{
int firstRun;
int nx;
int nu;
int nz;
int nDmax; // max(dims->ng)
qpData_t qpData;
double time_qp_solver_call;
int iter;
int status;
} ocp_qp_qpdunes_memory;
typedef struct ocp_qp_qpdunes_workspace_
{
double *H;
double *Q;
double *R;
double *S;
double *g;
double *ABt;
double *b;
double *Ct;
double *lc;
double *uc;
double *zLow;
double *zUpp;
} ocp_qp_qpdunes_workspace;
//
acados_size_t ocp_qp_qpdunes_opts_calculate_size(void *config_, ocp_qp_dims *dims);
//
void *ocp_qp_qpdunes_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory);
//
void ocp_qp_qpdunes_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_);
//
void ocp_qp_qpdunes_opts_update(void *config_, ocp_qp_dims *dims, void *opts_);
//
acados_size_t ocp_qp_qpdunes_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
//
void *ocp_qp_qpdunes_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory);
//
acados_size_t ocp_qp_qpdunes_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
//
int ocp_qp_qpdunes(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *memory_,
void *work_);
//
void ocp_qp_qpdunes_free_memory(void *mem_);
//
void ocp_qp_qpdunes_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
//
void ocp_qp_qpdunes_config_initialize_default(void *config_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_QP_OCP_QP_QPDUNES_H_

150
third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h vendored Normal file
View File

@@ -0,0 +1,150 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_
#define ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_
#ifdef __cplusplus
extern "C" {
#endif
// acados
#include "acados/ocp_qp/ocp_qp_common.h"
#include "acados/utils/types.h"
typedef struct
{
ocp_qp_dims *orig_dims;
void *xcond_dims;
} ocp_qp_xcond_solver_dims;
typedef struct ocp_qp_xcond_solver_opts_
{
void *xcond_opts;
void *qp_solver_opts;
} ocp_qp_xcond_solver_opts;
typedef struct ocp_qp_xcond_solver_memory_
{
void *xcond_memory;
void *solver_memory;
void *xcond_qp_in;
void *xcond_qp_out;
} ocp_qp_xcond_solver_memory;
typedef struct ocp_qp_xcond_solver_workspace_
{
void *xcond_work;
void *qp_solver_work;
} ocp_qp_xcond_solver_workspace;
typedef struct
{
acados_size_t (*dims_calculate_size)(void *config, int N);
ocp_qp_xcond_solver_dims *(*dims_assign)(void *config, int N, void *raw_memory);
void (*dims_set)(void *config_, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value);
void (*dims_get)(void *config_, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value);
acados_size_t (*opts_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims);
void *(*opts_assign)(void *config, ocp_qp_xcond_solver_dims *dims, void *raw_memory);
void (*opts_initialize_default)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
void (*opts_update)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
acados_size_t (*memory_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
void *(*memory_assign)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts, void *raw_memory);
void (*memory_get)(void *config_, void *mem_, const char *field, void* value);
void (*memory_reset)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work);
acados_size_t (*workspace_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
int (*evaluate)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work);
void (*eval_sens)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_out *sens_qp_out, void *opts, void *mem, void *work);
qp_solver_config *qp_solver; // either ocp_qp_solver or dense_solver
ocp_qp_xcond_config *xcond;
} ocp_qp_xcond_solver_config; // pcond - partial condensing or fcond - full condensing
/* config */
//
acados_size_t ocp_qp_xcond_solver_config_calculate_size();
//
ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_assign(void *raw_memory);
/* dims */
//
acados_size_t ocp_qp_xcond_solver_dims_calculate_size(void *config, int N);
//
ocp_qp_xcond_solver_dims *ocp_qp_xcond_solver_dims_assign(void *config, int N, void *raw_memory);
//
void ocp_qp_xcond_solver_dims_set_(void *config, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value);
/* opts */
//
acados_size_t ocp_qp_xcond_solver_opts_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims);
//
void *ocp_qp_xcond_solver_opts_assign(void *config, ocp_qp_xcond_solver_dims *dims, void *raw_memory);
//
void ocp_qp_xcond_solver_opts_initialize_default(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_);
//
void ocp_qp_xcond_solver_opts_update(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_);
//
void ocp_qp_xcond_solver_opts_set_(void *config_, void *opts_, const char *field, void* value);
/* memory */
//
acados_size_t ocp_qp_xcond_solver_memory_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_);
//
void *ocp_qp_xcond_solver_memory_assign(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_, void *raw_memory);
/* workspace */
//
acados_size_t ocp_qp_xcond_solver_workspace_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_);
/* config */
//
int ocp_qp_xcond_solver(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *mem_, void *work_);
//
void ocp_qp_xcond_solver_config_initialize_default(void *config_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_

97
third_party/acados/include/acados/sim/sim_collocation_utils.h vendored Normal file
View File

@@ -0,0 +1,97 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_SIM_COLLOCATION_UTILS_H_
#define ACADOS_SIM_SIM_COLLOCATION_UTILS_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "acados/utils/types.h"
// enum Newton_type_collocation
// {
// exact = 0,
// simplified_in,
// simplified_inis
// };
// typedef struct
// {
// enum Newton_type_collocation type;
// double *eig;
// double *low_tria;
// bool single;
// bool freeze;
// double *transf1;
// double *transf2;
// double *transf1_T;
// double *transf2_T;
// } Newton_scheme;
typedef enum
{
GAUSS_LEGENDRE,
GAUSS_RADAU_IIA,
} sim_collocation_type;
//
// acados_size_t gauss_legendre_nodes_work_calculate_size(int ns);
//
// void gauss_legendre_nodes(int ns, double *nodes, void *raw_memory);
//
// acados_size_t gauss_simplified_work_calculate_size(int ns);
// //
// void gauss_simplified(int ns, Newton_scheme *scheme, void *work);
//
acados_size_t butcher_tableau_work_calculate_size(int ns);
//
// void calculate_butcher_tableau_from_nodes(int ns, double *nodes, double *b, double *A, void *work);
//
void calculate_butcher_tableau(int ns, sim_collocation_type collocation_type, double *c_vec,
double *b_vec, double *A_mat, void *work);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_SIM_SIM_COLLOCATION_UTILS_H_

221
third_party/acados/include/acados/sim/sim_common.h vendored Normal file
View File

@@ -0,0 +1,221 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_SIM_COMMON_H_
#define ACADOS_SIM_SIM_COMMON_H_
#include <stdbool.h>
#include "acados/sim/sim_collocation_utils.h"
#include "acados/utils/timing.h"
#include "acados/utils/types.h"
#include "acados/utils/external_function_generic.h"
// maximum number of integration stages
#define NS_MAX 15
typedef enum
{
// ERK and LIFTED_ERK
EXPL_ODE_FUN,
EXPL_ODE_HES, // wrt x and u ???
EXPL_VDE_FOR,
EXPL_VDE_ADJ,
// IRK
IMPL_ODE_FUN,
IMPL_ODE_FUN_JAC_X_XDOT,
IMPL_ODE_JAC_X_XDOT_U,
IMPL_ODE_FUN_JAC_X_XDOT_U,
IMPL_ODE_HESS,
// gnsf
PHI_FUN,
PHI_FUN_JAC_Y,
PHI_JAC_Y_UHAT,
LO_FUN,
GET_GNSF_MATRICES
} sim_function_t;
typedef struct
{
void *dims;
double *x; // x[NX] - initial state value for simulation
double *u; // u[NU] - control - constant over simulation time
double *S_forw; // forward seed [Sx, Su]
double *S_adj; // backward seed
bool identity_seed; // indicating if S_forw = [eye(nx), zeros(nx x nu)]
void *model;
double T; // simulation time
} sim_in;
typedef struct
{
double CPUtime; // in seconds
double LAtime; // in seconds
double ADtime; // in seconds
} sim_info;
typedef struct
{
double *xn; // xn[NX]
double *S_forw; // S_forw[NX*(NX+NU)]
double *S_adj; //
double *S_hess; //
double *zn; // z - algebraic variables - reported at start of simulation interval
double *S_algebraic; // sensitivities of reported value of algebraic variables w.r.t.
// initial stat & control (x_n,u)
double *grad; // gradient correction
sim_info *info;
} sim_out;
typedef struct
{
int ns; // number of integration stages
int num_steps;
int num_forw_sens;
int tableau_size; // check that is consistent with ns
// only update when butcher tableau is changed
// kind of private -> no setter!
double *A_mat;
double *c_vec;
double *b_vec;
bool sens_forw;
bool sens_adj;
bool sens_hess;
bool output_z; // 1 -- if zn should be computed
bool sens_algebraic; // 1 -- if S_algebraic should be computed
bool exact_z_output; // 1 -- if z, S_algebraic should be computed exactly, extra Newton iterations
sim_collocation_type collocation_type;
// for explicit integrators: newton_iter == 0 && scheme == NULL
// && jac_reuse=false
int newton_iter;
bool jac_reuse;
// Newton_scheme *scheme;
double newton_tol; // optinally used in implicit integrators
// workspace
void *work;
} sim_opts;
typedef struct
{
int (*evaluate)(void *config_, sim_in *in, sim_out *out, void *opts, void *mem, void *work);
int (*precompute)(void *config_, sim_in *in, sim_out *out, void *opts, void *mem, void *work);
// opts
acados_size_t (*opts_calculate_size)(void *config_, void *dims);
void *(*opts_assign)(void *config_, void *dims, void *raw_memory);
void (*opts_initialize_default)(void *config_, void *dims, void *opts);
void (*opts_update)(void *config_, void *dims, void *opts);
void (*opts_set)(void *config_, void *opts_, const char *field, void *value);
void (*opts_get)(void *config_, void *opts_, const char *field, void *value);
// mem
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
int (*memory_set)(void *config, void *dims, void *mem, const char *field, void *value);
int (*memory_set_to_zero)(void *config, void *dims, void *opts, void *mem, const char *field);
void (*memory_get)(void *config, void *dims, void *mem, const char *field, void *value);
// work
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
// model
acados_size_t (*model_calculate_size)(void *config, void *dims);
void *(*model_assign)(void *config, void *dims, void *raw_memory);
int (*model_set)(void *model, const char *field, void *value);
// config
void (*config_initialize_default)(void *config);
// dims
acados_size_t (*dims_calculate_size)();
void *(*dims_assign)(void *config, void *raw_memory);
void (*dims_set)(void *config, void *dims, const char *field, const int *value);
void (*dims_get)(void *config, void *dims, const char *field, int *value);
} sim_config;
/* config */
//
acados_size_t sim_config_calculate_size();
//
sim_config *sim_config_assign(void *raw_memory);
/* in */
//
acados_size_t sim_in_calculate_size(void *config, void *dims);
//
sim_in *sim_in_assign(void *config, void *dims, void *raw_memory);
//
int sim_in_set_(void *config_, void *dims_, sim_in *in, const char *field, void *value);
/* out */
//
acados_size_t sim_out_calculate_size(void *config, void *dims);
//
sim_out *sim_out_assign(void *config, void *dims, void *raw_memory);
//
int sim_out_get_(void *config, void *dims, sim_out *out, const char *field, void *value);
/* opts */
//
void sim_opts_set_(sim_opts *opts, const char *field, void *value);
//
void sim_opts_get_(sim_config *config, sim_opts *opts, const char *field, void *value);
#endif // ACADOS_SIM_SIM_COMMON_H_

140
third_party/acados/include/acados/sim/sim_erk_integrator.h vendored Normal file
View File

@@ -0,0 +1,140 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_SIM_ERK_INTEGRATOR_H_
#define ACADOS_SIM_SIM_ERK_INTEGRATOR_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "acados/sim/sim_common.h"
#include "acados/utils/types.h"
typedef struct
{
int nx;
int nu;
int nz;
} sim_erk_dims;
typedef struct
{
/* external functions */
// explicit ode
external_function_generic *expl_ode_fun;
// hessian explicit ode
external_function_generic *expl_ode_hes;
// forward explicit vde
external_function_generic *expl_vde_for;
// adjoint explicit vde
external_function_generic *expl_vde_adj;
} erk_model;
typedef struct
{
// memory
double time_sim;
double time_ad;
double time_la;
// workspace structs
} sim_erk_memory;
typedef struct
{
// workspace mem
double *rhs_forw_in; // x + S + p
double *K_traj; // (stages*nX) or (steps*stages*nX) for adj
double *out_forw_traj; // S or (steps+1)*nX for adj
double *rhs_adj_in;
double *out_adj_tmp;
double *adj_traj;
} sim_erk_workspace;
// dims
acados_size_t sim_erk_dims_calculate_size();
void *sim_erk_dims_assign(void *config_, void *raw_memory);
void sim_erk_dims_set(void *config_, void *dims_, const char *field, const int* value);
void sim_erk_dims_get(void *config_, void *dims_, const char *field, int* value);
// model
acados_size_t sim_erk_model_calculate_size(void *config, void *dims);
void *sim_erk_model_assign(void *config, void *dims, void *raw_memory);
int sim_erk_model_set(void *model, const char *field, void *value);
// opts
acados_size_t sim_erk_opts_calculate_size(void *config, void *dims);
//
void sim_erk_opts_update(void *config_, void *dims, void *opts_);
//
void *sim_erk_opts_assign(void *config, void *dims, void *raw_memory);
//
void sim_erk_opts_initialize_default(void *config, void *dims, void *opts_);
//
void sim_erk_opts_set(void *config_, void *opts_, const char *field, void *value);
// memory
acados_size_t sim_erk_memory_calculate_size(void *config, void *dims, void *opts_);
//
void *sim_erk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
//
int sim_erk_memory_set(void *config_, void *dims_, void *mem_, const char *field, void *value);
// workspace
acados_size_t sim_erk_workspace_calculate_size(void *config, void *dims, void *opts_);
//
int sim_erk(void *config, sim_in *in, sim_out *out, void *opts_, void *mem_, void *work_);
//
void sim_erk_config_initialize_default(void *config);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_SIM_SIM_ERK_INTEGRATOR_H_

364
third_party/acados/include/acados/sim/sim_gnsf.h vendored Normal file
View File

@@ -0,0 +1,364 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_SIM_GNSF_H_
#define ACADOS_SIM_SIM_GNSF_H_
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include "acados/utils/timing.h"
#include "acados/utils/types.h"
#include "acados/sim/sim_common.h"
#include "blasfeo/include/blasfeo_common.h"
// #include "blasfeo/include/blasfeo_d_aux.h"
// #include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
// #include "blasfeo/include/blasfeo_d_blas.h"
// #include "blasfeo/include/blasfeo_d_kernel.h"
// #include "blasfeo/include/blasfeo_i_aux_ext_dep.h"
// #include "blasfeo/include/blasfeo_target.h"
/*
GNSF - Generalized Nonlinear Static Feedback Model
has the following form
https://github.com/acados/acados/files/3359595/gnsf_structure_blo.pdf
Details on the algorithm can be found in master thesis of Jonathan Frey,
which presents a slightly different format without the terms B_LO, c_LO.
https://github.com/acados/acados/files/2318322/gnsf_structure.pdf
https://cdn.syscop.de/publications/Frey2018.pdf
https://cdn.syscop.de/publications/Frey2019.pdf
*/
typedef struct
{
int nx; // total number of differential states
int nu; // total number of inputs
int nz; // total number of algebraic states
int nx1; // number of differential states in NSF part
int nz1; // number of algebraic states in NSF part
int n_out; // output dimension of phi
int ny; // dimension of first input of phi
int nuhat; // dimension of second input of phi
} sim_gnsf_dims;
typedef struct
{
/* external functions */
// phi: nonlinearity function
external_function_generic *phi_fun;
external_function_generic *phi_fun_jac_y;
external_function_generic *phi_jac_y_uhat;
// f_lo: linear output function
external_function_generic *f_lo_fun_jac_x1_x1dot_u_z;
// to import model matrices
external_function_generic *get_gnsf_matrices;
// flag indicating, if model defining matrices are imported via external (casadi) function,
// [default]: true -> auto;
bool auto_import_gnsf;
// booleans from structure detection
bool nontrivial_f_LO; // indicates if f_LO is constant zero function
bool fully_linear; // indicates if model is fully linear LOS
/* model defining matrices */
// TODO: add setters to set manually
double *A;
double *B;
double *C;
double *E;
double *L_x;
double *L_xdot;
double *L_z;
double *L_u;
double *A_LO;
double *B_LO;
double *E_LO;
/* constant vector */
double *c;
double *c_LO;
// permutation vector - to have GNSF order of x, z within sim_gnsf only
int *ipiv_x;
int *ipiv_z;
double *ipiv_x_double;
double *ipiv_z_double;
} gnsf_model;
// pre_workspace - workspace used in the precomputation phase
typedef struct
{
struct blasfeo_dmat E11;
struct blasfeo_dmat E12;
struct blasfeo_dmat E21;
struct blasfeo_dmat E22;
struct blasfeo_dmat A1;
struct blasfeo_dmat A2;
struct blasfeo_dmat B1;
struct blasfeo_dmat B2;
struct blasfeo_dmat C1;
struct blasfeo_dmat C2;
struct blasfeo_dmat AA1;
struct blasfeo_dmat AA2;
struct blasfeo_dmat BB1;
struct blasfeo_dmat BB2;
struct blasfeo_dmat CC1;
struct blasfeo_dmat CC2;
struct blasfeo_dmat DD1;
struct blasfeo_dmat DD2;
struct blasfeo_dmat EE1;
struct blasfeo_dmat EE2;
struct blasfeo_dmat QQ1;
struct blasfeo_dmat LLZ;
struct blasfeo_dmat LLx;
struct blasfeo_dmat LLK;
int *ipivEE1; // index of pivot vector
int *ipivEE2;
int *ipivQQ1;
// for algebraic sensitivity propagation
struct blasfeo_dmat Q1;
// for constant term in NSF
struct blasfeo_dvec cc1;
struct blasfeo_dvec cc2;
} gnsf_pre_workspace;
// workspace
typedef struct
{
double *Z_work; // used to perform computations to get out->zn
int *ipiv; // index of pivot vector
struct blasfeo_dvec *vv_traj;
struct blasfeo_dvec *yy_traj;
struct blasfeo_dmat *f_LO_jac_traj;
struct blasfeo_dvec K2_val;
struct blasfeo_dvec x0_traj;
struct blasfeo_dvec res_val;
struct blasfeo_dvec u0;
struct blasfeo_dvec lambda;
struct blasfeo_dvec lambda_old;
struct blasfeo_dvec yyu;
struct blasfeo_dvec yyss;
struct blasfeo_dvec K1_val;
struct blasfeo_dvec f_LO_val;
struct blasfeo_dvec x1_stage_val;
struct blasfeo_dvec Z1_val;
struct blasfeo_dvec K1u;
struct blasfeo_dvec Zu;
struct blasfeo_dvec ALOtimesx02;
struct blasfeo_dvec BLOtimesu0;
struct blasfeo_dvec uhat;
struct blasfeo_dmat J_r_vv;
struct blasfeo_dmat J_r_x1u;
struct blasfeo_dmat dK1_dx1;
struct blasfeo_dmat dK1_du;
struct blasfeo_dmat dZ_dx1;
struct blasfeo_dmat dZ_du;
struct blasfeo_dmat J_G2_K1;
struct blasfeo_dmat dK2_dx1;
struct blasfeo_dmat dK2_dvv;
struct blasfeo_dmat dxf_dwn;
struct blasfeo_dmat S_forw_new;
struct blasfeo_dmat S_algebraic_aux;
struct blasfeo_dmat dPsi_dvv;
struct blasfeo_dmat dPsi_dx;
struct blasfeo_dmat dPsi_du;
struct blasfeo_dmat dPHI_dyuhat;
struct blasfeo_dvec z0;
// memory only available if (opts->sens_algebraic)
// struct blasfeo_dvec y_one_stage;
// struct blasfeo_dvec x0dot_1;
// struct blasfeo_dmat dz10_dx1u; // (nz1) * (nx1+nu);
// struct blasfeo_dmat dr0_dvv0; // (n_out * n_out)
// struct blasfeo_dmat f_LO_jac0; // (nx2+nz2) * (2*nx1 + nz1 + nu)
// struct blasfeo_dmat sens_z2_rhs; // (nx2 + nz2) * (nx1 + nu)
// int *ipiv_vv0;
} gnsf_workspace;
// memory
typedef struct
{
bool first_call;
// simulation time for one step
double dt;
// (scaled) butcher table
double *A_dt;
double *b_dt;
double *c_butcher;
// value used to initialize integration variables - corresponding to value of phi
double *phi_guess; // n_out
struct blasfeo_dmat S_forw;
struct blasfeo_dmat S_algebraic;
// precomputed matrices
struct blasfeo_dmat KKv;
struct blasfeo_dmat KKx;
struct blasfeo_dmat KKu;
struct blasfeo_dmat YYv;
struct blasfeo_dmat YYx;
struct blasfeo_dmat YYu;
struct blasfeo_dmat ZZv;
struct blasfeo_dmat ZZx;
struct blasfeo_dmat ZZu;
struct blasfeo_dmat ALO;
struct blasfeo_dmat BLO;
struct blasfeo_dmat M2_LU;
int *ipivM2;
struct blasfeo_dmat dK2_dx2;
struct blasfeo_dmat dK2_du;
struct blasfeo_dmat dx2f_dx2u;
struct blasfeo_dmat Lu;
// precomputed vectors for constant term in NSF
struct blasfeo_dvec KK0;
struct blasfeo_dvec YY0;
struct blasfeo_dvec ZZ0;
// for algebraic sensitivities only;
// struct blasfeo_dmat *Z0x;
// struct blasfeo_dmat *Z0u;
// struct blasfeo_dmat *Z0v;
// struct blasfeo_dmat *Y0x;
// struct blasfeo_dmat *Y0u;
// struct blasfeo_dmat *Y0v;
// struct blasfeo_dmat *K0x;
// struct blasfeo_dmat *K0u;
// struct blasfeo_dmat *K0v;
// struct blasfeo_dmat *ELO_LU;
// int *ipiv_ELO;
// struct blasfeo_dmat *ELO_inv_ALO;
// struct blasfeo_dmat *Lx;
// struct blasfeo_dmat *Lxdot;
// struct blasfeo_dmat *Lz;
double time_sim;
double time_ad;
double time_la;
} sim_gnsf_memory;
// gnsf dims
acados_size_t sim_gnsf_dims_calculate_size();
void *sim_gnsf_dims_assign(void *config_, void *raw_memory);
// get & set functions
void sim_gnsf_dims_set(void *config_, void *dims_, const char *field, const int *value);
void sim_gnsf_dims_get(void *config_, void *dims_, const char *field, int* value);
// opts
acados_size_t sim_gnsf_opts_calculate_size(void *config, void *dims);
void *sim_gnsf_opts_assign(void *config, void *dims, void *raw_memory);
void sim_gnsf_opts_initialize_default(void *config, void *dims, void *opts_);
void sim_gnsf_opts_update(void *config_, void *dims, void *opts_);
void sim_gnsf_opts_set(void *config_, void *opts_, const char *field, void *value);
// model
acados_size_t sim_gnsf_model_calculate_size(void *config, void *dims_);
void *sim_gnsf_model_assign(void *config, void *dims_, void *raw_memory);
int sim_gnsf_model_set(void *model_, const char *field, void *value);
// precomputation
int sim_gnsf_precompute(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_,
void *work_);
// workspace & memory
acados_size_t sim_gnsf_workspace_calculate_size(void *config, void *dims_, void *args);
acados_size_t sim_gnsf_memory_calculate_size(void *config, void *dims_, void *opts_);
void *sim_gnsf_memory_assign(void *config, void *dims_, void *opts_, void *raw_memory);
// interface
void sim_gnsf_config_initialize_default(void *config_);
// integrator
int sim_gnsf(void *config, sim_in *in, sim_out *out, void *opts, void *mem_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_SIM_SIM_GNSF_H_

183
third_party/acados/include/acados/sim/sim_irk_integrator.h vendored Normal file
View File

@@ -0,0 +1,183 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_SIM_IRK_INTEGRATOR_H_
#define ACADOS_SIM_SIM_IRK_INTEGRATOR_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "acados/sim/sim_common.h"
#include "acados/utils/types.h"
#include "blasfeo/include/blasfeo_common.h"
typedef struct
{
int nx;
int nu;
int nz;
} sim_irk_dims;
typedef struct
{
/* external functions */
// implicit fun - can either be fully implicit ode or dae
// - i.e. dae has z as additional last argument & nz > 0
external_function_generic *impl_ode_fun;
// implicit ode & jac_x & jax_xdot & jac_z
external_function_generic *impl_ode_fun_jac_x_xdot_z;
// jax_x & jac_xdot & jac_u & jac_z of implicit ode
external_function_generic *impl_ode_jac_x_xdot_u_z;
// hessian of implicit ode:
external_function_generic *impl_ode_hess;
} irk_model;
typedef struct
{
struct blasfeo_dvec *rG; // residuals of G (nx*ns)
struct blasfeo_dvec *K; // internal K variables ((nx+nz)*ns)
struct blasfeo_dvec *xt; // temporary x
struct blasfeo_dvec *xn; // x at each integration step
struct blasfeo_dvec xtdot; // temporary xdot
struct blasfeo_dvec *lambda; // adjoint sensitivities (nx + nu)
struct blasfeo_dvec *lambdaK; // auxiliary variable ((nx+nz)*ns) for adjoint propagation
struct blasfeo_dmat df_dx; // temporary Jacobian of ode w.r.t x (nx+nz, nx)
struct blasfeo_dmat df_dxdot; // temporary Jacobian of ode w.r.t xdot (nx+nz, nx)
struct blasfeo_dmat df_du; // temporary Jacobian of ode w.r.t u (nx+nz, nu)
struct blasfeo_dmat df_dz; // temporary Jacobian of ode w.r.t z (nx+nz, nu)
/* NOTE: the memory allocation corresponding to the following fields is CONDITIONAL */
// only allocated if (opts->sens_algebraic || opts->output_z)
int *ipiv_one_stage; // index of pivot vector (nx + nz)
double *Z_work; // used to perform computations to get out->zn (ns)
// df_dxdotz, dk0_dxu, only allocated if (opts->sens_algebraic && opts->exact_z_output)
// used for algebraic sensitivity generation
struct blasfeo_dmat df_dxdotz; // temporary Jacobian of ode w.r.t. xdot,z (nx+nz, nx+nz);
struct blasfeo_dmat dk0_dxu; // intermediate result, (nx+nz, nx+nu)
// dK_dxu: if (!opts->sens_hess) - single blasfeo_dmat that is reused
// if ( opts->sens_hess) - array of (num_steps) blasfeo_dmat
// to store intermediate results
struct blasfeo_dmat *dK_dxu; // jacobian of (K,Z) over x and u ((nx+nz)*ns, nx+nu);
// S_forw: if (!opts->sens_hess) - single blasfeo_dmat that is reused
// if ( opts->sens_hess) - array of (num_steps + 1) blasfeo_dmat
// to store intermediate results
struct blasfeo_dmat *S_forw; // forward sensitivities (nx, nx+nu)
// dG_dxu: if (!opts->sens_hess) - single blasfeo_dmat that is reused
// if ( opts->sens_hess) - array of blasfeo_dmat to store intermediate results
struct blasfeo_dmat *dG_dxu; // jacobian of G over x and u ((nx+nz)*ns, nx+nu)
// dG_dK: if (!opts->sens_hess) - single blasfeo_dmat that is reused
// if ( opts->sens_hess) - array of blasfeo_dmat to store intermediate results
struct blasfeo_dmat *dG_dK; // jacobian of G over K ((nx+nz)*ns, (nx+nz)*ns)
// ipiv: index of pivot vector
// if (!opts->sens_hess) - array (ns * (nx + nz)) that is reused
// if ( opts->sens_hess) - array (ns * (nx + nz)) * num_steps, to store all
// pivot vectors for dG_dxu
int *ipiv; // index of pivot vector
// xn_traj, K_traj only available if( opts->sens_adj || opts->sens_hess )
struct blasfeo_dvec *xn_traj; // xn trajectory
struct blasfeo_dvec *K_traj; // K trajectory
/* the following variables are only available if (opts->sens_hess) */
// For Hessian propagation
struct blasfeo_dmat Hess; // temporary Hessian (nx + nu, nx + nu)
// output of impl_ode_hess
struct blasfeo_dmat f_hess; // size: (nx + nu, nx + nu)
struct blasfeo_dmat dxkzu_dw0; // size (2*nx + nu + nz) x (nx + nu)
struct blasfeo_dmat tmp_dxkzu_dw0; // size (2*nx + nu + nz) x (nx + nu)
} sim_irk_workspace;
typedef struct
{
double *xdot; // xdot[NX] - initialization for state derivatives k within the integrator
double *z; // z[NZ] - initialization for algebraic variables z
double time_sim;
double time_ad;
double time_la;
} sim_irk_memory;
// get & set functions
void sim_irk_dims_set(void *config_, void *dims_, const char *field, const int *value);
void sim_irk_dims_get(void *config_, void *dims_, const char *field, int* value);
// dims
acados_size_t sim_irk_dims_calculate_size();
void *sim_irk_dims_assign(void *config_, void *raw_memory);
// model
acados_size_t sim_irk_model_calculate_size(void *config, void *dims);
void *sim_irk_model_assign(void *config, void *dims, void *raw_memory);
int sim_irk_model_set(void *model, const char *field, void *value);
// opts
acados_size_t sim_irk_opts_calculate_size(void *config, void *dims);
void *sim_irk_opts_assign(void *config, void *dims, void *raw_memory);
void sim_irk_opts_initialize_default(void *config, void *dims, void *opts_);
void sim_irk_opts_update(void *config_, void *dims, void *opts_);
void sim_irk_opts_set(void *config_, void *opts_, const char *field, void *value);
// memory
acados_size_t sim_irk_memory_calculate_size(void *config, void *dims, void *opts_);
void *sim_irk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
int sim_irk_memory_set(void *config_, void *dims_, void *mem_, const char *field, void *value);
// workspace
acados_size_t sim_irk_workspace_calculate_size(void *config, void *dims, void *opts_);
void sim_irk_config_initialize_default(void *config);
// main
int sim_irk(void *config, sim_in *in, sim_out *out, void *opts_, void *mem_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_SIM_SIM_IRK_INTEGRATOR_H_

157
third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h vendored Normal file
View File

@@ -0,0 +1,157 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_
#define ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "acados/sim/sim_common.h"
#include "acados/utils/types.h"
typedef struct
{
int nx;
int nu;
int nz;
} sim_lifted_irk_dims;
typedef struct
{
/* external functions */
// implicit ode
external_function_generic *impl_ode_fun;
// implicit ode & jax_x & jac_xdot & jac_u implicit ode
external_function_generic *impl_ode_fun_jac_x_xdot_u;
} lifted_irk_model;
typedef struct
{
struct blasfeo_dmat *J_temp_x; // temporary Jacobian of ode w.r.t x (nx, nx)
struct blasfeo_dmat *J_temp_xdot; // temporary Jacobian of ode w.r.t xdot (nx, nx)
struct blasfeo_dmat *J_temp_u; // temporary Jacobian of ode w.r.t u (nx, nu)
struct blasfeo_dvec *rG; // residuals of G (nx*ns)
struct blasfeo_dvec *xt; // temporary x
struct blasfeo_dvec *xn; // x at each integration step (for evaluations)
struct blasfeo_dvec *xn_out; // x at each integration step (output)
struct blasfeo_dvec *dxn; // dx at each integration step
struct blasfeo_dvec *w; // stacked x and u
int *ipiv; // index of pivot vector
} sim_lifted_irk_workspace;
typedef struct
{
// memory for lifted integrators
struct blasfeo_dmat *S_forw; // forward sensitivities
struct blasfeo_dmat *JGK; // jacobian of G over K (nx*ns, nx*ns)
struct blasfeo_dmat *JGf; // jacobian of G over x and u (nx*ns, nx+nu);
struct blasfeo_dmat *JKf; // jacobian of K over x and u (nx*ns, nx+nu);
struct blasfeo_dvec *K; // internal variables (nx*ns)
struct blasfeo_dvec *x; // states (nx) -- for expansion step
struct blasfeo_dvec *u; // controls (nu) -- for expansion step
int update_sens;
// int init_K;
double time_sim;
double time_ad;
double time_la;
} sim_lifted_irk_memory;
/* dims */
void sim_lifted_irk_dims_set(void *config_, void *dims_, const char *field, const int *value);
void sim_lifted_irk_dims_get(void *config_, void *dims_, const char *field, int* value);
acados_size_t sim_lifted_irk_dims_calculate_size();
//
void *sim_lifted_irk_dims_assign(void* config_, void *raw_memory);
/* model */
//
acados_size_t sim_lifted_irk_model_calculate_size(void *config, void *dims);
//
void *sim_lifted_irk_model_assign(void *config, void *dims, void *raw_memory);
//
int sim_lifted_irk_model_set(void *model_, const char *field, void *value);
/* opts */
//
acados_size_t sim_lifted_irk_opts_calculate_size(void *config, void *dims);
//
void *sim_lifted_irk_opts_assign(void *config, void *dims, void *raw_memory);
//
void sim_lifted_irk_opts_initialize_default(void *config, void *dims, void *opts_);
//
void sim_lifted_irk_opts_update(void *config_, void *dims, void *opts_);
//
void sim_lifted_irk_opts_set(void *config_, void *opts_, const char *field, void *value);
/* memory */
//
acados_size_t sim_lifted_irk_memory_calculate_size(void *config, void *dims, void *opts_);
//
void *sim_lifted_irk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
/* workspace */
//
acados_size_t sim_lifted_irk_workspace_calculate_size(void *config, void *dims, void *opts_);
//
void sim_lifted_irk_config_initialize_default(void *config);
/* solver */
//
int sim_lifted_irk(void *config, sim_in *in, sim_out *out, void *opts_,
void *mem_, void *work_);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_

242
third_party/acados/include/acados/utils/external_function_generic.h vendored Normal file
View File

@@ -0,0 +1,242 @@
/*
* Copyright (c) The acados authors.
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_
#define ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "acados/utils/types.h"
/************************************************
* generic external function
************************************************/
// type of arguments
typedef enum {
COLMAJ,
BLASFEO_DMAT,
BLASFEO_DVEC,
COLMAJ_ARGS,
BLASFEO_DMAT_ARGS,
BLASFEO_DVEC_ARGS,
IGNORE_ARGUMENT
} ext_fun_arg_t;
struct colmaj_args
{
double *A;
int lda;
};
struct blasfeo_dmat_args
{
struct blasfeo_dmat *A;
int ai;
int aj;
};
struct blasfeo_dvec_args
{
struct blasfeo_dvec *x;
int xi;
};
// prototype of an external function
typedef struct
{
// public members (have to be before private ones)
void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **);
// private members
// .....
} external_function_generic;
/************************************************
* generic external parametric function
************************************************/
// prototype of a parametric external function
typedef struct
{
// public members for core (have to be before private ones)
void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **);
// public members for interfaces
void (*get_nparam)(void *, int *);
void (*set_param)(void *, double *);
void (*set_param_sparse)(void *, int n_update, int *idx, double *);
// private members
void *ptr_ext_mem; // pointer to external memory
int (*fun)(void **, void **, void *);
double *p; // parameters
int np; // number of parameters
// .....
} external_function_param_generic;
//
acados_size_t external_function_param_generic_struct_size();
//
void external_function_param_generic_set_fun(external_function_param_generic *fun, void *value);
//
acados_size_t external_function_param_generic_calculate_size(external_function_param_generic *fun, int np);
//
void external_function_param_generic_assign(external_function_param_generic *fun, void *mem);
//
void external_function_param_generic_wrapper(void *self, ext_fun_arg_t *type_in, void **in, ext_fun_arg_t *type_out, void **out);
//
void external_function_param_generic_get_nparam(void *self, int *np);
//
void external_function_param_generic_set_param(void *self, double *p);
/************************************************
* casadi external function
************************************************/
typedef struct
{
// public members (have to be the same as in the prototype, and before the private ones)
void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **);
// private members
void *ptr_ext_mem; // pointer to external memory
int (*casadi_fun)(const double **, double **, int *, double *, void *);
int (*casadi_work)(int *, int *, int *, int *);
const int *(*casadi_sparsity_in)(int);
const int *(*casadi_sparsity_out)(int);
int (*casadi_n_in)(void);
int (*casadi_n_out)(void);
double **args;
double **res;
double *w;
int *iw;
int *args_size; // size of args[i]
int *res_size; // size of res[i]
int args_num; // number of args arrays
int args_size_tot; // total size of args arrays
int res_num; // number of res arrays
int res_size_tot; // total size of res arrays
int in_num; // number of input arrays
int out_num; // number of output arrays
int iw_size; // number of ints for worksapce
int w_size; // number of doubles for workspace
} external_function_casadi;
//
acados_size_t external_function_casadi_struct_size();
//
void external_function_casadi_set_fun(external_function_casadi *fun, void *value);
//
void external_function_casadi_set_work(external_function_casadi *fun, void *value);
//
void external_function_casadi_set_sparsity_in(external_function_casadi *fun, void *value);
//
void external_function_casadi_set_sparsity_out(external_function_casadi *fun, void *value);
//
void external_function_casadi_set_n_in(external_function_casadi *fun, void *value);
//
void external_function_casadi_set_n_out(external_function_casadi *fun, void *value);
//
acados_size_t external_function_casadi_calculate_size(external_function_casadi *fun);
//
void external_function_casadi_assign(external_function_casadi *fun, void *mem);
//
void external_function_casadi_wrapper(void *self, ext_fun_arg_t *type_in, void **in,
ext_fun_arg_t *type_out, void **out);
/************************************************
* casadi external parametric function
************************************************/
typedef struct
{
// public members for core (have to be the same as in the prototype, and before the private ones)
void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **);
// public members for interfaces
void (*get_nparam)(void *, int *);
void (*set_param)(void *, double *);
void (*set_param_sparse)(void *, int n_update, int *idx, double *);
// private members
void *ptr_ext_mem; // pointer to external memory
int (*casadi_fun)(const double **, double **, int *, double *, void *);
int (*casadi_work)(int *, int *, int *, int *);
const int *(*casadi_sparsity_in)(int);
const int *(*casadi_sparsity_out)(int);
int (*casadi_n_in)(void);
int (*casadi_n_out)(void);
double **args;
double **res;
double *w;
int *iw;
int *args_size; // size of args[i]
int *res_size; // size of res[i]
int args_num; // number of args arrays
int args_size_tot; // total size of args arrays
int res_num; // number of res arrays
int res_size_tot; // total size of res arrays
int in_num; // number of input arrays
int out_num; // number of output arrays
int iw_size; // number of ints for worksapce
int w_size; // number of doubles for workspace
int np; // number of parameters
} external_function_param_casadi;
//
acados_size_t external_function_param_casadi_struct_size();
//
void external_function_param_casadi_set_fun(external_function_param_casadi *fun, void *value);
//
void external_function_param_casadi_set_work(external_function_param_casadi *fun, void *value);
//
void external_function_param_casadi_set_sparsity_in(external_function_param_casadi *fun, void *value);
//
void external_function_param_casadi_set_sparsity_out(external_function_param_casadi *fun, void *value);
//
void external_function_param_casadi_set_n_in(external_function_param_casadi *fun, void *value);
//
void external_function_param_casadi_set_n_out(external_function_param_casadi *fun, void *value);
//
acados_size_t external_function_param_casadi_calculate_size(external_function_param_casadi *fun, int np);
//
void external_function_param_casadi_assign(external_function_param_casadi *fun, void *mem);
//
void external_function_param_casadi_wrapper(void *self, ext_fun_arg_t *type_in, void **in,
ext_fun_arg_t *type_out, void **out);
//
void external_function_param_casadi_get_nparam(void *self, int *np);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_

Some files were not shown because too many files have changed in this diff Show More