openpilot v0.9.6 release
date: 2024-01-12T10:13:37 master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6
This commit is contained in:
FrogAi
5
third_party/acados/.gitignore
vendored
Normal file
5
third_party/acados/.gitignore
vendored
Normal file
@@ -0,0 +1,5 @@
|
||||
acados_repo/
|
||||
lib
|
||||
!x86_64/
|
||||
!larch64/
|
||||
!aarch64/
|
||||
40
third_party/acados/acados_template/__init__.py
vendored
Normal file
40
third_party/acados/acados_template/__init__.py
vendored
Normal file
@@ -0,0 +1,40 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
from .acados_model import AcadosModel
|
||||
from .acados_ocp import AcadosOcp, AcadosOcpConstraints, AcadosOcpCost, AcadosOcpDims, AcadosOcpOptions
|
||||
from .acados_sim import AcadosSim, AcadosSimDims, AcadosSimOpts
|
||||
from .acados_ocp_solver import AcadosOcpSolver, get_simulink_default_opts, ocp_get_default_cmake_builder
|
||||
from .acados_sim_solver import AcadosSimSolver, sim_get_default_cmake_builder
|
||||
from .utils import print_casadi_expression, get_acados_path, get_python_interface_path, \
|
||||
get_tera_exec_path, get_tera, check_casadi_version, acados_dae_model_json_dump, \
|
||||
casadi_length, make_object_json_dumpable, J_to_idx, get_default_simulink_opts
|
||||
|
||||
from .zoro_description import ZoroDescription, process_zoro_description
|
||||
834
third_party/acados/acados_template/acados_layout.json
vendored
Normal file
834
third_party/acados/acados_template/acados_layout.json
vendored
Normal file
@@ -0,0 +1,834 @@
|
||||
{
|
||||
"code_export_directory": [
|
||||
"str"
|
||||
],
|
||||
"acados_include_path": [
|
||||
"str"
|
||||
],
|
||||
"cython_include_dirs": [
|
||||
"list"
|
||||
],
|
||||
"json_file": [
|
||||
"str"
|
||||
],
|
||||
"shared_lib_ext": [
|
||||
"str"
|
||||
],
|
||||
"model": {
|
||||
"name" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_ext_fun_type" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_generic_source" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_impl_dae_fun" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_impl_dae_fun_jac" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_impl_dae_jac" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_disc_fun_jac_hess" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_disc_fun_jac" : [
|
||||
"str"
|
||||
],
|
||||
"dyn_disc_fun" : [
|
||||
"str"
|
||||
],
|
||||
"gnsf" : {
|
||||
"nontrivial_f_LO": [
|
||||
"int"
|
||||
],
|
||||
"purely_linear": [
|
||||
"int"
|
||||
]
|
||||
}
|
||||
},
|
||||
"parameter_values": [
|
||||
"ndarray",
|
||||
[
|
||||
"np"
|
||||
]
|
||||
],
|
||||
"acados_lib_path": [
|
||||
"str"
|
||||
],
|
||||
"problem_class": [
|
||||
"str"
|
||||
],
|
||||
"constraints": {
|
||||
"constr_type": [
|
||||
"str"
|
||||
],
|
||||
"constr_type_e": [
|
||||
"str"
|
||||
],
|
||||
"lbx": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx"
|
||||
]
|
||||
],
|
||||
"lbu": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbu"
|
||||
]
|
||||
],
|
||||
"ubx": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx"
|
||||
]
|
||||
],
|
||||
"ubu": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbu"
|
||||
]
|
||||
],
|
||||
"idxbx": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx"
|
||||
]
|
||||
],
|
||||
"idxbu": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbu"
|
||||
]
|
||||
],
|
||||
"lbx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx_e"
|
||||
]
|
||||
],
|
||||
"ubx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx_e"
|
||||
]
|
||||
],
|
||||
"idxbx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx_e"
|
||||
]
|
||||
],
|
||||
"lbx_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx_0"
|
||||
]
|
||||
],
|
||||
"ubx_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx_0"
|
||||
]
|
||||
],
|
||||
"idxbx_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbx_0"
|
||||
]
|
||||
],
|
||||
"idxbxe_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"nbxe_0"
|
||||
]
|
||||
],
|
||||
"lg": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng"
|
||||
]
|
||||
],
|
||||
"ug": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng"
|
||||
]
|
||||
],
|
||||
"D": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng",
|
||||
"nu"
|
||||
]
|
||||
],
|
||||
"C": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng",
|
||||
"nx"
|
||||
]
|
||||
],
|
||||
"C_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng_e",
|
||||
"nx"
|
||||
]
|
||||
],
|
||||
"lg_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng_e"
|
||||
]
|
||||
],
|
||||
"ug_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ng_e"
|
||||
]
|
||||
],
|
||||
"lh": [
|
||||
"ndarray",
|
||||
[
|
||||
"nh"
|
||||
]
|
||||
],
|
||||
"uh": [
|
||||
"ndarray",
|
||||
[
|
||||
"nh"
|
||||
]
|
||||
],
|
||||
"lh_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nh_e"
|
||||
]
|
||||
],
|
||||
"uh_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nh_e"
|
||||
]
|
||||
],
|
||||
"lphi": [
|
||||
"ndarray",
|
||||
[
|
||||
"nphi"
|
||||
]
|
||||
],
|
||||
"uphi": [
|
||||
"ndarray",
|
||||
[
|
||||
"nphi"
|
||||
]
|
||||
],
|
||||
"lphi_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nphi_e"
|
||||
]
|
||||
],
|
||||
"uphi_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nphi_e"
|
||||
]
|
||||
],
|
||||
"lsbx": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbx"
|
||||
]
|
||||
],
|
||||
"usbx": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbx"
|
||||
]
|
||||
],
|
||||
"lsbu": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbu"
|
||||
]
|
||||
],
|
||||
"usbu": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbu"
|
||||
]
|
||||
],
|
||||
"idxsbx": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbx"
|
||||
]
|
||||
],
|
||||
"idxsbu": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbu"
|
||||
]
|
||||
],
|
||||
"lsbx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbx_e"
|
||||
]
|
||||
],
|
||||
"usbx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbx_e"
|
||||
]
|
||||
],
|
||||
"idxsbx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsbx_e"
|
||||
]
|
||||
],
|
||||
"lsg": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsg"
|
||||
]
|
||||
],
|
||||
"usg": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsg"
|
||||
]
|
||||
],
|
||||
"idxsg": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsg"
|
||||
]
|
||||
],
|
||||
"lsg_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsg_e"
|
||||
]
|
||||
],
|
||||
"usg_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsg_e"
|
||||
]
|
||||
],
|
||||
"idxsg_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsg_e"
|
||||
]
|
||||
],
|
||||
"lsh": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsh"
|
||||
]
|
||||
],
|
||||
"ush": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsh"
|
||||
]
|
||||
],
|
||||
"idxsh": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsh"
|
||||
]
|
||||
],
|
||||
"lsh_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsh_e"
|
||||
]
|
||||
],
|
||||
"ush_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsh_e"
|
||||
]
|
||||
],
|
||||
"idxsh_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsh_e"
|
||||
]
|
||||
],
|
||||
"lsphi": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsphi"
|
||||
]
|
||||
],
|
||||
"usphi": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsphi"
|
||||
]
|
||||
],
|
||||
"idxsphi": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsphi"
|
||||
]
|
||||
],
|
||||
"lsphi_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsphi_e"
|
||||
]
|
||||
],
|
||||
"usphi_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsphi_e"
|
||||
]
|
||||
],
|
||||
"idxsphi_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"nsphi_e"
|
||||
]
|
||||
]
|
||||
},
|
||||
"cost": {
|
||||
"cost_type_0": [
|
||||
"str"
|
||||
],
|
||||
"cost_type": [
|
||||
"str"
|
||||
],
|
||||
"cost_type_e": [
|
||||
"str"
|
||||
],
|
||||
"cost_ext_fun_type_0": [
|
||||
"str"
|
||||
],
|
||||
"cost_ext_fun_type": [
|
||||
"str"
|
||||
],
|
||||
"cost_ext_fun_type_e": [
|
||||
"str"
|
||||
],
|
||||
"Vu_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_0",
|
||||
"nu"
|
||||
]
|
||||
],
|
||||
"Vu": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny",
|
||||
"nu"
|
||||
]
|
||||
],
|
||||
"Vx_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_0",
|
||||
"nx"
|
||||
]
|
||||
],
|
||||
"Vx": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny",
|
||||
"nx"
|
||||
]
|
||||
],
|
||||
"Vx_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_e",
|
||||
"nx"
|
||||
]
|
||||
],
|
||||
"Vz_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_0",
|
||||
"nz"
|
||||
]
|
||||
],
|
||||
"Vz": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny",
|
||||
"nz"
|
||||
]
|
||||
],
|
||||
"W_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_0",
|
||||
"ny_0"
|
||||
]
|
||||
],
|
||||
"W": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny",
|
||||
"ny"
|
||||
]
|
||||
],
|
||||
"Zl": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns"
|
||||
]
|
||||
],
|
||||
"Zu": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns"
|
||||
]
|
||||
],
|
||||
"zl": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns"
|
||||
]
|
||||
],
|
||||
"zu": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns"
|
||||
]
|
||||
],
|
||||
"W_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_e",
|
||||
"ny_e"
|
||||
]
|
||||
],
|
||||
"yref_0": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_0"
|
||||
]
|
||||
],
|
||||
"yref": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny"
|
||||
]
|
||||
],
|
||||
"yref_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ny_e"
|
||||
]
|
||||
],
|
||||
"Zl_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns_e"
|
||||
]
|
||||
],
|
||||
"Zu_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns_e"
|
||||
]
|
||||
],
|
||||
"zl_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns_e"
|
||||
]
|
||||
],
|
||||
"zu_e": [
|
||||
"ndarray",
|
||||
[
|
||||
"ns_e"
|
||||
]
|
||||
]
|
||||
},
|
||||
"dims": {
|
||||
"N": [
|
||||
"int"
|
||||
],
|
||||
"nbu": [
|
||||
"int"
|
||||
],
|
||||
"nbx": [
|
||||
"int"
|
||||
],
|
||||
"nsbu": [
|
||||
"int"
|
||||
],
|
||||
"nsbx": [
|
||||
"int"
|
||||
],
|
||||
"nsbx_e": [
|
||||
"int"
|
||||
],
|
||||
"nbx_0": [
|
||||
"int"
|
||||
],
|
||||
"nbx_e": [
|
||||
"int"
|
||||
],
|
||||
"nbxe_0": [
|
||||
"int"
|
||||
],
|
||||
"nsg": [
|
||||
"int"
|
||||
],
|
||||
"nsg_e": [
|
||||
"int"
|
||||
],
|
||||
"nsh": [
|
||||
"int"
|
||||
],
|
||||
"nsh_e": [
|
||||
"int"
|
||||
],
|
||||
"nsphi": [
|
||||
"int"
|
||||
],
|
||||
"nsphi_e": [
|
||||
"int"
|
||||
],
|
||||
"ns": [
|
||||
"int"
|
||||
],
|
||||
"ns_e": [
|
||||
"int"
|
||||
],
|
||||
"ng": [
|
||||
"int"
|
||||
],
|
||||
"ng_e": [
|
||||
"int"
|
||||
],
|
||||
"np": [
|
||||
"int"
|
||||
],
|
||||
"nr": [
|
||||
"int"
|
||||
],
|
||||
"nr_e": [
|
||||
"int"
|
||||
],
|
||||
"nh": [
|
||||
"int"
|
||||
],
|
||||
"nh_e": [
|
||||
"int"
|
||||
],
|
||||
"nphi": [
|
||||
"int"
|
||||
],
|
||||
"nphi_e": [
|
||||
"int"
|
||||
],
|
||||
"nu": [
|
||||
"int"
|
||||
],
|
||||
"nx": [
|
||||
"int"
|
||||
],
|
||||
"ny": [
|
||||
"int"
|
||||
],
|
||||
"ny_0": [
|
||||
"int"
|
||||
],
|
||||
"ny_e": [
|
||||
"int"
|
||||
],
|
||||
"nz": [
|
||||
"int"
|
||||
],
|
||||
"gnsf_nx1": [
|
||||
"int"
|
||||
],
|
||||
"gnsf_nz1": [
|
||||
"int"
|
||||
],
|
||||
"gnsf_nuhat": [
|
||||
"int"
|
||||
],
|
||||
"gnsf_ny": [
|
||||
"int"
|
||||
],
|
||||
"gnsf_nout": [
|
||||
"int"
|
||||
]
|
||||
},
|
||||
"solver_options": {
|
||||
"time_steps": [
|
||||
"ndarray",
|
||||
[
|
||||
"N"
|
||||
]
|
||||
],
|
||||
"hessian_approx": [
|
||||
"str"
|
||||
],
|
||||
"hpipm_mode": [
|
||||
"str"
|
||||
],
|
||||
"regularize_method": [
|
||||
"str"
|
||||
],
|
||||
"integrator_type": [
|
||||
"str"
|
||||
],
|
||||
"nlp_solver_type": [
|
||||
"str"
|
||||
],
|
||||
"collocation_type": [
|
||||
"str"
|
||||
],
|
||||
"globalization": [
|
||||
"str"
|
||||
],
|
||||
"nlp_solver_step_length": [
|
||||
"float"
|
||||
],
|
||||
"levenberg_marquardt": [
|
||||
"float"
|
||||
],
|
||||
"qp_solver": [
|
||||
"str"
|
||||
],
|
||||
"tf": [
|
||||
"float"
|
||||
],
|
||||
"Tsim": [
|
||||
"float"
|
||||
],
|
||||
"alpha_min": [
|
||||
"float"
|
||||
],
|
||||
"alpha_reduction": [
|
||||
"float"
|
||||
],
|
||||
"line_search_use_sufficient_descent": [
|
||||
"int"
|
||||
],
|
||||
"globalization_use_SOC": [
|
||||
"int"
|
||||
],
|
||||
"full_step_dual": [
|
||||
"int"
|
||||
],
|
||||
"eps_sufficient_descent": [
|
||||
"float"
|
||||
],
|
||||
"sim_method_num_stages": [
|
||||
"ndarray",
|
||||
[
|
||||
"N"
|
||||
]
|
||||
],
|
||||
"sim_method_num_steps": [
|
||||
"ndarray",
|
||||
[
|
||||
"N"
|
||||
]
|
||||
],
|
||||
"sim_method_newton_iter": [
|
||||
"int"
|
||||
],
|
||||
"sim_method_newton_tol": [
|
||||
"float"
|
||||
],
|
||||
"sim_method_jac_reuse": [
|
||||
"ndarray",
|
||||
[
|
||||
"N"
|
||||
]
|
||||
],
|
||||
"qp_solver_cond_N": [
|
||||
"int"
|
||||
],
|
||||
"qp_solver_warm_start": [
|
||||
"int"
|
||||
],
|
||||
"qp_solver_tol_stat": [
|
||||
"float"
|
||||
],
|
||||
"qp_solver_tol_eq": [
|
||||
"float"
|
||||
],
|
||||
"qp_solver_tol_ineq": [
|
||||
"float"
|
||||
],
|
||||
"qp_solver_tol_comp": [
|
||||
"float"
|
||||
],
|
||||
"qp_solver_iter_max": [
|
||||
"int"
|
||||
],
|
||||
"qp_solver_cond_ric_alg": [
|
||||
"int"
|
||||
],
|
||||
"qp_solver_ric_alg": [
|
||||
"int"
|
||||
],
|
||||
"nlp_solver_tol_stat": [
|
||||
"float"
|
||||
],
|
||||
"nlp_solver_tol_eq": [
|
||||
"float"
|
||||
],
|
||||
"nlp_solver_tol_ineq": [
|
||||
"float"
|
||||
],
|
||||
"nlp_solver_tol_comp": [
|
||||
"float"
|
||||
],
|
||||
"nlp_solver_max_iter": [
|
||||
"int"
|
||||
],
|
||||
"nlp_solver_ext_qp_res": [
|
||||
"int"
|
||||
],
|
||||
"print_level": [
|
||||
"int"
|
||||
],
|
||||
"initialize_t_slacks": [
|
||||
"int"
|
||||
],
|
||||
"exact_hess_cost": [
|
||||
"int"
|
||||
],
|
||||
"exact_hess_constr": [
|
||||
"int"
|
||||
],
|
||||
"exact_hess_dyn": [
|
||||
"int"
|
||||
],
|
||||
"ext_cost_num_hess": [
|
||||
"int"
|
||||
],
|
||||
"ext_fun_compile_flags": [
|
||||
"str"
|
||||
],
|
||||
"model_external_shared_lib_dir": [
|
||||
"str"
|
||||
],
|
||||
"model_external_shared_lib_name": [
|
||||
"str"
|
||||
]
|
||||
}
|
||||
}
|
||||
154
third_party/acados/acados_template/acados_model.py
vendored
Normal file
154
third_party/acados/acados_template/acados_model.py
vendored
Normal file
@@ -0,0 +1,154 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
|
||||
class AcadosModel():
|
||||
"""
|
||||
Class containing all the information to code generate the external CasADi functions
|
||||
that are needed when creating an acados ocp solver or acados integrator.
|
||||
Thus, this class contains:
|
||||
|
||||
a) the :py:attr:`name` of the model,
|
||||
b) all CasADi variables/expressions needed in the CasADi function generation process.
|
||||
"""
|
||||
def __init__(self):
|
||||
## common for OCP and Integrator
|
||||
self.name = None
|
||||
"""
|
||||
The model name is used for code generation. Type: string. Default: :code:`None`
|
||||
"""
|
||||
self.x = None #: CasADi variable describing the state of the system; Default: :code:`None`
|
||||
self.xdot = None #: CasADi variable describing the derivative of the state wrt time; Default: :code:`None`
|
||||
self.u = None #: CasADi variable describing the input of the system; Default: :code:`None`
|
||||
self.z = [] #: CasADi variable describing the algebraic variables of the DAE; Default: :code:`empty`
|
||||
self.p = [] #: CasADi variable describing parameters of the DAE; Default: :code:`empty`
|
||||
# dynamics
|
||||
self.f_impl_expr = None
|
||||
"""
|
||||
CasADi expression for the implicit dynamics :math:`f_\\text{impl}(\dot{x}, x, u, z, p) = 0`.
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'IRK'.
|
||||
Default: :code:`None`
|
||||
"""
|
||||
self.f_expl_expr = None
|
||||
"""
|
||||
CasADi expression for the explicit dynamics :math:`\dot{x} = f_\\text{expl}(x, u, p)`.
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'ERK'.
|
||||
Default: :code:`None`
|
||||
"""
|
||||
self.disc_dyn_expr = None
|
||||
"""
|
||||
CasADi expression for the discrete dynamics :math:`x_{+} = f_\\text{disc}(x, u, p)`.
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'DISCRETE'.
|
||||
Default: :code:`None`
|
||||
"""
|
||||
|
||||
self.dyn_ext_fun_type = 'casadi' #: type of external functions for dynamics module; 'casadi' or 'generic'; Default: 'casadi'
|
||||
self.dyn_generic_source = None #: name of source file for discrete dyanamics; Default: :code:`None`
|
||||
self.dyn_disc_fun_jac_hess = None #: name of function discrete dyanamics + jacobian and hessian; Default: :code:`None`
|
||||
self.dyn_disc_fun_jac = None #: name of function discrete dyanamics + jacobian; Default: :code:`None`
|
||||
self.dyn_disc_fun = None #: name of function discrete dyanamics; Default: :code:`None`
|
||||
|
||||
# for GNSF models
|
||||
self.gnsf = {'nontrivial_f_LO': 1, 'purely_linear': 0}
|
||||
"""
|
||||
dictionary containing information on GNSF structure needed when rendering templates.
|
||||
Contains integers `nontrivial_f_LO`, `purely_linear`.
|
||||
"""
|
||||
|
||||
## for OCP
|
||||
# constraints
|
||||
# BGH(default): lh <= h(x, u) <= uh
|
||||
self.con_h_expr = None #: CasADi expression for the constraint :math:`h`; Default: :code:`None`
|
||||
# BGP(convex over nonlinear): lphi <= phi(r(x, u)) <= uphi
|
||||
self.con_phi_expr = None #: CasADi expression for the constraint phi; Default: :code:`None`
|
||||
self.con_r_expr = None #: CasADi expression for the constraint phi(r); Default: :code:`None`
|
||||
self.con_r_in_phi = None
|
||||
# terminal
|
||||
self.con_h_expr_e = None #: CasADi expression for the terminal constraint :math:`h^e`; Default: :code:`None`
|
||||
self.con_r_expr_e = None #: CasADi expression for the terminal constraint; Default: :code:`None`
|
||||
self.con_phi_expr_e = None #: CasADi expression for the terminal constraint; Default: :code:`None`
|
||||
self.con_r_in_phi_e = None
|
||||
# cost
|
||||
self.cost_y_expr = None #: CasADi expression for nonlinear least squares; Default: :code:`None`
|
||||
self.cost_y_expr_e = None #: CasADi expression for nonlinear least squares, terminal; Default: :code:`None`
|
||||
self.cost_y_expr_0 = None #: CasADi expression for nonlinear least squares, initial; Default: :code:`None`
|
||||
self.cost_expr_ext_cost = None #: CasADi expression for external cost; Default: :code:`None`
|
||||
self.cost_expr_ext_cost_e = None #: CasADi expression for external cost, terminal; Default: :code:`None`
|
||||
self.cost_expr_ext_cost_0 = None #: CasADi expression for external cost, initial; Default: :code:`None`
|
||||
self.cost_expr_ext_cost_custom_hess = None #: CasADi expression for custom hessian (only for external cost); Default: :code:`None`
|
||||
self.cost_expr_ext_cost_custom_hess_e = None #: CasADi expression for custom hessian (only for external cost), terminal; Default: :code:`None`
|
||||
self.cost_expr_ext_cost_custom_hess_0 = None #: CasADi expression for custom hessian (only for external cost), initial; Default: :code:`None`
|
||||
|
||||
## CONVEX_OVER_NONLINEAR convex-over-nonlinear cost: psi(y(x, u, p) - y_ref; p)
|
||||
self.cost_psi_expr_0 = None
|
||||
"""
|
||||
CasADi expression for the outer loss function :math:`\psi(r, p)`, initial; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_psi_expr = None
|
||||
"""
|
||||
CasADi expression for the outer loss function :math:`\psi(r, p)`; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_psi_expr_e = None
|
||||
"""
|
||||
CasADi expression for the outer loss function :math:`\psi(r, p)`, terminal; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_r_in_psi_expr_0 = None
|
||||
"""
|
||||
CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`, initial; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_r_in_psi_expr = None
|
||||
"""
|
||||
CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_r_in_psi_expr_e = None
|
||||
"""
|
||||
CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`, terminal; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_conl_custom_outer_hess_0 = None
|
||||
"""
|
||||
CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), initial; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_conl_custom_outer_hess = None
|
||||
"""
|
||||
CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost); Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
self.cost_conl_custom_outer_hess_e = None
|
||||
"""
|
||||
CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), terminal; Default: :code:`None`
|
||||
Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'.
|
||||
"""
|
||||
3167
third_party/acados/acados_template/acados_ocp.py
vendored
Normal file
3167
third_party/acados/acados_template/acados_ocp.py
vendored
Normal file
File diff suppressed because it is too large
Load Diff
1991
third_party/acados/acados_template/acados_ocp_solver.py
vendored
Normal file
1991
third_party/acados/acados_template/acados_ocp_solver.py
vendored
Normal file
File diff suppressed because it is too large
Load Diff
795
third_party/acados/acados_template/acados_ocp_solver_pyx.pyx
vendored
Normal file
795
third_party/acados/acados_template/acados_ocp_solver_pyx.pyx
vendored
Normal file
@@ -0,0 +1,795 @@
|
||||
# -*- coding: future_fstrings -*-
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
# cython: language_level=3
|
||||
# cython: profile=False
|
||||
# distutils: language=c
|
||||
|
||||
cimport cython
|
||||
from libc cimport string
|
||||
|
||||
cimport acados_solver_common
|
||||
# TODO: make this import more clear? it is not a general solver, but problem specific.
|
||||
cimport acados_solver
|
||||
|
||||
cimport numpy as cnp
|
||||
|
||||
import os
|
||||
from datetime import datetime
|
||||
import numpy as np
|
||||
|
||||
|
||||
cdef class AcadosOcpSolverCython:
|
||||
"""
|
||||
Class to interact with the acados ocp solver C object.
|
||||
"""
|
||||
|
||||
cdef acados_solver.nlp_solver_capsule *capsule
|
||||
cdef void *nlp_opts
|
||||
cdef acados_solver_common.ocp_nlp_dims *nlp_dims
|
||||
cdef acados_solver_common.ocp_nlp_config *nlp_config
|
||||
cdef acados_solver_common.ocp_nlp_out *nlp_out
|
||||
cdef acados_solver_common.ocp_nlp_out *sens_out
|
||||
cdef acados_solver_common.ocp_nlp_in *nlp_in
|
||||
cdef acados_solver_common.ocp_nlp_solver *nlp_solver
|
||||
|
||||
cdef bint solver_created
|
||||
|
||||
cdef str model_name
|
||||
cdef int N
|
||||
|
||||
cdef str nlp_solver_type
|
||||
|
||||
def __cinit__(self, model_name, nlp_solver_type, N):
|
||||
|
||||
self.solver_created = False
|
||||
|
||||
self.N = N
|
||||
self.model_name = model_name
|
||||
self.nlp_solver_type = nlp_solver_type
|
||||
|
||||
# create capsule
|
||||
self.capsule = acados_solver.acados_create_capsule()
|
||||
|
||||
# create solver
|
||||
assert acados_solver.acados_create(self.capsule) == 0
|
||||
self.solver_created = True
|
||||
|
||||
# get pointers solver
|
||||
self.__get_pointers_solver()
|
||||
|
||||
|
||||
def __get_pointers_solver(self):
|
||||
"""
|
||||
Private function to get the pointers for solver
|
||||
"""
|
||||
# get pointers solver
|
||||
self.nlp_opts = acados_solver.acados_get_nlp_opts(self.capsule)
|
||||
self.nlp_dims = acados_solver.acados_get_nlp_dims(self.capsule)
|
||||
self.nlp_config = acados_solver.acados_get_nlp_config(self.capsule)
|
||||
self.nlp_out = acados_solver.acados_get_nlp_out(self.capsule)
|
||||
self.sens_out = acados_solver.acados_get_sens_out(self.capsule)
|
||||
self.nlp_in = acados_solver.acados_get_nlp_in(self.capsule)
|
||||
self.nlp_solver = acados_solver.acados_get_nlp_solver(self.capsule)
|
||||
|
||||
|
||||
def solve_for_x0(self, x0_bar):
|
||||
"""
|
||||
Wrapper around `solve()` which sets initial state constraint, solves the OCP, and returns u0.
|
||||
"""
|
||||
self.set(0, "lbx", x0_bar)
|
||||
self.set(0, "ubx", x0_bar)
|
||||
|
||||
status = self.solve()
|
||||
|
||||
if status == 2:
|
||||
print("Warning: acados_ocp_solver reached maximum iterations.")
|
||||
elif status != 0:
|
||||
raise Exception(f'acados acados_ocp_solver returned status {status}')
|
||||
|
||||
u0 = self.get(0, "u")
|
||||
return u0
|
||||
|
||||
|
||||
def solve(self):
|
||||
"""
|
||||
Solve the ocp with current input.
|
||||
"""
|
||||
return acados_solver.acados_solve(self.capsule)
|
||||
|
||||
|
||||
def reset(self, reset_qp_solver_mem=1):
|
||||
"""
|
||||
Sets current iterate to all zeros.
|
||||
"""
|
||||
return acados_solver.acados_reset(self.capsule, reset_qp_solver_mem)
|
||||
|
||||
|
||||
def custom_update(self, data_):
|
||||
"""
|
||||
A custom function that can be implemented by a user to be called between solver calls.
|
||||
By default this does nothing.
|
||||
The idea is to have a convenient wrapper to do complex updates of parameters and numerical data efficiently in C,
|
||||
in a function that is compiled into the solver library and can be conveniently used in the Python environment.
|
||||
"""
|
||||
data_len = len(data_)
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] data = np.ascontiguousarray(data_, dtype=np.float64)
|
||||
|
||||
return acados_solver.acados_custom_update(self.capsule, <double *> data.data, data_len)
|
||||
|
||||
|
||||
def set_new_time_steps(self, new_time_steps):
|
||||
"""
|
||||
Set new time steps.
|
||||
Recreates the solver if N changes.
|
||||
|
||||
:param new_time_steps: 1 dimensional np array of new time steps for the solver
|
||||
|
||||
.. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
|
||||
the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
|
||||
do not require a new code export and compilation.
|
||||
"""
|
||||
|
||||
raise NotImplementedError("AcadosOcpSolverCython: does not support set_new_time_steps() since it is only a prototyping feature")
|
||||
# # unlikely but still possible
|
||||
# if not self.solver_created:
|
||||
# raise Exception('Solver was not yet created!')
|
||||
|
||||
# ## check if time steps really changed in value
|
||||
# # get time steps
|
||||
# cdef cnp.ndarray[cnp.float64_t, ndim=1] old_time_steps = np.ascontiguousarray(np.zeros((self.N,)), dtype=np.float64)
|
||||
# assert acados_solver.acados_get_time_steps(self.capsule, self.N, <double *> old_time_steps.data)
|
||||
|
||||
# if np.array_equal(old_time_steps, new_time_steps):
|
||||
# return
|
||||
|
||||
# N = new_time_steps.size
|
||||
# cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(new_time_steps, dtype=np.float64)
|
||||
|
||||
# # check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
|
||||
# if len(old_time_steps) == N:
|
||||
# assert acados_solver.acados_update_time_steps(self.capsule, N, <double *> value.data) == 0
|
||||
|
||||
# else: # recreate the solver with the new time steps
|
||||
# self.solver_created = False
|
||||
|
||||
# # delete old memory (analog to __del__)
|
||||
# acados_solver.acados_free(self.capsule)
|
||||
|
||||
# # create solver with new time steps
|
||||
# assert acados_solver.acados_create_with_discretization(self.capsule, N, <double *> value.data) == 0
|
||||
|
||||
# self.solver_created = True
|
||||
|
||||
# # get pointers solver
|
||||
# self.__get_pointers_solver()
|
||||
|
||||
# # store time_steps, N
|
||||
# self.time_steps = new_time_steps
|
||||
# self.N = N
|
||||
|
||||
|
||||
def update_qp_solver_cond_N(self, qp_solver_cond_N: int):
|
||||
"""
|
||||
Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver.
|
||||
This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or
|
||||
the influence of a different `qp_solver_cond_N` is studied without code export and compilation.
|
||||
:param qp_solver_cond_N: new number of condensing stages for the solver
|
||||
|
||||
.. note:: This function can only be used in combination with a partial condensing QP solver.
|
||||
|
||||
.. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be
|
||||
necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically
|
||||
`qp_solver_cond_N < N`.
|
||||
"""
|
||||
raise NotImplementedError("AcadosOcpSolverCython: does not support update_qp_solver_cond_N() since it is only a prototyping feature")
|
||||
|
||||
# # unlikely but still possible
|
||||
# if not self.solver_created:
|
||||
# raise Exception('Solver was not yet created!')
|
||||
# if self.N < qp_solver_cond_N:
|
||||
# raise Exception('Setting qp_solver_cond_N to be larger than N does not work!')
|
||||
# if self.qp_solver_cond_N != qp_solver_cond_N:
|
||||
# self.solver_created = False
|
||||
|
||||
# # recreate the solver
|
||||
# acados_solver.acados_update_qp_solver_cond_N(self.capsule, qp_solver_cond_N)
|
||||
|
||||
# # store the new value
|
||||
# self.qp_solver_cond_N = qp_solver_cond_N
|
||||
# self.solver_created = True
|
||||
|
||||
# # get pointers solver
|
||||
# self.__get_pointers_solver()
|
||||
|
||||
|
||||
def eval_param_sens(self, index, stage=0, field="ex"):
|
||||
"""
|
||||
Calculate the sensitivity of the curent solution with respect to the initial state component of index
|
||||
|
||||
:param index: integer corresponding to initial state index in range(nx)
|
||||
"""
|
||||
|
||||
field_ = field
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
# checks
|
||||
if not isinstance(index, int):
|
||||
raise Exception('AcadosOcpSolverCython.eval_param_sens(): index must be Integer.')
|
||||
|
||||
cdef int nx = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8'))
|
||||
|
||||
if index < 0 or index > nx:
|
||||
raise Exception(f'AcadosOcpSolverCython.eval_param_sens(): index must be in [0, nx-1], got: {index}.')
|
||||
|
||||
# actual eval_param
|
||||
acados_solver_common.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out)
|
||||
|
||||
return
|
||||
|
||||
|
||||
def get(self, int stage, str field_):
|
||||
"""
|
||||
Get the last solution of the solver:
|
||||
|
||||
:param stage: integer corresponding to shooting node
|
||||
:param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',]
|
||||
|
||||
.. note:: regarding lam, t: \n
|
||||
the inequalities are internally organized in the following order: \n
|
||||
[ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
|
||||
lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
|
||||
|
||||
.. note:: pi: multipliers for dynamics equality constraints \n
|
||||
lam: multipliers for inequalities \n
|
||||
t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
|
||||
sl: slack variables of soft lower inequality constraints \n
|
||||
su: slack variables of soft upper inequality constraints \n
|
||||
"""
|
||||
|
||||
out_fields = ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su']
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
if field_ not in out_fields:
|
||||
raise Exception('AcadosOcpSolverCython.get(): {} is an invalid argument.\
|
||||
\n Possible values are {}.'.format(field_, out_fields))
|
||||
|
||||
if stage < 0 or stage > self.N:
|
||||
raise Exception('AcadosOcpSolverCython.get(): stage index must be in [0, N], got: {}.'.format(self.N))
|
||||
|
||||
if stage == self.N and field_ == 'pi':
|
||||
raise Exception('AcadosOcpSolverCython.get(): field {} does not exist at final stage {}.'\
|
||||
.format(field_, stage))
|
||||
|
||||
cdef int dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_out, stage, field)
|
||||
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((dims,))
|
||||
acados_solver_common.ocp_nlp_out_get(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, stage, field, <void *> out.data)
|
||||
|
||||
return out
|
||||
|
||||
|
||||
def print_statistics(self):
|
||||
"""
|
||||
prints statistics of previous solver run as a table:
|
||||
- iter: iteration number
|
||||
- res_stat: stationarity residual
|
||||
- res_eq: residual wrt equality constraints (dynamics)
|
||||
- res_ineq: residual wrt inequality constraints (constraints)
|
||||
- res_comp: residual wrt complementarity conditions
|
||||
- qp_stat: status of QP solver
|
||||
- qp_iter: number of QP iterations
|
||||
- qp_res_stat: stationarity residual of the last QP solution
|
||||
- qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution
|
||||
- qp_res_ineq: residual wrt inequality constraints (constraints) of the last QP solution
|
||||
- qp_res_comp: residual wrt complementarity conditions of the last QP solution
|
||||
"""
|
||||
acados_solver.acados_print_stats(self.capsule)
|
||||
|
||||
|
||||
def store_iterate(self, filename='', overwrite=False):
|
||||
"""
|
||||
Stores the current iterate of the ocp solver in a json file.
|
||||
|
||||
:param filename: if not set, use model_name + timestamp + '.json'
|
||||
:param overwrite: if false and filename exists add timestamp to filename
|
||||
"""
|
||||
import json
|
||||
if filename == '':
|
||||
filename += self.model_name + '_' + 'iterate' + '.json'
|
||||
|
||||
if not overwrite:
|
||||
# append timestamp
|
||||
if os.path.isfile(filename):
|
||||
filename = filename[:-5]
|
||||
filename += datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f') + '.json'
|
||||
|
||||
# get iterate:
|
||||
solution = dict()
|
||||
|
||||
lN = len(str(self.N+1))
|
||||
for i in range(self.N+1):
|
||||
i_string = f'{i:0{lN}d}'
|
||||
solution['x_'+i_string] = self.get(i,'x')
|
||||
solution['u_'+i_string] = self.get(i,'u')
|
||||
solution['z_'+i_string] = self.get(i,'z')
|
||||
solution['lam_'+i_string] = self.get(i,'lam')
|
||||
solution['t_'+i_string] = self.get(i, 't')
|
||||
solution['sl_'+i_string] = self.get(i, 'sl')
|
||||
solution['su_'+i_string] = self.get(i, 'su')
|
||||
if i < self.N:
|
||||
solution['pi_'+i_string] = self.get(i,'pi')
|
||||
|
||||
for k in list(solution.keys()):
|
||||
if len(solution[k]) == 0:
|
||||
del solution[k]
|
||||
|
||||
# save
|
||||
with open(filename, 'w') as f:
|
||||
json.dump(solution, f, default=lambda x: x.tolist(), indent=4, sort_keys=True)
|
||||
print("stored current iterate in ", os.path.join(os.getcwd(), filename))
|
||||
|
||||
|
||||
def load_iterate(self, filename):
|
||||
"""
|
||||
Loads the iterate stored in json file with filename into the ocp solver.
|
||||
"""
|
||||
import json
|
||||
if not os.path.isfile(filename):
|
||||
raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename))
|
||||
|
||||
with open(filename, 'r') as f:
|
||||
solution = json.load(f)
|
||||
|
||||
for key in solution.keys():
|
||||
(field, stage) = key.split('_')
|
||||
self.set(int(stage), field, np.array(solution[key]))
|
||||
|
||||
|
||||
def get_stats(self, field_):
|
||||
"""
|
||||
Get the information of the last solver call.
|
||||
|
||||
:param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
|
||||
Available fileds:
|
||||
- time_tot: total CPU time previous call
|
||||
- time_lin: CPU time for linearization
|
||||
- time_sim: CPU time for integrator
|
||||
- time_sim_ad: CPU time for integrator contribution of external function calls
|
||||
- time_sim_la: CPU time for integrator contribution of linear algebra
|
||||
- time_qp: CPU time qp solution
|
||||
- time_qp_solver_call: CPU time inside qp solver (without converting the QP)
|
||||
- time_qp_xcond: time_glob: CPU time globalization
|
||||
- time_solution_sensitivities: CPU time for previous call to eval_param_sens
|
||||
- time_reg: CPU time regularization
|
||||
- sqp_iter: number of SQP iterations
|
||||
- qp_iter: vector of QP iterations for last SQP call
|
||||
- statistics: table with info about last iteration
|
||||
- stat_m: number of rows in statistics matrix
|
||||
- stat_n: number of columns in statistics matrix
|
||||
- residuals: residuals of last iterate
|
||||
- alpha: step sizes of SQP iterations
|
||||
"""
|
||||
|
||||
double_fields = ['time_tot',
|
||||
'time_lin',
|
||||
'time_sim',
|
||||
'time_sim_ad',
|
||||
'time_sim_la',
|
||||
'time_qp',
|
||||
'time_qp_solver_call',
|
||||
'time_qp_xcond',
|
||||
'time_glob',
|
||||
'time_solution_sensitivities',
|
||||
'time_reg'
|
||||
]
|
||||
fields = double_fields + [
|
||||
'sqp_iter',
|
||||
'qp_iter',
|
||||
'statistics',
|
||||
'stat_m',
|
||||
'stat_n',
|
||||
'residuals',
|
||||
'alpha',
|
||||
]
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
|
||||
return self.__get_stat_int(field)
|
||||
|
||||
elif field_ in double_fields:
|
||||
return self.__get_stat_double(field)
|
||||
|
||||
elif field_ == 'statistics':
|
||||
sqp_iter = self.get_stats("sqp_iter")
|
||||
stat_m = self.get_stats("stat_m")
|
||||
stat_n = self.get_stats("stat_n")
|
||||
min_size = min([stat_m, sqp_iter+1])
|
||||
return self.__get_stat_matrix(field, stat_n+1, min_size)
|
||||
|
||||
elif field_ == 'qp_iter':
|
||||
full_stats = self.get_stats('statistics')
|
||||
if self.nlp_solver_type == 'SQP':
|
||||
return full_stats[6, :]
|
||||
elif self.nlp_solver_type == 'SQP_RTI':
|
||||
return full_stats[2, :]
|
||||
|
||||
elif field_ == 'alpha':
|
||||
full_stats = self.get_stats('statistics')
|
||||
if self.nlp_solver_type == 'SQP':
|
||||
return full_stats[7, :]
|
||||
else: # self.nlp_solver_type == 'SQP_RTI':
|
||||
raise Exception("alpha values are not available for SQP_RTI")
|
||||
|
||||
elif field_ == 'residuals':
|
||||
return self.get_residuals()
|
||||
|
||||
else:
|
||||
raise NotImplementedError("TODO!")
|
||||
|
||||
|
||||
def __get_stat_int(self, field):
|
||||
cdef int out
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &out)
|
||||
return out
|
||||
|
||||
def __get_stat_double(self, field):
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((1,))
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out.data)
|
||||
return out
|
||||
|
||||
def __get_stat_matrix(self, field, n, m):
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=2] out_mat = np.ascontiguousarray(np.zeros((n, m)), dtype=np.float64)
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out_mat.data)
|
||||
return out_mat
|
||||
|
||||
|
||||
def get_cost(self):
|
||||
"""
|
||||
Returns the cost value of the current solution.
|
||||
"""
|
||||
# compute cost internally
|
||||
acados_solver_common.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out)
|
||||
|
||||
# create output
|
||||
cdef double out
|
||||
|
||||
# call getter
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, "cost_value", <void *> &out)
|
||||
|
||||
return out
|
||||
|
||||
|
||||
def get_residuals(self, recompute=False):
|
||||
"""
|
||||
Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
|
||||
"""
|
||||
# compute residuals if RTI
|
||||
if self.nlp_solver_type == 'SQP_RTI' or recompute:
|
||||
acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
|
||||
|
||||
# create output array
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((4,), dtype=np.float64))
|
||||
cdef double double_value
|
||||
|
||||
field = "res_stat".encode('utf-8')
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
|
||||
out[0] = double_value
|
||||
|
||||
field = "res_eq".encode('utf-8')
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
|
||||
out[1] = double_value
|
||||
|
||||
field = "res_ineq".encode('utf-8')
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
|
||||
out[2] = double_value
|
||||
|
||||
field = "res_comp".encode('utf-8')
|
||||
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> &double_value)
|
||||
out[3] = double_value
|
||||
|
||||
return out
|
||||
|
||||
|
||||
# Note: this function should not be used anymore, better use cost_set, constraints_set
|
||||
def set(self, int stage, str field_, value_):
|
||||
|
||||
"""
|
||||
Set numerical data inside the solver.
|
||||
|
||||
:param stage: integer corresponding to shooting node
|
||||
:param field: string in ['x', 'u', 'pi', 'lam', 't', 'p']
|
||||
|
||||
.. note:: regarding lam, t: \n
|
||||
the inequalities are internally organized in the following order: \n
|
||||
[ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
|
||||
lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
|
||||
|
||||
.. note:: pi: multipliers for dynamics equality constraints \n
|
||||
lam: multipliers for inequalities \n
|
||||
t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
|
||||
sl: slack variables of soft lower inequality constraints \n
|
||||
su: slack variables of soft upper inequality constraints \n
|
||||
"""
|
||||
if not isinstance(value_, np.ndarray):
|
||||
raise Exception(f"set: value must be numpy array, got {type(value_)}.")
|
||||
cost_fields = ['y_ref', 'yref']
|
||||
constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
|
||||
out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
|
||||
mem_fields = ['xdot_guess', 'z_guess']
|
||||
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(value_, dtype=np.float64)
|
||||
|
||||
# treat parameters separately
|
||||
if field_ == 'p':
|
||||
assert acados_solver.acados_update_params(self.capsule, stage, <double *> value.data, value.shape[0]) == 0
|
||||
else:
|
||||
if field_ not in constraints_fields + cost_fields + out_fields:
|
||||
raise Exception("AcadosOcpSolverCython.set(): {} is not a valid argument.\
|
||||
\nPossible values are {}.".format(field, \
|
||||
constraints_fields + cost_fields + out_fields + ['p']))
|
||||
|
||||
dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_out, stage, field)
|
||||
|
||||
if value_.shape[0] != dims:
|
||||
msg = 'AcadosOcpSolverCython.set(): mismatching dimension for field "{}" '.format(field_)
|
||||
msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0])
|
||||
raise Exception(msg)
|
||||
|
||||
if field_ in constraints_fields:
|
||||
acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_in, stage, field, <void *> value.data)
|
||||
elif field_ in cost_fields:
|
||||
acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_in, stage, field, <void *> value.data)
|
||||
elif field_ in out_fields:
|
||||
acados_solver_common.ocp_nlp_out_set(self.nlp_config,
|
||||
self.nlp_dims, self.nlp_out, stage, field, <void *> value.data)
|
||||
elif field_ in mem_fields:
|
||||
acados_solver_common.ocp_nlp_set(self.nlp_config, \
|
||||
self.nlp_solver, stage, field, <void *> value.data)
|
||||
|
||||
if field_ == 'z':
|
||||
field = 'z_guess'.encode('utf-8')
|
||||
acados_solver_common.ocp_nlp_set(self.nlp_config, \
|
||||
self.nlp_solver, stage, field, <void *> value.data)
|
||||
return
|
||||
|
||||
def cost_set(self, int stage, str field_, value_):
|
||||
"""
|
||||
Set numerical data in the cost module of the solver.
|
||||
|
||||
:param stage: integer corresponding to shooting node
|
||||
:param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess'
|
||||
:param value: of appropriate size
|
||||
"""
|
||||
if not isinstance(value_, np.ndarray):
|
||||
raise Exception(f"cost_set: value must be numpy array, got {type(value_)}.")
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
cdef int dims[2]
|
||||
acados_solver_common.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, stage, field, &dims[0])
|
||||
|
||||
cdef double[::1,:] value
|
||||
|
||||
value_shape = value_.shape
|
||||
if len(value_shape) == 1:
|
||||
value_shape = (value_shape[0], 0)
|
||||
value = np.asfortranarray(value_[None,:])
|
||||
|
||||
elif len(value_shape) == 2:
|
||||
# Get elements in column major order
|
||||
value = np.asfortranarray(value_)
|
||||
|
||||
if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
|
||||
raise Exception('AcadosOcpSolverCython.cost_set(): mismatching dimension' +
|
||||
f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
|
||||
|
||||
acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
|
||||
|
||||
|
||||
def constraints_set(self, int stage, str field_, value_):
|
||||
"""
|
||||
Set numerical data in the constraint module of the solver.
|
||||
|
||||
:param stage: integer corresponding to shooting node
|
||||
:param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D']
|
||||
:param value: of appropriate size
|
||||
"""
|
||||
if not isinstance(value_, np.ndarray):
|
||||
raise Exception(f"constraints_set: value must be numpy array, got {type(value_)}.")
|
||||
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
cdef int dims[2]
|
||||
acados_solver_common.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_out, stage, field, &dims[0])
|
||||
|
||||
cdef double[::1,:] value
|
||||
|
||||
value_shape = value_.shape
|
||||
if len(value_shape) == 1:
|
||||
value_shape = (value_shape[0], 0)
|
||||
value = np.asfortranarray(value_[None,:])
|
||||
|
||||
elif len(value_shape) == 2:
|
||||
# Get elements in column major order
|
||||
value = np.asfortranarray(value_)
|
||||
|
||||
if value_shape != tuple(dims):
|
||||
raise Exception(f'AcadosOcpSolverCython.constraints_set(): mismatching dimension' +
|
||||
f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
|
||||
|
||||
acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \
|
||||
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
|
||||
|
||||
return
|
||||
|
||||
|
||||
def get_from_qp_in(self, int stage, str field_):
|
||||
"""
|
||||
Get numerical data from the dynamics module of the solver:
|
||||
|
||||
:param stage: integer corresponding to shooting node
|
||||
:param field: string, e.g. 'A'
|
||||
"""
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
# get dims
|
||||
cdef int[2] dims
|
||||
acados_solver_common.ocp_nlp_qp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0])
|
||||
|
||||
# create output data
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F')
|
||||
|
||||
# call getter
|
||||
acados_solver_common.ocp_nlp_get_at_stage(self.nlp_config, self.nlp_dims, self.nlp_solver, stage, field, <void *> out.data)
|
||||
|
||||
return out
|
||||
|
||||
|
||||
def options_set(self, str field_, value_):
|
||||
"""
|
||||
Set options of the solver.
|
||||
|
||||
:param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0'
|
||||
|
||||
:param value: of type int, float, string
|
||||
|
||||
- qp_tol_stat: QP solver tolerance stationarity
|
||||
- qp_tol_eq: QP solver tolerance equalities
|
||||
- qp_tol_ineq: QP solver tolerance inequalities
|
||||
- qp_tol_comp: QP solver tolerance complementarity
|
||||
- qp_tau_min: for HPIPM QP solvers: minimum value of barrier parameter in HPIPM
|
||||
- qp_mu0: for HPIPM QP solvers: initial value for complementarity slackness
|
||||
- warm_start_first_qp: indicates if first QP in SQP is warm_started
|
||||
"""
|
||||
int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'warm_start_first_qp']
|
||||
double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent',
|
||||
'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0']
|
||||
string_fields = ['globalization']
|
||||
|
||||
# encode
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
cdef int int_value
|
||||
cdef double double_value
|
||||
cdef unsigned char[::1] string_value
|
||||
|
||||
# check field availability and type
|
||||
if field_ in int_fields:
|
||||
if not isinstance(value_, int):
|
||||
raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_)))
|
||||
|
||||
if field_ == 'rti_phase':
|
||||
if value_ < 0 or value_ > 2:
|
||||
raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
|
||||
'take only values 0, 1, 2 for SQP-RTI-type solvers')
|
||||
if self.nlp_solver_type != 'SQP_RTI' and value_ > 0:
|
||||
raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
|
||||
'take only value 0 for SQP-type solvers')
|
||||
|
||||
int_value = value_
|
||||
acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &int_value)
|
||||
|
||||
elif field_ in double_fields:
|
||||
if not isinstance(value_, float):
|
||||
raise Exception('solver option {} must be of type float. You have {}.'.format(field_, type(value_)))
|
||||
|
||||
double_value = value_
|
||||
acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &double_value)
|
||||
|
||||
elif field_ in string_fields:
|
||||
if not isinstance(value_, bytes):
|
||||
raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_)))
|
||||
|
||||
string_value = value_.encode('utf-8')
|
||||
acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &string_value[0])
|
||||
|
||||
else:
|
||||
raise Exception('AcadosOcpSolverCython.options_set() does not support field {}.'\
|
||||
'\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
|
||||
|
||||
|
||||
def set_params_sparse(self, int stage, idx_values_, param_values_):
|
||||
"""
|
||||
set parameters of the solvers external function partially:
|
||||
Pseudo: solver.param[idx_values_] = param_values_;
|
||||
Parameters:
|
||||
:param stage_: integer corresponding to shooting node
|
||||
:param idx_values_: 0 based integer array corresponding to parameter indices to be set
|
||||
:param param_values_: new parameter values as numpy array
|
||||
"""
|
||||
|
||||
if not isinstance(param_values_, np.ndarray):
|
||||
raise Exception('param_values_ must be np.array.')
|
||||
|
||||
if param_values_.shape[0] != len(idx_values_):
|
||||
raise Exception(f'param_values_ and idx_values_ must be of the same size.' +
|
||||
f' Got sizes idx {param_values_.shape[0]}, param_values {len(idx_values_)}.')
|
||||
|
||||
# n_update = c_int(len(param_values_))
|
||||
|
||||
# param_data = cast(param_values_.ctypes.data, POINTER(c_double))
|
||||
# c_idx_values = np.ascontiguousarray(idx_values_, dtype=np.intc)
|
||||
# idx_data = cast(c_idx_values.ctypes.data, POINTER(c_int))
|
||||
|
||||
# getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse").argtypes = \
|
||||
# [c_void_p, c_int, POINTER(c_int), POINTER(c_double), c_int]
|
||||
# getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse").restype = c_int
|
||||
# getattr(self.shared_lib, f"{self.model_name}_acados_update_params_sparse") \
|
||||
# (self.capsule, stage, idx_data, param_data, n_update)
|
||||
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(param_values_, dtype=np.float64)
|
||||
# cdef cnp.ndarray[cnp.intc, ndim=1] idx = np.ascontiguousarray(idx_values_, dtype=np.intc)
|
||||
|
||||
# NOTE: this does throw an error somehow:
|
||||
# ValueError: Buffer dtype mismatch, expected 'int object' but got 'int'
|
||||
# cdef cnp.ndarray[cnp.int, ndim=1] idx = np.ascontiguousarray(idx_values_, dtype=np.intc)
|
||||
|
||||
cdef cnp.ndarray[cnp.int32_t, ndim=1] idx = np.ascontiguousarray(idx_values_, dtype=np.int32)
|
||||
cdef int n_update = value.shape[0]
|
||||
# print(f"in set_params_sparse Cython n_update {n_update}")
|
||||
|
||||
assert acados_solver.acados_update_params_sparse(self.capsule, stage, <int *> idx.data, <double *> value.data, n_update) == 0
|
||||
return
|
||||
|
||||
|
||||
def __del__(self):
|
||||
if self.solver_created:
|
||||
acados_solver.acados_free(self.capsule)
|
||||
acados_solver.acados_free_capsule(self.capsule)
|
||||
367
third_party/acados/acados_template/acados_sim.py
vendored
Normal file
367
third_party/acados/acados_template/acados_sim.py
vendored
Normal file
@@ -0,0 +1,367 @@
|
||||
# -*- coding: future_fstrings -*-
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
import numpy as np
|
||||
import os
|
||||
from .acados_model import AcadosModel
|
||||
from .utils import get_acados_path, get_lib_ext
|
||||
|
||||
class AcadosSimDims:
|
||||
"""
|
||||
Class containing the dimensions of the model to be simulated.
|
||||
"""
|
||||
def __init__(self):
|
||||
self.__nx = None
|
||||
self.__nu = None
|
||||
self.__nz = 0
|
||||
self.__np = 0
|
||||
|
||||
@property
|
||||
def nx(self):
|
||||
""":math:`n_x` - number of states. Type: int > 0"""
|
||||
return self.__nx
|
||||
|
||||
@property
|
||||
def nz(self):
|
||||
""":math:`n_z` - number of algebraic variables. Type: int >= 0"""
|
||||
return self.__nz
|
||||
|
||||
@property
|
||||
def nu(self):
|
||||
""":math:`n_u` - number of inputs. Type: int >= 0"""
|
||||
return self.__nu
|
||||
|
||||
@property
|
||||
def np(self):
|
||||
""":math:`n_p` - number of parameters. Type: int >= 0"""
|
||||
return self.__np
|
||||
|
||||
@nx.setter
|
||||
def nx(self, nx):
|
||||
if isinstance(nx, int) and nx > 0:
|
||||
self.__nx = nx
|
||||
else:
|
||||
raise Exception('Invalid nx value, expected positive integer.')
|
||||
|
||||
@nz.setter
|
||||
def nz(self, nz):
|
||||
if isinstance(nz, int) and nz > -1:
|
||||
self.__nz = nz
|
||||
else:
|
||||
raise Exception('Invalid nz value, expected nonnegative integer.')
|
||||
|
||||
@nu.setter
|
||||
def nu(self, nu):
|
||||
if isinstance(nu, int) and nu > -1:
|
||||
self.__nu = nu
|
||||
else:
|
||||
raise Exception('Invalid nu value, expected nonnegative integer.')
|
||||
|
||||
@np.setter
|
||||
def np(self, np):
|
||||
if isinstance(np, int) and np > -1:
|
||||
self.__np = np
|
||||
else:
|
||||
raise Exception('Invalid np value, expected nonnegative integer.')
|
||||
|
||||
def set(self, attr, value):
|
||||
setattr(self, attr, value)
|
||||
|
||||
|
||||
class AcadosSimOpts:
|
||||
"""
|
||||
class containing the solver options
|
||||
"""
|
||||
def __init__(self):
|
||||
self.__integrator_type = 'ERK'
|
||||
self.__collocation_type = 'GAUSS_LEGENDRE'
|
||||
self.__Tsim = None
|
||||
# ints
|
||||
self.__sim_method_num_stages = 1
|
||||
self.__sim_method_num_steps = 1
|
||||
self.__sim_method_newton_iter = 3
|
||||
# doubles
|
||||
self.__sim_method_newton_tol = 0.0
|
||||
# bools
|
||||
self.__sens_forw = True
|
||||
self.__sens_adj = False
|
||||
self.__sens_algebraic = False
|
||||
self.__sens_hess = False
|
||||
self.__output_z = True
|
||||
self.__sim_method_jac_reuse = 0
|
||||
self.__ext_fun_compile_flags = '-O2'
|
||||
|
||||
@property
|
||||
def integrator_type(self):
|
||||
"""Integrator type. Default: 'ERK'."""
|
||||
return self.__integrator_type
|
||||
|
||||
@property
|
||||
def num_stages(self):
|
||||
"""Number of stages in the integrator. Default: 1"""
|
||||
return self.__sim_method_num_stages
|
||||
|
||||
@property
|
||||
def num_steps(self):
|
||||
"""Number of steps in the integrator. Default: 1"""
|
||||
return self.__sim_method_num_steps
|
||||
|
||||
@property
|
||||
def newton_iter(self):
|
||||
"""Number of Newton iterations in simulation method. Default: 3"""
|
||||
return self.__sim_method_newton_iter
|
||||
|
||||
@property
|
||||
def newton_tol(self):
|
||||
"""
|
||||
Tolerance for Newton system solved in implicit integrator (IRK, GNSF).
|
||||
0.0 means this is not used and exactly newton_iter iterations are carried out.
|
||||
Default: 0.0
|
||||
"""
|
||||
return self.__sim_method_newton_tol
|
||||
|
||||
@property
|
||||
def sens_forw(self):
|
||||
"""Boolean determining if forward sensitivities are computed. Default: True"""
|
||||
return self.__sens_forw
|
||||
|
||||
@property
|
||||
def sens_adj(self):
|
||||
"""Boolean determining if adjoint sensitivities are computed. Default: False"""
|
||||
return self.__sens_adj
|
||||
|
||||
@property
|
||||
def sens_algebraic(self):
|
||||
"""Boolean determining if sensitivities wrt algebraic variables are computed. Default: False"""
|
||||
return self.__sens_algebraic
|
||||
|
||||
@property
|
||||
def sens_hess(self):
|
||||
"""Boolean determining if hessians are computed. Default: False"""
|
||||
return self.__sens_hess
|
||||
|
||||
@property
|
||||
def output_z(self):
|
||||
"""Boolean determining if values for algebraic variables (corresponding to start of simulation interval) are computed. Default: True"""
|
||||
return self.__output_z
|
||||
|
||||
@property
|
||||
def sim_method_jac_reuse(self):
|
||||
"""Integer determining if jacobians are reused (0 or 1). Default: 0"""
|
||||
return self.__sim_method_jac_reuse
|
||||
|
||||
@property
|
||||
def T(self):
|
||||
"""Time horizon"""
|
||||
return self.__Tsim
|
||||
|
||||
@property
|
||||
def collocation_type(self):
|
||||
"""Collocation type: relevant for implicit integrators
|
||||
-- string in {GAUSS_RADAU_IIA, GAUSS_LEGENDRE}
|
||||
|
||||
Default: GAUSS_LEGENDRE
|
||||
"""
|
||||
return self.__collocation_type
|
||||
|
||||
@property
|
||||
def ext_fun_compile_flags(self):
|
||||
"""
|
||||
String with compiler flags for external function compilation.
|
||||
Default: '-O2'.
|
||||
"""
|
||||
return self.__ext_fun_compile_flags
|
||||
|
||||
@ext_fun_compile_flags.setter
|
||||
def ext_fun_compile_flags(self, ext_fun_compile_flags):
|
||||
if isinstance(ext_fun_compile_flags, str):
|
||||
self.__ext_fun_compile_flags = ext_fun_compile_flags
|
||||
else:
|
||||
raise Exception('Invalid ext_fun_compile_flags, expected a string.\n')
|
||||
|
||||
@integrator_type.setter
|
||||
def integrator_type(self, integrator_type):
|
||||
integrator_types = ('ERK', 'IRK', 'GNSF')
|
||||
if integrator_type in integrator_types:
|
||||
self.__integrator_type = integrator_type
|
||||
else:
|
||||
raise Exception('Invalid integrator_type value. Possible values are:\n\n' \
|
||||
+ ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\n')
|
||||
|
||||
@collocation_type.setter
|
||||
def collocation_type(self, collocation_type):
|
||||
collocation_types = ('GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE')
|
||||
if collocation_type in collocation_types:
|
||||
self.__collocation_type = collocation_type
|
||||
else:
|
||||
raise Exception('Invalid collocation_type value. Possible values are:\n\n' \
|
||||
+ ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\n')
|
||||
|
||||
@T.setter
|
||||
def T(self, T):
|
||||
self.__Tsim = T
|
||||
|
||||
@num_stages.setter
|
||||
def num_stages(self, num_stages):
|
||||
if isinstance(num_stages, int):
|
||||
self.__sim_method_num_stages = num_stages
|
||||
else:
|
||||
raise Exception('Invalid num_stages value. num_stages must be an integer.')
|
||||
|
||||
@num_steps.setter
|
||||
def num_steps(self, num_steps):
|
||||
if isinstance(num_steps, int):
|
||||
self.__sim_method_num_steps = num_steps
|
||||
else:
|
||||
raise Exception('Invalid num_steps value. num_steps must be an integer.')
|
||||
|
||||
@newton_iter.setter
|
||||
def newton_iter(self, newton_iter):
|
||||
if isinstance(newton_iter, int):
|
||||
self.__sim_method_newton_iter = newton_iter
|
||||
else:
|
||||
raise Exception('Invalid newton_iter value. newton_iter must be an integer.')
|
||||
|
||||
@newton_tol.setter
|
||||
def newton_tol(self, newton_tol):
|
||||
if isinstance(newton_tol, float):
|
||||
self.__sim_method_newton_tol = newton_tol
|
||||
else:
|
||||
raise Exception('Invalid newton_tol value. newton_tol must be an float.')
|
||||
|
||||
@sens_forw.setter
|
||||
def sens_forw(self, sens_forw):
|
||||
if sens_forw in (True, False):
|
||||
self.__sens_forw = sens_forw
|
||||
else:
|
||||
raise Exception('Invalid sens_forw value. sens_forw must be a Boolean.')
|
||||
|
||||
@sens_adj.setter
|
||||
def sens_adj(self, sens_adj):
|
||||
if sens_adj in (True, False):
|
||||
self.__sens_adj = sens_adj
|
||||
else:
|
||||
raise Exception('Invalid sens_adj value. sens_adj must be a Boolean.')
|
||||
|
||||
@sens_hess.setter
|
||||
def sens_hess(self, sens_hess):
|
||||
if sens_hess in (True, False):
|
||||
self.__sens_hess = sens_hess
|
||||
else:
|
||||
raise Exception('Invalid sens_hess value. sens_hess must be a Boolean.')
|
||||
|
||||
@sens_algebraic.setter
|
||||
def sens_algebraic(self, sens_algebraic):
|
||||
if sens_algebraic in (True, False):
|
||||
self.__sens_algebraic = sens_algebraic
|
||||
else:
|
||||
raise Exception('Invalid sens_algebraic value. sens_algebraic must be a Boolean.')
|
||||
|
||||
@output_z.setter
|
||||
def output_z(self, output_z):
|
||||
if output_z in (True, False):
|
||||
self.__output_z = output_z
|
||||
else:
|
||||
raise Exception('Invalid output_z value. output_z must be a Boolean.')
|
||||
|
||||
@sim_method_jac_reuse.setter
|
||||
def sim_method_jac_reuse(self, sim_method_jac_reuse):
|
||||
if sim_method_jac_reuse in (0, 1):
|
||||
self.__sim_method_jac_reuse = sim_method_jac_reuse
|
||||
else:
|
||||
raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be 0 or 1.')
|
||||
|
||||
class AcadosSim:
|
||||
"""
|
||||
The class has the following properties that can be modified to formulate a specific simulation problem, see below:
|
||||
|
||||
:param acados_path: string with the path to acados. It is used to generate the include and lib paths.
|
||||
|
||||
- :py:attr:`dims` of type :py:class:`acados_template.acados_ocp.AcadosSimDims` - are automatically detected from model
|
||||
- :py:attr:`model` of type :py:class:`acados_template.acados_model.AcadosModel`
|
||||
- :py:attr:`solver_options` of type :py:class:`acados_template.acados_sim.AcadosSimOpts`
|
||||
|
||||
- :py:attr:`acados_include_path` (set automatically)
|
||||
- :py:attr:`shared_lib_ext` (set automatically)
|
||||
- :py:attr:`acados_lib_path` (set automatically)
|
||||
- :py:attr:`parameter_values` - used to initialize the parameters (can be changed)
|
||||
|
||||
"""
|
||||
def __init__(self, acados_path=''):
|
||||
if acados_path == '':
|
||||
acados_path = get_acados_path()
|
||||
self.dims = AcadosSimDims()
|
||||
"""Dimension definitions, automatically detected from :py:attr:`model`. Type :py:class:`acados_template.acados_sim.AcadosSimDims`"""
|
||||
self.model = AcadosModel()
|
||||
"""Model definitions, type :py:class:`acados_template.acados_model.AcadosModel`"""
|
||||
self.solver_options = AcadosSimOpts()
|
||||
"""Solver Options, type :py:class:`acados_template.acados_sim.AcadosSimOpts`"""
|
||||
|
||||
self.acados_include_path = os.path.join(acados_path, 'include').replace(os.sep, '/') # the replace part is important on Windows for CMake
|
||||
"""Path to acados include directory (set automatically), type: `string`"""
|
||||
self.acados_lib_path = os.path.join(acados_path, 'lib').replace(os.sep, '/') # the replace part is important on Windows for CMake
|
||||
"""Path to where acados library is located (set automatically), type: `string`"""
|
||||
|
||||
self.code_export_directory = 'c_generated_code'
|
||||
"""Path to where code will be exported. Default: `c_generated_code`."""
|
||||
self.shared_lib_ext = get_lib_ext()
|
||||
|
||||
# get cython paths
|
||||
from sysconfig import get_paths
|
||||
self.cython_include_dirs = [np.get_include(), get_paths()['include']]
|
||||
|
||||
self.__parameter_values = np.array([])
|
||||
self.__problem_class = 'SIM'
|
||||
|
||||
@property
|
||||
def parameter_values(self):
|
||||
""":math:`p` - initial values for parameter - can be updated"""
|
||||
return self.__parameter_values
|
||||
|
||||
@parameter_values.setter
|
||||
def parameter_values(self, parameter_values):
|
||||
if isinstance(parameter_values, np.ndarray):
|
||||
self.__parameter_values = parameter_values
|
||||
else:
|
||||
raise Exception('Invalid parameter_values value. ' +
|
||||
f'Expected numpy array, got {type(parameter_values)}.')
|
||||
|
||||
def set(self, attr, value):
|
||||
# tokenize string
|
||||
tokens = attr.split('_', 1)
|
||||
if len(tokens) > 1:
|
||||
setter_to_call = getattr(getattr(self, tokens[0]), 'set')
|
||||
else:
|
||||
setter_to_call = getattr(self, 'set')
|
||||
|
||||
setter_to_call(tokens[1], value)
|
||||
|
||||
return
|
||||
53
third_party/acados/acados_template/acados_sim_layout.json
vendored
Normal file
53
third_party/acados/acados_template/acados_sim_layout.json
vendored
Normal file
@@ -0,0 +1,53 @@
|
||||
{
|
||||
"acados_include_path": [
|
||||
"str"
|
||||
],
|
||||
"model": {
|
||||
"name" : [
|
||||
"str"
|
||||
]
|
||||
},
|
||||
"acados_lib_path": [
|
||||
"str"
|
||||
],
|
||||
"dims": {
|
||||
"np": [
|
||||
"int"
|
||||
],
|
||||
"nu": [
|
||||
"int"
|
||||
],
|
||||
"nx": [
|
||||
"int"
|
||||
],
|
||||
"nz": [
|
||||
"int"
|
||||
]
|
||||
},
|
||||
"solver_options": {
|
||||
"integrator_type": [
|
||||
"str"
|
||||
],
|
||||
"collocation_type": [
|
||||
"str"
|
||||
],
|
||||
"Tsim": [
|
||||
"float"
|
||||
],
|
||||
"sim_method_num_stages": [
|
||||
"int"
|
||||
],
|
||||
"sim_method_num_steps": [
|
||||
"int"
|
||||
],
|
||||
"sim_method_newton_iter": [
|
||||
"int"
|
||||
],
|
||||
"sim_method_newton_tol": [
|
||||
"float"
|
||||
],
|
||||
"ext_fun_compile_flags": [
|
||||
"str"
|
||||
]
|
||||
}
|
||||
}
|
||||
559
third_party/acados/acados_template/acados_sim_solver.py
vendored
Normal file
559
third_party/acados/acados_template/acados_sim_solver.py
vendored
Normal file
@@ -0,0 +1,559 @@
|
||||
# -*- coding: future_fstrings -*-
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
import sys
|
||||
import os
|
||||
import json
|
||||
import importlib
|
||||
|
||||
import numpy as np
|
||||
|
||||
from subprocess import DEVNULL, call, STDOUT
|
||||
|
||||
from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_bool, byref
|
||||
from copy import deepcopy
|
||||
|
||||
from .casadi_function_generation import generate_c_code_implicit_ode, generate_c_code_gnsf, generate_c_code_explicit_ode
|
||||
from .acados_sim import AcadosSim
|
||||
from .acados_ocp import AcadosOcp
|
||||
from .utils import is_column, render_template, format_class_dict, make_object_json_dumpable,\
|
||||
make_model_consistent, set_up_imported_gnsf_model, get_python_interface_path, get_lib_ext,\
|
||||
casadi_length, is_empty, check_casadi_version
|
||||
from .builders import CMakeBuilder
|
||||
from .gnsf.detect_gnsf_structure import detect_gnsf_structure
|
||||
|
||||
|
||||
|
||||
def make_sim_dims_consistent(acados_sim: AcadosSim):
|
||||
dims = acados_sim.dims
|
||||
model = acados_sim.model
|
||||
# nx
|
||||
if is_column(model.x):
|
||||
dims.nx = casadi_length(model.x)
|
||||
else:
|
||||
raise Exception('model.x should be column vector!')
|
||||
|
||||
# nu
|
||||
if is_empty(model.u):
|
||||
dims.nu = 0
|
||||
else:
|
||||
dims.nu = casadi_length(model.u)
|
||||
|
||||
# nz
|
||||
if is_empty(model.z):
|
||||
dims.nz = 0
|
||||
else:
|
||||
dims.nz = casadi_length(model.z)
|
||||
|
||||
# np
|
||||
if is_empty(model.p):
|
||||
dims.np = 0
|
||||
else:
|
||||
dims.np = casadi_length(model.p)
|
||||
if acados_sim.parameter_values.shape[0] != dims.np:
|
||||
raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \
|
||||
f'\nGot np = {dims.np}, acados_sim.parameter_values.shape = {acados_sim.parameter_values.shape[0]}\n')
|
||||
|
||||
|
||||
def get_sim_layout():
|
||||
python_interface_path = get_python_interface_path()
|
||||
abs_path = os.path.join(python_interface_path, 'acados_sim_layout.json')
|
||||
with open(abs_path, 'r') as f:
|
||||
sim_layout = json.load(f)
|
||||
return sim_layout
|
||||
|
||||
|
||||
def sim_formulation_json_dump(acados_sim: AcadosSim, json_file='acados_sim.json'):
|
||||
# Load acados_sim structure description
|
||||
sim_layout = get_sim_layout()
|
||||
|
||||
# Copy input sim object dictionary
|
||||
sim_dict = dict(deepcopy(acados_sim).__dict__)
|
||||
|
||||
for key, v in sim_layout.items():
|
||||
# skip non dict attributes
|
||||
if not isinstance(v, dict): continue
|
||||
# Copy sim object attributes dictionaries
|
||||
sim_dict[key]=dict(getattr(acados_sim, key).__dict__)
|
||||
|
||||
sim_json = format_class_dict(sim_dict)
|
||||
|
||||
with open(json_file, 'w') as f:
|
||||
json.dump(sim_json, f, default=make_object_json_dumpable, indent=4, sort_keys=True)
|
||||
|
||||
|
||||
def sim_get_default_cmake_builder() -> CMakeBuilder:
|
||||
"""
|
||||
If :py:class:`~acados_template.acados_sim_solver.AcadosSimSolver` is used with `CMake` this function returns a good first setting.
|
||||
:return: default :py:class:`~acados_template.builders.CMakeBuilder`
|
||||
"""
|
||||
cmake_builder = CMakeBuilder()
|
||||
cmake_builder.options_on = ['BUILD_ACADOS_SIM_SOLVER_LIB']
|
||||
return cmake_builder
|
||||
|
||||
|
||||
def sim_render_templates(json_file, model_name: str, code_export_dir, cmake_options: CMakeBuilder = None):
|
||||
# setting up loader and environment
|
||||
json_path = os.path.join(os.getcwd(), json_file)
|
||||
|
||||
if not os.path.exists(json_path):
|
||||
raise Exception(f"{json_path} not found!")
|
||||
|
||||
# Render templates
|
||||
in_file = 'acados_sim_solver.in.c'
|
||||
out_file = f'acados_sim_solver_{model_name}.c'
|
||||
render_template(in_file, out_file, code_export_dir, json_path)
|
||||
|
||||
in_file = 'acados_sim_solver.in.h'
|
||||
out_file = f'acados_sim_solver_{model_name}.h'
|
||||
render_template(in_file, out_file, code_export_dir, json_path)
|
||||
|
||||
in_file = 'acados_sim_solver.in.pxd'
|
||||
out_file = f'acados_sim_solver.pxd'
|
||||
render_template(in_file, out_file, code_export_dir, json_path)
|
||||
|
||||
# Builder
|
||||
if cmake_options is not None:
|
||||
in_file = 'CMakeLists.in.txt'
|
||||
out_file = 'CMakeLists.txt'
|
||||
render_template(in_file, out_file, code_export_dir, json_path)
|
||||
else:
|
||||
in_file = 'Makefile.in'
|
||||
out_file = 'Makefile'
|
||||
render_template(in_file, out_file, code_export_dir, json_path)
|
||||
|
||||
in_file = 'main_sim.in.c'
|
||||
out_file = f'main_sim_{model_name}.c'
|
||||
render_template(in_file, out_file, code_export_dir, json_path)
|
||||
|
||||
# folder model
|
||||
model_dir = os.path.join(code_export_dir, model_name + '_model')
|
||||
|
||||
in_file = 'model.in.h'
|
||||
out_file = f'{model_name}_model.h'
|
||||
render_template(in_file, out_file, model_dir, json_path)
|
||||
|
||||
|
||||
def sim_generate_external_functions(acados_sim: AcadosSim):
|
||||
model = acados_sim.model
|
||||
model = make_model_consistent(model)
|
||||
|
||||
integrator_type = acados_sim.solver_options.integrator_type
|
||||
|
||||
opts = dict(generate_hess = acados_sim.solver_options.sens_hess,
|
||||
code_export_directory = acados_sim.code_export_directory)
|
||||
|
||||
# create code_export_dir, model_dir
|
||||
code_export_dir = acados_sim.code_export_directory
|
||||
opts['code_export_directory'] = code_export_dir
|
||||
model_dir = os.path.join(code_export_dir, model.name + '_model')
|
||||
if not os.path.exists(model_dir):
|
||||
os.makedirs(model_dir)
|
||||
|
||||
# generate external functions
|
||||
check_casadi_version()
|
||||
if integrator_type == 'ERK':
|
||||
generate_c_code_explicit_ode(model, opts)
|
||||
elif integrator_type == 'IRK':
|
||||
generate_c_code_implicit_ode(model, opts)
|
||||
elif integrator_type == 'GNSF':
|
||||
generate_c_code_gnsf(model, opts)
|
||||
|
||||
|
||||
class AcadosSimSolver:
|
||||
"""
|
||||
Class to interact with the acados integrator C object.
|
||||
|
||||
:param acados_sim: type :py:class:`~acados_template.acados_ocp.AcadosOcp` (takes values to generate an instance :py:class:`~acados_template.acados_sim.AcadosSim`) or :py:class:`~acados_template.acados_sim.AcadosSim`
|
||||
:param json_file: Default: 'acados_sim.json'
|
||||
:param build: Default: True
|
||||
:param cmake_builder: type :py:class:`~acados_template.utils.CMakeBuilder` generate a `CMakeLists.txt` and use
|
||||
the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with
|
||||
`MS Visual Studio`); default: `None`
|
||||
"""
|
||||
if sys.platform=="win32":
|
||||
from ctypes import wintypes
|
||||
from ctypes import WinDLL
|
||||
dlclose = WinDLL('kernel32', use_last_error=True).FreeLibrary
|
||||
dlclose.argtypes = [wintypes.HMODULE]
|
||||
else:
|
||||
dlclose = CDLL(None).dlclose
|
||||
dlclose.argtypes = [c_void_p]
|
||||
|
||||
|
||||
@classmethod
|
||||
def generate(cls, acados_sim: AcadosSim, json_file='acados_sim.json', cmake_builder: CMakeBuilder = None):
|
||||
"""
|
||||
Generates the code for an acados sim solver, given the description in acados_sim
|
||||
"""
|
||||
|
||||
acados_sim.code_export_directory = os.path.abspath(acados_sim.code_export_directory)
|
||||
|
||||
# make dims consistent
|
||||
make_sim_dims_consistent(acados_sim)
|
||||
|
||||
# module dependent post processing
|
||||
if acados_sim.solver_options.integrator_type == 'GNSF':
|
||||
if acados_sim.solver_options.sens_hess == True:
|
||||
raise Exception("AcadosSimSolver: GNSF does not support sens_hess = True.")
|
||||
if 'gnsf_model' in acados_sim.__dict__:
|
||||
set_up_imported_gnsf_model(acados_sim)
|
||||
else:
|
||||
detect_gnsf_structure(acados_sim)
|
||||
|
||||
# generate external functions
|
||||
sim_generate_external_functions(acados_sim)
|
||||
|
||||
# dump to json
|
||||
sim_formulation_json_dump(acados_sim, json_file)
|
||||
|
||||
# render templates
|
||||
sim_render_templates(json_file, acados_sim.model.name, acados_sim.code_export_directory, cmake_builder)
|
||||
|
||||
|
||||
@classmethod
|
||||
def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None, verbose: bool = True):
|
||||
# Compile solver
|
||||
cwd = os.getcwd()
|
||||
os.chdir(code_export_dir)
|
||||
if with_cython:
|
||||
call(
|
||||
['make', 'clean_sim_cython'],
|
||||
stdout=None if verbose else DEVNULL,
|
||||
stderr=None if verbose else STDOUT
|
||||
)
|
||||
call(
|
||||
['make', 'sim_cython'],
|
||||
stdout=None if verbose else DEVNULL,
|
||||
stderr=None if verbose else STDOUT
|
||||
)
|
||||
else:
|
||||
if cmake_builder is not None:
|
||||
cmake_builder.exec(code_export_dir, verbose=verbose)
|
||||
else:
|
||||
call(
|
||||
['make', 'sim_shared_lib'],
|
||||
stdout=None if verbose else DEVNULL,
|
||||
stderr=None if verbose else STDOUT
|
||||
)
|
||||
os.chdir(cwd)
|
||||
|
||||
|
||||
@classmethod
|
||||
def create_cython_solver(cls, json_file):
|
||||
"""
|
||||
"""
|
||||
with open(json_file, 'r') as f:
|
||||
acados_sim_json = json.load(f)
|
||||
code_export_directory = acados_sim_json['code_export_directory']
|
||||
|
||||
importlib.invalidate_caches()
|
||||
rel_code_export_directory = os.path.relpath(code_export_directory)
|
||||
acados_sim_solver_pyx = importlib.import_module(f'{rel_code_export_directory}.acados_sim_solver_pyx')
|
||||
|
||||
AcadosSimSolverCython = getattr(acados_sim_solver_pyx, 'AcadosSimSolverCython')
|
||||
return AcadosSimSolverCython(acados_sim_json['model']['name'])
|
||||
|
||||
def __init__(self, acados_sim, json_file='acados_sim.json', generate=True, build=True, cmake_builder: CMakeBuilder = None, verbose: bool = True):
|
||||
|
||||
self.solver_created = False
|
||||
self.acados_sim = acados_sim
|
||||
model_name = acados_sim.model.name
|
||||
self.model_name = model_name
|
||||
|
||||
code_export_dir = os.path.abspath(acados_sim.code_export_directory)
|
||||
|
||||
# reuse existing json and casadi functions, when creating integrator from ocp
|
||||
if generate and not isinstance(acados_sim, AcadosOcp):
|
||||
self.generate(acados_sim, json_file=json_file, cmake_builder=cmake_builder)
|
||||
|
||||
if build:
|
||||
self.build(code_export_dir, cmake_builder=cmake_builder, verbose=True)
|
||||
|
||||
# prepare library loading
|
||||
lib_prefix = 'lib'
|
||||
lib_ext = get_lib_ext()
|
||||
if os.name == 'nt':
|
||||
lib_prefix = ''
|
||||
|
||||
# Load acados library to avoid unloading the library.
|
||||
# This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed.
|
||||
# Unloading a library which uses OpenMP results in a segfault (on any platform?).
|
||||
# see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp]
|
||||
# or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html]
|
||||
libacados_name = f'{lib_prefix}acados{lib_ext}'
|
||||
libacados_filepath = os.path.join(acados_sim.acados_lib_path, libacados_name)
|
||||
self.__acados_lib = CDLL(libacados_filepath)
|
||||
# find out if acados was compiled with OpenMP
|
||||
try:
|
||||
self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None
|
||||
except AttributeError as e:
|
||||
self.__acados_lib_uses_omp = False
|
||||
if self.__acados_lib_uses_omp:
|
||||
print('acados was compiled with OpenMP.')
|
||||
else:
|
||||
print('acados was compiled without OpenMP.')
|
||||
libacados_sim_solver_name = f'{lib_prefix}acados_sim_solver_{self.model_name}{lib_ext}'
|
||||
self.shared_lib_name = os.path.join(code_export_dir, libacados_sim_solver_name)
|
||||
|
||||
# get shared_lib
|
||||
self.shared_lib = CDLL(self.shared_lib_name)
|
||||
|
||||
# create capsule
|
||||
getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule").restype = c_void_p
|
||||
self.capsule = getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule")()
|
||||
|
||||
# create solver
|
||||
getattr(self.shared_lib, f"{model_name}_acados_sim_create").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_sim_create").restype = c_int
|
||||
assert getattr(self.shared_lib, f"{model_name}_acados_sim_create")(self.capsule)==0
|
||||
self.solver_created = True
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").restype = c_void_p
|
||||
self.sim_opts = getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").restype = c_void_p
|
||||
self.sim_dims = getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").restype = c_void_p
|
||||
self.sim_config = getattr(self.shared_lib, f"{model_name}_acados_get_sim_config")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").restype = c_void_p
|
||||
self.sim_out = getattr(self.shared_lib, f"{model_name}_acados_get_sim_out")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").restype = c_void_p
|
||||
self.sim_in = getattr(self.shared_lib, f"{model_name}_acados_get_sim_in")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").restype = c_void_p
|
||||
self.sim_solver = getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver")(self.capsule)
|
||||
|
||||
self.gettable_vectors = ['x', 'u', 'z', 'S_adj']
|
||||
self.gettable_matrices = ['S_forw', 'Sx', 'Su', 'S_hess', 'S_algebraic']
|
||||
self.gettable_scalars = ['CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la']
|
||||
|
||||
|
||||
def simulate(self, x=None, u=None, z=None, p=None):
|
||||
"""
|
||||
Simulate the system forward for the given x, u, z, p and return x_next.
|
||||
Wrapper around `solve()` taking care of setting/getting inputs/outputs.
|
||||
"""
|
||||
if x is not None:
|
||||
self.set('x', x)
|
||||
if u is not None:
|
||||
self.set('u', u)
|
||||
if z is not None:
|
||||
self.set('z', z)
|
||||
if p is not None:
|
||||
self.set('p', p)
|
||||
|
||||
status = self.solve()
|
||||
|
||||
if status == 2:
|
||||
print("Warning: acados_sim_solver reached maximum iterations.")
|
||||
elif status != 0:
|
||||
raise Exception(f'acados_sim_solver for model {self.model_name} returned status {status}.')
|
||||
|
||||
x_next = self.get('x')
|
||||
return x_next
|
||||
|
||||
|
||||
def solve(self):
|
||||
"""
|
||||
Solve the simulation problem with current input.
|
||||
"""
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").restype = c_int
|
||||
|
||||
status = getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve")(self.capsule)
|
||||
return status
|
||||
|
||||
|
||||
def get(self, field_):
|
||||
"""
|
||||
Get the last solution of the solver.
|
||||
|
||||
:param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic', 'CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la']
|
||||
"""
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
if field_ in self.gettable_vectors:
|
||||
# get dims
|
||||
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
|
||||
dims_data = cast(dims.ctypes.data, POINTER(c_int))
|
||||
|
||||
self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data)
|
||||
|
||||
# allocate array
|
||||
out = np.ascontiguousarray(np.zeros((dims[0],)), dtype=np.float64)
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
|
||||
self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data)
|
||||
|
||||
elif field_ in self.gettable_matrices:
|
||||
# get dims
|
||||
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
|
||||
dims_data = cast(dims.ctypes.data, POINTER(c_int))
|
||||
|
||||
self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data)
|
||||
|
||||
out = np.zeros((dims[0], dims[1]), order='F')
|
||||
out_data = cast(out.ctypes.data, POINTER(c_double))
|
||||
|
||||
self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data)
|
||||
|
||||
elif field_ in self.gettable_scalars:
|
||||
scalar = c_double()
|
||||
scalar_data = byref(scalar)
|
||||
self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, scalar_data)
|
||||
|
||||
out = scalar.value
|
||||
else:
|
||||
raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \
|
||||
f' available fields are {", ".join(self.gettable_vectors+self.gettable_matrices)}, {", ".join(self.gettable_scalars)}')
|
||||
|
||||
return out
|
||||
|
||||
|
||||
|
||||
def set(self, field_: str, value_):
|
||||
"""
|
||||
Set numerical data inside the solver.
|
||||
|
||||
:param field: string in ['x', 'u', 'p', 'xdot', 'z', 'seed_adj', 'T']
|
||||
:param value: the value with appropriate size.
|
||||
"""
|
||||
settable = ['x', 'u', 'p', 'xdot', 'z', 'seed_adj', 'T'] # S_forw
|
||||
|
||||
# TODO: check and throw error here. then remove checks in Cython for speed
|
||||
# cast value_ to avoid conversion issues
|
||||
if isinstance(value_, (float, int)):
|
||||
value_ = np.array([value_])
|
||||
|
||||
value_ = value_.astype(float)
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
value_data_p = cast((value_data), c_void_p)
|
||||
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
# treat parameters separately
|
||||
if field_ == 'p':
|
||||
model_name = self.acados_sim.model.name
|
||||
getattr(self.shared_lib, f"{model_name}_acados_sim_update_params").argtypes = [c_void_p, POINTER(c_double), c_int]
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double))
|
||||
getattr(self.shared_lib, f"{model_name}_acados_sim_update_params")(self.capsule, value_data, value_.shape[0])
|
||||
return
|
||||
else:
|
||||
# dimension check
|
||||
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
|
||||
dims_data = cast(dims.ctypes.data, POINTER(c_int))
|
||||
|
||||
self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)]
|
||||
self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data)
|
||||
|
||||
value_ = np.ravel(value_, order='F')
|
||||
|
||||
value_shape = value_.shape
|
||||
if len(value_shape) == 1:
|
||||
value_shape = (value_shape[0], 0)
|
||||
|
||||
if value_shape != tuple(dims):
|
||||
raise Exception(f'AcadosSimSolver.set(): mismatching dimension' \
|
||||
f' for field "{field_}" with dimension {tuple(dims)} (you have {value_shape}).')
|
||||
|
||||
# set
|
||||
if field_ in ['xdot', 'z']:
|
||||
self.shared_lib.sim_solver_set.argtypes = [c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.sim_solver_set(self.sim_solver, field, value_data_p)
|
||||
elif field_ in settable:
|
||||
self.shared_lib.sim_in_set.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
|
||||
self.shared_lib.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, value_data_p)
|
||||
else:
|
||||
raise Exception(f'AcadosSimSolver.set(): Unknown field {field_},' \
|
||||
f' available fields are {", ".join(settable)}')
|
||||
|
||||
return
|
||||
|
||||
|
||||
def options_set(self, field_: str, value_: bool):
|
||||
"""
|
||||
Set solver options
|
||||
|
||||
:param field: string in ['sens_forw', 'sens_adj', 'sens_hess']
|
||||
:param value: Boolean
|
||||
"""
|
||||
fields = ['sens_forw', 'sens_adj', 'sens_hess']
|
||||
if field_ not in fields:
|
||||
raise Exception(f"field {field_} not supported. Supported values are {', '.join(fields)}.\n")
|
||||
|
||||
field = field_.encode('utf-8')
|
||||
value_ctypes = c_bool(value_)
|
||||
|
||||
if not isinstance(value_, bool):
|
||||
raise TypeError("options_set: expected boolean for value")
|
||||
|
||||
# only allow setting
|
||||
if getattr(self.acados_sim.solver_options, field_) or value_ == False:
|
||||
self.shared_lib.sim_opts_set.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_bool)]
|
||||
self.shared_lib.sim_opts_set(self.sim_config, self.sim_opts, field, value_ctypes)
|
||||
else:
|
||||
raise RuntimeError(f"Cannot set option {field_} to True, because it was False in original solver options.\n")
|
||||
|
||||
return
|
||||
|
||||
|
||||
def __del__(self):
|
||||
|
||||
if self.solver_created:
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").restype = c_int
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_free")(self.capsule)
|
||||
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").argtypes = [c_void_p]
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").restype = c_int
|
||||
getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule")(self.capsule)
|
||||
|
||||
try:
|
||||
self.dlclose(self.shared_lib._handle)
|
||||
except:
|
||||
print(f"WARNING: acados Python interface could not close shared_lib handle of AcadosSimSolver {self.model_name}.\n",
|
||||
"Attempting to create a new one with the same name will likely result in the old one being used!")
|
||||
pass
|
||||
64
third_party/acados/acados_template/acados_sim_solver_common.pxd
vendored
Normal file
64
third_party/acados/acados_template/acados_sim_solver_common.pxd
vendored
Normal file
@@ -0,0 +1,64 @@
|
||||
# -*- coding: future_fstrings -*-
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
|
||||
cdef extern from "acados/sim/sim_common.h":
|
||||
ctypedef struct sim_config:
|
||||
pass
|
||||
|
||||
ctypedef struct sim_opts:
|
||||
pass
|
||||
|
||||
ctypedef struct sim_in:
|
||||
pass
|
||||
|
||||
ctypedef struct sim_out:
|
||||
pass
|
||||
|
||||
|
||||
cdef extern from "acados_c/sim_interface.h":
|
||||
|
||||
ctypedef struct sim_plan:
|
||||
pass
|
||||
|
||||
ctypedef struct sim_solver:
|
||||
pass
|
||||
|
||||
# out
|
||||
void sim_out_get(sim_config *config, void *dims, sim_out *out, const char *field, void *value)
|
||||
int sim_dims_get_from_attr(sim_config *config, void *dims, const char *field, void *dims_data)
|
||||
|
||||
# opts
|
||||
void sim_opts_set(sim_config *config, void *opts_, const char *field, void *value)
|
||||
|
||||
# get/set
|
||||
void sim_in_set(sim_config *config, void *dims, sim_in *sim_in, const char *field, void *value)
|
||||
void sim_solver_set(sim_solver *solver, const char *field, void *value)
|
||||
256
third_party/acados/acados_template/acados_sim_solver_pyx.pyx
vendored
Normal file
256
third_party/acados/acados_template/acados_sim_solver_pyx.pyx
vendored
Normal file
@@ -0,0 +1,256 @@
|
||||
# -*- coding: future_fstrings -*-
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
# cython: language_level=3
|
||||
# cython: profile=False
|
||||
# distutils: language=c
|
||||
|
||||
cimport cython
|
||||
from libc cimport string
|
||||
# from libc cimport bool as bool_t
|
||||
|
||||
cimport acados_sim_solver_common
|
||||
cimport acados_sim_solver
|
||||
|
||||
cimport numpy as cnp
|
||||
|
||||
import os
|
||||
from datetime import datetime
|
||||
import numpy as np
|
||||
|
||||
|
||||
cdef class AcadosSimSolverCython:
|
||||
"""
|
||||
Class to interact with the acados sim solver C object.
|
||||
"""
|
||||
|
||||
cdef acados_sim_solver.sim_solver_capsule *capsule
|
||||
cdef void *sim_dims
|
||||
cdef acados_sim_solver_common.sim_opts *sim_opts
|
||||
cdef acados_sim_solver_common.sim_config *sim_config
|
||||
cdef acados_sim_solver_common.sim_out *sim_out
|
||||
cdef acados_sim_solver_common.sim_in *sim_in
|
||||
cdef acados_sim_solver_common.sim_solver *sim_solver
|
||||
|
||||
cdef bint solver_created
|
||||
|
||||
cdef str model_name
|
||||
|
||||
cdef str sim_solver_type
|
||||
|
||||
cdef list gettable_vectors
|
||||
cdef list gettable_matrices
|
||||
cdef list gettable_scalars
|
||||
|
||||
def __cinit__(self, model_name):
|
||||
|
||||
self.solver_created = False
|
||||
|
||||
self.model_name = model_name
|
||||
|
||||
# create capsule
|
||||
self.capsule = acados_sim_solver.acados_sim_solver_create_capsule()
|
||||
|
||||
# create solver
|
||||
assert acados_sim_solver.acados_sim_create(self.capsule) == 0
|
||||
self.solver_created = True
|
||||
|
||||
# get pointers solver
|
||||
self.__get_pointers_solver()
|
||||
|
||||
self.gettable_vectors = ['x', 'u', 'z', 'S_adj']
|
||||
self.gettable_matrices = ['S_forw', 'Sx', 'Su', 'S_hess', 'S_algebraic']
|
||||
self.gettable_scalars = ['CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la']
|
||||
|
||||
def __get_pointers_solver(self):
|
||||
"""
|
||||
Private function to get the pointers for solver
|
||||
"""
|
||||
# get pointers solver
|
||||
self.sim_opts = acados_sim_solver.acados_get_sim_opts(self.capsule)
|
||||
self.sim_dims = acados_sim_solver.acados_get_sim_dims(self.capsule)
|
||||
self.sim_config = acados_sim_solver.acados_get_sim_config(self.capsule)
|
||||
self.sim_out = acados_sim_solver.acados_get_sim_out(self.capsule)
|
||||
self.sim_in = acados_sim_solver.acados_get_sim_in(self.capsule)
|
||||
self.sim_solver = acados_sim_solver.acados_get_sim_solver(self.capsule)
|
||||
|
||||
|
||||
def simulate(self, x=None, u=None, z=None, p=None):
|
||||
"""
|
||||
Simulate the system forward for the given x, u, z, p and return x_next.
|
||||
Wrapper around `solve()` taking care of setting/getting inputs/outputs.
|
||||
"""
|
||||
if x is not None:
|
||||
self.set('x', x)
|
||||
if u is not None:
|
||||
self.set('u', u)
|
||||
if z is not None:
|
||||
self.set('z', z)
|
||||
if p is not None:
|
||||
self.set('p', p)
|
||||
|
||||
status = self.solve()
|
||||
|
||||
if status == 2:
|
||||
print("Warning: acados_sim_solver reached maximum iterations.")
|
||||
elif status != 0:
|
||||
raise Exception(f'acados_sim_solver for model {self.model_name} returned status {status}.')
|
||||
|
||||
x_next = self.get('x')
|
||||
return x_next
|
||||
|
||||
|
||||
def solve(self):
|
||||
"""
|
||||
Solve the sim with current input.
|
||||
"""
|
||||
return acados_sim_solver.acados_sim_solve(self.capsule)
|
||||
|
||||
|
||||
def get(self, field_):
|
||||
"""
|
||||
Get the last solution of the solver.
|
||||
|
||||
:param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic', 'CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la']
|
||||
"""
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
if field_ in self.gettable_vectors:
|
||||
return self.__get_vector(field)
|
||||
elif field_ in self.gettable_matrices:
|
||||
return self.__get_matrix(field)
|
||||
elif field_ in self.gettable_scalars:
|
||||
return self.__get_scalar(field)
|
||||
else:
|
||||
raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \
|
||||
f' available fields are {", ".join(self.gettable.keys())}')
|
||||
|
||||
|
||||
def __get_scalar(self, field):
|
||||
cdef double scalar
|
||||
acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> &scalar)
|
||||
return scalar
|
||||
|
||||
|
||||
def __get_vector(self, field):
|
||||
cdef int[2] dims
|
||||
acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0])
|
||||
# cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((dims[0],), dtype=np.float64))
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((dims[0]),)
|
||||
acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> out.data)
|
||||
return out
|
||||
|
||||
|
||||
def __get_matrix(self, field):
|
||||
cdef int[2] dims
|
||||
acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0])
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F', dtype=np.float64)
|
||||
acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> out.data)
|
||||
return out
|
||||
|
||||
|
||||
def set(self, field_: str, value_):
|
||||
"""
|
||||
Set numerical data inside the solver.
|
||||
|
||||
:param field: string in ['p', 'seed_adj', 'T', 'x', 'u', 'xdot', 'z']
|
||||
:param value: the value with appropriate size.
|
||||
"""
|
||||
settable = ['seed_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw
|
||||
|
||||
# cast value_ to avoid conversion issues
|
||||
if isinstance(value_, (float, int)):
|
||||
value_ = np.array([value_])
|
||||
# if len(value_.shape) > 1:
|
||||
# raise RuntimeError('AcadosSimSolverCython.set(): value_ should be 1 dimensional')
|
||||
|
||||
cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(value_, dtype=np.float64).flatten()
|
||||
|
||||
field = field_.encode('utf-8')
|
||||
cdef int[2] dims
|
||||
|
||||
# treat parameters separately
|
||||
if field_ == 'p':
|
||||
assert acados_sim_solver.acados_sim_update_params(self.capsule, <double *> value.data, value.shape[0]) == 0
|
||||
return
|
||||
else:
|
||||
acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0])
|
||||
|
||||
value_ = np.ravel(value_, order='F')
|
||||
|
||||
value_shape = value_.shape
|
||||
if len(value_shape) == 1:
|
||||
value_shape = (value_shape[0], 0)
|
||||
|
||||
if value_shape != tuple(dims):
|
||||
raise Exception(f'AcadosSimSolverCython.set(): mismatching dimension' \
|
||||
f' for field "{field_}" with dimension {tuple(dims)} (you have {value_shape}).')
|
||||
|
||||
# set
|
||||
if field_ in ['xdot', 'z']:
|
||||
acados_sim_solver_common.sim_solver_set(self.sim_solver, field, <void *> value.data)
|
||||
elif field_ in settable:
|
||||
acados_sim_solver_common.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, <void *> value.data)
|
||||
else:
|
||||
raise Exception(f'AcadosSimSolverCython.set(): Unknown field {field_},' \
|
||||
f' available fields are {", ".join(settable)}')
|
||||
|
||||
|
||||
def options_set(self, field_: str, value_: bool):
|
||||
"""
|
||||
Set solver options
|
||||
|
||||
:param field: string in ['sens_forw', 'sens_adj', 'sens_hess']
|
||||
:param value: Boolean
|
||||
"""
|
||||
fields = ['sens_forw', 'sens_adj', 'sens_hess']
|
||||
if field_ not in fields:
|
||||
raise Exception(f"field {field_} not supported. Supported values are {', '.join(fields)}.\n")
|
||||
|
||||
field = field_.encode('utf-8')
|
||||
|
||||
if not isinstance(value_, bool):
|
||||
raise TypeError("options_set: expected boolean for value")
|
||||
|
||||
cdef bint bool_value = value_
|
||||
acados_sim_solver_common.sim_opts_set(self.sim_config, self.sim_opts, field, <void *> &bool_value)
|
||||
# TODO: only allow setting
|
||||
# if getattr(self.acados_sim.solver_options, field_) or value_ == False:
|
||||
# acados_sim_solver_common.sim_opts_set(self.sim_config, self.sim_opts, field, <void *> &bool_value)
|
||||
# else:
|
||||
# raise RuntimeError(f"Cannot set option {field_} to True, because it was False in original solver options.\n")
|
||||
|
||||
return
|
||||
|
||||
|
||||
def __del__(self):
|
||||
if self.solver_created:
|
||||
acados_sim_solver.acados_sim_free(self.capsule)
|
||||
acados_sim_solver.acados_sim_solver_free_capsule(self.capsule)
|
||||
100
third_party/acados/acados_template/acados_solver_common.pxd
vendored
Normal file
100
third_party/acados/acados_template/acados_solver_common.pxd
vendored
Normal file
@@ -0,0 +1,100 @@
|
||||
# -*- coding: future_fstrings -*-
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
|
||||
cdef extern from "acados/ocp_nlp/ocp_nlp_common.h":
|
||||
ctypedef struct ocp_nlp_config:
|
||||
pass
|
||||
|
||||
ctypedef struct ocp_nlp_dims:
|
||||
pass
|
||||
|
||||
ctypedef struct ocp_nlp_in:
|
||||
pass
|
||||
|
||||
ctypedef struct ocp_nlp_out:
|
||||
pass
|
||||
|
||||
|
||||
cdef extern from "acados_c/ocp_nlp_interface.h":
|
||||
ctypedef enum ocp_nlp_solver_t:
|
||||
pass
|
||||
|
||||
ctypedef enum ocp_nlp_cost_t:
|
||||
pass
|
||||
|
||||
ctypedef enum ocp_nlp_dynamics_t:
|
||||
pass
|
||||
|
||||
ctypedef enum ocp_nlp_constraints_t:
|
||||
pass
|
||||
|
||||
ctypedef enum ocp_nlp_reg_t:
|
||||
pass
|
||||
|
||||
ctypedef struct ocp_nlp_plan:
|
||||
pass
|
||||
|
||||
ctypedef struct ocp_nlp_solver:
|
||||
pass
|
||||
|
||||
int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in_,
|
||||
int start_stage, const char *field, void *value)
|
||||
int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims,
|
||||
ocp_nlp_in *in_, int stage, const char *field, void *value)
|
||||
|
||||
# out
|
||||
void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
|
||||
int stage, const char *field, void *value)
|
||||
void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
|
||||
int stage, const char *field, void *value)
|
||||
void ocp_nlp_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_solver *solver,
|
||||
int stage, const char *field, void *value)
|
||||
int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
|
||||
int stage, const char *field)
|
||||
void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
|
||||
int stage, const char *field, int *dims_out)
|
||||
void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
|
||||
int stage, const char *field, int *dims_out)
|
||||
void ocp_nlp_qp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
|
||||
int stage, const char *field, int *dims_out)
|
||||
|
||||
# opts
|
||||
void ocp_nlp_solver_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void* value)
|
||||
|
||||
# solver
|
||||
void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out)
|
||||
void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out)
|
||||
void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in_, ocp_nlp_out *nlp_out)
|
||||
|
||||
# get/set
|
||||
void ocp_nlp_get(ocp_nlp_config *config, ocp_nlp_solver *solver, const char *field, void *return_value_)
|
||||
void ocp_nlp_set(ocp_nlp_config *config, ocp_nlp_solver *solver, int stage, const char *field, void *value)
|
||||
131
third_party/acados/acados_template/builders.py
vendored
Normal file
131
third_party/acados/acados_template/builders.py
vendored
Normal file
@@ -0,0 +1,131 @@
|
||||
# -*- coding: future_fstrings -*-
|
||||
#
|
||||
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
import os
|
||||
import sys
|
||||
from subprocess import DEVNULL, call, STDOUT
|
||||
|
||||
|
||||
class CMakeBuilder:
|
||||
"""
|
||||
Class to work with the `CMake` build system.
|
||||
"""
|
||||
def __init__(self):
|
||||
self._source_dir = None # private source directory, this is set to code_export_dir
|
||||
self.build_dir = 'build'
|
||||
self._build_dir = None # private build directory, usually rendered to abspath(build_dir)
|
||||
self.generator = None
|
||||
"""Defines the generator, options can be found via `cmake --help` under 'Generator'. Type: string. Linux default 'Unix Makefiles', Windows 'Visual Studio 15 2017 Win64'; default value: `None`."""
|
||||
# set something for Windows
|
||||
if os.name == 'nt':
|
||||
self.generator = 'Visual Studio 15 2017 Win64'
|
||||
self.build_targets = None
|
||||
"""A comma-separated list of the build targets, if `None` then all targets will be build; type: List of strings; default: `None`."""
|
||||
self.options_on = None
|
||||
"""List of strings as CMake options which are translated to '-D Opt[0]=ON -D Opt[1]=ON ...'; default: `None`."""
|
||||
|
||||
# Generate the command string for handling the cmake command.
|
||||
def get_cmd1_cmake(self):
|
||||
defines_str = ''
|
||||
if self.options_on is not None:
|
||||
defines_arr = [f' -D{opt}=ON' for opt in self.options_on]
|
||||
defines_str = ' '.join(defines_arr)
|
||||
generator_str = ''
|
||||
if self.generator is not None:
|
||||
generator_str = f' -G"{self.generator}"'
|
||||
return f'cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX="{self._source_dir}"{defines_str}{generator_str} -Wdev -S"{self._source_dir}" -B"{self._build_dir}"'
|
||||
|
||||
# Generate the command string for handling the build.
|
||||
def get_cmd2_build(self):
|
||||
import multiprocessing
|
||||
cmd = f'cmake --build "{self._build_dir}" --config Release -j{multiprocessing.cpu_count()}'
|
||||
if self.build_targets is not None:
|
||||
cmd += f' -t {self.build_targets}'
|
||||
return cmd
|
||||
|
||||
# Generate the command string for handling the install command.
|
||||
def get_cmd3_install(self):
|
||||
return f'cmake --install "{self._build_dir}"'
|
||||
|
||||
def exec(self, code_export_directory, verbose=True):
|
||||
"""
|
||||
Execute the compilation using `CMake` with the given settings.
|
||||
:param code_export_directory: must be the absolute path to the directory where the code was exported to
|
||||
"""
|
||||
if(os.path.isabs(code_export_directory) is False):
|
||||
print(f'(W) the code export directory "{code_export_directory}" is not an absolute path!')
|
||||
self._source_dir = code_export_directory
|
||||
self._build_dir = os.path.abspath(self.build_dir)
|
||||
try:
|
||||
os.mkdir(self._build_dir)
|
||||
except FileExistsError as e:
|
||||
pass
|
||||
|
||||
try:
|
||||
os.chdir(self._build_dir)
|
||||
cmd_str = self.get_cmd1_cmake()
|
||||
print(f'call("{cmd_str})"')
|
||||
retcode = call(
|
||||
cmd_str,
|
||||
shell=True,
|
||||
stdout=None if verbose else DEVNULL,
|
||||
stderr=None if verbose else STDOUT
|
||||
)
|
||||
if retcode != 0:
|
||||
raise RuntimeError(f'CMake command "{cmd_str}" was terminated by signal {retcode}')
|
||||
cmd_str = self.get_cmd2_build()
|
||||
print(f'call("{cmd_str}")')
|
||||
retcode = call(
|
||||
cmd_str,
|
||||
shell=True,
|
||||
stdout=None if verbose else DEVNULL,
|
||||
stderr=None if verbose else STDOUT
|
||||
)
|
||||
if retcode != 0:
|
||||
raise RuntimeError(f'Build command "{cmd_str}" was terminated by signal {retcode}')
|
||||
cmd_str = self.get_cmd3_install()
|
||||
print(f'call("{cmd_str}")')
|
||||
retcode = call(
|
||||
cmd_str,
|
||||
shell=True,
|
||||
stdout=None if verbose else DEVNULL,
|
||||
stderr=None if verbose else STDOUT
|
||||
)
|
||||
if retcode != 0:
|
||||
raise RuntimeError(f'Install command "{cmd_str}" was terminated by signal {retcode}')
|
||||
except OSError as e:
|
||||
print("Execution failed:", e, file=sys.stderr)
|
||||
except Exception as e:
|
||||
print("Execution failed:", e, file=sys.stderr)
|
||||
exit(1)
|
||||
397
third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt
vendored
Normal file
397
third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt
vendored
Normal file
@@ -0,0 +1,397 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
{%- if solver_options.qp_solver %}
|
||||
{%- set qp_solver = solver_options.qp_solver %}
|
||||
{%- else %}
|
||||
{%- set qp_solver = "FULL_CONDENSING_HPIPM" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if solver_options.hessian_approx %}
|
||||
{%- set hessian_approx = solver_options.hessian_approx %}
|
||||
{%- elif solver_options.sens_hess %}
|
||||
{%- set hessian_approx = "EXACT" %}
|
||||
{%- else %}
|
||||
{%- set hessian_approx = "GAUSS_NEWTON" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constraints.constr_type %}
|
||||
{%- set constr_type = constraints.constr_type %}
|
||||
{%- else %}
|
||||
{%- set constr_type = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constraints.constr_type_e %}
|
||||
{%- set constr_type_e = constraints.constr_type_e %}
|
||||
{%- else %}
|
||||
{%- set constr_type_e = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type %}
|
||||
{%- set cost_type = cost.cost_type %}
|
||||
{%- else %}
|
||||
{%- set cost_type = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type_e %}
|
||||
{%- set cost_type_e = cost.cost_type_e %}
|
||||
{%- else %}
|
||||
{%- set cost_type_e = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type_0 %}
|
||||
{%- set cost_type_0 = cost.cost_type_0 %}
|
||||
{%- else %}
|
||||
{%- set cost_type_0 = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nh %}
|
||||
{%- set dims_nh = dims.nh %}
|
||||
{%- else %}
|
||||
{%- set dims_nh = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nphi %}
|
||||
{%- set dims_nphi = dims.nphi %}
|
||||
{%- else %}
|
||||
{%- set dims_nphi = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nh_e %}
|
||||
{%- set dims_nh_e = dims.nh_e %}
|
||||
{%- else %}
|
||||
{%- set dims_nh_e = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nphi_e %}
|
||||
{%- set dims_nphi_e = dims.nphi_e %}
|
||||
{%- else %}
|
||||
{%- set dims_nphi_e = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if solver_options.model_external_shared_lib_dir %}
|
||||
{%- set model_external_shared_lib_dir = solver_options.model_external_shared_lib_dir %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if solver_options.model_external_shared_lib_name %}
|
||||
{%- set model_external_shared_lib_name = solver_options.model_external_shared_lib_name %}
|
||||
{%- endif %}
|
||||
|
||||
{#- control operator #}
|
||||
{%- if os and os == "pc" %}
|
||||
{%- set control = "&" %}
|
||||
{%- else %}
|
||||
{%- set control = ";" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if acados_link_libs and os and os == "pc" %}{# acados linking libraries and flags #}
|
||||
{%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp -%}
|
||||
{%- set openmp_flag = acados_link_libs.openmp %}
|
||||
{%- else %}
|
||||
{%- set openmp_flag = " " %}
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
{%- set link_libs = "-lqpOASES_e" %}
|
||||
{%- elif qp_solver == "FULL_CONDENSING_DAQP" %}
|
||||
{%- set link_libs = "-ldaqp" %}
|
||||
{%- else %}
|
||||
{%- set link_libs = "" %}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
cmake_minimum_required(VERSION 3.13)
|
||||
|
||||
project({{ model.name }})
|
||||
|
||||
# build options.
|
||||
option(BUILD_ACADOS_SOLVER_LIB "Should the solver library acados_solver_{{ model.name }} be build?" OFF)
|
||||
option(BUILD_ACADOS_OCP_SOLVER_LIB "Should the OCP solver library acados_ocp_solver_{{ model.name }} be build?" OFF)
|
||||
option(BUILD_EXAMPLE "Should the example main_{{ model.name }} be build?" OFF)
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
option(BUILD_SIM_EXAMPLE "Should the simulation example main_sim_{{ model.name }} be build?" OFF)
|
||||
option(BUILD_ACADOS_SIM_SOLVER_LIB "Should the simulation solver library acados_sim_solver_{{ model.name }} be build?" OFF)
|
||||
{%- endif %}
|
||||
|
||||
|
||||
if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" AND CMAKE_SYSTEM_NAME MATCHES "Windows")
|
||||
# MinGW, change to .lib such that mex recognizes it
|
||||
set(CMAKE_SHARED_LIBRARY_SUFFIX ".lib")
|
||||
set(CMAKE_SHARED_LIBRARY_PREFIX "")
|
||||
endif()
|
||||
|
||||
|
||||
# object target names
|
||||
set(MODEL_OBJ model_{{ model.name }})
|
||||
set(OCP_OBJ ocp_{{ model.name }})
|
||||
set(SIM_OBJ sim_{{ model.name }})
|
||||
|
||||
# model
|
||||
set(MODEL_SRC
|
||||
{%- if model.dyn_ext_fun_type == "casadi" %}
|
||||
{%- if solver_options.integrator_type == "ERK" %}
|
||||
{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c
|
||||
{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c
|
||||
{{ model.name }}_model/{{ model.name }}_expl_vde_adj.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c
|
||||
{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c
|
||||
{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c
|
||||
{%- elif solver_options.integrator_type == "DISCRETE" %}
|
||||
{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c
|
||||
{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c
|
||||
{%- endif %}
|
||||
{%- endif -%}
|
||||
{%- else %}
|
||||
{{ model.name }}_model/{{ model.dyn_generic_source }}
|
||||
{%- endif %}
|
||||
)
|
||||
add_library(${MODEL_OBJ} OBJECT ${MODEL_SRC} )
|
||||
|
||||
# optimal control problem - mostly CasADi exports
|
||||
if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMPLE})
|
||||
set(OCP_SRC
|
||||
{%- if constr_type == "BGP" and dims_nphi > 0 %}
|
||||
{{ model.name }}_constraints/{{ model.name }}_phi_constraint.c
|
||||
{%- endif %}
|
||||
{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
|
||||
{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c
|
||||
{%- endif %}
|
||||
|
||||
{%- if constr_type == "BGH" and dims_nh > 0 %}
|
||||
{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c
|
||||
{{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
|
||||
{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
|
||||
{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost_type_0 == "NONLINEAR_LS" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
|
||||
{%- elif cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun_jac_hess.c
|
||||
{%- elif cost_type_0 == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type_0 == "casadi" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
|
||||
{%- else %}
|
||||
{{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost_type == "NONLINEAR_LS" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
|
||||
{%- elif cost_type == "CONVEX_OVER_NONLINEAR" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_conl_cost_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_conl_cost_fun_jac_hess.c
|
||||
{%- elif cost_type == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type == "casadi" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
|
||||
{%- elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
|
||||
{{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost_type_e == "NONLINEAR_LS" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
|
||||
{%- elif cost_type_e == "CONVEX_OVER_NONLINEAR" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun_jac_hess.c
|
||||
{%- elif cost_type_e == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type_e == "casadi" %}
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
|
||||
{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
|
||||
{%- elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
|
||||
{{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
acados_solver_{{ model.name }}.c)
|
||||
add_library(${OCP_OBJ} OBJECT ${OCP_SRC})
|
||||
endif()
|
||||
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
# for sim solver
|
||||
if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_EXAMPLE}
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
OR ${BUILD_SIM_EXAMPLE} OR ${BUILD_ACADOS_SIM_SOLVER_LIB}
|
||||
{%- endif -%}
|
||||
)
|
||||
set(SIM_SRC acados_sim_solver_{{ model.name }}.c)
|
||||
add_library(${SIM_OBJ} OBJECT ${SIM_SRC})
|
||||
endif()
|
||||
{%- endif %}
|
||||
|
||||
# for target example
|
||||
set(EX_SRC main_{{ model.name }}.c)
|
||||
set(EX_EXE main_{{ model.name }})
|
||||
|
||||
{%- if model_external_shared_lib_dir and model_external_shared_lib_name %}
|
||||
set(EXTERNAL_DIR {{ model_external_shared_lib_dir | replace(from="\", to="/") }})
|
||||
set(EXTERNAL_LIB {{ model_external_shared_lib_name }})
|
||||
{%- else %}
|
||||
set(EXTERNAL_DIR)
|
||||
set(EXTERNAL_LIB)
|
||||
{%- endif %}
|
||||
|
||||
# set some search paths for preprocessor and linker
|
||||
set(ACADOS_INCLUDE_PATH {{ acados_include_path | replace(from="\", to="/") }} CACHE PATH "Define the path which contains the include directory for acados.")
|
||||
set(ACADOS_LIB_PATH {{ acados_lib_path | replace(from="\", to="/") }} CACHE PATH "Define the path which contains the lib directory for acados.")
|
||||
|
||||
# c-compiler flags for debugging
|
||||
set(CMAKE_C_FLAGS_DEBUG "-O0 -ggdb")
|
||||
|
||||
set(CMAKE_C_FLAGS "-fPIC -std=c99 {{ openmp_flag }}
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" -%}
|
||||
-DACADOS_WITH_QPOASES
|
||||
{%- endif -%}
|
||||
{%- if qp_solver == "FULL_CONDENSING_DAQP" -%}
|
||||
-DACADOS_WITH_DAQP
|
||||
{%- endif -%}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" -%}
|
||||
-DACADOS_WITH_OSQP
|
||||
{%- endif -%}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" -%}
|
||||
-DACADOS_WITH_QPDUNES
|
||||
{%- endif -%}
|
||||
")
|
||||
#-fno-diagnostics-show-line-numbers -g
|
||||
|
||||
include_directories(
|
||||
${ACADOS_INCLUDE_PATH}
|
||||
${ACADOS_INCLUDE_PATH}/acados
|
||||
${ACADOS_INCLUDE_PATH}/blasfeo/include
|
||||
${ACADOS_INCLUDE_PATH}/hpipm/include
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
${ACADOS_INCLUDE_PATH}/qpOASES_e/
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "FULL_CONDENSING_DAQP" %}
|
||||
${ACADOS_INCLUDE_PATH}/daqp/include
|
||||
{%- endif %}
|
||||
)
|
||||
|
||||
# linker flags
|
||||
link_directories(${ACADOS_LIB_PATH})
|
||||
|
||||
# link to libraries
|
||||
if(UNIX)
|
||||
link_libraries(acados hpipm blasfeo m {{ link_libs }})
|
||||
else()
|
||||
link_libraries(acados hpipm blasfeo {{ link_libs }})
|
||||
endif()
|
||||
|
||||
# the targets
|
||||
|
||||
# bundled_shared_lib
|
||||
if(${BUILD_ACADOS_SOLVER_LIB})
|
||||
set(LIB_ACADOS_SOLVER acados_solver_{{ model.name }})
|
||||
add_library(${LIB_ACADOS_SOLVER} SHARED $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${OCP_OBJ}>
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
$<TARGET_OBJECTS:${SIM_OBJ}>
|
||||
{%- endif -%}
|
||||
)
|
||||
install(TARGETS ${LIB_ACADOS_SOLVER} DESTINATION ${CMAKE_INSTALL_PREFIX})
|
||||
endif(${BUILD_ACADOS_SOLVER_LIB})
|
||||
|
||||
# ocp_shared_lib
|
||||
if(${BUILD_ACADOS_OCP_SOLVER_LIB})
|
||||
set(LIB_ACADOS_OCP_SOLVER acados_ocp_solver_{{ model.name }})
|
||||
add_library(${LIB_ACADOS_OCP_SOLVER} SHARED $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${OCP_OBJ}>)
|
||||
# Specify libraries or flags to use when linking a given target and/or its dependents.
|
||||
target_link_libraries(${LIB_ACADOS_OCP_SOLVER} PRIVATE ${EXTERNAL_LIB})
|
||||
target_link_directories(${LIB_ACADOS_OCP_SOLVER} PRIVATE ${EXTERNAL_DIR})
|
||||
install(TARGETS ${LIB_ACADOS_OCP_SOLVER} DESTINATION ${CMAKE_INSTALL_PREFIX})
|
||||
endif(${BUILD_ACADOS_OCP_SOLVER_LIB})
|
||||
|
||||
# example
|
||||
if(${BUILD_EXAMPLE})
|
||||
add_executable(${EX_EXE} ${EX_SRC} $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${OCP_OBJ}>
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
$<TARGET_OBJECTS:${SIM_OBJ}>
|
||||
{%- endif -%}
|
||||
)
|
||||
install(TARGETS ${EX_EXE} DESTINATION ${CMAKE_INSTALL_PREFIX})
|
||||
endif(${BUILD_EXAMPLE})
|
||||
|
||||
{% if solver_options.integrator_type != "DISCRETE" -%}
|
||||
# example_sim
|
||||
if(${BUILD_SIM_EXAMPLE})
|
||||
set(EX_SIM_SRC main_sim_{{ model.name }}.c)
|
||||
set(EX_SIM_EXE main_sim_{{ model.name }})
|
||||
add_executable(${EX_SIM_EXE} ${EX_SIM_SRC} $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${SIM_OBJ}>)
|
||||
install(TARGETS ${EX_SIM_EXE} DESTINATION ${CMAKE_INSTALL_PREFIX})
|
||||
endif(${BUILD_SIM_EXAMPLE})
|
||||
|
||||
# sim_shared_lib
|
||||
if(${BUILD_ACADOS_SIM_SOLVER_LIB})
|
||||
set(LIB_ACADOS_SIM_SOLVER acados_sim_solver_{{ model.name }})
|
||||
add_library(${LIB_ACADOS_SIM_SOLVER} SHARED $<TARGET_OBJECTS:${MODEL_OBJ}> $<TARGET_OBJECTS:${SIM_OBJ}>)
|
||||
install(TARGETS ${LIB_ACADOS_SIM_SOLVER} DESTINATION ${CMAKE_INSTALL_PREFIX})
|
||||
endif(${BUILD_ACADOS_SIM_SOLVER_LIB})
|
||||
{%- endif %}
|
||||
|
||||
468
third_party/acados/acados_template/c_templates_tera/Makefile.in
vendored
Normal file
468
third_party/acados/acados_template/c_templates_tera/Makefile.in
vendored
Normal file
@@ -0,0 +1,468 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
{%- if solver_options.qp_solver %}
|
||||
{%- set qp_solver = solver_options.qp_solver %}
|
||||
{%- else %}
|
||||
{%- set qp_solver = "FULL_CONDENSING_HPIPM" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if solver_options.hessian_approx %}
|
||||
{%- set hessian_approx = solver_options.hessian_approx %}
|
||||
{%- elif solver_options.sens_hess %}
|
||||
{%- set hessian_approx = "EXACT" %}
|
||||
{%- else %}
|
||||
{%- set hessian_approx = "GAUSS_NEWTON" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constraints.constr_type %}
|
||||
{%- set constr_type = constraints.constr_type %}
|
||||
{%- else %}
|
||||
{%- set constr_type = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constraints.constr_type_e %}
|
||||
{%- set constr_type_e = constraints.constr_type_e %}
|
||||
{%- else %}
|
||||
{%- set constr_type_e = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type %}
|
||||
{%- set cost_type = cost.cost_type %}
|
||||
{%- else %}
|
||||
{%- set cost_type = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type_e %}
|
||||
{%- set cost_type_e = cost.cost_type_e %}
|
||||
{%- else %}
|
||||
{%- set cost_type_e = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type_0 %}
|
||||
{%- set cost_type_0 = cost.cost_type_0 %}
|
||||
{%- else %}
|
||||
{%- set cost_type_0 = "NONE" %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nh %}
|
||||
{%- set dims_nh = dims.nh %}
|
||||
{%- else %}
|
||||
{%- set dims_nh = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nphi %}
|
||||
{%- set dims_nphi = dims.nphi %}
|
||||
{%- else %}
|
||||
{%- set dims_nphi = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nh_e %}
|
||||
{%- set dims_nh_e = dims.nh_e %}
|
||||
{%- else %}
|
||||
{%- set dims_nh_e = 0 %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nphi_e %}
|
||||
{%- set dims_nphi_e = dims.nphi_e %}
|
||||
{%- else %}
|
||||
{%- set dims_nphi_e = 0 %}
|
||||
{%- endif %}
|
||||
{%- if solver_options.model_external_shared_lib_dir %}
|
||||
{%- set model_external_shared_lib_dir = solver_options.model_external_shared_lib_dir %}
|
||||
{%- endif %}
|
||||
{%- if solver_options.model_external_shared_lib_name %}
|
||||
{%- set model_external_shared_lib_name = solver_options.model_external_shared_lib_name %}
|
||||
{%- endif %}
|
||||
|
||||
{# control operator #}
|
||||
{%- if os and os == "pc" %}
|
||||
{%- set control = "&" %}
|
||||
{%- else %}
|
||||
{%- set control = ";" %}
|
||||
{%- endif %}
|
||||
|
||||
{# acados linking libraries and flags #}
|
||||
{%- if acados_link_libs and os and os == "pc" %}
|
||||
{%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp -%}
|
||||
{%- set openmp_flag = acados_link_libs.openmp %}
|
||||
{%- else %}
|
||||
{%- set openmp_flag = " " %}
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
{%- set link_libs = "-lqpOASES_e" %}
|
||||
{%- elif qp_solver == "FULL_CONDENSING_DAQP" %}
|
||||
{%- set link_libs = "-ldaqp" %}
|
||||
{%- else %}
|
||||
{%- set link_libs = "" %}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
# define sources and use make's implicit rules to generate object files (*.o)
|
||||
|
||||
# model
|
||||
MODEL_SRC=
|
||||
{%- if model.dyn_ext_fun_type == "casadi" %}
|
||||
{%- if solver_options.integrator_type == "ERK" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_adj.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c
|
||||
{%- elif solver_options.integrator_type == "DISCRETE" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- else %}
|
||||
MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_generic_source }}
|
||||
{%- endif %}
|
||||
MODEL_OBJ := $(MODEL_SRC:.c=.o)
|
||||
|
||||
# optimal control problem - mostly CasADi exports
|
||||
OCP_SRC=
|
||||
{%- if constr_type == "BGP" and dims_nphi > 0 %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.c
|
||||
{%- endif %}
|
||||
{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c
|
||||
{%- endif %}
|
||||
|
||||
{%- if constr_type == "BGH" and dims_nh > 0 %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost_type_0 == "NONLINEAR_LS" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
|
||||
{%- elif cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_0_fun_jac_hess.c
|
||||
{%- elif cost_type_0 == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type_0 == "casadi" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
|
||||
{%- else %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost_type == "NONLINEAR_LS" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
|
||||
{%- elif cost_type == "CONVEX_OVER_NONLINEAR" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_fun_jac_hess.c
|
||||
{%- elif cost_type == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type == "casadi" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
|
||||
{%- elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost_type_e == "NONLINEAR_LS" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
|
||||
{%- elif cost_type_e == "CONVEX_OVER_NONLINEAR" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_conl_cost_e_fun_jac_hess.c
|
||||
{%- elif cost_type_e == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type_e == "casadi" %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
|
||||
{%- elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
|
||||
OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if solver_options.custom_update_filename %}
|
||||
{%- if solver_options.custom_update_filename != "" %}
|
||||
OCP_SRC+= {{ solver_options.custom_update_filename }}
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
OCP_SRC+= acados_solver_{{ model.name }}.c
|
||||
OCP_OBJ := $(OCP_SRC:.c=.o)
|
||||
|
||||
# for sim solver
|
||||
SIM_SRC= acados_sim_solver_{{ model.name }}.c
|
||||
SIM_OBJ := $(SIM_SRC:.c=.o)
|
||||
|
||||
# for target example
|
||||
EX_SRC= main_{{ model.name }}.c
|
||||
EX_OBJ := $(EX_SRC:.c=.o)
|
||||
EX_EXE := $(EX_SRC:.c=)
|
||||
|
||||
# for target example_sim
|
||||
EX_SIM_SRC= main_sim_{{ model.name }}.c
|
||||
EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
|
||||
EX_SIM_EXE := $(EX_SIM_SRC:.c=)
|
||||
|
||||
# combine model, sim and ocp object files
|
||||
OBJ=
|
||||
OBJ+= $(MODEL_OBJ)
|
||||
{%- if solver_options.integrator_type != "DISCRETE" %}
|
||||
OBJ+= $(SIM_OBJ)
|
||||
{%- endif %}
|
||||
OBJ+= $(OCP_OBJ)
|
||||
|
||||
EXTERNAL_DIR=
|
||||
EXTERNAL_LIB=
|
||||
|
||||
{%- if model_external_shared_lib_dir and model_external_shared_lib_name %}
|
||||
EXTERNAL_DIR+= {{ model_external_shared_lib_dir }}
|
||||
EXTERNAL_LIB+= {{ model_external_shared_lib_name }}
|
||||
{%- endif %}
|
||||
|
||||
INCLUDE_PATH = {{ acados_include_path }}
|
||||
LIB_PATH = {{ acados_lib_path }}
|
||||
|
||||
# preprocessor flags for make's implicit rules
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
CPPFLAGS += -DACADOS_WITH_QPOASES
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "FULL_CONDENSING_DAQP" %}
|
||||
CPPFLAGS += -DACADOS_WITH_DAQP
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
|
||||
CPPFLAGS += -DACADOS_WITH_OSQP
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %}
|
||||
CPPFLAGS += -DACADOS_WITH_QPDUNES
|
||||
{%- endif %}
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/acados
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include
|
||||
CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
|
||||
{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
|
||||
CPPFLAGS+= -I $(INCLUDE_PATH)/qpOASES_e/
|
||||
{%- endif %}
|
||||
{%- if qp_solver == "FULL_CONDENSING_DAQP" %}
|
||||
CPPFLAGS+= -I $(INCLUDE_PATH)/daqp/include
|
||||
{%- endif %}
|
||||
|
||||
{# c-compiler flags #}
|
||||
# define the c-compiler flags for make's implicit rules
|
||||
CFLAGS = -fPIC -std=c99 {{ openmp_flag }} {{ solver_options.ext_fun_compile_flags }}#-fno-diagnostics-show-line-numbers -g
|
||||
# # Debugging
|
||||
# CFLAGS += -g3
|
||||
|
||||
# linker flags
|
||||
LDFLAGS+= -L$(LIB_PATH)
|
||||
|
||||
# link to libraries
|
||||
LDLIBS+= -lacados
|
||||
LDLIBS+= -lhpipm
|
||||
LDLIBS+= -lblasfeo
|
||||
LDLIBS+= -lm
|
||||
LDLIBS+= {{ link_libs }}
|
||||
|
||||
# libraries
|
||||
LIBACADOS_SOLVER=libacados_solver_{{ model.name }}{{ shared_lib_ext }}
|
||||
LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }}
|
||||
LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c={{ shared_lib_ext }})
|
||||
|
||||
# virtual targets
|
||||
.PHONY : all clean
|
||||
|
||||
#all: clean example_sim example shared_lib
|
||||
{% if solver_options.integrator_type == "DISCRETE" -%}
|
||||
all: clean example
|
||||
shared_lib: ocp_shared_lib
|
||||
{%- else %}
|
||||
all: clean example_sim example
|
||||
shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
|
||||
{%- endif %}
|
||||
|
||||
# some linker targets
|
||||
example: $(EX_OBJ) $(OBJ)
|
||||
$(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
|
||||
$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
{% if solver_options.integrator_type != "DISCRETE" -%}
|
||||
bundled_shared_lib: $(OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
|
||||
{%- endif %}
|
||||
|
||||
ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \
|
||||
-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB)
|
||||
|
||||
sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
|
||||
$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
|
||||
|
||||
|
||||
# Cython targets
|
||||
ocp_cython_c: ocp_shared_lib
|
||||
cython \
|
||||
-o acados_ocp_solver_pyx.c \
|
||||
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
|
||||
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
|
||||
-I {{ code_export_directory }} \
|
||||
|
||||
ocp_cython_o: ocp_cython_c
|
||||
$(CC) $(ACADOS_FLAGS) -c -O2 \
|
||||
-fPIC \
|
||||
-o acados_ocp_solver_pyx.o \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
{%- for path in cython_include_dirs %}
|
||||
-I {{ path }} \
|
||||
{%- endfor %}
|
||||
acados_ocp_solver_pyx.c \
|
||||
|
||||
ocp_cython: ocp_cython_o
|
||||
$(CC) $(ACADOS_FLAGS) -shared \
|
||||
-o acados_ocp_solver_pyx{{ shared_lib_ext }} \
|
||||
-Wl,-rpath=$(LIB_PATH) \
|
||||
acados_ocp_solver_pyx.o \
|
||||
$(abspath .)/libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} \
|
||||
$(LDFLAGS) $(LDLIBS)
|
||||
|
||||
# Sim Cython targets
|
||||
sim_cython_c: sim_shared_lib
|
||||
cython \
|
||||
-o acados_sim_solver_pyx.c \
|
||||
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
|
||||
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \
|
||||
-I {{ code_export_directory }} \
|
||||
|
||||
sim_cython_o: sim_cython_c
|
||||
$(CC) $(ACADOS_FLAGS) -c -O2 \
|
||||
-fPIC \
|
||||
-o acados_sim_solver_pyx.o \
|
||||
-I $(INCLUDE_PATH)/blasfeo/include/ \
|
||||
-I $(INCLUDE_PATH)/hpipm/include/ \
|
||||
-I $(INCLUDE_PATH) \
|
||||
{%- for path in cython_include_dirs %}
|
||||
-I {{ path }} \
|
||||
{%- endfor %}
|
||||
acados_sim_solver_pyx.c \
|
||||
|
||||
sim_cython: sim_cython_o
|
||||
$(CC) $(ACADOS_FLAGS) -shared \
|
||||
-o acados_sim_solver_pyx{{ shared_lib_ext }} \
|
||||
-Wl,-rpath=$(LIB_PATH) \
|
||||
acados_sim_solver_pyx.o \
|
||||
$(abspath .)/libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }} \
|
||||
$(LDFLAGS) $(LDLIBS)
|
||||
|
||||
{%- if os and os == "pc" %}
|
||||
|
||||
clean:
|
||||
del \Q *.o 2>nul
|
||||
del \Q *{{ shared_lib_ext }} 2>nul
|
||||
del \Q main_{{ model.name }} 2>nul
|
||||
|
||||
clean_ocp_shared_lib:
|
||||
del \Q libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} 2>nul
|
||||
del \Q acados_solver_{{ model.name }}.o 2>nul
|
||||
|
||||
clean_ocp_cython:
|
||||
del \Q libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} 2>nul
|
||||
del \Q acados_solver_{{ model.name }}.o 2>nul
|
||||
del \Q acados_ocp_solver_pyx{{ shared_lib_ext }} 2>nul
|
||||
del \Q acados_ocp_solver_pyx.o 2>nul
|
||||
|
||||
clean_sim_cython:
|
||||
del \Q libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }} 2>nul
|
||||
del \Q acados_sim_solver_{{ model.name }}.o 2>nul
|
||||
del \Q acados_sim_solver_pyx{{ shared_lib_ext }} 2>nul
|
||||
del \Q acados_sim_solver_pyx.o 2>nul
|
||||
|
||||
{%- else %}
|
||||
|
||||
clean:
|
||||
$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
|
||||
$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
|
||||
$(RM) $(EX_EXE) $(EX_SIM_EXE)
|
||||
|
||||
clean_ocp_shared_lib:
|
||||
$(RM) $(LIBACADOS_OCP_SOLVER)
|
||||
$(RM) $(OCP_OBJ)
|
||||
|
||||
clean_ocp_cython:
|
||||
$(RM) libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }}
|
||||
$(RM) acados_solver_{{ model.name }}.o
|
||||
$(RM) acados_ocp_solver_pyx{{ shared_lib_ext }}
|
||||
$(RM) acados_ocp_solver_pyx.o
|
||||
|
||||
clean_sim_cython:
|
||||
$(RM) libacados_sim_solver_{{ model.name }}{{ shared_lib_ext }}
|
||||
$(RM) acados_sim_solver_{{ model.name }}.o
|
||||
$(RM) acados_sim_solver_pyx{{ shared_lib_ext }}
|
||||
$(RM) acados_sim_solver_pyx.o
|
||||
|
||||
{%- endif %}
|
||||
526
third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c
vendored
Normal file
526
third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c
vendored
Normal file
@@ -0,0 +1,526 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
{%- if solver_options.hessian_approx %}
|
||||
{%- set hessian_approx = solver_options.hessian_approx %}
|
||||
{%- elif solver_options.sens_hess %}
|
||||
{%- set hessian_approx = "EXACT" %}
|
||||
{%- else %}
|
||||
{%- set hessian_approx = "GAUSS_NEWTON" %}
|
||||
{%- endif %}
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
// acados
|
||||
#include "acados_c/external_function_interface.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#include "acados/sim/sim_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/print.h"
|
||||
|
||||
|
||||
// example specific
|
||||
#include "{{ model.name }}_model/{{ model.name }}_model.h"
|
||||
#include "acados_sim_solver_{{ model.name }}.h"
|
||||
|
||||
|
||||
// ** solver data **
|
||||
|
||||
sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule()
|
||||
{
|
||||
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
|
||||
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
|
||||
|
||||
return capsule;
|
||||
}
|
||||
|
||||
|
||||
int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
|
||||
{
|
||||
free(capsule);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
|
||||
{
|
||||
// initialize
|
||||
const int nx = {{ model.name | upper }}_NX;
|
||||
const int nu = {{ model.name | upper }}_NU;
|
||||
const int nz = {{ model.name | upper }}_NZ;
|
||||
const int np = {{ model.name | upper }}_NP;
|
||||
bool tmp_bool;
|
||||
|
||||
{#// double Tsim = {{ solver_options.tf / dims.N }};#}
|
||||
double Tsim = {{ solver_options.Tsim }};
|
||||
|
||||
{% if solver_options.integrator_type == "IRK" %}
|
||||
capsule->sim_impl_dae_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
|
||||
|
||||
{%- if model.dyn_ext_fun_type == "casadi" %}
|
||||
// external functions (implicit model)
|
||||
capsule->sim_impl_dae_fun->casadi_fun = &{{ model.name }}_impl_dae_fun;
|
||||
capsule->sim_impl_dae_fun->casadi_work = &{{ model.name }}_impl_dae_fun_work;
|
||||
capsule->sim_impl_dae_fun->casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in;
|
||||
capsule->sim_impl_dae_fun->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out;
|
||||
capsule->sim_impl_dae_fun->casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in;
|
||||
capsule->sim_impl_dae_fun->casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_fun, np);
|
||||
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z;
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_work;
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in;
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out;
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in;
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np);
|
||||
|
||||
// external_function_param_{{ model.dyn_ext_fun_type }} impl_dae_jac_x_xdot_u_z;
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_fun = &{{ model.name }}_impl_dae_jac_x_xdot_u_z;
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_work = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_work;
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in;
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out;
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in;
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np);
|
||||
{%- else %}
|
||||
capsule->sim_impl_dae_fun->fun = &{{ model.dyn_impl_dae_fun }};
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z->fun = &{{ model.dyn_impl_dae_fun_jac }};
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z->fun = &{{ model.dyn_impl_dae_jac }};
|
||||
{%- endif %}
|
||||
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
capsule->sim_impl_dae_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
|
||||
// external_function_param_{{ model.dyn_ext_fun_type }} impl_dae_jac_x_xdot_u_z;
|
||||
capsule->sim_impl_dae_hess->casadi_fun = &{{ model.name }}_impl_dae_hess;
|
||||
capsule->sim_impl_dae_hess->casadi_work = &{{ model.name }}_impl_dae_hess_work;
|
||||
capsule->sim_impl_dae_hess->casadi_sparsity_in = &{{ model.name }}_impl_dae_hess_sparsity_in;
|
||||
capsule->sim_impl_dae_hess->casadi_sparsity_out = &{{ model.name }}_impl_dae_hess_sparsity_out;
|
||||
capsule->sim_impl_dae_hess->casadi_n_in = &{{ model.name }}_impl_dae_hess_n_in;
|
||||
capsule->sim_impl_dae_hess->casadi_n_out = &{{ model.name }}_impl_dae_hess_n_out;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_hess, np);
|
||||
{%- endif %}
|
||||
|
||||
{% elif solver_options.integrator_type == "ERK" %}
|
||||
// explicit ode
|
||||
capsule->sim_forw_vde_casadi = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
|
||||
capsule->sim_vde_adj_casadi = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
|
||||
capsule->sim_expl_ode_fun_casadi = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
|
||||
|
||||
capsule->sim_forw_vde_casadi->casadi_fun = &{{ model.name }}_expl_vde_forw;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_in = &{{ model.name }}_expl_vde_forw_n_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_out = &{{ model.name }}_expl_vde_forw_n_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &{{ model.name }}_expl_vde_forw_sparsity_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &{{ model.name }}_expl_vde_forw_sparsity_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_work = &{{ model.name }}_expl_vde_forw_work;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_forw_vde_casadi, np);
|
||||
|
||||
capsule->sim_vde_adj_casadi->casadi_fun = &{{ model.name }}_expl_vde_adj;
|
||||
capsule->sim_vde_adj_casadi->casadi_n_in = &{{ model.name }}_expl_vde_adj_n_in;
|
||||
capsule->sim_vde_adj_casadi->casadi_n_out = &{{ model.name }}_expl_vde_adj_n_out;
|
||||
capsule->sim_vde_adj_casadi->casadi_sparsity_in = &{{ model.name }}_expl_vde_adj_sparsity_in;
|
||||
capsule->sim_vde_adj_casadi->casadi_sparsity_out = &{{ model.name }}_expl_vde_adj_sparsity_out;
|
||||
capsule->sim_vde_adj_casadi->casadi_work = &{{ model.name }}_expl_vde_adj_work;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_vde_adj_casadi, np);
|
||||
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_fun = &{{ model.name }}_expl_ode_fun;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &{{ model.name }}_expl_ode_fun_n_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &{{ model.name }}_expl_ode_fun_n_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &{{ model.name }}_expl_ode_fun_sparsity_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &{{ model.name }}_expl_ode_fun_sparsity_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_work = &{{ model.name }}_expl_ode_fun_work;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_expl_ode_fun_casadi, np);
|
||||
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
capsule->sim_expl_ode_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
|
||||
// external_function_param_{{ model.dyn_ext_fun_type }} impl_dae_jac_x_xdot_u_z;
|
||||
capsule->sim_expl_ode_hess->casadi_fun = &{{ model.name }}_expl_ode_hess;
|
||||
capsule->sim_expl_ode_hess->casadi_work = &{{ model.name }}_expl_ode_hess_work;
|
||||
capsule->sim_expl_ode_hess->casadi_sparsity_in = &{{ model.name }}_expl_ode_hess_sparsity_in;
|
||||
capsule->sim_expl_ode_hess->casadi_sparsity_out = &{{ model.name }}_expl_ode_hess_sparsity_out;
|
||||
capsule->sim_expl_ode_hess->casadi_n_in = &{{ model.name }}_expl_ode_hess_n_in;
|
||||
capsule->sim_expl_ode_hess->casadi_n_out = &{{ model.name }}_expl_ode_hess_n_out;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_expl_ode_hess, np);
|
||||
{%- endif %}
|
||||
|
||||
{% elif solver_options.integrator_type == "GNSF" -%}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
capsule->sim_gnsf_phi_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
|
||||
capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
|
||||
capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
capsule->sim_gnsf_get_matrices_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
|
||||
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
capsule->sim_gnsf_phi_fun->casadi_fun = &{{ model.name }}_gnsf_phi_fun;
|
||||
capsule->sim_gnsf_phi_fun->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in;
|
||||
capsule->sim_gnsf_phi_fun->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out;
|
||||
capsule->sim_gnsf_phi_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_sparsity_in;
|
||||
capsule->sim_gnsf_phi_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_sparsity_out;
|
||||
capsule->sim_gnsf_phi_fun->casadi_work = &{{ model.name }}_gnsf_phi_fun_work;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_phi_fun, np);
|
||||
|
||||
capsule->sim_gnsf_phi_fun_jac_y->casadi_fun = &{{ model.name }}_gnsf_phi_fun_jac_y;
|
||||
capsule->sim_gnsf_phi_fun_jac_y->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_jac_y_n_in;
|
||||
capsule->sim_gnsf_phi_fun_jac_y->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_jac_y_n_out;
|
||||
capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in;
|
||||
capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out;
|
||||
capsule->sim_gnsf_phi_fun_jac_y->casadi_work = &{{ model.name }}_gnsf_phi_fun_jac_y_work;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_phi_fun_jac_y, np);
|
||||
|
||||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_fun = &{{ model.name }}_gnsf_phi_jac_y_uhat;
|
||||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_in;
|
||||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_out;
|
||||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in;
|
||||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out;
|
||||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_phi_jac_y_uhat, np);
|
||||
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_n_out = &{{ model.name }}_gnsf_get_matrices_fun_n_out;
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_in;
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_out;
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_work = &{{ model.name }}_gnsf_get_matrices_fun_work;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_get_matrices_fun, np);
|
||||
{% endif %}
|
||||
|
||||
// sim plan & config
|
||||
sim_solver_plan_t plan;
|
||||
plan.sim_solver = {{ solver_options.integrator_type }};
|
||||
|
||||
// create correct config based on plan
|
||||
sim_config * {{ model.name }}_sim_config = sim_config_create(plan);
|
||||
capsule->acados_sim_config = {{ model.name }}_sim_config;
|
||||
|
||||
// sim dims
|
||||
void *{{ model.name }}_sim_dims = sim_dims_create({{ model.name }}_sim_config);
|
||||
capsule->acados_sim_dims = {{ model.name }}_sim_dims;
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nx", &nx);
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nu", &nu);
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nz", &nz);
|
||||
{% if solver_options.integrator_type == "GNSF" %}
|
||||
int gnsf_nx1 = {{ dims.gnsf_nx1 }};
|
||||
int gnsf_nz1 = {{ dims.gnsf_nz1 }};
|
||||
int gnsf_nout = {{ dims.gnsf_nout }};
|
||||
int gnsf_ny = {{ dims.gnsf_ny }};
|
||||
int gnsf_nuhat = {{ dims.gnsf_nuhat }};
|
||||
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nx1", &gnsf_nx1);
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nz1", &gnsf_nz1);
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nout", &gnsf_nout);
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "ny", &gnsf_ny);
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nuhat", &gnsf_nuhat);
|
||||
{% endif %}
|
||||
|
||||
// sim opts
|
||||
sim_opts *{{ model.name }}_sim_opts = sim_opts_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims);
|
||||
capsule->acados_sim_opts = {{ model.name }}_sim_opts;
|
||||
int tmp_int = {{ solver_options.sim_method_newton_iter }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "newton_iter", &tmp_int);
|
||||
double tmp_double = {{ solver_options.sim_method_newton_tol }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "newton_tol", &tmp_double);
|
||||
sim_collocation_type collocation_type = {{ solver_options.collocation_type }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "collocation_type", &collocation_type);
|
||||
|
||||
{% if problem_class == "SIM" %}
|
||||
tmp_int = {{ solver_options.sim_method_num_stages }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_stages", &tmp_int);
|
||||
tmp_int = {{ solver_options.sim_method_num_steps }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_steps", &tmp_int);
|
||||
|
||||
// options that are not available to AcadosOcpSolver
|
||||
// (in OCP they will be determined by other options, like exact_hessian)
|
||||
tmp_bool = {{ solver_options.sens_forw }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_forw", &tmp_bool);
|
||||
tmp_bool = {{ solver_options.sens_adj }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_adj", &tmp_bool);
|
||||
tmp_bool = {{ solver_options.sens_algebraic }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_algebraic", &tmp_bool);
|
||||
tmp_bool = {{ solver_options.sens_hess }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_hess", &tmp_bool);
|
||||
tmp_bool = {{ solver_options.output_z }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "output_z", &tmp_bool);
|
||||
|
||||
{% else %} {# num_stages and num_steps of first shooting interval are used #}
|
||||
tmp_int = {{ solver_options.sim_method_num_stages[0] }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_stages", &tmp_int);
|
||||
tmp_int = {{ solver_options.sim_method_num_steps[0] }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_steps", &tmp_int);
|
||||
tmp_bool = {{ solver_options.sim_method_jac_reuse[0] }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "jac_reuse", &tmp_bool);
|
||||
{% endif %}
|
||||
|
||||
// sim in / out
|
||||
sim_in *{{ model.name }}_sim_in = sim_in_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims);
|
||||
capsule->acados_sim_in = {{ model.name }}_sim_in;
|
||||
sim_out *{{ model.name }}_sim_out = sim_out_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims);
|
||||
capsule->acados_sim_out = {{ model.name }}_sim_out;
|
||||
|
||||
sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
|
||||
{{ model.name }}_sim_in, "T", &Tsim);
|
||||
|
||||
// model functions
|
||||
{%- if solver_options.integrator_type == "IRK" %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"impl_ode_fun", capsule->sim_impl_dae_fun);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"impl_ode_fun_jac_x_xdot", capsule->sim_impl_dae_fun_jac_x_xdot_z);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"impl_ode_jac_x_xdot_u", capsule->sim_impl_dae_jac_x_xdot_u_z);
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"impl_dae_hess", capsule->sim_impl_dae_hess);
|
||||
{%- endif %}
|
||||
|
||||
{%- elif solver_options.integrator_type == "ERK" %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"expl_vde_forw", capsule->sim_forw_vde_casadi);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"expl_vde_adj", capsule->sim_vde_adj_casadi);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"expl_ode_hess", capsule->sim_expl_ode_hess);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"phi_fun", capsule->sim_gnsf_phi_fun);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"phi_fun_jac_y", capsule->sim_gnsf_phi_fun_jac_y);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"phi_jac_y_uhat", capsule->sim_gnsf_phi_jac_y_uhat);
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"f_lo_jac_x1_x1dot_u_z", capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"gnsf_get_matrices_fun", capsule->sim_gnsf_get_matrices_fun);
|
||||
{%- endif %}
|
||||
|
||||
// sim solver
|
||||
sim_solver *{{ model.name }}_sim_solver = sim_solver_create({{ model.name }}_sim_config,
|
||||
{{ model.name }}_sim_dims, {{ model.name }}_sim_opts);
|
||||
capsule->acados_sim_solver = {{ model.name }}_sim_solver;
|
||||
|
||||
{% if dims.np > 0 %}
|
||||
/* initialize parameter values */
|
||||
double* p = calloc(np, sizeof(double));
|
||||
{% for item in parameter_values %}
|
||||
{%- if item != 0 %}
|
||||
p[{{ loop.index0 }}] = {{ item }};
|
||||
{%- endif %}
|
||||
{%- endfor %}
|
||||
|
||||
{{ model.name }}_acados_sim_update_params(capsule, p, np);
|
||||
free(p);
|
||||
{% endif %}{# if dims.np #}
|
||||
|
||||
/* initialize input */
|
||||
// x
|
||||
double x0[{{ dims.nx }}];
|
||||
for (int ii = 0; ii < {{ dims.nx }}; ii++)
|
||||
x0[ii] = 0.0;
|
||||
|
||||
sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
|
||||
{{ model.name }}_sim_in, "x", x0);
|
||||
|
||||
|
||||
// u
|
||||
double u0[{{ dims.nu }}];
|
||||
for (int ii = 0; ii < {{ dims.nu }}; ii++)
|
||||
u0[ii] = 0.0;
|
||||
|
||||
sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
|
||||
{{ model.name }}_sim_in, "u", u0);
|
||||
|
||||
// S_forw
|
||||
double S_forw[{{ dims.nx * (dims.nx + dims.nu) }}];
|
||||
for (int ii = 0; ii < {{ dims.nx * (dims.nx + dims.nu) }}; ii++)
|
||||
S_forw[ii] = 0.0;
|
||||
for (int ii = 0; ii < {{ dims.nx }}; ii++)
|
||||
S_forw[ii + ii * {{ dims.nx }} ] = 1.0;
|
||||
|
||||
|
||||
sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
|
||||
{{ model.name }}_sim_in, "S_forw", S_forw);
|
||||
|
||||
int status = sim_precompute({{ model.name }}_sim_solver, {{ model.name }}_sim_in, {{ model.name }}_sim_out);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int {{ model.name }}_acados_sim_solve(sim_solver_capsule *capsule)
|
||||
{
|
||||
// integrate dynamics using acados sim_solver
|
||||
int status = sim_solve(capsule->acados_sim_solver,
|
||||
capsule->acados_sim_in, capsule->acados_sim_out);
|
||||
if (status != 0)
|
||||
printf("error in {{ model.name }}_acados_sim_solve()! Exiting.\n");
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule)
|
||||
{
|
||||
// free memory
|
||||
sim_solver_destroy(capsule->acados_sim_solver);
|
||||
sim_in_destroy(capsule->acados_sim_in);
|
||||
sim_out_destroy(capsule->acados_sim_out);
|
||||
sim_opts_destroy(capsule->acados_sim_opts);
|
||||
sim_dims_destroy(capsule->acados_sim_dims);
|
||||
sim_config_destroy(capsule->acados_sim_config);
|
||||
|
||||
// free external function
|
||||
{%- if solver_options.integrator_type == "IRK" %}
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_fun);
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_fun_jac_x_xdot_z);
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_jac_x_xdot_u_z);
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_hess);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "ERK" %}
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_forw_vde_casadi);
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_vde_adj_casadi);
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_expl_ode_fun_casadi);
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_expl_ode_hess);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_phi_fun);
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_phi_fun_jac_y);
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_phi_jac_y_uhat);
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_get_matrices_fun);
|
||||
{% endif %}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
|
||||
{
|
||||
int status = 0;
|
||||
int casadi_np = {{ model.name | upper }}_NP;
|
||||
|
||||
if (casadi_np != np) {
|
||||
printf("{{ model.name }}_acados_sim_update_params: trying to set %i parameters for external functions."
|
||||
" External function has %i parameters. Exiting.\n", np, casadi_np);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
{%- if solver_options.integrator_type == "ERK" %}
|
||||
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
|
||||
capsule->sim_vde_adj_casadi[0].set_param(capsule->sim_vde_adj_casadi, p);
|
||||
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
capsule->sim_expl_ode_hess[0].set_param(capsule->sim_expl_ode_hess, p);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
capsule->sim_impl_dae_fun[0].set_param(capsule->sim_impl_dae_fun, p);
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z[0].set_param(capsule->sim_impl_dae_fun_jac_x_xdot_z, p);
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z[0].set_param(capsule->sim_impl_dae_jac_x_xdot_u_z, p);
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
capsule->sim_impl_dae_hess[0].set_param(capsule->sim_impl_dae_hess, p);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
capsule->sim_gnsf_phi_fun[0].set_param(capsule->sim_gnsf_phi_fun, p);
|
||||
capsule->sim_gnsf_phi_fun_jac_y[0].set_param(capsule->sim_gnsf_phi_fun_jac_y, p);
|
||||
capsule->sim_gnsf_phi_jac_y_uhat[0].set_param(capsule->sim_gnsf_phi_jac_y_uhat, p);
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p);
|
||||
{% endif %}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
/* getters pointers to C objects*/
|
||||
sim_config * {{ model.name }}_acados_get_sim_config(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_config;
|
||||
};
|
||||
|
||||
sim_in * {{ model.name }}_acados_get_sim_in(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_in;
|
||||
};
|
||||
|
||||
sim_out * {{ model.name }}_acados_get_sim_out(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_out;
|
||||
};
|
||||
|
||||
void * {{ model.name }}_acados_get_sim_dims(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_dims;
|
||||
};
|
||||
|
||||
sim_opts * {{ model.name }}_acados_get_sim_opts(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_opts;
|
||||
};
|
||||
|
||||
sim_solver * {{ model.name }}_acados_get_sim_solver(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_solver;
|
||||
};
|
||||
|
||||
101
third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h
vendored
Normal file
101
third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h
vendored
Normal file
@@ -0,0 +1,101 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#ifndef ACADOS_SIM_{{ model.name }}_H_
|
||||
#define ACADOS_SIM_{{ model.name }}_H_
|
||||
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#define {{ model.name | upper }}_NX {{ dims.nx }}
|
||||
#define {{ model.name | upper }}_NZ {{ dims.nz }}
|
||||
#define {{ model.name | upper }}_NU {{ dims.nu }}
|
||||
#define {{ model.name | upper }}_NP {{ dims.np }}
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
// ** capsule for solver data **
|
||||
typedef struct sim_solver_capsule
|
||||
{
|
||||
// acados objects
|
||||
sim_in *acados_sim_in;
|
||||
sim_out *acados_sim_out;
|
||||
sim_solver *acados_sim_solver;
|
||||
sim_opts *acados_sim_opts;
|
||||
sim_config *acados_sim_config;
|
||||
void *acados_sim_dims;
|
||||
|
||||
/* external functions */
|
||||
// ERK
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} * sim_forw_vde_casadi;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} * sim_vde_adj_casadi;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} * sim_expl_ode_fun_casadi;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} * sim_expl_ode_hess;
|
||||
|
||||
// IRK
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_fun;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_fun_jac_x_xdot_z;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_jac_x_xdot_u_z;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_hess;
|
||||
|
||||
// GNSF
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_fun;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_fun_jac_y;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_jac_y_uhat;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_get_matrices_fun;
|
||||
|
||||
} sim_solver_capsule;
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_create(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_solve(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_config * {{ model.name }}_acados_get_sim_config(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_in * {{ model.name }}_acados_get_sim_in(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_out * {{ model.name }}_acados_get_sim_out(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT void * {{ model.name }}_acados_get_sim_dims(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_opts * {{ model.name }}_acados_get_sim_opts(sim_solver_capsule *capsule);
|
||||
ACADOS_SYMBOL_EXPORT sim_solver * {{ model.name }}_acados_get_sim_solver(sim_solver_capsule *capsule);
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule(void);
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SIM_{{ model.name }}_H_
|
||||
51
third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.pxd
vendored
Normal file
51
third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.pxd
vendored
Normal file
@@ -0,0 +1,51 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
cimport acados_sim_solver_common
|
||||
|
||||
cdef extern from "acados_sim_solver_{{ model.name }}.h":
|
||||
ctypedef struct sim_solver_capsule "sim_solver_capsule":
|
||||
pass
|
||||
|
||||
sim_solver_capsule * acados_sim_solver_create_capsule "{{ model.name }}_acados_sim_solver_create_capsule"()
|
||||
int acados_sim_solver_free_capsule "{{ model.name }}_acados_sim_solver_free_capsule"(sim_solver_capsule *capsule)
|
||||
|
||||
int acados_sim_create "{{ model.name }}_acados_sim_create"(sim_solver_capsule * capsule)
|
||||
int acados_sim_solve "{{ model.name }}_acados_sim_solve"(sim_solver_capsule * capsule)
|
||||
int acados_sim_free "{{ model.name }}_acados_sim_free"(sim_solver_capsule * capsule)
|
||||
int acados_sim_update_params "{{ model.name }}_acados_sim_update_params"(sim_solver_capsule * capsule, double *value, int np_)
|
||||
# int acados_sim_update_params_sparse "{{ model.name }}_acados_sim_update_params_sparse"(sim_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
|
||||
|
||||
acados_sim_solver_common.sim_in *acados_get_sim_in "{{ model.name }}_acados_get_sim_in"(sim_solver_capsule * capsule)
|
||||
acados_sim_solver_common.sim_out *acados_get_sim_out "{{ model.name }}_acados_get_sim_out"(sim_solver_capsule * capsule)
|
||||
acados_sim_solver_common.sim_solver *acados_get_sim_solver "{{ model.name }}_acados_get_sim_solver"(sim_solver_capsule * capsule)
|
||||
acados_sim_solver_common.sim_config *acados_get_sim_config "{{ model.name }}_acados_get_sim_config"(sim_solver_capsule * capsule)
|
||||
acados_sim_solver_common.sim_opts *acados_get_sim_opts "{{ model.name }}_acados_get_sim_opts"(sim_solver_capsule * capsule)
|
||||
void *acados_get_sim_dims "{{ model.name }}_acados_get_sim_dims"(sim_solver_capsule * capsule)
|
||||
2741
third_party/acados/acados_template/c_templates_tera/acados_solver.in.c
vendored
Normal file
2741
third_party/acados/acados_template/c_templates_tera/acados_solver.in.c
vendored
Normal file
File diff suppressed because it is too large
Load Diff
241
third_party/acados/acados_template/c_templates_tera/acados_solver.in.h
vendored
Normal file
241
third_party/acados/acados_template/c_templates_tera/acados_solver.in.h
vendored
Normal file
@@ -0,0 +1,241 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#ifndef ACADOS_SOLVER_{{ model.name }}_H_
|
||||
#define ACADOS_SOLVER_{{ model.name }}_H_
|
||||
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#define {{ model.name | upper }}_NX {{ dims.nx }}
|
||||
#define {{ model.name | upper }}_NZ {{ dims.nz }}
|
||||
#define {{ model.name | upper }}_NU {{ dims.nu }}
|
||||
#define {{ model.name | upper }}_NP {{ dims.np }}
|
||||
#define {{ model.name | upper }}_NBX {{ dims.nbx }}
|
||||
#define {{ model.name | upper }}_NBX0 {{ dims.nbx_0 }}
|
||||
#define {{ model.name | upper }}_NBU {{ dims.nbu }}
|
||||
#define {{ model.name | upper }}_NSBX {{ dims.nsbx }}
|
||||
#define {{ model.name | upper }}_NSBU {{ dims.nsbu }}
|
||||
#define {{ model.name | upper }}_NSH {{ dims.nsh }}
|
||||
#define {{ model.name | upper }}_NSG {{ dims.nsg }}
|
||||
#define {{ model.name | upper }}_NSPHI {{ dims.nsphi }}
|
||||
#define {{ model.name | upper }}_NSHN {{ dims.nsh_e }}
|
||||
#define {{ model.name | upper }}_NSGN {{ dims.nsg_e }}
|
||||
#define {{ model.name | upper }}_NSPHIN {{ dims.nsphi_e }}
|
||||
#define {{ model.name | upper }}_NSBXN {{ dims.nsbx_e }}
|
||||
#define {{ model.name | upper }}_NS {{ dims.ns }}
|
||||
#define {{ model.name | upper }}_NSN {{ dims.ns_e }}
|
||||
#define {{ model.name | upper }}_NG {{ dims.ng }}
|
||||
#define {{ model.name | upper }}_NBXN {{ dims.nbx_e }}
|
||||
#define {{ model.name | upper }}_NGN {{ dims.ng_e }}
|
||||
#define {{ model.name | upper }}_NY0 {{ dims.ny_0 }}
|
||||
#define {{ model.name | upper }}_NY {{ dims.ny }}
|
||||
#define {{ model.name | upper }}_NYN {{ dims.ny_e }}
|
||||
#define {{ model.name | upper }}_N {{ dims.N }}
|
||||
#define {{ model.name | upper }}_NH {{ dims.nh }}
|
||||
#define {{ model.name | upper }}_NPHI {{ dims.nphi }}
|
||||
#define {{ model.name | upper }}_NHN {{ dims.nh_e }}
|
||||
#define {{ model.name | upper }}_NPHIN {{ dims.nphi_e }}
|
||||
#define {{ model.name | upper }}_NR {{ dims.nr }}
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
{%- if not solver_options.custom_update_filename %}
|
||||
{%- set custom_update_filename = "" %}
|
||||
{% else %}
|
||||
{%- set custom_update_filename = solver_options.custom_update_filename %}
|
||||
{%- endif %}
|
||||
|
||||
// ** capsule for solver data **
|
||||
typedef struct {{ model.name }}_solver_capsule
|
||||
{
|
||||
// acados objects
|
||||
ocp_nlp_in *nlp_in;
|
||||
ocp_nlp_out *nlp_out;
|
||||
ocp_nlp_out *sens_out;
|
||||
ocp_nlp_solver *nlp_solver;
|
||||
void *nlp_opts;
|
||||
ocp_nlp_plan_t *nlp_solver_plan;
|
||||
ocp_nlp_config *nlp_config;
|
||||
ocp_nlp_dims *nlp_dims;
|
||||
|
||||
// number of expected runtime parameters
|
||||
unsigned int nlp_np;
|
||||
|
||||
/* external functions */
|
||||
// dynamics
|
||||
{% if solver_options.integrator_type == "ERK" %}
|
||||
external_function_param_casadi *forw_vde_casadi;
|
||||
external_function_param_casadi *expl_ode_fun;
|
||||
{% if solver_options.hessian_approx == "EXACT" %}
|
||||
external_function_param_casadi *hess_vde_casadi;
|
||||
{%- endif %}
|
||||
{% elif solver_options.integrator_type == "IRK" %}
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun_jac_x_xdot_z;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_jac_x_xdot_u_z;
|
||||
{% if solver_options.hessian_approx == "EXACT" %}
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_hess;
|
||||
{%- endif %}
|
||||
{% elif solver_options.integrator_type == "LIFTED_IRK" %}
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun_jac_x_xdot_u;
|
||||
{% elif solver_options.integrator_type == "GNSF" %}
|
||||
external_function_param_casadi *gnsf_phi_fun;
|
||||
external_function_param_casadi *gnsf_phi_fun_jac_y;
|
||||
external_function_param_casadi *gnsf_phi_jac_y_uhat;
|
||||
external_function_param_casadi *gnsf_f_lo_jac_x1_x1dot_u_z;
|
||||
external_function_param_casadi *gnsf_get_matrices_fun;
|
||||
{% elif solver_options.integrator_type == "DISCRETE" %}
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun;
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt;
|
||||
{%- if solver_options.hessian_approx == "EXACT" %}
|
||||
external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt_hess;
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
|
||||
// cost
|
||||
{% if cost.cost_type == "NONLINEAR_LS" %}
|
||||
external_function_param_casadi *cost_y_fun;
|
||||
external_function_param_casadi *cost_y_fun_jac_ut_xt;
|
||||
external_function_param_casadi *cost_y_hess;
|
||||
{% elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %}
|
||||
external_function_param_casadi *conl_cost_fun;
|
||||
external_function_param_casadi *conl_cost_fun_jac_hess;
|
||||
{%- elif cost.cost_type == "EXTERNAL" %}
|
||||
external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun;
|
||||
external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac;
|
||||
external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac_hess;
|
||||
{% endif %}
|
||||
|
||||
{% if cost.cost_type_0 == "NONLINEAR_LS" %}
|
||||
external_function_param_casadi cost_y_0_fun;
|
||||
external_function_param_casadi cost_y_0_fun_jac_ut_xt;
|
||||
external_function_param_casadi cost_y_0_hess;
|
||||
{% elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
|
||||
external_function_param_casadi conl_cost_0_fun;
|
||||
external_function_param_casadi conl_cost_0_fun_jac_hess;
|
||||
{% elif cost.cost_type_0 == "EXTERNAL" %}
|
||||
external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun;
|
||||
external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac;
|
||||
external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac_hess;
|
||||
{%- endif %}
|
||||
|
||||
{% if cost.cost_type_e == "NONLINEAR_LS" %}
|
||||
external_function_param_casadi cost_y_e_fun;
|
||||
external_function_param_casadi cost_y_e_fun_jac_ut_xt;
|
||||
external_function_param_casadi cost_y_e_hess;
|
||||
{% elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %}
|
||||
external_function_param_casadi conl_cost_e_fun;
|
||||
external_function_param_casadi conl_cost_e_fun_jac_hess;
|
||||
{% elif cost.cost_type_e == "EXTERNAL" %}
|
||||
external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun;
|
||||
external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac;
|
||||
external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac_hess;
|
||||
{%- endif %}
|
||||
|
||||
// constraints
|
||||
{%- if constraints.constr_type == "BGP" %}
|
||||
external_function_param_casadi *phi_constraint;
|
||||
{% elif constraints.constr_type == "BGH" and dims.nh > 0 %}
|
||||
external_function_param_casadi *nl_constr_h_fun_jac;
|
||||
external_function_param_casadi *nl_constr_h_fun;
|
||||
{%- if solver_options.hessian_approx == "EXACT" %}
|
||||
external_function_param_casadi *nl_constr_h_fun_jac_hess;
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
|
||||
{% if constraints.constr_type_e == "BGP" %}
|
||||
external_function_param_casadi phi_e_constraint;
|
||||
{% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
|
||||
external_function_param_casadi nl_constr_h_e_fun_jac;
|
||||
external_function_param_casadi nl_constr_h_e_fun;
|
||||
{%- if solver_options.hessian_approx == "EXACT" %}
|
||||
external_function_param_casadi nl_constr_h_e_fun_jac_hess;
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if custom_update_filename != "" %}
|
||||
void * custom_update_memory;
|
||||
{%- endif %}
|
||||
|
||||
} {{ model.name }}_solver_capsule;
|
||||
|
||||
ACADOS_SYMBOL_EXPORT {{ model.name }}_solver_capsule * {{ model.name }}_acados_create_capsule(void);
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free_capsule({{ model.name }}_solver_capsule *capsule);
|
||||
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_create({{ model.name }}_solver_capsule * capsule);
|
||||
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule, int reset_qp_solver_mem);
|
||||
|
||||
/**
|
||||
* Generic version of {{ model.name }}_acados_create which allows to use a different number of shooting intervals than
|
||||
* the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code
|
||||
* generation, the time-steps from code generation is used.
|
||||
*/
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule * capsule, int n_time_steps, double* new_time_steps);
|
||||
/**
|
||||
* Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the
|
||||
* nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0.
|
||||
*/
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps);
|
||||
/**
|
||||
* This function is used for updating an already initialized solver with a different number of qp_cond_N.
|
||||
*/
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule * capsule, int qp_solver_cond_N);
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *value, int np);
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_params_sparse({{ model.name }}_solver_capsule * capsule, int stage, int *idx, double *p, int n_update);
|
||||
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_custom_update({{ model.name }}_solver_capsule* capsule, double* data, int data_len);
|
||||
|
||||
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule);
|
||||
ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SOLVER_{{ model.name }}_H_
|
||||
62
third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd
vendored
Normal file
62
third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd
vendored
Normal file
@@ -0,0 +1,62 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
cimport acados_solver_common
|
||||
|
||||
cdef extern from "acados_solver_{{ model.name }}.h":
|
||||
ctypedef struct nlp_solver_capsule "{{ model.name }}_solver_capsule":
|
||||
pass
|
||||
|
||||
nlp_solver_capsule * acados_create_capsule "{{ model.name }}_acados_create_capsule"()
|
||||
int acados_free_capsule "{{ model.name }}_acados_free_capsule"(nlp_solver_capsule *capsule)
|
||||
|
||||
int acados_create "{{ model.name }}_acados_create"(nlp_solver_capsule * capsule)
|
||||
|
||||
int acados_create_with_discretization "{{ model.name }}_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps)
|
||||
int acados_update_time_steps "{{ model.name }}_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps)
|
||||
int acados_update_qp_solver_cond_N "{{ model.name }}_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
|
||||
|
||||
int acados_update_params "{{ model.name }}_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
|
||||
int acados_update_params_sparse "{{ model.name }}_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update)
|
||||
int acados_solve "{{ model.name }}_acados_solve"(nlp_solver_capsule * capsule)
|
||||
int acados_reset "{{ model.name }}_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem)
|
||||
int acados_free "{{ model.name }}_acados_free"(nlp_solver_capsule * capsule)
|
||||
void acados_print_stats "{{ model.name }}_acados_print_stats"(nlp_solver_capsule * capsule)
|
||||
|
||||
int acados_custom_update "{{ model.name }}_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len)
|
||||
|
||||
acados_solver_common.ocp_nlp_in *acados_get_nlp_in "{{ model.name }}_acados_get_nlp_in"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_out *acados_get_nlp_out "{{ model.name }}_acados_get_nlp_out"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_out *acados_get_sens_out "{{ model.name }}_acados_get_sens_out"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "{{ model.name }}_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_config *acados_get_nlp_config "{{ model.name }}_acados_get_nlp_config"(nlp_solver_capsule * capsule)
|
||||
void *acados_get_nlp_opts "{{ model.name }}_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "{{ model.name }}_acados_get_nlp_dims"(nlp_solver_capsule * capsule)
|
||||
acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "{{ model.name }}_acados_get_nlp_plan"(nlp_solver_capsule * capsule)
|
||||
110
third_party/acados/acados_template/c_templates_tera/constraints.in.h
vendored
Normal file
110
third_party/acados/acados_template/c_templates_tera/constraints.in.h
vendored
Normal file
@@ -0,0 +1,110 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#ifndef {{ model.name }}_CONSTRAINTS
|
||||
#define {{ model.name }}_CONSTRAINTS
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
{% if dims.nphi > 0 %}
|
||||
int {{ model.name }}_phi_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_phi_constraint_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_phi_constraint_sparsity_in(int);
|
||||
const int *{{ model.name }}_phi_constraint_sparsity_out(int);
|
||||
int {{ model.name }}_phi_constraint_n_in(void);
|
||||
int {{ model.name }}_phi_constraint_n_out(void);
|
||||
{% endif %}
|
||||
|
||||
{% if dims.nphi_e > 0 %}
|
||||
int {{ model.name }}_phi_e_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_phi_e_constraint_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_phi_e_constraint_sparsity_in(int);
|
||||
const int *{{ model.name }}_phi_e_constraint_sparsity_out(int);
|
||||
int {{ model.name }}_phi_e_constraint_n_in(void);
|
||||
int {{ model.name }}_phi_e_constraint_n_out(void);
|
||||
{% endif %}
|
||||
|
||||
{% if dims.nh > 0 %}
|
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in(int);
|
||||
const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out(int);
|
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(void);
|
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(void);
|
||||
|
||||
int {{ model.name }}_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_constr_h_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_constr_h_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_constr_h_fun_sparsity_out(int);
|
||||
int {{ model.name }}_constr_h_fun_n_in(void);
|
||||
int {{ model.name }}_constr_h_fun_n_out(void);
|
||||
|
||||
{% if solver_options.hessian_approx == "EXACT" -%}
|
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out(int);
|
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(void);
|
||||
int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(void);
|
||||
{% endif %}
|
||||
{% endif %}
|
||||
|
||||
{% if dims.nh_e > 0 %}
|
||||
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_in(int);
|
||||
const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_out(int);
|
||||
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in(void);
|
||||
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out(void);
|
||||
|
||||
int {{ model.name }}_constr_h_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_constr_h_e_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_constr_h_e_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_constr_h_e_fun_sparsity_out(int);
|
||||
int {{ model.name }}_constr_h_e_fun_n_in(void);
|
||||
int {{ model.name }}_constr_h_e_fun_n_out(void);
|
||||
|
||||
{% if solver_options.hessian_approx == "EXACT" -%}
|
||||
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_out(int);
|
||||
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in(void);
|
||||
int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out(void);
|
||||
{% endif %}
|
||||
{% endif %}
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // {{ model.name }}_CONSTRAINTS
|
||||
238
third_party/acados/acados_template/c_templates_tera/cost.in.h
vendored
Normal file
238
third_party/acados/acados_template/c_templates_tera/cost.in.h
vendored
Normal file
@@ -0,0 +1,238 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef {{ model.name }}_COST
|
||||
#define {{ model.name }}_COST
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
// Cost at initial shooting node
|
||||
{% if cost.cost_type_0 == "NONLINEAR_LS" %}
|
||||
int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int);
|
||||
int {{ model.name }}_cost_y_0_fun_n_in(void);
|
||||
int {{ model.name }}_cost_y_0_fun_n_out(void);
|
||||
|
||||
int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
|
||||
int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(void);
|
||||
int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(void);
|
||||
|
||||
int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int);
|
||||
int {{ model.name }}_cost_y_0_hess_n_in(void);
|
||||
int {{ model.name }}_cost_y_0_hess_n_out(void);
|
||||
{% elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %}
|
||||
|
||||
int {{ model.name }}_conl_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_conl_cost_0_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_conl_cost_0_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_conl_cost_0_fun_sparsity_out(int);
|
||||
int {{ model.name }}_conl_cost_0_fun_n_in(void);
|
||||
int {{ model.name }}_conl_cost_0_fun_n_out(void);
|
||||
|
||||
int {{ model.name }}_conl_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_conl_cost_0_fun_jac_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_conl_cost_0_fun_jac_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_conl_cost_0_fun_jac_hess_sparsity_out(int);
|
||||
int {{ model.name }}_conl_cost_0_fun_jac_hess_n_in(void);
|
||||
int {{ model.name }}_conl_cost_0_fun_jac_hess_n_out(void);
|
||||
|
||||
{% elif cost.cost_type_0 == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type_0 == "casadi" %}
|
||||
int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int);
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_n_in(void);
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_n_out(void);
|
||||
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int);
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(void);
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(void);
|
||||
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int);
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(void);
|
||||
int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(void);
|
||||
{%- else %}
|
||||
int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *);
|
||||
{%- endif %}
|
||||
{% endif %}
|
||||
|
||||
|
||||
// Cost at path shooting node
|
||||
{% if cost.cost_type == "NONLINEAR_LS" %}
|
||||
int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_y_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_y_fun_sparsity_out(int);
|
||||
int {{ model.name }}_cost_y_fun_n_in(void);
|
||||
int {{ model.name }}_cost_y_fun_n_out(void);
|
||||
|
||||
int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int);
|
||||
int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(void);
|
||||
int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(void);
|
||||
|
||||
int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_y_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_y_hess_sparsity_out(int);
|
||||
int {{ model.name }}_cost_y_hess_n_in(void);
|
||||
int {{ model.name }}_cost_y_hess_n_out(void);
|
||||
|
||||
{% elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %}
|
||||
int {{ model.name }}_conl_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_conl_cost_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_conl_cost_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_conl_cost_fun_sparsity_out(int);
|
||||
int {{ model.name }}_conl_cost_fun_n_in(void);
|
||||
int {{ model.name }}_conl_cost_fun_n_out(void);
|
||||
|
||||
int {{ model.name }}_conl_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_conl_cost_fun_jac_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_conl_cost_fun_jac_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_conl_cost_fun_jac_hess_sparsity_out(int);
|
||||
int {{ model.name }}_conl_cost_fun_jac_hess_n_in(void);
|
||||
int {{ model.name }}_conl_cost_fun_jac_hess_n_out(void);
|
||||
{% elif cost.cost_type == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type == "casadi" %}
|
||||
int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int);
|
||||
int {{ model.name }}_cost_ext_cost_fun_n_in(void);
|
||||
int {{ model.name }}_cost_ext_cost_fun_n_out(void);
|
||||
|
||||
int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int);
|
||||
int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(void);
|
||||
int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(void);
|
||||
|
||||
int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int);
|
||||
int {{ model.name }}_cost_ext_cost_fun_jac_n_in(void);
|
||||
int {{ model.name }}_cost_ext_cost_fun_jac_n_out(void);
|
||||
{%- else %}
|
||||
int {{ cost.cost_function_ext_cost }}(void **, void **, void *);
|
||||
{%- endif %}
|
||||
{% endif %}
|
||||
|
||||
// Cost at terminal shooting node
|
||||
{% if cost.cost_type_e == "NONLINEAR_LS" %}
|
||||
int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int);
|
||||
int {{ model.name }}_cost_y_e_fun_n_in(void);
|
||||
int {{ model.name }}_cost_y_e_fun_n_out(void);
|
||||
|
||||
int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
|
||||
int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(void);
|
||||
int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(void);
|
||||
|
||||
int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int);
|
||||
int {{ model.name }}_cost_y_e_hess_n_in(void);
|
||||
int {{ model.name }}_cost_y_e_hess_n_out(void);
|
||||
{% elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %}
|
||||
int {{ model.name }}_conl_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_conl_cost_e_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_conl_cost_e_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_conl_cost_e_fun_sparsity_out(int);
|
||||
int {{ model.name }}_conl_cost_e_fun_n_in(void);
|
||||
int {{ model.name }}_conl_cost_e_fun_n_out(void);
|
||||
|
||||
int {{ model.name }}_conl_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_conl_cost_e_fun_jac_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_conl_cost_e_fun_jac_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_conl_cost_e_fun_jac_hess_sparsity_out(int);
|
||||
int {{ model.name }}_conl_cost_e_fun_jac_hess_n_in(void);
|
||||
int {{ model.name }}_conl_cost_e_fun_jac_hess_n_out(void);
|
||||
{% elif cost.cost_type_e == "EXTERNAL" %}
|
||||
{%- if cost.cost_ext_fun_type_e == "casadi" %}
|
||||
int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int);
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_n_in(void);
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_n_out(void);
|
||||
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int);
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(void);
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(void);
|
||||
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int);
|
||||
const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int);
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(void);
|
||||
int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(void);
|
||||
{%- else %}
|
||||
int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *);
|
||||
{%- endif %}
|
||||
{% endif %}
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // {{ model.name }}_COST
|
||||
226
third_party/acados/acados_template/c_templates_tera/main.in.c
vendored
Normal file
226
third_party/acados/acados_template/c_templates_tera/main.in.c
vendored
Normal file
@@ -0,0 +1,226 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
{%- if not solver_options.custom_update_filename %}
|
||||
{%- set custom_update_filename = "" %}
|
||||
{% else %}
|
||||
{%- set custom_update_filename = solver_options.custom_update_filename %}
|
||||
{%- endif %}
|
||||
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados/utils/math.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
|
||||
|
||||
#define NX {{ model.name | upper }}_NX
|
||||
#define NZ {{ model.name | upper }}_NZ
|
||||
#define NU {{ model.name | upper }}_NU
|
||||
#define NP {{ model.name | upper }}_NP
|
||||
#define NBX {{ model.name | upper }}_NBX
|
||||
#define NBX0 {{ model.name | upper }}_NBX0
|
||||
#define NBU {{ model.name | upper }}_NBU
|
||||
#define NSBX {{ model.name | upper }}_NSBX
|
||||
#define NSBU {{ model.name | upper }}_NSBU
|
||||
#define NSH {{ model.name | upper }}_NSH
|
||||
#define NSG {{ model.name | upper }}_NSG
|
||||
#define NSPHI {{ model.name | upper }}_NSPHI
|
||||
#define NSHN {{ model.name | upper }}_NSHN
|
||||
#define NSGN {{ model.name | upper }}_NSGN
|
||||
#define NSPHIN {{ model.name | upper }}_NSPHIN
|
||||
#define NSBXN {{ model.name | upper }}_NSBXN
|
||||
#define NS {{ model.name | upper }}_NS
|
||||
#define NSN {{ model.name | upper }}_NSN
|
||||
#define NG {{ model.name | upper }}_NG
|
||||
#define NBXN {{ model.name | upper }}_NBXN
|
||||
#define NGN {{ model.name | upper }}_NGN
|
||||
#define NY0 {{ model.name | upper }}_NY0
|
||||
#define NY {{ model.name | upper }}_NY
|
||||
#define NYN {{ model.name | upper }}_NYN
|
||||
#define NH {{ model.name | upper }}_NH
|
||||
#define NPHI {{ model.name | upper }}_NPHI
|
||||
#define NHN {{ model.name | upper }}_NHN
|
||||
#define NPHIN {{ model.name | upper }}_NPHIN
|
||||
#define NR {{ model.name | upper }}_NR
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
|
||||
{{ model.name }}_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
|
||||
// there is an opportunity to change the number of shooting intervals in C without new code generation
|
||||
int N = {{ model.name | upper }}_N;
|
||||
// allocate the array and fill it accordingly
|
||||
double* new_time_steps = NULL;
|
||||
int status = {{ model.name }}_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps);
|
||||
|
||||
if (status)
|
||||
{
|
||||
printf("{{ model.name }}_acados_create() returned status %d. Exiting.\n", status);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule);
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule);
|
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule);
|
||||
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(acados_ocp_capsule);
|
||||
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(acados_ocp_capsule);
|
||||
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule);
|
||||
|
||||
// initial condition
|
||||
int idxbx0[NBX0];
|
||||
{%- for i in range(end=dims.nbx_0) %}
|
||||
idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }};
|
||||
{%- endfor %}
|
||||
|
||||
double lbx0[NBX0];
|
||||
double ubx0[NBX0];
|
||||
{%- for i in range(end=dims.nbx_0) %}
|
||||
lbx0[{{ i }}] = {{ constraints.lbx_0[i] }};
|
||||
ubx0[{{ i }}] = {{ constraints.ubx_0[i] }};
|
||||
{%- endfor %}
|
||||
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
|
||||
|
||||
// initialization for state values
|
||||
double x_init[NX];
|
||||
{%- for i in range(end=dims.nx) %}
|
||||
x_init[{{ i }}] = 0.0;
|
||||
{%- endfor %}
|
||||
|
||||
// initial value for control input
|
||||
double u0[NU];
|
||||
{%- for i in range(end=dims.nu) %}
|
||||
u0[{{ i }}] = 0.0;
|
||||
{%- endfor %}
|
||||
|
||||
|
||||
{%- if dims.np > 0 %}
|
||||
// set parameters
|
||||
double p[NP];
|
||||
{%- for item in parameter_values %}
|
||||
p[{{ loop.index0 }}] = {{ item }};
|
||||
{%- endfor %}
|
||||
|
||||
for (int ii = 0; ii <= N; ii++)
|
||||
{
|
||||
{{ model.name }}_acados_update_params(acados_ocp_capsule, ii, p, NP);
|
||||
}
|
||||
{% endif %}{# if np > 0 #}
|
||||
|
||||
// prepare evaluation
|
||||
int NTIMINGS = 1;
|
||||
double min_time = 1e12;
|
||||
double kkt_norm_inf;
|
||||
double elapsed_time;
|
||||
int sqp_iter;
|
||||
|
||||
double xtraj[NX * (N+1)];
|
||||
double utraj[NU * N];
|
||||
|
||||
|
||||
// solve ocp in loop
|
||||
int rti_phase = 0;
|
||||
|
||||
for (int ii = 0; ii < NTIMINGS; ii++)
|
||||
{
|
||||
// initialize solution
|
||||
for (int i = 0; i < N; i++)
|
||||
{
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
|
||||
}
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x_init);
|
||||
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
|
||||
status = {{ model.name }}_acados_solve(acados_ocp_capsule);
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
|
||||
min_time = MIN(elapsed_time, min_time);
|
||||
}
|
||||
|
||||
/* print solution and statistics */
|
||||
for (int ii = 0; ii <= nlp_dims->N; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]);
|
||||
for (int ii = 0; ii < nlp_dims->N; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]);
|
||||
|
||||
printf("\n--- xtraj ---\n");
|
||||
d_print_exp_tran_mat( NX, N+1, xtraj, NX);
|
||||
printf("\n--- utraj ---\n");
|
||||
d_print_exp_tran_mat( NU, N, utraj, NU );
|
||||
// ocp_nlp_out_print(nlp_solver->dims, nlp_out);
|
||||
|
||||
printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
|
||||
|
||||
if (status == ACADOS_SUCCESS)
|
||||
{
|
||||
printf("{{ model.name }}_acados_solve(): SUCCESS!\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("{{ model.name }}_acados_solve() failed with status %d.\n", status);
|
||||
}
|
||||
|
||||
|
||||
{%- if custom_update_filename != "" %}
|
||||
{{ model.name }}_acados_custom_update(acados_ocp_capsule, xtraj, NX*(N+1));
|
||||
{%- endif %}
|
||||
|
||||
// get solution
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
|
||||
|
||||
{{ model.name }}_acados_print_stats(acados_ocp_capsule);
|
||||
|
||||
printf("\nSolver info:\n");
|
||||
printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n",
|
||||
sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf);
|
||||
|
||||
// free solver
|
||||
status = {{ model.name }}_acados_free(acados_ocp_capsule);
|
||||
if (status) {
|
||||
printf("{{ model.name }}_acados_free() returned status %d. \n", status);
|
||||
}
|
||||
// free solver capsule
|
||||
status = {{ model.name }}_acados_free_capsule(acados_ocp_capsule);
|
||||
if (status) {
|
||||
printf("{{ model.name }}_acados_free_capsule() returned status %d. \n", status);
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
132
third_party/acados/acados_template/c_templates_tera/main_sim.in.c
vendored
Normal file
132
third_party/acados/acados_template/c_templates_tera/main_sim.in.c
vendored
Normal file
@@ -0,0 +1,132 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados/utils/math.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_sim_solver_{{ model.name }}.h"
|
||||
|
||||
#define NX {{ model.name | upper }}_NX
|
||||
#define NZ {{ model.name | upper }}_NZ
|
||||
#define NU {{ model.name | upper }}_NU
|
||||
#define NP {{ model.name | upper }}_NP
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
int status = 0;
|
||||
sim_solver_capsule *capsule = {{ model.name }}_acados_sim_solver_create_capsule();
|
||||
status = {{ model.name }}_acados_sim_create(capsule);
|
||||
|
||||
if (status)
|
||||
{
|
||||
printf("acados_create() returned status %d. Exiting.\n", status);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
sim_config *acados_sim_config = {{ model.name }}_acados_get_sim_config(capsule);
|
||||
sim_in *acados_sim_in = {{ model.name }}_acados_get_sim_in(capsule);
|
||||
sim_out *acados_sim_out = {{ model.name }}_acados_get_sim_out(capsule);
|
||||
void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule);
|
||||
|
||||
// initial condition
|
||||
double x_current[NX];
|
||||
{%- for i in range(end=dims.nx) %}
|
||||
x_current[{{ i }}] = 0.0;
|
||||
{%- endfor %}
|
||||
|
||||
{% if constraints.lbx_0 %}
|
||||
{%- for i in range(end=dims.nbx_0) %}
|
||||
x_current[{{ constraints.idxbx_0[i] }}] = {{ constraints.lbx_0[i] }};
|
||||
{%- endfor %}
|
||||
{% if dims.nbx_0 != dims.nx %}
|
||||
printf("main_sim: NOTE: initial state not fully defined via lbx_0, using 0.0 for indices that are not in idxbx_0.");
|
||||
{%- endif %}
|
||||
{% else %}
|
||||
printf("main_sim: initial state not defined, should be in lbx_0, using zero vector.");
|
||||
{%- endif %}
|
||||
|
||||
|
||||
// initial value for control input
|
||||
double u0[NU];
|
||||
{%- for i in range(end=dims.nu) %}
|
||||
u0[{{ i }}] = 0.0;
|
||||
{%- endfor %}
|
||||
|
||||
{%- if dims.np > 0 %}
|
||||
// set parameters
|
||||
double p[NP];
|
||||
{%- for item in parameter_values %}
|
||||
p[{{ loop.index0 }}] = {{ item }};
|
||||
{%- endfor %}
|
||||
|
||||
{{ model.name }}_acados_sim_update_params(capsule, p, NP);
|
||||
{% endif %}{# if np > 0 #}
|
||||
|
||||
int n_sim_steps = 3;
|
||||
// solve ocp in loop
|
||||
for (int ii = 0; ii < n_sim_steps; ii++)
|
||||
{
|
||||
sim_in_set(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_in, "x", x_current);
|
||||
status = {{ model.name }}_acados_sim_solve(capsule);
|
||||
|
||||
if (status != ACADOS_SUCCESS)
|
||||
{
|
||||
printf("acados_solve() failed with status %d.\n", status);
|
||||
}
|
||||
|
||||
sim_out_get(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_out, "x", x_current);
|
||||
|
||||
printf("\nx_current, %d\n", ii);
|
||||
for (int jj = 0; jj < NX; jj++)
|
||||
{
|
||||
printf("%e\n", x_current[jj]);
|
||||
}
|
||||
}
|
||||
|
||||
printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
|
||||
|
||||
// free solver
|
||||
status = {{ model.name }}_acados_sim_free(capsule);
|
||||
if (status) {
|
||||
printf("{{ model.name }}_acados_sim_free() returned status %d. \n", status);
|
||||
}
|
||||
|
||||
{{ model.name }}_acados_sim_solver_free_capsule(capsule);
|
||||
|
||||
return status;
|
||||
}
|
||||
384
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c
vendored
Normal file
384
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c
vendored
Normal file
@@ -0,0 +1,384 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
|
||||
// mex
|
||||
#include "mex.h"
|
||||
|
||||
|
||||
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
||||
{
|
||||
long long *l_ptr;
|
||||
int status = 0;
|
||||
|
||||
// create solver
|
||||
{{ model.name }}_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
|
||||
|
||||
status = {{ model.name }}_acados_create(acados_ocp_capsule);
|
||||
|
||||
if (status)
|
||||
{
|
||||
mexPrintf("{{ model.name }}_acados_create() returned status %d.\n", status);
|
||||
}
|
||||
mexPrintf("{{ model.name }}_acados_create() -> success!\n");
|
||||
|
||||
// get pointers to nlp solver related objects
|
||||
ocp_nlp_plan_t *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
|
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule);
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule);
|
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule);
|
||||
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(acados_ocp_capsule);
|
||||
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(acados_ocp_capsule);
|
||||
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule);
|
||||
|
||||
// mexPrintf("acados: got pointer to objectes!\n");
|
||||
|
||||
// field names of output struct
|
||||
#define FIELDS_OCP 9
|
||||
#define FIELDS_EXT_FUN 25
|
||||
#define MAX_FIELDS 25
|
||||
char *fieldnames[MAX_FIELDS];
|
||||
|
||||
for (int i = 0; i < MAX_FIELDS; i++)
|
||||
{
|
||||
fieldnames[i] = (char*) mxMalloc(50);
|
||||
}
|
||||
|
||||
memcpy(fieldnames[0],"config",sizeof("config"));
|
||||
memcpy(fieldnames[1],"dims",sizeof("dims"));
|
||||
memcpy(fieldnames[2],"opts",sizeof("opts"));
|
||||
memcpy(fieldnames[3],"in",sizeof("in"));
|
||||
memcpy(fieldnames[4],"out",sizeof("out"));
|
||||
memcpy(fieldnames[5],"solver",sizeof("solver"));
|
||||
memcpy(fieldnames[6],"sens_out",sizeof("sens_out"));
|
||||
memcpy(fieldnames[7],"plan",sizeof("plan"));
|
||||
memcpy(fieldnames[8],"capsule",sizeof("capsule"));
|
||||
|
||||
// create output struct - C_ocp
|
||||
plhs[0] = mxCreateStructMatrix(1, 1, 9, (const char **) fieldnames);
|
||||
|
||||
// MEX: config, dims, opts, in, out, solver, sens_out, plan
|
||||
// plan
|
||||
mxArray *plan_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(plan_mat);
|
||||
l_ptr[0] = (long long) nlp_plan;
|
||||
mxSetField(plhs[0], 0, "plan", plan_mat);
|
||||
|
||||
// config
|
||||
mxArray *config_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(config_mat);
|
||||
l_ptr[0] = (long long) nlp_config;
|
||||
mxSetField(plhs[0], 0, "config", config_mat);
|
||||
|
||||
// dims
|
||||
mxArray *dims_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(dims_mat);
|
||||
l_ptr[0] = (long long) nlp_dims;
|
||||
mxSetField(plhs[0], 0, "dims", dims_mat);
|
||||
|
||||
// opts
|
||||
mxArray *opts_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(opts_mat);
|
||||
l_ptr[0] = (long long) nlp_opts;
|
||||
mxSetField(plhs[0], 0, "opts", opts_mat);
|
||||
|
||||
// in
|
||||
mxArray *in_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(in_mat);
|
||||
l_ptr[0] = (long long) nlp_in;
|
||||
mxSetField(plhs[0], 0, "in", in_mat);
|
||||
|
||||
// out
|
||||
mxArray *out_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(out_mat);
|
||||
l_ptr[0] = (long long) nlp_out;
|
||||
mxSetField(plhs[0], 0, "out", out_mat);
|
||||
|
||||
// solver
|
||||
mxArray *solver_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(solver_mat);
|
||||
l_ptr[0] = (long long) nlp_solver;
|
||||
mxSetField(plhs[0], 0, "solver", solver_mat);
|
||||
|
||||
// TODO: sens_out not actually implemented in templates..
|
||||
// sens_out
|
||||
mxArray *sens_out_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(sens_out_mat);
|
||||
l_ptr[0] = (long long) 1;
|
||||
mxSetField(plhs[0], 0, "sens_out", sens_out_mat);
|
||||
|
||||
// capsule
|
||||
mxArray *capsule_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(capsule_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule;
|
||||
mxSetField(plhs[0], 0, "capsule", capsule_mat);
|
||||
|
||||
/* store external function pointers */
|
||||
// dyn
|
||||
memcpy(fieldnames[0],"expl_ode_fun",sizeof("expl_ode_fun"));
|
||||
memcpy(fieldnames[1],"forw_vde",sizeof("forw_vde"));
|
||||
memcpy(fieldnames[2],"hess_vde",sizeof("hess_vde"));
|
||||
memcpy(fieldnames[3],"impl_dae_fun",sizeof("impl_dae_fun"));
|
||||
memcpy(fieldnames[4],"impl_dae_fun_jac_x_xdot_z",sizeof("impl_dae_fun_jac_x_xdot_z"));
|
||||
memcpy(fieldnames[5],"impl_dae_jac_x_xdot_u_z",sizeof("impl_dae_jac_x_xdot_u_z"));
|
||||
memcpy(fieldnames[6],"impl_dae_hess",sizeof("impl_dae_hess"));
|
||||
|
||||
memcpy(fieldnames[7],"gnsf_phi_fun",sizeof("gnsf_phi_fun"));
|
||||
memcpy(fieldnames[8],"gnsf_phi_fun_jac_y",sizeof("gnsf_phi_fun_jac_y"));
|
||||
memcpy(fieldnames[9],"gnsf_phi_jac_y_uhat",sizeof("gnsf_phi_jac_y_uhat"));
|
||||
memcpy(fieldnames[10],"gnsf_f_lo_jac_x1_x1dot_u_z",sizeof("gnsf_f_lo_jac_x1_x1dot_u_z"));
|
||||
memcpy(fieldnames[11],"gnsf_get_matrices_fun",sizeof("gnsf_get_matrices_fun"));
|
||||
|
||||
memcpy(fieldnames[12],"disc_phi_fun",sizeof("disc_phi_fun"));
|
||||
memcpy(fieldnames[13],"disc_phi_fun_jac",sizeof("disc_phi_fun_jac"));
|
||||
memcpy(fieldnames[14],"disc_phi_fun_jac_hess",sizeof("disc_phi_fun_jac_hess"));
|
||||
|
||||
// cost
|
||||
memcpy(fieldnames[15],"cost_y_fun",sizeof("cost_y_fun"));
|
||||
memcpy(fieldnames[16],"cost_y_fun_jac_ut_xt",sizeof("cost_y_fun_jac_ut_xt"));
|
||||
memcpy(fieldnames[17],"cost_y_hess",sizeof("cost_y_hess"));
|
||||
memcpy(fieldnames[18],"ext_cost_fun",sizeof("ext_cost_fun"));
|
||||
memcpy(fieldnames[19],"ext_cost_fun_jac",sizeof("ext_cost_fun_jac"));
|
||||
memcpy(fieldnames[20],"ext_cost_fun_jac_hess",sizeof("ext_cost_fun_jac_hess"));
|
||||
|
||||
// constraints
|
||||
memcpy(fieldnames[21],"phi_constraint",sizeof("phi_constraint"));
|
||||
memcpy(fieldnames[22],"nl_constr_h_fun_jac",sizeof("nl_constr_h_fun_jac"));
|
||||
memcpy(fieldnames[23],"nl_constr_h_fun",sizeof("nl_constr_h_fun"));
|
||||
memcpy(fieldnames[24],"nl_constr_h_fun_jac_hess",sizeof("nl_constr_h_fun_jac_hess"));
|
||||
|
||||
|
||||
// create output struct - C_ocp_ext_fun
|
||||
plhs[1] = mxCreateStructMatrix(1, 1, FIELDS_EXT_FUN, (const char **) fieldnames);
|
||||
|
||||
|
||||
for (int i = 0; i < FIELDS_EXT_FUN; i++)
|
||||
{
|
||||
mxFree( fieldnames[i] );
|
||||
}
|
||||
|
||||
/* dynamics */
|
||||
mxArray *expl_ode_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *forw_vde_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *hess_vde_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *impl_dae_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *impl_dae_fun_jac_x_xdot_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *impl_dae_jac_x_xdot_u_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *impl_dae_hess_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
|
||||
mxArray *gnsf_phi_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *gnsf_phi_fun_jac_y_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *gnsf_phi_jac_y_uhat_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *gnsf_f_lo_jac_x1_x1dot_u_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *gnsf_get_matrices_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
|
||||
mxArray *disc_phi_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *disc_phi_fun_jac_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *disc_phi_fun_jac_hess_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
|
||||
{% if solver_options.integrator_type == "ERK" %}
|
||||
{# TODO: remove _casadi from these names.. #}
|
||||
l_ptr = mxGetData(forw_vde_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->forw_vde_casadi;
|
||||
l_ptr = mxGetData(expl_ode_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->expl_ode_fun;
|
||||
{% if solver_options.hessian_approx == "EXACT" %}
|
||||
l_ptr = mxGetData(hess_vde_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->hess_vde_casadi;
|
||||
{%- endif %}
|
||||
{% elif solver_options.integrator_type == "IRK" %}
|
||||
l_ptr = mxGetData(impl_dae_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_fun;
|
||||
l_ptr = mxGetData(impl_dae_fun_jac_x_xdot_z_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_fun_jac_x_xdot_z;
|
||||
l_ptr = mxGetData(impl_dae_jac_x_xdot_u_z_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_jac_x_xdot_u_z;
|
||||
{% if solver_options.hessian_approx == "EXACT" %}
|
||||
l_ptr = mxGetData(impl_dae_hess_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_hess;
|
||||
{%- endif %}
|
||||
{% elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
l_ptr = mxGetData(gnsf_phi_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun;
|
||||
l_ptr = mxGetData(gnsf_phi_fun_jac_y_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun_jac_y;
|
||||
l_ptr = mxGetData(gnsf_phi_jac_y_uhat_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_jac_y_uhat;
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
l_ptr = mxGetData(gnsf_f_lo_jac_x1_x1dot_u_z_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_f_lo_jac_x1_x1dot_u_z;
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
l_ptr = mxGetData(gnsf_get_matrices_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_get_matrices_fun;
|
||||
{% elif solver_options.integrator_type == "DISCRETE" %}
|
||||
l_ptr = mxGetData(disc_phi_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun;
|
||||
l_ptr = mxGetData(disc_phi_fun_jac_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun_jac_ut_xt;
|
||||
{% if solver_options.hessian_approx == "EXACT" %}
|
||||
l_ptr = mxGetData(disc_phi_fun_jac_hess_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun_jac_ut_xt_hess;
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "expl_ode_fun", expl_ode_fun_mat);
|
||||
mxSetField(plhs[1], 0, "forw_vde", forw_vde_mat);
|
||||
mxSetField(plhs[1], 0, "hess_vde", hess_vde_mat);
|
||||
|
||||
mxSetField(plhs[1], 0, "gnsf_phi_fun", gnsf_phi_fun_mat);
|
||||
mxSetField(plhs[1], 0, "gnsf_phi_fun_jac_y", gnsf_phi_fun_jac_y_mat);
|
||||
mxSetField(plhs[1], 0, "gnsf_phi_jac_y_uhat", gnsf_phi_jac_y_uhat_mat);
|
||||
mxSetField(plhs[1], 0, "gnsf_f_lo_jac_x1_x1dot_u_z", gnsf_f_lo_jac_x1_x1dot_u_z_mat);
|
||||
mxSetField(plhs[1], 0, "gnsf_get_matrices_fun", gnsf_get_matrices_fun_mat);
|
||||
|
||||
mxSetField(plhs[1], 0, "impl_dae_fun", impl_dae_fun_mat);
|
||||
mxSetField(plhs[1], 0, "impl_dae_fun_jac_x_xdot_z", impl_dae_fun_jac_x_xdot_z_mat);
|
||||
mxSetField(plhs[1], 0, "impl_dae_jac_x_xdot_u_z", impl_dae_jac_x_xdot_u_z_mat);
|
||||
mxSetField(plhs[1], 0, "impl_dae_hess", impl_dae_hess_mat);
|
||||
|
||||
mxSetField(plhs[1], 0, "disc_phi_fun", disc_phi_fun_mat);
|
||||
mxSetField(plhs[1], 0, "disc_phi_fun_jac", disc_phi_fun_jac_mat);
|
||||
mxSetField(plhs[1], 0, "disc_phi_fun_jac_hess", disc_phi_fun_jac_hess_mat);
|
||||
/* constaints */
|
||||
mxArray *phi_constraint_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(phi_constraint_mat);
|
||||
{%- if constraints.constr_type == "BGP" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->phi_constraint;
|
||||
{% endif %}
|
||||
{% if constraints.constr_type_e == "BGP" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->phi_e_constraint;
|
||||
{% endif %}
|
||||
mxSetField(plhs[1], 0, "phi_constraint", phi_constraint_mat);
|
||||
|
||||
mxArray *nl_constr_h_fun_jac_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(nl_constr_h_fun_jac_mat);
|
||||
{% if constraints.constr_type == "BGH" and dims.nh > 0 %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun_jac;
|
||||
{% endif %}
|
||||
{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun_jac;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "nl_constr_h_fun_jac", nl_constr_h_fun_jac_mat);
|
||||
|
||||
mxArray *nl_constr_h_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(nl_constr_h_fun_mat);
|
||||
{% if constraints.constr_type == "BGH" and dims.nh > 0 %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun;
|
||||
{% endif %}
|
||||
{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "nl_constr_h_fun", nl_constr_h_fun_mat);
|
||||
|
||||
mxArray *nl_constr_h_fun_jac_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(nl_constr_h_fun_jac_hess_mat);
|
||||
{% if constraints.constr_type == "BGH" and dims.nh > 0 and solver_options.hessian_approx == "EXACT" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun_jac_hess;
|
||||
{% endif %}
|
||||
{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 and solver_options.hessian_approx == "EXACT" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun_jac_hess;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "nl_constr_h_fun_jac_hess", nl_constr_h_fun_jac_hess_mat);
|
||||
|
||||
/* cost */
|
||||
mxArray *cost_y_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(cost_y_fun_mat);
|
||||
{% if cost.cost_type == "NONLINEAR_LS" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->cost_y_fun;
|
||||
{% endif %}
|
||||
{% if cost.cost_type_e == "NONLINEAR_LS" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_fun;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "cost_y_fun", cost_y_fun_mat);
|
||||
|
||||
mxArray *cost_y_fun_jac_ut_xt_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(cost_y_fun_jac_ut_xt_mat);
|
||||
{% if cost.cost_type == "NONLINEAR_LS" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->cost_y_fun_jac_ut_xt;
|
||||
{% endif %}
|
||||
{% if cost.cost_type_e == "NONLINEAR_LS" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_fun_jac_ut_xt;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "cost_y_fun_jac_ut_xt", cost_y_fun_jac_ut_xt_mat);
|
||||
|
||||
mxArray *cost_y_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(cost_y_hess_mat);
|
||||
{% if cost.cost_type == "NONLINEAR_LS" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->cost_y_hess;
|
||||
{% endif %}
|
||||
{% if cost.cost_type_e == "NONLINEAR_LS" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_hess;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "cost_y_hess", cost_y_hess_mat);
|
||||
|
||||
mxArray *ext_cost_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(ext_cost_fun_mat);
|
||||
{% if cost.cost_type == "EXTERNAL" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun;
|
||||
{% endif -%}
|
||||
{% if cost.cost_type_e == "EXTERNAL" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "ext_cost_fun", ext_cost_fun_mat);
|
||||
|
||||
mxArray *ext_cost_fun_jac_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(ext_cost_fun_jac_mat);
|
||||
{% if cost.cost_type == "EXTERNAL" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun_jac;
|
||||
{% endif -%}
|
||||
{% if cost.cost_type_e == "EXTERNAL" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun_jac;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "ext_cost_fun_jac", ext_cost_fun_jac_mat);
|
||||
|
||||
mxArray *ext_cost_fun_jac_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(ext_cost_fun_jac_hess_mat);
|
||||
{% if cost.cost_type == "EXTERNAL" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun_jac_hess;
|
||||
{% endif -%}
|
||||
{% if cost.cost_type_e == "EXTERNAL" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun_jac_hess;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "ext_cost_fun_jac_hess", ext_cost_fun_jac_hess_mat);
|
||||
|
||||
|
||||
return;
|
||||
}
|
||||
67
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_free.in.c
vendored
Normal file
67
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_free.in.c
vendored
Normal file
@@ -0,0 +1,67 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
// system
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
// acados
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
|
||||
// mex
|
||||
#include "mex.h"
|
||||
|
||||
|
||||
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
||||
{
|
||||
int status = 0;
|
||||
long long *ptr;
|
||||
|
||||
// mexPrintf("\nin mex_acados_free\n");
|
||||
const mxArray *C_ocp = prhs[0];
|
||||
// capsule
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
|
||||
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
|
||||
|
||||
status = {{ model.name }}_acados_free(capsule);
|
||||
if (status)
|
||||
{
|
||||
mexPrintf("{{ model.name }}_acados_free() returned status %d.\n", status);
|
||||
}
|
||||
|
||||
status = {{ model.name }}_acados_free_capsule(capsule);
|
||||
if (status)
|
||||
{
|
||||
mexPrintf("{{ model.name }}_acados_free_capsule() returned status %d.\n", status);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
632
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c
vendored
Normal file
632
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c
vendored
Normal file
@@ -0,0 +1,632 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
|
||||
// mex
|
||||
#include "mex.h"
|
||||
#include "mex_macros.h"
|
||||
|
||||
|
||||
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
||||
{
|
||||
|
||||
long long *ptr;
|
||||
int acados_size;
|
||||
mxArray *mex_field;
|
||||
char fun_name[20] = "ocp_set";
|
||||
char buffer [500]; // for error messages
|
||||
|
||||
/* RHS */
|
||||
int min_nrhs = 6;
|
||||
|
||||
// C ocp
|
||||
const mxArray *C_ocp = prhs[2];
|
||||
// capsule
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
|
||||
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
|
||||
// plan
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "plan" ) );
|
||||
ocp_nlp_plan_t *plan = (ocp_nlp_plan_t *) ptr[0];
|
||||
// config
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "config" ) );
|
||||
ocp_nlp_config *config = (ocp_nlp_config *) ptr[0];
|
||||
// dims
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "dims" ) );
|
||||
ocp_nlp_dims *dims = (ocp_nlp_dims *) ptr[0];
|
||||
// opts
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "opts" ) );
|
||||
void *opts = (void *) ptr[0];
|
||||
// in
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "in" ) );
|
||||
ocp_nlp_in *in = (ocp_nlp_in *) ptr[0];
|
||||
// out
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "out" ) );
|
||||
ocp_nlp_out *out = (ocp_nlp_out *) ptr[0];
|
||||
// solver
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "solver" ) );
|
||||
ocp_nlp_solver *solver = (ocp_nlp_solver *) ptr[0];
|
||||
|
||||
const mxArray *C_ext_fun_pointers = prhs[3];
|
||||
// field
|
||||
char *field = mxArrayToString( prhs[4] );
|
||||
// value
|
||||
double *value = mxGetPr( prhs[5] );
|
||||
|
||||
// for checks
|
||||
int matlab_size = (int) mxGetNumberOfElements( prhs[5] );
|
||||
int nrow = (int) mxGetM( prhs[5] );
|
||||
int ncol = (int) mxGetN( prhs[5] );
|
||||
|
||||
int N = dims->N;
|
||||
int nu = dims->nu[0];
|
||||
int nx = dims->nx[0];
|
||||
|
||||
// stage
|
||||
int s0, se;
|
||||
if (nrhs==min_nrhs)
|
||||
{
|
||||
s0 = 0;
|
||||
se = N;
|
||||
}
|
||||
else if (nrhs==min_nrhs+1)
|
||||
{
|
||||
s0 = mxGetScalar( prhs[6] );
|
||||
if (s0 > N)
|
||||
{
|
||||
sprintf(buffer, "ocp_set: N < specified stage = %d\n", s0);
|
||||
mexErrMsgTxt(buffer);
|
||||
}
|
||||
se = s0 + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buffer, "ocp_set: wrong nrhs: %d\n", nrhs);
|
||||
mexErrMsgTxt(buffer);
|
||||
}
|
||||
|
||||
/* Set value */
|
||||
// constraints
|
||||
if (!strcmp(field, "constr_x0"))
|
||||
{
|
||||
acados_size = nx;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_constraints_model_set(config, dims, in, 0, "lbx", value);
|
||||
ocp_nlp_constraints_model_set(config, dims, in, 0, "ubx", value);
|
||||
}
|
||||
else if (!strcmp(field, "constr_C"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
int ng = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
|
||||
MEX_DIM_CHECK_MAT(fun_name, "constr_C", nrow, ncol, ng, nx);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "C", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_lbx"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lbx");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "lbx", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_ubx"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ubx");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "ubx", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_lbu"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lbu");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "lbu", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_ubu"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ubu");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "ubu", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_D"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
int ng = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
|
||||
MEX_DIM_CHECK_MAT(fun_name, "constr_D", nrow, ncol, ng, nu);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "D", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_lg"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lg");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "lg", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_ug"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "ug", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_lh"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lh");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "lh", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_uh"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "uh");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "uh", value);
|
||||
}
|
||||
}
|
||||
// cost:
|
||||
else if (!strcmp(field, "cost_y_ref"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "y_ref", value);
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_y_ref_e"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, N, "y_ref");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_cost_model_set(config, dims, in, N, "y_ref", value);
|
||||
}
|
||||
else if (!strcmp(field, "cost_Vu"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
|
||||
{
|
||||
int ny = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref");
|
||||
int nu = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "u");
|
||||
acados_size = ny * nu;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "Vu", value);
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_Vx"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
|
||||
{
|
||||
int ny = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref");
|
||||
int nx = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "x");
|
||||
acados_size = ny * nx;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "Vx", value);
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_W"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
|
||||
{
|
||||
int ny = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "y_ref");
|
||||
acados_size = ny * ny;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "W", value);
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_Z"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "cost_Z");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "Z", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_Zl"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "Zl");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "Zl", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_Zu"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "Zu");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "Zu", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_z"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "cost_z");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "z", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_zl"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "zl");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "zl", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_zu"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "zu");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "zu", value);
|
||||
}
|
||||
}
|
||||
// constraints TODO
|
||||
// // NOTE(oj): how is it with Jbx, Jbu, idxb can they be changed?!
|
||||
// else if (!strcmp(field, "constr_lbx"))
|
||||
// {
|
||||
// // bounds at 0 are a special case.
|
||||
// if (s0==0)
|
||||
// {
|
||||
// sprintf(buffer, "%s cannot set %s for stage 0", fun_name, field);
|
||||
// mexErrMsgTxt(buffer);
|
||||
// }
|
||||
// }
|
||||
// initializations
|
||||
else if (!strcmp(field, "init_x") || !strcmp(field, "x"))
|
||||
{
|
||||
if (nrhs == min_nrhs)
|
||||
{
|
||||
acados_size = (N+1) * nx;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=0; ii<=N; ii++)
|
||||
{
|
||||
ocp_nlp_out_set(config, dims, out, ii, "x", value+ii*nx);
|
||||
}
|
||||
}
|
||||
else // (nrhs == min_nrhs + 1)
|
||||
{
|
||||
acados_size = nx;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_out_set(config, dims, out, s0, "x", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_u") || !strcmp(field, "u"))
|
||||
{
|
||||
if (nrhs==min_nrhs)
|
||||
{
|
||||
acados_size = N*nu;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=0; ii<N; ii++)
|
||||
{
|
||||
ocp_nlp_out_set(config, dims, out, ii, "u", value+ii*nu);
|
||||
}
|
||||
}
|
||||
else // (nrhs == min_nrhs + 1)
|
||||
{
|
||||
acados_size = nu;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_out_set(config, dims, out, s0, "u", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_z")||!strcmp(field, "z"))
|
||||
{
|
||||
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_t type = sim_plan.sim_solver;
|
||||
if (type == IRK)
|
||||
{
|
||||
int nz = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "z");
|
||||
if (nrhs==min_nrhs)
|
||||
{
|
||||
acados_size = N*nz;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=0; ii<N; ii++)
|
||||
{
|
||||
ocp_nlp_set(config, solver, ii, "z_guess", value+ii*nz);
|
||||
}
|
||||
}
|
||||
else // (nrhs==min_nrhs+1)
|
||||
{
|
||||
acados_size = nz;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_set(config, solver, s0, "z_guess", value);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk")
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_xdot")||!strcmp(field, "xdot"))
|
||||
{
|
||||
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_t type = sim_plan.sim_solver;
|
||||
if (type == IRK)
|
||||
{
|
||||
int nx = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "x");
|
||||
if (nrhs==min_nrhs)
|
||||
{
|
||||
acados_size = N*nx;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=0; ii<N; ii++)
|
||||
{
|
||||
ocp_nlp_set(config, solver, ii, "xdot_guess", value+ii*nx);
|
||||
}
|
||||
}
|
||||
else // nrhs==min_nrhs+1)
|
||||
{
|
||||
acados_size = nx;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_set(config, solver, s0, "xdot_guess", value);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk")
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_gnsf_phi")||!strcmp(field, "gnsf_phi"))
|
||||
{
|
||||
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_t type = sim_plan.sim_solver;
|
||||
if (type == GNSF)
|
||||
{
|
||||
int nout = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "init_gnsf_phi");
|
||||
|
||||
if (nrhs==min_nrhs)
|
||||
{
|
||||
acados_size = N*nout;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=0; ii<N; ii++)
|
||||
{
|
||||
ocp_nlp_set(config, solver, ii, "gnsf_phi_guess", value+ii*nx);
|
||||
}
|
||||
}
|
||||
else // (nrhs==min_nrhs+1)
|
||||
{
|
||||
acados_size = nout;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_set(config, solver, s0, "gnsf_phi_guess", value);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_gnsf_phi", "irk_gnsf")
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_pi")||!strcmp(field, "pi"))
|
||||
{
|
||||
if (nrhs==min_nrhs)
|
||||
{
|
||||
acados_size = N*nx;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=0; ii<N; ii++)
|
||||
{
|
||||
ocp_nlp_out_set(config, dims, out, ii, "pi", value+ii*nx);
|
||||
}
|
||||
}
|
||||
else // (nrhs==min_nrhs+1)
|
||||
{
|
||||
acados_size = nx;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_out_set(config, dims, out, s0, "pi", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_lam")||!strcmp(field, "lam"))
|
||||
{
|
||||
if (nrhs==min_nrhs)
|
||||
{
|
||||
MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
|
||||
}
|
||||
else //(nrhs==min_nrhs+1)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "lam");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_out_set(config, dims, out, s0, "lam", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_t")||!strcmp(field, "t"))
|
||||
{
|
||||
if (nrhs==min_nrhs)
|
||||
{
|
||||
MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
|
||||
}
|
||||
else //(nrhs==min_nrhs+1)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "t");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_out_set(config, dims, out, s0, "t", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_sl")||!strcmp(field, "sl"))
|
||||
{
|
||||
if (nrhs==min_nrhs)
|
||||
{
|
||||
MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
|
||||
}
|
||||
else //(nrhs==min_nrhs+1)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "sl");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_out_set(config, dims, out, s0, field, value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_su")||!strcmp(field, "su"))
|
||||
{
|
||||
if (nrhs==min_nrhs)
|
||||
{
|
||||
MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
|
||||
}
|
||||
else //(nrhs==min_nrhs+1)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "su");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_out_set(config, dims, out, s0, field, value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "p"))
|
||||
{
|
||||
if (nrhs==min_nrhs) // all stages
|
||||
{
|
||||
for (int ii=0; ii<=N; ii++)
|
||||
{
|
||||
{{ model.name }}_acados_update_params(capsule, ii, value, matlab_size);
|
||||
}
|
||||
}
|
||||
else if (nrhs==min_nrhs+1) // one stage
|
||||
{
|
||||
int stage = mxGetScalar( prhs[6] );
|
||||
{{ model.name }}_acados_update_params(capsule, stage, value, matlab_size);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "nlp_solver_max_iter"))
|
||||
{
|
||||
acados_size = 1;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
int nlp_solver_max_iter = (int) value[0];
|
||||
ocp_nlp_solver_opts_set(config, opts, "max_iter", &nlp_solver_max_iter);
|
||||
}
|
||||
else if (!strcmp(field, "rti_phase"))
|
||||
{
|
||||
acados_size = 1;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
int rti_phase = (int) value[0];
|
||||
if (plan->nlp_solver == SQP && rti_phase != 0)
|
||||
{
|
||||
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, field, "sqp_rti")
|
||||
}
|
||||
ocp_nlp_solver_opts_set(config, opts, "rti_phase", &rti_phase);
|
||||
}
|
||||
else if (!strcmp(field, "qp_warm_start"))
|
||||
{
|
||||
acados_size = 1;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
int qp_warm_start = (int) value[0];
|
||||
ocp_nlp_solver_opts_set(config, opts, "qp_warm_start", &qp_warm_start);
|
||||
}
|
||||
else if (!strcmp(field, "warm_start_first_qp"))
|
||||
{
|
||||
acados_size = 1;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
int warm_start_first_qp = (int) value[0];
|
||||
ocp_nlp_solver_opts_set(config, opts, "warm_start_first_qp", &warm_start_first_qp);
|
||||
}
|
||||
else if (!strcmp(field, "print_level"))
|
||||
{
|
||||
acados_size = 1;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
int print_level = (int) value[0];
|
||||
ocp_nlp_solver_opts_set(config, opts, "print_level", &print_level);
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_NOT_SUPPORTED_SUGGEST(fun_name, field, "p, constr_x0,\
|
||||
constr_lbx, constr_ubx, constr_C, constr_D, constr_lg, constr_ug, constr_lh, constr_uh,\
|
||||
constr_lbu, constr_ubu, cost_y_ref[_e], sl, su, x, xdot, u, pi, lam, z, \
|
||||
cost_Vu, cost_Vx, cost_Vz, cost_W, cost_Z, cost_Zl, cost_Zu, cost_z,\
|
||||
cost_zl, cost_zu, init_x, init_u, init_z, init_xdot, init_gnsf_phi,\
|
||||
init_pi, nlp_solver_max_iter, qp_warm_start, warm_start_first_qp, print_level");
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
56
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_solve.in.c
vendored
Normal file
56
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_solve.in.c
vendored
Normal file
@@ -0,0 +1,56 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
// system
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
// acados
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
|
||||
// mex
|
||||
#include "mex.h"
|
||||
|
||||
|
||||
|
||||
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
||||
{
|
||||
// C_ocp
|
||||
long long *ptr;
|
||||
const mxArray *C_ocp = prhs[0];
|
||||
|
||||
// capsule
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
|
||||
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
|
||||
|
||||
// solve
|
||||
{{ model.name }}_acados_solve(capsule);
|
||||
|
||||
}
|
||||
230
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_sim_solver_sfun.in.c
vendored
Normal file
230
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_sim_solver_sfun.in.c
vendored
Normal file
@@ -0,0 +1,230 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#define S_FUNCTION_NAME acados_sim_solver_sfunction_{{ model.name }}
|
||||
#define S_FUNCTION_LEVEL 2
|
||||
|
||||
#define MDL_START
|
||||
|
||||
// acados
|
||||
// #include "acados/utils/print.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
// example specific
|
||||
#include "{{ model.name }}_model/{{ model.name }}_model.h"
|
||||
#include "acados_sim_solver_{{ model.name }}.h"
|
||||
|
||||
#include "simstruc.h"
|
||||
|
||||
#define SAMPLINGTIME {{ solver_options.Tsim }}
|
||||
|
||||
|
||||
static void mdlInitializeSizes (SimStruct *S)
|
||||
{
|
||||
// specify the number of continuous and discrete states
|
||||
ssSetNumContStates(S, 0);
|
||||
ssSetNumDiscStates(S, 0);
|
||||
|
||||
{# compute number of input ports #}
|
||||
{%- set n_inputs = 1 %} {# x0 #}
|
||||
{%- if dims.nu > 0 %} {# u0 -#}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif %}
|
||||
{%- if dims.np > 0 %} {# parameters #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif %}
|
||||
|
||||
// specify the number of input ports
|
||||
if ( !ssSetNumInputPorts(S, {{ n_inputs }}) )
|
||||
return;
|
||||
|
||||
// specify the number of output ports
|
||||
if ( !ssSetNumOutputPorts(S, 1) )
|
||||
return;
|
||||
|
||||
// specify dimension information for the input ports
|
||||
{%- set i_input = 0 %}
|
||||
// x0
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nx }});
|
||||
|
||||
{%- if dims.nu > 0 %}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// u0
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nu }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.np > 0 %}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// parameters
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.np }});
|
||||
{%- endif %}
|
||||
|
||||
// specify dimension information for the output ports
|
||||
ssSetOutputPortVectorDimension(S, 0, {{ dims.nx }} ); // xnext
|
||||
|
||||
// specify the direct feedthrough status
|
||||
// should be set to 1 for all inputs used in mdlOutputs
|
||||
{%- for i in range(end=n_inputs) %}
|
||||
ssSetInputPortDirectFeedThrough(S, {{ i }}, 1);
|
||||
{%- endfor %}
|
||||
|
||||
// one sample time
|
||||
ssSetNumSampleTimes(S, 1);
|
||||
}
|
||||
|
||||
|
||||
#if defined(MATLAB_MEX_FILE)
|
||||
|
||||
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
|
||||
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
|
||||
|
||||
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
#endif /* MATLAB_MEX_FILE */
|
||||
|
||||
|
||||
static void mdlInitializeSampleTimes(SimStruct *S)
|
||||
{
|
||||
ssSetSampleTime(S, 0, SAMPLINGTIME);
|
||||
ssSetOffsetTime(S, 0, 0.0);
|
||||
}
|
||||
|
||||
|
||||
static void mdlStart(SimStruct *S)
|
||||
{
|
||||
sim_solver_capsule *capsule = {{ model.name }}_acados_sim_solver_create_capsule();
|
||||
{{ model.name }}_acados_sim_create(capsule);
|
||||
|
||||
ssSetUserData(S, (void*)capsule);
|
||||
}
|
||||
|
||||
static void mdlOutputs(SimStruct *S, int_T tid)
|
||||
{
|
||||
sim_solver_capsule *capsule = ssGetUserData(S);
|
||||
|
||||
sim_config *acados_sim_config = {{ model.name }}_acados_get_sim_config(capsule);
|
||||
sim_in *acados_sim_in = {{ model.name }}_acados_get_sim_in(capsule);
|
||||
sim_out *acados_sim_out = {{ model.name }}_acados_get_sim_out(capsule);
|
||||
void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule);
|
||||
// sim_opts * {{ model.name }}_acados_get_sim_opts(capsule);
|
||||
// sim_solver * {{ model.name }}_acados_get_sim_solver(capsule);
|
||||
|
||||
InputRealPtrsType in_sign;
|
||||
{% set input_sizes = [dims.nx, dims.nu, dims.np] %}
|
||||
|
||||
// local buffer
|
||||
{%- set buffer_size = input_sizes | sort | last %}
|
||||
real_t buffer[{{ buffer_size }}];
|
||||
|
||||
|
||||
/* go through inputs */
|
||||
{%- set i_input = 0 %}
|
||||
// initial condition
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.nx }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
sim_in_set(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_in, "x", buffer);
|
||||
|
||||
|
||||
// ssPrintf("\nin acados sim:\n");
|
||||
// for (int i = 0; i < {{ dims.nx }}; i++) ssPrintf("x0[%d] = %f\n", i, buffer[i]);
|
||||
// ssPrintf("\n");
|
||||
|
||||
{% if dims.nu > 0 %}
|
||||
// control input - u
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.nu }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
sim_in_set(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_in, "u", buffer);
|
||||
{%- endif %}
|
||||
|
||||
|
||||
{% if dims.np > 0 %}
|
||||
// parameters
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.np }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
// update value of parameters
|
||||
{{ model.name }}_acados_sim_update_params(capsule, buffer, {{ dims.np }});
|
||||
{%- endif %}
|
||||
|
||||
|
||||
/* call solver */
|
||||
int acados_status = {{ model.name }}_acados_sim_solve(capsule);
|
||||
|
||||
|
||||
/* set outputs */
|
||||
real_t *out_x = ssGetOutputPortRealSignal(S, 0);
|
||||
|
||||
// get simulated state
|
||||
sim_out_get(acados_sim_config, acados_sim_dims, acados_sim_out,
|
||||
"xn", (void *) out_x);
|
||||
|
||||
// ssPrintf("\nacados sim solve: returned %d\n", acados_status);
|
||||
// for (int i = 0; i < {{ dims.nx }}; i++) ssPrintf("x_sim[%d] = %f\n", i, out_x[i]);
|
||||
// ssPrintf("\n");
|
||||
|
||||
}
|
||||
|
||||
|
||||
static void mdlTerminate(SimStruct *S)
|
||||
{
|
||||
sim_solver_capsule *capsule = ssGetUserData(S);
|
||||
|
||||
{{ model.name }}_acados_sim_free(capsule);
|
||||
{{ model.name }}_acados_sim_solver_free_capsule(capsule);
|
||||
}
|
||||
|
||||
|
||||
#ifdef MATLAB_MEX_FILE
|
||||
#include "simulink.c"
|
||||
#else
|
||||
#include "cg_sfun.h"
|
||||
#endif
|
||||
853
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c
vendored
Normal file
853
third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c
vendored
Normal file
@@ -0,0 +1,853 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#define S_FUNCTION_NAME acados_solver_sfunction_{{ model.name }}
|
||||
#define S_FUNCTION_LEVEL 2
|
||||
|
||||
#define MDL_START
|
||||
|
||||
// acados
|
||||
// #include "acados/utils/print.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
// example specific
|
||||
#include "{{ model.name }}_model/{{ model.name }}_model.h"
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
|
||||
#include "simstruc.h"
|
||||
|
||||
{% if simulink_opts.samplingtime == "t0" -%}
|
||||
#define SAMPLINGTIME {{ solver_options.time_steps[0] }}
|
||||
{%- elif simulink_opts.samplingtime == "-1" -%}
|
||||
#define SAMPLINGTIME -1
|
||||
{%- else -%}
|
||||
{{ throw(message = "simulink_opts.samplingtime must be '-1' or 't0', got val") }}
|
||||
{%- endif %}
|
||||
|
||||
static void mdlInitializeSizes (SimStruct *S)
|
||||
{
|
||||
// specify the number of continuous and discrete states
|
||||
ssSetNumContStates(S, 0);
|
||||
ssSetNumDiscStates(S, 0);
|
||||
|
||||
int N = {{ model.name | upper }}_N;
|
||||
|
||||
{%- for key, val in simulink_opts.inputs -%}
|
||||
{%- if val != 0 and val != 1 -%}
|
||||
{{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }}
|
||||
{%- endif -%}
|
||||
{%- endfor -%}
|
||||
|
||||
{#- compute number of input ports #}
|
||||
{%- set n_inputs = 0 -%}
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 -%} {#- y_ref_0 -#}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref -%} {#- y_ref -#}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e -%} {#- y_ref_e #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
|
||||
{%- for key, val in simulink_opts.inputs -%}
|
||||
{%- if val != 0 and val != 1 -%}
|
||||
{{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }}
|
||||
{%- endif -%}
|
||||
{%- endfor -%}
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
|
||||
{%- set n_inputs = n_inputs + 1 %}
|
||||
{%- endif -%}
|
||||
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
|
||||
{%- set n_inputs = n_inputs + 1 %}
|
||||
{%- endif -%}
|
||||
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.inputs.reset_solver -%} {#- reset_solver #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.inputs.x_init -%} {#- x_init #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.inputs.u_init -%} {#- u_init #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
|
||||
// specify the number of input ports
|
||||
if ( !ssSetNumInputPorts(S, {{ n_inputs }}) )
|
||||
return;
|
||||
|
||||
// specify the number of output ports
|
||||
{%- set_global n_outputs = 0 %}
|
||||
{%- for key, val in simulink_opts.outputs %}
|
||||
{%- if val == 1 %}
|
||||
{%- set_global n_outputs = n_outputs + val %}
|
||||
{%- elif val != 0 %}
|
||||
{{ throw(message = "simulink_opts.outputs must be 0 or 1, got val") }}
|
||||
{%- endif %}
|
||||
{%- endfor %}
|
||||
if ( !ssSetNumOutputPorts(S, {{ n_outputs }}) )
|
||||
return;
|
||||
|
||||
// specify dimension information for the input ports
|
||||
{%- set i_input = -1 %}{# note here i_input is 0-based #}
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// lbx_0
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_0 }});
|
||||
{%- endif %}
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// ubx_0
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_0 }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// parameters
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, (N+1) * {{ dims.np }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny > 0 and simulink_opts.inputs.y_ref_0 %}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// y_ref_0
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_0 }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// y_ref
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.ny }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// y_ref_e
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_e }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// lbx
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.nbx }});
|
||||
{%- endif %}
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// ubx
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.nbx }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// lbx_e
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_e }});
|
||||
{%- endif %}
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// ubx_e
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_e }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// lbu
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nbu }});
|
||||
{%- endif -%}
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// ubu
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nbu }});
|
||||
{%- endif -%}
|
||||
|
||||
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// lg
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.ng }});
|
||||
{%- endif -%}
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// ug
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.ng }});
|
||||
{%- endif -%}
|
||||
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// lh
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nh }});
|
||||
{%- endif -%}
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// uh
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nh }});
|
||||
{%- endif -%}
|
||||
|
||||
{%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// lh_e
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh_e }});
|
||||
{%- endif -%}
|
||||
{%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// uh_e
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh_e }});
|
||||
{%- endif -%}
|
||||
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// cost_W_0
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_0 * dims.ny_0 }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// cost_W
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny * dims.ny }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// cost_W_e
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_e * dims.ny_e }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.reset_solver -%} {#- reset_solver #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// reset_solver
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, 1);
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.inputs.x_init -%} {#- x_init #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// x_init
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nx * (dims.N+1) }});
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.inputs.u_init -%} {#- u_init #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// u_init
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nu * (dims.N) }});
|
||||
{%- endif -%}
|
||||
|
||||
/* specify dimension information for the OUTPUT ports */
|
||||
{%- set i_output = -1 %}{# note here i_output is 0-based #}
|
||||
{%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nu }} );
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.utraj == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nu * dims.N }} );
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.xtraj == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx * (dims.N+1) }} );
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.solver_status == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.cost_value == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.KKT_residual == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.KKT_residuals == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 4 );
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx }} ); // state at shooting node 1
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_sim == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_qp == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_lin == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.sqp_iter == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
|
||||
{%- endif %}
|
||||
|
||||
// specify the direct feedthrough status
|
||||
// should be set to 1 for all inputs used in mdlOutputs
|
||||
{%- for i in range(end=n_inputs) %}
|
||||
ssSetInputPortDirectFeedThrough(S, {{ i }}, 1);
|
||||
{%- endfor %}
|
||||
|
||||
// one sample time
|
||||
ssSetNumSampleTimes(S, 1);
|
||||
}
|
||||
|
||||
|
||||
#if defined(MATLAB_MEX_FILE)
|
||||
|
||||
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
|
||||
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
|
||||
|
||||
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
#endif /* MATLAB_MEX_FILE */
|
||||
|
||||
|
||||
static void mdlInitializeSampleTimes(SimStruct *S)
|
||||
{
|
||||
ssSetSampleTime(S, 0, SAMPLINGTIME);
|
||||
ssSetOffsetTime(S, 0, 0.0);
|
||||
}
|
||||
|
||||
|
||||
static void mdlStart(SimStruct *S)
|
||||
{
|
||||
{{ model.name }}_solver_capsule *capsule = {{ model.name }}_acados_create_capsule();
|
||||
{{ model.name }}_acados_create(capsule);
|
||||
|
||||
ssSetUserData(S, (void*)capsule);
|
||||
}
|
||||
|
||||
|
||||
static void mdlOutputs(SimStruct *S, int_T tid)
|
||||
{
|
||||
{{ model.name }}_solver_capsule *capsule = ssGetUserData(S);
|
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule);
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
|
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule);
|
||||
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(capsule);
|
||||
|
||||
InputRealPtrsType in_sign;
|
||||
|
||||
int N = {{ model.name | upper }}_N;
|
||||
|
||||
{%- set buffer_sizes = [dims.nbx_0, dims.np, dims.nbx, dims.nbx_e, dims.nbu, dims.ng, dims.nh, dims.ng_e, dims.nh_e] -%}
|
||||
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %} {# y_ref_0 #}
|
||||
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0)) %}
|
||||
{%- endif %}
|
||||
{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %} {# y_ref #}
|
||||
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny)) %}
|
||||
{%- endif %}
|
||||
{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %} {# y_ref_e #}
|
||||
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_e)) %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
|
||||
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0 * dims.ny_0)) %}
|
||||
{%- endif %}
|
||||
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
|
||||
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny * dims.ny)) %}
|
||||
{%- endif %}
|
||||
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
|
||||
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_e * dims.ny_e)) %}
|
||||
{%- endif %}
|
||||
|
||||
// local buffer
|
||||
{%- set buffer_size = buffer_sizes | sort | last %}
|
||||
real_t buffer[{{ buffer_size }}];
|
||||
|
||||
/* go through inputs */
|
||||
{%- set i_input = -1 %}
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #}
|
||||
// lbx_0
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.nbx_0 }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #}
|
||||
// ubx_0
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.nbx_0 }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #}
|
||||
// parameters - stage-variant !!!
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
// update value of parameters
|
||||
for (int ii = 0; ii <= N; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.np }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*{{dims.np}}+jj]);
|
||||
{{ model.name }}_acados_update_params(capsule, ii, buffer, {{ dims.np }});
|
||||
}
|
||||
{%- endif %}
|
||||
|
||||
{% if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %}
|
||||
// y_ref_0
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.ny_0 }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
|
||||
{%- endif %}
|
||||
|
||||
{% if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %}
|
||||
// y_ref - for stages 1 to N-1
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int ii = 1; ii < N; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.ny }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.ny }}+jj]);
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
|
||||
}
|
||||
{%- endif %}
|
||||
|
||||
{% if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %}
|
||||
// y_ref_e
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.ny_e }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", (void *) buffer);
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #}
|
||||
// lbx
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int ii = 1; ii < N; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nbx }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
|
||||
}
|
||||
{%- endif %}
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #}
|
||||
// ubx
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int ii = 1; ii < N; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nbx }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
|
||||
}
|
||||
{%- endif %}
|
||||
|
||||
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #}
|
||||
// lbx_e
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.nbx_e }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", buffer);
|
||||
{%- endif %}
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #}
|
||||
// ubx_e
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.nbx_e }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", buffer);
|
||||
{%- endif %}
|
||||
|
||||
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #}
|
||||
// lbu
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int ii = 0; ii < N; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nbu }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbu", (void *) buffer);
|
||||
}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #}
|
||||
// ubu
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int ii = 0; ii < N; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nbu }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubu", (void *) buffer);
|
||||
}
|
||||
{%- endif -%}
|
||||
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #}
|
||||
// lg
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int ii = 0; ii < N; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.ng }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*{{ dims.ng }}+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lg", (void *) buffer);
|
||||
}
|
||||
{%- endif -%}
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #}
|
||||
// ug
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int ii = 0; ii < N; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.ng }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*{{ dims.ng }}+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ug", (void *) buffer);
|
||||
}
|
||||
{%- endif -%}
|
||||
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #}
|
||||
// lh
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int ii = 0; ii < N; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nh }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*{{ dims.nh }}+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", (void *) buffer);
|
||||
}
|
||||
{%- endif -%}
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #}
|
||||
// uh
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int ii = 0; ii < N; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nh }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*{{ dims.nh }}+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", (void *) buffer);
|
||||
}
|
||||
{%- endif -%}
|
||||
|
||||
|
||||
{%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #}
|
||||
// lh_e
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.nh_e }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", buffer);
|
||||
{%- endif -%}
|
||||
{%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #}
|
||||
// uh_e
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.nh_e }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", buffer);
|
||||
{%- endif -%}
|
||||
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
|
||||
// cost_W_0
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.ny_0 * dims.ny_0 }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", buffer);
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
|
||||
// cost_W
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.ny * dims.ny }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
for (int ii = 1; ii < N; ii++)
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "W", buffer);
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
|
||||
// cost_W_e
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.ny_e * dims.ny_e }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", buffer);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.reset_solver %} {#- reset_solver #}
|
||||
// reset_solver
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
double reset = (double)(*in_sign[0]);
|
||||
if (reset)
|
||||
{
|
||||
{{ model.name }}_acados_reset(capsule, 1);
|
||||
}
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.x_init %} {#- x_init #}
|
||||
// x_init
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int ii = 0; ii < {{ dims.N + 1 }}; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nx }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nx }}+jj]);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "x", (void *) buffer);
|
||||
}
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.u_init %} {#- u_init #}
|
||||
// u_init
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int ii = 0; ii < N; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nu }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nu }}+jj]);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "u", (void *) buffer);
|
||||
}
|
||||
{%- endif %}
|
||||
|
||||
/* call solver */
|
||||
int rti_phase = 0;
|
||||
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
|
||||
int acados_status = {{ model.name }}_acados_solve(capsule);
|
||||
|
||||
|
||||
/* set outputs */
|
||||
// assign pointers to output signals
|
||||
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_KKT_residuals, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin, *out_cost_value;
|
||||
int tmp_int;
|
||||
|
||||
{%- set i_output = -1 -%}{# note here i_output is 0-based #}
|
||||
{%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_u0 = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.utraj == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_utraj = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
for (int ii = 0; ii < N; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii,
|
||||
"u", (void *) (out_utraj + ii * {{ dims.nu }}));
|
||||
{%- endif %}
|
||||
|
||||
{% if simulink_opts.outputs.xtraj == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
|
||||
out_xtraj = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
for (int ii = 0; ii < {{ dims.N + 1 }}; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii,
|
||||
"x", (void *) (out_xtraj + ii * {{ dims.nx }}));
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.solver_status == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_status = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
*out_status = (real_t) acados_status;
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.cost_value == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_cost_value = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
ocp_nlp_eval_cost(capsule->nlp_solver, nlp_in, nlp_out);
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "cost_value", (void *) out_cost_value);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.KKT_residual == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_KKT_res = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.KKT_residuals == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_KKT_residuals = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
|
||||
{%- if solver_options.nlp_solver_type == "SQP_RTI" %}
|
||||
ocp_nlp_eval_residuals(capsule->nlp_solver, nlp_in, nlp_out);
|
||||
{%- endif %}
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_stat", (void *) &out_KKT_residuals[0]);
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_eq", (void *) &out_KKT_residuals[1]);
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_ineq", (void *) &out_KKT_residuals[2]);
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "res_comp", (void *) &out_KKT_residuals[3]);
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_x1 = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_cpu_time = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
// get solution time
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_sim == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_cpu_time_sim = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_sim", (void *) out_cpu_time_sim);
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_qp == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_cpu_time_qp = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_qp", (void *) out_cpu_time_qp);
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_lin == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_cpu_time_lin = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_lin", (void *) out_cpu_time_lin);
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.outputs.sqp_iter == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_sqp_iter = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
// get sqp iter
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
|
||||
*out_sqp_iter = (real_t) tmp_int;
|
||||
{%- endif %}
|
||||
|
||||
}
|
||||
|
||||
static void mdlTerminate(SimStruct *S)
|
||||
{
|
||||
{{ model.name }}_solver_capsule *capsule = ssGetUserData(S);
|
||||
|
||||
{{ model.name }}_acados_free(capsule);
|
||||
{{ model.name }}_acados_free_capsule(capsule);
|
||||
}
|
||||
|
||||
|
||||
#ifdef MATLAB_MEX_FILE
|
||||
#include "simulink.c"
|
||||
#else
|
||||
#include "cg_sfun.h"
|
||||
#endif
|
||||
181
third_party/acados/acados_template/c_templates_tera/matlab_templates/main_mex.in.c
vendored
Normal file
181
third_party/acados/acados_template/c_templates_tera/matlab_templates/main_mex.in.c
vendored
Normal file
@@ -0,0 +1,181 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados/utils/math.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
// mex
|
||||
#include "mex.h"
|
||||
|
||||
/* auxilary mex */
|
||||
// prints a matrix in column-major format (exponential notation)
|
||||
void MEX_print_exp_mat(int m, int n, double *A, int lda)
|
||||
{
|
||||
for (int i=0; i<m; i++)
|
||||
{
|
||||
for (int j=0; j<n; j++)
|
||||
{
|
||||
mexPrintf("%e\t", A[i+lda*j]);
|
||||
}
|
||||
mexPrintf("\n");
|
||||
}
|
||||
mexPrintf("\n");
|
||||
}
|
||||
|
||||
// prints the transposed of a matrix in column-major format (exponential notation)
|
||||
void MEX_print_exp_tran_mat(int row, int col, double *A, int lda)
|
||||
{
|
||||
for (int j=0; j<col; j++)
|
||||
{
|
||||
for (int i=0; i<row; i++)
|
||||
{
|
||||
mexPrintf("%e\t", A[i+lda*j]);
|
||||
}
|
||||
mexPrintf("\n");
|
||||
}
|
||||
mexPrintf("\n");
|
||||
}
|
||||
|
||||
|
||||
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
||||
{
|
||||
|
||||
int status = 0;
|
||||
status = {{ model.name }}_acados_create();
|
||||
|
||||
if (status)
|
||||
{
|
||||
mexPrintf("{{ model.name }}_acados_create() returned status %d. Exiting.\n", status);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
// get pointers to nlp solver related objects
|
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config();
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims();
|
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in();
|
||||
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out();
|
||||
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver();
|
||||
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts();
|
||||
|
||||
// initial condition
|
||||
int idxbx0[{{ dims.nbx_0 }}];
|
||||
{% for i in range(end=dims.nbx_0) %}
|
||||
idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }};
|
||||
{%- endfor %}
|
||||
|
||||
double lbx0[{{ dims.nbx_0 }}];
|
||||
double ubx0[{{ dims.nbx_0 }}];
|
||||
{% for i in range(end=dims.nbx_0) %}
|
||||
lbx0[{{ i }}] = {{ constraints.lbx_0[i] }};
|
||||
ubx0[{{ i }}] = {{ constraints.ubx_0[i] }};
|
||||
{%- endfor %}
|
||||
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
|
||||
|
||||
// initialization for state values
|
||||
double x_init[{{ dims.nx }}];
|
||||
{%- for i in range(end=dims.nx) %}
|
||||
x_init[{{ i }}] = 0.0;
|
||||
{%- endfor %}
|
||||
|
||||
// initial value for control input
|
||||
double u0[{{ dims.nu }}];
|
||||
{%- for i in range(end=dims.nu) %}
|
||||
u0[{{ i }}] = 0.0;
|
||||
{%- endfor %}
|
||||
|
||||
// prepare evaluation
|
||||
int NTIMINGS = 10;
|
||||
double min_time = 1e12;
|
||||
double kkt_norm_inf;
|
||||
double elapsed_time;
|
||||
int sqp_iter;
|
||||
|
||||
double xtraj[{{ dims.nx }} * ({{ dims.N }}+1)];
|
||||
double utraj[{{ dims.nu }} * ({{ dims.N }})];
|
||||
|
||||
// solve ocp in loop
|
||||
for (int ii = 0; ii < NTIMINGS; ii++)
|
||||
{
|
||||
// initialize primal solution
|
||||
for (int i = 0; i <= nlp_dims->N; i++)
|
||||
{
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
|
||||
}
|
||||
status = {{ model.name }}_acados_solve();
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
|
||||
min_time = MIN(elapsed_time, min_time);
|
||||
}
|
||||
|
||||
/* print solution and statistics */
|
||||
for (int ii = 0; ii <= nlp_dims->N; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*{{ dims.nx }}]);
|
||||
for (int ii = 0; ii < nlp_dims->N; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*{{ dims.nu }}]);
|
||||
|
||||
mexPrintf("\n--- xtraj ---\n");
|
||||
MEX_print_exp_tran_mat( {{ dims.nx }}, {{ dims.N }}+1, xtraj, {{ dims.nx }} );
|
||||
mexPrintf("\n--- utraj ---\n");
|
||||
MEX_print_exp_tran_mat( {{ dims.nu }}, {{ dims.N }}, utraj, {{ dims.nu }} );
|
||||
|
||||
mexPrintf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
|
||||
|
||||
if (status == ACADOS_SUCCESS)
|
||||
mexPrintf("{{ model.name }}_acados_solve(): SUCCESS!\n");
|
||||
else
|
||||
mexPrintf("{{ model.name }}_acados_solve() failed with status %d.\n", status);
|
||||
|
||||
// get solution
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
|
||||
|
||||
mexPrintf("\nSolver info:\n");
|
||||
mexPrintf(" SQP iterations %2d\n minimum time for 1 solve %f [ms]\n KKT %e\n",
|
||||
sqp_iter, min_time*1000, kkt_norm_inf);
|
||||
|
||||
// free solver
|
||||
status = {{ model.name }}_acados_free();
|
||||
if (status)
|
||||
{
|
||||
mexPrintf("{{ model.name }}_acados_free() returned status %d.\n", status);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
103
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m
vendored
Normal file
103
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m
vendored
Normal file
@@ -0,0 +1,103 @@
|
||||
%
|
||||
% Copyright (c) The acados authors.
|
||||
%
|
||||
% This file is part of acados.
|
||||
%
|
||||
% The 2-Clause BSD License
|
||||
%
|
||||
% Redistribution and use in source and binary forms, with or without
|
||||
% modification, are permitted provided that the following conditions are met:
|
||||
%
|
||||
% 1. Redistributions of source code must retain the above copyright notice,
|
||||
% this list of conditions and the following disclaimer.
|
||||
%
|
||||
% 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
% this list of conditions and the following disclaimer in the documentation
|
||||
% and/or other materials provided with the distribution.
|
||||
%
|
||||
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
% POSSIBILITY OF SUCH DAMAGE.;
|
||||
|
||||
%
|
||||
|
||||
function make_main_mex_{{ model.name }}()
|
||||
|
||||
opts.output_dir = pwd;
|
||||
|
||||
% get acados folder
|
||||
acados_folder = getenv('ACADOS_INSTALL_DIR');
|
||||
|
||||
% set paths
|
||||
acados_include = ['-I' fullfile(acados_folder, 'include')];
|
||||
template_lib_include = ['-l' 'acados_solver_{{ model.name }}'];
|
||||
template_lib_path = ['-L' fullfile(pwd)];
|
||||
|
||||
acados_lib_path = ['-L' fullfile(acados_folder, 'lib')];
|
||||
external_include = ['-I', fullfile(acados_folder, 'external')];
|
||||
blasfeo_include = ['-I', fullfile(acados_folder, 'external', 'blasfeo', 'include')];
|
||||
hpipm_include = ['-I', fullfile(acados_folder, 'external', 'hpipm', 'include')];
|
||||
|
||||
mex_names = { ...
|
||||
'main_mex_{{ model.name }}' ...
|
||||
};
|
||||
|
||||
mex_files = cell(length(mex_names), 1);
|
||||
for k=1:length(mex_names)
|
||||
mex_files{k} = fullfile([mex_names{k}, '.c']);
|
||||
end
|
||||
|
||||
%% octave C flags
|
||||
if is_octave()
|
||||
if ~exist(fullfile(opts.output_dir, 'cflags_octave.txt'), 'file')
|
||||
diary(fullfile(opts.output_dir, 'cflags_octave.txt'));
|
||||
diary on
|
||||
mkoctfile -p CFLAGS
|
||||
diary off
|
||||
input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r');
|
||||
cflags_tmp = fscanf(input_file, '%[^\n]s');
|
||||
fclose(input_file);
|
||||
if ~ismac()
|
||||
cflags_tmp = [cflags_tmp, ' -std=c99 -fopenmp'];
|
||||
else
|
||||
cflags_tmp = [cflags_tmp, ' -std=c99'];
|
||||
end
|
||||
input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'w');
|
||||
fprintf(input_file, '%s', cflags_tmp);
|
||||
fclose(input_file);
|
||||
end
|
||||
% read cflags from file
|
||||
input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r');
|
||||
cflags_tmp = fscanf(input_file, '%[^\n]s');
|
||||
fclose(input_file);
|
||||
setenv('CFLAGS', cflags_tmp);
|
||||
end
|
||||
|
||||
%% compile mex
|
||||
for ii=1:length(mex_files)
|
||||
disp(['compiling ', mex_files{ii}])
|
||||
if is_octave()
|
||||
% mkoctfile -p CFLAGS
|
||||
mex(acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
|
||||
acados_lib_path, template_lib_path, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii})
|
||||
else
|
||||
if ismac()
|
||||
FLAGS = 'CFLAGS=$CFLAGS -std=c99';
|
||||
else
|
||||
FLAGS = 'CFLAGS=$CFLAGS -std=c99 -fopenmp';
|
||||
end
|
||||
mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
|
||||
acados_lib_path, template_lib_path, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii})
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
127
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m
vendored
Normal file
127
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m
vendored
Normal file
@@ -0,0 +1,127 @@
|
||||
%
|
||||
% Copyright (c) The acados authors.
|
||||
%
|
||||
% This file is part of acados.
|
||||
%
|
||||
% The 2-Clause BSD License
|
||||
%
|
||||
% Redistribution and use in source and binary forms, with or without
|
||||
% modification, are permitted provided that the following conditions are met:
|
||||
%
|
||||
% 1. Redistributions of source code must retain the above copyright notice,
|
||||
% this list of conditions and the following disclaimer.
|
||||
%
|
||||
% 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
% this list of conditions and the following disclaimer in the documentation
|
||||
% and/or other materials provided with the distribution.
|
||||
%
|
||||
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
% POSSIBILITY OF SUCH DAMAGE.;
|
||||
|
||||
%
|
||||
|
||||
function make_mex_{{ model.name }}()
|
||||
|
||||
opts.output_dir = pwd;
|
||||
|
||||
% get acados folder
|
||||
acados_folder = getenv('ACADOS_INSTALL_DIR');
|
||||
|
||||
% set paths
|
||||
acados_include = ['-I' fullfile(acados_folder, 'include')];
|
||||
template_lib_include = ['-l' 'acados_ocp_solver_{{ model.name }}'];
|
||||
template_lib_path = ['-L' fullfile(pwd)];
|
||||
|
||||
acados_lib_path = ['-L' fullfile(acados_folder, 'lib')];
|
||||
external_include = ['-I', fullfile(acados_folder, 'external')];
|
||||
blasfeo_include = ['-I', fullfile(acados_folder, 'external', 'blasfeo', 'include')];
|
||||
hpipm_include = ['-I', fullfile(acados_folder, 'external', 'hpipm', 'include')];
|
||||
|
||||
% load linking information of compiled acados
|
||||
link_libs_core_filename = fullfile(acados_folder, 'lib', 'link_libs.json');
|
||||
addpath(fullfile(acados_folder, 'external', 'jsonlab'));
|
||||
link_libs = loadjson(link_libs_core_filename);
|
||||
|
||||
% add necessary link instructs
|
||||
acados_lib_extra = {};
|
||||
lib_names = fieldnames(link_libs);
|
||||
for idx = 1 : numel(lib_names)
|
||||
lib_name = lib_names{idx};
|
||||
link_arg = link_libs.(lib_name);
|
||||
if ~isempty(link_arg)
|
||||
acados_lib_extra = [acados_lib_extra, link_arg];
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
mex_include = ['-I', fullfile(acados_folder, 'interfaces', 'acados_matlab_octave')];
|
||||
|
||||
mex_names = { ...
|
||||
'acados_mex_create_{{ model.name }}' ...
|
||||
'acados_mex_free_{{ model.name }}' ...
|
||||
'acados_mex_solve_{{ model.name }}' ...
|
||||
'acados_mex_set_{{ model.name }}' ...
|
||||
};
|
||||
|
||||
mex_files = cell(length(mex_names), 1);
|
||||
for k=1:length(mex_names)
|
||||
mex_files{k} = fullfile([mex_names{k}, '.c']);
|
||||
end
|
||||
|
||||
%% octave C flags
|
||||
if is_octave()
|
||||
if ~exist(fullfile(opts.output_dir, 'cflags_octave.txt'), 'file')
|
||||
diary(fullfile(opts.output_dir, 'cflags_octave.txt'));
|
||||
diary on
|
||||
mkoctfile -p CFLAGS
|
||||
diary off
|
||||
input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r');
|
||||
cflags_tmp = fscanf(input_file, '%[^\n]s');
|
||||
fclose(input_file);
|
||||
if ~ismac()
|
||||
cflags_tmp = [cflags_tmp, ' -std=c99 -fopenmp'];
|
||||
else
|
||||
cflags_tmp = [cflags_tmp, ' -std=c99'];
|
||||
end
|
||||
input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'w');
|
||||
fprintf(input_file, '%s', cflags_tmp);
|
||||
fclose(input_file);
|
||||
end
|
||||
% read cflags from file
|
||||
input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r');
|
||||
cflags_tmp = fscanf(input_file, '%[^\n]s');
|
||||
fclose(input_file);
|
||||
setenv('CFLAGS', cflags_tmp);
|
||||
end
|
||||
|
||||
%% compile mex
|
||||
for ii=1:length(mex_files)
|
||||
disp(['compiling ', mex_files{ii}])
|
||||
if is_octave()
|
||||
% mkoctfile -p CFLAGS
|
||||
mex(acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
|
||||
template_lib_path, mex_include, acados_lib_path, '-lacados', '-lhpipm', '-lblasfeo',...
|
||||
acados_lib_extra{:}, mex_files{ii})
|
||||
else
|
||||
if ismac()
|
||||
FLAGS = 'CFLAGS=$CFLAGS -std=c99';
|
||||
else
|
||||
FLAGS = 'CFLAGS=$CFLAGS -std=c99 -fopenmp';
|
||||
end
|
||||
mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
|
||||
template_lib_path, mex_include, acados_lib_path, '-lacados', '-lhpipm', '-lblasfeo',...
|
||||
acados_lib_extra{:}, mex_files{ii})
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
432
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m
vendored
Normal file
432
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m
vendored
Normal file
@@ -0,0 +1,432 @@
|
||||
%
|
||||
% Copyright (c) The acados authors.
|
||||
%
|
||||
% This file is part of acados.
|
||||
%
|
||||
% The 2-Clause BSD License
|
||||
%
|
||||
% Redistribution and use in source and binary forms, with or without
|
||||
% modification, are permitted provided that the following conditions are met:
|
||||
%
|
||||
% 1. Redistributions of source code must retain the above copyright notice,
|
||||
% this list of conditions and the following disclaimer.
|
||||
%
|
||||
% 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
% this list of conditions and the following disclaimer in the documentation
|
||||
% and/or other materials provided with the distribution.
|
||||
%
|
||||
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
% POSSIBILITY OF SUCH DAMAGE.;
|
||||
|
||||
%
|
||||
|
||||
SOURCES = { ...
|
||||
{%- if solver_options.integrator_type == 'ERK' %}
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c', ...
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c',...
|
||||
{%- if solver_options.hessian_approx == 'EXACT' %}
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c',...
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c', ...
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c', ...
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c', ...
|
||||
{%- if solver_options.hessian_approx == 'EXACT' %}
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c',...
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c',...
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c',...
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c',...
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c',...
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c',...
|
||||
{%- elif solver_options.integrator_type == "DISCRETE" %}
|
||||
'{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c',...
|
||||
'{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c',...
|
||||
{%- if solver_options.hessian_approx == "EXACT" %}
|
||||
'{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c',...
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{%- if cost.cost_type_0 == "NONLINEAR_LS" %}
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c',...
|
||||
{%- elif cost.cost_type_0 == "EXTERNAL" %}
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c',...
|
||||
{%- endif %}
|
||||
|
||||
{%- if cost.cost_type == "NONLINEAR_LS" %}
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_fun.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_hess.c',...
|
||||
{%- elif cost.cost_type == "EXTERNAL" %}
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c',...
|
||||
{%- endif %}
|
||||
{%- if cost.cost_type_e == "NONLINEAR_LS" %}
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c',...
|
||||
{%- elif cost.cost_type_e == "EXTERNAL" %}
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c',...
|
||||
'{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c',...
|
||||
{%- endif %}
|
||||
{%- if constraints.constr_type == "BGH" and dims.nh > 0 %}
|
||||
'{{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c', ...
|
||||
'{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c', ...
|
||||
'{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c', ...
|
||||
{%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %}
|
||||
'{{ model.name }}_constraints/{{ model.name }}_phi_constraint.c', ...
|
||||
{%- endif %}
|
||||
{%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
|
||||
'{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c', ...
|
||||
'{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c', ...
|
||||
'{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c', ...
|
||||
{%- elif constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %}
|
||||
'{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c', ...
|
||||
{%- endif %}
|
||||
'acados_solver_sfunction_{{ model.name }}.c', ...
|
||||
'acados_solver_{{ model.name }}.c'
|
||||
};
|
||||
|
||||
INC_PATH = '{{ acados_include_path }}';
|
||||
|
||||
INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
|
||||
['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
|
||||
['-I', fullfile(INC_PATH, 'acados')], ...
|
||||
['-I', fullfile(INC_PATH)]};
|
||||
|
||||
{% if solver_options.qp_solver is containing("QPOASES") %}
|
||||
INCS{end+1} = ['-I', fullfile(INC_PATH, 'qpOASES_e')];
|
||||
{% endif %}
|
||||
|
||||
CFLAGS = 'CFLAGS=$CFLAGS';
|
||||
LDFLAGS = 'LDFLAGS=$LDFLAGS';
|
||||
COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
|
||||
COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
|
||||
|
||||
{% if solver_options.qp_solver is containing("QPOASES") %}
|
||||
CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPOASES ' ];
|
||||
COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPOASES ' ];
|
||||
{%- elif solver_options.qp_solver is containing("OSQP") %}
|
||||
CFLAGS = [ CFLAGS, ' -DACADOS_WITH_OSQP ' ];
|
||||
COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_OSQP ' ];
|
||||
{%- elif solver_options.qp_solver is containing("QPDUNES") %}
|
||||
CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPDUNES ' ];
|
||||
COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPDUNES ' ];
|
||||
{%- elif solver_options.qp_solver is containing("DAQP") %}
|
||||
CFLAGS = [ CFLAGS, ' -DACADOS_WITH_DAQP' ];
|
||||
COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_DAQP' ];
|
||||
{%- elif solver_options.qp_solver is containing("HPMPC") %}
|
||||
CFLAGS = [ CFLAGS, ' -DACADOS_WITH_HPMPC ' ];
|
||||
COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_HPMPC ' ];
|
||||
{% endif %}
|
||||
|
||||
LIB_PATH = ['-L', fullfile('{{ acados_lib_path }}')];
|
||||
|
||||
LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
|
||||
|
||||
% acados linking libraries and flags
|
||||
{%- if acados_link_libs and os and os == "pc" %}
|
||||
LDFLAGS = [LDFLAGS ' {{ acados_link_libs.openmp }}'];
|
||||
COMPFLAGS = [COMPFLAGS ' {{ acados_link_libs.openmp }}'];
|
||||
LIBS{end+1} = '{{ acados_link_libs.qpoases }}';
|
||||
LIBS{end+1} = '{{ acados_link_libs.hpmpc }}';
|
||||
LIBS{end+1} = '{{ acados_link_libs.osqp }}';
|
||||
{%- else %}
|
||||
{% if solver_options.qp_solver is containing("QPOASES") %}
|
||||
LIBS{end+1} = '-lqpOASES_e';
|
||||
{% endif %}
|
||||
{% if solver_options.qp_solver is containing("DAQP") %}
|
||||
LIBS{end+1} = '-ldaqp';
|
||||
{% endif %}
|
||||
{%- endif %}
|
||||
|
||||
|
||||
try
|
||||
% mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
|
||||
mex('-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
|
||||
LIB_PATH, LIBS{:}, SOURCES{:}, ...
|
||||
'-output', 'acados_solver_sfunction_{{ model.name }}' );
|
||||
catch exception
|
||||
disp('make_sfun failed with the following exception:')
|
||||
disp(exception);
|
||||
disp('Try adding -v to the mex command above to get more information.')
|
||||
keyboard
|
||||
end
|
||||
|
||||
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_{{ model.name }}', '.', ...
|
||||
eval('mexext')] );
|
||||
|
||||
|
||||
%% print note on usage of s-function, and create I/O port names vectors
|
||||
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
|
||||
input_note = 'Inputs are:\n';
|
||||
i_in = 1;
|
||||
|
||||
global sfun_input_names
|
||||
sfun_input_names = {};
|
||||
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
|
||||
' size [{{ dims.nbx_0 }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lbx_0 [{{ dims.nbx_0 }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
|
||||
' size [{{ dims.nbx_0 }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'ubx_0 [{{ dims.nbx_0 }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N,',...
|
||||
' size [{{ (dims.N+1)*dims.np }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'parameter_traj [{{ (dims.N+1)*dims.np }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %}
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [{{ dims.ny_0 }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'y_ref_0 [{{ dims.ny_0 }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %}
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
|
||||
' size [{{ (dims.N-1) * dims.ny }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'y_ref [{{ (dims.N-1) * dims.ny }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %}
|
||||
input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [{{ dims.ny_e }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'y_ref_e [{{ dims.ny_e }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lbx [{{ (dims.N-1) * dims.nbx }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'ubx [{{ (dims.N-1) * dims.nbx }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lbx_e (lbx at shooting node N), size [{{ dims.nbx_e }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lbx_e [{{ dims.nbx_e }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubx_e (ubx at shooting node N), size [{{ dims.nbx_e }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'ubx_e [{{ dims.nbx_e }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lbu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lbu [{{ dims.N*dims.nbu }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif -%}
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') ubu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'ubu [{{ dims.N*dims.nbu }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif -%}
|
||||
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lg for shooting nodes 0 to N-1, size [{{ dims.N*dims.ng }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lg [{{ dims.N*dims.ng }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') ug for shooting nodes 0 to N-1, size [{{ dims.N*dims.ng }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'ug [{{ dims.N*dims.ng }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lh for shooting nodes 0 to N-1, size [{{ dims.N*dims.nh }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lh [{{ dims.N*dims.nh }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') uh for shooting nodes 0 to N-1, size [{{ dims.N*dims.nh }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'uh [{{ dims.N*dims.nh }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%} {#- lh_e #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') lh_e, size [{{ dims.nh_e }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'lh_e [{{ dims.nh_e }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
{%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%} {#- uh_e #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') uh_e, size [{{ dims.nh_e }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'uh_e [{{ dims.nh_e }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') cost_W_0 in column-major format, size [{{ dims.ny_0 * dims.ny_0 }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'cost_W_0 [{{ dims.ny_0 * dims.ny_0 }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') cost_W in column-major format, that is set for all intermediate shooting nodes: 1 to N-1, size [{{ dims.ny * dims.ny }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'cost_W [{{ dims.ny * dims.ny }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') cost_W_e in column-major format, size [{{ dims.ny_e * dims.ny_e }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'cost_W_e [{{ dims.ny_e * dims.ny_e }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.reset_solver %} {#- reset_solver #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') reset_solver determines if iterate is set to all zeros before other initializations (x_init, u_init) are set and before solver is called, size [1]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'reset_solver [1]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.x_init %} {#- x_init #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') initialization of x for all shooting nodes, size [{{ dims.nx * (dims.N+1) }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'x_init [{{ dims.nx * (dims.N+1) }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.u_init %} {#- u_init #}
|
||||
input_note = strcat(input_note, num2str(i_in), ') initialization of u for shooting nodes 0 to N-1, size [{{ dims.nu * (dims.N) }}]\n ');
|
||||
sfun_input_names = [sfun_input_names; 'u_init [{{ dims.nu * (dims.N) }}]'];
|
||||
i_in = i_in + 1;
|
||||
{%- endif %}
|
||||
|
||||
fprintf(input_note)
|
||||
|
||||
disp(' ')
|
||||
|
||||
output_note = 'Outputs are:\n';
|
||||
i_out = 0;
|
||||
|
||||
global sfun_output_names
|
||||
sfun_output_names = {};
|
||||
|
||||
{%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [{{ dims.nu }}]\n ');
|
||||
sfun_output_names = [sfun_output_names; 'u0 [{{ dims.nu }}]'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.utraj == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') utraj, control input concatenated for nodes 0 to N-1, size [{{ dims.nu * dims.N }}]\n ');
|
||||
sfun_output_names = [sfun_output_names; 'utraj [{{ dims.nu * dims.N }}]'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.xtraj == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') xtraj, state concatenated for nodes 0 to N, size [{{ dims.nx * (dims.N + 1) }}]\n ');
|
||||
sfun_output_names = [sfun_output_names; 'xtraj [{{ dims.nx * (dims.N + 1) }}]'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.solver_status == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
|
||||
sfun_output_names = [sfun_output_names; 'solver_status'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.cost_value == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') cost function value\n ');
|
||||
sfun_output_names = [sfun_output_names; 'cost_value'];
|
||||
{%- endif %}
|
||||
|
||||
|
||||
{%- if simulink_opts.outputs.KKT_residual == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
|
||||
sfun_output_names = [sfun_output_names; 'KKT_residual'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.KKT_residuals == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') KKT residuals, size [4] (stat, eq, ineq, comp)\n ');
|
||||
sfun_output_names = [sfun_output_names; 'KKT_residuals [4]'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
|
||||
sfun_output_names = [sfun_output_names; 'x1'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
|
||||
sfun_output_names = [sfun_output_names; 'CPU_time'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_sim == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') CPU time integrator\n ');
|
||||
sfun_output_names = [sfun_output_names; 'CPU_time_sim'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_qp == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') CPU time QP solution\n ');
|
||||
sfun_output_names = [sfun_output_names; 'CPU_time_qp'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_lin == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') CPU time linearization (including integrator)\n ');
|
||||
sfun_output_names = [sfun_output_names; 'CPU_time_lin'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.sqp_iter == 1 %}
|
||||
i_out = i_out + 1;
|
||||
output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
|
||||
sfun_output_names = [sfun_output_names; 'sqp_iter'];
|
||||
{%- endif %}
|
||||
|
||||
fprintf(output_note)
|
||||
|
||||
% The mask drawing command is:
|
||||
% ---
|
||||
% global sfun_input_names sfun_output_names
|
||||
% for i = 1:length(sfun_input_names)
|
||||
% port_label('input', i, sfun_input_names{i})
|
||||
% end
|
||||
% for i = 1:length(sfun_output_names)
|
||||
% port_label('output', i, sfun_output_names{i})
|
||||
% end
|
||||
% ---
|
||||
% It can be used by copying it in sfunction/Mask/Edit mask/Icon drawing commands
|
||||
% (you can access it wirth ctrl+M on the s-function)
|
||||
137
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m
vendored
Normal file
137
third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m
vendored
Normal file
@@ -0,0 +1,137 @@
|
||||
%
|
||||
% Copyright (c) The acados authors.
|
||||
%
|
||||
% This file is part of acados.
|
||||
%
|
||||
% The 2-Clause BSD License
|
||||
%
|
||||
% Redistribution and use in source and binary forms, with or without
|
||||
% modification, are permitted provided that the following conditions are met:
|
||||
%
|
||||
% 1. Redistributions of source code must retain the above copyright notice,
|
||||
% this list of conditions and the following disclaimer.
|
||||
%
|
||||
% 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
% this list of conditions and the following disclaimer in the documentation
|
||||
% and/or other materials provided with the distribution.
|
||||
%
|
||||
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
% POSSIBILITY OF SUCH DAMAGE.;
|
||||
|
||||
%
|
||||
|
||||
SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ...
|
||||
'acados_sim_solver_{{ model.name }}.c ', ...
|
||||
{%- if solver_options.integrator_type == 'ERK' %}
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c ',...
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c ',...
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_vde_adj.c ',...
|
||||
{%- if solver_options.hessian_approx == 'EXACT' %}
|
||||
'{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c ',...
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c ', ...
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c ', ...
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c ', ...
|
||||
{%- if solver_options.hessian_approx == 'EXACT' %}
|
||||
'{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c ',...
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{%- if model.gnsf.purely_linear != 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c ',...
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c ',...
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c ',...
|
||||
{%- if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c ',...
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
'{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c ',...
|
||||
{%- endif %}
|
||||
];
|
||||
|
||||
INC_PATH = '{{ acados_include_path }}';
|
||||
|
||||
INCS = [ ' -I', fullfile(INC_PATH, 'blasfeo', 'include'), ...
|
||||
' -I', fullfile(INC_PATH, 'hpipm', 'include'), ...
|
||||
' -I', INC_PATH, ' -I', fullfile(INC_PATH, 'acados'), ' '];
|
||||
|
||||
CFLAGS = ' -O';
|
||||
|
||||
LIB_PATH = '{{ acados_lib_path }}';
|
||||
|
||||
LIBS = '-lacados -lblasfeo -lhpipm';
|
||||
|
||||
try
|
||||
% eval( [ 'mex -v -output acados_sim_solver_sfunction_{{ model.name }} ', ...
|
||||
eval( [ 'mex -output acados_sim_solver_sfunction_{{ model.name }} ', ...
|
||||
CFLAGS, INCS, ' ', SOURCES, ' -L', LIB_PATH, ' ', LIBS ]);
|
||||
|
||||
catch exception
|
||||
disp('make_sfun failed with the following exception:')
|
||||
disp(exception);
|
||||
disp('Try adding -v to the mex command above to get more information.')
|
||||
keyboard
|
||||
end
|
||||
|
||||
|
||||
fprintf( [ '\n\nSuccessfully created sfunction:\nacados_sim_solver_sfunction_{{ model.name }}', '.', ...
|
||||
eval('mexext')] );
|
||||
|
||||
|
||||
global sfun_sim_input_names
|
||||
sfun_sim_input_names = {};
|
||||
|
||||
%% print note on usage of s-function
|
||||
fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
|
||||
input_note = 'Inputs are:\n1) x0, initial state, size [{{ dims.nx }}]\n ';
|
||||
i_in = 2;
|
||||
sfun_sim_input_names = [sfun_sim_input_names; 'x0 [{{ dims.nx }}]'];
|
||||
|
||||
{%- if dims.nu > 0 %}
|
||||
input_note = strcat(input_note, num2str(i_in), ') u, size [{{ dims.nu }}]\n ');
|
||||
i_in = i_in + 1;
|
||||
sfun_sim_input_names = [sfun_sim_input_names; 'u [{{ dims.nu }}]'];
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.np > 0 %}
|
||||
input_note = strcat(input_note, num2str(i_in), ') parameters, size [{{ dims.np }}]\n ');
|
||||
i_in = i_in + 1;
|
||||
sfun_sim_input_names = [sfun_sim_input_names; 'p [{{ dims.np }}]'];
|
||||
{%- endif %}
|
||||
|
||||
|
||||
fprintf(input_note)
|
||||
|
||||
disp(' ')
|
||||
|
||||
global sfun_sim_output_names
|
||||
sfun_sim_output_names = {};
|
||||
|
||||
output_note = strcat('Outputs are:\n', ...
|
||||
'1) x1 - simulated state, size [{{ dims.nx }}]\n');
|
||||
sfun_sim_output_names = [sfun_sim_output_names; 'x1 [{{ dims.nx }}]'];
|
||||
|
||||
fprintf(output_note)
|
||||
|
||||
|
||||
% The mask drawing command is:
|
||||
% ---
|
||||
% global sfun_sim_input_names sfun_sim_output_names
|
||||
% for i = 1:length(sfun_sim_input_names)
|
||||
% port_label('input', i, sfun_sim_input_names{i})
|
||||
% end
|
||||
% for i = 1:length(sfun_sim_output_names)
|
||||
% port_label('output', i, sfun_sim_output_names{i})
|
||||
% end
|
||||
% ---
|
||||
% It can be used by copying it in sfunction/Mask/Edit mask/Icon drawing commands
|
||||
% (you can access it wirth ctrl+M on the s-function)
|
||||
270
third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m
vendored
Normal file
270
third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m
vendored
Normal file
@@ -0,0 +1,270 @@
|
||||
%
|
||||
% Copyright (c) The acados authors.
|
||||
%
|
||||
% This file is part of acados.
|
||||
%
|
||||
% The 2-Clause BSD License
|
||||
%
|
||||
% Redistribution and use in source and binary forms, with or without
|
||||
% modification, are permitted provided that the following conditions are met:
|
||||
%
|
||||
% 1. Redistributions of source code must retain the above copyright notice,
|
||||
% this list of conditions and the following disclaimer.
|
||||
%
|
||||
% 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
% this list of conditions and the following disclaimer in the documentation
|
||||
% and/or other materials provided with the distribution.
|
||||
%
|
||||
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
% POSSIBILITY OF SUCH DAMAGE.;
|
||||
|
||||
%
|
||||
|
||||
classdef {{ model.name }}_mex_solver < handle
|
||||
|
||||
properties
|
||||
C_ocp
|
||||
C_ocp_ext_fun
|
||||
cost_ext_fun_type
|
||||
cost_ext_fun_type_e
|
||||
N
|
||||
name
|
||||
code_gen_dir
|
||||
end % properties
|
||||
|
||||
|
||||
|
||||
methods
|
||||
|
||||
% constructor
|
||||
function obj = {{ model.name }}_mex_solver()
|
||||
make_mex_{{ model.name }}();
|
||||
[obj.C_ocp, obj.C_ocp_ext_fun] = acados_mex_create_{{ model.name }}();
|
||||
% to have path to destructor when changing directory
|
||||
addpath('.')
|
||||
obj.cost_ext_fun_type = '{{ cost.cost_ext_fun_type }}';
|
||||
obj.cost_ext_fun_type_e = '{{ cost.cost_ext_fun_type_e }}';
|
||||
obj.N = {{ dims.N }};
|
||||
obj.name = '{{ model.name }}';
|
||||
obj.code_gen_dir = pwd();
|
||||
end
|
||||
|
||||
% destructor
|
||||
function delete(obj)
|
||||
disp("delete template...");
|
||||
return_dir = pwd();
|
||||
cd(obj.code_gen_dir);
|
||||
if ~isempty(obj.C_ocp)
|
||||
acados_mex_free_{{ model.name }}(obj.C_ocp);
|
||||
end
|
||||
cd(return_dir);
|
||||
disp("done.");
|
||||
end
|
||||
|
||||
% solve
|
||||
function solve(obj)
|
||||
acados_mex_solve_{{ model.name }}(obj.C_ocp);
|
||||
end
|
||||
|
||||
% set -- borrowed from MEX interface
|
||||
function set(varargin)
|
||||
obj = varargin{1};
|
||||
field = varargin{2};
|
||||
value = varargin{3};
|
||||
if ~isa(field, 'char')
|
||||
error('field must be a char vector, use '' ''');
|
||||
end
|
||||
if nargin==3
|
||||
acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value);
|
||||
elseif nargin==4
|
||||
stage = varargin{4};
|
||||
acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value, stage);
|
||||
else
|
||||
disp('acados_ocp.set: wrong number of input arguments (2 or 3 allowed)');
|
||||
end
|
||||
end
|
||||
|
||||
function value = get_cost(obj)
|
||||
value = ocp_get_cost(obj.C_ocp);
|
||||
end
|
||||
|
||||
% get -- borrowed from MEX interface
|
||||
function value = get(varargin)
|
||||
% usage:
|
||||
% obj.get(field, value, [stage])
|
||||
obj = varargin{1};
|
||||
field = varargin{2};
|
||||
if any(strfind('sens', field))
|
||||
error('field sens* (sensitivities of optimal solution) not yet supported for templated MEX.')
|
||||
end
|
||||
if ~isa(field, 'char')
|
||||
error('field must be a char vector, use '' ''');
|
||||
end
|
||||
|
||||
if nargin==2
|
||||
value = ocp_get(obj.C_ocp, field);
|
||||
elseif nargin==3
|
||||
stage = varargin{3};
|
||||
value = ocp_get(obj.C_ocp, field, stage);
|
||||
else
|
||||
disp('acados_ocp.get: wrong number of input arguments (1 or 2 allowed)');
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
function [] = store_iterate(varargin)
|
||||
%%% Stores the current iterate of the ocp solver in a json file.
|
||||
%%% param1: filename: if not set, use model_name + timestamp + '.json'
|
||||
%%% param2: overwrite: if false and filename exists add timestamp to filename
|
||||
|
||||
obj = varargin{1};
|
||||
filename = '';
|
||||
overwrite = false;
|
||||
|
||||
if nargin>=2
|
||||
filename = varargin{2};
|
||||
if ~isa(filename, 'char')
|
||||
error('filename must be a char vector, use '' ''');
|
||||
end
|
||||
end
|
||||
|
||||
if nargin==3
|
||||
overwrite = varargin{3};
|
||||
end
|
||||
|
||||
if nargin > 3
|
||||
disp('acados_ocp.get: wrong number of input arguments (1 or 2 allowed)');
|
||||
end
|
||||
|
||||
if strcmp(filename,'')
|
||||
filename = [obj.name '_iterate.json'];
|
||||
end
|
||||
if ~overwrite
|
||||
% append timestamp
|
||||
if exist(filename, 'file')
|
||||
filename = filename(1:end-5);
|
||||
filename = [filename '_' datestr(now,'yyyy-mm-dd-HH:MM:SS') '.json'];
|
||||
end
|
||||
end
|
||||
filename = fullfile(pwd, filename);
|
||||
|
||||
% get iterate:
|
||||
solution = struct();
|
||||
for i=0:obj.N
|
||||
solution.(['x_' num2str(i)]) = obj.get('x', i);
|
||||
solution.(['lam_' num2str(i)]) = obj.get('lam', i);
|
||||
solution.(['t_' num2str(i)]) = obj.get('t', i);
|
||||
solution.(['sl_' num2str(i)]) = obj.get('sl', i);
|
||||
solution.(['su_' num2str(i)]) = obj.get('su', i);
|
||||
end
|
||||
for i=0:obj.N-1
|
||||
solution.(['z_' num2str(i)]) = obj.get('z', i);
|
||||
solution.(['u_' num2str(i)]) = obj.get('u', i);
|
||||
solution.(['pi_' num2str(i)]) = obj.get('pi', i);
|
||||
end
|
||||
|
||||
acados_folder = getenv('ACADOS_INSTALL_DIR');
|
||||
addpath(fullfile(acados_folder, 'external', 'jsonlab'));
|
||||
savejson('', solution, filename);
|
||||
|
||||
json_string = savejson('', solution, 'ForceRootName', 0);
|
||||
|
||||
fid = fopen(filename, 'w');
|
||||
if fid == -1, error('store_iterate: Cannot create JSON file'); end
|
||||
fwrite(fid, json_string, 'char');
|
||||
fclose(fid);
|
||||
|
||||
disp(['stored current iterate in ' filename]);
|
||||
end
|
||||
|
||||
|
||||
function [] = load_iterate(obj, filename)
|
||||
%%% Loads the iterate stored in json file with filename into the ocp solver.
|
||||
acados_folder = getenv('ACADOS_INSTALL_DIR');
|
||||
addpath(fullfile(acados_folder, 'external', 'jsonlab'));
|
||||
filename = fullfile(pwd, filename);
|
||||
|
||||
if ~exist(filename, 'file')
|
||||
error(['load_iterate: failed, file does not exist: ' filename])
|
||||
end
|
||||
|
||||
solution = loadjson(filename);
|
||||
keys = fieldnames(solution);
|
||||
|
||||
for k = 1:numel(keys)
|
||||
key = keys{k};
|
||||
key_parts = strsplit(key, '_');
|
||||
field = key_parts{1};
|
||||
stage = key_parts{2};
|
||||
|
||||
val = solution.(key);
|
||||
|
||||
% check if array is empty (can happen for z)
|
||||
if numel(val) > 0
|
||||
obj.set(field, val, str2num(stage))
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
% print
|
||||
function print(varargin)
|
||||
if nargin < 2
|
||||
field = 'stat';
|
||||
else
|
||||
field = varargin{2};
|
||||
end
|
||||
|
||||
obj = varargin{1};
|
||||
|
||||
if strcmp(field, 'stat')
|
||||
stat = obj.get('stat');
|
||||
{%- if solver_options.nlp_solver_type == "SQP" %}
|
||||
fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha');
|
||||
if size(stat,2)>8
|
||||
fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp');
|
||||
end
|
||||
fprintf('\n');
|
||||
for jj=1:size(stat,1)
|
||||
fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d\t%e', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7), stat(jj, 8));
|
||||
if size(stat,2)>8
|
||||
fprintf('\t%e\t%e\t%e\t%e', stat(jj,9), stat(jj,10), stat(jj,11), stat(jj,12));
|
||||
end
|
||||
fprintf('\n');
|
||||
end
|
||||
fprintf('\n');
|
||||
{%- else %}
|
||||
fprintf('\niter\tqp_status\tqp_iter');
|
||||
if size(stat,2)>3
|
||||
fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp');
|
||||
end
|
||||
fprintf('\n');
|
||||
for jj=1:size(stat,1)
|
||||
fprintf('%d\t%d\t\t%d', stat(jj,1), stat(jj,2), stat(jj,3));
|
||||
if size(stat,2)>3
|
||||
fprintf('\t%e\t%e\t%e\t%e', stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7));
|
||||
end
|
||||
fprintf('\n');
|
||||
end
|
||||
{% endif %}
|
||||
|
||||
else
|
||||
fprintf('unsupported field in function print of acados_ocp.print, got %s', field);
|
||||
keyboard
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
end % methods
|
||||
|
||||
end % class
|
||||
|
||||
218
third_party/acados/acados_template/c_templates_tera/model.in.h
vendored
Normal file
218
third_party/acados/acados_template/c_templates_tera/model.in.h
vendored
Normal file
@@ -0,0 +1,218 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#ifndef {{ model.name }}_MODEL
|
||||
#define {{ model.name }}_MODEL
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
{%- if solver_options.hessian_approx %}
|
||||
{%- set hessian_approx = solver_options.hessian_approx %}
|
||||
{%- elif solver_options.sens_hess %}
|
||||
{%- set hessian_approx = "EXACT" %}
|
||||
{%- else %}
|
||||
{%- set hessian_approx = "GAUSS_NEWTON" %}
|
||||
{%- endif %}
|
||||
|
||||
{% if solver_options.integrator_type == "IRK" or solver_options.integrator_type == "LIFTED_IRK" %}
|
||||
{% if model.dyn_ext_fun_type == "casadi" %}
|
||||
// implicit ODE: function
|
||||
int {{ model.name }}_impl_dae_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_impl_dae_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_impl_dae_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_impl_dae_fun_sparsity_out(int);
|
||||
int {{ model.name }}_impl_dae_fun_n_in(void);
|
||||
int {{ model.name }}_impl_dae_fun_n_out(void);
|
||||
|
||||
// implicit ODE: function + jacobians
|
||||
int {{ model.name }}_impl_dae_fun_jac_x_xdot_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in(int);
|
||||
const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out(int);
|
||||
int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in(void);
|
||||
int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out(void);
|
||||
|
||||
// implicit ODE: jacobians only
|
||||
int {{ model.name }}_impl_dae_jac_x_xdot_u_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_impl_dae_jac_x_xdot_u_z_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in(int);
|
||||
const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out(int);
|
||||
int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in(void);
|
||||
int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out(void);
|
||||
|
||||
// implicit ODE - for lifted_irk
|
||||
int {{ model.name }}_impl_dae_fun_jac_x_xdot_u(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_in(int);
|
||||
const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out(int);
|
||||
int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in(void);
|
||||
int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out(void);
|
||||
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
// implicit ODE - hessian
|
||||
int {{ model.name }}_impl_dae_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_impl_dae_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_impl_dae_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_impl_dae_hess_sparsity_out(int);
|
||||
int {{ model.name }}_impl_dae_hess_n_in(void);
|
||||
int {{ model.name }}_impl_dae_hess_n_out(void);
|
||||
{% endif %}
|
||||
{% else %}{# ext_fun_type #}
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
int {{ model.dyn_impl_dae_hess }}(void **, void **, void *);
|
||||
{% endif %}
|
||||
int {{ model.dyn_impl_dae_fun_jac }}(void **, void **, void *);
|
||||
int {{ model.dyn_impl_dae_jac }}(void **, void **, void *);
|
||||
int {{ model.dyn_impl_dae_fun }}(void **, void **, void *);
|
||||
{% endif %}{# ext_fun_type #}
|
||||
|
||||
{% elif solver_options.integrator_type == "GNSF" %}
|
||||
/* GNSF Functions */
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
// phi_fun
|
||||
int {{ model.name }}_gnsf_phi_fun(const double** arg, double** res, int* iw, double* w, void *mem);
|
||||
int {{ model.name }}_gnsf_phi_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_gnsf_phi_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_gnsf_phi_fun_sparsity_out(int);
|
||||
int {{ model.name }}_gnsf_phi_fun_n_in(void);
|
||||
int {{ model.name }}_gnsf_phi_fun_n_out(void);
|
||||
|
||||
// phi_fun_jac_y
|
||||
int {{ model.name }}_gnsf_phi_fun_jac_y(const double** arg, double** res, int* iw, double* w, void *mem);
|
||||
int {{ model.name }}_gnsf_phi_fun_jac_y_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in(int);
|
||||
const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out(int);
|
||||
int {{ model.name }}_gnsf_phi_fun_jac_y_n_in(void);
|
||||
int {{ model.name }}_gnsf_phi_fun_jac_y_n_out(void);
|
||||
|
||||
// phi_jac_y_uhat
|
||||
int {{ model.name }}_gnsf_phi_jac_y_uhat(const double** arg, double** res, int* iw, double* w, void *mem);
|
||||
int {{ model.name }}_gnsf_phi_jac_y_uhat_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in(int);
|
||||
const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out(int);
|
||||
int {{ model.name }}_gnsf_phi_jac_y_uhat_n_in(void);
|
||||
int {{ model.name }}_gnsf_phi_jac_y_uhat_n_out(void);
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
// f_lo_fun_jac_x1k1uz
|
||||
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz(const double** arg, double** res, int* iw, double* w, void *mem);
|
||||
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in(int);
|
||||
const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out(int);
|
||||
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in(void);
|
||||
int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out(void);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
// used to import model matrices
|
||||
int {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_n_in(void);
|
||||
int {{ model.name }}_gnsf_get_matrices_fun_n_out(void);
|
||||
|
||||
{% elif solver_options.integrator_type == "ERK" %}
|
||||
/* explicit ODE */
|
||||
|
||||
// explicit ODE
|
||||
int {{ model.name }}_expl_ode_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_expl_ode_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_expl_ode_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_expl_ode_fun_sparsity_out(int);
|
||||
int {{ model.name }}_expl_ode_fun_n_in(void);
|
||||
int {{ model.name }}_expl_ode_fun_n_out(void);
|
||||
|
||||
// explicit forward VDE
|
||||
int {{ model.name }}_expl_vde_forw(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_expl_vde_forw_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_expl_vde_forw_sparsity_in(int);
|
||||
const int *{{ model.name }}_expl_vde_forw_sparsity_out(int);
|
||||
int {{ model.name }}_expl_vde_forw_n_in(void);
|
||||
int {{ model.name }}_expl_vde_forw_n_out(void);
|
||||
|
||||
// explicit adjoint VDE
|
||||
int {{ model.name }}_expl_vde_adj(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_expl_vde_adj_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_expl_vde_adj_sparsity_in(int);
|
||||
const int *{{ model.name }}_expl_vde_adj_sparsity_out(int);
|
||||
int {{ model.name }}_expl_vde_adj_n_in(void);
|
||||
int {{ model.name }}_expl_vde_adj_n_out(void);
|
||||
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
int {{ model.name }}_expl_ode_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_expl_ode_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_expl_ode_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_expl_ode_hess_sparsity_out(int);
|
||||
int {{ model.name }}_expl_ode_hess_n_in(void);
|
||||
int {{ model.name }}_expl_ode_hess_n_out(void);
|
||||
{%- endif %}
|
||||
|
||||
{% elif solver_options.integrator_type == "DISCRETE" %}
|
||||
|
||||
{% if model.dyn_ext_fun_type == "casadi" %}
|
||||
int {{ model.name }}_dyn_disc_phi_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_dyn_disc_phi_fun_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_in(int);
|
||||
const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_out(int);
|
||||
int {{ model.name }}_dyn_disc_phi_fun_n_in(void);
|
||||
int {{ model.name }}_dyn_disc_phi_fun_n_out(void);
|
||||
|
||||
int {{ model.name }}_dyn_disc_phi_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_dyn_disc_phi_fun_jac_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_in(int);
|
||||
const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_out(int);
|
||||
int {{ model.name }}_dyn_disc_phi_fun_jac_n_in(void);
|
||||
int {{ model.name }}_dyn_disc_phi_fun_jac_n_out(void);
|
||||
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
int {{ model.name }}_dyn_disc_phi_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
|
||||
int {{ model.name }}_dyn_disc_phi_fun_jac_hess_work(int *, int *, int *, int *);
|
||||
const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_in(int);
|
||||
const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_out(int);
|
||||
int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_in(void);
|
||||
int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_out(void);
|
||||
{%- endif %}
|
||||
{% else %}
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
int {{ model.dyn_disc_fun_jac_hess }}(void **, void **, void *);
|
||||
{% endif %}
|
||||
int {{ model.dyn_disc_fun_jac }}(void **, void **, void *);
|
||||
int {{ model.dyn_disc_fun }}(void **, void **, void *);
|
||||
{% endif %}
|
||||
|
||||
|
||||
{% endif %}
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // {{ model.name }}_MODEL
|
||||
708
third_party/acados/acados_template/casadi_function_generation.py
vendored
Normal file
708
third_party/acados/acados_template/casadi_function_generation.py
vendored
Normal file
@@ -0,0 +1,708 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
import os
|
||||
import casadi as ca
|
||||
from .utils import is_empty, casadi_length
|
||||
|
||||
|
||||
def get_casadi_symbol(x):
|
||||
if isinstance(x, ca.MX):
|
||||
return ca.MX.sym
|
||||
elif isinstance(x, ca.SX):
|
||||
return ca.SX.sym
|
||||
else:
|
||||
raise TypeError("Expected casadi SX or MX.")
|
||||
|
||||
################
|
||||
# Dynamics
|
||||
################
|
||||
|
||||
|
||||
def generate_c_code_discrete_dynamics( model, opts ):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
# load model
|
||||
x = model.x
|
||||
u = model.u
|
||||
p = model.p
|
||||
phi = model.disc_dyn_expr
|
||||
model_name = model.name
|
||||
nx = casadi_length(x)
|
||||
|
||||
symbol = get_casadi_symbol(x)
|
||||
# assume nx1 = nx !!!
|
||||
lam = symbol('lam', nx, 1)
|
||||
|
||||
# generate jacobians
|
||||
ux = ca.vertcat(u,x)
|
||||
jac_ux = ca.jacobian(phi, ux)
|
||||
# generate adjoint
|
||||
adj_ux = ca.jtimes(phi, ux, lam, True)
|
||||
# generate hessian
|
||||
hess_ux = ca.jacobian(adj_ux, ux)
|
||||
|
||||
# change directory
|
||||
cwd = os.getcwd()
|
||||
model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model'))
|
||||
if not os.path.exists(model_dir):
|
||||
os.makedirs(model_dir)
|
||||
os.chdir(model_dir)
|
||||
|
||||
# set up & generate ca.Functions
|
||||
fun_name = model_name + '_dyn_disc_phi_fun'
|
||||
phi_fun = ca.Function(fun_name, [x, u, p], [phi])
|
||||
phi_fun.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_dyn_disc_phi_fun_jac'
|
||||
phi_fun_jac_ut_xt = ca.Function(fun_name, [x, u, p], [phi, jac_ux.T])
|
||||
phi_fun_jac_ut_xt.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_dyn_disc_phi_fun_jac_hess'
|
||||
phi_fun_jac_ut_xt_hess = ca.Function(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux])
|
||||
phi_fun_jac_ut_xt_hess.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
os.chdir(cwd)
|
||||
return
|
||||
|
||||
|
||||
|
||||
def generate_c_code_explicit_ode( model, opts ):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
generate_hess = opts["generate_hess"]
|
||||
|
||||
# load model
|
||||
x = model.x
|
||||
u = model.u
|
||||
p = model.p
|
||||
f_expl = model.f_expl_expr
|
||||
model_name = model.name
|
||||
|
||||
## get model dimensions
|
||||
nx = x.size()[0]
|
||||
nu = u.size()[0]
|
||||
|
||||
symbol = get_casadi_symbol(x)
|
||||
|
||||
## set up functions to be exported
|
||||
Sx = symbol('Sx', nx, nx)
|
||||
Sp = symbol('Sp', nx, nu)
|
||||
lambdaX = symbol('lambdaX', nx, 1)
|
||||
|
||||
fun_name = model_name + '_expl_ode_fun'
|
||||
|
||||
## Set up functions
|
||||
expl_ode_fun = ca.Function(fun_name, [x, u, p], [f_expl])
|
||||
|
||||
vdeX = ca.jtimes(f_expl,x,Sx)
|
||||
vdeP = ca.jacobian(f_expl,u) + ca.jtimes(f_expl,x,Sp)
|
||||
|
||||
fun_name = model_name + '_expl_vde_forw'
|
||||
|
||||
expl_vde_forw = ca.Function(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP])
|
||||
|
||||
adj = ca.jtimes(f_expl, ca.vertcat(x, u), lambdaX, True)
|
||||
|
||||
fun_name = model_name + '_expl_vde_adj'
|
||||
expl_vde_adj = ca.Function(fun_name, [x, lambdaX, u, p], [adj])
|
||||
|
||||
if generate_hess:
|
||||
S_forw = ca.vertcat(ca.horzcat(Sx, Sp), ca.horzcat(ca.DM.zeros(nu,nx), ca.DM.eye(nu)))
|
||||
hess = ca.mtimes(ca.transpose(S_forw),ca.jtimes(adj, ca.vertcat(x,u), S_forw))
|
||||
hess2 = []
|
||||
for j in range(nx+nu):
|
||||
for i in range(j,nx+nu):
|
||||
hess2 = ca.vertcat(hess2, hess[i,j])
|
||||
|
||||
fun_name = model_name + '_expl_ode_hess'
|
||||
expl_ode_hess = ca.Function(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2])
|
||||
|
||||
# change directory
|
||||
cwd = os.getcwd()
|
||||
model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model'))
|
||||
if not os.path.exists(model_dir):
|
||||
os.makedirs(model_dir)
|
||||
os.chdir(model_dir)
|
||||
|
||||
# generate C code
|
||||
fun_name = model_name + '_expl_ode_fun'
|
||||
expl_ode_fun.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_expl_vde_forw'
|
||||
expl_vde_forw.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_expl_vde_adj'
|
||||
expl_vde_adj.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
if generate_hess:
|
||||
fun_name = model_name + '_expl_ode_hess'
|
||||
expl_ode_hess.generate(fun_name, casadi_codegen_opts)
|
||||
os.chdir(cwd)
|
||||
|
||||
return
|
||||
|
||||
|
||||
def generate_c_code_implicit_ode( model, opts ):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
# load model
|
||||
x = model.x
|
||||
xdot = model.xdot
|
||||
u = model.u
|
||||
z = model.z
|
||||
p = model.p
|
||||
f_impl = model.f_impl_expr
|
||||
model_name = model.name
|
||||
|
||||
# get model dimensions
|
||||
nx = casadi_length(x)
|
||||
nz = casadi_length(z)
|
||||
|
||||
# generate jacobians
|
||||
jac_x = ca.jacobian(f_impl, x)
|
||||
jac_xdot = ca.jacobian(f_impl, xdot)
|
||||
jac_u = ca.jacobian(f_impl, u)
|
||||
jac_z = ca.jacobian(f_impl, z)
|
||||
|
||||
# Set up functions
|
||||
p = model.p
|
||||
fun_name = model_name + '_impl_dae_fun'
|
||||
impl_dae_fun = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl])
|
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z'
|
||||
impl_dae_fun_jac_x_xdot_z = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z])
|
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z'
|
||||
impl_dae_fun_jac_x_xdot_u_z = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z])
|
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u'
|
||||
impl_dae_fun_jac_x_xdot_u = ca.Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u])
|
||||
|
||||
fun_name = model_name + '_impl_dae_jac_x_xdot_u_z'
|
||||
impl_dae_jac_x_xdot_u_z = ca.Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z])
|
||||
|
||||
if opts["generate_hess"]:
|
||||
x_xdot_z_u = ca.vertcat(x, xdot, z, u)
|
||||
symbol = get_casadi_symbol(x)
|
||||
multiplier = symbol('multiplier', nx + nz)
|
||||
ADJ = ca.jtimes(f_impl, x_xdot_z_u, multiplier, True)
|
||||
HESS = ca.jacobian(ADJ, x_xdot_z_u)
|
||||
fun_name = model_name + '_impl_dae_hess'
|
||||
impl_dae_hess = ca.Function(fun_name, [x, xdot, u, z, multiplier, p], [HESS])
|
||||
|
||||
# change directory
|
||||
cwd = os.getcwd()
|
||||
model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model'))
|
||||
if not os.path.exists(model_dir):
|
||||
os.makedirs(model_dir)
|
||||
os.chdir(model_dir)
|
||||
|
||||
# generate C code
|
||||
fun_name = model_name + '_impl_dae_fun'
|
||||
impl_dae_fun.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z'
|
||||
impl_dae_fun_jac_x_xdot_z.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_impl_dae_jac_x_xdot_u_z'
|
||||
impl_dae_jac_x_xdot_u_z.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z'
|
||||
impl_dae_fun_jac_x_xdot_u_z.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u'
|
||||
impl_dae_fun_jac_x_xdot_u.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
if opts["generate_hess"]:
|
||||
fun_name = model_name + '_impl_dae_hess'
|
||||
impl_dae_hess.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
os.chdir(cwd)
|
||||
return
|
||||
|
||||
|
||||
def generate_c_code_gnsf( model, opts ):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
model_name = model.name
|
||||
|
||||
# set up directory
|
||||
cwd = os.getcwd()
|
||||
model_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model_name}_model'))
|
||||
if not os.path.exists(model_dir):
|
||||
os.makedirs(model_dir)
|
||||
os.chdir(model_dir)
|
||||
|
||||
# obtain gnsf dimensions
|
||||
get_matrices_fun = model.get_matrices_fun
|
||||
phi_fun = model.phi_fun
|
||||
|
||||
size_gnsf_A = get_matrices_fun.size_out(0)
|
||||
gnsf_nx1 = size_gnsf_A[1]
|
||||
gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
|
||||
gnsf_nuhat = max(phi_fun.size_in(1))
|
||||
gnsf_ny = max(phi_fun.size_in(0))
|
||||
gnsf_nout = max(phi_fun.size_out(0))
|
||||
|
||||
# set up expressions
|
||||
# if the model uses ca.MX because of cost/constraints
|
||||
# the DAE can be exported as ca.SX -> detect GNSF in Matlab
|
||||
# -> evaluated ca.SX GNSF functions with ca.MX.
|
||||
u = model.u
|
||||
symbol = get_casadi_symbol(u)
|
||||
|
||||
y = symbol("y", gnsf_ny, 1)
|
||||
uhat = symbol("uhat", gnsf_nuhat, 1)
|
||||
p = model.p
|
||||
x1 = symbol("gnsf_x1", gnsf_nx1, 1)
|
||||
x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1)
|
||||
z1 = symbol("gnsf_z1", gnsf_nz1, 1)
|
||||
dummy = symbol("gnsf_dummy", 1, 1)
|
||||
empty_var = symbol("gnsf_empty_var", 0, 0)
|
||||
|
||||
## generate C code
|
||||
fun_name = model_name + '_gnsf_phi_fun'
|
||||
phi_fun_ = ca.Function(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)])
|
||||
phi_fun_.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_gnsf_phi_fun_jac_y'
|
||||
phi_fun_jac_y = model.phi_fun_jac_y
|
||||
phi_fun_jac_y_ = ca.Function(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p))
|
||||
phi_fun_jac_y_.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_gnsf_phi_jac_y_uhat'
|
||||
phi_jac_y_uhat = model.phi_jac_y_uhat
|
||||
phi_jac_y_uhat_ = ca.Function(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p))
|
||||
phi_jac_y_uhat_.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_gnsf_f_lo_fun_jac_x1k1uz'
|
||||
f_lo_fun_jac_x1k1uz = model.f_lo_fun_jac_x1k1uz
|
||||
f_lo_fun_jac_x1k1uz_eval = f_lo_fun_jac_x1k1uz(x1, x1dot, z1, u, p)
|
||||
|
||||
# avoid codegeneration issue
|
||||
if not isinstance(f_lo_fun_jac_x1k1uz_eval, tuple) and is_empty(f_lo_fun_jac_x1k1uz_eval):
|
||||
f_lo_fun_jac_x1k1uz_eval = [empty_var]
|
||||
|
||||
f_lo_fun_jac_x1k1uz_ = ca.Function(fun_name, [x1, x1dot, z1, u, p],
|
||||
f_lo_fun_jac_x1k1uz_eval)
|
||||
f_lo_fun_jac_x1k1uz_.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
fun_name = model_name + '_gnsf_get_matrices_fun'
|
||||
get_matrices_fun_ = ca.Function(fun_name, [dummy], get_matrices_fun(1))
|
||||
get_matrices_fun_.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
# remove fields for json dump
|
||||
del model.phi_fun
|
||||
del model.phi_fun_jac_y
|
||||
del model.phi_jac_y_uhat
|
||||
del model.f_lo_fun_jac_x1k1uz
|
||||
del model.get_matrices_fun
|
||||
|
||||
os.chdir(cwd)
|
||||
|
||||
return
|
||||
|
||||
|
||||
################
|
||||
# Cost
|
||||
################
|
||||
|
||||
def generate_c_code_external_cost(model, stage_type, opts):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
x = model.x
|
||||
p = model.p
|
||||
u = model.u
|
||||
z = model.z
|
||||
symbol = get_casadi_symbol(x)
|
||||
|
||||
if stage_type == 'terminal':
|
||||
suffix_name = "_cost_ext_cost_e_fun"
|
||||
suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess"
|
||||
suffix_name_jac = "_cost_ext_cost_e_fun_jac"
|
||||
ext_cost = model.cost_expr_ext_cost_e
|
||||
custom_hess = model.cost_expr_ext_cost_custom_hess_e
|
||||
# Last stage cannot depend on u and z
|
||||
u = symbol("u", 0, 0)
|
||||
z = symbol("z", 0, 0)
|
||||
|
||||
elif stage_type == 'path':
|
||||
suffix_name = "_cost_ext_cost_fun"
|
||||
suffix_name_hess = "_cost_ext_cost_fun_jac_hess"
|
||||
suffix_name_jac = "_cost_ext_cost_fun_jac"
|
||||
ext_cost = model.cost_expr_ext_cost
|
||||
custom_hess = model.cost_expr_ext_cost_custom_hess
|
||||
|
||||
elif stage_type == 'initial':
|
||||
suffix_name = "_cost_ext_cost_0_fun"
|
||||
suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess"
|
||||
suffix_name_jac = "_cost_ext_cost_0_fun_jac"
|
||||
ext_cost = model.cost_expr_ext_cost_0
|
||||
custom_hess = model.cost_expr_ext_cost_custom_hess_0
|
||||
|
||||
nunx = x.shape[0] + u.shape[0]
|
||||
|
||||
# set up functions to be exported
|
||||
fun_name = model.name + suffix_name
|
||||
fun_name_hess = model.name + suffix_name_hess
|
||||
fun_name_jac = model.name + suffix_name_jac
|
||||
|
||||
# generate expression for full gradient and Hessian
|
||||
hess_uxz, grad_uxz = ca.hessian(ext_cost, ca.vertcat(u, x, z))
|
||||
|
||||
hess_ux = hess_uxz[:nunx, :nunx]
|
||||
hess_z = hess_uxz[nunx:, nunx:]
|
||||
hess_z_ux = hess_uxz[nunx:, :nunx]
|
||||
|
||||
if custom_hess is not None:
|
||||
hess_ux = custom_hess
|
||||
|
||||
ext_cost_fun = ca.Function(fun_name, [x, u, z, p], [ext_cost])
|
||||
|
||||
ext_cost_fun_jac_hess = ca.Function(
|
||||
fun_name_hess, [x, u, z, p], [ext_cost, grad_uxz, hess_ux, hess_z, hess_z_ux]
|
||||
)
|
||||
ext_cost_fun_jac = ca.Function(
|
||||
fun_name_jac, [x, u, z, p], [ext_cost, grad_uxz]
|
||||
)
|
||||
|
||||
# change directory
|
||||
cwd = os.getcwd()
|
||||
cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost'))
|
||||
if not os.path.exists(cost_dir):
|
||||
os.makedirs(cost_dir)
|
||||
os.chdir(cost_dir)
|
||||
|
||||
ext_cost_fun.generate(fun_name, casadi_codegen_opts)
|
||||
ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_codegen_opts)
|
||||
ext_cost_fun_jac.generate(fun_name_jac, casadi_codegen_opts)
|
||||
|
||||
os.chdir(cwd)
|
||||
return
|
||||
|
||||
|
||||
def generate_c_code_nls_cost( model, cost_name, stage_type, opts ):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
x = model.x
|
||||
z = model.z
|
||||
p = model.p
|
||||
u = model.u
|
||||
|
||||
symbol = get_casadi_symbol(x)
|
||||
|
||||
if stage_type == 'terminal':
|
||||
middle_name = '_cost_y_e'
|
||||
u = symbol('u', 0, 0)
|
||||
y_expr = model.cost_y_expr_e
|
||||
|
||||
elif stage_type == 'initial':
|
||||
middle_name = '_cost_y_0'
|
||||
y_expr = model.cost_y_expr_0
|
||||
|
||||
elif stage_type == 'path':
|
||||
middle_name = '_cost_y'
|
||||
y_expr = model.cost_y_expr
|
||||
|
||||
# change directory
|
||||
cwd = os.getcwd()
|
||||
cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost'))
|
||||
if not os.path.exists(cost_dir):
|
||||
os.makedirs(cost_dir)
|
||||
os.chdir(cost_dir)
|
||||
|
||||
# set up expressions
|
||||
cost_jac_expr = ca.transpose(ca.jacobian(y_expr, ca.vertcat(u, x)))
|
||||
dy_dz = ca.jacobian(y_expr, z)
|
||||
ny = casadi_length(y_expr)
|
||||
|
||||
y = symbol('y', ny, 1)
|
||||
|
||||
y_adj = ca.jtimes(y_expr, ca.vertcat(u, x), y, True)
|
||||
y_hess = ca.jacobian(y_adj, ca.vertcat(u, x))
|
||||
|
||||
## generate C code
|
||||
suffix_name = '_fun'
|
||||
fun_name = cost_name + middle_name + suffix_name
|
||||
y_fun = ca.Function( fun_name, [x, u, z, p], [ y_expr ])
|
||||
y_fun.generate( fun_name, casadi_codegen_opts )
|
||||
|
||||
suffix_name = '_fun_jac_ut_xt'
|
||||
fun_name = cost_name + middle_name + suffix_name
|
||||
y_fun_jac_ut_xt = ca.Function(fun_name, [x, u, z, p], [ y_expr, cost_jac_expr, dy_dz ])
|
||||
y_fun_jac_ut_xt.generate( fun_name, casadi_codegen_opts )
|
||||
|
||||
suffix_name = '_hess'
|
||||
fun_name = cost_name + middle_name + suffix_name
|
||||
y_hess = ca.Function(fun_name, [x, u, z, y, p], [ y_hess ])
|
||||
y_hess.generate( fun_name, casadi_codegen_opts )
|
||||
|
||||
os.chdir(cwd)
|
||||
|
||||
return
|
||||
|
||||
|
||||
|
||||
def generate_c_code_conl_cost(model, cost_name, stage_type, opts):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
x = model.x
|
||||
z = model.z
|
||||
p = model.p
|
||||
|
||||
symbol = get_casadi_symbol(x)
|
||||
|
||||
if stage_type == 'terminal':
|
||||
u = symbol('u', 0, 0)
|
||||
|
||||
yref = model.cost_r_in_psi_expr_e
|
||||
inner_expr = model.cost_y_expr_e - yref
|
||||
outer_expr = model.cost_psi_expr_e
|
||||
res_expr = model.cost_r_in_psi_expr_e
|
||||
|
||||
suffix_name_fun = '_conl_cost_e_fun'
|
||||
suffix_name_fun_jac_hess = '_conl_cost_e_fun_jac_hess'
|
||||
|
||||
custom_hess = model.cost_conl_custom_outer_hess_e
|
||||
|
||||
elif stage_type == 'initial':
|
||||
u = model.u
|
||||
|
||||
yref = model.cost_r_in_psi_expr_0
|
||||
inner_expr = model.cost_y_expr_0 - yref
|
||||
outer_expr = model.cost_psi_expr_0
|
||||
res_expr = model.cost_r_in_psi_expr_0
|
||||
|
||||
suffix_name_fun = '_conl_cost_0_fun'
|
||||
suffix_name_fun_jac_hess = '_conl_cost_0_fun_jac_hess'
|
||||
|
||||
custom_hess = model.cost_conl_custom_outer_hess_0
|
||||
|
||||
elif stage_type == 'path':
|
||||
u = model.u
|
||||
|
||||
yref = model.cost_r_in_psi_expr
|
||||
inner_expr = model.cost_y_expr - yref
|
||||
outer_expr = model.cost_psi_expr
|
||||
res_expr = model.cost_r_in_psi_expr
|
||||
|
||||
suffix_name_fun = '_conl_cost_fun'
|
||||
suffix_name_fun_jac_hess = '_conl_cost_fun_jac_hess'
|
||||
|
||||
custom_hess = model.cost_conl_custom_outer_hess
|
||||
|
||||
# set up function names
|
||||
fun_name_cost_fun = model.name + suffix_name_fun
|
||||
fun_name_cost_fun_jac_hess = model.name + suffix_name_fun_jac_hess
|
||||
|
||||
# set up functions to be exported
|
||||
outer_loss_fun = ca.Function('psi', [res_expr, p], [outer_expr])
|
||||
cost_expr = outer_loss_fun(inner_expr, p)
|
||||
|
||||
outer_loss_grad_fun = ca.Function('outer_loss_grad', [res_expr, p], [ca.jacobian(outer_expr, res_expr).T])
|
||||
|
||||
if custom_hess is None:
|
||||
outer_hess_fun = ca.Function('inner_hess', [res_expr, p], [ca.hessian(outer_loss_fun(res_expr, p), res_expr)[0]])
|
||||
else:
|
||||
outer_hess_fun = ca.Function('inner_hess', [res_expr, p], [custom_hess])
|
||||
|
||||
Jt_ux_expr = ca.jacobian(inner_expr, ca.vertcat(u, x)).T
|
||||
Jt_z_expr = ca.jacobian(inner_expr, z).T
|
||||
|
||||
cost_fun = ca.Function(
|
||||
fun_name_cost_fun,
|
||||
[x, u, z, yref, p],
|
||||
[cost_expr])
|
||||
|
||||
cost_fun_jac_hess = ca.Function(
|
||||
fun_name_cost_fun_jac_hess,
|
||||
[x, u, z, yref, p],
|
||||
[cost_expr, outer_loss_grad_fun(inner_expr, p), Jt_ux_expr, Jt_z_expr, outer_hess_fun(inner_expr, p)]
|
||||
)
|
||||
# change directory
|
||||
cwd = os.getcwd()
|
||||
cost_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_cost'))
|
||||
if not os.path.exists(cost_dir):
|
||||
os.makedirs(cost_dir)
|
||||
os.chdir(cost_dir)
|
||||
|
||||
# generate C code
|
||||
cost_fun.generate(fun_name_cost_fun, casadi_codegen_opts)
|
||||
cost_fun_jac_hess.generate(fun_name_cost_fun_jac_hess, casadi_codegen_opts)
|
||||
|
||||
os.chdir(cwd)
|
||||
|
||||
return
|
||||
|
||||
|
||||
################
|
||||
# Constraints
|
||||
################
|
||||
def generate_c_code_constraint( model, con_name, is_terminal, opts ):
|
||||
|
||||
casadi_codegen_opts = dict(mex=False, casadi_int='int', casadi_real='double')
|
||||
|
||||
# load constraint variables and expression
|
||||
x = model.x
|
||||
p = model.p
|
||||
|
||||
symbol = get_casadi_symbol(x)
|
||||
|
||||
if is_terminal:
|
||||
con_h_expr = model.con_h_expr_e
|
||||
con_phi_expr = model.con_phi_expr_e
|
||||
# create dummy u, z
|
||||
u = symbol('u', 0, 0)
|
||||
z = symbol('z', 0, 0)
|
||||
else:
|
||||
con_h_expr = model.con_h_expr
|
||||
con_phi_expr = model.con_phi_expr
|
||||
u = model.u
|
||||
z = model.z
|
||||
|
||||
if (not is_empty(con_h_expr)) and (not is_empty(con_phi_expr)):
|
||||
raise Exception("acados: you can either have constraint_h, or constraint_phi, not both.")
|
||||
|
||||
if (is_empty(con_h_expr) and is_empty(con_phi_expr)):
|
||||
# both empty -> nothing to generate
|
||||
return
|
||||
|
||||
if is_empty(con_h_expr):
|
||||
constr_type = 'BGP'
|
||||
else:
|
||||
constr_type = 'BGH'
|
||||
|
||||
if is_empty(p):
|
||||
p = symbol('p', 0, 0)
|
||||
|
||||
if is_empty(z):
|
||||
z = symbol('z', 0, 0)
|
||||
|
||||
if not (is_empty(con_h_expr)) and opts['generate_hess']:
|
||||
# multipliers for hessian
|
||||
nh = casadi_length(con_h_expr)
|
||||
lam_h = symbol('lam_h', nh, 1)
|
||||
|
||||
# set up & change directory
|
||||
cwd = os.getcwd()
|
||||
constraints_dir = os.path.abspath(os.path.join(opts["code_export_directory"], f'{model.name}_constraints'))
|
||||
if not os.path.exists(constraints_dir):
|
||||
os.makedirs(constraints_dir)
|
||||
os.chdir(constraints_dir)
|
||||
|
||||
# export casadi functions
|
||||
if constr_type == 'BGH':
|
||||
if is_terminal:
|
||||
fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt'
|
||||
else:
|
||||
fun_name = con_name + '_constr_h_fun_jac_uxt_zt'
|
||||
|
||||
jac_ux_t = ca.transpose(ca.jacobian(con_h_expr, ca.vertcat(u,x)))
|
||||
jac_z_t = ca.jacobian(con_h_expr, z)
|
||||
constraint_fun_jac_tran = ca.Function(fun_name, [x, u, z, p], \
|
||||
[con_h_expr, jac_ux_t, jac_z_t])
|
||||
|
||||
constraint_fun_jac_tran.generate(fun_name, casadi_codegen_opts)
|
||||
if opts['generate_hess']:
|
||||
|
||||
if is_terminal:
|
||||
fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt_hess'
|
||||
else:
|
||||
fun_name = con_name + '_constr_h_fun_jac_uxt_zt_hess'
|
||||
|
||||
# adjoint
|
||||
adj_ux = ca.jtimes(con_h_expr, ca.vertcat(u, x), lam_h, True)
|
||||
# hessian
|
||||
hess_ux = ca.jacobian(adj_ux, ca.vertcat(u, x))
|
||||
|
||||
adj_z = ca.jtimes(con_h_expr, z, lam_h, True)
|
||||
hess_z = ca.jacobian(adj_z, z)
|
||||
|
||||
# set up functions
|
||||
constraint_fun_jac_tran_hess = \
|
||||
ca.Function(fun_name, [x, u, lam_h, z, p], \
|
||||
[con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z])
|
||||
|
||||
# generate C code
|
||||
constraint_fun_jac_tran_hess.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
if is_terminal:
|
||||
fun_name = con_name + '_constr_h_e_fun'
|
||||
else:
|
||||
fun_name = con_name + '_constr_h_fun'
|
||||
h_fun = ca.Function(fun_name, [x, u, z, p], [con_h_expr])
|
||||
h_fun.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
else: # BGP constraint
|
||||
if is_terminal:
|
||||
fun_name = con_name + '_phi_e_constraint'
|
||||
r = model.con_r_in_phi_e
|
||||
con_r_expr = model.con_r_expr_e
|
||||
else:
|
||||
fun_name = con_name + '_phi_constraint'
|
||||
r = model.con_r_in_phi
|
||||
con_r_expr = model.con_r_expr
|
||||
|
||||
nphi = casadi_length(con_phi_expr)
|
||||
con_phi_expr_x_u_z = ca.substitute(con_phi_expr, r, con_r_expr)
|
||||
phi_jac_u = ca.jacobian(con_phi_expr_x_u_z, u)
|
||||
phi_jac_x = ca.jacobian(con_phi_expr_x_u_z, x)
|
||||
phi_jac_z = ca.jacobian(con_phi_expr_x_u_z, z)
|
||||
|
||||
hess = ca.hessian(con_phi_expr[0], r)[0]
|
||||
for i in range(1, nphi):
|
||||
hess = ca.vertcat(hess, ca.hessian(con_phi_expr[i], r)[0])
|
||||
|
||||
r_jac_u = ca.jacobian(con_r_expr, u)
|
||||
r_jac_x = ca.jacobian(con_r_expr, x)
|
||||
|
||||
constraint_phi = \
|
||||
ca.Function(fun_name, [x, u, z, p], \
|
||||
[con_phi_expr_x_u_z, \
|
||||
ca.vertcat(ca.transpose(phi_jac_u), ca.transpose(phi_jac_x)), \
|
||||
ca.transpose(phi_jac_z), \
|
||||
hess,
|
||||
ca.vertcat(ca.transpose(r_jac_u), ca.transpose(r_jac_x))])
|
||||
|
||||
constraint_phi.generate(fun_name, casadi_codegen_opts)
|
||||
|
||||
# change directory back
|
||||
os.chdir(cwd)
|
||||
|
||||
return
|
||||
|
||||
819
third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c
vendored
Normal file
819
third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c
vendored
Normal file
@@ -0,0 +1,819 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
// This is a template based custom_update function
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "custom_update_function.h"
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados/utils/mem.h"
|
||||
|
||||
#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
|
||||
#include "blasfeo/include/blasfeo_d_blasfeo_api.h"
|
||||
|
||||
|
||||
typedef struct custom_memory
|
||||
{
|
||||
// covariance matrics
|
||||
struct blasfeo_dmat *uncertainty_matrix_buffer; // shape = (N+1, nx, nx)
|
||||
// covariance matrix of the additive disturbance
|
||||
struct blasfeo_dmat W_mat; // shape = (nw, nw)
|
||||
struct blasfeo_dmat unc_jac_G_mat; // shape = (nx, nw)
|
||||
struct blasfeo_dmat temp_GW_mat; // shape = (nx, nw)
|
||||
struct blasfeo_dmat GWG_mat; // shape = (nx, nx)
|
||||
// sensitivity matrices
|
||||
struct blasfeo_dmat A_mat; // shape = (nx, nx)
|
||||
struct blasfeo_dmat B_mat; // shape = (nx, nu)
|
||||
// matrix in linear constraints
|
||||
struct blasfeo_dmat Cg_mat; // shape = (ng, nx)
|
||||
struct blasfeo_dmat Dg_mat; // shape = (ng, nu)
|
||||
struct blasfeo_dmat Cg_e_mat; // shape = (ng_e, nx)
|
||||
struct blasfeo_dmat dummy_Dgh_e_mat; // shape = (ngh_e_max, nu)
|
||||
// matrix in nonlinear constraints
|
||||
struct blasfeo_dmat Ch_mat; // shape = (nh, nx)
|
||||
struct blasfeo_dmat Dh_mat; // shape = (nh, nu)
|
||||
struct blasfeo_dmat Ch_e_mat; // shape = (nh_e, nx)
|
||||
// feedback gain matrix
|
||||
struct blasfeo_dmat K_mat; // shape = (nu, nx)
|
||||
// AK = A - B@K
|
||||
struct blasfeo_dmat AK_mat; // shape = (nx, nx)
|
||||
// A@P_k
|
||||
struct blasfeo_dmat temp_AP_mat; // shape = (nx, nx)
|
||||
// K@P_k, K@P_k@K^T
|
||||
struct blasfeo_dmat temp_KP_mat; // shape = (nu, nx)
|
||||
struct blasfeo_dmat temp_KPK_mat; // shape = (nu, nu)
|
||||
// C + D @ K, (C + D @ K) @ P_k
|
||||
struct blasfeo_dmat temp_CaDK_mat; // shape = (ngh_me_max, nx)
|
||||
struct blasfeo_dmat temp_CaDKmP_mat; // shape = (ngh_me_max, nx)
|
||||
struct blasfeo_dmat temp_beta_mat; // shape = (ngh_me_max, ngh_me_max)
|
||||
|
||||
double *d_A_mat; // shape = (nx, nx)
|
||||
double *d_B_mat; // shape = (nx, nu)
|
||||
double *d_Cg_mat; // shape = (ng, nx)
|
||||
double *d_Dg_mat; // shape = (ng, nu)
|
||||
double *d_Cg_e_mat; // shape = (ng_e, nx)
|
||||
double *d_Cgh_mat; // shape = (ng+nh, nx)
|
||||
double *d_Dgh_mat; // shape = (ng+nh, nu)
|
||||
double *d_Cgh_e_mat; // shape = (ng_e+nh_e, nx)
|
||||
double *d_state_vec;
|
||||
// upper and lower bounds on state variables
|
||||
double *d_lbx; // shape = (nbx,)
|
||||
double *d_ubx; // shape = (nbx,)
|
||||
double *d_lbx_e; // shape = (nbx_e,)
|
||||
double *d_ubx_e; // shape = (nbx_e,)
|
||||
// tightened upper and lower bounds on state variables
|
||||
double *d_lbx_tightened; // shape = (nbx,)
|
||||
double *d_ubx_tightened; // shape = (nbx,)
|
||||
double *d_lbx_e_tightened; // shape = (nbx_e,)
|
||||
double *d_ubx_e_tightened; // shape = (nbx_e,)
|
||||
// upper and lower bounds on control inputs
|
||||
double *d_lbu; // shape = (nbu,)
|
||||
double *d_ubu; // shape = (nbu,)
|
||||
// tightened upper and lower bounds on control inputs
|
||||
double *d_lbu_tightened; // shape = (nbu,)
|
||||
double *d_ubu_tightened; // shape = (nbu,)
|
||||
// upper and lower bounds on polytopic constraints
|
||||
double *d_lg; // shape = (ng,)
|
||||
double *d_ug; // shape = (ng,)
|
||||
double *d_lg_e; // shape = (ng_e,)
|
||||
double *d_ug_e; // shape = (ng_e,)
|
||||
// tightened lower bounds on polytopic constraints
|
||||
double *d_lg_tightened; // shape = (ng,)
|
||||
double *d_ug_tightened; // shape = (ng,)
|
||||
double *d_lg_e_tightened; // shape = (ng_e,)
|
||||
double *d_ug_e_tightened; // shape = (ng_e,)
|
||||
// upper and lower bounds on nonlinear constraints
|
||||
double *d_lh; // shape = (nh,)
|
||||
double *d_uh; // shape = (nh,)
|
||||
double *d_lh_e; // shape = (nh_e,)
|
||||
double *d_uh_e; // shape = (nh_e,)
|
||||
// tightened upper and lower bounds on nonlinear constraints
|
||||
double *d_lh_tightened; // shape = (nh,)
|
||||
double *d_uh_tightened; // shape = (nh,)
|
||||
double *d_lh_e_tightened; // shape = (nh_e,)
|
||||
double *d_uh_e_tightened; // shape = (nh_e,)
|
||||
|
||||
int *idxbx; // shape = (nbx,)
|
||||
int *idxbu; // shape = (nbu,)
|
||||
int *idxbx_e; // shape = (nbx_e,)
|
||||
|
||||
void *raw_memory; // Pointer to allocated memory, to be used for freeing
|
||||
} custom_memory;
|
||||
|
||||
static int int_max(int num1, int num2)
|
||||
{
|
||||
return (num1 > num2 ) ? num1 : num2;
|
||||
}
|
||||
|
||||
|
||||
static int custom_memory_calculate_size(ocp_nlp_config *nlp_config, ocp_nlp_dims *nlp_dims)
|
||||
{
|
||||
int N = nlp_dims->N;
|
||||
int nx = {{ dims.nx }};
|
||||
int nu = {{ dims.nu }};
|
||||
int nw = {{ zoro_description.nw }};
|
||||
|
||||
int ng = {{ dims.ng }};
|
||||
int nh = {{ dims.nh }};
|
||||
int nbx = {{ dims.nbx }};
|
||||
int nbu = {{ dims.nbu }};
|
||||
|
||||
int ng_e = {{ dims.ng_e }};
|
||||
int nh_e = {{ dims.nh_e }};
|
||||
int ngh_e_max = int_max(ng_e, nh_e);
|
||||
int ngh_me_max = int_max(ngh_e_max, int_max(ng, nh));
|
||||
int nbx_e = {{ dims.nbx_e }};
|
||||
|
||||
assert({{zoro_description.nlbx_t}} <= nbx);
|
||||
assert({{zoro_description.nubx_t}} <= nbx);
|
||||
assert({{zoro_description.nlbu_t}} <= nbu);
|
||||
assert({{zoro_description.nubu_t}} <= nbu);
|
||||
assert({{zoro_description.nlg_t}} <= ng);
|
||||
assert({{zoro_description.nug_t}} <= ng);
|
||||
assert({{zoro_description.nlh_t}} <= nh);
|
||||
assert({{zoro_description.nuh_t}} <= nh);
|
||||
assert({{zoro_description.nlbx_e_t}} <= nbx_e);
|
||||
assert({{zoro_description.nubx_e_t}} <= nbx_e);
|
||||
assert({{zoro_description.nlg_e_t}} <= ng_e);
|
||||
assert({{zoro_description.nug_e_t}} <= ng_e);
|
||||
assert({{zoro_description.nlh_e_t}} <= nh_e);
|
||||
assert({{zoro_description.nuh_e_t}} <= nh_e);
|
||||
|
||||
acados_size_t size = sizeof(custom_memory);
|
||||
size += nbx * sizeof(int);
|
||||
/* blasfeo structs */
|
||||
size += (N + 1) * sizeof(struct blasfeo_dmat);
|
||||
/* blasfeo mem: mat */
|
||||
size += (N + 1) * blasfeo_memsize_dmat(nx, nx); // uncertainty_matrix_buffer
|
||||
size += blasfeo_memsize_dmat(nw, nw); // W_mat
|
||||
size += 2 * blasfeo_memsize_dmat(nx, nw); // unc_jac_G_mat, temp_GW_mat
|
||||
size += 4 * blasfeo_memsize_dmat(nx, nx); // GWG_mat, A_mat, AK_mat, temp_AP_mat
|
||||
size += blasfeo_memsize_dmat(nx, nu); // B_mat
|
||||
size += 2 * blasfeo_memsize_dmat(nu, nx); // K_mat, temp_KP_mat
|
||||
size += blasfeo_memsize_dmat(nu, nu); // temp_KPK_mat
|
||||
size += blasfeo_memsize_dmat(ng, nx); // Cg_mat
|
||||
size += blasfeo_memsize_dmat(ng, nu); // Dg_mat
|
||||
size += blasfeo_memsize_dmat(ng_e, nx); // Cg_e_mat
|
||||
size += blasfeo_memsize_dmat(ngh_e_max, nu); // dummy_Dgh_e_mat
|
||||
size += blasfeo_memsize_dmat(nh, nx); // Ch_mat
|
||||
size += blasfeo_memsize_dmat(nh, nu); // Dh_mat
|
||||
size += blasfeo_memsize_dmat(nh_e, nx); // Ch_e_mat
|
||||
size += 2 * blasfeo_memsize_dmat(ngh_me_max, nx); // temp_CaDK_mat, temp_CaDKmP_mat
|
||||
size += blasfeo_memsize_dmat(ngh_me_max, ngh_me_max); // temp_beta_mat
|
||||
/* blasfeo mem: vec */
|
||||
/* Arrays */
|
||||
size += nx*nx *sizeof(double); // d_A_mat
|
||||
size += nx*nu *sizeof(double); // d_B_mat
|
||||
size += (ng + ng_e) * nx * sizeof(double); // d_Cg_mat, d_Cg_e_mat
|
||||
size += (ng) * nu * sizeof(double); // d_Dg_mat
|
||||
size += (nh + nh_e + ng + ng_e) * nx * sizeof(double); // d_Cgh_mat, d_Cgh_e_mat
|
||||
size += (nh + ng) * nu * sizeof(double); // d_Dgh_mat
|
||||
// d_state_vec
|
||||
size += nx *sizeof(double);
|
||||
// constraints and tightened constraints
|
||||
size += 4 * (nbx + nbu + ng + nh)*sizeof(double);
|
||||
size += 4 * (nbx_e + ng_e + nh_e)*sizeof(double);
|
||||
size += (nbx + nbu + nbx_e)*sizeof(int); // idxbx, idxbu, idxbx_e
|
||||
|
||||
size += 1 * 8; // initial alignment
|
||||
make_int_multiple_of(64, &size);
|
||||
size += 1 * 64;
|
||||
|
||||
return size;
|
||||
}
|
||||
|
||||
|
||||
static custom_memory *custom_memory_assign(ocp_nlp_config *nlp_config, ocp_nlp_dims *nlp_dims, void *raw_memory)
|
||||
{
|
||||
int N = nlp_dims->N;
|
||||
int nx = {{ dims.nx }};
|
||||
int nu = {{ dims.nu }};
|
||||
int nw = {{ zoro_description.nw }};
|
||||
|
||||
int ng = {{ dims.ng }};
|
||||
int nh = {{ dims.nh }};
|
||||
int nbx = {{ dims.nbx }};
|
||||
int nbu = {{ dims.nbu }};
|
||||
|
||||
int ng_e = {{ dims.ng_e }};
|
||||
int nh_e = {{ dims.nh_e }};
|
||||
int ngh_e_max = int_max(ng_e, nh_e);
|
||||
int ngh_me_max = int_max(ngh_e_max, int_max(ng, nh));
|
||||
int nbx_e = {{ dims.nbx_e }};
|
||||
|
||||
char *c_ptr = (char *) raw_memory;
|
||||
custom_memory *mem = (custom_memory *) c_ptr;
|
||||
c_ptr += sizeof(custom_memory);
|
||||
|
||||
align_char_to(8, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_structs(N+1, &mem->uncertainty_matrix_buffer, &c_ptr);
|
||||
|
||||
align_char_to(64, &c_ptr);
|
||||
|
||||
for (int ii = 0; ii <= N; ii++)
|
||||
{
|
||||
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->uncertainty_matrix_buffer[ii], &c_ptr);
|
||||
}
|
||||
// Disturbance Dynamics
|
||||
assign_and_advance_blasfeo_dmat_mem(nw, nw, &mem->W_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(nx, nw, &mem->unc_jac_G_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(nx, nw, &mem->temp_GW_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->GWG_mat, &c_ptr);
|
||||
// System Dynamics
|
||||
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->A_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(nx, nu, &mem->B_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(ng, nx, &mem->Cg_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(ng, nu, &mem->Dg_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(ng_e, nx, &mem->Cg_e_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(ngh_e_max, nu, &mem->dummy_Dgh_e_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(nh, nx, &mem->Ch_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(nh, nu, &mem->Dh_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(nh_e, nx, &mem->Ch_e_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(nu, nx, &mem->K_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->AK_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(nx, nx, &mem->temp_AP_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(nu, nx, &mem->temp_KP_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(nu, nu, &mem->temp_KPK_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(ngh_me_max, nx, &mem->temp_CaDK_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(ngh_me_max, nx, &mem->temp_CaDKmP_mat, &c_ptr);
|
||||
assign_and_advance_blasfeo_dmat_mem(ngh_me_max, ngh_me_max, &mem->temp_beta_mat, &c_ptr);
|
||||
|
||||
assign_and_advance_double(nx*nx, &mem->d_A_mat, &c_ptr);
|
||||
assign_and_advance_double(nx*nu, &mem->d_B_mat, &c_ptr);
|
||||
assign_and_advance_double(ng*nx, &mem->d_Cg_mat, &c_ptr);
|
||||
assign_and_advance_double(ng*nu, &mem->d_Dg_mat, &c_ptr);
|
||||
assign_and_advance_double(ng_e*nx, &mem->d_Cg_e_mat, &c_ptr);
|
||||
assign_and_advance_double((ng + nh)*nx, &mem->d_Cgh_mat, &c_ptr);
|
||||
assign_and_advance_double((ng + nh)*nu, &mem->d_Dgh_mat, &c_ptr);
|
||||
assign_and_advance_double((ng_e + nh_e)*nx, &mem->d_Cgh_e_mat, &c_ptr);
|
||||
assign_and_advance_double(nx, &mem->d_state_vec, &c_ptr);
|
||||
assign_and_advance_double(nbx, &mem->d_lbx, &c_ptr);
|
||||
assign_and_advance_double(nbx, &mem->d_ubx, &c_ptr);
|
||||
assign_and_advance_double(nbx_e, &mem->d_lbx_e, &c_ptr);
|
||||
assign_and_advance_double(nbx_e, &mem->d_ubx_e, &c_ptr);
|
||||
assign_and_advance_double(nbx, &mem->d_lbx_tightened, &c_ptr);
|
||||
assign_and_advance_double(nbx, &mem->d_ubx_tightened, &c_ptr);
|
||||
assign_and_advance_double(nbx_e, &mem->d_lbx_e_tightened, &c_ptr);
|
||||
assign_and_advance_double(nbx_e, &mem->d_ubx_e_tightened, &c_ptr);
|
||||
assign_and_advance_double(nbu, &mem->d_lbu, &c_ptr);
|
||||
assign_and_advance_double(nbu, &mem->d_ubu, &c_ptr);
|
||||
assign_and_advance_double(nbu, &mem->d_lbu_tightened, &c_ptr);
|
||||
assign_and_advance_double(nbu, &mem->d_ubu_tightened, &c_ptr);
|
||||
assign_and_advance_double(ng, &mem->d_lg, &c_ptr);
|
||||
assign_and_advance_double(ng, &mem->d_ug, &c_ptr);
|
||||
assign_and_advance_double(ng_e, &mem->d_lg_e, &c_ptr);
|
||||
assign_and_advance_double(ng_e, &mem->d_ug_e, &c_ptr);
|
||||
assign_and_advance_double(ng, &mem->d_lg_tightened, &c_ptr);
|
||||
assign_and_advance_double(ng, &mem->d_ug_tightened, &c_ptr);
|
||||
assign_and_advance_double(ng_e, &mem->d_lg_e_tightened, &c_ptr);
|
||||
assign_and_advance_double(ng_e, &mem->d_ug_e_tightened, &c_ptr);
|
||||
assign_and_advance_double(nh, &mem->d_lh, &c_ptr);
|
||||
assign_and_advance_double(nh, &mem->d_uh, &c_ptr);
|
||||
assign_and_advance_double(nh_e, &mem->d_lh_e, &c_ptr);
|
||||
assign_and_advance_double(nh_e, &mem->d_uh_e, &c_ptr);
|
||||
assign_and_advance_double(nh, &mem->d_lh_tightened, &c_ptr);
|
||||
assign_and_advance_double(nh, &mem->d_uh_tightened, &c_ptr);
|
||||
assign_and_advance_double(nh_e, &mem->d_lh_e_tightened, &c_ptr);
|
||||
assign_and_advance_double(nh_e, &mem->d_uh_e_tightened, &c_ptr);
|
||||
|
||||
assign_and_advance_int(nbx, &mem->idxbx, &c_ptr);
|
||||
assign_and_advance_int(nbu, &mem->idxbu, &c_ptr);
|
||||
assign_and_advance_int(nbx_e, &mem->idxbx_e, &c_ptr);
|
||||
|
||||
assert((char *) raw_memory + custom_memory_calculate_size(nlp_config, nlp_dims) >= c_ptr);
|
||||
mem->raw_memory = raw_memory;
|
||||
|
||||
return mem;
|
||||
}
|
||||
|
||||
|
||||
|
||||
static void *custom_memory_create({{ model.name }}_solver_capsule* capsule)
|
||||
{
|
||||
printf("\nin custom_memory_create_function\n");
|
||||
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
|
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule);
|
||||
acados_size_t bytes = custom_memory_calculate_size(nlp_config, nlp_dims);
|
||||
|
||||
void *ptr = acados_calloc(1, bytes);
|
||||
|
||||
custom_memory *custom_mem = custom_memory_assign(nlp_config, nlp_dims, ptr);
|
||||
custom_mem->raw_memory = ptr;
|
||||
|
||||
return custom_mem;
|
||||
}
|
||||
|
||||
|
||||
static void custom_val_init_function(ocp_nlp_dims *nlp_dims, ocp_nlp_in *nlp_in, ocp_nlp_solver *nlp_solver, custom_memory *custom_mem)
|
||||
{
|
||||
int N = nlp_dims->N;
|
||||
int nx = {{ dims.nx }};
|
||||
int nu = {{ dims.nu }};
|
||||
int nw = {{ zoro_description.nw }};
|
||||
|
||||
int ng = {{ dims.ng }};
|
||||
int nh = {{ dims.nh }};
|
||||
int nbx = {{ dims.nbx }};
|
||||
int nbu = {{ dims.nbu }};
|
||||
|
||||
int ng_e = {{ dims.ng_e }};
|
||||
int nh_e = {{ dims.nh_e }};
|
||||
int ngh_e_max = int_max(ng_e, nh_e);
|
||||
int nbx_e = {{ dims.nbx_e }};
|
||||
|
||||
/* Get the state constraint bounds */
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "idxbx", custom_mem->idxbx);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "idxbx", custom_mem->idxbx_e);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbx", custom_mem->d_lbx);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubx", custom_mem->d_ubx);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "idxbu", custom_mem->idxbu);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbu", custom_mem->d_lbu);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubu", custom_mem->d_ubu);
|
||||
// Get the Jacobians and the bounds of the linear constraints
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lg", custom_mem->d_lg);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ug", custom_mem->d_ug);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "C", custom_mem->d_Cg_mat);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "D", custom_mem->d_Dg_mat);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "C", custom_mem->d_Cg_e_mat);
|
||||
blasfeo_pack_dmat(ng, nx, custom_mem->d_Cg_mat, ng, &custom_mem->Cg_mat, 0, 0);
|
||||
blasfeo_pack_dmat(ng, nu, custom_mem->d_Dg_mat, ng, &custom_mem->Dg_mat, 0, 0);
|
||||
blasfeo_pack_dmat(ng_e, nx, custom_mem->d_Cg_e_mat, ng_e, &custom_mem->Cg_e_mat, 0, 0);
|
||||
blasfeo_dgese(ngh_e_max, nu, 0., &custom_mem->dummy_Dgh_e_mat, 0, 0); //fill with zeros
|
||||
// NOTE: fixed lower and upper bounds of nonlinear constraints
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lh", custom_mem->d_lh);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "uh", custom_mem->d_uh);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e);
|
||||
|
||||
/* Initilize tightened constraints*/
|
||||
// NOTE: tightened constraints are only initialized once
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbx", custom_mem->d_lbx_tightened);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubx", custom_mem->d_ubx_tightened);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e_tightened);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e_tightened);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lbu", custom_mem->d_lbu_tightened);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ubu", custom_mem->d_ubu_tightened);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lg", custom_mem->d_lg_tightened);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "ug", custom_mem->d_ug_tightened);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e_tightened);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e_tightened);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "lh", custom_mem->d_lh_tightened);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, 1, "uh", custom_mem->d_uh_tightened);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e_tightened);
|
||||
ocp_nlp_constraints_model_get(nlp_solver->config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e_tightened);
|
||||
|
||||
/* Initialize the W matrix */
|
||||
// blasfeo_dgese(nw, nw, 0., &custom_mem->W_mat, 0, 0);
|
||||
{%- for ir in range(end=zoro_description.nw) %}
|
||||
{%- for ic in range(end=zoro_description.nw) %}
|
||||
blasfeo_dgein1({{zoro_description.W_mat[ir][ic]}}, &custom_mem->W_mat, {{ir}}, {{ic}});
|
||||
{%- endfor %}
|
||||
{%- endfor %}
|
||||
|
||||
{%- for ir in range(end=dims.nx) %}
|
||||
{%- for ic in range(end=zoro_description.nw) %}
|
||||
blasfeo_dgein1({{zoro_description.unc_jac_G_mat[ir][ic]}}, &custom_mem->unc_jac_G_mat, {{ir}}, {{ic}});
|
||||
{%- endfor %}
|
||||
{%- endfor %}
|
||||
|
||||
// NOTE: if G is changing this is not in init!
|
||||
// temp_GW_mat = unc_jac_G_mat * W_mat
|
||||
blasfeo_dgemm_nn(nx, nw, nw, 1.0, &custom_mem->unc_jac_G_mat, 0, 0,
|
||||
&custom_mem->W_mat, 0, 0, 0.0,
|
||||
&custom_mem->temp_GW_mat, 0, 0, &custom_mem->temp_GW_mat, 0, 0);
|
||||
// GWG_mat = temp_GW_mat * unc_jac_G_mat^T
|
||||
blasfeo_dgemm_nt(nx, nx, nw, 1.0, &custom_mem->temp_GW_mat, 0, 0,
|
||||
&custom_mem->unc_jac_G_mat, 0, 0, 0.0,
|
||||
&custom_mem->GWG_mat, 0, 0, &custom_mem->GWG_mat, 0, 0);
|
||||
|
||||
/* Initialize the uncertainty_matrix_buffer[0] */
|
||||
{%- for ir in range(end=dims.nx) %}
|
||||
{%- for ic in range(end=dims.nx) %}
|
||||
blasfeo_dgein1({{zoro_description.P0_mat[ir][ic]}}, &custom_mem->uncertainty_matrix_buffer[0], {{ir}}, {{ic}});
|
||||
{%- endfor %}
|
||||
{%- endfor %}
|
||||
|
||||
/* Initialize the feedback gain matrix */
|
||||
{%- for ir in range(end=dims.nu) %}
|
||||
{%- for ic in range(end=dims.nx) %}
|
||||
blasfeo_dgein1({{zoro_description.fdbk_K_mat[ir][ic]}}, &custom_mem->K_mat, {{ir}}, {{ic}});
|
||||
{%- endfor %}
|
||||
{%- endfor %}
|
||||
}
|
||||
|
||||
|
||||
int custom_update_init_function({{ model.name }}_solver_capsule* capsule)
|
||||
{
|
||||
capsule->custom_update_memory = custom_memory_create(capsule);
|
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule);
|
||||
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
|
||||
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(capsule);
|
||||
custom_val_init_function(nlp_dims, nlp_in, nlp_solver, capsule->custom_update_memory);
|
||||
return 1;
|
||||
}
|
||||
|
||||
static void compute_gh_beta(struct blasfeo_dmat* K_mat, struct blasfeo_dmat* C_mat,
|
||||
struct blasfeo_dmat* D_mat, struct blasfeo_dmat* CaDK_mat,
|
||||
struct blasfeo_dmat* CaDKmP_mat, struct blasfeo_dmat* beta_mat,
|
||||
struct blasfeo_dmat* P_mat,
|
||||
int n_cstr, int nx, int nu)
|
||||
{
|
||||
// (C+DK)@P@(C^T+K^TD^T)
|
||||
// CaDK_mat = C_mat + D_mat @ K_mat
|
||||
blasfeo_dgemm_nn(n_cstr, nx, nu, 1.0, D_mat, 0, 0,
|
||||
K_mat, 0, 0, 1.0,
|
||||
C_mat, 0, 0, CaDK_mat, 0, 0);
|
||||
// CaDKmP_mat = CaDK_mat @ P_mat
|
||||
blasfeo_dgemm_nn(n_cstr, nx, nx, 1.0, CaDK_mat, 0, 0,
|
||||
P_mat, 0, 0, 0.0,
|
||||
CaDKmP_mat, 0, 0, CaDKmP_mat, 0, 0);
|
||||
// beta_mat = CaDKmP_mat @ CaDK_mat^T
|
||||
blasfeo_dgemm_nt(n_cstr, n_cstr, nx, 1.0, CaDKmP_mat, 0, 0,
|
||||
CaDK_mat, 0, 0, 0.0,
|
||||
beta_mat, 0, 0, beta_mat, 0, 0);
|
||||
}
|
||||
|
||||
static void compute_KPK(struct blasfeo_dmat* K_mat, struct blasfeo_dmat* temp_KP_mat,
|
||||
struct blasfeo_dmat* temp_KPK_mat, struct blasfeo_dmat* P_mat,
|
||||
int nx, int nu)
|
||||
{
|
||||
// K @ P_k @ K^T
|
||||
// temp_KP_mat = K_mat @ P_mat
|
||||
blasfeo_dgemm_nn(nu, nx, nx, 1.0, K_mat, 0, 0,
|
||||
P_mat, 0, 0, 0.0,
|
||||
temp_KP_mat, 0, 0, temp_KP_mat, 0, 0);
|
||||
// temp_KPK_mat = temp_KP_mat @ K_mat^T
|
||||
blasfeo_dgemm_nt(nu, nu, nx, 1.0, temp_KP_mat, 0, 0,
|
||||
K_mat, 0, 0, 0.0,
|
||||
temp_KPK_mat, 0, 0, temp_KPK_mat, 0, 0);
|
||||
}
|
||||
|
||||
static void compute_next_P_matrix(struct blasfeo_dmat* P_mat, struct blasfeo_dmat* P_next_mat,
|
||||
struct blasfeo_dmat* A_mat, struct blasfeo_dmat* B_mat,
|
||||
struct blasfeo_dmat* K_mat, struct blasfeo_dmat* W_mat,
|
||||
struct blasfeo_dmat* AK_mat, struct blasfeo_dmat* temp_AP_mat, int nx, int nu)
|
||||
{
|
||||
// AK_mat = -B@K + A
|
||||
blasfeo_dgemm_nn(nx, nx, nu, -1.0, B_mat, 0, 0, K_mat, 0, 0,
|
||||
1.0, A_mat, 0, 0, AK_mat, 0, 0);
|
||||
// temp_AP_mat = AK_mat @ P_k
|
||||
blasfeo_dgemm_nn(nx, nx, nx, 1.0, AK_mat, 0, 0,
|
||||
P_mat, 0, 0, 0.0,
|
||||
temp_AP_mat, 0, 0, temp_AP_mat, 0, 0);
|
||||
// P_{k+1} = temp_AP_mat @ AK_mat^T + GWG_mat
|
||||
blasfeo_dgemm_nt(nx, nx, nx, 1.0, temp_AP_mat, 0, 0,
|
||||
AK_mat, 0, 0, 1.0,
|
||||
W_mat, 0, 0, P_next_mat, 0, 0);
|
||||
}
|
||||
|
||||
static void reset_P0_matrix(ocp_nlp_dims *nlp_dims, struct blasfeo_dmat* P_mat, double* data)
|
||||
{
|
||||
int nx = nlp_dims->nx[0];
|
||||
blasfeo_pack_dmat(nx, nx, data, nx, P_mat, 0, 0);
|
||||
}
|
||||
|
||||
static void uncertainty_propagate_and_update(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out, custom_memory *custom_mem)
|
||||
{
|
||||
ocp_nlp_config *nlp_config = solver->config;
|
||||
ocp_nlp_dims *nlp_dims = solver->dims;
|
||||
|
||||
int N = nlp_dims->N;
|
||||
int nx = nlp_dims->nx[0];
|
||||
int nu = nlp_dims->nu[0];
|
||||
int nx_sqr = nx*nx;
|
||||
int nbx = {{ dims.nbx }};
|
||||
int nbu = {{ dims.nbu }};
|
||||
int ng = {{ dims.ng }};
|
||||
int nh = {{ dims.nh }};
|
||||
int ng_e = {{ dims.ng_e }};
|
||||
int nh_e = {{ dims.nh_e }};
|
||||
int nbx_e = {{ dims.nbx_e }};
|
||||
double backoff_scaling_gamma = {{ zoro_description.backoff_scaling_gamma }};
|
||||
|
||||
// First Stage
|
||||
// NOTE: lbx_0 and ubx_0 should not be tightened.
|
||||
// NOTE: lg_0 and ug_0 are not tightened.
|
||||
// NOTE: lh_0 and uh_0 are not tightened.
|
||||
{%- if zoro_description.nlbu_t + zoro_description.nubu_t > 0 %}
|
||||
compute_KPK(&custom_mem->K_mat, &custom_mem->temp_KP_mat,
|
||||
&custom_mem->temp_KPK_mat, &(custom_mem->uncertainty_matrix_buffer[0]), nx, nu);
|
||||
|
||||
{%- if zoro_description.nlbu_t > 0 %}
|
||||
// backoff lbu
|
||||
{%- for it in zoro_description.idx_lbu_t %}
|
||||
custom_mem->d_lbu_tightened[{{it}}]
|
||||
= custom_mem->d_lbu[{{it}}]
|
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat,
|
||||
custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}]));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbu", custom_mem->d_lbu_tightened);
|
||||
{%- endif %}
|
||||
{%- if zoro_description.nubu_t > 0 %}
|
||||
// backoff ubu
|
||||
{%- for it in zoro_description.idx_ubu_t %}
|
||||
custom_mem->d_ubu_tightened[{{it}}]
|
||||
= custom_mem->d_ubu[{{it}}]
|
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat,
|
||||
custom_mem->idxbu[{{it}}],custom_mem->idxbu[{{it}}]));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubu", custom_mem->d_ubu_tightened);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
// Middle Stages
|
||||
// constraint tightening: for next stage based on dynamics of ii stage
|
||||
// P[ii+1] = (A-B@K) @ P[ii] @ (A-B@K).T + G@W@G.T
|
||||
for (int ii = 0; ii < N-1; ii++)
|
||||
{
|
||||
// get and pack: A, B
|
||||
ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, ii, "A", custom_mem->d_A_mat);
|
||||
blasfeo_pack_dmat(nx, nx, custom_mem->d_A_mat, nx, &custom_mem->A_mat, 0, 0);
|
||||
ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, ii, "B", custom_mem->d_B_mat);
|
||||
blasfeo_pack_dmat(nx, nu, custom_mem->d_B_mat, nx, &custom_mem->B_mat, 0, 0);
|
||||
|
||||
compute_next_P_matrix(&(custom_mem->uncertainty_matrix_buffer[ii]),
|
||||
&(custom_mem->uncertainty_matrix_buffer[ii+1]),
|
||||
&custom_mem->A_mat, &custom_mem->B_mat,
|
||||
&custom_mem->K_mat, &custom_mem->GWG_mat,
|
||||
&custom_mem->AK_mat, &custom_mem->temp_AP_mat, nx, nu);
|
||||
|
||||
// state constraints
|
||||
{%- if zoro_description.nlbx_t + zoro_description.nubx_t> 0 %}
|
||||
{%- if zoro_description.nlbx_t > 0 %}
|
||||
// lbx
|
||||
{%- for it in zoro_description.idx_lbx_t %}
|
||||
custom_mem->d_lbx_tightened[{{it}}]
|
||||
= custom_mem->d_lbx[{{it}}]
|
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1],
|
||||
custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}]));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbx", custom_mem->d_lbx_tightened);
|
||||
{%- endif %}
|
||||
{% if zoro_description.nubx_t > 0 %}
|
||||
// ubx
|
||||
{%- for it in zoro_description.idx_ubx_t %}
|
||||
custom_mem->d_ubx_tightened[{{it}}] = custom_mem->d_ubx[{{it}}]
|
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[ii+1],
|
||||
custom_mem->idxbx[{{it}}],custom_mem->idxbx[{{it}}]));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubx", custom_mem->d_ubx_tightened);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if zoro_description.nlbu_t + zoro_description.nubu_t > 0 %}
|
||||
// input constraints
|
||||
compute_KPK(&custom_mem->K_mat, &custom_mem->temp_KP_mat,
|
||||
&custom_mem->temp_KPK_mat, &(custom_mem->uncertainty_matrix_buffer[ii+1]), nx, nu);
|
||||
|
||||
{%- if zoro_description.nlbu_t > 0 %}
|
||||
{%- for it in zoro_description.idx_lbu_t %}
|
||||
custom_mem->d_lbu_tightened[{{it}}] = custom_mem->d_lbu[{{it}}]
|
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat,
|
||||
custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}]));
|
||||
{%- endfor %}
|
||||
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lbu", custom_mem->d_lbu_tightened);
|
||||
{%- endif %}
|
||||
{%- if zoro_description.nubu_t > 0 %}
|
||||
{%- for it in zoro_description.idx_ubu_t %}
|
||||
custom_mem->d_ubu_tightened[{{it}}] = custom_mem->d_ubu[{{it}}]
|
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_KPK_mat,
|
||||
custom_mem->idxbu[{{it}}], custom_mem->idxbu[{{it}}]));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ubu", custom_mem->d_ubu_tightened);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if zoro_description.nlg_t + zoro_description.nug_t > 0 %}
|
||||
// Linear constraints: g
|
||||
compute_gh_beta(&custom_mem->K_mat, &custom_mem->Cg_mat,
|
||||
&custom_mem->Dg_mat, &custom_mem->temp_CaDK_mat,
|
||||
&custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat,
|
||||
&custom_mem->uncertainty_matrix_buffer[ii+1], ng, nx, nu);
|
||||
|
||||
{%- if zoro_description.nlg_t > 0 %}
|
||||
{%- for it in zoro_description.idx_lg_t %}
|
||||
custom_mem->d_lg_tightened[{{it}}]
|
||||
= custom_mem->d_lg[{{it}}]
|
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lg", custom_mem->d_lg_tightened);
|
||||
{%- endif %}
|
||||
{%- if zoro_description.nug_t > 0 %}
|
||||
{%- for it in zoro_description.idx_ug_t %}
|
||||
custom_mem->d_ug_tightened[{{it}}]
|
||||
= custom_mem->d_ug[{{it}}]
|
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "ug", custom_mem->d_ug_tightened);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
|
||||
{%- if zoro_description.nlh_t + zoro_description.nuh_t > 0 %}
|
||||
// nonlinear constraints: h
|
||||
// Get C_{k+1} and D_{k+1}
|
||||
ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, ii+1, "C", custom_mem->d_Cgh_mat);
|
||||
ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, ii+1, "D", custom_mem->d_Dgh_mat);
|
||||
// NOTE: the d_Cgh_mat is column-major, the first ng rows are the Jacobians of the linear constraints
|
||||
blasfeo_pack_dmat(nh, nx, custom_mem->d_Cgh_mat+ng, ng+nh, &custom_mem->Ch_mat, 0, 0);
|
||||
blasfeo_pack_dmat(nh, nu, custom_mem->d_Dgh_mat+ng, ng+nh, &custom_mem->Dh_mat, 0, 0);
|
||||
|
||||
compute_gh_beta(&custom_mem->K_mat, &custom_mem->Ch_mat,
|
||||
&custom_mem->Dh_mat, &custom_mem->temp_CaDK_mat,
|
||||
&custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat,
|
||||
&custom_mem->uncertainty_matrix_buffer[ii+1], nh, nx, nu);
|
||||
|
||||
{%- if zoro_description.nlh_t > 0 %}
|
||||
{%- for it in zoro_description.idx_lh_t %}
|
||||
custom_mem->d_lh_tightened[{{it}}]
|
||||
= custom_mem->d_lh[{{it}}]
|
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "lh", custom_mem->d_lh_tightened);
|
||||
{%- endif %}
|
||||
{%- if zoro_description.nuh_t > 0 %}
|
||||
{%- for it in zoro_description.idx_uh_t %}
|
||||
custom_mem->d_uh_tightened[{{it}}] = custom_mem->d_uh[{{it}}]
|
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii+1, "uh", custom_mem->d_uh_tightened);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
}
|
||||
|
||||
// Last stage
|
||||
// get and pack: A, B
|
||||
ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, N-1, "A", custom_mem->d_A_mat);
|
||||
blasfeo_pack_dmat(nx, nx, custom_mem->d_A_mat, nx, &custom_mem->A_mat, 0, 0);
|
||||
ocp_nlp_get_at_stage(nlp_config, nlp_dims, solver, N-1, "B", custom_mem->d_B_mat);
|
||||
blasfeo_pack_dmat(nx, nu, custom_mem->d_B_mat, nx, &custom_mem->B_mat, 0, 0);
|
||||
// AK_mat = -B*K + A
|
||||
compute_next_P_matrix(&(custom_mem->uncertainty_matrix_buffer[N-1]),
|
||||
&(custom_mem->uncertainty_matrix_buffer[N]),
|
||||
&custom_mem->A_mat, &custom_mem->B_mat,
|
||||
&custom_mem->K_mat, &custom_mem->GWG_mat,
|
||||
&custom_mem->AK_mat, &custom_mem->temp_AP_mat, nx, nu);
|
||||
|
||||
// state constraints nlbx_e_t
|
||||
{%- if zoro_description.nlbx_e_t + zoro_description.nubx_e_t> 0 %}
|
||||
{%- if zoro_description.nlbx_e_t > 0 %}
|
||||
// lbx_e
|
||||
{%- for it in zoro_description.idx_lbx_e_t %}
|
||||
custom_mem->d_lbx_e_tightened[{{it}}]
|
||||
= custom_mem->d_lbx_e[{{it}}]
|
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N],
|
||||
custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}]));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", custom_mem->d_lbx_e_tightened);
|
||||
{%- endif %}
|
||||
{% if zoro_description.nubx_e_t > 0 %}
|
||||
// ubx_e
|
||||
{%- for it in zoro_description.idx_ubx_e_t %}
|
||||
custom_mem->d_ubx_e_tightened[{{it}}] = custom_mem->d_ubx_e[{{it}}]
|
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->uncertainty_matrix_buffer[N],
|
||||
custom_mem->idxbx_e[{{it}}],custom_mem->idxbx_e[{{it}}]));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", custom_mem->d_ubx_e_tightened);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if zoro_description.nlg_e_t + zoro_description.nug_e_t > 0 %}
|
||||
// Linear constraints: g
|
||||
compute_gh_beta(&custom_mem->K_mat, &custom_mem->Cg_mat,
|
||||
&custom_mem->dummy_Dgh_e_mat, &custom_mem->temp_CaDK_mat,
|
||||
&custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat,
|
||||
&custom_mem->uncertainty_matrix_buffer[N], ng, nx, nu);
|
||||
|
||||
{%- if zoro_description.nlg_e_t > 0 %}
|
||||
{%- for it in zoro_description.idx_lg_e_t %}
|
||||
custom_mem->d_lg_e_tightened[{{it}}]
|
||||
= custom_mem->d_lg_e[{{it}}]
|
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", custom_mem->d_lg_e_tightened);
|
||||
{%- endif %}
|
||||
{%- if zoro_description.nug_e_t > 0 %}
|
||||
{%- for it in zoro_description.idx_ug_e_t %}
|
||||
custom_mem->d_ug_e_tightened[{{it}}]
|
||||
= custom_mem->d_ug_e[{{it}}]
|
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", custom_mem->d_ug_e_tightened);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
|
||||
{%- if zoro_description.nlh_e_t + zoro_description.nuh_e_t > 0 %}
|
||||
// nonlinear constraints: h
|
||||
// Get C_{k+1} and D_{k+1}
|
||||
ocp_nlp_get_at_stage(solver->config, nlp_dims, solver, N, "C", custom_mem->d_Cgh_mat);
|
||||
// NOTE: the d_Cgh_mat is column-major, the first ng rows are the Jacobians of the linear constraints
|
||||
blasfeo_pack_dmat(nh, nx, custom_mem->d_Cgh_mat+ng, ng+nh, &custom_mem->Ch_mat, 0, 0);
|
||||
|
||||
compute_gh_beta(&custom_mem->K_mat, &custom_mem->Ch_mat,
|
||||
&custom_mem->dummy_Dgh_e_mat, &custom_mem->temp_CaDK_mat,
|
||||
&custom_mem->temp_CaDKmP_mat, &custom_mem->temp_beta_mat,
|
||||
&custom_mem->uncertainty_matrix_buffer[N], nh, nx, nu);
|
||||
|
||||
{%- if zoro_description.nlh_e_t > 0 %}
|
||||
{%- for it in zoro_description.idx_lh_e_t %}
|
||||
custom_mem->d_lh_e_tightened[{{it}}]
|
||||
= custom_mem->d_lh_e[{{it}}]
|
||||
+ backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", custom_mem->d_lh_e_tightened);
|
||||
{%- endif %}
|
||||
{%- if zoro_description.nuh_e_t > 0 %}
|
||||
{%- for it in zoro_description.idx_uh_e_t %}
|
||||
custom_mem->d_uh_e_tightened[{{it}}] = custom_mem->d_uh_e[{{it}}]
|
||||
- backoff_scaling_gamma * sqrt(blasfeo_dgeex1(&custom_mem->temp_beta_mat, {{it}}, {{it}}));
|
||||
{%- endfor %}
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", custom_mem->d_uh_e_tightened);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
}
|
||||
|
||||
|
||||
int custom_update_function({{ model.name }}_solver_capsule* capsule, double* data, int data_len)
|
||||
{
|
||||
custom_memory *custom_mem = (custom_memory *) capsule->custom_update_memory;
|
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule);
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
|
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule);
|
||||
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(capsule);
|
||||
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(capsule);
|
||||
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(capsule);
|
||||
|
||||
if (data_len > 0)
|
||||
{
|
||||
reset_P0_matrix(nlp_dims, &custom_mem->uncertainty_matrix_buffer[0], data);
|
||||
}
|
||||
uncertainty_propagate_and_update(nlp_solver, nlp_in, nlp_out, custom_mem);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
int custom_update_terminate_function({{ model.name }}_solver_capsule* capsule)
|
||||
{
|
||||
custom_memory *mem = capsule->custom_update_memory;
|
||||
|
||||
free(mem->raw_memory);
|
||||
return 1;
|
||||
}
|
||||
|
||||
// useful prints for debugging
|
||||
|
||||
/*
|
||||
printf("A_mat:\n");
|
||||
blasfeo_print_exp_dmat(nx, nx, &custom_mem->A_mat, 0, 0);
|
||||
printf("B_mat:\n");
|
||||
blasfeo_print_exp_dmat(nx, nu, &custom_mem->B_mat, 0, 0);
|
||||
printf("K_mat:\n");
|
||||
blasfeo_print_exp_dmat(nu, nx, &custom_mem->K_mat, 0, 0);
|
||||
printf("AK_mat:\n");
|
||||
blasfeo_print_exp_dmat(nx, nx, &custom_mem->AK_mat, 0, 0);
|
||||
printf("temp_AP_mat:\n");
|
||||
blasfeo_print_exp_dmat(nx, nx, &custom_mem->temp_AP_mat, 0, 0);
|
||||
printf("W_mat:\n");
|
||||
blasfeo_print_exp_dmat(nx, nx, &custom_mem->W_mat, 0, 0);
|
||||
printf("P_k+1:\n");
|
||||
blasfeo_print_exp_dmat(nx, nx, &(custom_mem->uncertainty_matrix_buffer[ii+1]), 0, 0);*/
|
||||
@@ -0,0 +1,44 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
|
||||
// Called at the end of solver creation.
|
||||
// This is allowed to allocate memory and store the pointer to it into capsule->custom_update_memory.
|
||||
int custom_update_init_function({{ model.name }}_solver_capsule* capsule);
|
||||
|
||||
|
||||
// Custom update function that can be called between solver calls
|
||||
int custom_update_function({{ model.name }}_solver_capsule* capsule, double* data, int data_len);
|
||||
|
||||
|
||||
// Called just before destroying the solver.
|
||||
// Responsible to free allocated memory, stored at capsule->custom_update_memory.
|
||||
int custom_update_terminate_function({{ model.name }}_solver_capsule* capsule);
|
||||
1
third_party/acados/acados_template/gnsf/__init__.py
vendored
Normal file
1
third_party/acados/acados_template/gnsf/__init__.py
vendored
Normal file
@@ -0,0 +1 @@
|
||||
|
||||
216
third_party/acados/acados_template/gnsf/check_reformulation.py
vendored
Normal file
216
third_party/acados/acados_template/gnsf/check_reformulation.py
vendored
Normal file
@@ -0,0 +1,216 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
from acados_template.utils import casadi_length
|
||||
from casadi import *
|
||||
import numpy as np
|
||||
|
||||
|
||||
def check_reformulation(model, gnsf, print_info):
|
||||
|
||||
## Description:
|
||||
# this function takes the implicit ODE/ index-1 DAE and a gnsf structure
|
||||
# to evaluate both models at num_eval random points x0, x0dot, z0, u0
|
||||
# if for all points the relative error is <= TOL, the function will return::
|
||||
# 1, otherwise it will give an error.
|
||||
|
||||
TOL = 1e-14
|
||||
num_eval = 10
|
||||
|
||||
# get dimensions
|
||||
nx = gnsf["nx"]
|
||||
nu = gnsf["nu"]
|
||||
nz = gnsf["nz"]
|
||||
nx1 = gnsf["nx1"]
|
||||
nx2 = gnsf["nx2"]
|
||||
nz1 = gnsf["nz1"]
|
||||
nz2 = gnsf["nz2"]
|
||||
n_out = gnsf["n_out"]
|
||||
|
||||
# get model matrices
|
||||
A = gnsf["A"]
|
||||
B = gnsf["B"]
|
||||
C = gnsf["C"]
|
||||
E = gnsf["E"]
|
||||
c = gnsf["c"]
|
||||
|
||||
L_x = gnsf["L_x"]
|
||||
L_xdot = gnsf["L_xdot"]
|
||||
L_z = gnsf["L_z"]
|
||||
L_u = gnsf["L_u"]
|
||||
|
||||
A_LO = gnsf["A_LO"]
|
||||
E_LO = gnsf["E_LO"]
|
||||
B_LO = gnsf["B_LO"]
|
||||
c_LO = gnsf["c_LO"]
|
||||
|
||||
I_x1 = range(nx1)
|
||||
I_x2 = range(nx1, nx)
|
||||
|
||||
I_z1 = range(nz1)
|
||||
I_z2 = range(nz1, nz)
|
||||
|
||||
idx_perm_f = gnsf["idx_perm_f"]
|
||||
|
||||
# get casadi variables
|
||||
x = gnsf["x"]
|
||||
xdot = gnsf["xdot"]
|
||||
z = gnsf["z"]
|
||||
u = gnsf["u"]
|
||||
y = gnsf["y"]
|
||||
uhat = gnsf["uhat"]
|
||||
p = gnsf["p"]
|
||||
|
||||
# create functions
|
||||
impl_dae_fun = Function("impl_dae_fun", [x, xdot, u, z, p], [model.f_impl_expr])
|
||||
phi_fun = Function("phi_fun", [y, uhat, p], [gnsf["phi_expr"]])
|
||||
f_lo_fun = Function(
|
||||
"f_lo_fun", [x[range(nx1)], xdot[range(nx1)], z, u, p], [gnsf["f_lo_expr"]]
|
||||
)
|
||||
|
||||
# print(gnsf)
|
||||
# print(gnsf["n_out"])
|
||||
|
||||
for i_check in range(num_eval):
|
||||
# generate random values
|
||||
x0 = np.random.rand(nx, 1)
|
||||
x0dot = np.random.rand(nx, 1)
|
||||
z0 = np.random.rand(nz, 1)
|
||||
u0 = np.random.rand(nu, 1)
|
||||
|
||||
if gnsf["ny"] > 0:
|
||||
y0 = L_x @ x0[I_x1] + L_xdot @ x0dot[I_x1] + L_z @ z0[I_z1]
|
||||
else:
|
||||
y0 = []
|
||||
if gnsf["nuhat"] > 0:
|
||||
uhat0 = L_u @ u0
|
||||
else:
|
||||
uhat0 = []
|
||||
|
||||
# eval functions
|
||||
p0 = np.random.rand(gnsf["np"], 1)
|
||||
f_impl_val = impl_dae_fun(x0, x0dot, u0, z0, p0).full()
|
||||
phi_val = phi_fun(y0, uhat0, p0)
|
||||
f_lo_val = f_lo_fun(x0[I_x1], x0dot[I_x1], z0[I_z1], u0, p0)
|
||||
|
||||
f_impl_val = f_impl_val[idx_perm_f]
|
||||
# eval gnsf
|
||||
if n_out > 0:
|
||||
C_phi = C @ phi_val
|
||||
else:
|
||||
C_phi = np.zeros((nx1 + nz1, 1))
|
||||
try:
|
||||
gnsf_val1 = (
|
||||
A @ x0[I_x1] + B @ u0 + C_phi + c - E @ vertcat(x0dot[I_x1], z0[I_z1])
|
||||
)
|
||||
# gnsf_1 = (A @ x[I_x1] + B @ u + C_phi + c - E @ vertcat(xdot[I_x1], z[I_z1]))
|
||||
except:
|
||||
import pdb
|
||||
|
||||
pdb.set_trace()
|
||||
|
||||
if nx2 > 0: # eval LOS:
|
||||
gnsf_val2 = (
|
||||
A_LO @ x0[I_x2]
|
||||
+ B_LO @ u0
|
||||
+ c_LO
|
||||
+ f_lo_val
|
||||
- E_LO @ vertcat(x0dot[I_x2], z0[I_z2])
|
||||
)
|
||||
gnsf_val = vertcat(gnsf_val1, gnsf_val2).full()
|
||||
else:
|
||||
gnsf_val = gnsf_val1.full()
|
||||
# compute error and check
|
||||
rel_error = np.linalg.norm(f_impl_val - gnsf_val) / np.linalg.norm(f_impl_val)
|
||||
|
||||
if rel_error > TOL:
|
||||
print("transcription failed rel_error > TOL")
|
||||
print("you are in debug mode now: import pdb; pdb.set_trace()")
|
||||
abs_error = gnsf_val - f_impl_val
|
||||
# T = table(f_impl_val, gnsf_val, abs_error)
|
||||
# print(T)
|
||||
print("abs_error:", abs_error)
|
||||
# error('transcription failed rel_error > TOL')
|
||||
# check = 0
|
||||
import pdb
|
||||
|
||||
pdb.set_trace()
|
||||
if print_info:
|
||||
print(" ")
|
||||
print("model reformulation checked: relative error <= TOL = ", str(TOL))
|
||||
print(" ")
|
||||
check = 1
|
||||
## helpful for debugging:
|
||||
# # use in calling function and compare
|
||||
# # compare f_impl(i) with gnsf_val1(i)
|
||||
#
|
||||
|
||||
# nx = gnsf['nx']
|
||||
# nu = gnsf['nu']
|
||||
# nz = gnsf['nz']
|
||||
# nx1 = gnsf['nx1']
|
||||
# nx2 = gnsf['nx2']
|
||||
#
|
||||
# A = gnsf['A']
|
||||
# B = gnsf['B']
|
||||
# C = gnsf['C']
|
||||
# E = gnsf['E']
|
||||
# c = gnsf['c']
|
||||
#
|
||||
# L_x = gnsf['L_x']
|
||||
# L_z = gnsf['L_z']
|
||||
# L_xdot = gnsf['L_xdot']
|
||||
# L_u = gnsf['L_u']
|
||||
#
|
||||
# A_LO = gnsf['A_LO']
|
||||
#
|
||||
# x0 = rand(nx, 1)
|
||||
# x0dot = rand(nx, 1)
|
||||
# z0 = rand(nz, 1)
|
||||
# u0 = rand(nu, 1)
|
||||
# I_x1 = range(nx1)
|
||||
# I_x2 = nx1+range(nx)
|
||||
#
|
||||
# y0 = L_x @ x0[I_x1] + L_xdot @ x0dot[I_x1] + L_z @ z0
|
||||
# uhat0 = L_u @ u0
|
||||
#
|
||||
# gnsf_val1 = (A @ x[I_x1] + B @ u + # C @ phi_current + c) - E @ [xdot[I_x1] z]
|
||||
# gnsf_val1 = gnsf_val1.simplify()
|
||||
#
|
||||
# # gnsf_val2 = A_LO @ x[I_x2] + gnsf['f_lo_fun'](x[I_x1], xdot[I_x1], z, u) - xdot[I_x2]
|
||||
# gnsf_val2 = A_LO @ x[I_x2] + gnsf['f_lo_fun'](x[I_x1], xdot[I_x1], z, u) - xdot[I_x2]
|
||||
#
|
||||
#
|
||||
# gnsf_val = [gnsf_val1 gnsf_val2]
|
||||
# gnsf_val = gnsf_val.simplify()
|
||||
# dyn_expr_f = dyn_expr_f.simplify()
|
||||
# import pdb; pdb.set_trace()
|
||||
|
||||
return check
|
||||
278
third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py
vendored
Normal file
278
third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py
vendored
Normal file
@@ -0,0 +1,278 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
from casadi import *
|
||||
from .check_reformulation import check_reformulation
|
||||
from .determine_input_nonlinearity_function import determine_input_nonlinearity_function
|
||||
from ..utils import casadi_length, print_casadi_expression
|
||||
|
||||
|
||||
def detect_affine_terms_reduce_nonlinearity(gnsf, acados_ocp, print_info):
|
||||
|
||||
## Description
|
||||
# this function takes a gnsf structure with trivial model matrices (A, B,
|
||||
# E, c are zeros, and C is eye).
|
||||
# It detects all affine linear terms and sets up an equivalent model in the
|
||||
# GNSF structure, where all affine linear terms are modeled through the
|
||||
# matrices A, B, E, c and the linear output system (LOS) is empty.
|
||||
# NOTE: model is just taken as an argument to check equivalence of the
|
||||
# models within the function.
|
||||
|
||||
model = acados_ocp.model
|
||||
if print_info:
|
||||
print(" ")
|
||||
print("====================================================================")
|
||||
print(" ")
|
||||
print("============ Detect affine-linear dependencies ==================")
|
||||
print(" ")
|
||||
print("====================================================================")
|
||||
print(" ")
|
||||
# symbolics
|
||||
x = gnsf["x"]
|
||||
xdot = gnsf["xdot"]
|
||||
u = gnsf["u"]
|
||||
z = gnsf["z"]
|
||||
|
||||
# dimensions
|
||||
nx = gnsf["nx"]
|
||||
nu = gnsf["nu"]
|
||||
nz = gnsf["nz"]
|
||||
|
||||
ny_old = gnsf["ny"]
|
||||
nuhat_old = gnsf["nuhat"]
|
||||
|
||||
## Represent all affine dependencies through the model matrices A, B, E, c
|
||||
## determine A
|
||||
n_nodes_current = n_nodes(gnsf["phi_expr"])
|
||||
|
||||
for ii in range(casadi_length(gnsf["phi_expr"])):
|
||||
fii = gnsf["phi_expr"][ii]
|
||||
for ix in range(nx):
|
||||
var = x[ix]
|
||||
varname = var.name
|
||||
# symbolic jacobian of fii w.r.t. xi
|
||||
jac_fii_xi = jacobian(fii, var)
|
||||
if jac_fii_xi.is_constant():
|
||||
# jacobian value
|
||||
jac_fii_xi_fun = Function("jac_fii_xi_fun", [x[1]], [jac_fii_xi])
|
||||
# x[1] as input just to have a scalar input and call the function as follows:
|
||||
gnsf["A"][ii, ix] = jac_fii_xi_fun(0).full()
|
||||
else:
|
||||
gnsf["A"][ii, ix] = 0
|
||||
if print_info:
|
||||
print(
|
||||
"phi(",
|
||||
str(ii),
|
||||
") is nonlinear in x(",
|
||||
str(ix),
|
||||
") = ",
|
||||
varname,
|
||||
)
|
||||
print(fii)
|
||||
print("-----------------------------------------------------")
|
||||
f_next = gnsf["phi_expr"] - gnsf["A"] @ x
|
||||
f_next = simplify(f_next)
|
||||
n_nodes_next = n_nodes(f_next)
|
||||
|
||||
if print_info:
|
||||
print("\n")
|
||||
print(f"determined matrix A:")
|
||||
print(gnsf["A"])
|
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes")
|
||||
# assert(n_nodes_current >= n_nodes_next,'n_nodes_current >= n_nodes_next FAILED')
|
||||
gnsf["phi_expr"] = f_next
|
||||
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
## determine B
|
||||
n_nodes_current = n_nodes(gnsf["phi_expr"])
|
||||
|
||||
for ii in range(casadi_length(gnsf["phi_expr"])):
|
||||
fii = gnsf["phi_expr"][ii]
|
||||
for iu in range(nu):
|
||||
var = u[iu]
|
||||
varname = var.name
|
||||
# symbolic jacobian of fii w.r.t. ui
|
||||
jac_fii_ui = jacobian(fii, var)
|
||||
if jac_fii_ui.is_constant(): # i.e. hessian is structural zero:
|
||||
# jacobian value
|
||||
jac_fii_ui_fun = Function("jac_fii_ui_fun", [x[1]], [jac_fii_ui])
|
||||
gnsf["B"][ii, iu] = jac_fii_ui_fun(0).full()
|
||||
else:
|
||||
gnsf["B"][ii, iu] = 0
|
||||
if print_info:
|
||||
print(f"phi({ii}) is nonlinear in u(", str(iu), ") = ", varname)
|
||||
print(fii)
|
||||
print("-----------------------------------------------------")
|
||||
f_next = gnsf["phi_expr"] - gnsf["B"] @ u
|
||||
f_next = simplify(f_next)
|
||||
n_nodes_next = n_nodes(f_next)
|
||||
|
||||
if print_info:
|
||||
print("\n")
|
||||
print(f"determined matrix B:")
|
||||
print(gnsf["B"])
|
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes")
|
||||
|
||||
gnsf["phi_expr"] = f_next
|
||||
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
## determine E
|
||||
n_nodes_current = n_nodes(gnsf["phi_expr"])
|
||||
k = vertcat(xdot, z)
|
||||
|
||||
for ii in range(casadi_length(gnsf["phi_expr"])):
|
||||
fii = gnsf["phi_expr"][ii]
|
||||
for ik in range(casadi_length(k)):
|
||||
# symbolic jacobian of fii w.r.t. ui
|
||||
var = k[ik]
|
||||
varname = var.name
|
||||
jac_fii_ki = jacobian(fii, var)
|
||||
if jac_fii_ki.is_constant():
|
||||
# jacobian value
|
||||
jac_fii_ki_fun = Function("jac_fii_ki_fun", [x[1]], [jac_fii_ki])
|
||||
gnsf["E"][ii, ik] = -jac_fii_ki_fun(0).full()
|
||||
else:
|
||||
gnsf["E"][ii, ik] = 0
|
||||
if print_info:
|
||||
print(f"phi( {ii}) is nonlinear in xdot_z({ik}) = ", varname)
|
||||
print(fii)
|
||||
print("-----------------------------------------------------")
|
||||
f_next = gnsf["phi_expr"] + gnsf["E"] @ k
|
||||
f_next = simplify(f_next)
|
||||
n_nodes_next = n_nodes(f_next)
|
||||
|
||||
if print_info:
|
||||
print("\n")
|
||||
print(f"determined matrix E:")
|
||||
print(gnsf["E"])
|
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes")
|
||||
|
||||
gnsf["phi_expr"] = f_next
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
## determine constant term c
|
||||
|
||||
n_nodes_current = n_nodes(gnsf["phi_expr"])
|
||||
for ii in range(casadi_length(gnsf["phi_expr"])):
|
||||
fii = gnsf["phi_expr"][ii]
|
||||
if fii.is_constant():
|
||||
# function value goes into c
|
||||
fii_fun = Function("fii_fun", [x[1]], [fii])
|
||||
gnsf["c"][ii] = fii_fun(0).full()
|
||||
else:
|
||||
gnsf["c"][ii] = 0
|
||||
if print_info:
|
||||
print(f"phi(", str(ii), ") is NOT constant")
|
||||
print(fii)
|
||||
print("-----------------------------------------------------")
|
||||
gnsf["phi_expr"] = gnsf["phi_expr"] - gnsf["c"]
|
||||
gnsf["phi_expr"] = simplify(gnsf["phi_expr"])
|
||||
n_nodes_next = n_nodes(gnsf["phi_expr"])
|
||||
|
||||
if print_info:
|
||||
print("\n")
|
||||
print(f"determined vector c:")
|
||||
print(gnsf["c"])
|
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes")
|
||||
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
## determine nonlinearity & corresponding matrix C
|
||||
## Reduce dimension of phi
|
||||
n_nodes_current = n_nodes(gnsf["phi_expr"])
|
||||
ind_non_zero = []
|
||||
for ii in range(casadi_length(gnsf["phi_expr"])):
|
||||
fii = gnsf["phi_expr"][ii]
|
||||
fii = simplify(fii)
|
||||
if not fii.is_zero():
|
||||
ind_non_zero = list(set.union(set(ind_non_zero), set([ii])))
|
||||
gnsf["phi_expr"] = gnsf["phi_expr"][ind_non_zero]
|
||||
|
||||
# C
|
||||
gnsf["C"] = np.zeros((nx + nz, len(ind_non_zero)))
|
||||
for ii in range(len(ind_non_zero)):
|
||||
gnsf["C"][ind_non_zero[ii], ii] = 1
|
||||
gnsf = determine_input_nonlinearity_function(gnsf)
|
||||
n_nodes_next = n_nodes(gnsf["phi_expr"])
|
||||
|
||||
if print_info:
|
||||
print(" ")
|
||||
print("determined matrix C:")
|
||||
print(gnsf["C"])
|
||||
print(
|
||||
"---------------------------------------------------------------------------------"
|
||||
)
|
||||
print(
|
||||
"------------- Success: Affine linear terms detected -----------------------------"
|
||||
)
|
||||
print(
|
||||
"---------------------------------------------------------------------------------"
|
||||
)
|
||||
print(
|
||||
f'reduced nonlinearity dimension n_out from {nx+nz} to {gnsf["n_out"]}'
|
||||
)
|
||||
print(f"reduced nonlinearity from {n_nodes_current} to {n_nodes_next} nodes")
|
||||
print(" ")
|
||||
print("phi now reads as:")
|
||||
print_casadi_expression(gnsf["phi_expr"])
|
||||
|
||||
## determine input of nonlinearity function
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
gnsf["ny"] = casadi_length(gnsf["y"])
|
||||
gnsf["nuhat"] = casadi_length(gnsf["uhat"])
|
||||
|
||||
if print_info:
|
||||
print(
|
||||
"-----------------------------------------------------------------------------------"
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
f"reduced input ny of phi from ",
|
||||
str(ny_old),
|
||||
" to ",
|
||||
str(gnsf["ny"]),
|
||||
)
|
||||
print(
|
||||
f"reduced input nuhat of phi from ",
|
||||
str(nuhat_old),
|
||||
" to ",
|
||||
str(gnsf["nuhat"]),
|
||||
)
|
||||
print(
|
||||
"-----------------------------------------------------------------------------------"
|
||||
)
|
||||
|
||||
# if print_info:
|
||||
# print(f"gnsf: {gnsf}")
|
||||
|
||||
return gnsf
|
||||
240
third_party/acados/acados_template/gnsf/detect_gnsf_structure.py
vendored
Normal file
240
third_party/acados/acados_template/gnsf/detect_gnsf_structure.py
vendored
Normal file
@@ -0,0 +1,240 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
|
||||
|
||||
from casadi import Function, jacobian, SX, vertcat, horzcat
|
||||
|
||||
from .determine_trivial_gnsf_transcription import determine_trivial_gnsf_transcription
|
||||
from .detect_affine_terms_reduce_nonlinearity import (
|
||||
detect_affine_terms_reduce_nonlinearity,
|
||||
)
|
||||
from .reformulate_with_LOS import reformulate_with_LOS
|
||||
from .reformulate_with_invertible_E_mat import reformulate_with_invertible_E_mat
|
||||
from .structure_detection_print_summary import structure_detection_print_summary
|
||||
from .check_reformulation import check_reformulation
|
||||
|
||||
|
||||
def detect_gnsf_structure(acados_ocp, transcribe_opts=None):
|
||||
|
||||
## Description
|
||||
# This function takes a CasADi implicit ODE or index-1 DAE model "model"
|
||||
# consisting of a CasADi expression f_impl in the symbolic CasADi
|
||||
# variables x, xdot, u, z, (and possibly parameters p), which are also part
|
||||
# of the model, as well as a model name.
|
||||
# It will create a struct "gnsf" containing all information needed to use
|
||||
# it with the gnsf integrator in acados.
|
||||
# Additionally it will create the struct "reordered_model" which contains
|
||||
# the permuted state vector and permuted f_impl, in which additionally some
|
||||
# functions, which were made part of the linear output system of the gnsf,
|
||||
# have changed signs.
|
||||
|
||||
# Options: transcribe_opts is a Matlab struct consisting of booleans:
|
||||
# print_info: if extensive information on how the model is processed
|
||||
# is printed to the console.
|
||||
# generate_gnsf_model: if the neccessary C functions to simulate the gnsf
|
||||
# model with the acados implementation of the GNSF exploiting
|
||||
# integrator should be generated.
|
||||
# generate_gnsf_model: if the neccessary C functions to simulate the
|
||||
# reordered model with the acados implementation of the IRK
|
||||
# integrator should be generated.
|
||||
# check_E_invertibility: if the transcription method should check if the
|
||||
# assumption that the main blocks of the matrix gnsf.E are invertible
|
||||
# holds. If not, the method will try to reformulate the gnsf model
|
||||
# with a different model, such that the assumption holds.
|
||||
|
||||
# acados_root_dir = getenv('ACADOS_INSTALL_DIR')
|
||||
|
||||
## load transcribe_opts
|
||||
if transcribe_opts is None:
|
||||
print("WARNING: GNSF structure detection called without transcribe_opts")
|
||||
print(" using default settings")
|
||||
print("")
|
||||
transcribe_opts = dict()
|
||||
|
||||
if "print_info" in transcribe_opts:
|
||||
print_info = transcribe_opts["print_info"]
|
||||
else:
|
||||
print_info = 1
|
||||
print("print_info option was not set - default is true")
|
||||
|
||||
if "detect_LOS" in transcribe_opts:
|
||||
detect_LOS = transcribe_opts["detect_LOS"]
|
||||
else:
|
||||
detect_LOS = 1
|
||||
if print_info:
|
||||
print("detect_LOS option was not set - default is true")
|
||||
|
||||
if "check_E_invertibility" in transcribe_opts:
|
||||
check_E_invertibility = transcribe_opts["check_E_invertibility"]
|
||||
else:
|
||||
check_E_invertibility = 1
|
||||
if print_info:
|
||||
print("check_E_invertibility option was not set - default is true")
|
||||
|
||||
## Reformulate implicit index-1 DAE into GNSF form
|
||||
# (Generalized nonlinear static feedback)
|
||||
gnsf = determine_trivial_gnsf_transcription(acados_ocp, print_info)
|
||||
gnsf = detect_affine_terms_reduce_nonlinearity(gnsf, acados_ocp, print_info)
|
||||
|
||||
if detect_LOS:
|
||||
gnsf = reformulate_with_LOS(acados_ocp, gnsf, print_info)
|
||||
|
||||
if check_E_invertibility:
|
||||
gnsf = reformulate_with_invertible_E_mat(gnsf, acados_ocp, print_info)
|
||||
|
||||
# detect purely linear model
|
||||
if gnsf["nx1"] == 0 and gnsf["nz1"] == 0 and gnsf["nontrivial_f_LO"] == 0:
|
||||
gnsf["purely_linear"] = 1
|
||||
else:
|
||||
gnsf["purely_linear"] = 0
|
||||
|
||||
structure_detection_print_summary(gnsf, acados_ocp)
|
||||
check_reformulation(acados_ocp.model, gnsf, print_info)
|
||||
|
||||
## copy relevant fields from gnsf to model
|
||||
acados_ocp.model.get_matrices_fun = Function()
|
||||
dummy = acados_ocp.model.x[0]
|
||||
model_name = acados_ocp.model.name
|
||||
|
||||
get_matrices_fun = Function(
|
||||
f"{model_name}_gnsf_get_matrices_fun",
|
||||
[dummy],
|
||||
[
|
||||
gnsf["A"],
|
||||
gnsf["B"],
|
||||
gnsf["C"],
|
||||
gnsf["E"],
|
||||
gnsf["L_x"],
|
||||
gnsf["L_xdot"],
|
||||
gnsf["L_z"],
|
||||
gnsf["L_u"],
|
||||
gnsf["A_LO"],
|
||||
gnsf["c"],
|
||||
gnsf["E_LO"],
|
||||
gnsf["B_LO"],
|
||||
gnsf["nontrivial_f_LO"],
|
||||
gnsf["purely_linear"],
|
||||
gnsf["ipiv_x"] + 1,
|
||||
gnsf["ipiv_z"] + 1,
|
||||
gnsf["c_LO"],
|
||||
],
|
||||
)
|
||||
|
||||
phi = gnsf["phi_expr"]
|
||||
y = gnsf["y"]
|
||||
uhat = gnsf["uhat"]
|
||||
p = gnsf["p"]
|
||||
|
||||
jac_phi_y = jacobian(phi, y)
|
||||
jac_phi_uhat = jacobian(phi, uhat)
|
||||
|
||||
phi_fun = Function(f"{model_name}_gnsf_phi_fun", [y, uhat, p], [phi])
|
||||
acados_ocp.model.phi_fun = phi_fun
|
||||
acados_ocp.model.phi_fun_jac_y = Function(
|
||||
f"{model_name}_gnsf_phi_fun_jac_y", [y, uhat, p], [phi, jac_phi_y]
|
||||
)
|
||||
acados_ocp.model.phi_jac_y_uhat = Function(
|
||||
f"{model_name}_gnsf_phi_jac_y_uhat", [y, uhat, p], [jac_phi_y, jac_phi_uhat]
|
||||
)
|
||||
|
||||
x1 = acados_ocp.model.x[gnsf["idx_perm_x"][: gnsf["nx1"]]]
|
||||
x1dot = acados_ocp.model.xdot[gnsf["idx_perm_x"][: gnsf["nx1"]]]
|
||||
if gnsf["nz1"] > 0:
|
||||
z1 = acados_ocp.model.z[gnsf["idx_perm_z"][: gnsf["nz1"]]]
|
||||
else:
|
||||
z1 = SX.sym("z1", 0, 0)
|
||||
f_lo = gnsf["f_lo_expr"]
|
||||
u = acados_ocp.model.u
|
||||
acados_ocp.model.f_lo_fun_jac_x1k1uz = Function(
|
||||
f"{model_name}_gnsf_f_lo_fun_jac_x1k1uz",
|
||||
[x1, x1dot, z1, u, p],
|
||||
[
|
||||
f_lo,
|
||||
horzcat(
|
||||
jacobian(f_lo, x1),
|
||||
jacobian(f_lo, x1dot),
|
||||
jacobian(f_lo, u),
|
||||
jacobian(f_lo, z1),
|
||||
),
|
||||
],
|
||||
)
|
||||
|
||||
acados_ocp.model.get_matrices_fun = get_matrices_fun
|
||||
|
||||
size_gnsf_A = gnsf["A"].shape
|
||||
acados_ocp.dims.gnsf_nx1 = size_gnsf_A[1]
|
||||
acados_ocp.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
|
||||
acados_ocp.dims.gnsf_nuhat = max(phi_fun.size_in(1))
|
||||
acados_ocp.dims.gnsf_ny = max(phi_fun.size_in(0))
|
||||
acados_ocp.dims.gnsf_nout = max(phi_fun.size_out(0))
|
||||
|
||||
# # dim
|
||||
# model['dim_gnsf_nx1'] = gnsf['nx1']
|
||||
# model['dim_gnsf_nx2'] = gnsf['nx2']
|
||||
# model['dim_gnsf_nz1'] = gnsf['nz1']
|
||||
# model['dim_gnsf_nz2'] = gnsf['nz2']
|
||||
# model['dim_gnsf_nuhat'] = gnsf['nuhat']
|
||||
# model['dim_gnsf_ny'] = gnsf['ny']
|
||||
# model['dim_gnsf_nout'] = gnsf['n_out']
|
||||
|
||||
# # sym
|
||||
# model['sym_gnsf_y'] = gnsf['y']
|
||||
# model['sym_gnsf_uhat'] = gnsf['uhat']
|
||||
|
||||
# # data
|
||||
# model['dyn_gnsf_A'] = gnsf['A']
|
||||
# model['dyn_gnsf_A_LO'] = gnsf['A_LO']
|
||||
# model['dyn_gnsf_B'] = gnsf['B']
|
||||
# model['dyn_gnsf_B_LO'] = gnsf['B_LO']
|
||||
# model['dyn_gnsf_E'] = gnsf['E']
|
||||
# model['dyn_gnsf_E_LO'] = gnsf['E_LO']
|
||||
# model['dyn_gnsf_C'] = gnsf['C']
|
||||
# model['dyn_gnsf_c'] = gnsf['c']
|
||||
# model['dyn_gnsf_c_LO'] = gnsf['c_LO']
|
||||
# model['dyn_gnsf_L_x'] = gnsf['L_x']
|
||||
# model['dyn_gnsf_L_xdot'] = gnsf['L_xdot']
|
||||
# model['dyn_gnsf_L_z'] = gnsf['L_z']
|
||||
# model['dyn_gnsf_L_u'] = gnsf['L_u']
|
||||
# model['dyn_gnsf_idx_perm_x'] = gnsf['idx_perm_x']
|
||||
# model['dyn_gnsf_ipiv_x'] = gnsf['ipiv_x']
|
||||
# model['dyn_gnsf_idx_perm_z'] = gnsf['idx_perm_z']
|
||||
# model['dyn_gnsf_ipiv_z'] = gnsf['ipiv_z']
|
||||
# model['dyn_gnsf_idx_perm_f'] = gnsf['idx_perm_f']
|
||||
# model['dyn_gnsf_ipiv_f'] = gnsf['ipiv_f']
|
||||
|
||||
# # flags
|
||||
# model['dyn_gnsf_nontrivial_f_LO'] = gnsf['nontrivial_f_LO']
|
||||
# model['dyn_gnsf_purely_linear'] = gnsf['purely_linear']
|
||||
|
||||
# # casadi expr
|
||||
# model['dyn_gnsf_expr_phi'] = gnsf['phi_expr']
|
||||
# model['dyn_gnsf_expr_f_lo'] = gnsf['f_lo_expr']
|
||||
|
||||
return acados_ocp
|
||||
110
third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py
vendored
Normal file
110
third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py
vendored
Normal file
@@ -0,0 +1,110 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
|
||||
|
||||
from casadi import *
|
||||
from ..utils import casadi_length, is_empty
|
||||
|
||||
|
||||
def determine_input_nonlinearity_function(gnsf):
|
||||
|
||||
## Description
|
||||
# this function takes a structure gnsf and updates the matrices L_x,
|
||||
# L_xdot, L_z, L_u and CasADi vectors y, uhat of this structure as follows:
|
||||
|
||||
# given a CasADi expression phi_expr, which may depend on the variables
|
||||
# (x1, x1dot, z, u), this function determines a vector y (uhat) consisting
|
||||
# of all components of (x1, x1dot, z) (respectively u) that enter phi_expr.
|
||||
# Additionally matrices L_x, L_xdot, L_z, L_u are determined such that
|
||||
# y = L_x * x + L_xdot * xdot + L_z * z
|
||||
# uhat = L_u * u
|
||||
# Furthermore the dimensions ny, nuhat, n_out are updated
|
||||
|
||||
## y
|
||||
y = SX.sym('y', 0, 0)
|
||||
# components of x1
|
||||
for ii in range(gnsf["nx1"]):
|
||||
if which_depends(gnsf["phi_expr"], gnsf["x"][ii])[0]:
|
||||
y = vertcat(y, gnsf["x"][ii])
|
||||
# else:
|
||||
# x[ii] is not part of y
|
||||
# components of x1dot
|
||||
for ii in range(gnsf["nx1"]):
|
||||
if which_depends(gnsf["phi_expr"], gnsf["xdot"][ii])[0]:
|
||||
print(gnsf["phi_expr"], "depends on", gnsf["xdot"][ii])
|
||||
y = vertcat(y, gnsf["xdot"][ii])
|
||||
# else:
|
||||
# xdot[ii] is not part of y
|
||||
# components of z
|
||||
for ii in range(gnsf["nz1"]):
|
||||
if which_depends(gnsf["phi_expr"], gnsf["z"][ii])[0]:
|
||||
y = vertcat(y, gnsf["z"][ii])
|
||||
# else:
|
||||
# z[ii] is not part of y
|
||||
## uhat
|
||||
uhat = SX.sym('uhat', 0, 0)
|
||||
# components of u
|
||||
for ii in range(gnsf["nu"]):
|
||||
if which_depends(gnsf["phi_expr"], gnsf["u"][ii])[0]:
|
||||
uhat = vertcat(uhat, gnsf["u"][ii])
|
||||
# else:
|
||||
# u[ii] is not part of uhat
|
||||
## generate gnsf['phi_expr_fun']
|
||||
# linear input matrices
|
||||
if is_empty(y):
|
||||
gnsf["L_x"] = []
|
||||
gnsf["L_xdot"] = []
|
||||
gnsf["L_u"] = []
|
||||
gnsf["L_z"] = []
|
||||
else:
|
||||
dummy = SX.sym("dummy_input", 0)
|
||||
L_x_fun = Function(
|
||||
"L_x_fun", [dummy], [jacobian(y, gnsf["x"][range(gnsf["nx1"])])]
|
||||
)
|
||||
L_xdot_fun = Function(
|
||||
"L_xdot_fun", [dummy], [jacobian(y, gnsf["xdot"][range(gnsf["nx1"])])]
|
||||
)
|
||||
L_z_fun = Function(
|
||||
"L_z_fun", [dummy], [jacobian(y, gnsf["z"][range(gnsf["nz1"])])]
|
||||
)
|
||||
L_u_fun = Function("L_u_fun", [dummy], [jacobian(uhat, gnsf["u"])])
|
||||
|
||||
gnsf["L_x"] = L_x_fun(0).full()
|
||||
gnsf["L_xdot"] = L_xdot_fun(0).full()
|
||||
gnsf["L_u"] = L_u_fun(0).full()
|
||||
gnsf["L_z"] = L_z_fun(0).full()
|
||||
gnsf["y"] = y
|
||||
gnsf["uhat"] = uhat
|
||||
|
||||
gnsf["ny"] = casadi_length(y)
|
||||
gnsf["nuhat"] = casadi_length(uhat)
|
||||
gnsf["n_out"] = casadi_length(gnsf["phi_expr"])
|
||||
|
||||
return gnsf
|
||||
155
third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py
vendored
Normal file
155
third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py
vendored
Normal file
@@ -0,0 +1,155 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
from casadi import *
|
||||
import numpy as np
|
||||
from ..utils import casadi_length, idx_perm_to_ipiv
|
||||
from .determine_input_nonlinearity_function import determine_input_nonlinearity_function
|
||||
from .check_reformulation import check_reformulation
|
||||
|
||||
|
||||
def determine_trivial_gnsf_transcription(acados_ocp, print_info):
|
||||
## Description
|
||||
# this function takes a model of an implicit ODE/ index-1 DAE and sets up
|
||||
# an equivalent model in the GNSF structure, with empty linear output
|
||||
# system and trivial model matrices, i.e. A, B, E, c are zeros, and C is
|
||||
# eye. - no structure is exploited
|
||||
|
||||
model = acados_ocp.model
|
||||
# initial print
|
||||
print("*****************************************************************")
|
||||
print(" ")
|
||||
print(f"****** Restructuring {model.name} model ***********")
|
||||
print(" ")
|
||||
print("*****************************************************************")
|
||||
|
||||
# load model
|
||||
f_impl_expr = model.f_impl_expr
|
||||
|
||||
model_name_prefix = model.name
|
||||
|
||||
# x
|
||||
x = model.x
|
||||
nx = acados_ocp.dims.nx
|
||||
# check type
|
||||
if isinstance(x[0], SX):
|
||||
isSX = True
|
||||
else:
|
||||
print("GNSF detection only works for SX CasADi type!!!")
|
||||
import pdb
|
||||
|
||||
pdb.set_trace()
|
||||
# xdot
|
||||
xdot = model.xdot
|
||||
# u
|
||||
nu = acados_ocp.dims.nu
|
||||
if nu == 0:
|
||||
u = SX.sym("u", 0, 0)
|
||||
else:
|
||||
u = model.u
|
||||
|
||||
nz = acados_ocp.dims.nz
|
||||
if nz == 0:
|
||||
z = SX.sym("z", 0, 0)
|
||||
else:
|
||||
z = model.z
|
||||
|
||||
p = model.p
|
||||
nparam = acados_ocp.dims.np
|
||||
|
||||
# avoid SX of size 0x1
|
||||
if casadi_length(u) == 0:
|
||||
u = SX.sym("u", 0, 0)
|
||||
nu = 0
|
||||
## initialize gnsf struct
|
||||
# dimensions
|
||||
gnsf = {"nx": nx, "nu": nu, "nz": nz, "np": nparam}
|
||||
gnsf["nx1"] = nx
|
||||
gnsf["nx2"] = 0
|
||||
gnsf["nz1"] = nz
|
||||
gnsf["nz2"] = 0
|
||||
gnsf["nuhat"] = nu
|
||||
gnsf["ny"] = 2 * nx + nz
|
||||
|
||||
gnsf["phi_expr"] = f_impl_expr
|
||||
gnsf["A"] = np.zeros((nx + nz, nx))
|
||||
gnsf["B"] = np.zeros((nx + nz, nu))
|
||||
gnsf["E"] = np.zeros((nx + nz, nx + nz))
|
||||
gnsf["c"] = np.zeros((nx + nz, 1))
|
||||
gnsf["C"] = np.eye(nx + nz)
|
||||
gnsf["name"] = model_name_prefix
|
||||
|
||||
gnsf["x"] = x
|
||||
gnsf["xdot"] = xdot
|
||||
gnsf["z"] = z
|
||||
gnsf["u"] = u
|
||||
gnsf["p"] = p
|
||||
|
||||
gnsf = determine_input_nonlinearity_function(gnsf)
|
||||
|
||||
gnsf["A_LO"] = []
|
||||
gnsf["E_LO"] = []
|
||||
gnsf["B_LO"] = []
|
||||
gnsf["c_LO"] = []
|
||||
gnsf["f_lo_expr"] = []
|
||||
|
||||
# permutation
|
||||
gnsf["idx_perm_x"] = range(nx) # matlab-style)
|
||||
gnsf["ipiv_x"] = idx_perm_to_ipiv(gnsf["idx_perm_x"]) # blasfeo-style
|
||||
gnsf["idx_perm_z"] = range(nz)
|
||||
gnsf["ipiv_z"] = idx_perm_to_ipiv(gnsf["idx_perm_z"])
|
||||
gnsf["idx_perm_f"] = range((nx + nz))
|
||||
gnsf["ipiv_f"] = idx_perm_to_ipiv(gnsf["idx_perm_f"])
|
||||
|
||||
gnsf["nontrivial_f_LO"] = 0
|
||||
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
if print_info:
|
||||
print(f"Success: Set up equivalent GNSF model with trivial matrices")
|
||||
print(" ")
|
||||
if print_info:
|
||||
print(
|
||||
"-----------------------------------------------------------------------------------"
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
"reduced input ny of phi from ",
|
||||
str(2 * nx + nz),
|
||||
" to ",
|
||||
str(gnsf["ny"]),
|
||||
)
|
||||
print(
|
||||
"reduced input nuhat of phi from ", str(nu), " to ", str(gnsf["nuhat"])
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
"-----------------------------------------------------------------------------------"
|
||||
)
|
||||
return gnsf
|
||||
43
third_party/acados/acados_template/gnsf/matlab to python.md
vendored
Normal file
43
third_party/acados/acados_template/gnsf/matlab to python.md
vendored
Normal file
@@ -0,0 +1,43 @@
|
||||
# matlab to python
|
||||
|
||||
% -> #
|
||||
|
||||
; ->
|
||||
|
||||
from casadi import *
|
||||
->
|
||||
from casadi import *
|
||||
|
||||
|
||||
print\('(.*)'\)
|
||||
print('$1')
|
||||
|
||||
print\(\['(.*)'\]\)
|
||||
print(f'$1')
|
||||
|
||||
keyboard
|
||||
import pdb; pdb.set_trace()
|
||||
|
||||
|
||||
range((([^))]*))
|
||||
range($1)
|
||||
|
||||
\s*end
|
||||
->
|
||||
nothing
|
||||
|
||||
|
||||
if (.*)
|
||||
if $1:
|
||||
|
||||
else
|
||||
else:
|
||||
|
||||
num2str
|
||||
str
|
||||
|
||||
for ([a-z_]*) =
|
||||
for $1 in
|
||||
|
||||
length\(
|
||||
len(
|
||||
394
third_party/acados/acados_template/gnsf/reformulate_with_LOS.py
vendored
Normal file
394
third_party/acados/acados_template/gnsf/reformulate_with_LOS.py
vendored
Normal file
@@ -0,0 +1,394 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
|
||||
|
||||
from .determine_input_nonlinearity_function import determine_input_nonlinearity_function
|
||||
from .check_reformulation import check_reformulation
|
||||
from casadi import *
|
||||
from ..utils import casadi_length, idx_perm_to_ipiv, is_empty
|
||||
|
||||
|
||||
def reformulate_with_LOS(acados_ocp, gnsf, print_info):
|
||||
|
||||
## Description:
|
||||
# This function takes an intitial transcription of the implicit ODE model
|
||||
# "model" into "gnsf" and reformulates "gnsf" with a linear output system
|
||||
# (LOS), containing as many states of the model as possible.
|
||||
# Therefore it might be that the state vector and the implicit function
|
||||
# vector have to be reordered. This reordered model is part of the output,
|
||||
# namely reordered_model.
|
||||
|
||||
## import CasADi and load models
|
||||
model = acados_ocp.model
|
||||
|
||||
# symbolics
|
||||
x = gnsf["x"]
|
||||
xdot = gnsf["xdot"]
|
||||
u = gnsf["u"]
|
||||
z = gnsf["z"]
|
||||
|
||||
# dimensions
|
||||
nx = gnsf["nx"]
|
||||
nz = gnsf["nz"]
|
||||
|
||||
# get model matrices
|
||||
A = gnsf["A"]
|
||||
B = gnsf["B"]
|
||||
C = gnsf["C"]
|
||||
E = gnsf["E"]
|
||||
c = gnsf["c"]
|
||||
|
||||
A_LO = gnsf["A_LO"]
|
||||
|
||||
y = gnsf["y"]
|
||||
|
||||
phi_old = gnsf["phi_expr"]
|
||||
|
||||
if print_info:
|
||||
print(" ")
|
||||
print("=================================================================")
|
||||
print(" ")
|
||||
print("================ Detect Linear Output System ===============")
|
||||
print(" ")
|
||||
print("=================================================================")
|
||||
print(" ")
|
||||
## build initial I_x1 and I_x2_candidates
|
||||
# I_xrange( all components of x for which either xii or xdot_ii enters y):
|
||||
# I_LOS_candidates: the remaining components
|
||||
|
||||
I_nsf_components = set()
|
||||
I_LOS_candidates = set()
|
||||
|
||||
if gnsf["ny"] > 0:
|
||||
for ii in range(nx):
|
||||
if which_depends(y, x[ii])[0] or which_depends(y, xdot[ii])[0]:
|
||||
# i.e. xii or xiidot are part of y, and enter phi_expr
|
||||
if print_info:
|
||||
print(f"x_{ii} is part of x1")
|
||||
I_nsf_components = set.union(I_nsf_components, set([ii]))
|
||||
else:
|
||||
# i.e. neither xii nor xiidot are part of y, i.e. enter phi_expr
|
||||
I_LOS_candidates = set.union(I_LOS_candidates, set([ii]))
|
||||
if print_info:
|
||||
print(" ")
|
||||
for ii in range(nz):
|
||||
if which_depends(y, z[ii])[0]:
|
||||
# i.e. xii or xiidot are part of y, and enter phi_expr
|
||||
if print_info:
|
||||
print(f"z_{ii} is part of x1")
|
||||
I_nsf_components = set.union(I_nsf_components, set([ii + nx]))
|
||||
else:
|
||||
# i.e. neither xii nor xiidot are part of y, i.e. enter phi_expr
|
||||
I_LOS_candidates = set.union(I_LOS_candidates, set([ii + nx]))
|
||||
else:
|
||||
I_LOS_candidates = set(range((nx + nz)))
|
||||
if print_info:
|
||||
print(" ")
|
||||
print(f"I_LOS_candidates {I_LOS_candidates}")
|
||||
new_nsf_components = I_nsf_components
|
||||
I_nsf_eq = set([])
|
||||
unsorted_dyn = set(range(nx + nz))
|
||||
xdot_z = vertcat(xdot, z)
|
||||
|
||||
## determine components of Linear Output System
|
||||
# determine maximal index set I_x2
|
||||
# such that the components x(I_x2) can be written as a LOS
|
||||
Eq_map = []
|
||||
while True:
|
||||
## find equations corresponding to new_nsf_components
|
||||
for ii in new_nsf_components:
|
||||
current_var = xdot_z[ii]
|
||||
var_name = current_var.name
|
||||
|
||||
# print( unsorted_dyn)
|
||||
# print("np.nonzero(E[:,ii])[0]",np.nonzero(E[:,ii])[0])
|
||||
I_eq = set.intersection(set(np.nonzero(E[:, ii])[0]), unsorted_dyn)
|
||||
if len(I_eq) == 1:
|
||||
i_eq = I_eq.pop()
|
||||
if print_info:
|
||||
print(f"component {i_eq} is associated with state {ii}")
|
||||
elif len(I_eq) > 1: # x_ii_dot occurs in more than 1 eq linearly
|
||||
# find the equation with least linear dependencies on
|
||||
# I_LOS_cancidates
|
||||
number_of_eq = 0
|
||||
candidate_dependencies = np.zeros(len(I_eq), 1)
|
||||
I_x2_candidates = set.intersection(I_LOS_candidates, set(range(nx)))
|
||||
for eq in I_eq:
|
||||
depending_candidates = set.union(
|
||||
np.nonzero(E[eq, I_LOS_candidates])[0],
|
||||
np.nonzero(A[eq, I_x2_candidates])[0],
|
||||
)
|
||||
candidate_dependencies[number_of_eq] = +len(depending_candidates)
|
||||
number_of_eq += 1
|
||||
number_of_eq = np.argmin(candidate_dependencies)
|
||||
i_eq = I_eq[number_of_eq]
|
||||
else: ## x_ii_dot does not occur linearly in any of the unsorted dynamics
|
||||
for j in unsorted_dyn:
|
||||
phi_eq_j = gnsf["phi_expr"][np.nonzero(C[j, :])[0]]
|
||||
if which_depends(phi_eq_j, xdot_z(ii))[0]:
|
||||
I_eq = set.union(I_eq, j)
|
||||
if is_empty(I_eq):
|
||||
I_eq = unsorted_dyn
|
||||
# find the equation with least linear dependencies on I_LOS_cancidates
|
||||
number_of_eq = 0
|
||||
candidate_dependencies = np.zeros(len(I_eq), 1)
|
||||
I_x2_candidates = set.intersection(I_LOS_candidates, set(range(nx)))
|
||||
for eq in I_eq:
|
||||
depending_candidates = set.union(
|
||||
np.nonzero(E[eq, I_LOS_candidates])[0],
|
||||
np.nonzero(A[eq, I_x2_candidates])[0],
|
||||
)
|
||||
candidate_dependencies[number_of_eq] = +len(depending_candidates)
|
||||
number_of_eq += 1
|
||||
number_of_eq = np.argmin(candidate_dependencies)
|
||||
i_eq = I_eq[number_of_eq]
|
||||
## add 1 * [xdot,z](ii) to both sides of i_eq
|
||||
if print_info:
|
||||
print(
|
||||
"adding 1 * ",
|
||||
var_name,
|
||||
" to both sides of equation ",
|
||||
i_eq,
|
||||
".",
|
||||
)
|
||||
gnsf["E"][i_eq, ii] = 1
|
||||
i_phi = np.nonzero(gnsf["C"][i_eq, :])
|
||||
if is_empty(i_phi):
|
||||
i_phi = len(gnsf["phi_expr"]) + 1
|
||||
gnsf["C"][i_eq, i_phi] = 1 # add column to C with 1 entry
|
||||
gnsf["phi_expr"] = vertcat(gnsf["phi_expr"], 0)
|
||||
gnsf["phi_expr"][i_phi] = (
|
||||
gnsf["phi_expr"](i_phi)
|
||||
+ gnsf["E"][i_eq, ii] / gnsf["C"][i_eq, i_phi] * xdot_z[ii]
|
||||
)
|
||||
if print_info:
|
||||
print(
|
||||
"detected equation ",
|
||||
i_eq,
|
||||
" to correspond to variable ",
|
||||
var_name,
|
||||
)
|
||||
I_nsf_eq = set.union(I_nsf_eq, {i_eq})
|
||||
# remove i_eq from unsorted_dyn
|
||||
unsorted_dyn.remove(i_eq)
|
||||
Eq_map.append([ii, i_eq])
|
||||
|
||||
## add components to I_x1
|
||||
for eq in I_nsf_eq:
|
||||
I_linear_dependence = set.union(
|
||||
set(np.nonzero(A[eq, :])[0]), set(np.nonzero(E[eq, :])[0])
|
||||
)
|
||||
I_nsf_components = set.union(I_linear_dependence, I_nsf_components)
|
||||
# I_nsf_components = I_nsf_components[:]
|
||||
|
||||
new_nsf_components = set.intersection(I_LOS_candidates, I_nsf_components)
|
||||
if is_empty(new_nsf_components):
|
||||
if print_info:
|
||||
print("new_nsf_components is empty")
|
||||
break
|
||||
# remove new_nsf_components from candidates
|
||||
I_LOS_candidates = set.difference(I_LOS_candidates, new_nsf_components)
|
||||
if not is_empty(Eq_map):
|
||||
# [~, new_eq_order] = sort(Eq_map(1,:))
|
||||
# I_nsf_eq = Eq_map(2, new_eq_order )
|
||||
for count, m in enumerate(Eq_map):
|
||||
m.append(count)
|
||||
sorted(Eq_map, key=lambda x: x[1])
|
||||
new_eq_order = [m[2] for m in Eq_map]
|
||||
Eq_map = [Eq_map[i] for i in new_eq_order]
|
||||
I_nsf_eq = [m[1] for m in Eq_map]
|
||||
|
||||
else:
|
||||
I_nsf_eq = []
|
||||
|
||||
I_LOS_components = I_LOS_candidates
|
||||
I_LOS_eq = sorted(set.difference(set(range(nx + nz)), I_nsf_eq))
|
||||
I_nsf_eq = sorted(I_nsf_eq)
|
||||
|
||||
I_x1 = set.intersection(I_nsf_components, set(range(nx)))
|
||||
I_z1 = set.intersection(I_nsf_components, set(range(nx, nx + nz)))
|
||||
I_z1 = set([i - nx for i in I_z1])
|
||||
|
||||
I_x2 = set.intersection(I_LOS_components, set(range(nx)))
|
||||
I_z2 = set.intersection(I_LOS_components, set(range(nx, nx + nz)))
|
||||
I_z2 = set([i - nx for i in I_z2])
|
||||
|
||||
if print_info:
|
||||
print(f"I_x1 {I_x1}, I_x2 {I_x2}")
|
||||
|
||||
## permute x, xdot
|
||||
if is_empty(I_x1):
|
||||
x1 = []
|
||||
x1dot = []
|
||||
else:
|
||||
x1 = x[list(I_x1)]
|
||||
x1dot = xdot[list(I_x1)]
|
||||
if is_empty(I_x2):
|
||||
x2 = []
|
||||
x2dot = []
|
||||
else:
|
||||
x2 = x[list(I_x2)]
|
||||
x2dot = xdot[list(I_x2)]
|
||||
if is_empty(I_z1):
|
||||
z1 = []
|
||||
else:
|
||||
z1 = z(I_z1)
|
||||
if is_empty(I_z2):
|
||||
z2 = []
|
||||
else:
|
||||
z2 = z[list(I_z2)]
|
||||
|
||||
I_x1 = sorted(I_x1)
|
||||
I_x2 = sorted(I_x2)
|
||||
I_z1 = sorted(I_z1)
|
||||
I_z2 = sorted(I_z2)
|
||||
gnsf["xdot"] = vertcat(x1dot, x2dot)
|
||||
gnsf["x"] = vertcat(x1, x2)
|
||||
gnsf["z"] = vertcat(z1, z2)
|
||||
gnsf["nx1"] = len(I_x1)
|
||||
gnsf["nx2"] = len(I_x2)
|
||||
gnsf["nz1"] = len(I_z1)
|
||||
gnsf["nz2"] = len(I_z2)
|
||||
|
||||
# store permutations
|
||||
gnsf["idx_perm_x"] = I_x1 + I_x2
|
||||
gnsf["ipiv_x"] = idx_perm_to_ipiv(gnsf["idx_perm_x"])
|
||||
gnsf["idx_perm_z"] = I_z1 + I_z2
|
||||
gnsf["ipiv_z"] = idx_perm_to_ipiv(gnsf["idx_perm_z"])
|
||||
gnsf["idx_perm_f"] = I_nsf_eq + I_LOS_eq
|
||||
gnsf["ipiv_f"] = idx_perm_to_ipiv(gnsf["idx_perm_f"])
|
||||
|
||||
f_LO = SX.sym("f_LO", 0, 0)
|
||||
|
||||
## rewrite I_LOS_eq as LOS
|
||||
if gnsf["n_out"] == 0:
|
||||
C_phi = np.zeros(gnsf["nx"] + gnsf["nz"], 1)
|
||||
else:
|
||||
C_phi = C @ phi_old
|
||||
if gnsf["nx1"] == 0:
|
||||
Ax1 = np.zeros(gnsf["nx"] + gnsf["nz"], 1)
|
||||
else:
|
||||
Ax1 = A[:, sorted(I_x1)] @ x1
|
||||
if gnsf["nx1"] + gnsf["nz1"] == 0:
|
||||
lhs_nsf = np.zeros(gnsf["nx"] + gnsf["nz"], 1)
|
||||
else:
|
||||
lhs_nsf = E[:, sorted(I_nsf_components)] @ vertcat(x1, z1)
|
||||
n_LO = len(I_LOS_eq)
|
||||
B_LO = np.zeros((n_LO, gnsf["nu"]))
|
||||
A_LO = np.zeros((gnsf["nx2"] + gnsf["nz2"], gnsf["nx2"]))
|
||||
E_LO = np.zeros((n_LO, n_LO))
|
||||
c_LO = np.zeros((n_LO, 1))
|
||||
|
||||
I_LOS_eq = list(I_LOS_eq)
|
||||
for eq in I_LOS_eq:
|
||||
i_LO = I_LOS_eq.index(eq)
|
||||
f_LO = vertcat(f_LO, Ax1[eq] + C_phi[eq] - lhs_nsf[eq])
|
||||
print(f"eq {eq} I_LOS_components {I_LOS_components}, i_LO {i_LO}, f_LO {f_LO}")
|
||||
E_LO[i_LO, :] = E[eq, sorted(I_LOS_components)]
|
||||
A_LO[i_LO, :] = A[eq, I_x2]
|
||||
c_LO[i_LO, :] = c[eq]
|
||||
B_LO[i_LO, :] = B[eq, :]
|
||||
if casadi_length(f_LO) == 0:
|
||||
f_LO = SX.zeros((gnsf["nx2"] + gnsf["nz2"], 1))
|
||||
f_LO = simplify(f_LO)
|
||||
gnsf["A_LO"] = A_LO
|
||||
gnsf["E_LO"] = E_LO
|
||||
gnsf["B_LO"] = B_LO
|
||||
gnsf["c_LO"] = c_LO
|
||||
gnsf["f_lo_expr"] = f_LO
|
||||
|
||||
## remove I_LOS_eq from NSF type system
|
||||
gnsf["A"] = gnsf["A"][np.ix_(sorted(I_nsf_eq), sorted(I_x1))]
|
||||
gnsf["B"] = gnsf["B"][sorted(I_nsf_eq), :]
|
||||
gnsf["C"] = gnsf["C"][sorted(I_nsf_eq), :]
|
||||
gnsf["E"] = gnsf["E"][np.ix_(sorted(I_nsf_eq), sorted(I_nsf_components))]
|
||||
gnsf["c"] = gnsf["c"][sorted(I_nsf_eq), :]
|
||||
|
||||
## reduce phi, C
|
||||
I_nonzero = []
|
||||
for ii in range(gnsf["C"].shape[1]): # n_colums of C:
|
||||
print(f"ii {ii}")
|
||||
if not all(gnsf["C"][:, ii] == 0): # if column ~= 0
|
||||
I_nonzero.append(ii)
|
||||
gnsf["C"] = gnsf["C"][:, I_nonzero]
|
||||
gnsf["phi_expr"] = gnsf["phi_expr"][I_nonzero]
|
||||
|
||||
gnsf = determine_input_nonlinearity_function(gnsf)
|
||||
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
gnsf["nontrivial_f_LO"] = 0
|
||||
if not is_empty(gnsf["f_lo_expr"]):
|
||||
for ii in range(casadi_length(gnsf["f_lo_expr"])):
|
||||
fii = gnsf["f_lo_expr"][ii]
|
||||
if not fii.is_zero():
|
||||
gnsf["nontrivial_f_LO"] = 1
|
||||
if not gnsf["nontrivial_f_LO"] and print_info:
|
||||
print("f_LO is fully trivial (== 0)")
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
|
||||
if print_info:
|
||||
print("")
|
||||
print(
|
||||
"---------------------------------------------------------------------------------"
|
||||
)
|
||||
print(
|
||||
"------------- Success: Linear Output System (LOS) detected ----------------------"
|
||||
)
|
||||
print(
|
||||
"---------------------------------------------------------------------------------"
|
||||
)
|
||||
print("")
|
||||
print(
|
||||
"==>> moved ",
|
||||
gnsf["nx2"],
|
||||
"differential states and ",
|
||||
gnsf["nz2"],
|
||||
" algebraic variables to the Linear Output System",
|
||||
)
|
||||
print(
|
||||
"==>> recuced output dimension of phi from ",
|
||||
casadi_length(phi_old),
|
||||
" to ",
|
||||
casadi_length(gnsf["phi_expr"]),
|
||||
)
|
||||
print(" ")
|
||||
print("Matrices defining the LOS read as")
|
||||
print(" ")
|
||||
print("E_LO =")
|
||||
print(gnsf["E_LO"])
|
||||
print("A_LO =")
|
||||
print(gnsf["A_LO"])
|
||||
print("B_LO =")
|
||||
print(gnsf["B_LO"])
|
||||
print("c_LO =")
|
||||
print(gnsf["c_LO"])
|
||||
|
||||
return gnsf
|
||||
167
third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py
vendored
Normal file
167
third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py
vendored
Normal file
@@ -0,0 +1,167 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
|
||||
|
||||
from casadi import *
|
||||
from .determine_input_nonlinearity_function import determine_input_nonlinearity_function
|
||||
from .check_reformulation import check_reformulation
|
||||
|
||||
|
||||
def reformulate_with_invertible_E_mat(gnsf, model, print_info):
|
||||
## Description
|
||||
# this function checks that the necessary condition to apply the gnsf
|
||||
# structure exploiting integrator to a model, namely that the matrices E11,
|
||||
# E22 are invertible holds.
|
||||
# if this is not the case, it will make these matrices invertible and add:
|
||||
# corresponding terms, to the term C * phi, such that the obtained model is
|
||||
# still equivalent
|
||||
|
||||
# check invertibility of E11, E22 and reformulate if needed:
|
||||
ind_11 = range(gnsf["nx1"])
|
||||
ind_22 = range(gnsf["nx1"], gnsf["nx1"] + gnsf["nz1"])
|
||||
|
||||
if print_info:
|
||||
print(" ")
|
||||
print("----------------------------------------------------")
|
||||
print("checking rank of E11 and E22")
|
||||
print("----------------------------------------------------")
|
||||
## check if E11, E22 are invertible:
|
||||
z_check = False
|
||||
if gnsf["nz1"] > 0:
|
||||
z_check = (
|
||||
np.linalg.matrix_rank(gnsf["E"][np.ix_(ind_22, ind_22)]) != gnsf["nz1"]
|
||||
)
|
||||
|
||||
if (
|
||||
np.linalg.matrix_rank(gnsf["E"][np.ix_(ind_11, ind_11)]) != gnsf["nx1"]
|
||||
or z_check
|
||||
):
|
||||
# print warning (always)
|
||||
print(f"the rank of E11 or E22 is not full after the reformulation")
|
||||
print("")
|
||||
print(
|
||||
f"the script will try to reformulate the model with an invertible matrix instead"
|
||||
)
|
||||
print(
|
||||
f"NOTE: this feature is based on a heuristic, it should be used with care!!!"
|
||||
)
|
||||
|
||||
## load models
|
||||
xdot = gnsf["xdot"]
|
||||
z = gnsf["z"]
|
||||
|
||||
# # GNSF
|
||||
# get dimensions
|
||||
nx1 = gnsf["nx1"]
|
||||
x1dot = xdot[range(nx1)]
|
||||
|
||||
k = vertcat(x1dot, z)
|
||||
for i in [1, 2]:
|
||||
if i == 1:
|
||||
ind = range(gnsf["nx1"])
|
||||
else:
|
||||
ind = range(gnsf["nx1"], gnsf["nx1"] + gnsf["nz1"])
|
||||
mat = gnsf["E"][np.ix_(ind, ind)]
|
||||
import pdb
|
||||
|
||||
pdb.set_trace()
|
||||
while np.linalg.matrix_rank(mat) < len(ind):
|
||||
# import pdb; pdb.set_trace()
|
||||
if print_info:
|
||||
print(" ")
|
||||
print(f"the rank of E", str(i), str(i), " is not full")
|
||||
print(
|
||||
f"the algorithm will try to reformulate the model with an invertible matrix instead"
|
||||
)
|
||||
print(
|
||||
f"NOTE: this feature is not super stable and might need more testing!!!!!!"
|
||||
)
|
||||
for sub_max in ind:
|
||||
sub_ind = range(min(ind), sub_max)
|
||||
# regard the submatrix mat(sub_ind, sub_ind)
|
||||
sub_mat = gnsf["E"][sub_ind, sub_ind]
|
||||
if np.linalg.matrix_rank(sub_mat) < len(sub_ind):
|
||||
# reformulate the model by adding a 1 to last diagonal
|
||||
# element and changing rhs respectively.
|
||||
gnsf["E"][sub_max, sub_max] = gnsf["E"][sub_max, sub_max] + 1
|
||||
# this means adding the term 1 * k(sub_max) to the sub_max
|
||||
# row of the l.h.s
|
||||
if len(np.nonzero(gnsf["C"][sub_max, :])[0]) == 0:
|
||||
# if isempty(find(gnsf['C'](sub_max,:), 1)):
|
||||
# add new nonlinearity entry
|
||||
gnsf["C"][sub_max, gnsf["n_out"] + 1] = 1
|
||||
gnsf["phi_expr"] = vertcat(gnsf["phi_expr"], k[sub_max])
|
||||
else:
|
||||
ind_f = np.nonzero(gnsf["C"][sub_max, :])[0]
|
||||
if len(ind_f) != 1:
|
||||
raise Exception("C is assumed to be a selection matrix")
|
||||
else:
|
||||
ind_f = ind_f[0]
|
||||
# add term to corresponding nonlinearity entry
|
||||
# note: herbey we assume that C is a selection matrix,
|
||||
# i.e. gnsf['phi_expr'](ind_f) is only entering one equation
|
||||
|
||||
gnsf["phi_expr"][ind_f] = (
|
||||
gnsf["phi_expr"][ind_f]
|
||||
+ k[sub_max] / gnsf["C"][sub_max, ind_f]
|
||||
)
|
||||
gnsf = determine_input_nonlinearity_function(gnsf)
|
||||
check_reformulation(model, gnsf, print_info)
|
||||
print("successfully reformulated the model with invertible matrices E11, E22")
|
||||
else:
|
||||
if print_info:
|
||||
print(" ")
|
||||
print(
|
||||
"the rank of both E11 and E22 is naturally full after the reformulation "
|
||||
)
|
||||
print("==> model reformulation finished")
|
||||
print(" ")
|
||||
if (gnsf['nx2'] > 0 or gnsf['nz2'] > 0) and det(gnsf["E_LO"]) == 0:
|
||||
print(
|
||||
"_______________________________________________________________________________________________________"
|
||||
)
|
||||
print(" ")
|
||||
print("TAKE CARE ")
|
||||
print("E_LO matrix is NOT regular after automatic transcription!")
|
||||
print("->> this means the model CANNOT be used with the gnsf integrator")
|
||||
print(
|
||||
"->> it probably means that one entry (of xdot or z) that was moved to the linear output type system"
|
||||
)
|
||||
print(" does not appear in the model at all (zero column in E_LO)")
|
||||
print(" OR: the columns of E_LO are linearly dependent ")
|
||||
print(" ")
|
||||
print(
|
||||
" SOLUTIONs: a) go through your model & check equations the method wanted to move to LOS"
|
||||
)
|
||||
print(" b) deactivate the detect_LOS option")
|
||||
print(
|
||||
"_______________________________________________________________________________________________________"
|
||||
)
|
||||
return gnsf
|
||||
174
third_party/acados/acados_template/gnsf/structure_detection_print_summary.py
vendored
Normal file
174
third_party/acados/acados_template/gnsf/structure_detection_print_summary.py
vendored
Normal file
@@ -0,0 +1,174 @@
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
# Author: Jonathan Frey: jonathanpaulfrey(at)gmail.com
|
||||
|
||||
from casadi import n_nodes
|
||||
import numpy as np
|
||||
|
||||
|
||||
def structure_detection_print_summary(gnsf, acados_ocp):
|
||||
|
||||
## Description
|
||||
# this function prints the most important info after determining a GNSF
|
||||
# reformulation of the implicit model "initial_model" into "gnsf", which is
|
||||
# equivalent to the "reordered_model".
|
||||
model = acados_ocp.model
|
||||
# # GNSF
|
||||
# get dimensions
|
||||
nx = gnsf["nx"]
|
||||
nu = gnsf["nu"]
|
||||
nz = gnsf["nz"]
|
||||
|
||||
nx1 = gnsf["nx1"]
|
||||
nx2 = gnsf["nx2"]
|
||||
|
||||
nz1 = gnsf["nz1"]
|
||||
nz2 = gnsf["nz2"]
|
||||
|
||||
# np = gnsf['np']
|
||||
n_out = gnsf["n_out"]
|
||||
ny = gnsf["ny"]
|
||||
nuhat = gnsf["nuhat"]
|
||||
|
||||
#
|
||||
f_impl_expr = model.f_impl_expr
|
||||
n_nodes_initial = n_nodes(model.f_impl_expr)
|
||||
# x_old = model.x
|
||||
# f_impl_old = model.f_impl_expr
|
||||
|
||||
x = gnsf["x"]
|
||||
z = gnsf["z"]
|
||||
|
||||
phi_current = gnsf["phi_expr"]
|
||||
|
||||
## PRINT SUMMARY -- STRUCHTRE DETECTION
|
||||
print(" ")
|
||||
print(
|
||||
"*********************************************************************************************"
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
"****************** SUCCESS: GNSF STRUCTURE DETECTION COMPLETE !!! ***************"
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
"*********************************************************************************************"
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
f"========================= STRUCTURE DETECTION SUMMARY ===================================="
|
||||
)
|
||||
print(" ")
|
||||
print("-------- Nonlinear Static Feedback type system --------")
|
||||
print(" ")
|
||||
print(" successfully transcribed dynamic system model into GNSF structure ")
|
||||
print(" ")
|
||||
print(
|
||||
"reduced dimension of nonlinearity phi from ",
|
||||
str(nx + nz),
|
||||
" to ",
|
||||
str(gnsf["n_out"]),
|
||||
)
|
||||
print(" ")
|
||||
print(
|
||||
"reduced input dimension of nonlinearity phi from ",
|
||||
2 * nx + nz + nu,
|
||||
" to ",
|
||||
gnsf["ny"] + gnsf["nuhat"],
|
||||
)
|
||||
print(" ")
|
||||
print(f"reduced number of nodes in CasADi expression of nonlinearity phi from {n_nodes_initial} to {n_nodes(phi_current)}\n")
|
||||
print("----------- Linear Output System (LOS) ---------------")
|
||||
if nx2 + nz2 > 0:
|
||||
print(" ")
|
||||
print(f"introduced Linear Output System of size ", str(nx2 + nz2))
|
||||
print(" ")
|
||||
if nx2 > 0:
|
||||
print("consisting of the states:")
|
||||
print(" ")
|
||||
print(x[range(nx1, nx)])
|
||||
print(" ")
|
||||
if nz2 > 0:
|
||||
print("and algebraic variables:")
|
||||
print(" ")
|
||||
print(z[range(nz1, nz)])
|
||||
print(" ")
|
||||
if gnsf["purely_linear"] == 1:
|
||||
print(" ")
|
||||
print("Model is fully linear!")
|
||||
print(" ")
|
||||
if not all(gnsf["idx_perm_x"] == np.array(range(nx))):
|
||||
print(" ")
|
||||
print(
|
||||
"--------------------------------------------------------------------------------------------------"
|
||||
)
|
||||
print(
|
||||
"NOTE: permuted differential state vector x, such that x_gnsf = x(idx_perm_x) with idx_perm_x ="
|
||||
)
|
||||
print(" ")
|
||||
print(gnsf["idx_perm_x"])
|
||||
if nz != 0 and not all(gnsf["idx_perm_z"] == np.array(range(nz))):
|
||||
print(" ")
|
||||
print(
|
||||
"--------------------------------------------------------------------------------------------------"
|
||||
)
|
||||
print(
|
||||
"NOTE: permuted algebraic state vector z, such that z_gnsf = z(idx_perm_z) with idx_perm_z ="
|
||||
)
|
||||
print(" ")
|
||||
print(gnsf["idx_perm_z"])
|
||||
if not all(gnsf["idx_perm_f"] == np.array(range(nx + nz))):
|
||||
print(" ")
|
||||
print(
|
||||
"--------------------------------------------------------------------------------------------------"
|
||||
)
|
||||
print(
|
||||
"NOTE: permuted rhs expression vector f, such that f_gnsf = f(idx_perm_f) with idx_perm_f ="
|
||||
)
|
||||
print(" ")
|
||||
print(gnsf["idx_perm_f"])
|
||||
## print GNSF dimensions
|
||||
print(
|
||||
"--------------------------------------------------------------------------------------------------------"
|
||||
)
|
||||
print(" ")
|
||||
print("The dimensions of the GNSF reformulated model read as:")
|
||||
print(" ")
|
||||
# T_dim = table(nx, nu, nz, np, nx1, nz1, n_out, ny, nuhat)
|
||||
# print( T_dim )
|
||||
print(f"nx ", {nx})
|
||||
print(f"nu ", {nu})
|
||||
print(f"nz ", {nz})
|
||||
# print(f"np ", {np})
|
||||
print(f"nx1 ", {nx1})
|
||||
print(f"nz1 ", {nz1})
|
||||
print(f"n_out ", {n_out})
|
||||
print(f"ny ", {ny})
|
||||
print(f"nuhat ", {nuhat})
|
||||
44
third_party/acados/acados_template/simulink_default_opts.json
vendored
Normal file
44
third_party/acados/acados_template/simulink_default_opts.json
vendored
Normal file
@@ -0,0 +1,44 @@
|
||||
{
|
||||
"outputs": {
|
||||
"u0": 1,
|
||||
"utraj": 0,
|
||||
"xtraj": 0,
|
||||
"solver_status": 1,
|
||||
"cost_value": 0,
|
||||
"KKT_residual": 1,
|
||||
"KKT_residuals": 0,
|
||||
"x1": 1,
|
||||
"CPU_time": 1,
|
||||
"CPU_time_sim": 0,
|
||||
"CPU_time_qp": 0,
|
||||
"CPU_time_lin": 0,
|
||||
"sqp_iter": 1
|
||||
},
|
||||
"inputs": {
|
||||
"lbx_0": 1,
|
||||
"ubx_0": 1,
|
||||
"parameter_traj": 1,
|
||||
"y_ref_0": 1,
|
||||
"y_ref": 1,
|
||||
"y_ref_e": 1,
|
||||
"lbx": 1,
|
||||
"ubx": 1,
|
||||
"lbx_e": 1,
|
||||
"ubx_e": 1,
|
||||
"lbu": 1,
|
||||
"ubu": 1,
|
||||
"lg": 1,
|
||||
"ug": 1,
|
||||
"lh": 1,
|
||||
"uh": 1,
|
||||
"lh_e": 1,
|
||||
"uh_e": 1,
|
||||
"cost_W_0": 0,
|
||||
"cost_W": 0,
|
||||
"cost_W_e": 0,
|
||||
"reset_solver": 0,
|
||||
"x_init": 0,
|
||||
"u_init": 0
|
||||
},
|
||||
"samplingtime": "t0"
|
||||
}
|
||||
435
third_party/acados/acados_template/utils.py
vendored
Normal file
435
third_party/acados/acados_template/utils.py
vendored
Normal file
@@ -0,0 +1,435 @@
|
||||
# -*- coding: future_fstrings -*-
|
||||
#
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
#
|
||||
|
||||
import os, sys, json
|
||||
import urllib.request
|
||||
import shutil
|
||||
import numpy as np
|
||||
from casadi import SX, MX, DM, Function, CasadiMeta
|
||||
|
||||
ALLOWED_CASADI_VERSIONS = ('3.5.6', '3.5.5', '3.5.4', '3.5.3', '3.5.2', '3.5.1', '3.4.5', '3.4.0')
|
||||
|
||||
TERA_VERSION = "0.0.34"
|
||||
|
||||
PLATFORM2TERA = {
|
||||
"linux": "linux",
|
||||
"darwin": "osx",
|
||||
"win32": "windows"
|
||||
}
|
||||
|
||||
|
||||
def get_acados_path():
|
||||
ACADOS_PATH = os.environ.get('ACADOS_SOURCE_DIR')
|
||||
if not ACADOS_PATH:
|
||||
acados_template_path = os.path.dirname(os.path.abspath(__file__))
|
||||
acados_path = os.path.join(acados_template_path, '..','..','..')
|
||||
ACADOS_PATH = os.path.realpath(acados_path)
|
||||
msg = 'Warning: Did not find environment variable ACADOS_SOURCE_DIR, '
|
||||
msg += 'guessed ACADOS_PATH to be {}.\n'.format(ACADOS_PATH)
|
||||
msg += 'Please export ACADOS_SOURCE_DIR to avoid this warning.'
|
||||
print(msg)
|
||||
return ACADOS_PATH
|
||||
|
||||
|
||||
def get_python_interface_path():
|
||||
ACADOS_PYTHON_INTERFACE_PATH = os.environ.get('ACADOS_PYTHON_INTERFACE_PATH')
|
||||
if not ACADOS_PYTHON_INTERFACE_PATH:
|
||||
acados_path = get_acados_path()
|
||||
ACADOS_PYTHON_INTERFACE_PATH = os.path.join(acados_path, 'interfaces', 'acados_template', 'acados_template')
|
||||
return ACADOS_PYTHON_INTERFACE_PATH
|
||||
|
||||
|
||||
def get_tera_exec_path():
|
||||
TERA_PATH = os.environ.get('TERA_PATH')
|
||||
if not TERA_PATH:
|
||||
TERA_PATH = os.path.join(get_acados_path(), 'bin', 't_renderer')
|
||||
if os.name == 'nt':
|
||||
TERA_PATH += '.exe'
|
||||
return TERA_PATH
|
||||
|
||||
|
||||
def check_casadi_version():
|
||||
casadi_version = CasadiMeta.version()
|
||||
if casadi_version in ALLOWED_CASADI_VERSIONS:
|
||||
return
|
||||
else:
|
||||
msg = 'Warning: Please note that the following versions of CasADi are '
|
||||
msg += 'officially supported: {}.\n '.format(" or ".join(ALLOWED_CASADI_VERSIONS))
|
||||
msg += 'If there is an incompatibility with the CasADi generated code, '
|
||||
msg += 'please consider changing your CasADi version.\n'
|
||||
msg += 'Version {} currently in use.'.format(casadi_version)
|
||||
print(msg)
|
||||
|
||||
|
||||
def is_column(x):
|
||||
if isinstance(x, np.ndarray):
|
||||
if x.ndim == 1:
|
||||
return True
|
||||
elif x.ndim == 2 and x.shape[1] == 1:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
elif isinstance(x, (MX, SX, DM)):
|
||||
if x.shape[1] == 1:
|
||||
return True
|
||||
elif x.shape[0] == 0 and x.shape[1] == 0:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
elif x == None or x == []:
|
||||
return False
|
||||
else:
|
||||
raise Exception("is_column expects one of the following types: np.ndarray, casadi.MX, casadi.SX."
|
||||
+ " Got: " + str(type(x)))
|
||||
|
||||
|
||||
def is_empty(x):
|
||||
if isinstance(x, (MX, SX, DM)):
|
||||
return x.is_empty()
|
||||
elif isinstance(x, np.ndarray):
|
||||
if np.prod(x.shape) == 0:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
elif x == None:
|
||||
return True
|
||||
elif isinstance(x, (set, list)):
|
||||
if len(x)==0:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
else:
|
||||
raise Exception("is_empty expects one of the following types: casadi.MX, casadi.SX, "
|
||||
+ "None, numpy array empty list, set. Got: " + str(type(x)))
|
||||
|
||||
|
||||
def casadi_length(x):
|
||||
if isinstance(x, (MX, SX, DM)):
|
||||
return int(np.prod(x.shape))
|
||||
else:
|
||||
raise Exception("casadi_length expects one of the following types: casadi.MX, casadi.SX."
|
||||
+ " Got: " + str(type(x)))
|
||||
|
||||
|
||||
def make_model_consistent(model):
|
||||
x = model.x
|
||||
xdot = model.xdot
|
||||
u = model.u
|
||||
z = model.z
|
||||
p = model.p
|
||||
|
||||
if isinstance(x, MX):
|
||||
symbol = MX.sym
|
||||
elif isinstance(x, SX):
|
||||
symbol = SX.sym
|
||||
else:
|
||||
raise Exception("model.x must be casadi.SX or casadi.MX, got {}".format(type(x)))
|
||||
|
||||
if is_empty(p):
|
||||
model.p = symbol('p', 0, 0)
|
||||
|
||||
if is_empty(z):
|
||||
model.z = symbol('z', 0, 0)
|
||||
|
||||
return model
|
||||
|
||||
def get_lib_ext():
|
||||
lib_ext = '.so'
|
||||
if sys.platform == 'darwin':
|
||||
lib_ext = '.dylib'
|
||||
elif os.name == 'nt':
|
||||
lib_ext = ''
|
||||
|
||||
return lib_ext
|
||||
|
||||
def get_tera():
|
||||
tera_path = get_tera_exec_path()
|
||||
acados_path = get_acados_path()
|
||||
|
||||
if os.path.exists(tera_path) and os.access(tera_path, os.X_OK):
|
||||
return tera_path
|
||||
|
||||
repo_url = "https://github.com/acados/tera_renderer/releases"
|
||||
url = "{}/download/v{}/t_renderer-v{}-{}".format(
|
||||
repo_url, TERA_VERSION, TERA_VERSION, PLATFORM2TERA[sys.platform])
|
||||
|
||||
manual_install = 'For manual installation follow these instructions:\n'
|
||||
manual_install += '1 Download binaries from {}\n'.format(url)
|
||||
manual_install += '2 Copy them in {}/bin\n'.format(acados_path)
|
||||
manual_install += '3 Strip the version and platform from the binaries: '
|
||||
manual_install += 'as t_renderer-v0.0.34-X -> t_renderer)\n'
|
||||
manual_install += '4 Enable execution privilege on the file "t_renderer" with:\n'
|
||||
manual_install += '"chmod +x {}"\n\n'.format(tera_path)
|
||||
|
||||
msg = "\n"
|
||||
msg += 'Tera template render executable not found, '
|
||||
msg += 'while looking in path:\n{}\n'.format(tera_path)
|
||||
msg += 'In order to be able to render the templates, '
|
||||
msg += 'you need to download the tera renderer binaries from:\n'
|
||||
msg += '{}\n\n'.format(repo_url)
|
||||
msg += 'Do you wish to set up Tera renderer automatically?\n'
|
||||
msg += 'y/N? (press y to download tera or any key for manual installation)\n'
|
||||
|
||||
if input(msg) == 'y':
|
||||
print("Dowloading {}".format(url))
|
||||
with urllib.request.urlopen(url) as response, open(tera_path, 'wb') as out_file:
|
||||
shutil.copyfileobj(response, out_file)
|
||||
print("Successfully downloaded t_renderer.")
|
||||
os.chmod(tera_path, 0o755)
|
||||
return tera_path
|
||||
|
||||
msg_cancel = "\nYou cancelled automatic download.\n\n"
|
||||
msg_cancel += manual_install
|
||||
msg_cancel += "Once installed re-run your script.\n\n"
|
||||
print(msg_cancel)
|
||||
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
def render_template(in_file, out_file, output_dir, json_path, template_glob=None):
|
||||
|
||||
acados_path = os.path.dirname(os.path.abspath(__file__))
|
||||
if template_glob is None:
|
||||
template_glob = os.path.join(acados_path, 'c_templates_tera', '**', '*')
|
||||
cwd = os.getcwd()
|
||||
|
||||
if not os.path.exists(output_dir):
|
||||
os.makedirs(output_dir)
|
||||
os.chdir(output_dir)
|
||||
|
||||
tera_path = get_tera()
|
||||
|
||||
# call tera as system cmd
|
||||
os_cmd = f"{tera_path} '{template_glob}' '{in_file}' '{json_path}' '{out_file}'"
|
||||
# Windows cmd.exe can not cope with '...', so use "..." instead:
|
||||
if os.name == 'nt':
|
||||
os_cmd = os_cmd.replace('\'', '\"')
|
||||
|
||||
status = os.system(os_cmd)
|
||||
if (status != 0):
|
||||
raise Exception(f'Rendering of {in_file} failed!\n\nAttempted to execute OS command:\n{os_cmd}\n\n')
|
||||
|
||||
os.chdir(cwd)
|
||||
|
||||
|
||||
## Conversion functions
|
||||
def make_object_json_dumpable(input):
|
||||
if isinstance(input, (np.ndarray)):
|
||||
return input.tolist()
|
||||
elif isinstance(input, (SX)):
|
||||
return input.serialize()
|
||||
elif isinstance(input, (MX)):
|
||||
# NOTE: MX expressions can not be serialized, only Functions.
|
||||
return input.__str__()
|
||||
elif isinstance(input, (DM)):
|
||||
return input.full()
|
||||
else:
|
||||
raise TypeError(f"Cannot make input of type {type(input)} dumpable.")
|
||||
|
||||
|
||||
def format_class_dict(d):
|
||||
"""
|
||||
removes the __ artifact from class to dict conversion
|
||||
"""
|
||||
out = {}
|
||||
for k, v in d.items():
|
||||
if isinstance(v, dict):
|
||||
v = format_class_dict(v)
|
||||
|
||||
out_key = k.split('__', 1)[-1]
|
||||
out[k.replace(k, out_key)] = v
|
||||
return out
|
||||
|
||||
|
||||
def get_ocp_nlp_layout() -> dict:
|
||||
python_interface_path = get_python_interface_path()
|
||||
abs_path = os.path.join(python_interface_path, 'acados_layout.json')
|
||||
with open(abs_path, 'r') as f:
|
||||
ocp_nlp_layout = json.load(f)
|
||||
return ocp_nlp_layout
|
||||
|
||||
|
||||
def get_default_simulink_opts() -> dict:
|
||||
python_interface_path = get_python_interface_path()
|
||||
abs_path = os.path.join(python_interface_path, 'simulink_default_opts.json')
|
||||
with open(abs_path, 'r') as f:
|
||||
simulink_opts = json.load(f)
|
||||
return simulink_opts
|
||||
|
||||
|
||||
def J_to_idx(J):
|
||||
nrows = J.shape[0]
|
||||
idx = np.zeros((nrows, ))
|
||||
for i in range(nrows):
|
||||
this_idx = np.nonzero(J[i,:])[0]
|
||||
if len(this_idx) != 1:
|
||||
raise Exception('Invalid J matrix structure detected, ' \
|
||||
'must contain one nonzero element per row.')
|
||||
if this_idx.size > 0 and J[i,this_idx[0]] != 1:
|
||||
raise Exception('J matrices can only contain 1s.')
|
||||
idx[i] = this_idx[0]
|
||||
return idx
|
||||
|
||||
|
||||
def J_to_idx_slack(J):
|
||||
nrows = J.shape[0]
|
||||
ncol = J.shape[1]
|
||||
idx = np.zeros((ncol, ))
|
||||
i_idx = 0
|
||||
for i in range(nrows):
|
||||
this_idx = np.nonzero(J[i,:])[0]
|
||||
if len(this_idx) == 1:
|
||||
idx[i_idx] = i
|
||||
i_idx = i_idx + 1
|
||||
elif len(this_idx) > 1:
|
||||
raise Exception('J_to_idx_slack: Invalid J matrix. ' \
|
||||
'Found more than one nonzero in row ' + str(i))
|
||||
if this_idx.size > 0 and J[i,this_idx[0]] != 1:
|
||||
raise Exception('J_to_idx_slack: J matrices can only contain 1s, ' \
|
||||
'got J(' + str(i) + ', ' + str(this_idx[0]) + ') = ' + str(J[i,this_idx[0]]) )
|
||||
if not i_idx == ncol:
|
||||
raise Exception('J_to_idx_slack: J must contain a 1 in every column!')
|
||||
return idx
|
||||
|
||||
|
||||
def acados_dae_model_json_dump(model):
|
||||
|
||||
# load model
|
||||
x = model.x
|
||||
xdot = model.xdot
|
||||
u = model.u
|
||||
z = model.z
|
||||
p = model.p
|
||||
|
||||
f_impl = model.f_impl_expr
|
||||
model_name = model.name
|
||||
|
||||
# create struct with impl_dae_fun, casadi_version
|
||||
fun_name = model_name + '_impl_dae_fun'
|
||||
impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl])
|
||||
|
||||
casadi_version = CasadiMeta.version()
|
||||
str_impl_dae_fun = impl_dae_fun.serialize()
|
||||
|
||||
dae_dict = {"str_impl_dae_fun": str_impl_dae_fun, "casadi_version": casadi_version}
|
||||
|
||||
# dump
|
||||
json_file = model_name + '_acados_dae.json'
|
||||
with open(json_file, 'w') as f:
|
||||
json.dump(dae_dict, f, default=make_object_json_dumpable, indent=4, sort_keys=True)
|
||||
print("dumped ", model_name, " dae to file:", json_file, "\n")
|
||||
|
||||
|
||||
def set_up_imported_gnsf_model(acados_ocp):
|
||||
|
||||
gnsf = acados_ocp.gnsf_model
|
||||
|
||||
# check CasADi version
|
||||
# dump_casadi_version = gnsf['casadi_version']
|
||||
# casadi_version = CasadiMeta.version()
|
||||
|
||||
# if not casadi_version == dump_casadi_version:
|
||||
# print("WARNING: GNSF model was dumped with another CasADi version.\n"
|
||||
# + "This might yield errors. Please use the same version for compatibility, serialize version: "
|
||||
# + dump_casadi_version + " current Python CasADi verison: " + casadi_version)
|
||||
# input("Press any key to attempt to continue...")
|
||||
|
||||
# load model
|
||||
phi_fun = Function.deserialize(gnsf['phi_fun'])
|
||||
phi_fun_jac_y = Function.deserialize(gnsf['phi_fun_jac_y'])
|
||||
phi_jac_y_uhat = Function.deserialize(gnsf['phi_jac_y_uhat'])
|
||||
get_matrices_fun = Function.deserialize(gnsf['get_matrices_fun'])
|
||||
|
||||
# obtain gnsf dimensions
|
||||
size_gnsf_A = get_matrices_fun.size_out(0)
|
||||
acados_ocp.dims.gnsf_nx1 = size_gnsf_A[1]
|
||||
acados_ocp.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
|
||||
acados_ocp.dims.gnsf_nuhat = max(phi_fun.size_in(1))
|
||||
acados_ocp.dims.gnsf_ny = max(phi_fun.size_in(0))
|
||||
acados_ocp.dims.gnsf_nout = max(phi_fun.size_out(0))
|
||||
|
||||
# save gnsf functions in model
|
||||
acados_ocp.model.phi_fun = phi_fun
|
||||
acados_ocp.model.phi_fun_jac_y = phi_fun_jac_y
|
||||
acados_ocp.model.phi_jac_y_uhat = phi_jac_y_uhat
|
||||
acados_ocp.model.get_matrices_fun = get_matrices_fun
|
||||
|
||||
# get_matrices_fun = Function([model_name,'_gnsf_get_matrices_fun'], {dummy},...
|
||||
# {A, B, C, E, L_x, L_xdot, L_z, L_u, A_LO, c, E_LO, B_LO,...
|
||||
# nontrivial_f_LO, purely_linear, ipiv_x, ipiv_z, c_LO});
|
||||
get_matrices_out = get_matrices_fun(0)
|
||||
acados_ocp.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12])
|
||||
acados_ocp.model.gnsf['purely_linear'] = int(get_matrices_out[13])
|
||||
|
||||
if "f_lo_fun_jac_x1k1uz" in gnsf:
|
||||
f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz'])
|
||||
acados_ocp.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz
|
||||
else:
|
||||
dummy_var_x1 = SX.sym('dummy_var_x1', acados_ocp.dims.gnsf_nx1)
|
||||
dummy_var_x1dot = SX.sym('dummy_var_x1dot', acados_ocp.dims.gnsf_nx1)
|
||||
dummy_var_z1 = SX.sym('dummy_var_z1', acados_ocp.dims.gnsf_nz1)
|
||||
dummy_var_u = SX.sym('dummy_var_z1', acados_ocp.dims.nu)
|
||||
dummy_var_p = SX.sym('dummy_var_z1', acados_ocp.dims.np)
|
||||
empty_var = SX.sym('empty_var', 0, 0)
|
||||
|
||||
empty_fun = Function('empty_fun', \
|
||||
[dummy_var_x1, dummy_var_x1dot, dummy_var_z1, dummy_var_u, dummy_var_p],
|
||||
[empty_var])
|
||||
acados_ocp.model.f_lo_fun_jac_x1k1uz = empty_fun
|
||||
|
||||
del acados_ocp.gnsf_model
|
||||
|
||||
|
||||
def idx_perm_to_ipiv(idx_perm):
|
||||
n = len(idx_perm)
|
||||
vec = list(range(n))
|
||||
ipiv = np.zeros(n)
|
||||
|
||||
print(n, idx_perm)
|
||||
# import pdb; pdb.set_trace()
|
||||
for ii in range(n):
|
||||
idx0 = idx_perm[ii]
|
||||
for jj in range(ii,n):
|
||||
if vec[jj]==idx0:
|
||||
idx1 = jj
|
||||
break
|
||||
tmp = vec[ii]
|
||||
vec[ii] = vec[idx1]
|
||||
vec[idx1] = tmp
|
||||
ipiv[ii] = idx1
|
||||
|
||||
ipiv = ipiv-1 # C 0-based indexing
|
||||
return ipiv
|
||||
|
||||
|
||||
def print_casadi_expression(f):
|
||||
for ii in range(casadi_length(f)):
|
||||
print(f[ii,:])
|
||||
78
third_party/acados/acados_template/zoro_description.py
vendored
Normal file
78
third_party/acados/acados_template/zoro_description.py
vendored
Normal file
@@ -0,0 +1,78 @@
|
||||
# Copyright (c) The acados authors.
|
||||
#
|
||||
# This file is part of acados.
|
||||
#
|
||||
# The 2-Clause BSD License
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright notice,
|
||||
# this list of conditions and the following disclaimer.
|
||||
#
|
||||
# 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
# this list of conditions and the following disclaimer in the documentation
|
||||
# and/or other materials provided with the distribution.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.;
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass
|
||||
class ZoroDescription:
|
||||
"""
|
||||
Zero-Order Robust Optimization scheme.
|
||||
|
||||
For advanced users.
|
||||
"""
|
||||
backoff_scaling_gamma: float = 1.0
|
||||
fdbk_K_mat: np.ndarray = None
|
||||
unc_jac_G_mat: np.ndarray = None # default: an identity matrix
|
||||
P0_mat: np.ndarray = None
|
||||
W_mat: np.ndarray = None
|
||||
idx_lbx_t: list = field(default_factory=list)
|
||||
idx_ubx_t: list = field(default_factory=list)
|
||||
idx_lbx_e_t: list = field(default_factory=list)
|
||||
idx_ubx_e_t: list = field(default_factory=list)
|
||||
idx_lbu_t: list = field(default_factory=list)
|
||||
idx_ubu_t: list = field(default_factory=list)
|
||||
idx_lg_t: list = field(default_factory=list)
|
||||
idx_ug_t: list = field(default_factory=list)
|
||||
idx_lg_e_t: list = field(default_factory=list)
|
||||
idx_ug_e_t: list = field(default_factory=list)
|
||||
idx_lh_t: list = field(default_factory=list)
|
||||
idx_uh_t: list = field(default_factory=list)
|
||||
idx_lh_e_t: list = field(default_factory=list)
|
||||
idx_uh_e_t: list = field(default_factory=list)
|
||||
|
||||
def process_zoro_description(zoro_description: ZoroDescription):
|
||||
zoro_description.nw, _ = zoro_description.W_mat.shape
|
||||
if zoro_description.unc_jac_G_mat is None:
|
||||
zoro_description.unc_jac_G_mat = np.eye(zoro_description.nw)
|
||||
zoro_description.nlbx_t = len(zoro_description.idx_lbx_t)
|
||||
zoro_description.nubx_t = len(zoro_description.idx_ubx_t)
|
||||
zoro_description.nlbx_e_t = len(zoro_description.idx_lbx_e_t)
|
||||
zoro_description.nubx_e_t = len(zoro_description.idx_ubx_e_t)
|
||||
zoro_description.nlbu_t = len(zoro_description.idx_lbu_t)
|
||||
zoro_description.nubu_t = len(zoro_description.idx_ubu_t)
|
||||
zoro_description.nlg_t = len(zoro_description.idx_lg_t)
|
||||
zoro_description.nug_t = len(zoro_description.idx_ug_t)
|
||||
zoro_description.nlg_e_t = len(zoro_description.idx_lg_e_t)
|
||||
zoro_description.nug_e_t = len(zoro_description.idx_ug_e_t)
|
||||
zoro_description.nlh_t = len(zoro_description.idx_lh_t)
|
||||
zoro_description.nuh_t = len(zoro_description.idx_uh_t)
|
||||
zoro_description.nlh_e_t = len(zoro_description.idx_lh_e_t)
|
||||
zoro_description.nuh_e_t = len(zoro_description.idx_uh_e_t)
|
||||
return zoro_description.__dict__
|
||||
147
third_party/acados/include/acados/dense_qp/dense_qp_common.h
vendored
Normal file
147
third_party/acados/include/acados/dense_qp/dense_qp_common.h
vendored
Normal file
@@ -0,0 +1,147 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_DENSE_QP_DENSE_QP_COMMON_H_
|
||||
#define ACADOS_DENSE_QP_DENSE_QP_COMMON_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// hpipm
|
||||
#include "hpipm/include/hpipm_d_dense_qp.h"
|
||||
#include "hpipm/include/hpipm_d_dense_qp_res.h"
|
||||
#include "hpipm/include/hpipm_d_dense_qp_sol.h"
|
||||
// acados
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
typedef struct d_dense_qp_dim dense_qp_dims;
|
||||
typedef struct d_dense_qp dense_qp_in;
|
||||
typedef struct d_dense_qp_sol dense_qp_out;
|
||||
typedef struct d_dense_qp_res dense_qp_res;
|
||||
typedef struct d_dense_qp_res_ws dense_qp_res_ws;
|
||||
|
||||
|
||||
|
||||
#ifndef QP_SOLVER_CONFIG_
|
||||
#define QP_SOLVER_CONFIG_
|
||||
typedef struct
|
||||
{
|
||||
void (*dims_set)(void *config_, void *dims_, const char *field, const int* value);
|
||||
acados_size_t (*opts_calculate_size)(void *config, void *dims);
|
||||
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
|
||||
void (*opts_initialize_default)(void *config, void *dims, void *args);
|
||||
void (*opts_update)(void *config, void *dims, void *args);
|
||||
void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
|
||||
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *args);
|
||||
void *(*memory_assign)(void *config, void *dims, void *args, void *raw_memory);
|
||||
void (*memory_get)(void *config_, void *mem_, const char *field, void* value);
|
||||
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *args);
|
||||
int (*evaluate)(void *config, void *qp_in, void *qp_out, void *args, void *mem, void *work);
|
||||
void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
|
||||
} qp_solver_config;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#ifndef QP_INFO_
|
||||
#define QP_INFO_
|
||||
typedef struct
|
||||
{
|
||||
double solve_QP_time;
|
||||
double condensing_time;
|
||||
double interface_time;
|
||||
double total_time;
|
||||
int num_iter;
|
||||
int t_computed;
|
||||
} qp_info;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/* config */
|
||||
//
|
||||
acados_size_t dense_qp_solver_config_calculate_size();
|
||||
//
|
||||
qp_solver_config *dense_qp_solver_config_assign(void *raw_memory);
|
||||
|
||||
/* dims */
|
||||
//
|
||||
acados_size_t dense_qp_dims_calculate_size();
|
||||
//
|
||||
dense_qp_dims *dense_qp_dims_assign(void *raw_memory);
|
||||
//
|
||||
void dense_qp_dims_set(void *config_, void *dims_, const char *field, const int* value);
|
||||
//
|
||||
|
||||
/* in */
|
||||
//
|
||||
acados_size_t dense_qp_in_calculate_size(dense_qp_dims *dims);
|
||||
//
|
||||
dense_qp_in *dense_qp_in_assign(dense_qp_dims *dims, void *raw_memory);
|
||||
|
||||
/* out */
|
||||
//
|
||||
acados_size_t dense_qp_out_calculate_size(dense_qp_dims *dims);
|
||||
//
|
||||
dense_qp_out *dense_qp_out_assign(dense_qp_dims *dims, void *raw_memory);
|
||||
//
|
||||
void dense_qp_out_get(dense_qp_out *out, const char *field, void *value);
|
||||
|
||||
/* res */
|
||||
//
|
||||
acados_size_t dense_qp_res_calculate_size(dense_qp_dims *dims);
|
||||
//
|
||||
dense_qp_res *dense_qp_res_assign(dense_qp_dims *dims, void *raw_memory);
|
||||
//
|
||||
acados_size_t dense_qp_res_workspace_calculate_size(dense_qp_dims *dims);
|
||||
//
|
||||
dense_qp_res_ws *dense_qp_res_workspace_assign(dense_qp_dims *dims, void *raw_memory);
|
||||
//
|
||||
void dense_qp_compute_t(dense_qp_in *qp_in, dense_qp_out *qp_out);
|
||||
//
|
||||
void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res *qp_res, dense_qp_res_ws *res_ws);
|
||||
//
|
||||
void dense_qp_res_compute_nrm_inf(dense_qp_res *qp_res, double res[4]);
|
||||
|
||||
/* misc */
|
||||
//
|
||||
void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out);
|
||||
//
|
||||
void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out);
|
||||
//
|
||||
void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out *out);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_DENSE_QP_DENSE_QP_COMMON_H_
|
||||
110
third_party/acados/include/acados/dense_qp/dense_qp_daqp.h
vendored
Normal file
110
third_party/acados/include/acados/dense_qp/dense_qp_daqp.h
vendored
Normal file
@@ -0,0 +1,110 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_DENSE_QP_DENSE_QP_DAQP_H_
|
||||
#define ACADOS_DENSE_QP_DENSE_QP_DAQP_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// daqp
|
||||
#include "daqp/include/types.h"
|
||||
|
||||
// acados
|
||||
#include "acados/dense_qp/dense_qp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
typedef struct dense_qp_daqp_opts_
|
||||
{
|
||||
DAQPSettings* daqp_opts;
|
||||
int warm_start;
|
||||
} dense_qp_daqp_opts;
|
||||
|
||||
|
||||
typedef struct dense_qp_daqp_memory_
|
||||
{
|
||||
double* lb_tmp;
|
||||
double* ub_tmp;
|
||||
int* idxb;
|
||||
int* idxv_to_idxb;
|
||||
int* idxs;
|
||||
int* idxdaqp_to_idxs;
|
||||
|
||||
double* Zl;
|
||||
double* Zu;
|
||||
double* zl;
|
||||
double* zu;
|
||||
double* d_ls;
|
||||
double* d_us;
|
||||
|
||||
double time_qp_solver_call;
|
||||
int iter;
|
||||
DAQPWorkspace * daqp_work;
|
||||
|
||||
} dense_qp_daqp_memory;
|
||||
|
||||
// opts
|
||||
acados_size_t dense_qp_daqp_opts_calculate_size(void *config, dense_qp_dims *dims);
|
||||
//
|
||||
void *dense_qp_daqp_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory);
|
||||
//
|
||||
void dense_qp_daqp_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void dense_qp_daqp_opts_update(void *config, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
// memory
|
||||
acados_size_t dense_qp_daqp_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void *dense_qp_daqp_workspace_assign(void *config, dense_qp_dims *dims, void *raw_memory);
|
||||
//
|
||||
acados_size_t dense_qp_daqp_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void *dense_qp_daqp_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory);
|
||||
//
|
||||
// functions
|
||||
int dense_qp_daqp(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void dense_qp_daqp_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void dense_qp_daqp_config_initialize_default(void *config_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_DENSE_QP_DENSE_QP_DAQP_H_
|
||||
91
third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h
vendored
Normal file
91
third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h
vendored
Normal file
@@ -0,0 +1,91 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_
|
||||
#define ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// hpipm
|
||||
#include "hpipm/include/hpipm_d_dense_qp.h"
|
||||
#include "hpipm/include/hpipm_d_dense_qp_ipm.h"
|
||||
#include "hpipm/include/hpipm_d_dense_qp_sol.h"
|
||||
// acados
|
||||
#include "acados/dense_qp/dense_qp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
typedef struct dense_qp_hpipm_opts_
|
||||
{
|
||||
struct d_dense_qp_ipm_arg *hpipm_opts;
|
||||
} dense_qp_hpipm_opts;
|
||||
|
||||
|
||||
|
||||
typedef struct dense_qp_hpipm_memory_
|
||||
{
|
||||
struct d_dense_qp_ipm_ws *hpipm_workspace;
|
||||
double time_qp_solver_call;
|
||||
int iter;
|
||||
|
||||
} dense_qp_hpipm_memory;
|
||||
|
||||
|
||||
|
||||
//
|
||||
acados_size_t dense_qp_hpipm_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *dense_qp_hpipm_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void dense_qp_hpipm_opts_initialize_default(void *config, void *dims, void *opts_);
|
||||
//
|
||||
void dense_qp_hpipm_opts_update(void *config, void *dims, void *opts_);
|
||||
//
|
||||
acados_size_t dense_qp_hpipm_calculate_memory_size(void *dims, void *opts_);
|
||||
//
|
||||
void *dense_qp_hpipm_assign_memory(void *dims, void *opts_, void *raw_memory);
|
||||
//
|
||||
acados_size_t dense_qp_hpipm_calculate_workspace_size(void *dims, void *opts_);
|
||||
//
|
||||
int dense_qp_hpipm(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void dense_qp_hpipm_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void dense_qp_hpipm_config_initialize_default(void *config_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_
|
||||
125
third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h
vendored
Normal file
125
third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h
vendored
Normal file
@@ -0,0 +1,125 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_DENSE_QP_DENSE_QP_OOQP_H_
|
||||
#define ACADOS_DENSE_QP_DENSE_QP_OOQP_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "acados/dense_qp/dense_qp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
enum dense_qp_ooqp_termination_code
|
||||
{
|
||||
DENSE_SUCCESSFUL_TERMINATION = 0,
|
||||
DENSE_NOT_FINISHED,
|
||||
DENSE_MAX_ITS_EXCEEDED,
|
||||
DENSE_INFEASIBLE,
|
||||
DENSE_UNKNOWN
|
||||
};
|
||||
|
||||
typedef struct dense_qp_ooqp_opts_
|
||||
{
|
||||
int printLevel;
|
||||
int useDiagonalWeights; // TODO(dimitris): implement option
|
||||
int fixHessian;
|
||||
int fixDynamics;
|
||||
int fixInequalities;
|
||||
} dense_qp_ooqp_opts;
|
||||
|
||||
typedef struct dense_qp_ooqp_workspace_
|
||||
{
|
||||
double *x;
|
||||
double *gamma;
|
||||
double *phi;
|
||||
double *y;
|
||||
double *z;
|
||||
double *lambda;
|
||||
double *pi;
|
||||
double objectiveValue;
|
||||
} dense_qp_ooqp_workspace;
|
||||
|
||||
typedef struct dense_qp_ooqp_memory_
|
||||
{
|
||||
int firstRun;
|
||||
int nx;
|
||||
int my;
|
||||
int mz;
|
||||
double *c;
|
||||
double *dQ;
|
||||
double *xlow;
|
||||
char *ixlow;
|
||||
double *xupp;
|
||||
char *ixupp;
|
||||
double *dA;
|
||||
double *bA;
|
||||
double *dC;
|
||||
double *clow;
|
||||
char *iclow;
|
||||
double *cupp;
|
||||
char *icupp;
|
||||
double time_qp_solver_call;
|
||||
int iter;
|
||||
|
||||
} dense_qp_ooqp_memory;
|
||||
|
||||
//
|
||||
acados_size_t dense_qp_ooqp_opts_calculate_size(void *config_, dense_qp_dims *dims);
|
||||
//
|
||||
void *dense_qp_ooqp_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory);
|
||||
//
|
||||
void dense_qp_ooqp_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void dense_qp_ooqp_opts_update(void *config_, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
acados_size_t dense_qp_ooqp_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void *dense_qp_ooqp_memory_assign(void *config_, dense_qp_dims *dims, void *opts_,
|
||||
void *raw_memory);
|
||||
//
|
||||
acados_size_t dense_qp_ooqp_workspace_calculate_size(void *config_, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
int dense_qp_ooqp(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_,
|
||||
void *memory_, void *work_);
|
||||
//
|
||||
void dense_qp_ooqp_destroy(void *mem_, void *work);
|
||||
//
|
||||
void dense_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void dense_qp_ooqp_config_initialize_default(void *config_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_DENSE_QP_DENSE_QP_OOQP_H_
|
||||
124
third_party/acados/include/acados/dense_qp/dense_qp_qore.h
vendored
Normal file
124
third_party/acados/include/acados/dense_qp/dense_qp_qore.h
vendored
Normal file
@@ -0,0 +1,124 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_DENSE_QP_DENSE_QP_QORE_H_
|
||||
#define ACADOS_DENSE_QP_DENSE_QP_QORE_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// qore
|
||||
#include "qore/QPSOLVER_DENSE/include/qpsolver_dense.h"
|
||||
// acados
|
||||
#include "acados/dense_qp/dense_qp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
typedef struct dense_qp_qore_opts_
|
||||
{
|
||||
int nsmax; // maximum size of Schur complement
|
||||
int print_freq; // print frequency,
|
||||
// prtfreq < 0: disable printing;
|
||||
// prtfreq == 0: print on each call and include working set changes;
|
||||
// prtfreq > 0: print on every prtfreq seconds, but do not include working set
|
||||
// changes;
|
||||
int warm_start; // warm start with updated matrices H and C
|
||||
int warm_strategy; // 0: ramp-up from zero homotopy; 1: setup homotopy from the previous
|
||||
// solution
|
||||
int hot_start; // hot start with unchanged matrices H and C
|
||||
int max_iter; // maximum number of iterations
|
||||
int compute_t; // compute t in qp_out (to have correct residuals in NLP)
|
||||
} dense_qp_qore_opts;
|
||||
|
||||
typedef struct dense_qp_qore_memory_
|
||||
{
|
||||
double *H;
|
||||
double *HH;
|
||||
double *g;
|
||||
double *gg;
|
||||
double *Zl;
|
||||
double *Zu;
|
||||
double *zl;
|
||||
double *zu;
|
||||
double *A;
|
||||
double *b;
|
||||
double *C;
|
||||
double *CC;
|
||||
double *Ct;
|
||||
double *CCt;
|
||||
double *d_lb0;
|
||||
double *d_ub0;
|
||||
double *d_lb;
|
||||
double *d_ub;
|
||||
double *d_lg;
|
||||
double *d_ug;
|
||||
double *d_ls;
|
||||
double *d_us;
|
||||
double *lb;
|
||||
double *ub;
|
||||
int *idxb;
|
||||
int *idxb_stacked;
|
||||
int *idxs;
|
||||
double *prim_sol;
|
||||
double *dual_sol;
|
||||
QoreProblemDense *QP;
|
||||
int num_iter;
|
||||
dense_qp_in *qp_stacked;
|
||||
double time_qp_solver_call;
|
||||
int iter;
|
||||
|
||||
} dense_qp_qore_memory;
|
||||
|
||||
acados_size_t dense_qp_qore_opts_calculate_size(void *config, dense_qp_dims *dims);
|
||||
//
|
||||
void *dense_qp_qore_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory);
|
||||
//
|
||||
void dense_qp_qore_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void dense_qp_qore_opts_update(void *config, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
acados_size_t dense_qp_qore_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void *dense_qp_qore_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory);
|
||||
//
|
||||
acados_size_t dense_qp_qore_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
int dense_qp_qore(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void dense_qp_qore_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void dense_qp_qore_config_initialize_default(void *config);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_DENSE_QP_DENSE_QP_QORE_H_
|
||||
126
third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h
vendored
Normal file
126
third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h
vendored
Normal file
@@ -0,0 +1,126 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_
|
||||
#define ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// acados
|
||||
#include "acados/dense_qp/dense_qp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
typedef struct dense_qp_qpoases_opts_
|
||||
{
|
||||
double max_cputime; // maximum cpu time in seconds
|
||||
int max_nwsr; // maximum number of working set recalculations
|
||||
int warm_start; // warm start with dual_sol in memory
|
||||
int use_precomputed_cholesky;
|
||||
int hotstart; // this option requires constant data matrices! (eg linear MPC, inexact schemes
|
||||
// with frozen sensitivities)
|
||||
int set_acado_opts; // use same options as in acado code generation
|
||||
int compute_t; // compute t in qp_out (to have correct residuals in NLP)
|
||||
double tolerance; // terminationTolerance
|
||||
} dense_qp_qpoases_opts;
|
||||
|
||||
typedef struct dense_qp_qpoases_memory_
|
||||
{
|
||||
double *H;
|
||||
double *HH;
|
||||
double *R;
|
||||
double *g;
|
||||
double *gg;
|
||||
double *Zl;
|
||||
double *Zu;
|
||||
double *zl;
|
||||
double *zu;
|
||||
double *A;
|
||||
double *b;
|
||||
double *d_lb0;
|
||||
double *d_ub0;
|
||||
double *d_lb;
|
||||
double *d_ub;
|
||||
double *C;
|
||||
double *CC;
|
||||
double *d_lg0;
|
||||
double *d_ug0;
|
||||
double *d_lg;
|
||||
double *d_ug;
|
||||
double *d_ls;
|
||||
double *d_us;
|
||||
int *idxb;
|
||||
int *idxb_stacked;
|
||||
int *idxs;
|
||||
double *prim_sol;
|
||||
double *dual_sol;
|
||||
void *QPB; // NOTE(giaf): cast to QProblemB to use
|
||||
void *QP; // NOTE(giaf): cast to QProblem to use
|
||||
double cputime; // cputime of qpoases
|
||||
int nwsr; // performed number of working set recalculations
|
||||
int first_it; // to be used with hotstart
|
||||
dense_qp_in *qp_stacked;
|
||||
double time_qp_solver_call; // equal to cputime
|
||||
int iter;
|
||||
|
||||
} dense_qp_qpoases_memory;
|
||||
|
||||
acados_size_t dense_qp_qpoases_opts_calculate_size(void *config, dense_qp_dims *dims);
|
||||
//
|
||||
void *dense_qp_qpoases_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory);
|
||||
//
|
||||
void dense_qp_qpoases_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void dense_qp_qpoases_opts_update(void *config, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
acados_size_t dense_qp_qpoases__memorycalculate_size(void *config, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void *dense_qp_qpoases_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory);
|
||||
//
|
||||
acados_size_t dense_qp_qpoases_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
|
||||
//
|
||||
int dense_qp_qpoases(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void dense_qp_qpoases_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void dense_qp_qpoases_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void dense_qp_qpoases_config_initialize_default(void *config_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_
|
||||
446
third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h
vendored
Normal file
446
third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h
vendored
Normal file
@@ -0,0 +1,446 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/// \defgroup ocp_nlp ocp_nlp
|
||||
/// @{
|
||||
/// @}
|
||||
|
||||
/// \defgroup ocp_nlp_solver ocp_nlp_solver
|
||||
/// @{
|
||||
/// @}
|
||||
|
||||
/// \ingroup ocp_nlp
|
||||
/// @{
|
||||
|
||||
/// \ingroup ocp_nlp_solver
|
||||
/// @{
|
||||
|
||||
/// \defgroup ocp_nlp_common ocp_nlp_common
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
|
||||
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
|
||||
#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
|
||||
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
#include "acados/ocp_qp/ocp_qp_xcond_solver.h"
|
||||
#include "acados/sim/sim_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* config
|
||||
************************************************/
|
||||
|
||||
typedef struct ocp_nlp_config
|
||||
{
|
||||
int N; // number of stages
|
||||
|
||||
// solver-specific implementations of memory management functions
|
||||
acados_size_t (*opts_calculate_size)(void *config, void *dims);
|
||||
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
|
||||
void (*opts_initialize_default)(void *config, void *dims, void *opts_);
|
||||
void (*opts_update)(void *config, void *dims, void *opts_);
|
||||
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts_);
|
||||
void *(*memory_assign)(void *config, void *dims, void *opts_, void *raw_memory);
|
||||
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts_);
|
||||
void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
|
||||
void (*opts_set_at_stage)(void *config_, void *opts_, size_t stage, const char *field, void* value);
|
||||
// evaluate solver // TODO rename into solve
|
||||
int (*evaluate)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
|
||||
void (*eval_param_sens)(void *config, void *dims, void *opts_, void *mem, void *work,
|
||||
char *field, int stage, int index, void *sens_nlp_out);
|
||||
// prepare memory
|
||||
int (*precompute)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
|
||||
void (*memory_reset_qp_solver)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
|
||||
// initalize this struct with default values
|
||||
void (*config_initialize_default)(void *config);
|
||||
// general getter
|
||||
void (*get)(void *config_, void *dims, void *mem_, const char *field, void *return_value_);
|
||||
void (*opts_get)(void *config_, void *dims, void *opts_, const char *field, void *return_value_);
|
||||
void (*work_get)(void *config_, void *dims, void *work_, const char *field, void *return_value_);
|
||||
// config structs of submodules
|
||||
ocp_qp_xcond_solver_config *qp_solver; // TODO rename xcond_solver
|
||||
ocp_nlp_dynamics_config **dynamics;
|
||||
ocp_nlp_cost_config **cost;
|
||||
ocp_nlp_constraints_config **constraints;
|
||||
ocp_nlp_reg_config *regularize;
|
||||
|
||||
} ocp_nlp_config;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_config_calculate_size(int N);
|
||||
//
|
||||
ocp_nlp_config *ocp_nlp_config_assign(int N, void *raw_memory);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* dims
|
||||
************************************************/
|
||||
|
||||
/// Structure to store dimensions/number of variables.
|
||||
typedef struct ocp_nlp_dims
|
||||
{
|
||||
void **cost;
|
||||
void **dynamics;
|
||||
void **constraints;
|
||||
ocp_qp_xcond_solver_dims *qp_solver; // xcond solver instead ??
|
||||
ocp_nlp_reg_dims *regularize;
|
||||
|
||||
int *nv; // number of primal variables (states+controls+slacks)
|
||||
int *nx; // number of differential states
|
||||
int *nu; // number of inputs
|
||||
int *ni; // number of two-sided inequality constraints: nb+ng+nh+ns
|
||||
int *nz; // number of algebraic variables
|
||||
int *ns; // number of slack variables
|
||||
int N; // number of shooting nodes
|
||||
|
||||
void *raw_memory; // Pointer to allocated memory, to be used for freeing
|
||||
} ocp_nlp_dims;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_dims_calculate_size(void *config);
|
||||
//
|
||||
ocp_nlp_dims *ocp_nlp_dims_assign(void *config, void *raw_memory);
|
||||
|
||||
/// Sets the dimension of optimization variables
|
||||
/// (states, constrols, algebraic variables, slack variables).
|
||||
///
|
||||
/// \param config_ The configuration struct.
|
||||
/// \param dims_ The dimension struct.
|
||||
/// \param field The type of optimization variables, either nx, nu, nz, or ns.
|
||||
/// \param value_array Number of variables for each stage.
|
||||
void ocp_nlp_dims_set_opt_vars(void *config_, void *dims_,
|
||||
const char *field, const void* value_array);
|
||||
|
||||
/// Sets the dimensions of constraints functions for a stage
|
||||
/// (bounds on states, bounds on controls, equality constraints,
|
||||
/// inequality constraints).
|
||||
///
|
||||
/// \param config_ The configuration struct.
|
||||
/// \param dims_ The dimension struct.
|
||||
/// \param stage Stage number.
|
||||
/// \param field The type of constraint/bound, either nbx, nbu, ng, or nh.
|
||||
/// \param value_field Number of constraints/bounds for the given stage.
|
||||
void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage,
|
||||
const char *field, const void* value_field);
|
||||
|
||||
/// Sets the dimensions of the cost terms for a stage.
|
||||
///
|
||||
/// \param config_ The configuration struct.
|
||||
/// \param dims_ The dimension struct.
|
||||
/// \param stage Stage number.
|
||||
/// \param field Type of cost term, can be eiter ny.
|
||||
/// \param value_field Number of cost terms/residuals for the given stage.
|
||||
void ocp_nlp_dims_set_cost(void *config_, void *dims_, int stage, const char *field,
|
||||
const void* value_field);
|
||||
|
||||
/// Sets the dimensions of the dynamics for a stage.
|
||||
///
|
||||
/// \param config_ The configuration struct.
|
||||
/// \param dims_ The dimension struct.
|
||||
/// \param stage Stage number.
|
||||
/// \param field TBD
|
||||
/// \param value TBD
|
||||
void ocp_nlp_dims_set_dynamics(void *config_, void *dims_, int stage, const char *field,
|
||||
const void* value);
|
||||
|
||||
/************************************************
|
||||
* Inputs
|
||||
************************************************/
|
||||
|
||||
/// Struct for storing the inputs of an OCP NLP solver
|
||||
typedef struct ocp_nlp_in
|
||||
{
|
||||
/// Length of sampling intervals/timesteps.
|
||||
double *Ts;
|
||||
|
||||
/// Pointers to cost functions (TBC).
|
||||
void **cost;
|
||||
|
||||
/// Pointers to dynamics functions (TBC).
|
||||
void **dynamics;
|
||||
|
||||
/// Pointers to constraints functions (TBC).
|
||||
void **constraints;
|
||||
|
||||
/// Pointer to allocated memory, to be used for freeing.
|
||||
void *raw_memory;
|
||||
|
||||
} ocp_nlp_in;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_in_calculate_size_self(int N);
|
||||
//
|
||||
acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims);
|
||||
//
|
||||
ocp_nlp_in *ocp_nlp_in_assign_self(int N, void *raw_memory);
|
||||
//
|
||||
ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory);
|
||||
|
||||
|
||||
/************************************************
|
||||
* out
|
||||
************************************************/
|
||||
|
||||
typedef struct ocp_nlp_out
|
||||
{
|
||||
struct blasfeo_dvec *ux; // NOTE: this contains [u; x; s_l; s_u]! - rename to uxs?
|
||||
struct blasfeo_dvec *z; // algebraic vairables
|
||||
struct blasfeo_dvec *pi; // multipliers for dynamics
|
||||
struct blasfeo_dvec *lam; // inequality mulitpliers
|
||||
struct blasfeo_dvec *t; // slack variables corresponding to evaluation of all inequalities (at the solution)
|
||||
|
||||
// NOTE: the inequalities are internally organized in the following order:
|
||||
// [ lbu lbx lg lh lphi ubu ubx ug uh uphi; lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
|
||||
double inf_norm_res;
|
||||
|
||||
void *raw_memory; // Pointer to allocated memory, to be used for freeing
|
||||
|
||||
} ocp_nlp_out;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_out_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims);
|
||||
//
|
||||
ocp_nlp_out *ocp_nlp_out_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
|
||||
void *raw_memory);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
/// Globalization types
|
||||
typedef enum
|
||||
{
|
||||
FIXED_STEP,
|
||||
MERIT_BACKTRACKING,
|
||||
} ocp_nlp_globalization_t;
|
||||
|
||||
typedef struct ocp_nlp_opts
|
||||
{
|
||||
ocp_qp_xcond_solver_opts *qp_solver_opts; // xcond solver opts instead ???
|
||||
void *regularize;
|
||||
void **dynamics; // dynamics_opts
|
||||
void **cost; // cost_opts
|
||||
void **constraints; // constraints_opts
|
||||
double step_length; // step length in case of FIXED_STEP
|
||||
double levenberg_marquardt; // LM factor to be added to the hessian before regularization
|
||||
int reuse_workspace;
|
||||
int num_threads;
|
||||
int print_level;
|
||||
|
||||
// TODO: move to separate struct?
|
||||
ocp_nlp_globalization_t globalization;
|
||||
int full_step_dual;
|
||||
int line_search_use_sufficient_descent;
|
||||
int globalization_use_SOC;
|
||||
double alpha_min;
|
||||
double alpha_reduction;
|
||||
double eps_sufficient_descent;
|
||||
} ocp_nlp_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_opts_initialize_default(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_opts_update(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value);
|
||||
//
|
||||
void ocp_nlp_opts_set_at_stage(void *config, void *opts, int stage, const char *field, void *value);
|
||||
|
||||
|
||||
/************************************************
|
||||
* residuals
|
||||
************************************************/
|
||||
|
||||
typedef struct ocp_nlp_res
|
||||
{
|
||||
struct blasfeo_dvec *res_stat; // stationarity
|
||||
struct blasfeo_dvec *res_eq; // dynamics
|
||||
struct blasfeo_dvec *res_ineq; // inequality constraints
|
||||
struct blasfeo_dvec *res_comp; // complementarity
|
||||
struct blasfeo_dvec tmp; // tmp
|
||||
double inf_norm_res_stat;
|
||||
double inf_norm_res_eq;
|
||||
double inf_norm_res_ineq;
|
||||
double inf_norm_res_comp;
|
||||
acados_size_t memsize;
|
||||
} ocp_nlp_res;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_res_calculate_size(ocp_nlp_dims *dims);
|
||||
//
|
||||
ocp_nlp_res *ocp_nlp_res_assign(ocp_nlp_dims *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_res_get_inf_norm(ocp_nlp_res *res, double *out);
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
|
||||
typedef struct ocp_nlp_memory
|
||||
{
|
||||
// void *qp_solver_mem; // xcond solver mem instead ???
|
||||
ocp_qp_xcond_solver_memory *qp_solver_mem; // xcond solver mem instead ???
|
||||
void *regularize_mem;
|
||||
void **dynamics; // dynamics memory
|
||||
void **cost; // cost memory
|
||||
void **constraints; // constraints memory
|
||||
|
||||
// residuals
|
||||
ocp_nlp_res *nlp_res;
|
||||
|
||||
// qp in & out
|
||||
ocp_qp_in *qp_in;
|
||||
ocp_qp_out *qp_out;
|
||||
// QP stuff not entering the qp_in struct
|
||||
struct blasfeo_dmat *dzduxt; // dzdux transposed
|
||||
struct blasfeo_dvec *z_alg; // z_alg, output algebraic variables
|
||||
|
||||
struct blasfeo_dvec *cost_grad;
|
||||
struct blasfeo_dvec *ineq_fun;
|
||||
struct blasfeo_dvec *ineq_adj;
|
||||
struct blasfeo_dvec *dyn_fun;
|
||||
struct blasfeo_dvec *dyn_adj;
|
||||
|
||||
double cost_value;
|
||||
|
||||
bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables
|
||||
struct blasfeo_dvec *sim_guess;
|
||||
|
||||
int *sqp_iter; // pointer to iteration number
|
||||
|
||||
} ocp_nlp_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_memory_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts);
|
||||
//
|
||||
ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
|
||||
ocp_nlp_opts *opts, void *raw_memory);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
typedef struct ocp_nlp_workspace
|
||||
{
|
||||
|
||||
void *qp_work;
|
||||
void **dynamics; // dynamics_workspace
|
||||
void **cost; // cost_workspace
|
||||
void **constraints; // constraints_workspace
|
||||
|
||||
// for globalization: -> move to module?!
|
||||
ocp_nlp_out *tmp_nlp_out;
|
||||
ocp_nlp_out *weight_merit_fun;
|
||||
struct blasfeo_dvec tmp_nxu;
|
||||
struct blasfeo_dvec tmp_ni;
|
||||
struct blasfeo_dvec dxnext_dy;
|
||||
|
||||
} ocp_nlp_workspace;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts);
|
||||
//
|
||||
ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
|
||||
ocp_nlp_opts *opts, ocp_nlp_memory *mem, void *raw_memory);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* function
|
||||
************************************************/
|
||||
|
||||
void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
|
||||
//
|
||||
void ocp_nlp_initialize_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
|
||||
//
|
||||
void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
|
||||
//
|
||||
void ocp_nlp_approximate_qp_vectors_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
|
||||
//
|
||||
void ocp_nlp_embed_initial_value(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
|
||||
//
|
||||
void ocp_nlp_update_variables_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, double alpha);
|
||||
//
|
||||
int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
|
||||
//
|
||||
double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work,
|
||||
int check_early_termination);
|
||||
//
|
||||
double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
|
||||
//
|
||||
void ocp_nlp_initialize_t_slacks(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
|
||||
//
|
||||
void ocp_nlp_res_compute(ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out,
|
||||
ocp_nlp_res *res, ocp_nlp_memory *mem);
|
||||
//
|
||||
void ocp_nlp_cost_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
|
||||
ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
|
||||
/// @}
|
||||
/// @}
|
||||
/// @}
|
||||
238
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h
vendored
Normal file
238
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h
vendored
Normal file
@@ -0,0 +1,238 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_constraints
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* dims
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nx;
|
||||
int nu;
|
||||
int nz;
|
||||
int nb; // nbx + nbu
|
||||
int nbu; // number of input box constraints
|
||||
int nbx; // number of state box constraints
|
||||
int ng; // number of general linear constraints
|
||||
int nh; // number of nonlinear path constraints
|
||||
int ns; // nsbu + nsbx + nsg + nsh
|
||||
int nsbu; // number of softened input bounds
|
||||
int nsbx; // number of softened state bounds
|
||||
int nsg; // number of softened general linear constraints
|
||||
int nsh; // number of softened nonlinear constraints
|
||||
int nbue; // number of input box constraints which are equality
|
||||
int nbxe; // number of state box constraints which are equality
|
||||
int nge; // number of general linear constraints which are equality
|
||||
int nhe; // number of nonlinear path constraints which are equality
|
||||
} ocp_nlp_constraints_bgh_dims;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_constraints_bgh_dims_calculate_size(void *config);
|
||||
//
|
||||
void *ocp_nlp_constraints_bgh_dims_assign(void *config, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_dims_get(void *config_, void *dims_, const char *field, int* value);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_dims_set(void *config_, void *dims_,
|
||||
const char *field, const int* value);
|
||||
|
||||
|
||||
/************************************************
|
||||
* model
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int *idxb;
|
||||
int *idxs;
|
||||
int *idxe;
|
||||
struct blasfeo_dvec d; // gathers bounds
|
||||
struct blasfeo_dmat DCt; // general linear constraint matrix
|
||||
// lg <= [D, C] * [u; x] <= ug
|
||||
external_function_generic *nl_constr_h_fun; // nonlinear: lh <= h(x,u) <= uh
|
||||
external_function_generic *nl_constr_h_fun_jac; // nonlinear: lh <= h(x,u) <= uh
|
||||
external_function_generic *nl_constr_h_fun_jac_hess; // nonlinear: lh <= h(x,u) <= uh
|
||||
} ocp_nlp_constraints_bgh_model;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_constraints_bgh_model_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_,
|
||||
void *model_, const char *field, void *value);
|
||||
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_model_get(void *config_, void *dims_,
|
||||
void *model_, const char *field, void *value);
|
||||
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int compute_adj;
|
||||
int compute_hess;
|
||||
} ocp_nlp_constraints_bgh_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_constraints_bgh_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_constraints_bgh_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_opts_initialize_default(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_opts_update(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_opts_set(void *config, void *opts, char *field, void *value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dvec fun;
|
||||
struct blasfeo_dvec adj;
|
||||
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
|
||||
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
|
||||
struct blasfeo_dvec *lam; // pointer to lam in nlp_out
|
||||
struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out
|
||||
struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory
|
||||
struct blasfeo_dmat *DCt; // pointer to DCt in qp_in
|
||||
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
|
||||
struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory
|
||||
int *idxb; // pointer to idxb[ii] in qp_in
|
||||
int *idxs_rev; // pointer to idxs_rev[ii] in qp_in
|
||||
int *idxe; // pointer to idxe[ii] in qp_in
|
||||
} ocp_nlp_constraints_bgh_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_constraints_bgh_memory_calculate_size(void *config, void *dims, void *opts);
|
||||
//
|
||||
void *ocp_nlp_constraints_bgh_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
|
||||
//
|
||||
struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_fun_ptr(void *memory_);
|
||||
//
|
||||
struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_adj_ptr(void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_memory_set_idxb_ptr(int *idxb, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_memory_set_idxe_ptr(int *idxe, void *memory_);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dmat tmp_nv_nv;
|
||||
struct blasfeo_dmat tmp_nz_nh;
|
||||
struct blasfeo_dmat tmp_nv_nh;
|
||||
struct blasfeo_dmat tmp_nz_nv;
|
||||
struct blasfeo_dmat hess_z;
|
||||
struct blasfeo_dvec tmp_ni;
|
||||
struct blasfeo_dvec tmp_nh;
|
||||
} ocp_nlp_constraints_bgh_workspace;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_constraints_bgh_workspace_calculate_size(void *config, void *dims, void *opts);
|
||||
|
||||
/* functions */
|
||||
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_config_initialize_default(void *config);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_initialize(void *config, void *dims, void *model, void *opts,
|
||||
void *mem, void *work);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_update_qp_matrices(void *config_, void *dims, void *model_,
|
||||
void *opts_, void *memory_, void *work_);
|
||||
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims, void *model_,
|
||||
void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_bounds_update(void *config_, void *dims, void *model_,
|
||||
void *opts_, void *memory_, void *work_);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
|
||||
/// @}
|
||||
/// @}
|
||||
218
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h
vendored
Normal file
218
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h
vendored
Normal file
@@ -0,0 +1,218 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_constraints
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
/* dims */
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nx;
|
||||
int nu;
|
||||
int nz;
|
||||
int nb; // nbx + nbu
|
||||
int nbu;
|
||||
int nbx;
|
||||
int ng; // number of general linear constraints
|
||||
int nphi; // dimension of convex outer part
|
||||
int ns; // nsbu + nsbx + nsg + nsphi
|
||||
int nsbu; // number of softened input bounds
|
||||
int nsbx; // number of softened state bounds
|
||||
int nsg; // number of softened general linear constraints
|
||||
int nsphi; // number of softened nonlinear constraints
|
||||
int nr; // dimension of nonlinear function in convex_over_nonlinear constraint
|
||||
int nbue; // number of input box constraints which are equality
|
||||
int nbxe; // number of state box constraints which are equality
|
||||
int nge; // number of general linear constraints which are equality
|
||||
int nphie; // number of nonlinear path constraints which are equality
|
||||
} ocp_nlp_constraints_bgp_dims;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_constraints_bgp_dims_calculate_size(void *config);
|
||||
//
|
||||
void *ocp_nlp_constraints_bgp_dims_assign(void *config, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_dims_get(void *config_, void *dims_, const char *field, int* value);
|
||||
|
||||
|
||||
/* model */
|
||||
|
||||
typedef struct
|
||||
{
|
||||
// ocp_nlp_constraints_bgp_dims *dims;
|
||||
int *idxb;
|
||||
int *idxs;
|
||||
int *idxe;
|
||||
struct blasfeo_dvec d;
|
||||
struct blasfeo_dmat DCt;
|
||||
external_function_generic *nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux;
|
||||
external_function_generic *nl_constr_phi_o_r_fun;
|
||||
external_function_generic *nl_constr_r_fun_jac;
|
||||
} ocp_nlp_constraints_bgp_model;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_constraints_bgp_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_constraints_bgp_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_,
|
||||
void *model_, const char *field, void *value);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_model_get(void *config_, void *dims_,
|
||||
void *model_, const char *field, void *value);
|
||||
|
||||
/* options */
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int compute_adj;
|
||||
int compute_hess;
|
||||
} ocp_nlp_constraints_bgp_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_constraints_bgp_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_constraints_bgp_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_opts_initialize_default(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_opts_update(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_opts_set(void *config, void *opts, char *field, void *value);
|
||||
|
||||
/* memory */
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dvec fun;
|
||||
struct blasfeo_dvec adj;
|
||||
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
|
||||
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
|
||||
struct blasfeo_dvec *lam; // pointer to lam in nlp_out
|
||||
struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out
|
||||
struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory
|
||||
struct blasfeo_dmat *DCt; // pointer to DCt in qp_in
|
||||
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
|
||||
struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory
|
||||
int *idxb; // pointer to idxb[ii] in qp_in
|
||||
int *idxs_rev; // pointer to idxs_rev[ii] in qp_in
|
||||
int *idxe; // pointer to idxe[ii] in qp_in
|
||||
} ocp_nlp_constraints_bgp_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_constraints_bgp_memory_calculate_size(void *config, void *dims, void *opts);
|
||||
//
|
||||
void *ocp_nlp_constraints_bgp_memory_assign(void *config, void *dims, void *opts,
|
||||
void *raw_memory);
|
||||
//
|
||||
struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_fun_ptr(void *memory_);
|
||||
//
|
||||
struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_adj_ptr(void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_memory_set_idxb_ptr(int *idxb, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgh_memory_set_idxe_ptr(int *idxe, void *memory_);
|
||||
|
||||
/* workspace */
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dvec tmp_ni;
|
||||
struct blasfeo_dmat jac_r_ux_tran;
|
||||
struct blasfeo_dmat tmp_nr_nphi_nr;
|
||||
struct blasfeo_dmat tmp_nv_nr;
|
||||
struct blasfeo_dmat tmp_nv_nphi;
|
||||
struct blasfeo_dmat tmp_nz_nphi;
|
||||
} ocp_nlp_constraints_bgp_workspace;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_constraints_bgp_workspace_calculate_size(void *config, void *dims, void *opts);
|
||||
|
||||
/* functions */
|
||||
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_config_initialize_default(void *config);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_initialize(void *config, void *dims, void *model,
|
||||
void *opts, void *mem, void *work);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_update_qp_matrices(void *config_, void *dims,
|
||||
void *model_, void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims,
|
||||
void *model_, void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void ocp_nlp_constraints_bgp_bounds_update(void *config_, void *dims, void *model_,
|
||||
void *opts_, void *memory_, void *work_);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
|
||||
/// @}
|
||||
/// @}
|
||||
108
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h
vendored
Normal file
108
third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h
vendored
Normal file
@@ -0,0 +1,108 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \ingroup ocp_nlp
|
||||
/// @{
|
||||
|
||||
/// \defgroup ocp_nlp_constraints ocp_nlp_constraints
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* config
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
acados_size_t (*dims_calculate_size)(void *config);
|
||||
void *(*dims_assign)(void *config, void *raw_memory);
|
||||
acados_size_t (*model_calculate_size)(void *config, void *dims);
|
||||
void *(*model_assign)(void *config, void *dims, void *raw_memory);
|
||||
int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value);
|
||||
void (*model_get)(void *config_, void *dims_, void *model_, const char *field, void *value);
|
||||
acados_size_t (*opts_calculate_size)(void *config, void *dims);
|
||||
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
|
||||
void (*opts_initialize_default)(void *config, void *dims, void *opts);
|
||||
void (*opts_update)(void *config, void *dims, void *opts);
|
||||
void (*opts_set)(void *config, void *opts, char *field, void *value);
|
||||
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
|
||||
struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory);
|
||||
struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory);
|
||||
void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory);
|
||||
void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory);
|
||||
void (*memory_set_lam_ptr)(struct blasfeo_dvec *lam, void *memory);
|
||||
void (*memory_set_tmp_lam_ptr)(struct blasfeo_dvec *tmp_lam, void *memory);
|
||||
void (*memory_set_DCt_ptr)(struct blasfeo_dmat *DCt, void *memory);
|
||||
void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory);
|
||||
void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory);
|
||||
void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzduxt, void *memory);
|
||||
void (*memory_set_idxb_ptr)(int *idxb, void *memory);
|
||||
void (*memory_set_idxs_rev_ptr)(int *idxs_rev, void *memory);
|
||||
void (*memory_set_idxe_ptr)(int *idxe, void *memory);
|
||||
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
|
||||
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
|
||||
void (*initialize)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
|
||||
void (*update_qp_matrices)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
|
||||
void (*compute_fun)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
|
||||
void (*bounds_update)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
|
||||
void (*config_initialize_default)(void *config);
|
||||
// dimension setters
|
||||
void (*dims_set)(void *config_, void *dims_, const char *field, const int *value);
|
||||
void (*dims_get)(void *config_, void *dims_, const char *field, int* value);
|
||||
} ocp_nlp_constraints_config;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_constraints_config_calculate_size();
|
||||
//
|
||||
ocp_nlp_constraints_config *ocp_nlp_constraints_config_assign(void *raw_memory);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
|
||||
/// @}
|
||||
/// @}
|
||||
107
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h
vendored
Normal file
107
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h
vendored
Normal file
@@ -0,0 +1,107 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
///
|
||||
/// \defgroup ocp_nlp_cost ocp_nlp_cost
|
||||
///
|
||||
|
||||
/// \addtogroup ocp_nlp_cost ocp_nlp_cost
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_cost_common ocp_nlp_cost_common
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// acados
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* config
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
acados_size_t (*dims_calculate_size)(void *config);
|
||||
void *(*dims_assign)(void *config, void *raw_memory);
|
||||
void (*dims_set)(void *config_, void *dims_, const char *field, int *value);
|
||||
void (*dims_get)(void *config_, void *dims_, const char *field, int *value);
|
||||
acados_size_t (*model_calculate_size)(void *config, void *dims);
|
||||
void *(*model_assign)(void *config, void *dims, void *raw_memory);
|
||||
int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_);
|
||||
acados_size_t (*opts_calculate_size)(void *config, void *dims);
|
||||
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
|
||||
void (*opts_initialize_default)(void *config, void *dims, void *opts);
|
||||
void (*opts_update)(void *config, void *dims, void *opts);
|
||||
void (*opts_set)(void *config, void *opts, const char *field, void *value);
|
||||
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
|
||||
double *(*memory_get_fun_ptr)(void *memory);
|
||||
struct blasfeo_dvec *(*memory_get_grad_ptr)(void *memory);
|
||||
void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory);
|
||||
void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory);
|
||||
void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory);
|
||||
void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzdux, void *memory);
|
||||
void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory);
|
||||
void (*memory_set_Z_ptr)(struct blasfeo_dvec *Z, void *memory);
|
||||
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
|
||||
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
|
||||
void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
|
||||
|
||||
// computes the function value, gradient and hessian (approximation) of the cost function
|
||||
void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
|
||||
// computes the cost function value (intended for globalization)
|
||||
void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
|
||||
void (*config_initialize_default)(void *config);
|
||||
void (*precompute)(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
|
||||
|
||||
} ocp_nlp_cost_config;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_config_calculate_size();
|
||||
//
|
||||
ocp_nlp_cost_config *ocp_nlp_cost_config_assign(void *raw_memory);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
|
||||
/// @}
|
||||
/// @}
|
||||
207
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h
vendored
Normal file
207
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h
vendored
Normal file
@@ -0,0 +1,207 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_cost ocp_nlp_cost
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_cost_conl ocp_nlp_cost_conl
|
||||
/// \brief This module implements convex-over-nonlinear costs of the form
|
||||
/// \f$\min_{x,u,z} \psi(y(x,u,z,p) - y_{\text{ref}}, p)\f$,
|
||||
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* dims
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nx; // number of states
|
||||
int nz; // number of algebraic variables
|
||||
int nu; // number of inputs
|
||||
int ny; // number of outputs
|
||||
int ns; // number of slacks
|
||||
} ocp_nlp_cost_conl_dims;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_conl_dims_calculate_size(void *config);
|
||||
//
|
||||
void *ocp_nlp_cost_conl_dims_assign(void *config, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_cost_conl_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz);
|
||||
//
|
||||
void ocp_nlp_cost_conl_dims_set(void *config_, void *dims_, const char *field, int* value);
|
||||
//
|
||||
void ocp_nlp_cost_conl_dims_get(void *config_, void *dims_, const char *field, int* value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* model
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
// slack penalty has the form z^T * s + .5 * s^T * Z * s
|
||||
external_function_generic *conl_cost_fun;
|
||||
external_function_generic *conl_cost_fun_jac_hess;
|
||||
struct blasfeo_dvec y_ref;
|
||||
struct blasfeo_dvec Z; // diagonal Hessian of slacks as vector
|
||||
struct blasfeo_dvec z; // gradient of slacks as vector
|
||||
double scaling;
|
||||
} ocp_nlp_cost_conl_model;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_conl_model_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_cost_conl_model_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
int ocp_nlp_cost_conl_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
bool gauss_newton_hess; // dummy options, we always use a gauss-newton hessian
|
||||
} ocp_nlp_cost_conl_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_conl_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_cost_conl_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_cost_conl_opts_initialize_default(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_cost_conl_opts_update(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_cost_conl_opts_set(void *config, void *opts, const char *field, void *value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dvec grad; // gradient of cost function
|
||||
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
|
||||
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
|
||||
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
|
||||
struct blasfeo_dvec *Z; // pointer to Z in qp_in
|
||||
struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out
|
||||
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
|
||||
double fun; ///< value of the cost function
|
||||
} ocp_nlp_cost_conl_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_conl_memory_calculate_size(void *config, void *dims, void *opts);
|
||||
//
|
||||
void *ocp_nlp_cost_conl_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
|
||||
//
|
||||
double *ocp_nlp_cost_conl_memory_get_fun_ptr(void *memory_);
|
||||
//
|
||||
struct blasfeo_dvec *ocp_nlp_cost_conl_memory_get_grad_ptr(void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_conl_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
|
||||
//
|
||||
void ocp_nlp_cost_conl_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
|
||||
//
|
||||
void ocp_nlp_cost_conl_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_conl_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_conl_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_conl_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dmat W; // hessian of outer loss function
|
||||
struct blasfeo_dmat W_chol; // cholesky factor of hessian of outer loss function
|
||||
struct blasfeo_dmat Jt_ux; // jacobian of inner residual function
|
||||
struct blasfeo_dmat Jt_ux_tilde; // jacobian of inner residual function plus gradient contribution of algebraic variables
|
||||
struct blasfeo_dmat Jt_z; // jacobian of inner residual function wrt algebraic variables
|
||||
struct blasfeo_dmat tmp_nv_ny;
|
||||
struct blasfeo_dvec tmp_ny;
|
||||
struct blasfeo_dvec tmp_2ns;
|
||||
} ocp_nlp_cost_conl_workspace;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_conl_workspace_calculate_size(void *config, void *dims, void *opts);
|
||||
|
||||
/************************************************
|
||||
* functions
|
||||
************************************************/
|
||||
|
||||
//
|
||||
void ocp_nlp_cost_conl_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void ocp_nlp_cost_conl_config_initialize_default(void *config);
|
||||
//
|
||||
void ocp_nlp_cost_conl_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
|
||||
/// @}
|
||||
/// @}
|
||||
/// @}
|
||||
187
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h
vendored
Normal file
187
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h
vendored
Normal file
@@ -0,0 +1,187 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
/************************************************
|
||||
* dims
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nx; // number of states
|
||||
int nz; // number of algebraic variables
|
||||
int nu; // number of inputs
|
||||
int ns; // number of slacks
|
||||
} ocp_nlp_cost_external_dims;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_external_dims_calculate_size(void *config);
|
||||
//
|
||||
void *ocp_nlp_cost_external_dims_assign(void *config, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_cost_external_dims_set(void *config_, void *dims_, const char *field, int* value);
|
||||
//
|
||||
void ocp_nlp_cost_external_dims_get(void *config_, void *dims_, const char *field, int* value);
|
||||
|
||||
/************************************************
|
||||
* model
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
external_function_generic *ext_cost_fun; // function
|
||||
external_function_generic *ext_cost_fun_jac_hess; // function, gradient and hessian
|
||||
external_function_generic *ext_cost_fun_jac; // function, gradient
|
||||
struct blasfeo_dvec Z;
|
||||
struct blasfeo_dvec z;
|
||||
struct blasfeo_dmat numerical_hessian; // custom hessian approximation
|
||||
double scaling;
|
||||
} ocp_nlp_cost_external_model;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_external_model_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_cost_external_model_assign(void *config, void *dims, void *raw_memory);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int use_numerical_hessian; // > 0 indicating custom hessian is used instead of CasADi evaluation
|
||||
} ocp_nlp_cost_external_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_external_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_cost_external_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_cost_external_opts_initialize_default(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_cost_external_opts_update(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_cost_external_opts_set(void *config, void *opts, const char *field, void *value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dvec grad; // gradient of cost function
|
||||
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
|
||||
struct blasfeo_dvec *tmp_ux; // pointer to tmp_ux in nlp_out
|
||||
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
|
||||
struct blasfeo_dvec *Z; // pointer to Z in qp_in
|
||||
struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out
|
||||
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
|
||||
double fun; ///< value of the cost function
|
||||
} ocp_nlp_cost_external_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_external_memory_calculate_size(void *config, void *dims, void *opts);
|
||||
//
|
||||
void *ocp_nlp_cost_external_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
|
||||
//
|
||||
double *ocp_nlp_cost_external_memory_get_fun_ptr(void *memory_);
|
||||
//
|
||||
struct blasfeo_dvec *ocp_nlp_cost_external_memory_get_grad_ptr(void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_external_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
|
||||
//
|
||||
void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
|
||||
//
|
||||
void ocp_nlp_cost_external_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_external_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_external_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_external_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dmat tmp_nunx_nunx;
|
||||
struct blasfeo_dmat tmp_nz_nz;
|
||||
struct blasfeo_dmat tmp_nz_nunx;
|
||||
struct blasfeo_dvec tmp_nunxnz;
|
||||
struct blasfeo_dvec tmp_2ns; // temporary vector of dimension 2*ns
|
||||
} ocp_nlp_cost_external_workspace;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_external_workspace_calculate_size(void *config, void *dims, void *opts);
|
||||
|
||||
/************************************************
|
||||
* functions
|
||||
************************************************/
|
||||
|
||||
//
|
||||
void ocp_nlp_cost_external_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void ocp_nlp_cost_external_config_initialize_default(void *config);
|
||||
//
|
||||
void ocp_nlp_cost_external_initialize(void *config_, void *dims, void *model_,
|
||||
void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims, void *model_,
|
||||
void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void ocp_nlp_cost_external_compute_fun(void *config_, void *dims, void *model_,
|
||||
void *opts_, void *memory_, void *work_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_
|
||||
243
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h
vendored
Normal file
243
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h
vendored
Normal file
@@ -0,0 +1,243 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_cost ocp_nlp_cost
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_cost_ls ocp_nlp_cost_ls
|
||||
/// \brief This module implements linear-least squares costs of the form
|
||||
/// \f$\min_{x,u,z} \| V_x x + V_u u + V_z z - y_{\text{ref}}\|_W^2\f$.
|
||||
/// @{
|
||||
|
||||
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// dims //
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nx; // number of states
|
||||
int nz; // number of algebraic variables
|
||||
int nu; // number of inputs
|
||||
int ny; // number of outputs
|
||||
int ns; // number of slacks
|
||||
} ocp_nlp_cost_ls_dims;
|
||||
|
||||
|
||||
/// Calculate the size of the ocp_nlp_cost_ls_dims struct
|
||||
///
|
||||
/// \param[in] config_ structure containing configuration of ocp_nlp_cost
|
||||
/// module
|
||||
/// \param[out] []
|
||||
/// \return \c size of ocp_nlp_dims struct
|
||||
acados_size_t ocp_nlp_cost_ls_dims_calculate_size(void *config);
|
||||
|
||||
|
||||
/// Assign memory pointed to by raw_memory to ocp_nlp-cost_ls dims struct
|
||||
///
|
||||
/// \param[in] config structure containing configuration of ocp_nlp_cost
|
||||
/// module
|
||||
/// \param[in] raw_memory pointer to memory location
|
||||
/// \param[out] []
|
||||
/// \return dims
|
||||
void *ocp_nlp_cost_ls_dims_assign(void *config, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_cost_ls_dims_set(void *config_, void *dims_, const char *field, int* value);
|
||||
//
|
||||
void ocp_nlp_cost_ls_dims_get(void *config_, void *dims_, const char *field, int* value);
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// model //
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
/// structure containing the data describing the linear least-square cost
|
||||
typedef struct
|
||||
{
|
||||
// slack penalty has the form z^T * s + .5 * s^T * Z * s
|
||||
struct blasfeo_dmat Cyt; ///< output matrix: Cy * [u,x] = y; in transposed form
|
||||
struct blasfeo_dmat Vz; ///< Vz in ls cost Vx*x + Vu*u + Vz*z
|
||||
struct blasfeo_dmat W; ///< ls norm corresponding to this matrix
|
||||
struct blasfeo_dvec y_ref; ///< yref
|
||||
struct blasfeo_dvec Z; ///< diagonal Hessian of slacks as vector (lower and upper)
|
||||
struct blasfeo_dvec z; ///< gradient of slacks as vector (lower and upper)
|
||||
double scaling;
|
||||
int W_changed; ///< flag indicating whether W has changed and needs to be refactorized
|
||||
int Cyt_or_scaling_changed; ///< flag indicating whether Cyt or scaling has changed and Hessian needs to be recomputed
|
||||
} ocp_nlp_cost_ls_model;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_ls_model_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_cost_ls_model_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
int ocp_nlp_cost_ls_model_set(void *config_, void *dims_, void *model_,
|
||||
const char *field, void *value_);
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// options //
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int dummy; // struct can't be void
|
||||
} ocp_nlp_cost_ls_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_ls_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_cost_ls_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_cost_ls_opts_initialize_default(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_cost_ls_opts_update(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_cost_ls_opts_set(void *config, void *opts, const char *field, void *value);
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// memory //
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
|
||||
/// structure containing the memory associated with cost_ls component
|
||||
/// of the ocp_nlp module
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dmat hess; ///< hessian of cost function
|
||||
struct blasfeo_dmat W_chol; ///< cholesky factor of weight matrix
|
||||
struct blasfeo_dvec res; ///< ls residual r(x)
|
||||
struct blasfeo_dvec grad; ///< gradient of cost function
|
||||
struct blasfeo_dvec *ux; ///< pointer to ux in nlp_out
|
||||
struct blasfeo_dvec *tmp_ux; ///< pointer to ux in tmp_nlp_out
|
||||
struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out
|
||||
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
|
||||
struct blasfeo_dmat *RSQrq; ///< pointer to RSQrq in qp_in
|
||||
struct blasfeo_dvec *Z; ///< pointer to Z in qp_in
|
||||
double fun; ///< value of the cost function
|
||||
} ocp_nlp_cost_ls_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_ls_memory_calculate_size(void *config, void *dims, void *opts);
|
||||
//
|
||||
void *ocp_nlp_cost_ls_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
|
||||
//
|
||||
double *ocp_nlp_cost_ls_memory_get_fun_ptr(void *memory_);
|
||||
//
|
||||
struct blasfeo_dvec *ocp_nlp_cost_ls_memory_get_grad_ptr(void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_ls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
|
||||
//
|
||||
void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
|
||||
//
|
||||
void ocp_nlp_cost_ls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_ls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_ls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_ls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// workspace //
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dmat tmp_nv_ny; // temporary matrix of dimensions nv, ny
|
||||
struct blasfeo_dmat Cyt_tilde; // updated Cyt (after z elimination)
|
||||
struct blasfeo_dmat dzdux_tran; // derivatives of z wrt u and x (tran)
|
||||
struct blasfeo_dvec tmp_ny; // temporary vector of dimension ny
|
||||
struct blasfeo_dvec tmp_2ns; // temporary vector of dimension ny
|
||||
struct blasfeo_dvec tmp_nz; // temporary vector of dimension nz
|
||||
struct blasfeo_dvec y_ref_tilde; // updated y_ref (after z elimination)
|
||||
} ocp_nlp_cost_ls_workspace;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_ls_workspace_calculate_size(void *config, void *dims, void *opts);
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// functions //
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
// computations that are done once when solver is created
|
||||
void ocp_nlp_cost_ls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void ocp_nlp_cost_ls_config_initialize_default(void *config);
|
||||
//
|
||||
void ocp_nlp_cost_ls_initialize(void *config_, void *dims, void *model_, void *opts_,
|
||||
void *mem_, void *work_);
|
||||
//
|
||||
void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims, void *model_,
|
||||
void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
|
||||
/// @}
|
||||
/// @}
|
||||
/// @}
|
||||
211
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h
vendored
Normal file
211
third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h
vendored
Normal file
@@ -0,0 +1,211 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_cost ocp_nlp_cost
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_cost_nls ocp_nlp_cost_nls
|
||||
/// \brief This module implements nonlinear-least squares costs of the form
|
||||
/// \f$\min_{x,u,z} \| y(x,u,z,p) - y_{\text{ref}} \|_W^2\f$,
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* dims
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nx; // number of states
|
||||
int nz; // number of algebraic variables
|
||||
int nu; // number of inputs
|
||||
int ny; // number of outputs
|
||||
int ns; // number of slacks
|
||||
} ocp_nlp_cost_nls_dims;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_nls_dims_calculate_size(void *config);
|
||||
//
|
||||
void *ocp_nlp_cost_nls_dims_assign(void *config, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_cost_nls_dims_set(void *config_, void *dims_, const char *field, int* value);
|
||||
//
|
||||
void ocp_nlp_cost_nls_dims_get(void *config_, void *dims_, const char *field, int* value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* model
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
// nonliner function nls_y(x,u) replaces Cy * [x,u] in ls_cost
|
||||
// slack penalty has the form z^T * s + .5 * s^T * Z * s
|
||||
external_function_generic *nls_y_fun; // evaluation of nls function
|
||||
external_function_generic *nls_y_fun_jac; // evaluation nls function and jacobian
|
||||
external_function_generic *nls_y_hess; // hessian*seeds of nls residuals
|
||||
struct blasfeo_dmat W; //
|
||||
struct blasfeo_dvec y_ref;
|
||||
struct blasfeo_dvec Z; // diagonal Hessian of slacks as vector
|
||||
struct blasfeo_dvec z; // gradient of slacks as vector
|
||||
double scaling;
|
||||
int W_changed; ///< flag indicating whether W has changed and needs to be refactorized
|
||||
} ocp_nlp_cost_nls_model;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_nls_model_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_cost_nls_model_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
int ocp_nlp_cost_nls_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
bool gauss_newton_hess; // gauss-newton hessian approximation
|
||||
} ocp_nlp_cost_nls_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_nls_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_cost_nls_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_cost_nls_opts_initialize_default(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_cost_nls_opts_update(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_cost_nls_opts_set(void *config, void *opts, const char *field, void *value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dmat W_chol; // cholesky factor of weight matrix
|
||||
struct blasfeo_dmat Jt; // jacobian of nls fun
|
||||
struct blasfeo_dvec res; // nls residual r(x)
|
||||
struct blasfeo_dvec grad; // gradient of cost function
|
||||
struct blasfeo_dvec *ux; // pointer to ux in nlp_out
|
||||
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
|
||||
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
|
||||
struct blasfeo_dvec *Z; // pointer to Z in qp_in
|
||||
struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out
|
||||
struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out
|
||||
double fun; ///< value of the cost function
|
||||
} ocp_nlp_cost_nls_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_nls_memory_calculate_size(void *config, void *dims, void *opts);
|
||||
//
|
||||
void *ocp_nlp_cost_nls_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
|
||||
//
|
||||
double *ocp_nlp_cost_nls_memory_get_fun_ptr(void *memory_);
|
||||
//
|
||||
struct blasfeo_dvec *ocp_nlp_cost_nls_memory_get_grad_ptr(void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_nls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
|
||||
//
|
||||
void ocp_nlp_cost_nls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
|
||||
//
|
||||
void ocp_nlp_cost_nls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_nls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_nls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
|
||||
//
|
||||
void ocp_nlp_cost_nls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dmat tmp_nv_ny;
|
||||
struct blasfeo_dmat tmp_nv_nv;
|
||||
struct blasfeo_dmat Vz;
|
||||
struct blasfeo_dmat Cyt_tilde;
|
||||
struct blasfeo_dvec tmp_ny;
|
||||
struct blasfeo_dvec tmp_2ns;
|
||||
struct blasfeo_dvec tmp_nz;
|
||||
} ocp_nlp_cost_nls_workspace;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_cost_nls_workspace_calculate_size(void *config, void *dims, void *opts);
|
||||
|
||||
/************************************************
|
||||
* functions
|
||||
************************************************/
|
||||
|
||||
//
|
||||
void ocp_nlp_cost_nls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void ocp_nlp_cost_nls_config_initialize_default(void *config);
|
||||
//
|
||||
void ocp_nlp_cost_nls_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
|
||||
//
|
||||
void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
|
||||
/// @}
|
||||
/// @}
|
||||
/// @}
|
||||
119
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h
vendored
Normal file
119
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h
vendored
Normal file
@@ -0,0 +1,119 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \ingroup ocp_nlp
|
||||
/// @{
|
||||
|
||||
/// \defgroup ocp_nlp_dynamics ocp_nlp_dynamics
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// acados
|
||||
#include "acados/sim/sim_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* config
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
void (*config_initialize_default)(void *config);
|
||||
sim_config *sim_solver;
|
||||
/* dims */
|
||||
acados_size_t (*dims_calculate_size)(void *config);
|
||||
void *(*dims_assign)(void *config, void *raw_memory);
|
||||
void (*dims_set)(void *config_, void *dims_, const char *field, int *value);
|
||||
void (*dims_get)(void *config_, void *dims_, const char *field, int* value);
|
||||
/* model */
|
||||
acados_size_t (*model_calculate_size)(void *config, void *dims);
|
||||
void *(*model_assign)(void *config, void *dims, void *raw_memory);
|
||||
void (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_);
|
||||
/* opts */
|
||||
acados_size_t (*opts_calculate_size)(void *config, void *dims);
|
||||
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
|
||||
void (*opts_initialize_default)(void *config, void *dims, void *opts);
|
||||
void (*opts_set)(void *config_, void *opts_, const char *field, void *value);
|
||||
void (*opts_update)(void *config, void *dims, void *opts);
|
||||
/* memory */
|
||||
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
|
||||
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
|
||||
// get shooting node gap x_next(x_n, u_n) - x_{n+1}
|
||||
struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory_);
|
||||
struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory_);
|
||||
void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory_);
|
||||
void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory_);
|
||||
void (*memory_set_ux1_ptr)(struct blasfeo_dvec *ux1, void *memory_);
|
||||
void (*memory_set_tmp_ux1_ptr)(struct blasfeo_dvec *tmp_ux1, void *memory_);
|
||||
void (*memory_set_pi_ptr)(struct blasfeo_dvec *pi, void *memory_);
|
||||
void (*memory_set_tmp_pi_ptr)(struct blasfeo_dvec *tmp_pi, void *memory_);
|
||||
void (*memory_set_BAbt_ptr)(struct blasfeo_dmat *BAbt, void *memory_);
|
||||
void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory_);
|
||||
void (*memory_set_dzduxt_ptr)(struct blasfeo_dmat *mat, void *memory_);
|
||||
void (*memory_set_sim_guess_ptr)(struct blasfeo_dvec *vec, bool *bool_ptr, void *memory_);
|
||||
void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *vec, void *memory_);
|
||||
void (*memory_get)(void *config, void *dims, void *mem, const char *field, void* value);
|
||||
/* workspace */
|
||||
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
|
||||
void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
|
||||
void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
|
||||
void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
|
||||
int (*precompute)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
|
||||
} ocp_nlp_dynamics_config;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_dynamics_config_calculate_size();
|
||||
//
|
||||
ocp_nlp_dynamics_config *ocp_nlp_dynamics_config_assign(void *raw_memory);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
|
||||
/// @}
|
||||
/// @}
|
||||
209
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h
vendored
Normal file
209
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h
vendored
Normal file
@@ -0,0 +1,209 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_dynamics
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/types.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* dims
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
void *sim;
|
||||
int nx; // number of states at the current stage
|
||||
int nz; // number of algebraic states at the current stage
|
||||
int nu; // number of inputs at the current stage
|
||||
int nx1; // number of states at the next stage
|
||||
int nu1; // number of inputes at the next stage
|
||||
} ocp_nlp_dynamics_cont_dims;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_dynamics_cont_dims_calculate_size(void *config);
|
||||
//
|
||||
void *ocp_nlp_dynamics_cont_dims_assign(void *config, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_dims_set(void *config_, void *dims_, const char *field, int* value);
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
void *sim_solver;
|
||||
int compute_adj;
|
||||
int compute_hess;
|
||||
} ocp_nlp_dynamics_cont_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_dynamics_cont_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_dynamics_cont_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_opts_initialize_default(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_opts_update(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_opts_set(void *config, void *opts, const char *field, void* value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dvec fun;
|
||||
struct blasfeo_dvec adj;
|
||||
struct blasfeo_dvec *ux; // pointer to ux in nlp_out at current stage
|
||||
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out at current stage
|
||||
struct blasfeo_dvec *ux1; // pointer to ux in nlp_out at next stage
|
||||
struct blasfeo_dvec *tmp_ux1; // pointer to ux in tmp_nlp_out at next stage
|
||||
struct blasfeo_dvec *pi; // pointer to pi in nlp_out at current stage
|
||||
struct blasfeo_dvec *tmp_pi; // pointer to pi in tmp_nlp_out at current stage
|
||||
struct blasfeo_dmat *BAbt; // pointer to BAbt in qp_in
|
||||
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
|
||||
struct blasfeo_dvec *z_alg; // pointer to output z at t = 0
|
||||
bool *set_sim_guess; // indicate if initialization for integrator is set from outside
|
||||
struct blasfeo_dvec *sim_guess; // initializations for integrator
|
||||
// struct blasfeo_dvec *z; // pointer to (input) z in nlp_out at current stage
|
||||
struct blasfeo_dmat *dzduxt; // pointer to dzdux transposed
|
||||
void *sim_solver; // sim solver memory
|
||||
} ocp_nlp_dynamics_cont_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_dynamics_cont_memory_calculate_size(void *config, void *dims, void *opts);
|
||||
//
|
||||
void *ocp_nlp_dynamics_cont_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
|
||||
//
|
||||
struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_fun_ptr(void *memory);
|
||||
//
|
||||
struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_adj_ptr(void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dmat hess;
|
||||
sim_in *sim_in;
|
||||
sim_out *sim_out;
|
||||
void *sim_solver; // sim solver workspace
|
||||
} ocp_nlp_dynamics_cont_workspace;
|
||||
|
||||
acados_size_t ocp_nlp_dynamics_cont_workspace_calculate_size(void *config, void *dims, void *opts);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* model
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
void *sim_model;
|
||||
// double *state_transition; // TODO
|
||||
double T; // simulation time
|
||||
} ocp_nlp_dynamics_cont_model;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_dynamics_cont_model_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_dynamics_cont_model_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_model_set(void *config_, void *dims_, void *model_, const char *field, void *value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* functions
|
||||
************************************************/
|
||||
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_config_initialize_default(void *config);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
|
||||
//
|
||||
void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
|
||||
//
|
||||
int ocp_nlp_dynamics_cont_precompute(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
|
||||
/// @}
|
||||
/// @}
|
||||
192
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h
vendored
Normal file
192
third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h
vendored
Normal file
@@ -0,0 +1,192 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_dynamics
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
/************************************************
|
||||
* dims
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nx; // number of states at the current stage
|
||||
int nu; // number of inputs at the current stage
|
||||
int nx1; // number of states at the next stage
|
||||
int nu1; // number of inputes at the next stage
|
||||
} ocp_nlp_dynamics_disc_dims;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_dynamics_disc_dims_calculate_size(void *config);
|
||||
//
|
||||
void *ocp_nlp_dynamics_disc_dims_assign(void *config, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_dims_set(void *config_, void *dims_, const char *dim, int* value);
|
||||
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int compute_adj;
|
||||
int compute_hess;
|
||||
} ocp_nlp_dynamics_disc_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_dynamics_disc_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_dynamics_disc_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_opts_initialize_default(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_opts_update(void *config, void *dims, void *opts);
|
||||
//
|
||||
int ocp_nlp_dynamics_disc_precompute(void *config_, void *dims, void *model_, void *opts_,
|
||||
void *mem_, void *work_);
|
||||
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dvec fun;
|
||||
struct blasfeo_dvec adj;
|
||||
struct blasfeo_dvec *ux; // pointer to ux in nlp_out at current stage
|
||||
struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out at current stage
|
||||
struct blasfeo_dvec *ux1; // pointer to ux in nlp_out at next stage
|
||||
struct blasfeo_dvec *tmp_ux1;// pointer to ux in tmp_nlp_out at next stage
|
||||
struct blasfeo_dvec *pi; // pointer to pi in nlp_out at current stage
|
||||
struct blasfeo_dvec *tmp_pi; // pointer to pi in tmp_nlp_out at current stage
|
||||
struct blasfeo_dmat *BAbt; // pointer to BAbt in qp_in
|
||||
struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in
|
||||
} ocp_nlp_dynamics_disc_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_dynamics_disc_memory_calculate_size(void *config, void *dims, void *opts);
|
||||
//
|
||||
void *ocp_nlp_dynamics_disc_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
|
||||
//
|
||||
struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_fun_ptr(void *memory);
|
||||
//
|
||||
struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_adj_ptr(void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dmat tmp_nv_nv;
|
||||
} ocp_nlp_dynamics_disc_workspace;
|
||||
|
||||
acados_size_t ocp_nlp_dynamics_disc_workspace_calculate_size(void *config, void *dims, void *opts);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* model
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
external_function_generic *disc_dyn_fun;
|
||||
external_function_generic *disc_dyn_fun_jac;
|
||||
external_function_generic *disc_dyn_fun_jac_hess;
|
||||
} ocp_nlp_dynamics_disc_model;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_dynamics_disc_model_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_dynamics_disc_model_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_model_set(void *config_, void *dims_, void *model_, const char *field, void *value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* functions
|
||||
************************************************/
|
||||
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_config_initialize_default(void *config);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
|
||||
//
|
||||
void ocp_nlp_dynamics_disc_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
|
||||
/// @}
|
||||
/// @}
|
||||
122
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h
vendored
Normal file
122
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h
vendored
Normal file
@@ -0,0 +1,122 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \ingroup ocp_nlp
|
||||
/// @{
|
||||
|
||||
/// \defgroup ocp_nlp_reg ocp_nlp_reg
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
|
||||
|
||||
|
||||
/* dims */
|
||||
|
||||
//typedef ocp_qp_dims ocp_nlp_reg_dims;
|
||||
typedef struct
|
||||
{
|
||||
int *nx;
|
||||
int *nu;
|
||||
int *nbu;
|
||||
int *nbx;
|
||||
int *ng;
|
||||
int N;
|
||||
} ocp_nlp_reg_dims;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_dims_calculate_size(int N);
|
||||
//
|
||||
ocp_nlp_reg_dims *ocp_nlp_reg_dims_assign(int N, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_reg_dims_set(void *config_, ocp_nlp_reg_dims *dims, int stage, char *field, int* value);
|
||||
|
||||
|
||||
|
||||
/* config */
|
||||
|
||||
typedef struct
|
||||
{
|
||||
/* dims */
|
||||
acados_size_t (*dims_calculate_size)(int N);
|
||||
ocp_nlp_reg_dims *(*dims_assign)(int N, void *raw_memory);
|
||||
void (*dims_set)(void *config, ocp_nlp_reg_dims *dims, int stage, char *field, int *value);
|
||||
/* opts */
|
||||
acados_size_t (*opts_calculate_size)(void);
|
||||
void *(*opts_assign)(void *raw_memory);
|
||||
void (*opts_initialize_default)(void *config, ocp_nlp_reg_dims *dims, void *opts);
|
||||
void (*opts_set)(void *config, ocp_nlp_reg_dims *dims, void *opts, char *field, void* value);
|
||||
/* memory */
|
||||
acados_size_t (*memory_calculate_size)(void *config, ocp_nlp_reg_dims *dims, void *opts);
|
||||
void *(*memory_assign)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
|
||||
void (*memory_set)(void *config, ocp_nlp_reg_dims *dims, void *memory, char *field, void* value);
|
||||
void (*memory_set_RSQrq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
|
||||
void (*memory_set_rq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
|
||||
void (*memory_set_BAbt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
|
||||
void (*memory_set_b_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
|
||||
void (*memory_set_idxb_ptr)(ocp_nlp_reg_dims *dims, int **idxb, void *memory);
|
||||
void (*memory_set_DCt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
|
||||
void (*memory_set_ux_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
|
||||
void (*memory_set_pi_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
|
||||
void (*memory_set_lam_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
|
||||
/* functions */
|
||||
void (*regularize_hessian)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory);
|
||||
void (*correct_dual_sol)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory);
|
||||
} ocp_nlp_reg_config;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_config_calculate_size(void);
|
||||
//
|
||||
void *ocp_nlp_reg_config_assign(void *raw_memory);
|
||||
|
||||
|
||||
|
||||
/* regularization help functions */
|
||||
void acados_reconstruct_A(int dim, double *A, double *V, double *d);
|
||||
void acados_mirror(int dim, double *A, double *V, double *d, double *e, double epsilon);
|
||||
void acados_project(int dim, double *A, double *V, double *d, double *e, double epsilon);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
|
||||
/// @}
|
||||
/// @}
|
||||
146
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h
vendored
Normal file
146
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h
vendored
Normal file
@@ -0,0 +1,146 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_reg
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* dims
|
||||
************************************************/
|
||||
|
||||
// use the functions in ocp_nlp_reg_common
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double delta;
|
||||
double epsilon;
|
||||
// double gamma; // 0.0
|
||||
} ocp_nlp_reg_convexify_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_convexify_opts_calculate_size(void);
|
||||
//
|
||||
void *ocp_nlp_reg_convexify_opts_assign(void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_reg_convexify_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
|
||||
//
|
||||
void ocp_nlp_reg_convexify_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
|
||||
typedef struct {
|
||||
double *R;
|
||||
double *V; // TODO move to workspace
|
||||
double *d; // TODO move to workspace
|
||||
double *e; // TODO move to workspace
|
||||
double *reg_hess; // TODO move to workspace
|
||||
|
||||
struct blasfeo_dmat Q_tilde;
|
||||
struct blasfeo_dmat Q_bar;
|
||||
struct blasfeo_dmat BAQ;
|
||||
struct blasfeo_dmat L;
|
||||
struct blasfeo_dmat delta_eye;
|
||||
struct blasfeo_dmat St_copy;
|
||||
|
||||
struct blasfeo_dmat *original_RSQrq;
|
||||
struct blasfeo_dmat tmp_RSQ;
|
||||
|
||||
struct blasfeo_dvec tmp_nuxM;
|
||||
struct blasfeo_dvec tmp_nbgM;
|
||||
|
||||
// struct blasfeo_dvec grad;
|
||||
// struct blasfeo_dvec b2;
|
||||
|
||||
// giaf's
|
||||
struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in
|
||||
struct blasfeo_dvec **rq; // pointer to rq in qp_in
|
||||
struct blasfeo_dmat **BAbt; // pointer to BAbt in qp_in
|
||||
struct blasfeo_dvec **b; // pointer to b in qp_in
|
||||
struct blasfeo_dmat **DCt; // pointer to DCt in qp_in
|
||||
struct blasfeo_dvec **ux; // pointer to ux in qp_out
|
||||
struct blasfeo_dvec **pi; // pointer to pi in qp_out
|
||||
struct blasfeo_dvec **lam; // pointer to lam in qp_out
|
||||
int **idxb; // pointer to idxb in qp_in
|
||||
|
||||
} ocp_nlp_reg_convexify_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_convexify_calculate_memory_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
|
||||
//
|
||||
void *ocp_nlp_reg_convexify_assign_memory(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
// TODO
|
||||
|
||||
/************************************************
|
||||
* functions
|
||||
************************************************/
|
||||
|
||||
//
|
||||
void ocp_nlp_reg_convexify_config_initialize_default(ocp_nlp_reg_config *config);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
|
||||
/// @}
|
||||
/// @}
|
||||
121
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h
vendored
Normal file
121
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h
vendored
Normal file
@@ -0,0 +1,121 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_reg
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* dims
|
||||
************************************************/
|
||||
|
||||
// use the functions in ocp_nlp_reg_common
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double epsilon;
|
||||
} ocp_nlp_reg_mirror_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_mirror_opts_calculate_size(void);
|
||||
//
|
||||
void *ocp_nlp_reg_mirror_opts_assign(void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_reg_mirror_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
|
||||
//
|
||||
void ocp_nlp_reg_mirror_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double *reg_hess; // TODO move to workspace
|
||||
double *V; // TODO move to workspace
|
||||
double *d; // TODO move to workspace
|
||||
double *e; // TODO move to workspace
|
||||
|
||||
// giaf's
|
||||
struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in
|
||||
} ocp_nlp_reg_mirror_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_mirror_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
|
||||
//
|
||||
void *ocp_nlp_reg_mirror_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
// TODO
|
||||
|
||||
/************************************************
|
||||
* functions
|
||||
************************************************/
|
||||
|
||||
//
|
||||
void ocp_nlp_reg_mirror_config_initialize_default(ocp_nlp_reg_config *config);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
|
||||
/// @}
|
||||
/// @}
|
||||
106
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h
vendored
Normal file
106
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h
vendored
Normal file
@@ -0,0 +1,106 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_reg
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* dims
|
||||
************************************************/
|
||||
|
||||
// use the functions in ocp_nlp_reg_common
|
||||
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_noreg_opts_calculate_size(void);
|
||||
//
|
||||
void *ocp_nlp_reg_noreg_opts_assign(void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_reg_noreg_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
|
||||
//
|
||||
void ocp_nlp_reg_noreg_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_noreg_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
|
||||
//
|
||||
void *ocp_nlp_reg_noreg_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
// not needed
|
||||
|
||||
/************************************************
|
||||
* functions
|
||||
************************************************/
|
||||
|
||||
//
|
||||
void ocp_nlp_reg_noreg_config_initialize_default(ocp_nlp_reg_config *config);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
|
||||
|
||||
/// @}
|
||||
/// @}
|
||||
121
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h
vendored
Normal file
121
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h
vendored
Normal file
@@ -0,0 +1,121 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_reg
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* dims
|
||||
************************************************/
|
||||
|
||||
// use the functions in ocp_nlp_reg_common
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double epsilon;
|
||||
} ocp_nlp_reg_project_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_project_opts_calculate_size(void);
|
||||
//
|
||||
void *ocp_nlp_reg_project_opts_assign(void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_reg_project_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
|
||||
//
|
||||
void ocp_nlp_reg_project_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double *reg_hess; // TODO move to workspace
|
||||
double *V; // TODO move to workspace
|
||||
double *d; // TODO move to workspace
|
||||
double *e; // TODO move to workspace
|
||||
|
||||
// giaf's
|
||||
struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in
|
||||
} ocp_nlp_reg_project_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_project_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
|
||||
//
|
||||
void *ocp_nlp_reg_project_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
// TODO
|
||||
|
||||
/************************************************
|
||||
* functions
|
||||
************************************************/
|
||||
|
||||
//
|
||||
void ocp_nlp_reg_project_config_initialize_default(ocp_nlp_reg_config *config);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
|
||||
/// @}
|
||||
/// @}
|
||||
132
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h
vendored
Normal file
132
third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h
vendored
Normal file
@@ -0,0 +1,132 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_reg
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* dims
|
||||
************************************************/
|
||||
|
||||
// use the functions in ocp_nlp_reg_common
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double thr_eig;
|
||||
double min_eig;
|
||||
double min_pivot;
|
||||
int pivoting;
|
||||
} ocp_nlp_reg_project_reduc_hess_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_project_reduc_hess_opts_calculate_size(void);
|
||||
//
|
||||
void *ocp_nlp_reg_project_reduc_hess_opts_assign(void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_reg_project_reduc_hess_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
|
||||
//
|
||||
void ocp_nlp_reg_project_reduc_hess_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double *reg_hess; // TODO move to workspace
|
||||
double *V; // TODO move to workspace
|
||||
double *d; // TODO move to workspace
|
||||
double *e; // TODO move to workspace
|
||||
|
||||
// giaf's
|
||||
struct blasfeo_dmat L; // TODO move to workspace
|
||||
struct blasfeo_dmat L2; // TODO move to workspace
|
||||
struct blasfeo_dmat L3; // TODO move to workspace
|
||||
struct blasfeo_dmat Ls; // TODO move to workspace
|
||||
struct blasfeo_dmat P; // TODO move to workspace
|
||||
struct blasfeo_dmat AL; // TODO move to workspace
|
||||
|
||||
struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in
|
||||
struct blasfeo_dmat **BAbt; // pointer to RSQrq in qp_in
|
||||
} ocp_nlp_reg_project_reduc_hess_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_reg_project_reduc_hess_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
|
||||
//
|
||||
void *ocp_nlp_reg_project_reduc_hess_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
// TODO
|
||||
|
||||
/************************************************
|
||||
* functions
|
||||
************************************************/
|
||||
|
||||
//
|
||||
void ocp_nlp_reg_project_reduc_hess_config_initialize_default(ocp_nlp_reg_config *config);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
|
||||
/// @}
|
||||
/// @}
|
||||
170
third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h
vendored
Normal file
170
third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h
vendored
Normal file
@@ -0,0 +1,170 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_solver
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_sqp ocp_nlp_sqp
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_SQP_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_nlp/ocp_nlp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
ocp_nlp_opts *nlp_opts;
|
||||
double tol_stat; // exit tolerance on stationarity condition
|
||||
double tol_eq; // exit tolerance on equality constraints
|
||||
double tol_ineq; // exit tolerance on inequality constraints
|
||||
double tol_comp; // exit tolerance on complementarity condition
|
||||
int max_iter;
|
||||
int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
|
||||
int qp_warm_start; // qp_warm_start in all but the first sqp iterations
|
||||
bool warm_start_first_qp; // to set qp_warm_start in first iteration
|
||||
int rti_phase; // only phase 0 at the moment
|
||||
int initialize_t_slacks; // 0-false or 1-true
|
||||
|
||||
} ocp_nlp_sqp_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_sqp_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_nlp_sqp_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_sqp_opts_initialize_default(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_sqp_opts_update(void *config, void *dims, void *opts);
|
||||
//
|
||||
void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* value);
|
||||
//
|
||||
void ocp_nlp_sqp_opts_set_at_stage(void *config_, void *opts_, size_t stage, const char *field, void* value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
// nlp memory
|
||||
ocp_nlp_memory *nlp_mem;
|
||||
|
||||
double time_qp_sol;
|
||||
double time_qp_solver_call;
|
||||
double time_qp_xcond;
|
||||
double time_lin;
|
||||
double time_reg;
|
||||
double time_tot;
|
||||
double time_glob;
|
||||
double time_sim;
|
||||
double time_sim_la;
|
||||
double time_sim_ad;
|
||||
double time_solution_sensitivities;
|
||||
|
||||
// statistics
|
||||
double *stat;
|
||||
int stat_m;
|
||||
int stat_n;
|
||||
|
||||
int status;
|
||||
int sqp_iter;
|
||||
|
||||
} ocp_nlp_sqp_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_sqp_memory_calculate_size(void *config, void *dims, void *opts_);
|
||||
//
|
||||
void *ocp_nlp_sqp_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_sqp_memory_reset_qp_solver(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
|
||||
void *opts_, void *mem_, void *work_);
|
||||
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
ocp_nlp_workspace *nlp_work;
|
||||
|
||||
// temp QP in & out (to be used as workspace in param sens)
|
||||
ocp_qp_in *tmp_qp_in;
|
||||
ocp_qp_out *tmp_qp_out;
|
||||
|
||||
// qp residuals
|
||||
ocp_qp_res *qp_res;
|
||||
ocp_qp_res_ws *qp_res_ws;
|
||||
|
||||
} ocp_nlp_sqp_workspace;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_sqp_workspace_calculate_size(void *config, void *dims, void *opts_);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* functions
|
||||
************************************************/
|
||||
|
||||
//
|
||||
int ocp_nlp_sqp(void *config, void *dims, void *nlp_in, void *nlp_out,
|
||||
void *args, void *mem, void *work_);
|
||||
//
|
||||
void ocp_nlp_sqp_config_initialize_default(void *config_);
|
||||
//
|
||||
int ocp_nlp_sqp_precompute(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
|
||||
void *opts_, void *mem_, void *work_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_SQP_H_
|
||||
/// @}
|
||||
/// @}
|
||||
/// @}
|
||||
163
third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h
vendored
Normal file
163
third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h
vendored
Normal file
@@ -0,0 +1,163 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
/// \addtogroup ocp_nlp
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_solver
|
||||
/// @{
|
||||
/// \addtogroup ocp_nlp_sqp_rti ocp_nlp_sqp_rti
|
||||
/// @{
|
||||
|
||||
#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
|
||||
#define ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_nlp/ocp_nlp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* options
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
ocp_nlp_opts *nlp_opts;
|
||||
int compute_dual_sol;
|
||||
int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
|
||||
int qp_warm_start; // NOTE: this is not actually setting the warm_start! Just for compatibility with sqp.
|
||||
bool warm_start_first_qp; // to set qp_warm_start in first iteration
|
||||
int rti_phase; // phase of RTI. Possible values 1 (preparation), 2 (feedback) 0 (both)
|
||||
|
||||
} ocp_nlp_sqp_rti_opts;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_sqp_rti_opts_calculate_size(void *config_, void *dims_);
|
||||
//
|
||||
void *ocp_nlp_sqp_rti_opts_assign(void *config_, void *dims_, void *raw_memory);
|
||||
//
|
||||
void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, void *dims_, void *opts_);
|
||||
//
|
||||
void ocp_nlp_sqp_rti_opts_update(void *config_, void *dims_, void *opts_);
|
||||
//
|
||||
void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, void* value);
|
||||
//
|
||||
void ocp_nlp_sqp_rti_opts_set_at_stage(void *config_, void *opts_, size_t stage,
|
||||
const char *field, void* value);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* memory
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
// nlp memory
|
||||
ocp_nlp_memory *nlp_mem;
|
||||
|
||||
double time_qp_sol;
|
||||
double time_qp_solver_call;
|
||||
double time_qp_xcond;
|
||||
double time_lin;
|
||||
double time_reg;
|
||||
double time_tot;
|
||||
double time_glob;
|
||||
double time_solution_sensitivities;
|
||||
|
||||
// statistics
|
||||
double *stat;
|
||||
int stat_m;
|
||||
int stat_n;
|
||||
|
||||
int status;
|
||||
|
||||
} ocp_nlp_sqp_rti_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_sqp_rti_memory_calculate_size(void *config_, void *dims_, void *opts_);
|
||||
//
|
||||
void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, void *opts_,
|
||||
void *raw_memory);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* workspace
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
ocp_nlp_workspace *nlp_work;
|
||||
|
||||
// temp QP in & out (to be used as workspace in param sens)
|
||||
ocp_qp_in *tmp_qp_in;
|
||||
ocp_qp_out *tmp_qp_out;
|
||||
|
||||
// qp residuals
|
||||
ocp_qp_res *qp_res;
|
||||
ocp_qp_res_ws *qp_res_ws;
|
||||
|
||||
|
||||
} ocp_nlp_sqp_rti_workspace;
|
||||
|
||||
//
|
||||
acados_size_t ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, void *dims_, void *opts_);
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* functions
|
||||
************************************************/
|
||||
//
|
||||
int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
|
||||
void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void ocp_nlp_sqp_rti_config_initialize_default(void *config_);
|
||||
//
|
||||
int ocp_nlp_sqp_rti_precompute(void *config_, void *dims_,
|
||||
void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
|
||||
/// @}
|
||||
/// @}
|
||||
/// @}
|
||||
180
third_party/acados/include/acados/ocp_qp/ocp_qp_common.h
vendored
Normal file
180
third_party/acados/include/acados/ocp_qp/ocp_qp_common.h
vendored
Normal file
@@ -0,0 +1,180 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_OCP_QP_OCP_QP_COMMON_H_
|
||||
#define ACADOS_OCP_QP_OCP_QP_COMMON_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// hpipm
|
||||
#include "hpipm/include/hpipm_d_ocp_qp.h"
|
||||
#include "hpipm/include/hpipm_d_ocp_qp_dim.h"
|
||||
#include "hpipm/include/hpipm_d_ocp_qp_res.h"
|
||||
#include "hpipm/include/hpipm_d_ocp_qp_sol.h"
|
||||
// acados
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
typedef struct d_ocp_qp_dim ocp_qp_dims;
|
||||
typedef struct d_ocp_qp ocp_qp_in;
|
||||
typedef struct d_ocp_qp_sol ocp_qp_out;
|
||||
typedef struct d_ocp_qp_res ocp_qp_res;
|
||||
typedef struct d_ocp_qp_res_ws ocp_qp_res_ws;
|
||||
|
||||
|
||||
|
||||
#ifndef QP_SOLVER_CONFIG_
|
||||
#define QP_SOLVER_CONFIG_
|
||||
typedef struct
|
||||
{
|
||||
void (*dims_set)(void *config_, void *dims_, int stage, const char *field, int* value);
|
||||
acados_size_t (*opts_calculate_size)(void *config, void *dims);
|
||||
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
|
||||
void (*opts_initialize_default)(void *config, void *dims, void *opts);
|
||||
void (*opts_update)(void *config, void *dims, void *opts);
|
||||
void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
|
||||
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
|
||||
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
|
||||
void (*memory_get)(void *config_, void *mem_, const char *field, void* value);
|
||||
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
|
||||
int (*evaluate)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
|
||||
void (*memory_reset)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
|
||||
void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
|
||||
} qp_solver_config;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
acados_size_t (*dims_calculate_size)(void *config, int N);
|
||||
void *(*dims_assign)(void *config, int N, void *raw_memory);
|
||||
void (*dims_set)(void *config, void *dims_, int stage, const char *field, int* value);
|
||||
void (*dims_get)(void *config, void *dims, const char *field, void* value);
|
||||
// TODO add config everywhere !!!!!
|
||||
acados_size_t (*opts_calculate_size)(void *dims);
|
||||
void *(*opts_assign)(void *dims, void *raw_memory);
|
||||
void (*opts_initialize_default)(void *dims, void *opts);
|
||||
void (*opts_update)(void *dims, void *opts);
|
||||
void (*opts_set)(void *opts_, const char *field, void* value);
|
||||
acados_size_t (*memory_calculate_size)(void *dims, void *opts);
|
||||
void *(*memory_assign)(void *dims, void *opts, void *raw_memory);
|
||||
void (*memory_get)(void *config, void *mem, const char *field, void* value);
|
||||
acados_size_t (*workspace_calculate_size)(void *dims, void *opts);
|
||||
int (*condensing)(void *qp_in, void *qp_out, void *opts, void *mem, void *work);
|
||||
int (*condensing_rhs)(void *qp_in, void *qp_out, void *opts, void *mem, void *work);
|
||||
int (*expansion)(void *qp_in, void *qp_out, void *opts, void *mem, void *work);
|
||||
} ocp_qp_xcond_config;
|
||||
|
||||
|
||||
|
||||
/// Struct containing metrics of the qp solver.
|
||||
#ifndef QP_INFO_
|
||||
#define QP_INFO_
|
||||
typedef struct
|
||||
{
|
||||
double solve_QP_time;
|
||||
double condensing_time;
|
||||
double interface_time;
|
||||
double total_time;
|
||||
int num_iter;
|
||||
int t_computed;
|
||||
} qp_info;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
/* config */
|
||||
//
|
||||
acados_size_t ocp_qp_solver_config_calculate_size();
|
||||
//
|
||||
qp_solver_config *ocp_qp_solver_config_assign(void *raw_memory);
|
||||
//
|
||||
acados_size_t ocp_qp_condensing_config_calculate_size();
|
||||
//
|
||||
ocp_qp_xcond_config *ocp_qp_condensing_config_assign(void *raw_memory);
|
||||
|
||||
|
||||
/* dims */
|
||||
//
|
||||
acados_size_t ocp_qp_dims_calculate_size(int N);
|
||||
//
|
||||
ocp_qp_dims *ocp_qp_dims_assign(int N, void *raw_memory);
|
||||
//
|
||||
void ocp_qp_dims_set(void *config_, void *dims, int stage, const char *field, int* value);
|
||||
//
|
||||
void ocp_qp_dims_get(void *config_, void *dims, int stage, const char *field, int* value);
|
||||
|
||||
|
||||
/* in */
|
||||
//
|
||||
acados_size_t ocp_qp_in_calculate_size(ocp_qp_dims *dims);
|
||||
//
|
||||
ocp_qp_in *ocp_qp_in_assign(ocp_qp_dims *dims, void *raw_memory);
|
||||
|
||||
|
||||
/* out */
|
||||
//
|
||||
acados_size_t ocp_qp_out_calculate_size(ocp_qp_dims *dims);
|
||||
//
|
||||
ocp_qp_out *ocp_qp_out_assign(ocp_qp_dims *dims, void *raw_memory);
|
||||
|
||||
/* res */
|
||||
//
|
||||
acados_size_t ocp_qp_res_calculate_size(ocp_qp_dims *dims);
|
||||
//
|
||||
ocp_qp_res *ocp_qp_res_assign(ocp_qp_dims *dims, void *raw_memory);
|
||||
//
|
||||
acados_size_t ocp_qp_res_workspace_calculate_size(ocp_qp_dims *dims);
|
||||
//
|
||||
ocp_qp_res_ws *ocp_qp_res_workspace_assign(ocp_qp_dims *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res, ocp_qp_res_ws *res_ws);
|
||||
//
|
||||
void ocp_qp_res_compute_nrm_inf(ocp_qp_res *qp_res, double res[4]);
|
||||
|
||||
|
||||
/* misc */
|
||||
//
|
||||
void ocp_qp_stack_slacks_dims(ocp_qp_dims *in, ocp_qp_dims *out);
|
||||
//
|
||||
void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out);
|
||||
//
|
||||
void ocp_qp_compute_t(ocp_qp_in *qp_in, ocp_qp_out *qp_out);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_QP_OCP_QP_COMMON_H_
|
||||
118
third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h
vendored
Normal file
118
third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h
vendored
Normal file
@@ -0,0 +1,118 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_
|
||||
#define ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int N;
|
||||
int *nx;
|
||||
int *nu;
|
||||
int *nb;
|
||||
int *nc;
|
||||
double **A;
|
||||
double **B;
|
||||
double **b;
|
||||
double **Q;
|
||||
double **S;
|
||||
double **R;
|
||||
double **q;
|
||||
double **r;
|
||||
int **idxb;
|
||||
double **lb;
|
||||
double **ub;
|
||||
double **Cx;
|
||||
double **Cu;
|
||||
double **lc;
|
||||
double **uc;
|
||||
} colmaj_ocp_qp_in;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double **x;
|
||||
double **u;
|
||||
double **pi;
|
||||
double **lam;
|
||||
} colmaj_ocp_qp_out;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double **res_r;
|
||||
double **res_q;
|
||||
double **res_ls;
|
||||
double **res_us;
|
||||
double **res_b;
|
||||
double **res_d_lb;
|
||||
double **res_d_ub;
|
||||
double **res_d_lg;
|
||||
double **res_d_ug;
|
||||
double **res_d_ls;
|
||||
double **res_d_us;
|
||||
double **res_m_lb;
|
||||
double **res_m_ub;
|
||||
double **res_m_lg;
|
||||
double **res_m_ug;
|
||||
double **res_m_ls;
|
||||
double **res_m_us;
|
||||
double res_nrm_inf[4];
|
||||
} colmaj_ocp_qp_res;
|
||||
|
||||
//
|
||||
acados_size_t colmaj_ocp_qp_in_calculate_size(ocp_qp_dims *dims);
|
||||
//
|
||||
char *assign_colmaj_ocp_qp_in(ocp_qp_dims *dims, colmaj_ocp_qp_in **qp_in, void *ptr);
|
||||
//
|
||||
acados_size_t colmaj_ocp_qp_out_calculate_size(ocp_qp_dims *dims);
|
||||
//
|
||||
char *assign_colmaj_ocp_qp_out(ocp_qp_dims *dims, colmaj_ocp_qp_out **qp_out, void *ptr);
|
||||
//
|
||||
acados_size_t colmaj_ocp_qp_res_calculate_size(ocp_qp_dims *dims);
|
||||
//
|
||||
char *assign_colmaj_ocp_qp_res(ocp_qp_dims *dims, colmaj_ocp_qp_res **qp_res, void *ptr);
|
||||
//
|
||||
void convert_colmaj_to_ocp_qp_in(colmaj_ocp_qp_in *cm_qp_in, ocp_qp_in *qp_in);
|
||||
//
|
||||
void convert_ocp_qp_out_to_colmaj(ocp_qp_out *qp_out, colmaj_ocp_qp_out *cm_qp_out);
|
||||
//
|
||||
void convert_ocp_qp_res_to_colmaj(ocp_qp_res *qp_res, colmaj_ocp_qp_res *cm_qp_res);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_
|
||||
114
third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h
vendored
Normal file
114
third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h
vendored
Normal file
@@ -0,0 +1,114 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_
|
||||
#define ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// hpipm
|
||||
#include "hpipm/include/hpipm_d_ocp_qp_red.h"
|
||||
// acados
|
||||
#include "acados/dense_qp/dense_qp_common.h"
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
ocp_qp_dims *orig_dims;
|
||||
ocp_qp_dims *red_dims; // dims of reduced qp
|
||||
dense_qp_dims *fcond_dims;
|
||||
} ocp_qp_full_condensing_dims;
|
||||
|
||||
|
||||
|
||||
typedef struct ocp_qp_full_condensing_opts_
|
||||
{
|
||||
struct d_cond_qp_arg *hpipm_cond_opts;
|
||||
struct d_ocp_qp_reduce_eq_dof_arg *hpipm_red_opts;
|
||||
// dense_qp_dims *fcond_dims; // TODO(all): move to dims
|
||||
int cond_hess; // 0 cond only rhs, 1 cond hess + rhs
|
||||
int expand_dual_sol; // 0 primal sol only, 1 primal + dual sol
|
||||
int ric_alg;
|
||||
int mem_qp_in; // allocate qp_in in memory
|
||||
} ocp_qp_full_condensing_opts;
|
||||
|
||||
|
||||
|
||||
typedef struct ocp_qp_full_condensing_memory_
|
||||
{
|
||||
struct d_cond_qp_ws *hpipm_cond_work;
|
||||
struct d_ocp_qp_reduce_eq_dof_ws *hpipm_red_work;
|
||||
// in memory
|
||||
dense_qp_in *fcond_qp_in;
|
||||
dense_qp_out *fcond_qp_out;
|
||||
ocp_qp_in *red_qp; // reduced qp
|
||||
ocp_qp_out *red_sol; // reduced qp sol
|
||||
// only pointer
|
||||
ocp_qp_in *ptr_qp_in;
|
||||
qp_info *qp_out_info; // info in fcond_qp_in
|
||||
double time_qp_xcond;
|
||||
} ocp_qp_full_condensing_memory;
|
||||
|
||||
|
||||
|
||||
//
|
||||
acados_size_t ocp_qp_full_condensing_opts_calculate_size(void *dims);
|
||||
//
|
||||
void *ocp_qp_full_condensing_opts_assign(void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_qp_full_condensing_opts_initialize_default(void *dims, void *opts_);
|
||||
//
|
||||
void ocp_qp_full_condensing_opts_update(void *dims, void *opts_);
|
||||
//
|
||||
void ocp_qp_full_condensing_opts_set(void *opts_, const char *field, void* value);
|
||||
//
|
||||
acados_size_t ocp_qp_full_condensing_memory_calculate_size(void *dims, void *opts_);
|
||||
//
|
||||
void *ocp_qp_full_condensing_memory_assign(void *dims, void *opts_, void *raw_memory);
|
||||
//
|
||||
acados_size_t ocp_qp_full_condensing_workspace_calculate_size(void *dims, void *opts_);
|
||||
//
|
||||
int ocp_qp_full_condensing(void *in, void *out, void *opts, void *mem, void *work);
|
||||
//
|
||||
int ocp_qp_full_expansion(void *in, void *out, void *opts, void *mem, void *work);
|
||||
//
|
||||
void ocp_qp_full_condensing_config_initialize_default(void *config_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_
|
||||
100
third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h
vendored
Normal file
100
third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h
vendored
Normal file
@@ -0,0 +1,100 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_OCP_QP_OCP_QP_HPIPM_H_
|
||||
#define ACADOS_OCP_QP_OCP_QP_HPIPM_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// hpipm
|
||||
#include "hpipm/include/hpipm_d_ocp_qp_ipm.h"
|
||||
// acados
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
// struct of arguments to the solver
|
||||
// TODO(roversch): why not make this a typedef of the underlying struct?
|
||||
typedef struct ocp_qp_hpipm_opts_
|
||||
{
|
||||
struct d_ocp_qp_ipm_arg *hpipm_opts;
|
||||
} ocp_qp_hpipm_opts;
|
||||
|
||||
|
||||
|
||||
// TODO(roversch): why not make this a typedef of the underlying struct?
|
||||
// struct of the solver memory
|
||||
typedef struct ocp_qp_hpipm_memory_
|
||||
{
|
||||
struct d_ocp_qp_ipm_ws *hpipm_workspace;
|
||||
double time_qp_solver_call;
|
||||
int iter;
|
||||
int status;
|
||||
|
||||
} ocp_qp_hpipm_memory;
|
||||
|
||||
|
||||
|
||||
//
|
||||
acados_size_t ocp_qp_hpipm_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_qp_hpipm_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_qp_hpipm_opts_initialize_default(void *config, void *dims, void *opts_);
|
||||
//
|
||||
void ocp_qp_hpipm_opts_update(void *config, void *dims, void *opts_);
|
||||
//
|
||||
void ocp_qp_hpipm_opts_set(void *config_, void *opts_, const char *field, void *value);
|
||||
//
|
||||
acados_size_t ocp_qp_hpipm_memory_calculate_size(void *config, void *dims, void *opts_);
|
||||
//
|
||||
void *ocp_qp_hpipm_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
|
||||
//
|
||||
acados_size_t ocp_qp_hpipm_workspace_calculate_size(void *config, void *dims, void *opts_);
|
||||
//
|
||||
int ocp_qp_hpipm(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void ocp_qp_hpipm_memory_reset(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void ocp_qp_hpipm_eval_sens(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void ocp_qp_hpipm_config_initialize_default(void *config);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_QP_OCP_QP_HPIPM_H_
|
||||
127
third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h
vendored
Normal file
127
third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h
vendored
Normal file
@@ -0,0 +1,127 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_OCP_QP_OCP_QP_HPMPC_H_
|
||||
#define ACADOS_OCP_QP_OCP_QP_HPMPC_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
typedef enum hpmpc_options_t_ { HPMPC_DEFAULT_ARGUMENTS } hpmpc_options_t;
|
||||
|
||||
typedef struct ocp_qp_hpmpc_opts_
|
||||
{
|
||||
double tol;
|
||||
int max_iter;
|
||||
double mu0;
|
||||
double alpha_min;
|
||||
int warm_start;
|
||||
int N2; // horizion length of the partially condensed problem
|
||||
|
||||
// partial tightening
|
||||
double sigma_mu;
|
||||
int N;
|
||||
int M;
|
||||
} ocp_qp_hpmpc_opts;
|
||||
|
||||
// struct of the solver memory
|
||||
typedef struct ocp_qp_hpmpc_memory_
|
||||
{
|
||||
struct blasfeo_dvec *hpi;
|
||||
double *stats;
|
||||
|
||||
// workspace
|
||||
void *hpmpc_work; // raw workspace
|
||||
|
||||
// partial tightening-specific (init of extra variables)
|
||||
struct blasfeo_dvec *lam0;
|
||||
struct blasfeo_dvec *ux0;
|
||||
struct blasfeo_dvec *pi0;
|
||||
struct blasfeo_dvec *t0;
|
||||
|
||||
// 2. workspace
|
||||
struct blasfeo_dmat *hsL;
|
||||
struct blasfeo_dmat *hsric_work_mat;
|
||||
struct blasfeo_dmat sLxM;
|
||||
struct blasfeo_dmat sPpM;
|
||||
|
||||
struct blasfeo_dvec *hsQx;
|
||||
struct blasfeo_dvec *hsqx;
|
||||
struct blasfeo_dvec *hstinv;
|
||||
struct blasfeo_dvec *hsrq;
|
||||
struct blasfeo_dvec *hsdux;
|
||||
|
||||
struct blasfeo_dvec *hsdlam;
|
||||
struct blasfeo_dvec *hsdt;
|
||||
struct blasfeo_dvec *hsdpi;
|
||||
struct blasfeo_dvec *hslamt;
|
||||
|
||||
struct blasfeo_dvec *hsPb;
|
||||
|
||||
void *work_ric;
|
||||
|
||||
int out_iter;
|
||||
|
||||
double time_qp_solver_call;
|
||||
int iter;
|
||||
int status;
|
||||
|
||||
} ocp_qp_hpmpc_memory;
|
||||
|
||||
acados_size_t ocp_qp_hpmpc_opts_calculate_size(void *config_, ocp_qp_dims *dims);
|
||||
//
|
||||
void *ocp_qp_hpmpc_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_qp_hpmpc_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void ocp_qp_hpmpc_opts_update(void *config_, ocp_qp_dims *dims, void *opts_);
|
||||
//
|
||||
acados_size_t ocp_qp_hpmpc_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void *ocp_qp_hpmpc_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory);
|
||||
//
|
||||
acados_size_t ocp_qp_hpmpc_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
|
||||
//
|
||||
int ocp_qp_hpmpc(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void ocp_qp_hpmpc_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void ocp_qp_hpmpc_config_initialize_default(void *config_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_QP_OCP_QP_HPMPC_H_
|
||||
144
third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h
vendored
Normal file
144
third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h
vendored
Normal file
@@ -0,0 +1,144 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_OCP_QP_OCP_QP_OOQP_H_
|
||||
#define ACADOS_OCP_QP_OCP_QP_OOQP_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
enum ocp_qp_ooqp_termination_code
|
||||
{
|
||||
SPARSE_SUCCESSFUL_TERMINATION = 0,
|
||||
SPARSE_NOT_FINISHED,
|
||||
SPARSE_MAX_ITS_EXCEEDED,
|
||||
SPARSE_INFEASIBLE,
|
||||
SPARSE_UNKNOWN
|
||||
};
|
||||
|
||||
typedef struct ocp_qp_ooqp_opts_
|
||||
{
|
||||
int printLevel;
|
||||
int useDiagonalWeights; // TODO(dimitris): implement option
|
||||
int fixHessian;
|
||||
int fixHessianSparsity;
|
||||
int fixDynamics;
|
||||
int fixDynamicsSparsity;
|
||||
int fixInequalities;
|
||||
int fixInequalitiesSparsity;
|
||||
} ocp_qp_ooqp_opts;
|
||||
|
||||
typedef struct ocp_qp_ooqp_workspace_
|
||||
{
|
||||
double *x;
|
||||
double *gamma;
|
||||
double *phi;
|
||||
double *y;
|
||||
double *z;
|
||||
double *lambda;
|
||||
double *pi;
|
||||
double objectiveValue;
|
||||
int *tmpInt; // temporary vector to sort indicies sparse matrices
|
||||
double *tmpReal; // temporary vector to sort data of sparse matrices
|
||||
// int ierr;
|
||||
} ocp_qp_ooqp_workspace;
|
||||
|
||||
typedef struct ocp_qp_ooqp_memory_
|
||||
{
|
||||
int firstRun;
|
||||
double *c;
|
||||
int nx;
|
||||
int *irowQ;
|
||||
int nnzQ;
|
||||
int *jcolQ;
|
||||
int *orderQ;
|
||||
double *dQ;
|
||||
double *xlow;
|
||||
char *ixlow;
|
||||
double *xupp;
|
||||
char *ixupp;
|
||||
int *irowA;
|
||||
int nnzA;
|
||||
int *jcolA;
|
||||
int *orderA;
|
||||
double *dA;
|
||||
double *bA;
|
||||
int my;
|
||||
int *irowC;
|
||||
int nnzC;
|
||||
int *jcolC;
|
||||
int *orderC;
|
||||
double *dC;
|
||||
double *clow;
|
||||
int mz;
|
||||
char *iclow;
|
||||
double *cupp;
|
||||
char *icupp;
|
||||
int nnz; // max(nnzQ, nnzA, nnzC)
|
||||
double time_qp_solver_call;
|
||||
int iter;
|
||||
int status;
|
||||
|
||||
} ocp_qp_ooqp_memory;
|
||||
|
||||
//
|
||||
acados_size_t ocp_qp_ooqp_opts_calculate_size(void *config_, ocp_qp_dims *dims);
|
||||
//
|
||||
void *ocp_qp_ooqp_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_qp_ooqp_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void ocp_qp_ooqp_opts_update(void *config_, ocp_qp_dims *dims, void *opts_);
|
||||
//
|
||||
acados_size_t ocp_qp_ooqp_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void *ocp_qp_ooqp_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory);
|
||||
//
|
||||
acados_size_t ocp_qp_ooqp_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
|
||||
//
|
||||
int ocp_qp_ooqp(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *memory_,
|
||||
void *work_);
|
||||
//
|
||||
void ocp_qp_ooqp_destroy(void *mem_, void *work);
|
||||
//
|
||||
void ocp_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void ocp_qp_ooqp_config_initialize_default(void *config_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_QP_OCP_QP_OOQP_H_
|
||||
103
third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h
vendored
Normal file
103
third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h
vendored
Normal file
@@ -0,0 +1,103 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_OCP_QP_OCP_QP_OSQP_H_
|
||||
#define ACADOS_OCP_QP_OCP_QP_OSQP_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// osqp
|
||||
#include "osqp/include/types.h"
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
typedef struct ocp_qp_osqp_opts_
|
||||
{
|
||||
OSQPSettings *osqp_opts;
|
||||
} ocp_qp_osqp_opts;
|
||||
|
||||
|
||||
typedef struct ocp_qp_osqp_memory_
|
||||
{
|
||||
c_int first_run;
|
||||
|
||||
c_float *q;
|
||||
c_float *l;
|
||||
c_float *u;
|
||||
|
||||
c_int P_nnzmax;
|
||||
c_int *P_i;
|
||||
c_int *P_p;
|
||||
c_float *P_x;
|
||||
|
||||
c_int A_nnzmax;
|
||||
c_int *A_i;
|
||||
c_int *A_p;
|
||||
c_float *A_x;
|
||||
|
||||
OSQPData *osqp_data;
|
||||
OSQPWorkspace *osqp_work;
|
||||
|
||||
double time_qp_solver_call;
|
||||
int iter;
|
||||
int status;
|
||||
|
||||
} ocp_qp_osqp_memory;
|
||||
|
||||
acados_size_t ocp_qp_osqp_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *ocp_qp_osqp_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_qp_osqp_opts_initialize_default(void *config, void *dims, void *opts_);
|
||||
//
|
||||
void ocp_qp_osqp_opts_update(void *config, void *dims, void *opts_);
|
||||
//
|
||||
acados_size_t ocp_qp_osqp_memory_calculate_size(void *config, void *dims, void *opts_);
|
||||
//
|
||||
void *ocp_qp_osqp_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
|
||||
//
|
||||
acados_size_t ocp_qp_osqp_workspace_calculate_size(void *config, void *dims, void *opts_);
|
||||
//
|
||||
int ocp_qp_osqp(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void ocp_qp_osqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void ocp_qp_osqp_config_initialize_default(void *config);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_QP_OCP_QP_OSQP_H_
|
||||
120
third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h
vendored
Normal file
120
third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h
vendored
Normal file
@@ -0,0 +1,120 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_
|
||||
#define ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// hpipm
|
||||
#include "hpipm/include/hpipm_d_ocp_qp_red.h"
|
||||
// acados
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
ocp_qp_dims *orig_dims;
|
||||
ocp_qp_dims *red_dims; // dims of reduced qp
|
||||
ocp_qp_dims *pcond_dims;
|
||||
int *block_size;
|
||||
int N2;
|
||||
int N2_bkp;
|
||||
} ocp_qp_partial_condensing_dims;
|
||||
|
||||
|
||||
|
||||
typedef struct ocp_qp_partial_condensing_opts_
|
||||
{
|
||||
struct d_part_cond_qp_arg *hpipm_pcond_opts;
|
||||
struct d_ocp_qp_reduce_eq_dof_arg *hpipm_red_opts;
|
||||
// ocp_qp_dims *pcond_dims; // TODO(all): move to dims
|
||||
// int *block_size;
|
||||
int N2;
|
||||
int N2_bkp;
|
||||
// int expand_dual_sol; // 0 primal sol only, 1 primal + dual sol
|
||||
int ric_alg;
|
||||
int mem_qp_in; // allocate qp_in in memory
|
||||
} ocp_qp_partial_condensing_opts;
|
||||
|
||||
|
||||
|
||||
typedef struct ocp_qp_partial_condensing_memory_
|
||||
{
|
||||
struct d_part_cond_qp_ws *hpipm_pcond_work;
|
||||
struct d_ocp_qp_reduce_eq_dof_ws *hpipm_red_work;
|
||||
// in memory
|
||||
ocp_qp_in *pcond_qp_in;
|
||||
ocp_qp_out *pcond_qp_out;
|
||||
ocp_qp_in *red_qp; // reduced qp
|
||||
ocp_qp_out *red_sol; // reduced qp sol
|
||||
// only pointer
|
||||
ocp_qp_in *ptr_qp_in;
|
||||
ocp_qp_in *ptr_pcond_qp_in;
|
||||
qp_info *qp_out_info; // info in pcond_qp_in
|
||||
double time_qp_xcond;
|
||||
} ocp_qp_partial_condensing_memory;
|
||||
|
||||
|
||||
|
||||
//
|
||||
acados_size_t ocp_qp_partial_condensing_opts_calculate_size(void *dims);
|
||||
//
|
||||
void *ocp_qp_partial_condensing_opts_assign(void *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_qp_partial_condensing_opts_initialize_default(void *dims, void *opts_);
|
||||
//
|
||||
void ocp_qp_partial_condensing_opts_update(void *dims, void *opts_);
|
||||
//
|
||||
void ocp_qp_partial_condensing_opts_set(void *opts_, const char *field, void* value);
|
||||
//
|
||||
acados_size_t ocp_qp_partial_condensing_memory_calculate_size(void *dims, void *opts_);
|
||||
//
|
||||
void *ocp_qp_partial_condensing_memory_assign(void *dims, void *opts, void *raw_memory);
|
||||
//
|
||||
acados_size_t ocp_qp_partial_condensing_workspace_calculate_size(void *dims, void *opts_);
|
||||
//
|
||||
int ocp_qp_partial_condensing(void *in, void *out, void *opts, void *mem, void *work);
|
||||
//
|
||||
int ocp_qp_partial_expansion(void *in, void *out, void *opts, void *mem, void *work);
|
||||
//
|
||||
void ocp_qp_partial_condensing_config_initialize_default(void *config_);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_
|
||||
119
third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h
vendored
Normal file
119
third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h
vendored
Normal file
@@ -0,0 +1,119 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_OCP_QP_OCP_QP_QPDUNES_H_
|
||||
#define ACADOS_OCP_QP_OCP_QP_QPDUNES_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "qpDUNES.h"
|
||||
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
typedef enum qpdunes_options_t_ {
|
||||
QPDUNES_DEFAULT_ARGUMENTS,
|
||||
QPDUNES_LINEAR_MPC, // TODO(dimitris): partly implemented
|
||||
QPDUNES_NONLINEAR_MPC, // TODO(dimitris): not implemented yet
|
||||
QPDUNES_ACADO_SETTINGS
|
||||
} qpdunes_options_t;
|
||||
|
||||
typedef enum { QPDUNES_WITH_QPOASES, QPDUNES_WITH_CLIPPING } qpdunes_stage_qp_solver_t;
|
||||
|
||||
typedef struct ocp_qp_qpdunes_opts_
|
||||
{
|
||||
qpOptions_t options;
|
||||
qpdunes_stage_qp_solver_t stageQpSolver;
|
||||
int warmstart; // warmstart = 0: all multipliers set to zero, warmstart = 1: use previous mult.
|
||||
bool isLinearMPC;
|
||||
} ocp_qp_qpdunes_opts;
|
||||
|
||||
typedef struct ocp_qp_qpdunes_memory_
|
||||
{
|
||||
int firstRun;
|
||||
int nx;
|
||||
int nu;
|
||||
int nz;
|
||||
int nDmax; // max(dims->ng)
|
||||
qpData_t qpData;
|
||||
double time_qp_solver_call;
|
||||
int iter;
|
||||
int status;
|
||||
|
||||
} ocp_qp_qpdunes_memory;
|
||||
|
||||
typedef struct ocp_qp_qpdunes_workspace_
|
||||
{
|
||||
double *H;
|
||||
double *Q;
|
||||
double *R;
|
||||
double *S;
|
||||
double *g;
|
||||
double *ABt;
|
||||
double *b;
|
||||
double *Ct;
|
||||
double *lc;
|
||||
double *uc;
|
||||
double *zLow;
|
||||
double *zUpp;
|
||||
} ocp_qp_qpdunes_workspace;
|
||||
|
||||
//
|
||||
acados_size_t ocp_qp_qpdunes_opts_calculate_size(void *config_, ocp_qp_dims *dims);
|
||||
//
|
||||
void *ocp_qp_qpdunes_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_qp_qpdunes_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void ocp_qp_qpdunes_opts_update(void *config_, ocp_qp_dims *dims, void *opts_);
|
||||
//
|
||||
acados_size_t ocp_qp_qpdunes_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
|
||||
//
|
||||
void *ocp_qp_qpdunes_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory);
|
||||
//
|
||||
acados_size_t ocp_qp_qpdunes_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
|
||||
//
|
||||
int ocp_qp_qpdunes(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *memory_,
|
||||
void *work_);
|
||||
//
|
||||
void ocp_qp_qpdunes_free_memory(void *mem_);
|
||||
//
|
||||
void ocp_qp_qpdunes_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void ocp_qp_qpdunes_config_initialize_default(void *config_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_QP_OCP_QP_QPDUNES_H_
|
||||
150
third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h
vendored
Normal file
150
third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h
vendored
Normal file
@@ -0,0 +1,150 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_
|
||||
#define ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// acados
|
||||
#include "acados/ocp_qp/ocp_qp_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
ocp_qp_dims *orig_dims;
|
||||
void *xcond_dims;
|
||||
} ocp_qp_xcond_solver_dims;
|
||||
|
||||
|
||||
|
||||
typedef struct ocp_qp_xcond_solver_opts_
|
||||
{
|
||||
void *xcond_opts;
|
||||
void *qp_solver_opts;
|
||||
} ocp_qp_xcond_solver_opts;
|
||||
|
||||
|
||||
|
||||
typedef struct ocp_qp_xcond_solver_memory_
|
||||
{
|
||||
void *xcond_memory;
|
||||
void *solver_memory;
|
||||
void *xcond_qp_in;
|
||||
void *xcond_qp_out;
|
||||
} ocp_qp_xcond_solver_memory;
|
||||
|
||||
|
||||
|
||||
typedef struct ocp_qp_xcond_solver_workspace_
|
||||
{
|
||||
void *xcond_work;
|
||||
void *qp_solver_work;
|
||||
} ocp_qp_xcond_solver_workspace;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
acados_size_t (*dims_calculate_size)(void *config, int N);
|
||||
ocp_qp_xcond_solver_dims *(*dims_assign)(void *config, int N, void *raw_memory);
|
||||
void (*dims_set)(void *config_, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value);
|
||||
void (*dims_get)(void *config_, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value);
|
||||
acados_size_t (*opts_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims);
|
||||
void *(*opts_assign)(void *config, ocp_qp_xcond_solver_dims *dims, void *raw_memory);
|
||||
void (*opts_initialize_default)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
|
||||
void (*opts_update)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
|
||||
void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
|
||||
acados_size_t (*memory_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
|
||||
void *(*memory_assign)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts, void *raw_memory);
|
||||
void (*memory_get)(void *config_, void *mem_, const char *field, void* value);
|
||||
void (*memory_reset)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work);
|
||||
acados_size_t (*workspace_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
|
||||
int (*evaluate)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work);
|
||||
void (*eval_sens)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_out *sens_qp_out, void *opts, void *mem, void *work);
|
||||
qp_solver_config *qp_solver; // either ocp_qp_solver or dense_solver
|
||||
ocp_qp_xcond_config *xcond;
|
||||
} ocp_qp_xcond_solver_config; // pcond - partial condensing or fcond - full condensing
|
||||
|
||||
|
||||
|
||||
/* config */
|
||||
//
|
||||
acados_size_t ocp_qp_xcond_solver_config_calculate_size();
|
||||
//
|
||||
ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_assign(void *raw_memory);
|
||||
|
||||
/* dims */
|
||||
//
|
||||
acados_size_t ocp_qp_xcond_solver_dims_calculate_size(void *config, int N);
|
||||
//
|
||||
ocp_qp_xcond_solver_dims *ocp_qp_xcond_solver_dims_assign(void *config, int N, void *raw_memory);
|
||||
//
|
||||
void ocp_qp_xcond_solver_dims_set_(void *config, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value);
|
||||
|
||||
/* opts */
|
||||
//
|
||||
acados_size_t ocp_qp_xcond_solver_opts_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims);
|
||||
//
|
||||
void *ocp_qp_xcond_solver_opts_assign(void *config, ocp_qp_xcond_solver_dims *dims, void *raw_memory);
|
||||
//
|
||||
void ocp_qp_xcond_solver_opts_initialize_default(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_);
|
||||
//
|
||||
void ocp_qp_xcond_solver_opts_update(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_);
|
||||
//
|
||||
void ocp_qp_xcond_solver_opts_set_(void *config_, void *opts_, const char *field, void* value);
|
||||
|
||||
/* memory */
|
||||
//
|
||||
acados_size_t ocp_qp_xcond_solver_memory_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_);
|
||||
//
|
||||
void *ocp_qp_xcond_solver_memory_assign(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_, void *raw_memory);
|
||||
|
||||
/* workspace */
|
||||
//
|
||||
acados_size_t ocp_qp_xcond_solver_workspace_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_);
|
||||
|
||||
/* config */
|
||||
//
|
||||
int ocp_qp_xcond_solver(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *mem_, void *work_);
|
||||
|
||||
//
|
||||
void ocp_qp_xcond_solver_config_initialize_default(void *config_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_
|
||||
97
third_party/acados/include/acados/sim/sim_collocation_utils.h
vendored
Normal file
97
third_party/acados/include/acados/sim/sim_collocation_utils.h
vendored
Normal file
@@ -0,0 +1,97 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_SIM_SIM_COLLOCATION_UTILS_H_
|
||||
#define ACADOS_SIM_SIM_COLLOCATION_UTILS_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
// enum Newton_type_collocation
|
||||
// {
|
||||
// exact = 0,
|
||||
// simplified_in,
|
||||
// simplified_inis
|
||||
// };
|
||||
|
||||
|
||||
|
||||
// typedef struct
|
||||
// {
|
||||
// enum Newton_type_collocation type;
|
||||
// double *eig;
|
||||
// double *low_tria;
|
||||
// bool single;
|
||||
// bool freeze;
|
||||
|
||||
// double *transf1;
|
||||
// double *transf2;
|
||||
|
||||
// double *transf1_T;
|
||||
// double *transf2_T;
|
||||
// } Newton_scheme;
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
GAUSS_LEGENDRE,
|
||||
GAUSS_RADAU_IIA,
|
||||
} sim_collocation_type;
|
||||
|
||||
|
||||
//
|
||||
// acados_size_t gauss_legendre_nodes_work_calculate_size(int ns);
|
||||
//
|
||||
// void gauss_legendre_nodes(int ns, double *nodes, void *raw_memory);
|
||||
//
|
||||
// acados_size_t gauss_simplified_work_calculate_size(int ns);
|
||||
// //
|
||||
// void gauss_simplified(int ns, Newton_scheme *scheme, void *work);
|
||||
//
|
||||
acados_size_t butcher_tableau_work_calculate_size(int ns);
|
||||
//
|
||||
// void calculate_butcher_tableau_from_nodes(int ns, double *nodes, double *b, double *A, void *work);
|
||||
//
|
||||
void calculate_butcher_tableau(int ns, sim_collocation_type collocation_type, double *c_vec,
|
||||
double *b_vec, double *A_mat, void *work);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SIM_SIM_COLLOCATION_UTILS_H_
|
||||
221
third_party/acados/include/acados/sim/sim_common.h
vendored
Normal file
221
third_party/acados/include/acados/sim/sim_common.h
vendored
Normal file
@@ -0,0 +1,221 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_SIM_SIM_COMMON_H_
|
||||
#define ACADOS_SIM_SIM_COMMON_H_
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "acados/sim/sim_collocation_utils.h"
|
||||
#include "acados/utils/timing.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
|
||||
// maximum number of integration stages
|
||||
#define NS_MAX 15
|
||||
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
// ERK and LIFTED_ERK
|
||||
EXPL_ODE_FUN,
|
||||
EXPL_ODE_HES, // wrt x and u ???
|
||||
EXPL_VDE_FOR,
|
||||
EXPL_VDE_ADJ,
|
||||
// IRK
|
||||
IMPL_ODE_FUN,
|
||||
IMPL_ODE_FUN_JAC_X_XDOT,
|
||||
IMPL_ODE_JAC_X_XDOT_U,
|
||||
IMPL_ODE_FUN_JAC_X_XDOT_U,
|
||||
IMPL_ODE_HESS,
|
||||
// gnsf
|
||||
PHI_FUN,
|
||||
PHI_FUN_JAC_Y,
|
||||
PHI_JAC_Y_UHAT,
|
||||
LO_FUN,
|
||||
GET_GNSF_MATRICES
|
||||
} sim_function_t;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
void *dims;
|
||||
|
||||
double *x; // x[NX] - initial state value for simulation
|
||||
double *u; // u[NU] - control - constant over simulation time
|
||||
|
||||
double *S_forw; // forward seed [Sx, Su]
|
||||
double *S_adj; // backward seed
|
||||
|
||||
bool identity_seed; // indicating if S_forw = [eye(nx), zeros(nx x nu)]
|
||||
|
||||
void *model;
|
||||
|
||||
double T; // simulation time
|
||||
|
||||
} sim_in;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double CPUtime; // in seconds
|
||||
double LAtime; // in seconds
|
||||
double ADtime; // in seconds
|
||||
|
||||
} sim_info;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double *xn; // xn[NX]
|
||||
double *S_forw; // S_forw[NX*(NX+NU)]
|
||||
double *S_adj; //
|
||||
double *S_hess; //
|
||||
|
||||
double *zn; // z - algebraic variables - reported at start of simulation interval
|
||||
double *S_algebraic; // sensitivities of reported value of algebraic variables w.r.t.
|
||||
// initial stat & control (x_n,u)
|
||||
|
||||
double *grad; // gradient correction
|
||||
|
||||
sim_info *info;
|
||||
|
||||
} sim_out;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int ns; // number of integration stages
|
||||
|
||||
int num_steps;
|
||||
int num_forw_sens;
|
||||
|
||||
int tableau_size; // check that is consistent with ns
|
||||
// only update when butcher tableau is changed
|
||||
// kind of private -> no setter!
|
||||
double *A_mat;
|
||||
double *c_vec;
|
||||
double *b_vec;
|
||||
|
||||
bool sens_forw;
|
||||
bool sens_adj;
|
||||
bool sens_hess;
|
||||
|
||||
bool output_z; // 1 -- if zn should be computed
|
||||
bool sens_algebraic; // 1 -- if S_algebraic should be computed
|
||||
bool exact_z_output; // 1 -- if z, S_algebraic should be computed exactly, extra Newton iterations
|
||||
sim_collocation_type collocation_type;
|
||||
|
||||
// for explicit integrators: newton_iter == 0 && scheme == NULL
|
||||
// && jac_reuse=false
|
||||
int newton_iter;
|
||||
bool jac_reuse;
|
||||
// Newton_scheme *scheme;
|
||||
|
||||
double newton_tol; // optinally used in implicit integrators
|
||||
|
||||
// workspace
|
||||
void *work;
|
||||
|
||||
} sim_opts;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int (*evaluate)(void *config_, sim_in *in, sim_out *out, void *opts, void *mem, void *work);
|
||||
int (*precompute)(void *config_, sim_in *in, sim_out *out, void *opts, void *mem, void *work);
|
||||
// opts
|
||||
acados_size_t (*opts_calculate_size)(void *config_, void *dims);
|
||||
void *(*opts_assign)(void *config_, void *dims, void *raw_memory);
|
||||
void (*opts_initialize_default)(void *config_, void *dims, void *opts);
|
||||
void (*opts_update)(void *config_, void *dims, void *opts);
|
||||
void (*opts_set)(void *config_, void *opts_, const char *field, void *value);
|
||||
void (*opts_get)(void *config_, void *opts_, const char *field, void *value);
|
||||
// mem
|
||||
acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
|
||||
void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
|
||||
int (*memory_set)(void *config, void *dims, void *mem, const char *field, void *value);
|
||||
int (*memory_set_to_zero)(void *config, void *dims, void *opts, void *mem, const char *field);
|
||||
void (*memory_get)(void *config, void *dims, void *mem, const char *field, void *value);
|
||||
// work
|
||||
acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
|
||||
// model
|
||||
acados_size_t (*model_calculate_size)(void *config, void *dims);
|
||||
void *(*model_assign)(void *config, void *dims, void *raw_memory);
|
||||
int (*model_set)(void *model, const char *field, void *value);
|
||||
// config
|
||||
void (*config_initialize_default)(void *config);
|
||||
// dims
|
||||
acados_size_t (*dims_calculate_size)();
|
||||
void *(*dims_assign)(void *config, void *raw_memory);
|
||||
void (*dims_set)(void *config, void *dims, const char *field, const int *value);
|
||||
void (*dims_get)(void *config, void *dims, const char *field, int *value);
|
||||
|
||||
} sim_config;
|
||||
|
||||
|
||||
|
||||
/* config */
|
||||
//
|
||||
acados_size_t sim_config_calculate_size();
|
||||
//
|
||||
sim_config *sim_config_assign(void *raw_memory);
|
||||
|
||||
/* in */
|
||||
//
|
||||
acados_size_t sim_in_calculate_size(void *config, void *dims);
|
||||
//
|
||||
sim_in *sim_in_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
int sim_in_set_(void *config_, void *dims_, sim_in *in, const char *field, void *value);
|
||||
|
||||
/* out */
|
||||
//
|
||||
acados_size_t sim_out_calculate_size(void *config, void *dims);
|
||||
//
|
||||
sim_out *sim_out_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
int sim_out_get_(void *config, void *dims, sim_out *out, const char *field, void *value);
|
||||
|
||||
/* opts */
|
||||
//
|
||||
void sim_opts_set_(sim_opts *opts, const char *field, void *value);
|
||||
//
|
||||
void sim_opts_get_(sim_config *config, sim_opts *opts, const char *field, void *value);
|
||||
|
||||
#endif // ACADOS_SIM_SIM_COMMON_H_
|
||||
140
third_party/acados/include/acados/sim/sim_erk_integrator.h
vendored
Normal file
140
third_party/acados/include/acados/sim/sim_erk_integrator.h
vendored
Normal file
@@ -0,0 +1,140 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_SIM_SIM_ERK_INTEGRATOR_H_
|
||||
#define ACADOS_SIM_SIM_ERK_INTEGRATOR_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "acados/sim/sim_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nx;
|
||||
int nu;
|
||||
int nz;
|
||||
} sim_erk_dims;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
/* external functions */
|
||||
// explicit ode
|
||||
external_function_generic *expl_ode_fun;
|
||||
// hessian explicit ode
|
||||
external_function_generic *expl_ode_hes;
|
||||
// forward explicit vde
|
||||
external_function_generic *expl_vde_for;
|
||||
// adjoint explicit vde
|
||||
external_function_generic *expl_vde_adj;
|
||||
|
||||
} erk_model;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
// memory
|
||||
double time_sim;
|
||||
double time_ad;
|
||||
double time_la;
|
||||
|
||||
// workspace structs
|
||||
} sim_erk_memory;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
// workspace mem
|
||||
double *rhs_forw_in; // x + S + p
|
||||
|
||||
double *K_traj; // (stages*nX) or (steps*stages*nX) for adj
|
||||
double *out_forw_traj; // S or (steps+1)*nX for adj
|
||||
|
||||
double *rhs_adj_in;
|
||||
double *out_adj_tmp;
|
||||
double *adj_traj;
|
||||
|
||||
} sim_erk_workspace;
|
||||
|
||||
|
||||
|
||||
// dims
|
||||
acados_size_t sim_erk_dims_calculate_size();
|
||||
void *sim_erk_dims_assign(void *config_, void *raw_memory);
|
||||
void sim_erk_dims_set(void *config_, void *dims_, const char *field, const int* value);
|
||||
void sim_erk_dims_get(void *config_, void *dims_, const char *field, int* value);
|
||||
|
||||
// model
|
||||
acados_size_t sim_erk_model_calculate_size(void *config, void *dims);
|
||||
void *sim_erk_model_assign(void *config, void *dims, void *raw_memory);
|
||||
int sim_erk_model_set(void *model, const char *field, void *value);
|
||||
|
||||
// opts
|
||||
acados_size_t sim_erk_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void sim_erk_opts_update(void *config_, void *dims, void *opts_);
|
||||
//
|
||||
void *sim_erk_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void sim_erk_opts_initialize_default(void *config, void *dims, void *opts_);
|
||||
//
|
||||
void sim_erk_opts_set(void *config_, void *opts_, const char *field, void *value);
|
||||
|
||||
|
||||
// memory
|
||||
acados_size_t sim_erk_memory_calculate_size(void *config, void *dims, void *opts_);
|
||||
//
|
||||
void *sim_erk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
|
||||
//
|
||||
int sim_erk_memory_set(void *config_, void *dims_, void *mem_, const char *field, void *value);
|
||||
|
||||
|
||||
// workspace
|
||||
acados_size_t sim_erk_workspace_calculate_size(void *config, void *dims, void *opts_);
|
||||
|
||||
//
|
||||
int sim_erk(void *config, sim_in *in, sim_out *out, void *opts_, void *mem_, void *work_);
|
||||
//
|
||||
void sim_erk_config_initialize_default(void *config);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SIM_SIM_ERK_INTEGRATOR_H_
|
||||
364
third_party/acados/include/acados/sim/sim_gnsf.h
vendored
Normal file
364
third_party/acados/include/acados/sim/sim_gnsf.h
vendored
Normal file
@@ -0,0 +1,364 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_SIM_SIM_GNSF_H_
|
||||
#define ACADOS_SIM_SIM_GNSF_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "acados/utils/timing.h"
|
||||
#include "acados/utils/types.h"
|
||||
#include "acados/sim/sim_common.h"
|
||||
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
// #include "blasfeo/include/blasfeo_d_aux.h"
|
||||
// #include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
|
||||
// #include "blasfeo/include/blasfeo_d_blas.h"
|
||||
// #include "blasfeo/include/blasfeo_d_kernel.h"
|
||||
// #include "blasfeo/include/blasfeo_i_aux_ext_dep.h"
|
||||
// #include "blasfeo/include/blasfeo_target.h"
|
||||
|
||||
/*
|
||||
GNSF - Generalized Nonlinear Static Feedback Model
|
||||
has the following form
|
||||
https://github.com/acados/acados/files/3359595/gnsf_structure_blo.pdf
|
||||
|
||||
Details on the algorithm can be found in master thesis of Jonathan Frey,
|
||||
which presents a slightly different format without the terms B_LO, c_LO.
|
||||
https://github.com/acados/acados/files/2318322/gnsf_structure.pdf
|
||||
https://cdn.syscop.de/publications/Frey2018.pdf
|
||||
https://cdn.syscop.de/publications/Frey2019.pdf
|
||||
*/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nx; // total number of differential states
|
||||
int nu; // total number of inputs
|
||||
int nz; // total number of algebraic states
|
||||
int nx1; // number of differential states in NSF part
|
||||
int nz1; // number of algebraic states in NSF part
|
||||
int n_out; // output dimension of phi
|
||||
int ny; // dimension of first input of phi
|
||||
int nuhat; // dimension of second input of phi
|
||||
|
||||
} sim_gnsf_dims;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
/* external functions */
|
||||
// phi: nonlinearity function
|
||||
external_function_generic *phi_fun;
|
||||
external_function_generic *phi_fun_jac_y;
|
||||
external_function_generic *phi_jac_y_uhat;
|
||||
|
||||
// f_lo: linear output function
|
||||
external_function_generic *f_lo_fun_jac_x1_x1dot_u_z;
|
||||
|
||||
// to import model matrices
|
||||
external_function_generic *get_gnsf_matrices;
|
||||
|
||||
// flag indicating, if model defining matrices are imported via external (casadi) function,
|
||||
// [default]: true -> auto;
|
||||
bool auto_import_gnsf;
|
||||
|
||||
// booleans from structure detection
|
||||
bool nontrivial_f_LO; // indicates if f_LO is constant zero function
|
||||
bool fully_linear; // indicates if model is fully linear LOS
|
||||
|
||||
/* model defining matrices */
|
||||
// TODO: add setters to set manually
|
||||
double *A;
|
||||
double *B;
|
||||
double *C;
|
||||
double *E;
|
||||
|
||||
double *L_x;
|
||||
double *L_xdot;
|
||||
double *L_z;
|
||||
double *L_u;
|
||||
|
||||
double *A_LO;
|
||||
double *B_LO;
|
||||
double *E_LO;
|
||||
|
||||
/* constant vector */
|
||||
double *c;
|
||||
double *c_LO;
|
||||
|
||||
// permutation vector - to have GNSF order of x, z within sim_gnsf only
|
||||
int *ipiv_x;
|
||||
int *ipiv_z;
|
||||
|
||||
double *ipiv_x_double;
|
||||
double *ipiv_z_double;
|
||||
|
||||
} gnsf_model;
|
||||
|
||||
|
||||
|
||||
// pre_workspace - workspace used in the precomputation phase
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dmat E11;
|
||||
struct blasfeo_dmat E12;
|
||||
struct blasfeo_dmat E21;
|
||||
struct blasfeo_dmat E22;
|
||||
|
||||
struct blasfeo_dmat A1;
|
||||
struct blasfeo_dmat A2;
|
||||
struct blasfeo_dmat B1;
|
||||
struct blasfeo_dmat B2;
|
||||
struct blasfeo_dmat C1;
|
||||
struct blasfeo_dmat C2;
|
||||
|
||||
struct blasfeo_dmat AA1;
|
||||
struct blasfeo_dmat AA2;
|
||||
struct blasfeo_dmat BB1;
|
||||
struct blasfeo_dmat BB2;
|
||||
struct blasfeo_dmat CC1;
|
||||
struct blasfeo_dmat CC2;
|
||||
struct blasfeo_dmat DD1;
|
||||
struct blasfeo_dmat DD2;
|
||||
struct blasfeo_dmat EE1;
|
||||
struct blasfeo_dmat EE2;
|
||||
|
||||
struct blasfeo_dmat QQ1;
|
||||
|
||||
struct blasfeo_dmat LLZ;
|
||||
struct blasfeo_dmat LLx;
|
||||
struct blasfeo_dmat LLK;
|
||||
|
||||
int *ipivEE1; // index of pivot vector
|
||||
int *ipivEE2;
|
||||
int *ipivQQ1;
|
||||
|
||||
// for algebraic sensitivity propagation
|
||||
struct blasfeo_dmat Q1;
|
||||
|
||||
// for constant term in NSF
|
||||
struct blasfeo_dvec cc1;
|
||||
struct blasfeo_dvec cc2;
|
||||
|
||||
} gnsf_pre_workspace;
|
||||
|
||||
|
||||
|
||||
// workspace
|
||||
typedef struct
|
||||
{
|
||||
double *Z_work; // used to perform computations to get out->zn
|
||||
|
||||
int *ipiv; // index of pivot vector
|
||||
|
||||
struct blasfeo_dvec *vv_traj;
|
||||
struct blasfeo_dvec *yy_traj;
|
||||
struct blasfeo_dmat *f_LO_jac_traj;
|
||||
|
||||
struct blasfeo_dvec K2_val;
|
||||
struct blasfeo_dvec x0_traj;
|
||||
struct blasfeo_dvec res_val;
|
||||
struct blasfeo_dvec u0;
|
||||
struct blasfeo_dvec lambda;
|
||||
struct blasfeo_dvec lambda_old;
|
||||
|
||||
struct blasfeo_dvec yyu;
|
||||
struct blasfeo_dvec yyss;
|
||||
|
||||
struct blasfeo_dvec K1_val;
|
||||
struct blasfeo_dvec f_LO_val;
|
||||
struct blasfeo_dvec x1_stage_val;
|
||||
struct blasfeo_dvec Z1_val;
|
||||
|
||||
struct blasfeo_dvec K1u;
|
||||
struct blasfeo_dvec Zu;
|
||||
struct blasfeo_dvec ALOtimesx02;
|
||||
struct blasfeo_dvec BLOtimesu0;
|
||||
|
||||
struct blasfeo_dvec uhat;
|
||||
|
||||
struct blasfeo_dmat J_r_vv;
|
||||
struct blasfeo_dmat J_r_x1u;
|
||||
|
||||
struct blasfeo_dmat dK1_dx1;
|
||||
struct blasfeo_dmat dK1_du;
|
||||
struct blasfeo_dmat dZ_dx1;
|
||||
struct blasfeo_dmat dZ_du;
|
||||
struct blasfeo_dmat J_G2_K1;
|
||||
|
||||
struct blasfeo_dmat dK2_dx1;
|
||||
struct blasfeo_dmat dK2_dvv;
|
||||
struct blasfeo_dmat dxf_dwn;
|
||||
struct blasfeo_dmat S_forw_new;
|
||||
struct blasfeo_dmat S_algebraic_aux;
|
||||
|
||||
struct blasfeo_dmat dPsi_dvv;
|
||||
struct blasfeo_dmat dPsi_dx;
|
||||
struct blasfeo_dmat dPsi_du;
|
||||
|
||||
struct blasfeo_dmat dPHI_dyuhat;
|
||||
struct blasfeo_dvec z0;
|
||||
|
||||
// memory only available if (opts->sens_algebraic)
|
||||
// struct blasfeo_dvec y_one_stage;
|
||||
// struct blasfeo_dvec x0dot_1;
|
||||
// struct blasfeo_dmat dz10_dx1u; // (nz1) * (nx1+nu);
|
||||
// struct blasfeo_dmat dr0_dvv0; // (n_out * n_out)
|
||||
// struct blasfeo_dmat f_LO_jac0; // (nx2+nz2) * (2*nx1 + nz1 + nu)
|
||||
// struct blasfeo_dmat sens_z2_rhs; // (nx2 + nz2) * (nx1 + nu)
|
||||
// int *ipiv_vv0;
|
||||
|
||||
} gnsf_workspace;
|
||||
|
||||
|
||||
|
||||
// memory
|
||||
typedef struct
|
||||
{
|
||||
bool first_call;
|
||||
|
||||
// simulation time for one step
|
||||
double dt;
|
||||
|
||||
// (scaled) butcher table
|
||||
double *A_dt;
|
||||
double *b_dt;
|
||||
double *c_butcher;
|
||||
|
||||
// value used to initialize integration variables - corresponding to value of phi
|
||||
double *phi_guess; // n_out
|
||||
|
||||
struct blasfeo_dmat S_forw;
|
||||
struct blasfeo_dmat S_algebraic;
|
||||
|
||||
// precomputed matrices
|
||||
struct blasfeo_dmat KKv;
|
||||
struct blasfeo_dmat KKx;
|
||||
struct blasfeo_dmat KKu;
|
||||
|
||||
struct blasfeo_dmat YYv;
|
||||
struct blasfeo_dmat YYx;
|
||||
struct blasfeo_dmat YYu;
|
||||
|
||||
struct blasfeo_dmat ZZv;
|
||||
struct blasfeo_dmat ZZx;
|
||||
struct blasfeo_dmat ZZu;
|
||||
|
||||
struct blasfeo_dmat ALO;
|
||||
struct blasfeo_dmat BLO;
|
||||
struct blasfeo_dmat M2_LU;
|
||||
int *ipivM2;
|
||||
|
||||
struct blasfeo_dmat dK2_dx2;
|
||||
struct blasfeo_dmat dK2_du;
|
||||
struct blasfeo_dmat dx2f_dx2u;
|
||||
|
||||
struct blasfeo_dmat Lu;
|
||||
|
||||
// precomputed vectors for constant term in NSF
|
||||
struct blasfeo_dvec KK0;
|
||||
struct blasfeo_dvec YY0;
|
||||
struct blasfeo_dvec ZZ0;
|
||||
|
||||
// for algebraic sensitivities only;
|
||||
// struct blasfeo_dmat *Z0x;
|
||||
// struct blasfeo_dmat *Z0u;
|
||||
// struct blasfeo_dmat *Z0v;
|
||||
|
||||
// struct blasfeo_dmat *Y0x;
|
||||
// struct blasfeo_dmat *Y0u;
|
||||
// struct blasfeo_dmat *Y0v;
|
||||
|
||||
// struct blasfeo_dmat *K0x;
|
||||
// struct blasfeo_dmat *K0u;
|
||||
// struct blasfeo_dmat *K0v;
|
||||
|
||||
// struct blasfeo_dmat *ELO_LU;
|
||||
// int *ipiv_ELO;
|
||||
// struct blasfeo_dmat *ELO_inv_ALO;
|
||||
|
||||
// struct blasfeo_dmat *Lx;
|
||||
// struct blasfeo_dmat *Lxdot;
|
||||
// struct blasfeo_dmat *Lz;
|
||||
|
||||
double time_sim;
|
||||
double time_ad;
|
||||
double time_la;
|
||||
|
||||
} sim_gnsf_memory;
|
||||
|
||||
|
||||
|
||||
// gnsf dims
|
||||
acados_size_t sim_gnsf_dims_calculate_size();
|
||||
void *sim_gnsf_dims_assign(void *config_, void *raw_memory);
|
||||
|
||||
// get & set functions
|
||||
void sim_gnsf_dims_set(void *config_, void *dims_, const char *field, const int *value);
|
||||
void sim_gnsf_dims_get(void *config_, void *dims_, const char *field, int* value);
|
||||
|
||||
// opts
|
||||
acados_size_t sim_gnsf_opts_calculate_size(void *config, void *dims);
|
||||
void *sim_gnsf_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
void sim_gnsf_opts_initialize_default(void *config, void *dims, void *opts_);
|
||||
void sim_gnsf_opts_update(void *config_, void *dims, void *opts_);
|
||||
void sim_gnsf_opts_set(void *config_, void *opts_, const char *field, void *value);
|
||||
|
||||
// model
|
||||
acados_size_t sim_gnsf_model_calculate_size(void *config, void *dims_);
|
||||
void *sim_gnsf_model_assign(void *config, void *dims_, void *raw_memory);
|
||||
int sim_gnsf_model_set(void *model_, const char *field, void *value);
|
||||
|
||||
// precomputation
|
||||
int sim_gnsf_precompute(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_,
|
||||
void *work_);
|
||||
|
||||
// workspace & memory
|
||||
acados_size_t sim_gnsf_workspace_calculate_size(void *config, void *dims_, void *args);
|
||||
acados_size_t sim_gnsf_memory_calculate_size(void *config, void *dims_, void *opts_);
|
||||
void *sim_gnsf_memory_assign(void *config, void *dims_, void *opts_, void *raw_memory);
|
||||
|
||||
// interface
|
||||
void sim_gnsf_config_initialize_default(void *config_);
|
||||
|
||||
// integrator
|
||||
int sim_gnsf(void *config, sim_in *in, sim_out *out, void *opts, void *mem_, void *work_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SIM_SIM_GNSF_H_
|
||||
183
third_party/acados/include/acados/sim/sim_irk_integrator.h
vendored
Normal file
183
third_party/acados/include/acados/sim/sim_irk_integrator.h
vendored
Normal file
@@ -0,0 +1,183 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_SIM_SIM_IRK_INTEGRATOR_H_
|
||||
#define ACADOS_SIM_SIM_IRK_INTEGRATOR_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "acados/sim/sim_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
#include "blasfeo/include/blasfeo_common.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nx;
|
||||
int nu;
|
||||
int nz;
|
||||
|
||||
} sim_irk_dims;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
/* external functions */
|
||||
// implicit fun - can either be fully implicit ode or dae
|
||||
// - i.e. dae has z as additional last argument & nz > 0
|
||||
external_function_generic *impl_ode_fun;
|
||||
// implicit ode & jac_x & jax_xdot & jac_z
|
||||
external_function_generic *impl_ode_fun_jac_x_xdot_z;
|
||||
// jax_x & jac_xdot & jac_u & jac_z of implicit ode
|
||||
external_function_generic *impl_ode_jac_x_xdot_u_z;
|
||||
// hessian of implicit ode:
|
||||
external_function_generic *impl_ode_hess;
|
||||
} irk_model;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
struct blasfeo_dvec *rG; // residuals of G (nx*ns)
|
||||
struct blasfeo_dvec *K; // internal K variables ((nx+nz)*ns)
|
||||
struct blasfeo_dvec *xt; // temporary x
|
||||
struct blasfeo_dvec *xn; // x at each integration step
|
||||
struct blasfeo_dvec xtdot; // temporary xdot
|
||||
|
||||
struct blasfeo_dvec *lambda; // adjoint sensitivities (nx + nu)
|
||||
struct blasfeo_dvec *lambdaK; // auxiliary variable ((nx+nz)*ns) for adjoint propagation
|
||||
|
||||
struct blasfeo_dmat df_dx; // temporary Jacobian of ode w.r.t x (nx+nz, nx)
|
||||
struct blasfeo_dmat df_dxdot; // temporary Jacobian of ode w.r.t xdot (nx+nz, nx)
|
||||
struct blasfeo_dmat df_du; // temporary Jacobian of ode w.r.t u (nx+nz, nu)
|
||||
struct blasfeo_dmat df_dz; // temporary Jacobian of ode w.r.t z (nx+nz, nu)
|
||||
|
||||
/* NOTE: the memory allocation corresponding to the following fields is CONDITIONAL */
|
||||
|
||||
// only allocated if (opts->sens_algebraic || opts->output_z)
|
||||
int *ipiv_one_stage; // index of pivot vector (nx + nz)
|
||||
double *Z_work; // used to perform computations to get out->zn (ns)
|
||||
|
||||
// df_dxdotz, dk0_dxu, only allocated if (opts->sens_algebraic && opts->exact_z_output)
|
||||
// used for algebraic sensitivity generation
|
||||
struct blasfeo_dmat df_dxdotz; // temporary Jacobian of ode w.r.t. xdot,z (nx+nz, nx+nz);
|
||||
struct blasfeo_dmat dk0_dxu; // intermediate result, (nx+nz, nx+nu)
|
||||
|
||||
// dK_dxu: if (!opts->sens_hess) - single blasfeo_dmat that is reused
|
||||
// if ( opts->sens_hess) - array of (num_steps) blasfeo_dmat
|
||||
// to store intermediate results
|
||||
struct blasfeo_dmat *dK_dxu; // jacobian of (K,Z) over x and u ((nx+nz)*ns, nx+nu);
|
||||
|
||||
// S_forw: if (!opts->sens_hess) - single blasfeo_dmat that is reused
|
||||
// if ( opts->sens_hess) - array of (num_steps + 1) blasfeo_dmat
|
||||
// to store intermediate results
|
||||
struct blasfeo_dmat *S_forw; // forward sensitivities (nx, nx+nu)
|
||||
|
||||
// dG_dxu: if (!opts->sens_hess) - single blasfeo_dmat that is reused
|
||||
// if ( opts->sens_hess) - array of blasfeo_dmat to store intermediate results
|
||||
struct blasfeo_dmat *dG_dxu; // jacobian of G over x and u ((nx+nz)*ns, nx+nu)
|
||||
|
||||
// dG_dK: if (!opts->sens_hess) - single blasfeo_dmat that is reused
|
||||
// if ( opts->sens_hess) - array of blasfeo_dmat to store intermediate results
|
||||
struct blasfeo_dmat *dG_dK; // jacobian of G over K ((nx+nz)*ns, (nx+nz)*ns)
|
||||
|
||||
// ipiv: index of pivot vector
|
||||
// if (!opts->sens_hess) - array (ns * (nx + nz)) that is reused
|
||||
// if ( opts->sens_hess) - array (ns * (nx + nz)) * num_steps, to store all
|
||||
// pivot vectors for dG_dxu
|
||||
int *ipiv; // index of pivot vector
|
||||
|
||||
// xn_traj, K_traj only available if( opts->sens_adj || opts->sens_hess )
|
||||
struct blasfeo_dvec *xn_traj; // xn trajectory
|
||||
struct blasfeo_dvec *K_traj; // K trajectory
|
||||
|
||||
/* the following variables are only available if (opts->sens_hess) */
|
||||
// For Hessian propagation
|
||||
struct blasfeo_dmat Hess; // temporary Hessian (nx + nu, nx + nu)
|
||||
// output of impl_ode_hess
|
||||
struct blasfeo_dmat f_hess; // size: (nx + nu, nx + nu)
|
||||
struct blasfeo_dmat dxkzu_dw0; // size (2*nx + nu + nz) x (nx + nu)
|
||||
struct blasfeo_dmat tmp_dxkzu_dw0; // size (2*nx + nu + nz) x (nx + nu)
|
||||
|
||||
} sim_irk_workspace;
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double *xdot; // xdot[NX] - initialization for state derivatives k within the integrator
|
||||
double *z; // z[NZ] - initialization for algebraic variables z
|
||||
|
||||
double time_sim;
|
||||
double time_ad;
|
||||
double time_la;
|
||||
} sim_irk_memory;
|
||||
|
||||
|
||||
// get & set functions
|
||||
void sim_irk_dims_set(void *config_, void *dims_, const char *field, const int *value);
|
||||
void sim_irk_dims_get(void *config_, void *dims_, const char *field, int* value);
|
||||
|
||||
// dims
|
||||
acados_size_t sim_irk_dims_calculate_size();
|
||||
void *sim_irk_dims_assign(void *config_, void *raw_memory);
|
||||
|
||||
// model
|
||||
acados_size_t sim_irk_model_calculate_size(void *config, void *dims);
|
||||
void *sim_irk_model_assign(void *config, void *dims, void *raw_memory);
|
||||
int sim_irk_model_set(void *model, const char *field, void *value);
|
||||
|
||||
// opts
|
||||
acados_size_t sim_irk_opts_calculate_size(void *config, void *dims);
|
||||
void *sim_irk_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
void sim_irk_opts_initialize_default(void *config, void *dims, void *opts_);
|
||||
void sim_irk_opts_update(void *config_, void *dims, void *opts_);
|
||||
void sim_irk_opts_set(void *config_, void *opts_, const char *field, void *value);
|
||||
|
||||
// memory
|
||||
acados_size_t sim_irk_memory_calculate_size(void *config, void *dims, void *opts_);
|
||||
void *sim_irk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
|
||||
int sim_irk_memory_set(void *config_, void *dims_, void *mem_, const char *field, void *value);
|
||||
|
||||
// workspace
|
||||
acados_size_t sim_irk_workspace_calculate_size(void *config, void *dims, void *opts_);
|
||||
void sim_irk_config_initialize_default(void *config);
|
||||
|
||||
// main
|
||||
int sim_irk(void *config, sim_in *in, sim_out *out, void *opts_, void *mem_, void *work_);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SIM_SIM_IRK_INTEGRATOR_H_
|
||||
157
third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h
vendored
Normal file
157
third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h
vendored
Normal file
@@ -0,0 +1,157 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_
|
||||
#define ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "acados/sim/sim_common.h"
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nx;
|
||||
int nu;
|
||||
int nz;
|
||||
} sim_lifted_irk_dims;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
/* external functions */
|
||||
// implicit ode
|
||||
external_function_generic *impl_ode_fun;
|
||||
// implicit ode & jax_x & jac_xdot & jac_u implicit ode
|
||||
external_function_generic *impl_ode_fun_jac_x_xdot_u;
|
||||
|
||||
} lifted_irk_model;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
|
||||
struct blasfeo_dmat *J_temp_x; // temporary Jacobian of ode w.r.t x (nx, nx)
|
||||
struct blasfeo_dmat *J_temp_xdot; // temporary Jacobian of ode w.r.t xdot (nx, nx)
|
||||
struct blasfeo_dmat *J_temp_u; // temporary Jacobian of ode w.r.t u (nx, nu)
|
||||
|
||||
struct blasfeo_dvec *rG; // residuals of G (nx*ns)
|
||||
struct blasfeo_dvec *xt; // temporary x
|
||||
struct blasfeo_dvec *xn; // x at each integration step (for evaluations)
|
||||
struct blasfeo_dvec *xn_out; // x at each integration step (output)
|
||||
struct blasfeo_dvec *dxn; // dx at each integration step
|
||||
struct blasfeo_dvec *w; // stacked x and u
|
||||
|
||||
int *ipiv; // index of pivot vector
|
||||
|
||||
} sim_lifted_irk_workspace;
|
||||
|
||||
|
||||
|
||||
typedef struct
|
||||
{
|
||||
// memory for lifted integrators
|
||||
struct blasfeo_dmat *S_forw; // forward sensitivities
|
||||
struct blasfeo_dmat *JGK; // jacobian of G over K (nx*ns, nx*ns)
|
||||
struct blasfeo_dmat *JGf; // jacobian of G over x and u (nx*ns, nx+nu);
|
||||
struct blasfeo_dmat *JKf; // jacobian of K over x and u (nx*ns, nx+nu);
|
||||
|
||||
struct blasfeo_dvec *K; // internal variables (nx*ns)
|
||||
struct blasfeo_dvec *x; // states (nx) -- for expansion step
|
||||
struct blasfeo_dvec *u; // controls (nu) -- for expansion step
|
||||
|
||||
int update_sens;
|
||||
// int init_K;
|
||||
|
||||
double time_sim;
|
||||
double time_ad;
|
||||
double time_la;
|
||||
|
||||
} sim_lifted_irk_memory;
|
||||
|
||||
|
||||
|
||||
/* dims */
|
||||
void sim_lifted_irk_dims_set(void *config_, void *dims_, const char *field, const int *value);
|
||||
void sim_lifted_irk_dims_get(void *config_, void *dims_, const char *field, int* value);
|
||||
|
||||
acados_size_t sim_lifted_irk_dims_calculate_size();
|
||||
//
|
||||
void *sim_lifted_irk_dims_assign(void* config_, void *raw_memory);
|
||||
|
||||
/* model */
|
||||
//
|
||||
acados_size_t sim_lifted_irk_model_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *sim_lifted_irk_model_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
int sim_lifted_irk_model_set(void *model_, const char *field, void *value);
|
||||
|
||||
/* opts */
|
||||
//
|
||||
acados_size_t sim_lifted_irk_opts_calculate_size(void *config, void *dims);
|
||||
//
|
||||
void *sim_lifted_irk_opts_assign(void *config, void *dims, void *raw_memory);
|
||||
//
|
||||
void sim_lifted_irk_opts_initialize_default(void *config, void *dims, void *opts_);
|
||||
//
|
||||
void sim_lifted_irk_opts_update(void *config_, void *dims, void *opts_);
|
||||
//
|
||||
void sim_lifted_irk_opts_set(void *config_, void *opts_, const char *field, void *value);
|
||||
|
||||
/* memory */
|
||||
//
|
||||
acados_size_t sim_lifted_irk_memory_calculate_size(void *config, void *dims, void *opts_);
|
||||
//
|
||||
void *sim_lifted_irk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
|
||||
|
||||
/* workspace */
|
||||
//
|
||||
acados_size_t sim_lifted_irk_workspace_calculate_size(void *config, void *dims, void *opts_);
|
||||
//
|
||||
void sim_lifted_irk_config_initialize_default(void *config);
|
||||
|
||||
/* solver */
|
||||
//
|
||||
int sim_lifted_irk(void *config, sim_in *in, sim_out *out, void *opts_,
|
||||
void *mem_, void *work_);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_
|
||||
242
third_party/acados/include/acados/utils/external_function_generic.h
vendored
Normal file
242
third_party/acados/include/acados/utils/external_function_generic.h
vendored
Normal file
@@ -0,0 +1,242 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_
|
||||
#define ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
/************************************************
|
||||
* generic external function
|
||||
************************************************/
|
||||
|
||||
// type of arguments
|
||||
typedef enum {
|
||||
COLMAJ,
|
||||
BLASFEO_DMAT,
|
||||
BLASFEO_DVEC,
|
||||
COLMAJ_ARGS,
|
||||
BLASFEO_DMAT_ARGS,
|
||||
BLASFEO_DVEC_ARGS,
|
||||
IGNORE_ARGUMENT
|
||||
} ext_fun_arg_t;
|
||||
|
||||
struct colmaj_args
|
||||
{
|
||||
double *A;
|
||||
int lda;
|
||||
};
|
||||
|
||||
struct blasfeo_dmat_args
|
||||
{
|
||||
struct blasfeo_dmat *A;
|
||||
int ai;
|
||||
int aj;
|
||||
};
|
||||
|
||||
struct blasfeo_dvec_args
|
||||
{
|
||||
struct blasfeo_dvec *x;
|
||||
int xi;
|
||||
};
|
||||
|
||||
// prototype of an external function
|
||||
typedef struct
|
||||
{
|
||||
// public members (have to be before private ones)
|
||||
void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **);
|
||||
// private members
|
||||
// .....
|
||||
} external_function_generic;
|
||||
|
||||
|
||||
|
||||
/************************************************
|
||||
* generic external parametric function
|
||||
************************************************/
|
||||
|
||||
// prototype of a parametric external function
|
||||
typedef struct
|
||||
{
|
||||
// public members for core (have to be before private ones)
|
||||
void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **);
|
||||
// public members for interfaces
|
||||
void (*get_nparam)(void *, int *);
|
||||
void (*set_param)(void *, double *);
|
||||
void (*set_param_sparse)(void *, int n_update, int *idx, double *);
|
||||
// private members
|
||||
void *ptr_ext_mem; // pointer to external memory
|
||||
int (*fun)(void **, void **, void *);
|
||||
double *p; // parameters
|
||||
int np; // number of parameters
|
||||
// .....
|
||||
} external_function_param_generic;
|
||||
|
||||
//
|
||||
acados_size_t external_function_param_generic_struct_size();
|
||||
//
|
||||
void external_function_param_generic_set_fun(external_function_param_generic *fun, void *value);
|
||||
//
|
||||
acados_size_t external_function_param_generic_calculate_size(external_function_param_generic *fun, int np);
|
||||
//
|
||||
void external_function_param_generic_assign(external_function_param_generic *fun, void *mem);
|
||||
//
|
||||
void external_function_param_generic_wrapper(void *self, ext_fun_arg_t *type_in, void **in, ext_fun_arg_t *type_out, void **out);
|
||||
//
|
||||
void external_function_param_generic_get_nparam(void *self, int *np);
|
||||
//
|
||||
void external_function_param_generic_set_param(void *self, double *p);
|
||||
|
||||
|
||||
/************************************************
|
||||
* casadi external function
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
// public members (have to be the same as in the prototype, and before the private ones)
|
||||
void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **);
|
||||
// private members
|
||||
void *ptr_ext_mem; // pointer to external memory
|
||||
int (*casadi_fun)(const double **, double **, int *, double *, void *);
|
||||
int (*casadi_work)(int *, int *, int *, int *);
|
||||
const int *(*casadi_sparsity_in)(int);
|
||||
const int *(*casadi_sparsity_out)(int);
|
||||
int (*casadi_n_in)(void);
|
||||
int (*casadi_n_out)(void);
|
||||
double **args;
|
||||
double **res;
|
||||
double *w;
|
||||
int *iw;
|
||||
int *args_size; // size of args[i]
|
||||
int *res_size; // size of res[i]
|
||||
int args_num; // number of args arrays
|
||||
int args_size_tot; // total size of args arrays
|
||||
int res_num; // number of res arrays
|
||||
int res_size_tot; // total size of res arrays
|
||||
int in_num; // number of input arrays
|
||||
int out_num; // number of output arrays
|
||||
int iw_size; // number of ints for worksapce
|
||||
int w_size; // number of doubles for workspace
|
||||
} external_function_casadi;
|
||||
|
||||
//
|
||||
acados_size_t external_function_casadi_struct_size();
|
||||
//
|
||||
void external_function_casadi_set_fun(external_function_casadi *fun, void *value);
|
||||
//
|
||||
void external_function_casadi_set_work(external_function_casadi *fun, void *value);
|
||||
//
|
||||
void external_function_casadi_set_sparsity_in(external_function_casadi *fun, void *value);
|
||||
//
|
||||
void external_function_casadi_set_sparsity_out(external_function_casadi *fun, void *value);
|
||||
//
|
||||
void external_function_casadi_set_n_in(external_function_casadi *fun, void *value);
|
||||
//
|
||||
void external_function_casadi_set_n_out(external_function_casadi *fun, void *value);
|
||||
//
|
||||
acados_size_t external_function_casadi_calculate_size(external_function_casadi *fun);
|
||||
//
|
||||
void external_function_casadi_assign(external_function_casadi *fun, void *mem);
|
||||
//
|
||||
void external_function_casadi_wrapper(void *self, ext_fun_arg_t *type_in, void **in,
|
||||
ext_fun_arg_t *type_out, void **out);
|
||||
|
||||
/************************************************
|
||||
* casadi external parametric function
|
||||
************************************************/
|
||||
|
||||
typedef struct
|
||||
{
|
||||
// public members for core (have to be the same as in the prototype, and before the private ones)
|
||||
void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **);
|
||||
// public members for interfaces
|
||||
void (*get_nparam)(void *, int *);
|
||||
void (*set_param)(void *, double *);
|
||||
void (*set_param_sparse)(void *, int n_update, int *idx, double *);
|
||||
// private members
|
||||
void *ptr_ext_mem; // pointer to external memory
|
||||
int (*casadi_fun)(const double **, double **, int *, double *, void *);
|
||||
int (*casadi_work)(int *, int *, int *, int *);
|
||||
const int *(*casadi_sparsity_in)(int);
|
||||
const int *(*casadi_sparsity_out)(int);
|
||||
int (*casadi_n_in)(void);
|
||||
int (*casadi_n_out)(void);
|
||||
double **args;
|
||||
double **res;
|
||||
double *w;
|
||||
int *iw;
|
||||
int *args_size; // size of args[i]
|
||||
int *res_size; // size of res[i]
|
||||
int args_num; // number of args arrays
|
||||
int args_size_tot; // total size of args arrays
|
||||
int res_num; // number of res arrays
|
||||
int res_size_tot; // total size of res arrays
|
||||
int in_num; // number of input arrays
|
||||
int out_num; // number of output arrays
|
||||
int iw_size; // number of ints for worksapce
|
||||
int w_size; // number of doubles for workspace
|
||||
int np; // number of parameters
|
||||
} external_function_param_casadi;
|
||||
|
||||
//
|
||||
acados_size_t external_function_param_casadi_struct_size();
|
||||
//
|
||||
void external_function_param_casadi_set_fun(external_function_param_casadi *fun, void *value);
|
||||
//
|
||||
void external_function_param_casadi_set_work(external_function_param_casadi *fun, void *value);
|
||||
//
|
||||
void external_function_param_casadi_set_sparsity_in(external_function_param_casadi *fun, void *value);
|
||||
//
|
||||
void external_function_param_casadi_set_sparsity_out(external_function_param_casadi *fun, void *value);
|
||||
//
|
||||
void external_function_param_casadi_set_n_in(external_function_param_casadi *fun, void *value);
|
||||
//
|
||||
void external_function_param_casadi_set_n_out(external_function_param_casadi *fun, void *value);
|
||||
//
|
||||
acados_size_t external_function_param_casadi_calculate_size(external_function_param_casadi *fun, int np);
|
||||
//
|
||||
void external_function_param_casadi_assign(external_function_param_casadi *fun, void *mem);
|
||||
//
|
||||
void external_function_param_casadi_wrapper(void *self, ext_fun_arg_t *type_in, void **in,
|
||||
ext_fun_arg_t *type_out, void **out);
|
||||
//
|
||||
void external_function_param_casadi_get_nparam(void *self, int *np);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_
|
||||
103
third_party/acados/include/acados/utils/math.h
vendored
Normal file
103
third_party/acados/include/acados/utils/math.h
vendored
Normal file
@@ -0,0 +1,103 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
#ifndef ACADOS_UTILS_MATH_H_
|
||||
#define ACADOS_UTILS_MATH_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "acados/utils/types.h"
|
||||
|
||||
#if defined(__MABX2__)
|
||||
double fmax(double a, double b);
|
||||
int isnan(double x);
|
||||
#endif
|
||||
|
||||
#define MIN(a,b) (((a)<(b))?(a):(b))
|
||||
#define MAX(a,b) (((a)>(b))?(a):(b))
|
||||
|
||||
void dgemm_nn_3l(int m, int n, int k, double *A, int lda, double *B, int ldb, double *C, int ldc);
|
||||
// void dgemv_n_3l(int m, int n, double *A, int lda, double *x, double *y);
|
||||
// void dgemv_t_3l(int m, int n, double *A, int lda, double *x, double *y);
|
||||
// void dcopy_3l(int n, double *x, int incx, double *y, int incy);
|
||||
void daxpy_3l(int n, double da, double *dx, double *dy);
|
||||
void dscal_3l(int n, double da, double *dx);
|
||||
double twonormv(int n, double *ptrv);
|
||||
|
||||
/* copies a matrix into another matrix */
|
||||
void dmcopy(int row, int col, double *ptrA, int lda, double *ptrB, int ldb);
|
||||
|
||||
/* solution of a system of linear equations */
|
||||
void dgesv_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb, int *info);
|
||||
|
||||
/* matrix exponential */
|
||||
void expm(int row, double *A);
|
||||
|
||||
int idamax_3l(int n, double *x);
|
||||
|
||||
void dswap_3l(int n, double *x, int incx, double *y, int incy);
|
||||
|
||||
void dger_3l(int m, int n, double alpha, double *x, int incx, double *y, int incy, double *A,
|
||||
int lda);
|
||||
|
||||
void dgetf2_3l(int m, int n, double *A, int lda, int *ipiv, int *info);
|
||||
|
||||
void dlaswp_3l(int n, double *A, int lda, int k1, int k2, int *ipiv);
|
||||
|
||||
void dtrsm_l_l_n_u_3l(int m, int n, double *A, int lda, double *B, int ldb);
|
||||
|
||||
void dgetrs_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb);
|
||||
|
||||
void dgesv_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb, int *info);
|
||||
|
||||
double onenorm(int row, int col, double *ptrA);
|
||||
|
||||
// double twonormv(int n, double *ptrv);
|
||||
|
||||
void padeapprox(int m, int row, double *A);
|
||||
|
||||
void expm(int row, double *A);
|
||||
|
||||
// void d_compute_qp_size_ocp2dense_rev(int N, int *nx, int *nu, int *nb, int **hidxb, int *ng,
|
||||
// int *nvd, int *ned, int *nbd, int *ngd);
|
||||
|
||||
void acados_eigen_decomposition(int dim, double *A, double *V, double *d, double *e);
|
||||
|
||||
double minimum_of_doubles(double *x, int n);
|
||||
|
||||
void neville_algorithm(double xx, int n, double *x, double *Q, double *out);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_UTILS_MATH_H_
|
||||
110
third_party/acados/include/acados/utils/mem.h
vendored
Normal file
110
third_party/acados/include/acados/utils/mem.h
vendored
Normal file
@@ -0,0 +1,110 @@
|
||||
/*
|
||||
* Copyright (c) The acados authors.
|
||||
*
|
||||
* This file is part of acados.
|
||||
*
|
||||
* The 2-Clause BSD License
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright notice,
|
||||
* this list of conditions and the following disclaimer.
|
||||
*
|
||||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||||
* this list of conditions and the following disclaimer in the documentation
|
||||
* and/or other materials provided with the distribution.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.;
|
||||
*/
|
||||
|
||||
|
||||
#ifndef ACADOS_UTILS_MEM_H_
|
||||
#define ACADOS_UTILS_MEM_H_
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "types.h"
|
||||
|
||||
// blasfeo
|
||||
#include "blasfeo/include/blasfeo_d_aux.h"
|
||||
#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
|
||||
|
||||
// TODO(dimitris): probably does not belong here
|
||||
typedef struct
|
||||
{
|
||||
int (*fun)(void *);
|
||||
acados_size_t (*calculate_args_size)(void *);
|
||||
void *(*assign_args)(void *);
|
||||
void (*initialize_default_args)(void *);
|
||||
acados_size_t (*calculate_memory_size)(void *);
|
||||
void *(*assign_memory)(void *);
|
||||
acados_size_t (*calculate_workspace_size)(void *);
|
||||
} module_solver;
|
||||
|
||||
// make int counter of memory multiple of a number (typically 8 or 64)
|
||||
void make_int_multiple_of(acados_size_t num, acados_size_t *size);
|
||||
|
||||
// align char pointer to number (typically 8 for pointers and doubles,
|
||||
// 64 for blasfeo structs) and return offset
|
||||
int align_char_to(int num, char **c_ptr);
|
||||
|
||||
// switch between malloc and calloc (for valgrinding)
|
||||
void *acados_malloc(size_t nitems, acados_size_t size);
|
||||
|
||||
// uses always calloc
|
||||
void *acados_calloc(size_t nitems, acados_size_t size);
|
||||
|
||||
// allocate vector of pointers to vectors of doubles and advance pointer
|
||||
void assign_and_advance_double_ptrs(int n, double ***v, char **ptr);
|
||||
|
||||
// allocate vector of pointers to vectors of ints and advance pointer
|
||||
void assign_and_advance_int_ptrs(int n, int ***v, char **ptr);
|
||||
|
||||
// allocate vector of pointers to strvecs and advance pointer
|
||||
void assign_and_advance_blasfeo_dvec_structs(int n, struct blasfeo_dvec **sv, char **ptr);
|
||||
|
||||
// allocate vector of pointers to strmats and advance pointer
|
||||
void assign_and_advance_blasfeo_dmat_structs(int n, struct blasfeo_dmat **sm, char **ptr);
|
||||
|
||||
// allocate vector of pointers to vector of pointers to strmats and advance pointer
|
||||
void assign_and_advance_blasfeo_dmat_ptrs(int n, struct blasfeo_dmat ***sm, char **ptr);
|
||||
|
||||
// allocate vector of chars and advance pointer
|
||||
void assign_and_advance_char(int n, char **v, char **ptr);
|
||||
|
||||
// allocate vector of ints and advance pointer
|
||||
void assign_and_advance_int(int n, int **v, char **ptr);
|
||||
|
||||
// allocate vector of bools and advance pointer
|
||||
void assign_and_advance_bool(int n, bool **v, char **ptr);
|
||||
|
||||
// allocate vector of doubles and advance pointer
|
||||
void assign_and_advance_double(int n, double **v, char **ptr);
|
||||
|
||||
// allocate strvec and advance pointer
|
||||
void assign_and_advance_blasfeo_dvec_mem(int n, struct blasfeo_dvec *sv, char **ptr);
|
||||
|
||||
// allocate strmat and advance pointer
|
||||
void assign_and_advance_blasfeo_dmat_mem(int m, int n, struct blasfeo_dmat *sA, char **ptr);
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif // ACADOS_UTILS_MEM_H_
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user