mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-08-21 08:03:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot v2023.07.05

Browse Source 
version: dragonpilot v0.9.4 release
date: 2023-07-05T18:59:41
dp-dev(priv) master commit: 7b0489feab40283a422d2201ef95a9cb8c06f6cd
This commit is contained in:
Dragonpilot Team
2023-07-05 18:45:50 -07:00
commit d0beb4d392
1549 changed files with 511297 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
third_party/.gitignore
+1
View File
@@ -0,0 +1 @@
*.pyc
third_party/acados/.gitignore
+5
View File
@@ -0,0 +1,5 @@
acados_repo/
lib
!x86_64/
!larch64/
!aarch64/
third_party/acados/acados_template/__init__.py
Vendored
+43
View File
@@ -0,0 +1,43 @@
#
# 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.;
#
from .acados_model import *
from .generate_c_code_explicit_ode import *
from .generate_c_code_implicit_ode import *
from .generate_c_code_constraint import *
from .generate_c_code_nls_cost import *
from .acados_ocp import *
from .acados_sim import *
from .acados_ocp_solver import *
from .acados_sim_solver import *
from .utils import *
third_party/acados/acados_template/__pycache__/__init__.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/acados_model.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/acados_ocp.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/acados_ocp_solver.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/acados_sim.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/acados_sim_solver.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/builders.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_constraint.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_discrete_dynamics.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_explicit_ode.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_external_cost.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_gnsf.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_implicit_ode.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_nls_cost.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/utils.cpython-38.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/acados_layout.json
Vendored
+804
View File
@@ -0,0 +1,804 @@
{
"code_export_directory": [
"str"
],
"acados_include_path": [
"str"
],
"cython_include_dirs": [
"str"
],
"model": {
"name" : [
"str"
],
"dyn_ext_fun_type" : [
"str"
],
"dyn_source_discrete" : [
"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_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"
],
"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"
],
"print_level": [
"int"
],
"initialize_t_slacks": [
"int"
],
"exact_hess_cost": [
"int"
],
"exact_hess_constr": [
"int"
],
"exact_hess_dyn": [
"int"
],
"ext_cost_num_hess": [
"int"
],
"model_external_shared_lib_dir": [
"str"
],
"model_external_shared_lib_name": [
"str"
]
}
}
third_party/acados/acados_template/acados_model.py
Vendored
+167
View File
@@ -0,0 +1,167 @@
#
# 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.;
#
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_source_discrete = 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
self.con_h_expr = None #: CasADi expression for the constraint :math:`h`; Default: :code:`None`
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`
def acados_model_strip_casadi_symbolics(model):
out = model
if 'f_impl_expr' in out.keys():
del out['f_impl_expr']
if 'f_expl_expr' in out.keys():
del out['f_expl_expr']
if 'disc_dyn_expr' in out.keys():
del out['disc_dyn_expr']
if 'x' in out.keys():
del out['x']
if 'xdot' in out.keys():
del out['xdot']
if 'u' in out.keys():
del out['u']
if 'z' in out.keys():
del out['z']
if 'p' in out.keys():
del out['p']
# constraints
if 'con_phi_expr' in out.keys():
del out['con_phi_expr']
if 'con_h_expr' in out.keys():
del out['con_h_expr']
if 'con_r_expr' in out.keys():
del out['con_r_expr']
if 'con_r_in_phi' in out.keys():
del out['con_r_in_phi']
# terminal
if 'con_phi_expr_e' in out.keys():
del out['con_phi_expr_e']
if 'con_h_expr_e' in out.keys():
del out['con_h_expr_e']
if 'con_r_expr_e' in out.keys():
del out['con_r_expr_e']
if 'con_r_in_phi_e' in out.keys():
del out['con_r_in_phi_e']
# cost
if 'cost_y_expr' in out.keys():
del out['cost_y_expr']
if 'cost_y_expr_e' in out.keys():
del out['cost_y_expr_e']
if 'cost_y_expr_0' in out.keys():
del out['cost_y_expr_0']
if 'cost_expr_ext_cost' in out.keys():
del out['cost_expr_ext_cost']
if 'cost_expr_ext_cost_e' in out.keys():
del out['cost_expr_ext_cost_e']
if 'cost_expr_ext_cost_0' in out.keys():
del out['cost_expr_ext_cost_0']
if 'cost_expr_ext_cost_custom_hess' in out.keys():
del out['cost_expr_ext_cost_custom_hess']
if 'cost_expr_ext_cost_custom_hess_e' in out.keys():
del out['cost_expr_ext_cost_custom_hess_e']
if 'cost_expr_ext_cost_custom_hess_0' in out.keys():
del out['cost_expr_ext_cost_custom_hess_0']
return out
third_party/acados/acados_template/acados_ocp.py
Vendored
+2955
View File
File diff suppressed because it is too large Load Diff
third_party/acados/acados_template/acados_ocp_solver.py
Vendored
+1818
View File
File diff suppressed because it is too large Load Diff
third_party/acados/acados_template/acados_ocp_solver_pyx.pyx
Vendored
+707
View File
@@ -0,0 +1,707 @@
# -*- 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.;
#
# 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 int status
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()
self.status = 0
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(self):
"""
Solve the ocp with current input.
"""
return acados_solver.acados_solve(self.capsule)
def reset(self):
"""
Sets current iterate to all zeros.
"""
return acados_solver.acados_reset(self.capsule)
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 {}. Exiting.'.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()
for i in range(self.N+1):
solution['x_'+str(i)] = self.get(i,'x')
solution['u_'+str(i)] = self.get(i,'u')
solution['z_'+str(i)] = self.get(i,'z')
solution['lam_'+str(i)] = self.get(i,'lam')
solution['t_'+str(i)] = self.get(i, 't')
solution['sl_'+str(i)] = self.get(i, 'sl')
solution['su_'+str(i)] = self.get(i, 'su')
for i in range(self.N):
solution['pi_'+str(i)] = self.get(i,'pi')
# 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
"""
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 {}. Exiting.".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)
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
"""
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
"""
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[0] != dims[0] or value_shape[1] != dims[1]:
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 dynamics_get(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_dynamics_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 __del__(self):
if self.solver_created:
acados_solver.acados_free(self.capsule)
acados_solver.acados_free_capsule(self.capsule)
third_party/acados/acados_template/acados_sim.py
Vendored
+331
View File
@@ -0,0 +1,331 @@
# -*- 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 numpy as np
import casadi as ca
import os
from .acados_model import AcadosModel
from .utils import get_acados_path
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. Exiting.')
@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. Exiting.')
@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. Exiting.')
@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. Exiting.')
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
# bools
self.__sens_forw = True
self.__sens_adj = False
self.__sens_algebraic = False
self.__sens_hess = False
self.__output_z = False
self.__sim_method_jac_reuse = 0
@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 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: False"""
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
@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\nExiting.')
@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\nExiting.')
@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.')
@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:`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.cython_include_dirs = ''
self.__parameter_values = np.array([])
@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
third_party/acados/acados_template/acados_sim_layout.json
Vendored
+47
View File
@@ -0,0 +1,47 @@
{
"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"
]
}
}
third_party/acados/acados_template/acados_sim_solver.py
Vendored
+454
View File
@@ -0,0 +1,454 @@
# -*- 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 sys, os, json
import numpy as np
from ctypes import *
from copy import deepcopy
from .generate_c_code_explicit_ode import generate_c_code_explicit_ode
from .generate_c_code_implicit_ode import generate_c_code_implicit_ode
from .generate_c_code_gnsf import generate_c_code_gnsf
from .acados_sim import AcadosSim
from .acados_ocp import AcadosOcp
from .acados_model import acados_model_strip_casadi_symbolics
from .utils import is_column, render_template, format_class_dict, np_array_to_list,\
make_model_consistent, set_up_imported_gnsf_model, get_python_interface_path
from .builders import CMakeBuilder
def make_sim_dims_consistent(acados_sim):
dims = acados_sim.dims
model = acados_sim.model
# nx
if is_column(model.x):
dims.nx = model.x.shape[0]
else:
raise Exception("model.x should be column vector!")
# nu
if is_column(model.u):
dims.nu = model.u.shape[0]
elif model.u == None or model.u == []:
dims.nu = 0
else:
raise Exception("model.u should be column vector or None!")
# nz
if is_column(model.z):
dims.nz = model.z.shape[0]
elif model.z == None or model.z == []:
dims.nz = 0
else:
raise Exception("model.z should be column vector or None!")
# np
if is_column(model.p):
dims.np = model.p.shape[0]
elif model.p == None or model.p == []:
dims.np = 0
else:
raise Exception("model.p should be column vector or None!")
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, 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_dict['model'] = acados_model_strip_casadi_symbolics(sim_dict['model'])
sim_json = format_class_dict(sim_dict)
with open(json_file, 'w') as f:
json.dump(sim_json, f, default=np_array_to_list, 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, 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!")
template_dir = code_export_dir
## Render templates
in_file = 'acados_sim_solver.in.c'
out_file = f'acados_sim_solver_{model_name}.c'
render_template(in_file, out_file, template_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, template_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, template_dir, json_path)
else:
in_file = 'Makefile.in'
out_file = 'Makefile'
render_template(in_file, out_file, template_dir, json_path)
in_file = 'main_sim.in.c'
out_file = f'main_sim_{model_name}.c'
render_template(in_file, out_file, template_dir, json_path)
## folder model
template_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, template_dir, json_path)
def sim_generate_casadi_functions(acados_sim):
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)
# generate external functions
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`
"""
def __init__(self, acados_sim_, json_file='acados_sim.json', build=True, cmake_builder: CMakeBuilder = None):
self.solver_created = False
if isinstance(acados_sim_, AcadosOcp):
# set up acados_sim_
acados_sim = AcadosSim()
acados_sim.model = acados_sim_.model
acados_sim.dims.nx = acados_sim_.dims.nx
acados_sim.dims.nu = acados_sim_.dims.nu
acados_sim.dims.nz = acados_sim_.dims.nz
acados_sim.dims.np = acados_sim_.dims.np
acados_sim.solver_options.integrator_type = acados_sim_.solver_options.integrator_type
acados_sim.code_export_directory = acados_sim_.code_export_directory
elif isinstance(acados_sim_, AcadosSim):
acados_sim = acados_sim_
acados_sim.__problem_class = 'SIM'
model_name = acados_sim.model.name
make_sim_dims_consistent(acados_sim)
# reuse existing json and casadi functions, when creating integrator from ocp
if isinstance(acados_sim_, AcadosSim):
if acados_sim.solver_options.integrator_type == 'GNSF':
set_up_imported_gnsf_model(acados_sim)
sim_generate_casadi_functions(acados_sim)
sim_formulation_json_dump(acados_sim, json_file)
code_export_dir = acados_sim.code_export_directory
if build:
# render templates
sim_render_templates(json_file, model_name, code_export_dir, cmake_builder)
# Compile solver
cwd = os.getcwd()
code_export_dir = os.path.abspath(code_export_dir)
os.chdir(code_export_dir)
if cmake_builder is not None:
cmake_builder.exec(code_export_dir)
else:
os.system('make sim_shared_lib')
os.chdir(cwd)
self.sim_struct = acados_sim
model_name = self.sim_struct.model.name
self.model_name = model_name
# 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 = 'libacados.so'
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.')
# Ctypes
lib_prefix = 'lib'
lib_ext = '.so'
if os.name == 'nt':
lib_prefix = ''
lib_ext = ''
self.shared_lib_name = os.path.join(code_export_dir, f'{lib_prefix}acados_sim_solver_{model_name}{lib_ext}')
print(f'self.shared_lib_name = "{self.shared_lib_name}"')
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)
nu = self.sim_struct.dims.nu
nx = self.sim_struct.dims.nx
nz = self.sim_struct.dims.nz
self.gettable = {
'x': nx,
'xn': nx,
'u': nu,
'z': nz,
'S_forw': nx*(nx+nu),
'Sx': nx*nx,
'Su': nx*nu,
'S_adj': nx+nu,
'S_hess': (nx+nu)*(nx+nu),
'S_algebraic': (nz)*(nx+nu),
}
self.settable = ['S_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw
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']
"""
field = field_
field = field.encode('utf-8')
if field_ in self.gettable.keys():
# allocate array
dims = self.gettable[field_]
out = np.ascontiguousarray(np.zeros((dims,)), 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)
if field_ == 'S_forw':
nu = self.sim_struct.dims.nu
nx = self.sim_struct.dims.nx
out = out.reshape(nx, nx+nu, order='F')
elif field_ == 'Sx':
nx = self.sim_struct.dims.nx
out = out.reshape(nx, nx, order='F')
elif field_ == 'Su':
nx = self.sim_struct.dims.nx
nu = self.sim_struct.dims.nu
out = out.reshape(nx, nu, order='F')
elif field_ == 'S_hess':
nx = self.sim_struct.dims.nx
nu = self.sim_struct.dims.nu
out = out.reshape(nx+nu, nx+nu, order='F')
elif field_ == 'S_algebraic':
nx = self.sim_struct.dims.nx
nu = self.sim_struct.dims.nu
nz = self.sim_struct.dims.nz
out = out.reshape(nz, nx+nu, order='F')
else:
raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \
f' available fields are {", ".join(self.gettable.keys())}')
return out
def set(self, field_, value_):
"""
Set numerical data inside the solver.
:param field: string in ['p', 'S_adj', 'T', 'x', 'u', 'xdot', 'z']
:param value: the value with appropriate size.
"""
# 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_
field = field.encode('utf-8')
# treat parameters separately
if field_ == 'p':
model_name = self.sim_struct.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('AcadosSimSolver.set(): mismatching dimension' \
' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), 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 self.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(self.settable)}')
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:
pass
third_party/acados/acados_template/acados_solver_common.pxd
Vendored
+103
View File
@@ -0,0 +1,103 @@
# -*- 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.;
#
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_dynamics_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)
third_party/acados/acados_template/builders.py
Vendored
+116
View File
@@ -0,0 +1,116 @@
# -*- 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 call
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):
"""
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)
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)
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)
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)
third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt
Vendored
+374
View File
@@ -0,0 +1,374 @@
#
# 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.;
#
{%- 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" %}
{%- else %}
{%- set link_libs = "" %}
{%- endif %}
{%- endif %}
cmake_minimum_required(VERSION 3.10)
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 %}
# 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 solver_options.integrator_type == "ERK" %}
{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c
{{ model.name }}_model/{{ model.name }}_expl_vde_forw.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" %}
{%- if model.dyn_ext_fun_type == "casadi" %}
{{ 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 %}
{%- else %}
{{ model.name }}_model/{{ model.dyn_source_discrete }}
{%- endif %}
{%- 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 == "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 == "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 == "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 }})
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 }} CACHE PATH "Define the path which contains the include directory for acados.")
set(ACADOS_LIB_PATH {{ acados_lib_path }} 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 "
{%- if qp_solver == "FULL_CONDENSING_QPOASES" -%}
-DACADOS_WITH_QPOASES
{%- endif -%}
{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" -%}
-DACADOS_WITH_OSQP
{%- endif -%}
{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" -%}
-DACADOS_WITH_QPDUNES
{%- endif -%}
-fPIC -std=c99 {{ openmp_flag }}")
#-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 %}
)
# 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 %}
third_party/acados/acados_template/c_templates_tera/CPPLINT.cfg
Vendored
+1
View File
@@ -0,0 +1 @@
exclude_files=[main, acados_solver, acados_solver_sfun, Makefile, model].*\.?
third_party/acados/acados_template/c_templates_tera/Makefile.in
Vendored
+406
View File
@@ -0,0 +1,406 @@
#
# 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.;
#
{%- 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" %}
{%- else %}
{%- set link_libs = "" %}
{%- endif %}
{%- endif %}
# define sources and use make's implicit rules to generate object files (*.o)
# model
MODEL_SRC=
{%- 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
{%- 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" %}
{%- if model.dyn_ext_fun_type == "casadi" %}
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 %}
{%- else %}
MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
{%- endif %}
{%- 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 == "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 == "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 == "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 %}
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 == "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 %}
{# c-compiler flags #}
# define the c-compiler flags for make's implicit rules
CFLAGS = -fPIC -std=c99 {{ openmp_flag }} #-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 }}.so
LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}.so
LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
# 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 /usr/include/python3.8 \
-I $(INCLUDE_PATH)/blasfeo/include/ \
-I $(INCLUDE_PATH)/hpipm/include/ \
-I $(INCLUDE_PATH) \
-I {{ cython_include_dirs }} \
acados_ocp_solver_pyx.c \
ocp_cython: ocp_cython_o
$(CC) $(ACADOS_FLAGS) -shared \
-o acados_ocp_solver_pyx.so \
-Wl,-rpath=$(LIB_PATH) \
acados_ocp_solver_pyx.o \
$(abspath .)/libacados_ocp_solver_{{ model.name }}.so \
$(LDFLAGS) $(LDLIBS)
{%- if os and os == "pc" %}
clean:
del \Q *.o 2>nul
del \Q *.so 2>nul
del \Q main_{{ model.name }} 2>nul
clean_ocp_shared_lib:
del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
del \Q acados_solver_{{ model.name }}.o 2>nul
clean_ocp_cython:
del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
del \Q acados_solver_{{ model.name }}.o 2>nul
del \Q acados_ocp_solver_pyx.so 2>nul
del \Q acados_ocp_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 }}.so
$(RM) acados_solver_{{ model.name }}.o
$(RM) acados_ocp_solver_pyx.so
$(RM) acados_ocp_solver_pyx.o
{%- endif %}
third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h
Vendored
+103
View File
@@ -0,0 +1,103 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_{{ 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_casadi * sim_forw_vde_casadi;
external_function_param_casadi * sim_expl_ode_fun_casadi;
external_function_param_casadi * sim_expl_ode_hess;
// IRK
external_function_param_casadi * sim_impl_dae_fun;
external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
external_function_param_casadi * sim_impl_dae_hess;
// GNSF
external_function_param_casadi * sim_gnsf_phi_fun;
external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
external_function_param_casadi * sim_gnsf_get_matrices_fun;
} sim_solver_capsule;
ACADOS_SYMBOL_EXPORT int {{ 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_
third_party/acados/acados_template/c_templates_tera/acados_solver.in.h
Vendored
+217
View File
@@ -0,0 +1,217 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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
// ** 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_casadi *impl_dae_fun;
external_function_param_casadi *impl_dae_fun_jac_x_xdot_z;
external_function_param_casadi *impl_dae_jac_x_xdot_u_z;
{% if solver_options.hessian_approx == "EXACT" %}
external_function_param_casadi *impl_dae_hess;
{%- endif %}
{% elif solver_options.integrator_type == "LIFTED_IRK" %}
external_function_param_casadi *impl_dae_fun;
external_function_param_casadi *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 == "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 == "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 == "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;
external_function_param_casadi *nl_constr_h_fun_jac_hess;
{%- 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;
external_function_param_casadi nl_constr_h_e_fun_jac_hess;
{%- 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);
/**
* 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_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 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_
third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd
Vendored
+62
View File
@@ -0,0 +1,62 @@
#
# 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.;
#
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_solve "{{ model.name }}_acados_solve"(nlp_solver_capsule * capsule)
int acados_reset "{{ model.name }}_acados_reset"(nlp_solver_capsule * capsule)
int acados_free "{{ model.name }}_acados_free"(nlp_solver_capsule * capsule)
void acados_print_stats "{{ model.name }}_acados_print_stats"(nlp_solver_capsule * capsule)
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)
third_party/acados/acados_template/c_templates_tera/cost_y_0_fun.in.h
Vendored
+69
View File
@@ -0,0 +1,69 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef {{ model.name }}_Y_0_COST
#define {{ model.name }}_Y_0_COST
#ifdef __cplusplus
extern "C" {
#endif
{% 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);
{% endif %}
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // {{ model.name }}_Y_0_COST
third_party/acados/acados_template/c_templates_tera/cost_y_e_fun.in.h
Vendored
+69
View File
@@ -0,0 +1,69 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef {{ model.name }}_Y_E_COST
#define {{ model.name }}_Y_E_COST
#ifdef __cplusplus
extern "C" {
#endif
{% 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);
{% endif %}
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // {{ model.name }}_Y_E_COST
third_party/acados/acados_template/c_templates_tera/cost_y_fun.in.h
Vendored
+69
View File
@@ -0,0 +1,69 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef {{ model.name }}_Y_COST
#define {{ model.name }}_Y_COST
#ifdef __cplusplus
extern "C" {
#endif
{% 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);
{% endif %}
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // {{ model.name }}_Y_COST
third_party/acados/acados_template/c_templates_tera/external_cost.in.h
Vendored
+74
View File
@@ -0,0 +1,74 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef {{ model.name }}_EXT_COST
#define {{ model.name }}_EXT_COST
#ifdef __cplusplus
extern "C" {
#endif
{% if cost.cost_ext_fun_type == "casadi" %}
{% if cost.cost_type == "EXTERNAL" %}
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);
{% endif %}
{% else %}
int {{ cost.cost_function_ext_cost }}(void **, void **, void *);
{% endif %}
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // {{ model.name }}_EXT_COST
third_party/acados/acados_template/c_templates_tera/external_cost_0.in.h
Vendored
+75
View File
@@ -0,0 +1,75 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef {{ model.name }}_EXT_COST_0
#define {{ model.name }}_EXT_COST_0
#ifdef __cplusplus
extern "C" {
#endif
{% if cost.cost_ext_fun_type_0 == "casadi" %}
{% if cost.cost_type_0 == "EXTERNAL" %}
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);
{% endif %}
{% else %}
int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *);
{% endif %}
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // {{ model.name }}_EXT_COST_0
third_party/acados/acados_template/c_templates_tera/external_cost_e.in.h
Vendored
+74
View File
@@ -0,0 +1,74 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef {{ model.name }}_EXT_COST_E
#define {{ model.name }}_EXT_COST_E
#ifdef __cplusplus
extern "C" {
#endif
{% if cost.cost_ext_fun_type_e == "casadi" %}
{% if cost.cost_type_e == "EXTERNAL" %}
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);
{% endif %}
{% else %}
int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *);
{% endif %}
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // {{ model.name }}_EXT_COST_E
third_party/acados/acados_template/c_templates_tera/h_constraint.in.h
Vendored
+70
View File
@@ -0,0 +1,70 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef {{ model.name }}_H_CONSTRAINT
#define {{ model.name }}_H_CONSTRAINT
#ifdef __cplusplus
extern "C" {
#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 %}
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // {{ model.name }}_H_CONSTRAINT
third_party/acados/acados_template/c_templates_tera/h_e_constraint.in.h
Vendored
+71
View File
@@ -0,0 +1,71 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef {{ model.name }}_H_E_CONSTRAINT
#define {{ model.name }}_H_E_CONSTRAINT
#ifdef __cplusplus
extern "C" {
#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 }}_H_E_CONSTRAINT
third_party/acados/acados_template/c_templates_tera/make_main_mex.in.m
Vendored
+105
View File
@@ -0,0 +1,105 @@
%
% 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.;
%
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
third_party/acados/acados_template/c_templates_tera/make_mex.in.m
Vendored
+110
View File
@@ -0,0 +1,110 @@
%
% 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.;
%
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')];
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,...
acados_lib_path, template_lib_path, mex_include, '-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, mex_include, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii})
end
end
end
third_party/acados/acados_template/c_templates_tera/make_sfun.in.m
Vendored
+344
View File
@@ -0,0 +1,344 @@
%
% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
%
% This file is part of acados.
%
% The 2-Clause BSD License
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.;
%
SOURCES = { ...
{%- 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("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 %}
{%- endif %}
mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
LIB_PATH, LIBS{:}, SOURCES{:}, ...
'-output', 'acados_solver_sfunction_{{ model.name }}' );
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_{{ model.name }}', '.', ...
eval('mexext')] );
%% print note on usage of s-function
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
input_note = 'Inputs are:\n';
i_in = 1;
{%- 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 ');
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 ');
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+1,',...
' size [{{ (dims.N+1)*dims.np }}]\n ');
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 ');
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 ');
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 ');
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 ');
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 ');
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 ');
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 ');
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 ');
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 ');
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, size [{{ dims.ng }}]\n ');
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, size [{{ dims.ng }}]\n ');
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, size [{{ dims.nh }}]\n ');
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, size [{{ dims.nh }}]\n ');
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 ');
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 ');
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 ');
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 ');
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 ');
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 ');
i_in = i_in + 1;
{%- endif %}
fprintf(input_note)
disp(' ')
output_note = 'Outputs are:\n';
i_out = 0;
{%- 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 ');
{%- 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 ');
{%- 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 ');
{%- 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 ');
{%- endif %}
{%- if simulink_opts.outputs.KKT_residual == 1 %}
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
{%- 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 ');
{%- endif %}
{%- if simulink_opts.outputs.CPU_time == 1 %}
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
{%- 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 ');
{%- 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 ');
{%- 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 ');
{%- endif %}
{%- if simulink_opts.outputs.sqp_iter == 1 %}
i_out = i_out + 1;
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
{%- endif %}
fprintf(output_note)
third_party/acados/acados_template/c_templates_tera/make_sfun_sim.in.m
Vendored
+103
View File
@@ -0,0 +1,103 @@
%
% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
%
% This file is part of acados.
%
% The 2-Clause BSD License
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.;
%
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 ',...
{%- 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';
eval( [ 'mex -v -output acados_sim_solver_sfunction_{{ model.name }} ', ...
CFLAGS, INCS, ' ', SOURCES, ' -L', LIB_PATH, ' ', LIBS ]);
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_sim_solver_sfunction_{{ model.name }}', '.', ...
eval('mexext')] );
%% 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;
{%- if dims.nu > 0 %}
input_note = strcat(input_note, num2str(i_in), ') u, size [{{ dims.nu }}]\n ');
i_in = i_in + 1;
{%- endif %}
{%- if dims.np > 0 %}
input_note = strcat(input_note, num2str(i_in), ') parameters, size [{{ dims.np }}]\n ');
i_in = i_in + 1;
{%- endif %}
fprintf(input_note)
disp(' ')
output_note = strcat('Outputs are:\n', ...
'1) x1 - simulated state, size [{{ dims.nx }}]\n');
fprintf(output_note)
third_party/acados/acados_template/c_templates_tera/mex_solver.in.m
Vendored
+166
View File
@@ -0,0 +1,166 @@
%
% 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.;
%
classdef {{ model.name }}_mex_solver < handle
properties
C_ocp
C_ocp_ext_fun
cost_ext_fun_type
cost_ext_fun_type_e
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 }}';
end
% destructor
function delete(obj)
if ~isempty(obj.C_ocp)
acados_mex_free_{{ model.name }}(obj.C_ocp);
end
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
% 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
third_party/acados/acados_template/c_templates_tera/model.in.h
Vendored
+211
View File
@@ -0,0 +1,211 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef {{ 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" %}
// implicit ODE
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
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
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" %}
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 %}
{% 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
third_party/acados/acados_template/c_templates_tera/phi_constraint.in.h
Vendored
+55
View File
@@ -0,0 +1,55 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef {{ model.name }}_PHI_CONSTRAINT
#define {{ model.name }}_PHI_CONSTRAINT
#ifdef __cplusplus
extern "C" {
#endif
{% if dims.nphi > 0 %}
// implicit ODE
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 %}
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // {{ model.name }}_PHI_CONSTRAINT
third_party/acados/acados_template/c_templates_tera/phi_e_constraint.in.h
Vendored
+21
View File
@@ -0,0 +1,21 @@
#ifndef {{ model.name }}_PHI_E_CONSTRAINT
#define {{ model.name }}_PHI_E_CONSTRAINT
#ifdef __cplusplus
extern "C" {
#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 %}
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif // {{ model.name }}_PHI_E_CONSTRAINT
third_party/acados/acados_template/generate_c_code_constraint.py
Vendored
+180
View File
@@ -0,0 +1,180 @@
#
# 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
from casadi import *
from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning
def generate_c_code_constraint( model, con_name, is_terminal, opts ):
casadi_version = CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
casadi_version_warning(casadi_version)
# load constraint variables and expression
x = model.x
p = model.p
if isinstance(x, casadi.MX):
symbol = MX.sym
else:
symbol = SX.sym
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 not (is_empty(con_h_expr) and is_empty(con_phi_expr)):
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
code_export_dir = opts["code_export_directory"]
if not os.path.exists(code_export_dir):
os.makedirs(code_export_dir)
cwd = os.getcwd()
os.chdir(code_export_dir)
gen_dir = con_name + '_constraints'
if not os.path.exists(gen_dir):
os.mkdir(gen_dir)
gen_dir_location = os.path.join('.', gen_dir)
os.chdir(gen_dir_location)
# 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 = transpose(jacobian(con_h_expr, vertcat(u,x)))
jac_z_t = jacobian(con_h_expr, z)
constraint_fun_jac_tran = Function(fun_name, [x, u, z, p], \
[con_h_expr, jac_ux_t, jac_z_t])
constraint_fun_jac_tran.generate(fun_name, casadi_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 = jtimes(con_h_expr, vertcat(u, x), lam_h, True)
# hessian
hess_ux = jacobian(adj_ux, vertcat(u, x))
adj_z = jtimes(con_h_expr, z, lam_h, True)
hess_z = jacobian(adj_z, z)
# set up functions
constraint_fun_jac_tran_hess = \
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_opts)
if is_terminal:
fun_name = con_name + '_constr_h_e_fun'
else:
fun_name = con_name + '_constr_h_fun'
h_fun = Function(fun_name, [x, u, z, p], [con_h_expr])
h_fun.generate(fun_name, casadi_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 = substitute(con_phi_expr, r, con_r_expr)
phi_jac_u = jacobian(con_phi_expr_x_u_z, u)
phi_jac_x = jacobian(con_phi_expr_x_u_z, x)
phi_jac_z = jacobian(con_phi_expr_x_u_z, z)
hess = hessian(con_phi_expr[0], r)[0]
for i in range(1, nphi):
hess = vertcat(hess, hessian(con_phi_expr[i], r)[0])
r_jac_u = jacobian(con_r_expr, u)
r_jac_x = jacobian(con_r_expr, x)
constraint_phi = \
Function(fun_name, [x, u, z, p], \
[con_phi_expr_x_u_z, \
vertcat(transpose(phi_jac_u), \
transpose(phi_jac_x)), \
transpose(phi_jac_z), \
hess, vertcat(transpose(r_jac_u), \
transpose(r_jac_x))])
constraint_phi.generate(fun_name, casadi_opts)
# change directory back
os.chdir(cwd)
return
third_party/acados/acados_template/generate_c_code_discrete_dynamics.py
Vendored
+98
View File
@@ -0,0 +1,98 @@
#
# 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 casadi as ca
from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning
def generate_c_code_discrete_dynamics( model, opts ):
casadi_version = ca.CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
casadi_version_warning(casadi_version)
# load model
x = model.x
u = model.u
p = model.p
phi = model.disc_dyn_expr
model_name = model.name
nx = casadi_length(x)
if isinstance(phi, ca.MX):
symbol = ca.MX.sym
elif isinstance(phi, ca.SX):
symbol = ca.SX.sym
else:
Exception("generate_c_code_disc_dyn: disc_dyn_expr must be a CasADi expression, you have type: {}".format(type(phi)))
# 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
code_export_dir = opts["code_export_directory"]
if not os.path.exists(code_export_dir):
os.makedirs(code_export_dir)
cwd = os.getcwd()
os.chdir(code_export_dir)
model_dir = model_name + '_model'
if not os.path.exists(model_dir):
os.mkdir(model_dir)
model_dir_location = os.path.join('.', model_dir)
os.chdir(model_dir_location)
# set up & generate 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_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_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_opts)
os.chdir(cwd)
third_party/acados/acados_template/generate_c_code_explicit_ode.py
Vendored
+124
View File
@@ -0,0 +1,124 @@
#
# 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
from casadi import *
from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning
def generate_c_code_explicit_ode( model, opts ):
casadi_version = CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
casadi_version_warning(casadi_version)
generate_hess = opts["generate_hess"]
code_export_dir = opts["code_export_directory"]
# 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]
if isinstance(f_expl, casadi.MX):
symbol = MX.sym
elif isinstance(f_expl, casadi.SX):
symbol = SX.sym
else:
raise Exception("Invalid type for f_expl! Possible types are 'SX' and 'MX'. Exiting.")
## 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 = Function(fun_name, [x, u, p], [f_expl])
vdeX = jtimes(f_expl,x,Sx)
vdeP = jacobian(f_expl,u) + jtimes(f_expl,x,Sp)
fun_name = model_name + '_expl_vde_forw'
expl_vde_forw = Function(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP])
adj = jtimes(f_expl, vertcat(x, u), lambdaX, True)
fun_name = model_name + '_expl_vde_adj'
expl_vde_adj = Function(fun_name, [x, lambdaX, u, p], [adj])
if generate_hess:
S_forw = vertcat(horzcat(Sx, Sp), horzcat(DM.zeros(nu,nx), DM.eye(nu)))
hess = mtimes(transpose(S_forw),jtimes(adj, vertcat(x,u), S_forw))
hess2 = []
for j in range(nx+nu):
for i in range(j,nx+nu):
hess2 = vertcat(hess2, hess[i,j])
fun_name = model_name + '_expl_ode_hess'
expl_ode_hess = Function(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2])
## generate C code
if not os.path.exists(code_export_dir):
os.makedirs(code_export_dir)
cwd = os.getcwd()
os.chdir(code_export_dir)
model_dir = model_name + '_model'
if not os.path.exists(model_dir):
os.mkdir(model_dir)
model_dir_location = os.path.join('.', model_dir)
os.chdir(model_dir_location)
fun_name = model_name + '_expl_ode_fun'
expl_ode_fun.generate(fun_name, casadi_opts)
fun_name = model_name + '_expl_vde_forw'
expl_vde_forw.generate(fun_name, casadi_opts)
fun_name = model_name + '_expl_vde_adj'
expl_vde_adj.generate(fun_name, casadi_opts)
if generate_hess:
fun_name = model_name + '_expl_ode_hess'
expl_ode_hess.generate(fun_name, casadi_opts)
os.chdir(cwd)
return
third_party/acados/acados_template/generate_c_code_external_cost.py
Vendored
+116
View File
@@ -0,0 +1,116 @@
#
# 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
from casadi import SX, MX, Function, transpose, vertcat, horzcat, hessian, CasadiMeta
from .utils import ALLOWED_CASADI_VERSIONS, casadi_version_warning
def generate_c_code_external_cost(model, stage_type, opts):
casadi_version = CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double")
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
casadi_version_warning(casadi_version)
x = model.x
p = model.p
if isinstance(x, MX):
symbol = MX.sym
else:
symbol = SX.sym
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"
u = symbol("u", 0, 0)
ext_cost = model.cost_expr_ext_cost_e
custom_hess = model.cost_expr_ext_cost_custom_hess_e
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"
u = model.u
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"
u = model.u
ext_cost = model.cost_expr_ext_cost_0
custom_hess = model.cost_expr_ext_cost_custom_hess_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
full_hess, grad = hessian(ext_cost, vertcat(u, x))
if custom_hess is not None:
full_hess = custom_hess
ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost])
ext_cost_fun_jac_hess = Function(
fun_name_hess, [x, u, p], [ext_cost, grad, full_hess]
)
ext_cost_fun_jac = Function(
fun_name_jac, [x, u, p], [ext_cost, grad]
)
# generate C code
code_export_dir = opts["code_export_directory"]
if not os.path.exists(code_export_dir):
os.makedirs(code_export_dir)
cwd = os.getcwd()
os.chdir(code_export_dir)
gen_dir = model.name + '_cost'
if not os.path.exists(gen_dir):
os.mkdir(gen_dir)
gen_dir_location = "./" + gen_dir
os.chdir(gen_dir_location)
ext_cost_fun.generate(fun_name, casadi_opts)
ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts)
ext_cost_fun_jac.generate(fun_name_jac, casadi_opts)
os.chdir(cwd)
return
third_party/acados/acados_template/generate_c_code_gnsf.py
Vendored
+131
View File
@@ -0,0 +1,131 @@
#
# 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
from casadi import *
from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning
def generate_c_code_gnsf( model, opts ):
casadi_version = CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
casadi_version_warning(casadi_version)
model_name = model.name
code_export_dir = opts["code_export_directory"]
# set up directory
if not os.path.exists(code_export_dir):
os.makedirs(code_export_dir)
cwd = os.getcwd()
os.chdir(code_export_dir)
model_dir = model_name + '_model'
if not os.path.exists(model_dir):
os.mkdir(model_dir)
model_dir_location = os.path.join('.', model_dir)
os.chdir(model_dir_location)
# 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 MX because of cost/constraints
# the DAE can be exported as SX -> detect GNSF in Matlab
# -> evaluated SX GNSF functions with MX.
u = model.u
if isinstance(u, casadi.MX):
symbol = MX.sym
else:
symbol = SX.sym
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_ = Function(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)])
phi_fun_.generate(fun_name, casadi_opts)
fun_name = model_name + '_gnsf_phi_fun_jac_y'
phi_fun_jac_y = model.phi_fun_jac_y
phi_fun_jac_y_ = Function(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p))
phi_fun_jac_y_.generate(fun_name, casadi_opts)
fun_name = model_name + '_gnsf_phi_jac_y_uhat'
phi_jac_y_uhat = model.phi_jac_y_uhat
phi_jac_y_uhat_ = Function(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p))
phi_jac_y_uhat_.generate(fun_name, casadi_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_ = Function(fun_name, [x1, x1dot, z1, u, p],
f_lo_fun_jac_x1k1uz_eval)
f_lo_fun_jac_x1k1uz_.generate(fun_name, casadi_opts)
fun_name = model_name + '_gnsf_get_matrices_fun'
get_matrices_fun_ = Function(fun_name, [dummy], get_matrices_fun(1))
get_matrices_fun_.generate(fun_name, casadi_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
third_party/acados/acados_template/generate_c_code_implicit_ode.py
Vendored
+139
View File
@@ -0,0 +1,139 @@
#
# 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
from casadi import *
from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning
def generate_c_code_implicit_ode( model, opts ):
casadi_version = CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
casadi_version_warning(casadi_version)
generate_hess = opts["generate_hess"]
code_export_dir = opts["code_export_directory"]
## 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)
nu = casadi_length(u)
nz = casadi_length(z)
## generate jacobians
jac_x = jacobian(f_impl, x)
jac_xdot = jacobian(f_impl, xdot)
jac_u = jacobian(f_impl, u)
jac_z = jacobian(f_impl, z)
## generate hessian
x_xdot_z_u = vertcat(x, xdot, z, u)
if isinstance(x, casadi.MX):
symbol = MX.sym
else:
symbol = SX.sym
multiplier = symbol('multiplier', nx + nz)
ADJ = jtimes(f_impl, x_xdot_z_u, multiplier, True)
HESS = jacobian(ADJ, x_xdot_z_u)
## Set up functions
p = model.p
fun_name = model_name + '_impl_dae_fun'
impl_dae_fun = 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 = 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_z'
# impl_dae_fun_jac_x_xdot = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z])
# fun_name = model_name + '_impl_dae_jac_x_xdot_u'
# impl_dae_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z])
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z'
impl_dae_fun_jac_x_xdot_u_z = 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 = 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 = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z])
fun_name = model_name + '_impl_dae_hess'
impl_dae_hess = Function(fun_name, [x, xdot, u, z, multiplier, p], [HESS])
# generate C code
if not os.path.exists(code_export_dir):
os.makedirs(code_export_dir)
cwd = os.getcwd()
os.chdir(code_export_dir)
model_dir = model_name + '_model'
if not os.path.exists(model_dir):
os.mkdir(model_dir)
model_dir_location = os.path.join('.', model_dir)
os.chdir(model_dir_location)
fun_name = model_name + '_impl_dae_fun'
impl_dae_fun.generate(fun_name, casadi_opts)
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z'
impl_dae_fun_jac_x_xdot_z.generate(fun_name, casadi_opts)
fun_name = model_name + '_impl_dae_jac_x_xdot_u_z'
impl_dae_jac_x_xdot_u_z.generate(fun_name, casadi_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_opts)
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u'
impl_dae_fun_jac_x_xdot_u.generate(fun_name, casadi_opts)
if generate_hess:
fun_name = model_name + '_impl_dae_hess'
impl_dae_hess.generate(fun_name, casadi_opts)
os.chdir(cwd)
third_party/acados/acados_template/generate_c_code_nls_cost.py
Vendored
+113
View File
@@ -0,0 +1,113 @@
#
# 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
from casadi import *
from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning
def generate_c_code_nls_cost( model, cost_name, stage_type, opts ):
casadi_version = CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
casadi_version_warning(casadi_version)
x = model.x
p = model.p
if isinstance(x, casadi.MX):
symbol = MX.sym
else:
symbol = SX.sym
if stage_type == 'terminal':
middle_name = '_cost_y_e'
u = symbol('u', 0, 0)
cost_expr = model.cost_y_expr_e
elif stage_type == 'initial':
middle_name = '_cost_y_0'
u = model.u
cost_expr = model.cost_y_expr_0
elif stage_type == 'path':
middle_name = '_cost_y'
u = model.u
cost_expr = model.cost_y_expr
# set up directory
code_export_dir = opts["code_export_directory"]
if not os.path.exists(code_export_dir):
os.makedirs(code_export_dir)
cwd = os.getcwd()
os.chdir(code_export_dir)
gen_dir = cost_name + '_cost'
if not os.path.exists(gen_dir):
os.mkdir(gen_dir)
gen_dir_location = os.path.join('.', gen_dir)
os.chdir(gen_dir_location)
# set up expressions
cost_jac_expr = transpose(jacobian(cost_expr, vertcat(u, x)))
ny = casadi_length(cost_expr)
y = symbol('y', ny, 1)
y_adj = jtimes(cost_expr, vertcat(u, x), y, True)
y_hess = jacobian(y_adj, vertcat(u, x))
## generate C code
suffix_name = '_fun'
fun_name = cost_name + middle_name + suffix_name
y_fun = Function( fun_name, [x, u, p], \
[ cost_expr ])
y_fun.generate( fun_name, casadi_opts )
suffix_name = '_fun_jac_ut_xt'
fun_name = cost_name + middle_name + suffix_name
y_fun_jac_ut_xt = Function(fun_name, [x, u, p], \
[ cost_expr, cost_jac_expr ])
y_fun_jac_ut_xt.generate( fun_name, casadi_opts )
suffix_name = '_hess'
fun_name = cost_name + middle_name + suffix_name
y_hess = Function(fun_name, [x, u, y, p], [ y_hess ])
y_hess.generate( fun_name, casadi_opts )
os.chdir(cwd)
return
third_party/acados/acados_template/simulink_default_opts.json
Vendored
+40
View File
@@ -0,0 +1,40 @@
{
"outputs": {
"u0": 1,
"utraj": 0,
"xtraj": 0,
"solver_status": 1,
"KKT_residual": 1,
"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,
"cost_W_0": 0,
"cost_W": 0,
"cost_W_e": 0,
"reset_solver": 0,
"x_init": 0,
"u_init": 0
},
"samplingtime": "t0"
}
third_party/acados/acados_template/utils.py
Vendored
+440
View File
@@ -0,0 +1,440 @@
# -*- 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, sys, json
import urllib.request
import shutil
import numpy as np
from casadi import SX, MX, DM, Function, CasadiMeta
ALLOWED_CASADI_VERSIONS = ('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"
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
platform2tera = {
"linux": "linux",
"darwin": "osx",
"win32": "windows"
}
def casadi_version_warning(casadi_version):
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 or x == []:
return True
else:
raise Exception("is_empty expects one of the following types: casadi.MX, casadi.SX, "
+ "None, numpy array empty list. 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_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, template_dir, json_path):
cwd = os.getcwd()
if not os.path.exists(template_dir):
os.mkdir(template_dir)
os.chdir(template_dir)
tera_path = get_tera()
# setting up loader and environment
acados_path = os.path.dirname(os.path.abspath(__file__))
template_glob = os.path.join(acados_path, 'c_templates_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\nExiting.\n')
os.chdir(cwd)
## Conversion functions
def np_array_to_list(np_array):
if isinstance(np_array, (np.ndarray)):
return np_array.tolist()
elif isinstance(np_array, (SX)):
return DM(np_array).full()
elif isinstance(np_array, (DM)):
return np_array.full()
else:
raise(Exception(f"Cannot convert to list type {type(np_array)}"))
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():
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 ocp_check_against_layout(ocp_nlp, ocp_dims):
"""
Check dimensions against layout
Parameters
---------
ocp_nlp : dict
dictionary loaded from JSON to be post-processed.
ocp_dims : instance of AcadosOcpDims
"""
ocp_nlp_layout = get_ocp_nlp_layout()
ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, ocp_nlp_layout)
return
def ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, layout):
for key, item in ocp_nlp.items():
try:
layout_of_key = layout[key]
except KeyError:
raise Exception("ocp_check_against_layout_recursion: field" \
" '{0}' is not in layout but in OCP description.".format(key))
if isinstance(item, dict):
ocp_check_against_layout_recursion(item, ocp_dims, layout_of_key)
if 'ndarray' in layout_of_key:
if isinstance(item, int) or isinstance(item, float):
item = np.array([item])
if isinstance(item, (list, np.ndarray)) and (layout_of_key[0] != 'str'):
dim_layout = []
dim_names = layout_of_key[1]
for dim_name in dim_names:
dim_layout.append(ocp_dims[dim_name])
dims = tuple(dim_layout)
item = np.array(item)
item_dims = item.shape
if len(item_dims) != len(dims):
raise Exception('Mismatching dimensions for field {0}. ' \
'Expected {1} dimensional array, got {2} dimensional array.' \
.format(key, len(dims), len(item_dims)))
if np.prod(item_dims) != 0 or np.prod(dims) != 0:
if dims != item_dims:
raise Exception('acados -- mismatching dimensions for field {0}. ' \
'Provided data has dimensions {1}, ' \
'while associated dimensions {2} are {3}' \
.format(key, item_dims, dim_names, dims))
return
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. Exiting.')
if this_idx.size > 0 and J[i,this_idx[0]] != 1:
raise Exception('J matrices can only contain 1s. Exiting.')
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. Exiting. ' \
'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=np_array_to_list, indent=4, sort_keys=True)
print("dumped ", model_name, " dae to file:", json_file, "\n")
def set_up_imported_gnsf_model(acados_formulation):
gnsf = acados_formulation.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_formulation.dims.gnsf_nx1 = size_gnsf_A[1]
acados_formulation.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
acados_formulation.dims.gnsf_nuhat = max(phi_fun.size_in(1))
acados_formulation.dims.gnsf_ny = max(phi_fun.size_in(0))
acados_formulation.dims.gnsf_nout = max(phi_fun.size_out(0))
# save gnsf functions in model
acados_formulation.model.phi_fun = phi_fun
acados_formulation.model.phi_fun_jac_y = phi_fun_jac_y
acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat
acados_formulation.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_formulation.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12])
acados_formulation.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_formulation.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz
else:
dummy_var_x1 = SX.sym('dummy_var_x1', acados_formulation.dims.gnsf_nx1)
dummy_var_x1dot = SX.sym('dummy_var_x1dot', acados_formulation.dims.gnsf_nx1)
dummy_var_z1 = SX.sym('dummy_var_z1', acados_formulation.dims.gnsf_nz1)
dummy_var_u = SX.sym('dummy_var_z1', acados_formulation.dims.nu)
dummy_var_p = SX.sym('dummy_var_z1', acados_formulation.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_formulation.model.f_lo_fun_jac_x1k1uz = empty_fun
del acados_formulation.gnsf_model
third_party/acados/include/acados/dense_qp/dense_qp_common.h
Vendored
+150
View File
@@ -0,0 +1,150 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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_
third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h
Vendored
+94
View File
@@ -0,0 +1,94 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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_
third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h
Vendored
+128
View File
@@ -0,0 +1,128 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#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_
third_party/acados/include/acados/dense_qp/dense_qp_qore.h
Vendored
+127
View File
@@ -0,0 +1,127 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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_
third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h
Vendored
+129
View File
@@ -0,0 +1,129 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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_
third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h
Vendored
+446
View File
@@ -0,0 +1,446 @@
/*
* 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.;
*/
/// \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);
//
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_
/// @}
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h
Vendored
+241
View File
@@ -0,0 +1,241 @@
/*
* 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.;
*/
/// \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_initialize(void *config, void *dims, int nx, int nu, int nz, int nbx,
int nbu, int ng, int nh, int dummy0, int ns);
//
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);
/************************************************
* 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_
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h
Vendored
+221
View File
@@ -0,0 +1,221 @@
/*
* 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.;
*/
/// \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_initialize(void *config, void *dims, int nx, int nu, int nz,
int nbx, int nbu, int ng, int nphi, int nq, int ns);
//
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);
/* 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_
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h
Vendored
+112
View File
@@ -0,0 +1,112 @@
/*
* 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.;
*/
/// \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);
void (*dims_initialize)(void *config, void *dims, int nx, int nu, int nz, int nbx, int nbu, int ng,
int nh, int nq, int ns);
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, 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_
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h
Vendored
+109
View File
@@ -0,0 +1,109 @@
/*
* 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.;
*/
///
/// \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_initialize)(void *config, void *dims, int nx, int nu, int ny, int ns, int nz);
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);
} 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_
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h
Vendored
+187
View File
@@ -0,0 +1,187 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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 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_initialize(void *config, void *dims, int nx,
int nu, int ny, int ns, int nz);
//
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_nv_nv;
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_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_
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h
Vendored
+259
View File
@@ -0,0 +1,259 @@
/*
* 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.;
*/
/// \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);
/// Initialize the dimensions struct of the
/// ocp_nlp-cost_ls component
///
/// \param[in] config structure containing configuration ocp_nlp-cost_ls component
/// \param[in] nx number of states
/// \param[in] nu number of inputs
/// \param[in] ny number of residuals
/// \param[in] ns number of slacks
/// \param[in] nz number of algebraic variables
/// \param[out] dims
/// \return size
void ocp_nlp_cost_ls_dims_initialize(void *config, void *dims, int nx,
int nu, int ny, int ns, int nz);
//
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;
} 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 //
////////////////////////////////////////////////////////////////////////////////
//
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_
/// @}
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h
Vendored
+210
View File
@@ -0,0 +1,210 @@
/*
* 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.;
*/
/// \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} \| r(x,u) - 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 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_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz);
//
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;
} 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_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_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_dvec tmp_ny;
struct blasfeo_dvec tmp_2ns; // temporary vector of dimension ny
} 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_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_
/// @}
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h
Vendored
+123
View File
@@ -0,0 +1,123 @@
/*
* 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.;
*/
/// \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_initialize)(void *config, void *dims, int nx, int nu, int nx1, int nu1, int nz);
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_
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h
Vendored
+216
View File
@@ -0,0 +1,216 @@
/*
* 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.;
*/
/// \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_initialize(void *config, void *dims, int nx, int nu, int nx1,
int nu1, int nz);
//
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_
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h
Vendored
+199
View File
@@ -0,0 +1,199 @@
/*
* 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.;
*/
/// \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_initialize(void *config, void *dims, int nx, int nu, int nx1,
int nu1, int nz);
//
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_
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h
Vendored
+125
View File
@@ -0,0 +1,125 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
/// \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_
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h
Vendored
+149
View File
@@ -0,0 +1,149 @@
/*
* 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.;
*/
/// \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_
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h
Vendored
+124
View File
@@ -0,0 +1,124 @@
/*
* 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.;
*/
/// \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_
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h
Vendored
+109
View File
@@ -0,0 +1,109 @@
/*
* 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.;
*/
/// \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_
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h
Vendored
+124
View File
@@ -0,0 +1,124 @@
/*
* 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.;
*/
/// \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_
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h
Vendored
+135
View File
@@ -0,0 +1,135 @@
/*
* 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.;
*/
/// \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_
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h
Vendored
+173
View File
@@ -0,0 +1,173 @@
/*
* 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.;
*/
/// \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_
/// @}
/// @}
/// @}
third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h
Vendored
+172
View File
@@ -0,0 +1,172 @@
/*
* 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.;
*/
/// \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
************************************************/
void ocp_nlp_sqp_rti_preparation_step(void *config_, void *dims_,
void *nlp_in_, void *nlp_out_, void *opts, void *mem_, void *work_);
//
void ocp_nlp_sqp_rti_feedback_step(void *config_, void *dims_,
void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_);
//
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_
/// @}
/// @}
/// @}
third_party/acados/include/acados/ocp_qp/ocp_qp_common.h
Vendored
+183
View File
@@ -0,0 +1,183 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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_
third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h
Vendored
+121
View File
@@ -0,0 +1,121 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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_
third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h
Vendored
+117
View File
@@ -0,0 +1,117 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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_
third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h
Vendored
+103
View File
@@ -0,0 +1,103 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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_
third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h
Vendored
+130
View File
@@ -0,0 +1,130 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#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_
third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h
Vendored
+147
View File
@@ -0,0 +1,147 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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_
third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h
Vendored
+106
View File
@@ -0,0 +1,106 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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_
third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h
Vendored
+123
View File
@@ -0,0 +1,123 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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_
third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h
Vendored
+122
View File
@@ -0,0 +1,122 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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_
third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h
Vendored
+152
View File
@@ -0,0 +1,152 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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);
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_
third_party/acados/include/acados/sim/sim_collocation_utils.h
Vendored
+100
View File
@@ -0,0 +1,100 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_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_
third_party/acados/include/acados/sim/sim_common.h
Vendored
+222
View File
@@ -0,0 +1,222 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_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;
// 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_
third_party/acados/include/acados/sim/sim_erk_integrator.h
Vendored
+143
View File
@@ -0,0 +1,143 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_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_
third_party/acados/include/acados/sim/sim_gnsf.h
Vendored
+367
View File
@@ -0,0 +1,367 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_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_
third_party/acados/include/acados/sim/sim_irk_integrator.h
Vendored
+186
View File
@@ -0,0 +1,186 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_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_
third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h
Vendored
+160
View File
@@ -0,0 +1,160 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_SIM_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_
third_party/acados/include/acados/utils/external_function_generic.h
Vendored
+245
View File
@@ -0,0 +1,245 @@
/*
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
*
* This file is part of acados.
*
* The 2-Clause BSD License
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.;
*/
#ifndef ACADOS_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)();
int (*casadi_n_out)();
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)();
int (*casadi_n_out)();
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