diff --git a/SConstruct b/SConstruct index 6e4f301cbb..3d75dccf9b 100644 --- a/SConstruct +++ b/SConstruct @@ -80,7 +80,7 @@ assert arch in ["larch64", "aarch64", "x86_64", "Darwin"] lenv = { "PATH": os.environ['PATH'], "LD_LIBRARY_PATH": [Dir(f"#third_party/acados/{arch}/lib").abspath], - "PYTHONPATH": Dir("#").abspath, + "PYTHONPATH": Dir("#").abspath + ':' + Dir(f"#third_party/acados").abspath, "ACADOS_SOURCE_DIR": Dir("#third_party/acados").abspath, "ACADOS_PYTHON_INTERFACE_PATH": Dir("#third_party/acados/acados_template").abspath, diff --git a/third_party/acados/Darwin/lib/libacados.dylib b/third_party/acados/Darwin/lib/libacados.dylib index f484043a1b..8cd5a41f14 100755 --- a/third_party/acados/Darwin/lib/libacados.dylib +++ b/third_party/acados/Darwin/lib/libacados.dylib @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:2926c48d96e20086823c87899e5d6d834fada0f616916c0edfe0f58c2aa4b555 -size 834016 +oid sha256:620195ff61aaccf9adf00a66b8acf4e9915a0895d0a86f9f51ec952e44fad70a +size 853776 diff --git a/third_party/acados/Darwin/lib/libblasfeo.dylib b/third_party/acados/Darwin/lib/libblasfeo.dylib index 189a78079d..442a6c736d 100755 --- a/third_party/acados/Darwin/lib/libblasfeo.dylib +++ b/third_party/acados/Darwin/lib/libblasfeo.dylib @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:8ee2734e9a056573efbf011f34f85be4d76841ad6c152b52bd9aa2ee24a390b7 +oid sha256:1d9dbeb68b2ac13c884f606a1f6032f399640e0928cecad4c22b700a8d8ae970 size 2010877 diff --git a/third_party/acados/Darwin/lib/libhpipm.dylib b/third_party/acados/Darwin/lib/libhpipm.dylib index 576669b421..019ec8019e 100755 --- a/third_party/acados/Darwin/lib/libhpipm.dylib +++ b/third_party/acados/Darwin/lib/libhpipm.dylib @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:30249b5c6d5c14b064ff3bd69d7b0ebc201cd9484c9686181d99610268a59246 +oid sha256:35783dba64c2d5bbe7e4c98ad01accac56b478b1bf874969ab5c31d7495499a2 size 2290624 diff --git a/third_party/acados/Darwin/lib/libqpOASES_e.3.1.dylib b/third_party/acados/Darwin/lib/libqpOASES_e.3.1.dylib index 849dd58803..caaa8ec775 100755 --- a/third_party/acados/Darwin/lib/libqpOASES_e.3.1.dylib +++ b/third_party/acados/Darwin/lib/libqpOASES_e.3.1.dylib @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:d7153b9b69fb15c05a5e04afacc4b69d7b781da96381114390700d504dacd9c3 +oid sha256:f57eea51f4b91deff47fbf599eedc3eafedd395f23465b5fb24b2148fdaa2859 size 496707 diff --git a/third_party/acados/Darwin/t_renderer b/third_party/acados/Darwin/t_renderer index eddc66ab46..83a74ea5a3 100755 --- a/third_party/acados/Darwin/t_renderer +++ b/third_party/acados/Darwin/t_renderer @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:f2304f73cd27c821a8cc03cf990b74ca246a6a95bef46d28e2b5d5e6a3132fdb -size 7776232 +oid sha256:c6c78791cfdd841da14266c62a6099d0af53cec348051118f1a70518729aa5ba +size 8483768 diff --git a/third_party/acados/acados_template/__init__.py b/third_party/acados/acados_template/__init__.py index f33b75bb7b..bfbe907990 100644 --- a/third_party/acados/acados_template/__init__.py +++ b/third_party/acados/acados_template/__init__.py @@ -1,8 +1,5 @@ # -# 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 +# Copyright (c) The acados authors. # # This file is part of acados. # @@ -31,13 +28,13 @@ # POSSIBILITY OF SUCH DAMAGE.; # -from .acados_model import * -from .generate_c_code_explicit_ode import * -from .generate_c_code_implicit_ode import * -from .generate_c_code_constraint import * -from .generate_c_code_nls_cost import * -from .acados_ocp import * -from .acados_sim import * -from .acados_ocp_solver import * -from .acados_sim_solver import * -from .utils import * +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 diff --git a/third_party/acados/acados_template/acados_layout.json b/third_party/acados/acados_template/acados_layout.json index c9f0b90c73..a1cc5bbdf3 100644 --- a/third_party/acados/acados_template/acados_layout.json +++ b/third_party/acados/acados_template/acados_layout.json @@ -6,6 +6,12 @@ "str" ], "cython_include_dirs": [ + "list" + ], + "json_file": [ + "str" + ], + "shared_lib_ext": [ "str" ], "model": { @@ -15,7 +21,16 @@ "dyn_ext_fun_type" : [ "str" ], - "dyn_source_discrete" : [ + "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" : [ @@ -734,6 +749,9 @@ "sim_method_newton_iter": [ "int" ], + "sim_method_newton_tol": [ + "float" + ], "sim_method_jac_reuse": [ "ndarray", [ @@ -761,6 +779,12 @@ "qp_solver_iter_max": [ "int" ], + "qp_solver_cond_ric_alg": [ + "int" + ], + "qp_solver_ric_alg": [ + "int" + ], "nlp_solver_tol_stat": [ "float" ], @@ -776,6 +800,9 @@ "nlp_solver_max_iter": [ "int" ], + "nlp_solver_ext_qp_res": [ + "int" + ], "print_level": [ "int" ], @@ -794,6 +821,9 @@ "ext_cost_num_hess": [ "int" ], + "ext_fun_compile_flags": [ + "str" + ], "model_external_shared_lib_dir": [ "str" ], diff --git a/third_party/acados/acados_template/acados_model.py b/third_party/acados/acados_template/acados_model.py index e292cc7477..b7c6945442 100644 --- a/third_party/acados/acados_template/acados_model.py +++ b/third_party/acados/acados_template/acados_model.py @@ -1,8 +1,5 @@ # -# 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 +# Copyright (c) The acados authors. # # This file is part of acados. # @@ -37,7 +34,7 @@ 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. """ @@ -73,7 +70,7 @@ class AcadosModel(): """ self.dyn_ext_fun_type = 'casadi' #: type of external functions for dynamics module; 'casadi' or 'generic'; Default: 'casadi' - self.dyn_source_discrete = None #: name of source file for discrete dyanamics; Default: :code:`None` + self.dyn_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` @@ -87,7 +84,9 @@ class AcadosModel(): ## 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 @@ -107,61 +106,49 @@ class AcadosModel(): self.cost_expr_ext_cost_custom_hess_e = None #: CasADi expression for custom hessian (only for external cost), terminal; Default: :code:`None` self.cost_expr_ext_cost_custom_hess_0 = None #: CasADi expression for custom hessian (only for external cost), initial; Default: :code:`None` - -def acados_model_strip_casadi_symbolics(model): - out = model - if 'f_impl_expr' in out.keys(): - del out['f_impl_expr'] - if 'f_expl_expr' in out.keys(): - del out['f_expl_expr'] - if 'disc_dyn_expr' in out.keys(): - del out['disc_dyn_expr'] - if 'x' in out.keys(): - del out['x'] - if 'xdot' in out.keys(): - del out['xdot'] - if 'u' in out.keys(): - del out['u'] - if 'z' in out.keys(): - del out['z'] - if 'p' in out.keys(): - del out['p'] - # constraints - if 'con_phi_expr' in out.keys(): - del out['con_phi_expr'] - if 'con_h_expr' in out.keys(): - del out['con_h_expr'] - if 'con_r_expr' in out.keys(): - del out['con_r_expr'] - if 'con_r_in_phi' in out.keys(): - del out['con_r_in_phi'] - # terminal - if 'con_phi_expr_e' in out.keys(): - del out['con_phi_expr_e'] - if 'con_h_expr_e' in out.keys(): - del out['con_h_expr_e'] - if 'con_r_expr_e' in out.keys(): - del out['con_r_expr_e'] - if 'con_r_in_phi_e' in out.keys(): - del out['con_r_in_phi_e'] - # cost - if 'cost_y_expr' in out.keys(): - del out['cost_y_expr'] - if 'cost_y_expr_e' in out.keys(): - del out['cost_y_expr_e'] - if 'cost_y_expr_0' in out.keys(): - del out['cost_y_expr_0'] - if 'cost_expr_ext_cost' in out.keys(): - del out['cost_expr_ext_cost'] - if 'cost_expr_ext_cost_e' in out.keys(): - del out['cost_expr_ext_cost_e'] - if 'cost_expr_ext_cost_0' in out.keys(): - del out['cost_expr_ext_cost_0'] - if 'cost_expr_ext_cost_custom_hess' in out.keys(): - del out['cost_expr_ext_cost_custom_hess'] - if 'cost_expr_ext_cost_custom_hess_e' in out.keys(): - del out['cost_expr_ext_cost_custom_hess_e'] - if 'cost_expr_ext_cost_custom_hess_0' in out.keys(): - del out['cost_expr_ext_cost_custom_hess_0'] - - return out + ## 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'. + """ diff --git a/third_party/acados/acados_template/acados_ocp.py b/third_party/acados/acados_template/acados_ocp.py index 80970239eb..ec02822ceb 100644 --- a/third_party/acados/acados_template/acados_ocp.py +++ b/third_party/acados/acados_template/acados_ocp.py @@ -1,9 +1,6 @@ # -*- 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 +# Copyright (c) The acados authors. # # This file is part of acados. # @@ -35,7 +32,7 @@ import numpy as np import os from .acados_model import AcadosModel -from .utils import get_acados_path, J_to_idx, J_to_idx_slack +from .utils import get_acados_path, J_to_idx, J_to_idx_slack, get_lib_ext class AcadosOcpDims: """ @@ -273,224 +270,224 @@ class AcadosOcpDims: if isinstance(nx, int) and nx > 0: self.__nx = nx else: - raise Exception('Invalid nx value, expected positive integer. Exiting.') + 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. Exiting.') + 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. Exiting.') + 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. Exiting.') + raise Exception('Invalid np value, expected nonnegative integer.') @ny_0.setter def ny_0(self, ny_0): if isinstance(ny_0, int) and ny_0 > -1: self.__ny_0 = ny_0 else: - raise Exception('Invalid ny_0 value, expected nonnegative integer. Exiting.') + raise Exception('Invalid ny_0 value, expected nonnegative integer.') @ny.setter def ny(self, ny): if isinstance(ny, int) and ny > -1: self.__ny = ny else: - raise Exception('Invalid ny value, expected nonnegative integer. Exiting.') + raise Exception('Invalid ny value, expected nonnegative integer.') @ny_e.setter def ny_e(self, ny_e): if isinstance(ny_e, int) and ny_e > -1: self.__ny_e = ny_e else: - raise Exception('Invalid ny_e value, expected nonnegative integer. Exiting.') + raise Exception('Invalid ny_e value, expected nonnegative integer.') @nr.setter def nr(self, nr): if isinstance(nr, int) and nr > -1: self.__nr = nr else: - raise Exception('Invalid nr value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nr value, expected nonnegative integer.') @nr_e.setter def nr_e(self, nr_e): if isinstance(nr_e, int) and nr_e > -1: self.__nr_e = nr_e else: - raise Exception('Invalid nr_e value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nr_e value, expected nonnegative integer.') @nh.setter def nh(self, nh): if isinstance(nh, int) and nh > -1: self.__nh = nh else: - raise Exception('Invalid nh value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nh value, expected nonnegative integer.') @nh_e.setter def nh_e(self, nh_e): if isinstance(nh_e, int) and nh_e > -1: self.__nh_e = nh_e else: - raise Exception('Invalid nh_e value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nh_e value, expected nonnegative integer.') @nphi.setter def nphi(self, nphi): if isinstance(nphi, int) and nphi > -1: self.__nphi = nphi else: - raise Exception('Invalid nphi value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nphi value, expected nonnegative integer.') @nphi_e.setter def nphi_e(self, nphi_e): if isinstance(nphi_e, int) and nphi_e > -1: self.__nphi_e = nphi_e else: - raise Exception('Invalid nphi_e value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nphi_e value, expected nonnegative integer.') @nbx.setter def nbx(self, nbx): if isinstance(nbx, int) and nbx > -1: self.__nbx = nbx else: - raise Exception('Invalid nbx value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nbx value, expected nonnegative integer.') @nbxe_0.setter def nbxe_0(self, nbxe_0): if isinstance(nbxe_0, int) and nbxe_0 > -1: self.__nbxe_0 = nbxe_0 else: - raise Exception('Invalid nbxe_0 value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nbxe_0 value, expected nonnegative integer.') @nbx_0.setter def nbx_0(self, nbx_0): if isinstance(nbx_0, int) and nbx_0 > -1: self.__nbx_0 = nbx_0 else: - raise Exception('Invalid nbx_0 value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nbx_0 value, expected nonnegative integer.') @nbx_e.setter def nbx_e(self, nbx_e): if isinstance(nbx_e, int) and nbx_e > -1: self.__nbx_e = nbx_e else: - raise Exception('Invalid nbx_e value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nbx_e value, expected nonnegative integer.') @nbu.setter def nbu(self, nbu): if isinstance(nbu, int) and nbu > -1: self.__nbu = nbu else: - raise Exception('Invalid nbu value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nbu value, expected nonnegative integer.') @nsbx.setter def nsbx(self, nsbx): if isinstance(nsbx, int) and nsbx > -1: self.__nsbx = nsbx else: - raise Exception('Invalid nsbx value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nsbx value, expected nonnegative integer.') @nsbx_e.setter def nsbx_e(self, nsbx_e): if isinstance(nsbx_e, int) and nsbx_e > -1: self.__nsbx_e = nsbx_e else: - raise Exception('Invalid nsbx_e value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nsbx_e value, expected nonnegative integer.') @nsbu.setter def nsbu(self, nsbu): if isinstance(nsbu, int) and nsbu > -1: self.__nsbu = nsbu else: - raise Exception('Invalid nsbu value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nsbu value, expected nonnegative integer.') @nsg.setter def nsg(self, nsg): if isinstance(nsg, int) and nsg > -1: self.__nsg = nsg else: - raise Exception('Invalid nsg value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nsg value, expected nonnegative integer.') @nsg_e.setter def nsg_e(self, nsg_e): if isinstance(nsg_e, int) and nsg_e > -1: self.__nsg_e = nsg_e else: - raise Exception('Invalid nsg_e value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nsg_e value, expected nonnegative integer.') @nsh.setter def nsh(self, nsh): if isinstance(nsh, int) and nsh > -1: self.__nsh = nsh else: - raise Exception('Invalid nsh value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nsh value, expected nonnegative integer.') @nsh_e.setter def nsh_e(self, nsh_e): if isinstance(nsh_e, int) and nsh_e > -1: self.__nsh_e = nsh_e else: - raise Exception('Invalid nsh_e value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nsh_e value, expected nonnegative integer.') @nsphi.setter def nsphi(self, nsphi): if isinstance(nsphi, int) and nsphi > -1: self.__nsphi = nsphi else: - raise Exception('Invalid nsphi value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nsphi value, expected nonnegative integer.') @nsphi_e.setter def nsphi_e(self, nsphi_e): if isinstance(nsphi_e, int) and nsphi_e > -1: self.__nsphi_e = nsphi_e else: - raise Exception('Invalid nsphi_e value, expected nonnegative integer. Exiting.') + raise Exception('Invalid nsphi_e value, expected nonnegative integer.') @ns.setter def ns(self, ns): if isinstance(ns, int) and ns > -1: self.__ns = ns else: - raise Exception('Invalid ns value, expected nonnegative integer. Exiting.') + raise Exception('Invalid ns value, expected nonnegative integer.') @ns_e.setter def ns_e(self, ns_e): if isinstance(ns_e, int) and ns_e > -1: self.__ns_e = ns_e else: - raise Exception('Invalid ns_e value, expected nonnegative integer. Exiting.') + raise Exception('Invalid ns_e value, expected nonnegative integer.') @ng.setter def ng(self, ng): if isinstance(ng, int) and ng > -1: self.__ng = ng else: - raise Exception('Invalid ng value, expected nonnegative integer. Exiting.') + raise Exception('Invalid ng value, expected nonnegative integer.') @ng_e.setter def ng_e(self, ng_e): if isinstance(ng_e, int) and ng_e > -1: self.__ng_e = ng_e else: - raise Exception('Invalid ng_e value, expected nonnegative integer. Exiting.') + raise Exception('Invalid ng_e value, expected nonnegative integer.') @N.setter def N(self, N): if isinstance(N, int) and N > 0: self.__N = N else: - raise Exception('Invalid N value, expected positive integer. Exiting.') + raise Exception('Invalid N value, expected positive integer.') def set(self, attr, value): setattr(self, attr, value) @@ -500,6 +497,12 @@ class AcadosOcpCost: """ Class containing the numerical data of the cost: + NOTE: all cost terms, except for the terminal one are weighted with the corresponding time step. + This means given the time steps are :math:`\Delta t_0,..., \Delta t_N`, the total cost is given by: + :math:`c_\\text{total} = \Delta t_0 \cdot c_0(x_0, u_0, p_0, z_0) + ... + \Delta t_{N-1} \cdot c_{N-1}(x_0, u_0, p_0, z_0) + c_N(x_N, p_N)`. + + This means the Lagrange cost term is given in continuous time, this makes up for a seeminglessly OCP discretization with a nonuniform time grid. + In case of LINEAR_LS: stage cost is :math:`l(x,u,z) = || V_x \, x + V_u \, u + V_z \, z - y_\\text{ref}||^2_W`, @@ -508,9 +511,15 @@ class AcadosOcpCost: In case of NONLINEAR_LS: stage cost is - :math:`l(x,u,z) = || y(x,u,z) - y_\\text{ref}||^2_W`, + :math:`l(x,u,z,p) = || y(x,u,z,p) - y_\\text{ref}||^2_W`, + terminal cost is + :math:`m(x,p) = || y^e(x,p) - y_\\text{ref}^e||^2_{W^e}` + + In case of CONVEX_OVER_NONLINEAR: + stage cost is + :math:`l(x,u,p) = \psi(y(x,u,p) - y_\\text{ref}, p)`, terminal cost is - :math:`m(x) = || y^e(x) - y_\\text{ref}^e||^2_{W^e}` + :math:`m(x, p) = \psi^e (y^e(x,p) - y_\\text{ref}^e, p)` """ def __init__(self): # initial stage @@ -548,7 +557,7 @@ class AcadosOcpCost: @property def cost_type_0(self): """Cost type at initial shooting node (0) - -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS} or :code:`None`. + -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS, CONVEX_OVER_NONLINEAR} or :code:`None`. Default: :code:`None`. .. note:: Cost at initial stage is the same as for intermediate shooting nodes if not set differently explicitly. @@ -604,10 +613,10 @@ class AcadosOcpCost: @yref_0.setter def yref_0(self, yref_0): - if isinstance(yref_0, np.ndarray): + if isinstance(yref_0, np.ndarray) and len(yref_0.shape) == 1: self.__yref_0 = yref_0 else: - raise Exception('Invalid yref_0 value, expected numpy array. Exiting.') + raise Exception('Invalid yref_0 value, expected 1-dimensional numpy array.') @W_0.setter def W_0(self, W_0): @@ -615,7 +624,7 @@ class AcadosOcpCost: self.__W_0 = W_0 else: raise Exception('Invalid cost W_0 value. ' \ - + 'Should be 2 dimensional numpy array. Exiting.') + + 'Should be 2 dimensional numpy array.') @Vx_0.setter def Vx_0(self, Vx_0): @@ -623,7 +632,7 @@ class AcadosOcpCost: self.__Vx_0 = Vx_0 else: raise Exception('Invalid cost Vx_0 value. ' \ - + 'Should be 2 dimensional numpy array. Exiting.') + + 'Should be 2 dimensional numpy array.') @Vu_0.setter def Vu_0(self, Vu_0): @@ -631,7 +640,7 @@ class AcadosOcpCost: self.__Vu_0 = Vu_0 else: raise Exception('Invalid cost Vu_0 value. ' \ - + 'Should be 2 dimensional numpy array. Exiting.') + + 'Should be 2 dimensional numpy array.') @Vz_0.setter def Vz_0(self, Vz_0): @@ -639,21 +648,21 @@ class AcadosOcpCost: self.__Vz_0 = Vz_0 else: raise Exception('Invalid cost Vz_0 value. ' \ - + 'Should be 2 dimensional numpy array. Exiting.') + + 'Should be 2 dimensional numpy array.') @cost_ext_fun_type_0.setter def cost_ext_fun_type_0(self, cost_ext_fun_type_0): if cost_ext_fun_type_0 in ['casadi', 'generic']: self.__cost_ext_fun_type_0 = cost_ext_fun_type_0 else: - raise Exception('Invalid cost_ext_fun_type_0 value, expected numpy array. Exiting.') + raise Exception('Invalid cost_ext_fun_type_0 value, expected numpy array.') # Lagrange term @property def cost_type(self): """ Cost type at intermediate shooting nodes (1 to N-1) - -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS}. + -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS, CONVEX_OVER_NONLINEAR}. Default: 'LINEAR_LS'. """ return self.__cost_type @@ -695,28 +704,28 @@ class AcadosOcpCost: @property def Zl(self): - """:math:`Z_l` - diagonal of Hessian wrt lower slack at intermediate shooting nodes (1 to N-1). + """:math:`Z_l` - diagonal of Hessian wrt lower slack at intermediate shooting nodes (0 to N-1). Default: :code:`np.array([])`. """ return self.__Zl @property def Zu(self): - """:math:`Z_u` - diagonal of Hessian wrt upper slack at intermediate shooting nodes (1 to N-1). + """:math:`Z_u` - diagonal of Hessian wrt upper slack at intermediate shooting nodes (0 to N-1). Default: :code:`np.array([])`. """ return self.__Zu @property def zl(self): - """:math:`z_l` - gradient wrt lower slack at intermediate shooting nodes (1 to N-1). + """:math:`z_l` - gradient wrt lower slack at intermediate shooting nodes (0 to N-1). Default: :code:`np.array([])`. """ return self.__zl @property def zu(self): - """:math:`z_u` - gradient wrt upper slack at intermediate shooting nodes (1 to N-1). + """:math:`z_u` - gradient wrt upper slack at intermediate shooting nodes (0 to N-1). Default: :code:`np.array([])`. """ return self.__zu @@ -731,19 +740,19 @@ class AcadosOcpCost: @cost_type.setter def cost_type(self, cost_type): - cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL') + cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL', 'CONVEX_OVER_NONLINEAR') if cost_type in cost_types: self.__cost_type = cost_type else: - raise Exception('Invalid cost_type value. Exiting.') + raise Exception('Invalid cost_type value.') @cost_type_0.setter def cost_type_0(self, cost_type_0): - cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL') + cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL', 'CONVEX_OVER_NONLINEAR') if cost_type_0 in cost_types: self.__cost_type_0 = cost_type_0 else: - raise Exception('Invalid cost_type_0 value. Exiting.') + raise Exception('Invalid cost_type_0 value.') @W.setter def W(self, W): @@ -751,7 +760,7 @@ class AcadosOcpCost: self.__W = W else: raise Exception('Invalid cost W value. ' \ - + 'Should be 2 dimensional numpy array. Exiting.') + + 'Should be 2 dimensional numpy array.') @Vx.setter @@ -760,7 +769,7 @@ class AcadosOcpCost: self.__Vx = Vx else: raise Exception('Invalid cost Vx value. ' \ - + 'Should be 2 dimensional numpy array. Exiting.') + + 'Should be 2 dimensional numpy array.') @Vu.setter def Vu(self, Vu): @@ -768,7 +777,7 @@ class AcadosOcpCost: self.__Vu = Vu else: raise Exception('Invalid cost Vu value. ' \ - + 'Should be 2 dimensional numpy array. Exiting.') + + 'Should be 2 dimensional numpy array.') @Vz.setter def Vz(self, Vz): @@ -776,56 +785,56 @@ class AcadosOcpCost: self.__Vz = Vz else: raise Exception('Invalid cost Vz value. ' \ - + 'Should be 2 dimensional numpy array. Exiting.') + + 'Should be 2 dimensional numpy array.') @yref.setter def yref(self, yref): - if isinstance(yref, np.ndarray): + if isinstance(yref, np.ndarray) and len(yref.shape) == 1: self.__yref = yref else: - raise Exception('Invalid yref value, expected numpy array. Exiting.') + raise Exception('Invalid yref value, expected 1-dimensional numpy array.') @Zl.setter def Zl(self, Zl): if isinstance(Zl, np.ndarray): self.__Zl = Zl else: - raise Exception('Invalid Zl value, expected numpy array. Exiting.') + raise Exception('Invalid Zl value, expected numpy array.') @Zu.setter def Zu(self, Zu): if isinstance(Zu, np.ndarray): self.__Zu = Zu else: - raise Exception('Invalid Zu value, expected numpy array. Exiting.') + raise Exception('Invalid Zu value, expected numpy array.') @zl.setter def zl(self, zl): if isinstance(zl, np.ndarray): self.__zl = zl else: - raise Exception('Invalid zl value, expected numpy array. Exiting.') + raise Exception('Invalid zl value, expected numpy array.') @zu.setter def zu(self, zu): if isinstance(zu, np.ndarray): self.__zu = zu else: - raise Exception('Invalid zu value, expected numpy array. Exiting.') + raise Exception('Invalid zu value, expected numpy array.') @cost_ext_fun_type.setter def cost_ext_fun_type(self, cost_ext_fun_type): if cost_ext_fun_type in ['casadi', 'generic']: self.__cost_ext_fun_type = cost_ext_fun_type else: - raise Exception('Invalid cost_ext_fun_type value, expected numpy array. Exiting.') + raise Exception("Invalid cost_ext_fun_type value, expected one in ['casadi', 'generic'].") # Mayer term @property def cost_type_e(self): """ Cost type at terminal shooting node (N) - -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS}. + -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS, CONVEX_OVER_NONLINEAR}. Default: 'LINEAR_LS'. """ return self.__cost_type_e @@ -881,7 +890,7 @@ class AcadosOcpCost: @property def cost_ext_fun_type_e(self): - """Type of external function for cost at intermediate shooting nodes (1 to N-1). + """Type of external function for cost at terminal shooting node (N). -- string in {casadi, generic} Default: :code:'casadi'. """ @@ -889,12 +898,12 @@ class AcadosOcpCost: @cost_type_e.setter def cost_type_e(self, cost_type_e): - cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL') + cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL', 'CONVEX_OVER_NONLINEAR') if cost_type_e in cost_types: self.__cost_type_e = cost_type_e else: - raise Exception('Invalid cost_type_e value. Exiting.') + raise Exception('Invalid cost_type_e value.') @W_e.setter def W_e(self, W_e): @@ -902,7 +911,7 @@ class AcadosOcpCost: self.__W_e = W_e else: raise Exception('Invalid cost W_e value. ' \ - + 'Should be 2 dimensional numpy array. Exiting.') + + 'Should be 2 dimensional numpy array.') @Vx_e.setter def Vx_e(self, Vx_e): @@ -910,49 +919,49 @@ class AcadosOcpCost: self.__Vx_e = Vx_e else: raise Exception('Invalid cost Vx_e value. ' \ - + 'Should be 2 dimensional numpy array. Exiting.') + + 'Should be 2 dimensional numpy array.') @yref_e.setter def yref_e(self, yref_e): - if isinstance(yref_e, np.ndarray): + if isinstance(yref_e, np.ndarray) and len(yref_e.shape) == 1: self.__yref_e = yref_e else: - raise Exception('Invalid yref_e value, expected numpy array. Exiting.') + raise Exception('Invalid yref_e value, expected 1-dimensional numpy array.') @Zl_e.setter def Zl_e(self, Zl_e): if isinstance(Zl_e, np.ndarray): self.__Zl_e = Zl_e else: - raise Exception('Invalid Zl_e value, expected numpy array. Exiting.') + raise Exception('Invalid Zl_e value, expected numpy array.') @Zu_e.setter def Zu_e(self, Zu_e): if isinstance(Zu_e, np.ndarray): self.__Zu_e = Zu_e else: - raise Exception('Invalid Zu_e value, expected numpy array. Exiting.') + raise Exception('Invalid Zu_e value, expected numpy array.') @zl_e.setter def zl_e(self, zl_e): if isinstance(zl_e, np.ndarray): self.__zl_e = zl_e else: - raise Exception('Invalid zl_e value, expected numpy array. Exiting.') + raise Exception('Invalid zl_e value, expected numpy array.') @zu_e.setter def zu_e(self, zu_e): if isinstance(zu_e, np.ndarray): self.__zu_e = zu_e else: - raise Exception('Invalid zu_e value, expected numpy array. Exiting.') + raise Exception('Invalid zu_e value, expected numpy array.') @cost_ext_fun_type_e.setter def cost_ext_fun_type_e(self, cost_ext_fun_type_e): if cost_ext_fun_type_e in ['casadi', 'generic']: self.__cost_ext_fun_type_e = cost_ext_fun_type_e else: - raise Exception('Invalid cost_ext_fun_type_e value, expected numpy array. Exiting.') + raise Exception("Invalid cost_ext_fun_type_e value, expected one in ['casadi', 'generic'].") def set(self, attr, value): setattr(self, attr, value) @@ -1578,7 +1587,7 @@ class AcadosOcpConstraints: self.__constr_type = constr_type else: raise Exception('Invalid constr_type value. Possible values are:\n\n' \ - + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type + '.\n\nExiting.') + + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type + '.\n\n') @constr_type_e.setter def constr_type_e(self, constr_type_e): @@ -1587,7 +1596,7 @@ class AcadosOcpConstraints: self.__constr_type_e = constr_type_e else: raise Exception('Invalid constr_type_e value. Possible values are:\n\n' \ - + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type_e + '.\n\nExiting.') + + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type_e + '.\n\n') # initial x @lbx_0.setter @@ -1595,35 +1604,35 @@ class AcadosOcpConstraints: if isinstance(lbx_0, np.ndarray): self.__lbx_0 = lbx_0 else: - raise Exception('Invalid lbx_0 value. Exiting.') + raise Exception('Invalid lbx_0 value.') @ubx_0.setter def ubx_0(self, ubx_0): if isinstance(ubx_0, np.ndarray): self.__ubx_0 = ubx_0 else: - raise Exception('Invalid ubx_0 value. Exiting.') + raise Exception('Invalid ubx_0 value.') @idxbx_0.setter def idxbx_0(self, idxbx_0): if isinstance(idxbx_0, np.ndarray): self.__idxbx_0 = idxbx_0 else: - raise Exception('Invalid idxbx_0 value. Exiting.') + raise Exception('Invalid idxbx_0 value.') @Jbx_0.setter def Jbx_0(self, Jbx_0): if isinstance(Jbx_0, np.ndarray): self.__idxbx_0 = J_to_idx(Jbx_0) else: - raise Exception('Invalid Jbx_0 value. Exiting.') + raise Exception('Invalid Jbx_0 value.') @idxbxe_0.setter def idxbxe_0(self, idxbxe_0): if isinstance(idxbxe_0, np.ndarray): self.__idxbxe_0 = idxbxe_0 else: - raise Exception('Invalid idxbxe_0 value. Exiting.') + raise Exception('Invalid idxbxe_0 value.') @x0.setter @@ -1634,7 +1643,7 @@ class AcadosOcpConstraints: self.__idxbx_0 = np.arange(x0.size) self.__idxbxe_0 = np.arange(x0.size) else: - raise Exception('Invalid x0 value. Exiting.') + raise Exception('Invalid x0 value.') # bounds on x @lbx.setter @@ -1642,28 +1651,28 @@ class AcadosOcpConstraints: if isinstance(lbx, np.ndarray): self.__lbx = lbx else: - raise Exception('Invalid lbx value. Exiting.') + raise Exception('Invalid lbx value.') @ubx.setter def ubx(self, ubx): if isinstance(ubx, np.ndarray): self.__ubx = ubx else: - raise Exception('Invalid ubx value. Exiting.') + raise Exception('Invalid ubx value.') @idxbx.setter def idxbx(self, idxbx): if isinstance(idxbx, np.ndarray): self.__idxbx = idxbx else: - raise Exception('Invalid idxbx value. Exiting.') + raise Exception('Invalid idxbx value.') @Jbx.setter def Jbx(self, Jbx): if isinstance(Jbx, np.ndarray): self.__idxbx = J_to_idx(Jbx) else: - raise Exception('Invalid Jbx value. Exiting.') + raise Exception('Invalid Jbx value.') # bounds on u @lbu.setter @@ -1671,28 +1680,28 @@ class AcadosOcpConstraints: if isinstance(lbu, np.ndarray): self.__lbu = lbu else: - raise Exception('Invalid lbu value. Exiting.') + raise Exception('Invalid lbu value.') @ubu.setter def ubu(self, ubu): if isinstance(ubu, np.ndarray): self.__ubu = ubu else: - raise Exception('Invalid ubu value. Exiting.') + raise Exception('Invalid ubu value.') @idxbu.setter def idxbu(self, idxbu): if isinstance(idxbu, np.ndarray): self.__idxbu = idxbu else: - raise Exception('Invalid idxbu value. Exiting.') + raise Exception('Invalid idxbu value.') @Jbu.setter def Jbu(self, Jbu): if isinstance(Jbu, np.ndarray): self.__idxbu = J_to_idx(Jbu) else: - raise Exception('Invalid Jbu value. Exiting.') + raise Exception('Invalid Jbu value.') # bounds on x at shooting node N @lbx_e.setter @@ -1700,28 +1709,28 @@ class AcadosOcpConstraints: if isinstance(lbx_e, np.ndarray): self.__lbx_e = lbx_e else: - raise Exception('Invalid lbx_e value. Exiting.') + raise Exception('Invalid lbx_e value.') @ubx_e.setter def ubx_e(self, ubx_e): if isinstance(ubx_e, np.ndarray): self.__ubx_e = ubx_e else: - raise Exception('Invalid ubx_e value. Exiting.') + raise Exception('Invalid ubx_e value.') @idxbx_e.setter def idxbx_e(self, idxbx_e): if isinstance(idxbx_e, np.ndarray): self.__idxbx_e = idxbx_e else: - raise Exception('Invalid idxbx_e value. Exiting.') + raise Exception('Invalid idxbx_e value.') @Jbx_e.setter def Jbx_e(self, Jbx_e): if isinstance(Jbx_e, np.ndarray): self.__idxbx_e = J_to_idx(Jbx_e) else: - raise Exception('Invalid Jbx_e value. Exiting.') + raise Exception('Invalid Jbx_e value.') # polytopic constraints @D.setter @@ -1730,7 +1739,7 @@ class AcadosOcpConstraints: self.__D = D else: raise Exception('Invalid constraint D value.' \ - + 'Should be 2 dimensional numpy array. Exiting.') + + 'Should be 2 dimensional numpy array.') @C.setter def C(self, C): @@ -1738,21 +1747,21 @@ class AcadosOcpConstraints: self.__C = C else: raise Exception('Invalid constraint C value.' \ - + 'Should be 2 dimensional numpy array. Exiting.') + + 'Should be 2 dimensional numpy array.') @lg.setter def lg(self, lg): if isinstance(lg, np.ndarray): self.__lg = lg else: - raise Exception('Invalid lg value. Exiting.') + raise Exception('Invalid lg value.') @ug.setter def ug(self, ug): if isinstance(ug, np.ndarray): self.__ug = ug else: - raise Exception('Invalid ug value. Exiting.') + raise Exception('Invalid ug value.') # polytopic constraints at shooting node N @C_e.setter @@ -1761,21 +1770,21 @@ class AcadosOcpConstraints: self.__C_e = C_e else: raise Exception('Invalid constraint C_e value.' \ - + 'Should be 2 dimensional numpy array. Exiting.') + + 'Should be 2 dimensional numpy array.') @lg_e.setter def lg_e(self, lg_e): if isinstance(lg_e, np.ndarray): self.__lg_e = lg_e else: - raise Exception('Invalid lg_e value. Exiting.') + raise Exception('Invalid lg_e value.') @ug_e.setter def ug_e(self, ug_e): if isinstance(ug_e, np.ndarray): self.__ug_e = ug_e else: - raise Exception('Invalid ug_e value. Exiting.') + raise Exception('Invalid ug_e value.') # nonlinear constraints @lh.setter @@ -1783,14 +1792,14 @@ class AcadosOcpConstraints: if isinstance(lh, np.ndarray): self.__lh = lh else: - raise Exception('Invalid lh value. Exiting.') + raise Exception('Invalid lh value.') @uh.setter def uh(self, uh): if isinstance(uh, np.ndarray): self.__uh = uh else: - raise Exception('Invalid uh value. Exiting.') + raise Exception('Invalid uh value.') # convex-over-nonlinear constraints @lphi.setter @@ -1798,14 +1807,14 @@ class AcadosOcpConstraints: if isinstance(lphi, np.ndarray): self.__lphi = lphi else: - raise Exception('Invalid lphi value. Exiting.') + raise Exception('Invalid lphi value.') @uphi.setter def uphi(self, uphi): if isinstance(uphi, np.ndarray): self.__uphi = uphi else: - raise Exception('Invalid uphi value. Exiting.') + raise Exception('Invalid uphi value.') # nonlinear constraints at shooting node N @lh_e.setter @@ -1813,14 +1822,14 @@ class AcadosOcpConstraints: if isinstance(lh_e, np.ndarray): self.__lh_e = lh_e else: - raise Exception('Invalid lh_e value. Exiting.') + raise Exception('Invalid lh_e value.') @uh_e.setter def uh_e(self, uh_e): if isinstance(uh_e, np.ndarray): self.__uh_e = uh_e else: - raise Exception('Invalid uh_e value. Exiting.') + raise Exception('Invalid uh_e value.') # convex-over-nonlinear constraints at shooting node N @lphi_e.setter @@ -1828,14 +1837,14 @@ class AcadosOcpConstraints: if isinstance(lphi_e, np.ndarray): self.__lphi_e = lphi_e else: - raise Exception('Invalid lphi_e value. Exiting.') + raise Exception('Invalid lphi_e value.') @uphi_e.setter def uphi_e(self, uphi_e): if isinstance(uphi_e, np.ndarray): self.__uphi_e = uphi_e else: - raise Exception('Invalid uphi_e value. Exiting.') + raise Exception('Invalid uphi_e value.') # SLACK bounds # soft bounds on x @@ -1844,28 +1853,28 @@ class AcadosOcpConstraints: if isinstance(lsbx, np.ndarray): self.__lsbx = lsbx else: - raise Exception('Invalid lsbx value. Exiting.') + raise Exception('Invalid lsbx value.') @usbx.setter def usbx(self, usbx): if isinstance(usbx, np.ndarray): self.__usbx = usbx else: - raise Exception('Invalid usbx value. Exiting.') + raise Exception('Invalid usbx value.') @idxsbx.setter def idxsbx(self, idxsbx): if isinstance(idxsbx, np.ndarray): self.__idxsbx = idxsbx else: - raise Exception('Invalid idxsbx value. Exiting.') + raise Exception('Invalid idxsbx value.') @Jsbx.setter def Jsbx(self, Jsbx): if isinstance(Jsbx, np.ndarray): self.__idxsbx = J_to_idx_slack(Jsbx) else: - raise Exception('Invalid Jsbx value, expected numpy array. Exiting.') + raise Exception('Invalid Jsbx value, expected numpy array.') # soft bounds on u @lsbu.setter @@ -1873,28 +1882,28 @@ class AcadosOcpConstraints: if isinstance(lsbu, np.ndarray): self.__lsbu = lsbu else: - raise Exception('Invalid lsbu value. Exiting.') + raise Exception('Invalid lsbu value.') @usbu.setter def usbu(self, usbu): if isinstance(usbu, np.ndarray): self.__usbu = usbu else: - raise Exception('Invalid usbu value. Exiting.') + raise Exception('Invalid usbu value.') @idxsbu.setter def idxsbu(self, idxsbu): if isinstance(idxsbu, np.ndarray): self.__idxsbu = idxsbu else: - raise Exception('Invalid idxsbu value. Exiting.') + raise Exception('Invalid idxsbu value.') @Jsbu.setter def Jsbu(self, Jsbu): if isinstance(Jsbu, np.ndarray): self.__idxsbu = J_to_idx_slack(Jsbu) else: - raise Exception('Invalid Jsbu value. Exiting.') + raise Exception('Invalid Jsbu value.') # soft bounds on x at shooting node N @lsbx_e.setter @@ -1902,28 +1911,28 @@ class AcadosOcpConstraints: if isinstance(lsbx_e, np.ndarray): self.__lsbx_e = lsbx_e else: - raise Exception('Invalid lsbx_e value. Exiting.') + raise Exception('Invalid lsbx_e value.') @usbx_e.setter def usbx_e(self, usbx_e): if isinstance(usbx_e, np.ndarray): self.__usbx_e = usbx_e else: - raise Exception('Invalid usbx_e value. Exiting.') + raise Exception('Invalid usbx_e value.') @idxsbx_e.setter def idxsbx_e(self, idxsbx_e): if isinstance(idxsbx_e, np.ndarray): self.__idxsbx_e = idxsbx_e else: - raise Exception('Invalid idxsbx_e value. Exiting.') + raise Exception('Invalid idxsbx_e value.') @Jsbx_e.setter def Jsbx_e(self, Jsbx_e): if isinstance(Jsbx_e, np.ndarray): self.__idxsbx_e = J_to_idx_slack(Jsbx_e) else: - raise Exception('Invalid Jsbx_e value. Exiting.') + raise Exception('Invalid Jsbx_e value.') # soft bounds on general linear constraints @@ -1932,28 +1941,28 @@ class AcadosOcpConstraints: if isinstance(lsg, np.ndarray): self.__lsg = lsg else: - raise Exception('Invalid lsg value. Exiting.') + raise Exception('Invalid lsg value.') @usg.setter def usg(self, usg): if isinstance(usg, np.ndarray): self.__usg = usg else: - raise Exception('Invalid usg value. Exiting.') + raise Exception('Invalid usg value.') @idxsg.setter def idxsg(self, idxsg): if isinstance(idxsg, np.ndarray): self.__idxsg = idxsg else: - raise Exception('Invalid idxsg value. Exiting.') + raise Exception('Invalid idxsg value.') @Jsg.setter def Jsg(self, Jsg): if isinstance(Jsg, np.ndarray): self.__idxsg = J_to_idx_slack(Jsg) else: - raise Exception('Invalid Jsg value, expected numpy array. Exiting.') + raise Exception('Invalid Jsg value, expected numpy array.') # soft bounds on nonlinear constraints @@ -1962,21 +1971,21 @@ class AcadosOcpConstraints: if isinstance(lsh, np.ndarray): self.__lsh = lsh else: - raise Exception('Invalid lsh value. Exiting.') + raise Exception('Invalid lsh value.') @ush.setter def ush(self, ush): if isinstance(ush, np.ndarray): self.__ush = ush else: - raise Exception('Invalid ush value. Exiting.') + raise Exception('Invalid ush value.') @idxsh.setter def idxsh(self, idxsh): if isinstance(idxsh, np.ndarray): self.__idxsh = idxsh else: - raise Exception('Invalid idxsh value. Exiting.') + raise Exception('Invalid idxsh value.') @Jsh.setter @@ -1984,7 +1993,7 @@ class AcadosOcpConstraints: if isinstance(Jsh, np.ndarray): self.__idxsh = J_to_idx_slack(Jsh) else: - raise Exception('Invalid Jsh value, expected numpy array. Exiting.') + raise Exception('Invalid Jsh value, expected numpy array.') # soft bounds on convex-over-nonlinear constraints @lsphi.setter @@ -1992,28 +2001,28 @@ class AcadosOcpConstraints: if isinstance(lsphi, np.ndarray): self.__lsphi = lsphi else: - raise Exception('Invalid lsphi value. Exiting.') + raise Exception('Invalid lsphi value.') @usphi.setter def usphi(self, usphi): if isinstance(usphi, np.ndarray): self.__usphi = usphi else: - raise Exception('Invalid usphi value. Exiting.') + raise Exception('Invalid usphi value.') @idxsphi.setter def idxsphi(self, idxsphi): if isinstance(idxsphi, np.ndarray): self.__idxsphi = idxsphi else: - raise Exception('Invalid idxsphi value. Exiting.') + raise Exception('Invalid idxsphi value.') @Jsphi.setter def Jsphi(self, Jsphi): if isinstance(Jsphi, np.ndarray): self.__idxsphi = J_to_idx_slack(Jsphi) else: - raise Exception('Invalid Jsphi value, expected numpy array. Exiting.') + raise Exception('Invalid Jsphi value, expected numpy array.') # soft bounds on general linear constraints at shooting node N @lsg_e.setter @@ -2021,28 +2030,28 @@ class AcadosOcpConstraints: if isinstance(lsg_e, np.ndarray): self.__lsg_e = lsg_e else: - raise Exception('Invalid lsg_e value. Exiting.') + raise Exception('Invalid lsg_e value.') @usg_e.setter def usg_e(self, usg_e): if isinstance(usg_e, np.ndarray): self.__usg_e = usg_e else: - raise Exception('Invalid usg_e value. Exiting.') + raise Exception('Invalid usg_e value.') @idxsg_e.setter def idxsg_e(self, idxsg_e): if isinstance(idxsg_e, np.ndarray): self.__idxsg_e = idxsg_e else: - raise Exception('Invalid idxsg_e value. Exiting.') + raise Exception('Invalid idxsg_e value.') @Jsg_e.setter def Jsg_e(self, Jsg_e): if isinstance(Jsg_e, np.ndarray): self.__idxsg_e = J_to_idx_slack(Jsg_e) else: - raise Exception('Invalid Jsg_e value, expected numpy array. Exiting.') + raise Exception('Invalid Jsg_e value, expected numpy array.') # soft bounds on nonlinear constraints at shooting node N @lsh_e.setter @@ -2050,28 +2059,28 @@ class AcadosOcpConstraints: if isinstance(lsh_e, np.ndarray): self.__lsh_e = lsh_e else: - raise Exception('Invalid lsh_e value. Exiting.') + raise Exception('Invalid lsh_e value.') @ush_e.setter def ush_e(self, ush_e): if isinstance(ush_e, np.ndarray): self.__ush_e = ush_e else: - raise Exception('Invalid ush_e value. Exiting.') + raise Exception('Invalid ush_e value.') @idxsh_e.setter def idxsh_e(self, idxsh_e): if isinstance(idxsh_e, np.ndarray): self.__idxsh_e = idxsh_e else: - raise Exception('Invalid idxsh_e value. Exiting.') + raise Exception('Invalid idxsh_e value.') @Jsh_e.setter def Jsh_e(self, Jsh_e): if isinstance(Jsh_e, np.ndarray): self.__idxsh_e = J_to_idx_slack(Jsh_e) else: - raise Exception('Invalid Jsh_e value, expected numpy array. Exiting.') + raise Exception('Invalid Jsh_e value, expected numpy array.') # soft bounds on convex-over-nonlinear constraints at shooting node N @@ -2080,28 +2089,28 @@ class AcadosOcpConstraints: if isinstance(lsphi_e, np.ndarray): self.__lsphi_e = lsphi_e else: - raise Exception('Invalid lsphi_e value. Exiting.') + raise Exception('Invalid lsphi_e value.') @usphi_e.setter def usphi_e(self, usphi_e): if isinstance(usphi_e, np.ndarray): self.__usphi_e = usphi_e else: - raise Exception('Invalid usphi_e value. Exiting.') + raise Exception('Invalid usphi_e value.') @idxsphi_e.setter def idxsphi_e(self, idxsphi_e): if isinstance(idxsphi_e, np.ndarray): self.__idxsphi_e = idxsphi_e else: - raise Exception('Invalid idxsphi_e value. Exiting.') + raise Exception('Invalid idxsphi_e value.') @Jsphi_e.setter def Jsphi_e(self, Jsphi_e): if isinstance(Jsphi_e, np.ndarray): self.__idxsphi_e = J_to_idx_slack(Jsphi_e) else: - raise Exception('Invalid Jsphi_e value. Exiting.') + raise Exception('Invalid Jsphi_e value.') def set(self, attr, value): setattr(self, attr, value) @@ -2124,6 +2133,7 @@ class AcadosOcpOptions: self.__sim_method_num_stages = 4 # number of stages in the integrator self.__sim_method_num_steps = 1 # number of steps in the integrator self.__sim_method_newton_iter = 3 # number of Newton iterations in simulation method + self.__sim_method_newton_tol = 0.0 self.__sim_method_jac_reuse = 0 self.__qp_solver_tol_stat = None # QP solver stationarity tolerance self.__qp_solver_tol_eq = None # QP solver equality tolerance @@ -2132,16 +2142,17 @@ class AcadosOcpOptions: self.__qp_solver_iter_max = 50 # QP solver max iter self.__qp_solver_cond_N = None # QP solver: new horizon after partial condensing self.__qp_solver_warm_start = 0 + self.__qp_solver_cond_ric_alg = 1 + self.__qp_solver_ric_alg = 1 self.__nlp_solver_tol_stat = 1e-6 # NLP solver stationarity tolerance self.__nlp_solver_tol_eq = 1e-6 # NLP solver equality tolerance self.__nlp_solver_tol_ineq = 1e-6 # NLP solver inequality self.__nlp_solver_tol_comp = 1e-6 # NLP solver complementarity self.__nlp_solver_max_iter = 100 # NLP solver maximum number of iterations + self.__nlp_solver_ext_qp_res = 0 self.__Tsim = None # automatically calculated as tf/N self.__print_level = 0 # print level self.__initialize_t_slacks = 0 # possible values: 0, 1 - self.__model_external_shared_lib_dir = None # path to the the .so lib - self.__model_external_shared_lib_name = None # name of the the .so lib self.__regularize_method = None self.__time_steps = None self.__shooting_nodes = None @@ -2156,16 +2167,93 @@ class AcadosOcpOptions: self.__full_step_dual = 0 self.__eps_sufficient_descent = 1e-4 self.__hpipm_mode = 'BALANCE' - + # TODO: move those out? they are more about generation than about the acados OCP solver. + self.__ext_fun_compile_flags = '-O2' + self.__model_external_shared_lib_dir = None # path to the the .so lib + self.__model_external_shared_lib_name = None # name of the the .so lib + self.__custom_update_filename = '' + self.__custom_update_header_filename = '' + self.__custom_templates = [] + self.__custom_update_copy = True @property def qp_solver(self): """QP solver to be used in the NLP solver. - String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP'). + String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', 'FULL_CONDENSING_DAQP'). Default: 'PARTIAL_CONDENSING_HPIPM'. """ return self.__qp_solver + @property + def ext_fun_compile_flags(self): + """ + String with compiler flags for external function compilation. + Default: '-O2'. + """ + return self.__ext_fun_compile_flags + + + @property + def custom_update_filename(self): + """ + Filename of the custom C function to update solver data and parameters in between solver calls + + This file has to implement the functions + int custom_update_init_function([model.name]_solver_capsule* capsule); + int custom_update_function([model.name]_solver_capsule* capsule); + int custom_update_terminate_function([model.name]_solver_capsule* capsule); + + + Default: ''. + """ + return self.__custom_update_filename + + + @property + def custom_templates(self): + """ + List of tuples of the form: + (input_filename, output_filename) + + Custom templates are render in OCP solver generation. + + Default: []. + """ + return self.__custom_templates + + + @property + def custom_update_header_filename(self): + """ + Header filename of the custom C function to update solver data and parameters in between solver calls + + This file has to declare the custom_update functions and look as follows: + + ``` + // 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); + + Default: ''. + """ + return self.__custom_update_header_filename + + @property + def custom_update_copy(self): + """ + Boolean; + If True, the custom update function files are copied into the `code_export_directory`. + """ + return self.__custom_update_copy + + @property def hpipm_mode(self): """ @@ -2230,6 +2318,13 @@ class AcadosOcpOptions: """Regularization method for the Hessian. String in ('NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY') or :code:`None`. + - MIRROR: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, abs(D_ii)) + - PROJECT: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, D_ii) + - CONVEXIFY: Algorithm 6 from Verschueren2017, https://cdn.syscop.de/publications/Verschueren2017.pdf + - PROJECT_REDUC_HESS: experimental + + Note: default eps = 1e-4 + Default: :code:`None`. """ return self.__regularize_method @@ -2279,6 +2374,15 @@ class AcadosOcpOptions: """ return self.__sim_method_newton_iter + @property + def sim_method_newton_tol(self): + """ + Tolerance of Newton system in simulation method. + Type: float: 0.0 means not used + Default: 0.0 + """ + return self.__sim_method_newton_tol + @property def sim_method_jac_reuse(self): """ @@ -2328,10 +2432,42 @@ class AcadosOcpOptions: @property def qp_solver_warm_start(self): - """QP solver: Warm starting. - 0: no warm start; 1: warm start; 2: hot start.""" + """ + QP solver: Warm starting. + 0: no warm start; 1: warm start; 2: hot start. + Default: 0 + """ return self.__qp_solver_warm_start + @property + def qp_solver_cond_ric_alg(self): + """ + QP solver: Determines which algorithm is used in HPIPM condensing. + 0: dont factorize hessian in the condensing; 1: factorize. + Default: 1 + """ + return self.__qp_solver_cond_ric_alg + + @property + def qp_solver_ric_alg(self): + """ + QP solver: Determines which algorithm is used in HPIPM OCP QP solver. + 0 classical Riccati, 1 square-root Riccati. + + Note: taken from [HPIPM paper]: + + (a) the classical implementation requires the reduced Hessian with respect to the dynamics + equality constraints to be positive definite, but allows the full-space Hessian to be indefinite) + (b) the square-root implementation, which in order to reduce the flop count employs the Cholesky + factorization of the Riccati recursion matrix, and therefore requires the full-space Hessian to be positive definite + + [HPIPM paper]: HPIPM: a high-performance quadratic programming framework for model predictive control, Frison and Diehl, 2020 + https://cdn.syscop.de/publications/Frison2020a.pdf + + Default: 1 + """ + return self.__qp_solver_ric_alg + @property def qp_solver_iter_max(self): """ @@ -2429,6 +2565,15 @@ class AcadosOcpOptions: """NLP solver inequality tolerance""" return self.__nlp_solver_tol_ineq + @property + def nlp_solver_ext_qp_res(self): + """Determines if residuals of QP are computed externally within NLP solver (for debugging) + + Type: int; 0 or 1; + Default: 0. + """ + return self.__nlp_solver_ext_qp_res + @property def nlp_solver_tol_comp(self): """NLP solver complementarity tolerance""" @@ -2531,12 +2676,13 @@ class AcadosOcpOptions: def qp_solver(self, qp_solver): qp_solvers = ('PARTIAL_CONDENSING_HPIPM', \ 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', \ - 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP') + 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP', \ + 'FULL_CONDENSING_DAQP') if qp_solver in qp_solvers: self.__qp_solver = qp_solver else: raise Exception('Invalid qp_solver value. Possible values are:\n\n' \ - + ',\n'.join(qp_solvers) + '.\n\nYou have: ' + qp_solver + '.\n\nExiting.') + + ',\n'.join(qp_solvers) + '.\n\nYou have: ' + qp_solver + '.\n\n') @regularize_method.setter def regularize_method(self, regularize_method): @@ -2546,7 +2692,7 @@ class AcadosOcpOptions: self.__regularize_method = regularize_method else: raise Exception('Invalid regularize_method value. Possible values are:\n\n' \ - + ',\n'.join(regularize_methods) + '.\n\nYou have: ' + regularize_method + '.\n\nExiting.') + + ',\n'.join(regularize_methods) + '.\n\nYou have: ' + regularize_method + '.\n\n') @collocation_type.setter def collocation_type(self, collocation_type): @@ -2555,7 +2701,7 @@ class AcadosOcpOptions: self.__collocation_type = collocation_type else: raise Exception('Invalid collocation_type value. Possible values are:\n\n' \ - + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\nExiting.') + + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\n') @hpipm_mode.setter def hpipm_mode(self, hpipm_mode): @@ -2564,7 +2710,48 @@ class AcadosOcpOptions: self.__hpipm_mode = hpipm_mode else: raise Exception('Invalid hpipm_mode value. Possible values are:\n\n' \ - + ',\n'.join(hpipm_modes) + '.\n\nYou have: ' + hpipm_mode + '.\n\nExiting.') + + ',\n'.join(hpipm_modes) + '.\n\nYou have: ' + hpipm_mode + '.\n\n') + + @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') + + + @custom_update_filename.setter + def custom_update_filename(self, custom_update_filename): + if isinstance(custom_update_filename, str): + self.__custom_update_filename = custom_update_filename + else: + raise Exception('Invalid custom_update_filename, expected a string.\n') + + @custom_templates.setter + def custom_templates(self, custom_templates): + if not isinstance(custom_templates, list): + raise Exception('Invalid custom_templates, expected a list.\n') + for tup in custom_templates: + if not isinstance(tup, tuple): + raise Exception('Invalid custom_templates, shoubld be list of tuples.\n') + for s in tup: + if not isinstance(s, str): + raise Exception('Invalid custom_templates, shoubld be list of tuples of strings.\n') + self.__custom_templates = custom_templates + + @custom_update_header_filename.setter + def custom_update_header_filename(self, custom_update_header_filename): + if isinstance(custom_update_header_filename, str): + self.__custom_update_header_filename = custom_update_header_filename + else: + raise Exception('Invalid custom_update_header_filename, expected a string.\n') + + @custom_update_copy.setter + def custom_update_copy(self, custom_update_copy): + if isinstance(custom_update_copy, bool): + self.__custom_update_copy = custom_update_copy + else: + raise Exception('Invalid custom_update_copy, expected a bool.\n') @hessian_approx.setter def hessian_approx(self, hessian_approx): @@ -2573,7 +2760,7 @@ class AcadosOcpOptions: self.__hessian_approx = hessian_approx else: raise Exception('Invalid hessian_approx value. Possible values are:\n\n' \ - + ',\n'.join(hessian_approxs) + '.\n\nYou have: ' + hessian_approx + '.\n\nExiting.') + + ',\n'.join(hessian_approxs) + '.\n\nYou have: ' + hessian_approx + '.\n\n') @integrator_type.setter def integrator_type(self, integrator_type): @@ -2582,7 +2769,7 @@ class AcadosOcpOptions: self.__integrator_type = integrator_type else: raise Exception('Invalid integrator_type value. Possible values are:\n\n' \ - + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\nExiting.') + + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\n') @tf.setter def tf(self, tf): @@ -2619,7 +2806,7 @@ class AcadosOcpOptions: self.__globalization = globalization else: raise Exception('Invalid globalization value. Possible values are:\n\n' \ - + ',\n'.join(globalization_types) + '.\n\nYou have: ' + globalization + '.\n\nExiting.') + + ',\n'.join(globalization_types) + '.\n\nYou have: ' + globalization + '.\n\n') @alpha_min.setter def alpha_min(self, alpha_min): @@ -2655,7 +2842,7 @@ class AcadosOcpOptions: if isinstance(eps_sufficient_descent, float) and eps_sufficient_descent > 0: self.__eps_sufficient_descent = eps_sufficient_descent else: - raise Exception('Invalid eps_sufficient_descent value. eps_sufficient_descent must be a positive float. Exiting') + raise Exception('Invalid eps_sufficient_descent value. eps_sufficient_descent must be a positive float.') @sim_method_num_stages.setter def sim_method_num_stages(self, sim_method_num_stages): @@ -2663,7 +2850,7 @@ class AcadosOcpOptions: # if isinstance(sim_method_num_stages, int): # self.__sim_method_num_stages = sim_method_num_stages # else: - # raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer. Exiting.') + # raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer.') self.__sim_method_num_stages = sim_method_num_stages @@ -2673,7 +2860,7 @@ class AcadosOcpOptions: # if isinstance(sim_method_num_steps, int): # self.__sim_method_num_steps = sim_method_num_steps # else: - # raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer. Exiting.') + # raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer.') self.__sim_method_num_steps = sim_method_num_steps @@ -2683,7 +2870,7 @@ class AcadosOcpOptions: if isinstance(sim_method_newton_iter, int): self.__sim_method_newton_iter = sim_method_newton_iter else: - raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer. Exiting.') + raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer.') @sim_method_jac_reuse.setter def sim_method_jac_reuse(self, sim_method_jac_reuse): @@ -2699,42 +2886,57 @@ class AcadosOcpOptions: self.__nlp_solver_type = nlp_solver_type else: raise Exception('Invalid nlp_solver_type value. Possible values are:\n\n' \ - + ',\n'.join(nlp_solver_types) + '.\n\nYou have: ' + nlp_solver_type + '.\n\nExiting.') + + ',\n'.join(nlp_solver_types) + '.\n\nYou have: ' + nlp_solver_type + '.\n\n') @nlp_solver_step_length.setter def nlp_solver_step_length(self, nlp_solver_step_length): if isinstance(nlp_solver_step_length, float) and nlp_solver_step_length > 0: self.__nlp_solver_step_length = nlp_solver_step_length else: - raise Exception('Invalid nlp_solver_step_length value. nlp_solver_step_length must be a positive float. Exiting') + raise Exception('Invalid nlp_solver_step_length value. nlp_solver_step_length must be a positive float.') @levenberg_marquardt.setter def levenberg_marquardt(self, levenberg_marquardt): if isinstance(levenberg_marquardt, float) and levenberg_marquardt >= 0: self.__levenberg_marquardt = levenberg_marquardt else: - raise Exception('Invalid levenberg_marquardt value. levenberg_marquardt must be a positive float. Exiting') + raise Exception('Invalid levenberg_marquardt value. levenberg_marquardt must be a positive float.') @qp_solver_iter_max.setter def qp_solver_iter_max(self, qp_solver_iter_max): if isinstance(qp_solver_iter_max, int) and qp_solver_iter_max > 0: self.__qp_solver_iter_max = qp_solver_iter_max else: - raise Exception('Invalid qp_solver_iter_max value. qp_solver_iter_max must be a positive int. Exiting') + raise Exception('Invalid qp_solver_iter_max value. qp_solver_iter_max must be a positive int.') + + @qp_solver_ric_alg.setter + def qp_solver_ric_alg(self, qp_solver_ric_alg): + if qp_solver_ric_alg in [0, 1]: + self.__qp_solver_ric_alg = qp_solver_ric_alg + else: + raise Exception(f'Invalid qp_solver_ric_alg value. qp_solver_ric_alg must be in [0, 1], got {qp_solver_ric_alg}.') + + @qp_solver_cond_ric_alg.setter + def qp_solver_cond_ric_alg(self, qp_solver_cond_ric_alg): + if qp_solver_cond_ric_alg in [0, 1]: + self.__qp_solver_cond_ric_alg = qp_solver_cond_ric_alg + else: + raise Exception(f'Invalid qp_solver_cond_ric_alg value. qp_solver_cond_ric_alg must be in [0, 1], got {qp_solver_cond_ric_alg}.') + @qp_solver_cond_N.setter def qp_solver_cond_N(self, qp_solver_cond_N): if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N >= 0: self.__qp_solver_cond_N = qp_solver_cond_N else: - raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int. Exiting') + raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int.') @qp_solver_warm_start.setter def qp_solver_warm_start(self, qp_solver_warm_start): if qp_solver_warm_start in [0, 1, 2]: self.__qp_solver_warm_start = qp_solver_warm_start else: - raise Exception('Invalid qp_solver_warm_start value. qp_solver_warm_start must be 0 or 1 or 2. Exiting') + raise Exception('Invalid qp_solver_warm_start value. qp_solver_warm_start must be 0 or 1 or 2.') @qp_tol.setter def qp_tol(self, qp_tol): @@ -2744,35 +2946,35 @@ class AcadosOcpOptions: self.__qp_solver_tol_stat = qp_tol self.__qp_solver_tol_comp = qp_tol else: - raise Exception('Invalid qp_tol value. qp_tol must be a positive float. Exiting') + raise Exception('Invalid qp_tol value. qp_tol must be a positive float.') @qp_solver_tol_stat.setter def qp_solver_tol_stat(self, qp_solver_tol_stat): if isinstance(qp_solver_tol_stat, float) and qp_solver_tol_stat > 0: self.__qp_solver_tol_stat = qp_solver_tol_stat else: - raise Exception('Invalid qp_solver_tol_stat value. qp_solver_tol_stat must be a positive float. Exiting') + raise Exception('Invalid qp_solver_tol_stat value. qp_solver_tol_stat must be a positive float.') @qp_solver_tol_eq.setter def qp_solver_tol_eq(self, qp_solver_tol_eq): if isinstance(qp_solver_tol_eq, float) and qp_solver_tol_eq > 0: self.__qp_solver_tol_eq = qp_solver_tol_eq else: - raise Exception('Invalid qp_solver_tol_eq value. qp_solver_tol_eq must be a positive float. Exiting') + raise Exception('Invalid qp_solver_tol_eq value. qp_solver_tol_eq must be a positive float.') @qp_solver_tol_ineq.setter def qp_solver_tol_ineq(self, qp_solver_tol_ineq): if isinstance(qp_solver_tol_ineq, float) and qp_solver_tol_ineq > 0: self.__qp_solver_tol_ineq = qp_solver_tol_ineq else: - raise Exception('Invalid qp_solver_tol_ineq value. qp_solver_tol_ineq must be a positive float. Exiting') + raise Exception('Invalid qp_solver_tol_ineq value. qp_solver_tol_ineq must be a positive float.') @qp_solver_tol_comp.setter def qp_solver_tol_comp(self, qp_solver_tol_comp): if isinstance(qp_solver_tol_comp, float) and qp_solver_tol_comp > 0: self.__qp_solver_tol_comp = qp_solver_tol_comp else: - raise Exception('Invalid qp_solver_tol_comp value. qp_solver_tol_comp must be a positive float. Exiting') + raise Exception('Invalid qp_solver_tol_comp value. qp_solver_tol_comp must be a positive float.') @tol.setter def tol(self, tol): @@ -2782,35 +2984,42 @@ class AcadosOcpOptions: self.__nlp_solver_tol_stat = tol self.__nlp_solver_tol_comp = tol else: - raise Exception('Invalid tol value. tol must be a positive float. Exiting') + raise Exception('Invalid tol value. tol must be a positive float.') @nlp_solver_tol_stat.setter def nlp_solver_tol_stat(self, nlp_solver_tol_stat): if isinstance(nlp_solver_tol_stat, float) and nlp_solver_tol_stat > 0: self.__nlp_solver_tol_stat = nlp_solver_tol_stat else: - raise Exception('Invalid nlp_solver_tol_stat value. nlp_solver_tol_stat must be a positive float. Exiting') + raise Exception('Invalid nlp_solver_tol_stat value. nlp_solver_tol_stat must be a positive float.') @nlp_solver_tol_eq.setter def nlp_solver_tol_eq(self, nlp_solver_tol_eq): if isinstance(nlp_solver_tol_eq, float) and nlp_solver_tol_eq > 0: self.__nlp_solver_tol_eq = nlp_solver_tol_eq else: - raise Exception('Invalid nlp_solver_tol_eq value. nlp_solver_tol_eq must be a positive float. Exiting') + raise Exception('Invalid nlp_solver_tol_eq value. nlp_solver_tol_eq must be a positive float.') @nlp_solver_tol_ineq.setter def nlp_solver_tol_ineq(self, nlp_solver_tol_ineq): if isinstance(nlp_solver_tol_ineq, float) and nlp_solver_tol_ineq > 0: self.__nlp_solver_tol_ineq = nlp_solver_tol_ineq else: - raise Exception('Invalid nlp_solver_tol_ineq value. nlp_solver_tol_ineq must be a positive float. Exiting') + raise Exception('Invalid nlp_solver_tol_ineq value. nlp_solver_tol_ineq must be a positive float.') + + @nlp_solver_ext_qp_res.setter + def nlp_solver_ext_qp_res(self, nlp_solver_ext_qp_res): + if nlp_solver_ext_qp_res in [0, 1]: + self.__nlp_solver_ext_qp_res = nlp_solver_ext_qp_res + else: + raise Exception('Invalid nlp_solver_ext_qp_res value. nlp_solver_ext_qp_res must be in [0, 1].') @nlp_solver_tol_comp.setter def nlp_solver_tol_comp(self, nlp_solver_tol_comp): if isinstance(nlp_solver_tol_comp, float) and nlp_solver_tol_comp > 0: self.__nlp_solver_tol_comp = nlp_solver_tol_comp else: - raise Exception('Invalid nlp_solver_tol_comp value. nlp_solver_tol_comp must be a positive float. Exiting') + raise Exception('Invalid nlp_solver_tol_comp value. nlp_solver_tol_comp must be a positive float.') @nlp_solver_max_iter.setter def nlp_solver_max_iter(self, nlp_solver_max_iter): @@ -2818,14 +3027,14 @@ class AcadosOcpOptions: if isinstance(nlp_solver_max_iter, int) and nlp_solver_max_iter > 0: self.__nlp_solver_max_iter = nlp_solver_max_iter else: - raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a positive int. Exiting') + raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a positive int.') @print_level.setter def print_level(self, print_level): if isinstance(print_level, int) and print_level >= 0: self.__print_level = print_level else: - raise Exception('Invalid print_level value. print_level takes one of the values >=0. Exiting') + raise Exception('Invalid print_level value. print_level takes one of the values >=0.') @model_external_shared_lib_dir.setter def model_external_shared_lib_dir(self, model_external_shared_lib_dir): @@ -2833,47 +3042,47 @@ class AcadosOcpOptions: self.__model_external_shared_lib_dir = model_external_shared_lib_dir else: raise Exception('Invalid model_external_shared_lib_dir value. Str expected.' \ - + '.\n\nYou have: ' + type(model_external_shared_lib_dir) + '.\n\nExiting.') + + '.\n\nYou have: ' + type(model_external_shared_lib_dir) + '.\n\n') @model_external_shared_lib_name.setter def model_external_shared_lib_name(self, model_external_shared_lib_name): if isinstance(model_external_shared_lib_name, str) : if model_external_shared_lib_name[-3:] == '.so' : raise Exception('Invalid model_external_shared_lib_name value. Remove the .so extension.' \ - + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.') + + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\n') else : self.__model_external_shared_lib_name = model_external_shared_lib_name else: raise Exception('Invalid model_external_shared_lib_name value. Str expected.' \ - + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.') + + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\n') @exact_hess_constr.setter def exact_hess_constr(self, exact_hess_constr): if exact_hess_constr in [0, 1]: self.__exact_hess_constr = exact_hess_constr else: - raise Exception('Invalid exact_hess_constr value. exact_hess_constr takes one of the values 0, 1. Exiting') + raise Exception('Invalid exact_hess_constr value. exact_hess_constr takes one of the values 0, 1.') @exact_hess_cost.setter def exact_hess_cost(self, exact_hess_cost): if exact_hess_cost in [0, 1]: self.__exact_hess_cost = exact_hess_cost else: - raise Exception('Invalid exact_hess_cost value. exact_hess_cost takes one of the values 0, 1. Exiting') + raise Exception('Invalid exact_hess_cost value. exact_hess_cost takes one of the values 0, 1.') @exact_hess_dyn.setter def exact_hess_dyn(self, exact_hess_dyn): if exact_hess_dyn in [0, 1]: self.__exact_hess_dyn = exact_hess_dyn else: - raise Exception('Invalid exact_hess_dyn value. exact_hess_dyn takes one of the values 0, 1. Exiting') + raise Exception('Invalid exact_hess_dyn value. exact_hess_dyn takes one of the values 0, 1.') @ext_cost_num_hess.setter def ext_cost_num_hess(self, ext_cost_num_hess): if ext_cost_num_hess in [0, 1]: self.__ext_cost_num_hess = ext_cost_num_hess else: - raise Exception('Invalid ext_cost_num_hess value. ext_cost_num_hess takes one of the values 0, 1. Exiting') + raise Exception('Invalid ext_cost_num_hess value. ext_cost_num_hess takes one of the values 0, 1.') def set(self, attr, value): setattr(self, attr, value) @@ -2893,6 +3102,7 @@ class AcadosOcp: - :py:attr:`solver_options` of type :py:class:`acados_template.acados_ocp.AcadosOcpOptions` - :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) """ @@ -2914,14 +3124,16 @@ class AcadosOcp: """Constraints definitions, type :py:class:`acados_template.acados_ocp.AcadosOcpConstraints`""" self.solver_options = AcadosOcpOptions() """Solver Options, type :py:class:`acados_template.acados_ocp.AcadosOcpOptions`""" - + 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, type: `string`""" + self.shared_lib_ext = get_lib_ext() - import numpy - self.cython_include_dirs = numpy.get_include() + # 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 = 'OCP' diff --git a/third_party/acados/acados_template/acados_ocp_solver.py b/third_party/acados/acados_template/acados_ocp_solver.py index beeda2cb0c..ffc9cf4b0e 100644 --- a/third_party/acados/acados_template/acados_ocp_solver.py +++ b/third_party/acados/acados_template/acados_ocp_solver.py @@ -1,9 +1,6 @@ # -*- 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 +# Copyright (c) The acados authors. # # This file is part of acados. # @@ -38,26 +35,29 @@ import json import numpy as np from datetime import datetime import importlib +import shutil + +from subprocess import DEVNULL, call, STDOUT + from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_int64, byref from copy import deepcopy +from pathlib import Path -from .generate_c_code_explicit_ode import generate_c_code_explicit_ode -from .generate_c_code_implicit_ode import generate_c_code_implicit_ode -from .generate_c_code_gnsf import generate_c_code_gnsf -from .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics -from .generate_c_code_constraint import generate_c_code_constraint -from .generate_c_code_nls_cost import generate_c_code_nls_cost -from .generate_c_code_external_cost import generate_c_code_external_cost +from .casadi_function_generation import generate_c_code_explicit_ode, \ + generate_c_code_implicit_ode, generate_c_code_gnsf, generate_c_code_discrete_dynamics, \ + generate_c_code_constraint, generate_c_code_nls_cost, generate_c_code_conl_cost, \ + generate_c_code_external_cost +from .gnsf.detect_gnsf_structure import detect_gnsf_structure from .acados_ocp import AcadosOcp -from .acados_model import acados_model_strip_casadi_symbolics +from .acados_model import AcadosModel from .utils import is_column, is_empty, casadi_length, render_template,\ - format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\ - set_up_imported_gnsf_model, get_ocp_nlp_layout, get_python_interface_path + format_class_dict, make_object_json_dumpable, make_model_consistent,\ + set_up_imported_gnsf_model, get_ocp_nlp_layout, get_python_interface_path, get_lib_ext, check_casadi_version from .builders import CMakeBuilder -def make_ocp_dims_consistent(acados_ocp): +def make_ocp_dims_consistent(acados_ocp: AcadosOcp): dims = acados_ocp.dims cost = acados_ocp.cost constraints = acados_ocp.constraints @@ -105,6 +105,9 @@ def make_ocp_dims_consistent(acados_ocp): model.cost_expr_ext_cost_0 = model.cost_expr_ext_cost model.cost_expr_ext_cost_custom_hess_0 = model.cost_expr_ext_cost_custom_hess + model.cost_psi_expr_0 = model.cost_psi_expr + model.cost_r_in_psi_expr_0 = model.cost_r_in_psi_expr + if cost.cost_type_0 == 'LINEAR_LS': ny_0 = cost.W_0.shape[0] if cost.Vx_0.shape[0] != ny_0 or cost.Vu_0.shape[0] != ny_0: @@ -133,6 +136,22 @@ def make_ocp_dims_consistent(acados_ocp): f'\nGot W_0[{cost.W.shape}], yref_0[{cost.yref_0.shape}]\n') dims.ny_0 = ny_0 + elif cost.cost_type_0 == 'CONVEX_OVER_NONLINEAR': + if is_empty(model.cost_y_expr_0): + raise Exception('cost_y_expr_0 and/or cost_y_expr not provided.') + ny_0 = casadi_length(model.cost_y_expr_0) + if is_empty(model.cost_r_in_psi_expr_0) or casadi_length(model.cost_r_in_psi_expr_0) != ny_0: + raise Exception('inconsistent dimension ny_0: regarding cost_y_expr_0 and cost_r_in_psi_0.') + if is_empty(model.cost_psi_expr_0) or casadi_length(model.cost_psi_expr_0) != 1: + raise Exception('cost_psi_expr_0 not provided or not scalar-valued.') + if cost.yref_0.shape[0] != ny_0: + raise Exception('inconsistent dimension: regarding yref_0 and cost_y_expr_0, cost_r_in_psi_0.') + dims.ny_0 = ny_0 + + if not (opts.hessian_approx=='EXACT' and opts.exact_hess_cost==False) and opts.hessian_approx != 'GAUSS_NEWTON': + raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" + "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n") + elif cost.cost_type_0 == 'EXTERNAL': if opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and model.cost_expr_ext_cost_custom_hess_0 is None: print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not possible!\n" @@ -171,6 +190,23 @@ def make_ocp_dims_consistent(acados_ocp): f'\nGot W[{cost.W.shape}], yref[{cost.yref.shape}]\n') dims.ny = ny + elif cost.cost_type == 'CONVEX_OVER_NONLINEAR': + if is_empty(model.cost_y_expr): + raise Exception('cost_y_expr and/or cost_y_expr not provided.') + ny = casadi_length(model.cost_y_expr) + if is_empty(model.cost_r_in_psi_expr) or casadi_length(model.cost_r_in_psi_expr) != ny: + raise Exception('inconsistent dimension ny: regarding cost_y_expr and cost_r_in_psi.') + if is_empty(model.cost_psi_expr) or casadi_length(model.cost_psi_expr) != 1: + raise Exception('cost_psi_expr not provided or not scalar-valued.') + if cost.yref.shape[0] != ny: + raise Exception('inconsistent dimension: regarding yref and cost_y_expr, cost_r_in_psi.') + dims.ny = ny + + if not (opts.hessian_approx=='EXACT' and opts.exact_hess_cost==False) and opts.hessian_approx != 'GAUSS_NEWTON': + raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" + "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n") + + elif cost.cost_type == 'EXTERNAL': if opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and model.cost_expr_ext_cost_custom_hess is None: print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not possible!\n" @@ -202,6 +238,24 @@ def make_ocp_dims_consistent(acados_ocp): raise Exception('inconsistent dimension: regarding W_e, yref_e.') dims.ny_e = ny_e + elif cost.cost_type_e == 'CONVEX_OVER_NONLINEAR': + if is_empty(model.cost_y_expr_e): + raise Exception('cost_y_expr_e not provided.') + ny_e = casadi_length(model.cost_y_expr_e) + if is_empty(model.cost_r_in_psi_expr_e) or casadi_length(model.cost_r_in_psi_expr_e) != ny_e: + raise Exception('inconsistent dimension ny_e: regarding cost_y_expr_e and cost_r_in_psi_e.') + if is_empty(model.cost_psi_expr_e) or casadi_length(model.cost_psi_expr_e) != 1: + raise Exception('cost_psi_expr_e not provided or not scalar-valued.') + if cost.yref_e.shape[0] != ny_e: + raise Exception('inconsistent dimension: regarding yref_e and cost_y_expr_e, cost_r_in_psi_e.') + dims.ny_e = ny_e + + if not (opts.hessian_approx=='EXACT' and opts.exact_hess_cost==False) and opts.hessian_approx != 'GAUSS_NEWTON': + raise Exception("\nWith CONVEX_OVER_NONLINEAR cost type, possible Hessian approximations are:\n" + "GAUSS_NEWTON or EXACT with 'exact_hess_cost' == False.\n") + + + elif cost.cost_type_e == 'EXTERNAL': if opts.hessian_approx == 'GAUSS_NEWTON' and opts.ext_cost_num_hess == 0 and model.cost_expr_ext_cost_custom_hess_e is None: print("\nWARNING: Gauss-Newton Hessian approximation with EXTERNAL cost type not possible!\n" @@ -213,16 +267,13 @@ def make_ocp_dims_consistent(acados_ocp): ## constraints # initial - if (constraints.lbx_0 == [] and constraints.ubx_0 == []): - dims.nbx_0 = 0 - else: - this_shape = constraints.lbx_0.shape - other_shape = constraints.ubx_0.shape - if not this_shape == other_shape: - raise Exception('lbx_0, ubx_0 have different shapes!') - if not is_column(constraints.lbx_0): - raise Exception('lbx_0, ubx_0 must be column vectors!') - dims.nbx_0 = constraints.lbx_0.size + this_shape = constraints.lbx_0.shape + other_shape = constraints.ubx_0.shape + if not this_shape == other_shape: + raise Exception('lbx_0, ubx_0 have different shapes!') + if not is_column(constraints.lbx_0): + raise Exception('lbx_0, ubx_0 must be column vectors!') + dims.nbx_0 = constraints.lbx_0.size if all(constraints.lbx_0 == constraints.ubx_0) and dims.nbx_0 == dims.nx \ and dims.nbxe_0 is None \ @@ -230,8 +281,10 @@ def make_ocp_dims_consistent(acados_ocp): and all(constraints.idxbxe_0 == constraints.idxbx_0): # case: x0 was set: nbx0 are all equlities. dims.nbxe_0 = dims.nbx_0 + elif constraints.idxbxe_0 is not None: + dims.nbxe_0 = constraints.idxbxe_0.shape[0] elif dims.nbxe_0 is None: - # case: x0 was not set -> dont assume nbx0 to be equality constraints. + # case: x0 and idxbxe_0 were not set -> dont assume nbx0 to be equality constraints. dims.nbxe_0 = 0 # path @@ -309,6 +362,8 @@ def make_ocp_dims_consistent(acados_ocp): # Slack dimensions nsbx = constraints.idxsbx.shape[0] + if nsbx > nbx: + raise Exception(f'inconsistent dimension nsbx = {nsbx}. Is greater than nbx = {nbx}.') if is_empty(constraints.lsbx): constraints.lsbx = np.zeros((nsbx,)) elif constraints.lsbx.shape[0] != nsbx: @@ -320,6 +375,8 @@ def make_ocp_dims_consistent(acados_ocp): dims.nsbx = nsbx nsbu = constraints.idxsbu.shape[0] + if nsbu > nbu: + raise Exception(f'inconsistent dimension nsbu = {nsbu}. Is greater than nbu = {nbu}.') if is_empty(constraints.lsbu): constraints.lsbu = np.zeros((nsbu,)) elif constraints.lsbu.shape[0] != nsbu: @@ -331,6 +388,8 @@ def make_ocp_dims_consistent(acados_ocp): dims.nsbu = nsbu nsh = constraints.idxsh.shape[0] + if nsh > nh: + raise Exception(f'inconsistent dimension nsh = {nsh}. Is greater than nh = {nh}.') if is_empty(constraints.lsh): constraints.lsh = np.zeros((nsh,)) elif constraints.lsh.shape[0] != nsh: @@ -342,6 +401,8 @@ def make_ocp_dims_consistent(acados_ocp): dims.nsh = nsh nsphi = constraints.idxsphi.shape[0] + if nsphi > dims.nphi: + raise Exception(f'inconsistent dimension nsphi = {nsphi}. Is greater than nphi = {dims.nphi}.') if is_empty(constraints.lsphi): constraints.lsphi = np.zeros((nsphi,)) elif constraints.lsphi.shape[0] != nsphi: @@ -353,6 +414,8 @@ def make_ocp_dims_consistent(acados_ocp): dims.nsphi = nsphi nsg = constraints.idxsg.shape[0] + if nsg > ng: + raise Exception(f'inconsistent dimension nsg = {nsg}. Is greater than ng = {ng}.') if is_empty(constraints.lsg): constraints.lsg = np.zeros((nsg,)) elif constraints.lsg.shape[0] != nsg: @@ -386,6 +449,8 @@ def make_ocp_dims_consistent(acados_ocp): dims.ns = ns nsbx_e = constraints.idxsbx_e.shape[0] + if nsbx_e > nbx_e: + raise Exception(f'inconsistent dimension nsbx_e = {nsbx_e}. Is greater than nbx_e = {nbx_e}.') if is_empty(constraints.lsbx_e): constraints.lsbx_e = np.zeros((nsbx_e,)) elif constraints.lsbx_e.shape[0] != nsbx_e: @@ -397,6 +462,8 @@ def make_ocp_dims_consistent(acados_ocp): dims.nsbx_e = nsbx_e nsh_e = constraints.idxsh_e.shape[0] + if nsh_e > nh_e: + raise Exception(f'inconsistent dimension nsh_e = {nsh_e}. Is greater than nh_e = {nh_e}.') if is_empty(constraints.lsh_e): constraints.lsh_e = np.zeros((nsh_e,)) elif constraints.lsh_e.shape[0] != nsh_e: @@ -408,6 +475,8 @@ def make_ocp_dims_consistent(acados_ocp): dims.nsh_e = nsh_e nsg_e = constraints.idxsg_e.shape[0] + if nsg_e > ng_e: + raise Exception(f'inconsistent dimension nsg_e = {nsg_e}. Is greater than ng_e = {ng_e}.') if is_empty(constraints.lsg_e): constraints.lsg_e = np.zeros((nsg_e,)) elif constraints.lsg_e.shape[0] != nsg_e: @@ -419,6 +488,8 @@ def make_ocp_dims_consistent(acados_ocp): dims.nsg_e = nsg_e nsphi_e = constraints.idxsphi_e.shape[0] + if nsphi_e > dims.nphi_e: + raise Exception(f'inconsistent dimension nsphi_e = {nsphi_e}. Is greater than nphi_e = {dims.nphi_e}.') if is_empty(constraints.lsphi_e): constraints.lsphi_e = np.zeros((nsphi_e,)) elif constraints.lsphi_e.shape[0] != nsphi_e: @@ -525,7 +596,7 @@ def get_simulink_default_opts(): return simulink_default_opts -def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_nlp.json'): +def ocp_formulation_json_dump(acados_ocp, simulink_opts=None, json_file='acados_ocp_nlp.json'): # Load acados_ocp_nlp structure description ocp_layout = get_ocp_nlp_layout() @@ -543,20 +614,11 @@ def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_n ocp_nlp_dict = format_class_dict(ocp_nlp_dict) - # strip symbolics - ocp_nlp_dict['model'] = acados_model_strip_casadi_symbolics(ocp_nlp_dict['model']) - - # strip shooting_nodes - ocp_nlp_dict['solver_options'].pop('shooting_nodes', None) - dims_dict = format_class_dict(acados_ocp.dims.__dict__) - - ocp_check_against_layout(ocp_nlp_dict, dims_dict) - - # add simulink options - ocp_nlp_dict['simulink_opts'] = simulink_opts + if simulink_opts is not None: + ocp_nlp_dict['simulink_opts'] = simulink_opts with open(json_file, 'w') as f: - json.dump(ocp_nlp_dict, f, default=np_array_to_list, indent=4, sort_keys=True) + json.dump(ocp_nlp_dict, f, default=make_object_json_dumpable, indent=4, sort_keys=True) @@ -587,7 +649,7 @@ def ocp_formulation_json_load(json_file='acados_ocp_nlp.json'): return acados_ocp -def ocp_generate_external_functions(acados_ocp, model): +def ocp_generate_external_functions(acados_ocp: AcadosOcp, model: AcadosModel): model = make_model_consistent(model) @@ -595,27 +657,32 @@ def ocp_generate_external_functions(acados_ocp, model): opts = dict(generate_hess=1) else: opts = dict(generate_hess=0) + + # create code_export_dir, model_dir code_export_dir = acados_ocp.code_export_directory opts['code_export_directory'] = code_export_dir - - if acados_ocp.model.dyn_ext_fun_type != 'casadi': - raise Exception("ocp_generate_external_functions: dyn_ext_fun_type only supports 'casadi' for now.\ - Extending the Python interface with generic function support is welcome.") - - if acados_ocp.solver_options.integrator_type == 'ERK': - # explicit model -- generate C code - generate_c_code_explicit_ode(model, opts) - elif acados_ocp.solver_options.integrator_type == 'IRK': - # implicit model -- generate C code - generate_c_code_implicit_ode(model, opts) - elif acados_ocp.solver_options.integrator_type == 'LIFTED_IRK': - generate_c_code_implicit_ode(model, opts) - elif acados_ocp.solver_options.integrator_type == 'GNSF': - generate_c_code_gnsf(model, opts) - elif acados_ocp.solver_options.integrator_type == 'DISCRETE': - generate_c_code_discrete_dynamics(model, opts) + model_dir = os.path.join(code_export_dir, model.name + '_model') + if not os.path.exists(model_dir): + os.makedirs(model_dir) + + check_casadi_version() + # TODO: remove dir gen from all the generate_c_* functions + if acados_ocp.model.dyn_ext_fun_type == 'casadi': + if acados_ocp.solver_options.integrator_type == 'ERK': + generate_c_code_explicit_ode(model, opts) + elif acados_ocp.solver_options.integrator_type == 'IRK': + generate_c_code_implicit_ode(model, opts) + elif acados_ocp.solver_options.integrator_type == 'LIFTED_IRK': + generate_c_code_implicit_ode(model, opts) + elif acados_ocp.solver_options.integrator_type == 'GNSF': + generate_c_code_gnsf(model, opts) + elif acados_ocp.solver_options.integrator_type == 'DISCRETE': + generate_c_code_discrete_dynamics(model, opts) + else: + raise Exception("ocp_generate_external_functions: unknown integrator type.") else: - raise Exception("ocp_generate_external_functions: unknown integrator type.") + target_location = os.path.join(code_export_dir, model_dir, model.dyn_generic_source) + shutil.copyfile(model.dyn_generic_source, target_location) if acados_ocp.dims.nphi > 0 or acados_ocp.dims.nh > 0: generate_c_code_constraint(model, model.name, False, opts) @@ -623,28 +690,24 @@ def ocp_generate_external_functions(acados_ocp, model): if acados_ocp.dims.nphi_e > 0 or acados_ocp.dims.nh_e > 0: generate_c_code_constraint(model, model.name, True, opts) - # dummy matrices - if not acados_ocp.cost.cost_type_0 == 'LINEAR_LS': - acados_ocp.cost.Vx_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nx)) - acados_ocp.cost.Vu_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nu)) - if not acados_ocp.cost.cost_type == 'LINEAR_LS': - acados_ocp.cost.Vx = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nx)) - acados_ocp.cost.Vu = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nu)) - if not acados_ocp.cost.cost_type_e == 'LINEAR_LS': - acados_ocp.cost.Vx_e = np.zeros((acados_ocp.dims.ny_e, acados_ocp.dims.nx)) - if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS': generate_c_code_nls_cost(model, model.name, 'initial', opts) + elif acados_ocp.cost.cost_type_0 == 'CONVEX_OVER_NONLINEAR': + generate_c_code_conl_cost(model, model.name, 'initial', opts) elif acados_ocp.cost.cost_type_0 == 'EXTERNAL': generate_c_code_external_cost(model, 'initial', opts) if acados_ocp.cost.cost_type == 'NONLINEAR_LS': generate_c_code_nls_cost(model, model.name, 'path', opts) + elif acados_ocp.cost.cost_type == 'CONVEX_OVER_NONLINEAR': + generate_c_code_conl_cost(model, model.name, 'path', opts) elif acados_ocp.cost.cost_type == 'EXTERNAL': generate_c_code_external_cost(model, 'path', opts) if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS': generate_c_code_nls_cost(model, model.name, 'terminal', opts) + elif acados_ocp.cost.cost_type_e == 'CONVEX_OVER_NONLINEAR': + generate_c_code_conl_cost(model, model.name, 'terminal', opts) elif acados_ocp.cost.cost_type_e == 'EXTERNAL': generate_c_code_external_cost(model, 'terminal', opts) @@ -659,9 +722,8 @@ def ocp_get_default_cmake_builder() -> CMakeBuilder: return cmake_builder -def ocp_render_templates(acados_ocp, json_file, cmake_builder=None): - name = acados_ocp.model.name +def ocp_render_templates(acados_ocp: AcadosOcp, json_file, cmake_builder=None, simulink_opts=None): # setting up loader and environment json_path = os.path.abspath(json_file) @@ -669,132 +731,69 @@ def ocp_render_templates(acados_ocp, json_file, cmake_builder=None): if not os.path.exists(json_path): raise Exception(f'Path "{json_path}" not found!') - code_export_dir = acados_ocp.code_export_directory - template_dir = code_export_dir + # Render templates + template_list = __ocp_get_template_list(acados_ocp, cmake_builder=cmake_builder, simulink_opts=simulink_opts) + for tup in template_list: + if len(tup) > 2: + output_dir = tup[2] + else: + output_dir = acados_ocp.code_export_directory + render_template(tup[0], tup[1], output_dir, json_path) - ## Render templates - in_file = 'main.in.c' - out_file = f'main_{name}.c' - render_template(in_file, out_file, template_dir, json_path) + # Custom templates + acados_template_path = os.path.dirname(os.path.abspath(__file__)) + custom_template_glob = os.path.join(acados_template_path, 'custom_update_templates', '*') + for tup in acados_ocp.solver_options.custom_templates: + render_template(tup[0], tup[1], acados_ocp.code_export_directory, json_path, template_glob=custom_template_glob) - in_file = 'acados_solver.in.c' - out_file = f'acados_solver_{name}.c' - render_template(in_file, out_file, template_dir, json_path) + return - in_file = 'acados_solver.in.h' - out_file = f'acados_solver_{name}.h' - render_template(in_file, out_file, template_dir, json_path) - in_file = 'acados_solver.in.pxd' - out_file = f'acados_solver.pxd' - render_template(in_file, out_file, template_dir, json_path) +def __ocp_get_template_list(acados_ocp: AcadosOcp, cmake_builder=None, simulink_opts=None) -> list: + """ + returns a list of tuples in the form: + (input_filename, output_filname) + or + (input_filename, output_filname, output_directory) + """ + name = acados_ocp.model.name + code_export_directory = acados_ocp.code_export_directory + template_list = [] + + template_list.append(('main.in.c', f'main_{name}.c')) + template_list.append(('acados_solver.in.c', f'acados_solver_{name}.c')) + template_list.append(('acados_solver.in.h', f'acados_solver_{name}.h')) + template_list.append(('acados_solver.in.pxd', f'acados_solver.pxd')) if cmake_builder is not None: - in_file = 'CMakeLists.in.txt' - out_file = 'CMakeLists.txt' - render_template(in_file, out_file, template_dir, json_path) + template_list.append(('CMakeLists.in.txt', 'CMakeLists.txt')) else: - in_file = 'Makefile.in' - out_file = 'Makefile' - render_template(in_file, out_file, template_dir, json_path) - - in_file = 'acados_solver_sfun.in.c' - out_file = f'acados_solver_sfunction_{name}.c' - render_template(in_file, out_file, template_dir, json_path) + template_list.append(('Makefile.in', 'Makefile')) - in_file = 'make_sfun.in.m' - out_file = f'make_sfun_{name}.m' - render_template(in_file, out_file, template_dir, json_path) # sim - in_file = 'acados_sim_solver.in.c' - out_file = f'acados_sim_solver_{name}.c' - render_template(in_file, out_file, template_dir, json_path) - - in_file = 'acados_sim_solver.in.h' - out_file = f'acados_sim_solver_{name}.h' - render_template(in_file, out_file, template_dir, json_path) - - in_file = 'main_sim.in.c' - out_file = f'main_sim_{name}.c' - render_template(in_file, out_file, template_dir, json_path) - - ## folder model - template_dir = os.path.join(code_export_dir, name + '_model') - in_file = 'model.in.h' - out_file = f'{name}_model.h' - render_template(in_file, out_file, template_dir, json_path) - - # constraints on convex over nonlinear function - if acados_ocp.constraints.constr_type == 'BGP' and acados_ocp.dims.nphi > 0: - # constraints on outer function - template_dir = os.path.join(code_export_dir, name + '_constraints') - in_file = 'phi_constraint.in.h' - out_file = f'{name}_phi_constraint.h' - render_template(in_file, out_file, template_dir, json_path) - - # terminal constraints on convex over nonlinear function - if acados_ocp.constraints.constr_type_e == 'BGP' and acados_ocp.dims.nphi_e > 0: - # terminal constraints on outer function - template_dir = os.path.join(code_export_dir, name + '_constraints') - in_file = 'phi_e_constraint.in.h' - out_file = f'{name}_phi_e_constraint.h' - render_template(in_file, out_file, template_dir, json_path) - - # nonlinear constraints - if acados_ocp.constraints.constr_type == 'BGH' and acados_ocp.dims.nh > 0: - template_dir = os.path.join(code_export_dir, name + '_constraints') - in_file = 'h_constraint.in.h' - out_file = f'{name}_h_constraint.h' - render_template(in_file, out_file, template_dir, json_path) - - # terminal nonlinear constraints - if acados_ocp.constraints.constr_type_e == 'BGH' and acados_ocp.dims.nh_e > 0: - template_dir = os.path.join(code_export_dir, name + '_constraints') - in_file = 'h_e_constraint.in.h' - out_file = f'{name}_h_e_constraint.h' - render_template(in_file, out_file, template_dir, json_path) - - # initial stage Nonlinear LS cost function - if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS': - template_dir = os.path.join(code_export_dir, name + '_cost') - in_file = 'cost_y_0_fun.in.h' - out_file = f'{name}_cost_y_0_fun.h' - render_template(in_file, out_file, template_dir, json_path) - # external cost - terminal - elif acados_ocp.cost.cost_type_0 == 'EXTERNAL': - template_dir = os.path.join(code_export_dir, name + '_cost') - in_file = 'external_cost_0.in.h' - out_file = f'{name}_external_cost_0.h' - render_template(in_file, out_file, template_dir, json_path) - - # path Nonlinear LS cost function - if acados_ocp.cost.cost_type == 'NONLINEAR_LS': - template_dir = os.path.join(code_export_dir, name + '_cost') - in_file = 'cost_y_fun.in.h' - out_file = f'{name}_cost_y_fun.h' - render_template(in_file, out_file, template_dir, json_path) - - # terminal Nonlinear LS cost function - if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS': - template_dir = os.path.join(code_export_dir, name + '_cost') - in_file = 'cost_y_e_fun.in.h' - out_file = f'{name}_cost_y_e_fun.h' - render_template(in_file, out_file, template_dir, json_path) - - # external cost - if acados_ocp.cost.cost_type == 'EXTERNAL': - template_dir = os.path.join(code_export_dir, name + '_cost') - in_file = 'external_cost.in.h' - out_file = f'{name}_external_cost.h' - render_template(in_file, out_file, template_dir, json_path) - - # external cost - terminal - if acados_ocp.cost.cost_type_e == 'EXTERNAL': - template_dir = os.path.join(code_export_dir, name + '_cost') - in_file = 'external_cost_e.in.h' - out_file = f'{name}_external_cost_e.h' - render_template(in_file, out_file, template_dir, json_path) + template_list.append(('acados_sim_solver.in.c', f'acados_sim_solver_{name}.c')) + template_list.append(('acados_sim_solver.in.h', f'acados_sim_solver_{name}.h')) + template_list.append(('main_sim.in.c', f'main_sim_{name}.c')) + + # model + model_dir = os.path.join(code_export_directory, f'{name}_model') + template_list.append(('model.in.h', f'{name}_model.h', model_dir)) + # constraints + constraints_dir = os.path.join(code_export_directory, f'{name}_constraints') + template_list.append(('constraints.in.h', f'{name}_constraints.h', constraints_dir)) + # cost + cost_dir = os.path.join(code_export_directory, f'{name}_cost') + template_list.append(('cost.in.h', f'{name}_cost.h', cost_dir)) + + # Simulink + if simulink_opts is not None: + template_file = os.path.join('matlab_templates', 'acados_solver_sfun.in.c') + template_list.append((template_file, f'acados_solver_sfunction_{name}.c')) + template_file = os.path.join('matlab_templates', 'acados_solver_sfun.in.c') + template_list.append((template_file, f'make_sfun_{name}.m')) + + return template_list def remove_x0_elimination(acados_ocp): @@ -820,7 +819,7 @@ class AcadosOcpSolver: dlclose.argtypes = [c_void_p] @classmethod - def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, cmake_builder: CMakeBuilder = None): + def generate(cls, acados_ocp: AcadosOcp, json_file='acados_ocp_nlp.json', simulink_opts=None, cmake_builder: CMakeBuilder = None): """ Generates the code for an acados OCP solver, given the description in acados_ocp. :param acados_ocp: type AcadosOcp - description of the OCP for acados @@ -834,15 +833,15 @@ class AcadosOcpSolver: model = acados_ocp.model acados_ocp.code_export_directory = os.path.abspath(acados_ocp.code_export_directory) - if simulink_opts is None: - simulink_opts = get_simulink_default_opts() - # make dims consistent make_ocp_dims_consistent(acados_ocp) # module dependent post processing if acados_ocp.solver_options.integrator_type == 'GNSF': - set_up_imported_gnsf_model(acados_ocp) + if 'gnsf_model' in acados_ocp.__dict__: + set_up_imported_gnsf_model(acados_ocp) + else: + detect_gnsf_structure(acados_ocp) if acados_ocp.solver_options.qp_solver == 'PARTIAL_CONDENSING_QPDUNES': remove_x0_elimination(acados_ocp) @@ -854,15 +853,23 @@ class AcadosOcpSolver: ocp_generate_external_functions(acados_ocp, model) # dump to json - ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file) + acados_ocp.json_file = json_file + ocp_formulation_json_dump(acados_ocp, simulink_opts=simulink_opts, json_file=json_file) # render templates - ocp_render_templates(acados_ocp, json_file, cmake_builder=cmake_builder) - acados_ocp.json_file = json_file + ocp_render_templates(acados_ocp, json_file, cmake_builder=cmake_builder, simulink_opts=simulink_opts) + + # copy custom update function + if acados_ocp.solver_options.custom_update_filename != "" and acados_ocp.solver_options.custom_update_copy: + target_location = os.path.join(acados_ocp.code_export_directory, acados_ocp.solver_options.custom_update_filename) + shutil.copyfile(acados_ocp.solver_options.custom_update_filename, target_location) + if acados_ocp.solver_options.custom_update_header_filename != "" and acados_ocp.solver_options.custom_update_copy: + target_location = os.path.join(acados_ocp.code_export_directory, acados_ocp.solver_options.custom_update_header_filename) + shutil.copyfile(acados_ocp.solver_options.custom_update_header_filename, target_location) @classmethod - def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None): + def build(cls, code_export_dir, with_cython=False, cmake_builder: CMakeBuilder = None, verbose: bool = True): """ Builds the code for an acados OCP solver, that has been generated in code_export_dir :param code_export_dir: directory in which acados OCP solver has been generated, see generate() @@ -870,19 +877,36 @@ class AcadosOcpSolver: :param cmake_builder: type :py:class:`~acados_template.builders.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` + :param verbose: indicating if build command is printed """ code_export_dir = os.path.abspath(code_export_dir) - cwd=os.getcwd() + cwd = os.getcwd() os.chdir(code_export_dir) if with_cython: - os.system('make clean_ocp_cython') - os.system('make ocp_cython') + call( + ['make', 'clean_all'], + stdout=None if verbose else DEVNULL, + stderr=None if verbose else STDOUT + ) + call( + ['make', 'ocp_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) else: - os.system('make clean_ocp_shared_lib') - os.system('make ocp_shared_lib') + call( + ['make', 'clean_ocp_shared_lib'], + stdout=None if verbose else DEVNULL, + stderr=None if verbose else STDOUT + ) + call( + ['make', 'ocp_shared_lib'], + stdout=None if verbose else DEVNULL, + stderr=None if verbose else STDOUT + ) os.chdir(cwd) @@ -910,7 +934,7 @@ class AcadosOcpSolver: acados_ocp_json['dims']['N']) - def __init__(self, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True, generate=True, cmake_builder: CMakeBuilder = None): + def __init__(self, acados_ocp: AcadosOcp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True, generate=True, cmake_builder: CMakeBuilder = None, verbose=True): self.solver_created = False if generate: @@ -927,15 +951,13 @@ class AcadosOcpSolver: code_export_directory = acados_ocp_json['code_export_directory'] if build: - self.build(code_export_directory, with_cython=False, cmake_builder=cmake_builder) + self.build(code_export_directory, with_cython=False, cmake_builder=cmake_builder, verbose=verbose) # prepare library loading lib_prefix = 'lib' - lib_ext = '.so' + lib_ext = get_lib_ext() if os.name == 'nt': lib_prefix = '' - lib_ext = '' - # ToDo: check for mac # 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. @@ -970,16 +992,23 @@ class AcadosOcpSolver: assert getattr(self.shared_lib, f"{self.model_name}_acados_create")(self.capsule)==0 self.solver_created = True + self.acados_ocp = acados_ocp + # get pointers solver self.__get_pointers_solver() self.status = 0 + # gettable fields + self.__qp_dynamics_fields = ['A', 'B', 'b'] + self.__qp_cost_fields = ['Q', 'R', 'S', 'q', 'r'] + self.__qp_constraint_fields = ['C', 'D', 'lg', 'ug', 'lbx', 'ubx', 'lbu', 'ubu'] + def __get_pointers_solver(self): - """ - Private function to get the pointers for solver - """ + # """ + # Private function to get the pointers for solver + # """ # get pointers solver getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").argtypes = [c_void_p] getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").restype = c_void_p @@ -1010,6 +1039,25 @@ class AcadosOcpSolver: self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_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. @@ -1021,13 +1069,31 @@ class AcadosOcpSolver: return self.status - def reset(self): + def custom_update(self, data_: np.ndarray): + """ + 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 = np.ascontiguousarray(data_, dtype=np.float64) + c_data = cast(data.ctypes.data, POINTER(c_double)) + data_len = len(data) + + getattr(self.shared_lib, f"{self.model_name}_acados_custom_update").argtypes = [c_void_p, POINTER(c_double), c_int] + getattr(self.shared_lib, f"{self.model_name}_acados_custom_update").restype = c_int + status = getattr(self.shared_lib, f"{self.model_name}_acados_custom_update")(self.capsule, c_data, data_len) + + return status + + + def reset(self, reset_qp_solver_mem=1): """ Sets current iterate to all zeros. """ - getattr(self.shared_lib, f"{self.model_name}_acados_reset").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_reset").argtypes = [c_void_p, c_int] getattr(self.shared_lib, f"{self.model_name}_acados_reset").restype = c_int - getattr(self.shared_lib, f"{self.model_name}_acados_reset")(self.capsule) + getattr(self.shared_lib, f"{self.model_name}_acados_reset")(self.capsule, reset_qp_solver_mem) return @@ -1175,18 +1241,17 @@ class AcadosOcpSolver: field = field_ if (field_ not in all_fields): - raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\ - \n Possible values are {}. Exiting.'.format(field_, all_fields)) + raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): \'{field_}\' is an invalid argument.\ + \n Possible values are {all_fields}.') if not isinstance(stage_, int): - raise Exception('AcadosOcpSolver.get(): stage index must be Integer.') + raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): stage index must be an integer, got type {type(stage_)}.') if stage_ < 0 or stage_ > self.N: - raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(stage_)) + raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): stage index must be in [0, {self.N}], got: {stage_}.') if stage_ == self.N and field_ == 'pi': - raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\ - .format(field_, stage_)) + raise Exception(f'AcadosOcpSolver.get(stage={stage_}, field={field_}): field \'{field_}\' does not exist at final stage {stage_}.') if field_ in sens_fields: field = field_.replace('sens_', '') @@ -1265,15 +1330,15 @@ class AcadosOcpSolver: return - def store_iterate(self, filename='', overwrite=False): + def store_iterate(self, filename: str = '', 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 filename: if not set, use f'{self.model_name}_iterate.json' :param overwrite: if false and filename exists add timestamp to filename """ if filename == '': - filename += self.model_name + '_' + 'iterate' + '.json' + filename = f'{self.model_name}_iterate.json' if not overwrite: # append timestamp @@ -1284,23 +1349,70 @@ class AcadosOcpSolver: # get iterate: solution = dict() + lN = len(str(self.N+1)) for i in range(self.N+1): - solution['x_'+str(i)] = self.get(i,'x') - solution['u_'+str(i)] = self.get(i,'u') - solution['z_'+str(i)] = self.get(i,'z') - solution['lam_'+str(i)] = self.get(i,'lam') - solution['t_'+str(i)] = self.get(i, 't') - solution['sl_'+str(i)] = self.get(i, 'sl') - solution['su_'+str(i)] = self.get(i, 'su') - for i in range(self.N): - solution['pi_'+str(i)] = self.get(i,'pi') + 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=np_array_to_list, indent=4, sort_keys=True) + json.dump(solution, f, default=make_object_json_dumpable, indent=4, sort_keys=True) print("stored current iterate in ", os.path.join(os.getcwd(), filename)) + + def dump_last_qp_to_json(self, filename: str = '', overwrite=False): + """ + Dumps the latest QP data into a json file + + :param filename: if not set, use model_name + timestamp + '.json' + :param overwrite: if false and filename exists add timestamp to filename + """ + if filename == '': + filename = f'{self.model_name}_QP.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 QP data: + qp_data = dict() + + lN = len(str(self.N+1)) + for field in self.__qp_dynamics_fields: + for i in range(self.N): + qp_data[f'{field}_{i:0{lN}d}'] = self.get_from_qp_in(i,field) + + for field in self.__qp_constraint_fields + self.__qp_cost_fields: + for i in range(self.N+1): + qp_data[f'{field}_{i:0{lN}d}'] = self.get_from_qp_in(i,field) + + # remove empty fields + for k in list(qp_data.keys()): + if len(qp_data[k]) == 0: + del qp_data[k] + + # save + with open(filename, 'w') as f: + json.dump(qp_data, f, default=make_object_json_dumpable, indent=4, sort_keys=True) + print("stored qp from solver memory 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. @@ -1412,7 +1524,7 @@ class AcadosOcpSolver: return self.get_residuals() else: - raise Exception(f'AcadosOcpSolver.get_stats(): {field} is not a valid argument.' + raise Exception(f'AcadosOcpSolver.get_stats(): \'{field}\' is not a valid argument.' + f'\n Possible values are {fields}.') @@ -1440,6 +1552,12 @@ class AcadosOcpSolver: def get_residuals(self, recompute=False): """ Returns an array of the form [res_stat, res_eq, res_ineq, res_comp]. + This residual has to be computed for SQP_RTI solver, since it is not available by default. + + - res_stat: stationarity residual + - res_eq: residual wrt equality constraints (dynamics) + - res_ineq: residual wrt inequality constraints (constraints) + - res_comp: residual wrt complementarity conditions """ # compute residuals if RTI if self.solver_options['nlp_solver_type'] == 'SQP_RTI' or recompute: @@ -1499,24 +1617,22 @@ class AcadosOcpSolver: value_ = np.array([value_]) value_ = value_.astype(float) - field = field_ - field = field.encode('utf-8') + field = field_.encode('utf-8') stage = c_int(stage_) # treat parameters separately if field_ == 'p': - getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)] + getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double), c_int] getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int value_data = cast(value_.ctypes.data, POINTER(c_double)) assert getattr(self.shared_lib, f"{self.model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0 else: - if field_ not in constraints_fields + cost_fields + out_fields: - raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\ - \nPossible values are {}. Exiting.".format(field, \ - constraints_fields + cost_fields + out_fields + ['p'])) + if field_ not in constraints_fields + cost_fields + out_fields + mem_fields: + raise Exception(f"AcadosOcpSolver.set(): '{field}' is not a valid argument.\n" + f" Possible values are {constraints_fields + cost_fields + out_fields + mem_fields + ['p']}.") self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \ [c_void_p, c_void_p, c_void_p, c_int, c_char_p] @@ -1526,8 +1642,8 @@ class AcadosOcpSolver: self.nlp_dims, self.nlp_out, stage_, field) if value_.shape[0] != dims: - msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_) - msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0]) + msg = f'AcadosOcpSolver.set(): mismatching dimension for field "{field_}" ' + msg += f'with dimension {dims} (you have {value_.shape[0]})' raise Exception(msg) value_data = cast(value_.ctypes.data, POINTER(c_double)) @@ -1553,6 +1669,13 @@ class AcadosOcpSolver: [c_void_p, c_void_p, c_int, c_char_p, c_void_p] self.shared_lib.ocp_nlp_set(self.nlp_config, \ self.nlp_solver, stage, field, value_data_p) + # also set z_guess, when setting z. + if field_ == 'z': + field = 'z_guess'.encode('utf-8') + self.shared_lib.ocp_nlp_set.argtypes = \ + [c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_set(self.nlp_config, \ + self.nlp_solver, stage, field, value_data_p) return @@ -1561,7 +1684,7 @@ class AcadosOcpSolver: 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 field: string, e.g. 'yref', 'W', 'ext_cost_num_hess', 'zl', 'zu', 'Zl', 'Zu' :param value: of appropriate size """ # cast value_ to avoid conversion issues @@ -1595,7 +1718,7 @@ class AcadosOcpSolver: raise Exception("Ambiguity in API detected.\n" "Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n" "Are you seeing this error suddenly in previously running code? Read on.\n" - " You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) + + f" You are relying on a now-fixed bug in cost_set for field '{field_}'.\n" + " acados_template now correctly passes on any matrices to acados in column major format.\n" + " Two options to fix this error: \n" + " * Add api='old' to cost_set to restore old incorrect behaviour\n" + @@ -1663,7 +1786,7 @@ class AcadosOcpSolver: raise Exception("Ambiguity in API detected.\n" "Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n" "Are you seeing this error suddenly in previously running code? Read on.\n" - " You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) + + f" You are relying on a now-fixed bug in constraints_set for field '{field}'.\n" + " acados_template now correctly passes on any matrices to acados in column major format.\n" + " Two options to fix this error: \n" + " * Add api='old' to constraints_set to restore old incorrect behaviour\n" + @@ -1676,7 +1799,7 @@ class AcadosOcpSolver: # Get elements in column major order value_ = np.ravel(value_, order='F') else: - raise Exception("Unknown api: '{}'".format(api)) + raise Exception(f"Unknown api: '{api}'") if value_shape != tuple(dims): raise Exception(f'AcadosOcpSolver.constraints_set(): mismatching dimension' + @@ -1693,27 +1816,35 @@ class AcadosOcpSolver: return - def dynamics_get(self, stage_, field_): + def get_from_qp_in(self, stage_: int, field_: str): """ - Get numerical data from the dynamics module of the solver: + Get numerical data from the current QP. :param stage: integer corresponding to shooting node - :param field: string, e.g. 'A' + :param field: string in ['A', 'B', 'b', 'Q', 'R', 'S', 'q', 'r', 'C', 'D', 'lg', 'ug', 'lbx', 'ubx', 'lbu', 'ubu'] """ + # idx* should be added too.. + if not isinstance(stage_, int): + raise TypeError("stage should be int") + if stage_ > self.N: + raise Exception("stage should be <= self.N") + if field_ in self.__qp_dynamics_fields and stage_ >= self.N: + raise ValueError(f"dynamics field {field_} not available at terminal stage") + if field_ not in self.__qp_dynamics_fields + self.__qp_cost_fields + self.__qp_constraint_fields: + raise Exception(f"field {field_} not supported.") - field = field_ - field = field.encode('utf-8') + field = field_.encode('utf-8') stage = c_int(stage_) # get dims - self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr.argtypes = \ + self.shared_lib.ocp_nlp_qp_dims_get_from_attr.argtypes = \ [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] - self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr.restype = c_int + self.shared_lib.ocp_nlp_qp_dims_get_from_attr.restype = c_int dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) dims_data = cast(dims.ctypes.data, POINTER(c_int)) - self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, \ + self.shared_lib.ocp_nlp_qp_dims_get_from_attr(self.nlp_config, \ self.nlp_dims, self.nlp_out, stage_, field, dims_data) # create output data @@ -1748,32 +1879,34 @@ class AcadosOcpSolver: - 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'] + 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'] # 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_))) + raise Exception(f'solver option \'{field_}\' must be of type int. You have {type(value_)}.') else: value_ctypes = c_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_))) + raise Exception(f'solver option \'{field_}\' must be of type float. You have {type(value_)}.') else: value_ctypes = c_double(value_) elif field_ in string_fields: if not isinstance(value_, str): - raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_))) + raise Exception(f'solver option \'{field_}\' must be of type str. You have {type(value_)}.') else: value_ctypes = value_.encode('utf-8') else: - raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\ - '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields))) + fields = ', '.join(int_fields + double_fields + string_fields) + raise Exception(f'AcadosOcpSolver.options_set() does not support field \'{field_}\'.\n'\ + f' Possible values are {fields}.') if field_ == 'rti_phase': @@ -1802,6 +1935,44 @@ class AcadosOcpSolver: return + def set_params_sparse(self, 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 np array (or iterable) of integers: indices of parameter to be set + :param param_values_: new parameter values as numpy array + """ + + # if not isinstance(idx_values_, np.ndarray) or not issubclass(type(idx_values_[0]), np.integer): + # raise Exception('idx_values_ must be np.array of integers.') + + if not isinstance(param_values_, np.ndarray): + raise Exception('param_values_ must be np.array.') + elif np.float64 != param_values_.dtype: + raise TypeError('param_values_ must be np.array of float64.') + + 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_)}.') + + if any(idx_values_ >= self.acados_ocp.dims.np): + raise Exception(f'idx_values_ contains value >= np = {self.acados_ocp.dims.np}') + + stage = c_int(stage_) + 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) + def __del__(self): if self.solver_created: getattr(self.shared_lib, f"{self.model_name}_acados_free").argtypes = [c_void_p] @@ -1815,4 +1986,6 @@ class AcadosOcpSolver: try: self.dlclose(self.shared_lib._handle) except: + print(f"WARNING: acados Python interface could not close shared_lib handle of AcadosOcpSolver {self.model_name}.\n", + "Attempting to create a new one with the same name will likely result in the old one being used!") pass diff --git a/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx b/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx index fe7fa8425a..acd7f02d0a 100644 --- a/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx +++ b/third_party/acados/acados_template/acados_ocp_solver_pyx.pyx @@ -1,9 +1,6 @@ # -*- 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 +# Copyright (c) The acados authors. # # This file is part of acados. # @@ -63,7 +60,6 @@ cdef class AcadosOcpSolverCython: cdef acados_solver_common.ocp_nlp_in *nlp_in cdef acados_solver_common.ocp_nlp_solver *nlp_solver - cdef int status cdef bint solver_created cdef str model_name @@ -88,7 +84,6 @@ cdef class AcadosOcpSolverCython: # get pointers solver self.__get_pointers_solver() - self.status = 0 def __get_pointers_solver(self): @@ -105,6 +100,24 @@ cdef class AcadosOcpSolverCython: 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. @@ -112,11 +125,24 @@ cdef class AcadosOcpSolverCython: return acados_solver.acados_solve(self.capsule) - def reset(self): + def reset(self, reset_qp_solver_mem=1): """ Sets current iterate to all zeros. """ - return acados_solver.acados_reset(self.capsule) + 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, data.data, data_len) def set_new_time_steps(self, new_time_steps): @@ -253,7 +279,7 @@ cdef class AcadosOcpSolverCython: if field_ not in out_fields: raise Exception('AcadosOcpSolverCython.get(): {} is an invalid argument.\ - \n Possible values are {}. Exiting.'.format(field_, out_fields)) + \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)) @@ -310,16 +336,22 @@ cdef class AcadosOcpSolverCython: # get iterate: solution = dict() + lN = len(str(self.N+1)) for i in range(self.N+1): - solution['x_'+str(i)] = self.get(i,'x') - solution['u_'+str(i)] = self.get(i,'u') - solution['z_'+str(i)] = self.get(i,'z') - solution['lam_'+str(i)] = self.get(i,'lam') - solution['t_'+str(i)] = self.get(i, 't') - solution['sl_'+str(i)] = self.get(i, 'sl') - solution['su_'+str(i)] = self.get(i, 'su') - for i in range(self.N): - solution['pi_'+str(i)] = self.get(i,'pi') + 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: @@ -508,6 +540,8 @@ cdef class AcadosOcpSolverCython: 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'] @@ -523,7 +557,7 @@ cdef class AcadosOcpSolverCython: else: if field_ not in constraints_fields + cost_fields + out_fields: raise Exception("AcadosOcpSolverCython.set(): {} is not a valid argument.\ - \nPossible values are {}. Exiting.".format(field, \ + \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, @@ -547,6 +581,11 @@ cdef class AcadosOcpSolverCython: acados_solver_common.ocp_nlp_set(self.nlp_config, \ self.nlp_solver, stage, field, 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, value.data) + return def cost_set(self, int stage, str field_, value_): """ @@ -556,6 +595,8 @@ cdef class AcadosOcpSolverCython: :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] @@ -589,6 +630,9 @@ cdef class AcadosOcpSolverCython: :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] @@ -606,7 +650,7 @@ cdef class AcadosOcpSolverCython: # Get elements in column major order value = np.asfortranarray(value_) - if value_shape[0] != dims[0] or value_shape[1] != dims[1]: + 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})') @@ -616,7 +660,7 @@ cdef class AcadosOcpSolverCython: return - def dynamics_get(self, int stage, str field_): + def get_from_qp_in(self, int stage, str field_): """ Get numerical data from the dynamics module of the solver: @@ -627,7 +671,7 @@ cdef class AcadosOcpSolverCython: # get dims cdef int[2] dims - acados_solver_common.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0]) + 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') @@ -701,6 +745,50 @@ cdef class AcadosOcpSolverCython: '\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, idx.data, value.data, n_update) == 0 + return + + def __del__(self): if self.solver_created: acados_solver.acados_free(self.capsule) diff --git a/third_party/acados/acados_template/acados_sim.py b/third_party/acados/acados_template/acados_sim.py index 93d5f298db..c0d6937a49 100644 --- a/third_party/acados/acados_template/acados_sim.py +++ b/third_party/acados/acados_template/acados_sim.py @@ -1,9 +1,6 @@ # -*- 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 +# Copyright (c) The acados authors. # # This file is part of acados. # @@ -33,10 +30,9 @@ # import numpy as np -import casadi as ca import os from .acados_model import AcadosModel -from .utils import get_acados_path +from .utils import get_acados_path, get_lib_ext class AcadosSimDims: """ @@ -73,28 +69,28 @@ class AcadosSimDims: if isinstance(nx, int) and nx > 0: self.__nx = nx else: - raise Exception('Invalid nx value, expected positive integer. Exiting.') + 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. Exiting.') + 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. Exiting.') + 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. Exiting.') + raise Exception('Invalid np value, expected nonnegative integer.') def set(self, attr, value): setattr(self, attr, value) @@ -112,13 +108,16 @@ class AcadosSimOpts: 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 = False + self.__output_z = True self.__sim_method_jac_reuse = 0 + self.__ext_fun_compile_flags = '-O2' @property def integrator_type(self): @@ -140,6 +139,15 @@ class AcadosSimOpts: """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""" @@ -162,7 +170,7 @@ class AcadosSimOpts: @property def output_z(self): - """Boolean determining if values for algebraic variables (corresponding to start of simulation interval) are computed. Default: False""" + """Boolean determining if values for algebraic variables (corresponding to start of simulation interval) are computed. Default: True""" return self.__output_z @property @@ -184,6 +192,21 @@ class AcadosSimOpts: """ 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') @@ -191,7 +214,7 @@ class AcadosSimOpts: self.__integrator_type = integrator_type else: raise Exception('Invalid integrator_type value. Possible values are:\n\n' \ - + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\nExiting.') + + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\n') @collocation_type.setter def collocation_type(self, collocation_type): @@ -200,7 +223,7 @@ class AcadosSimOpts: self.__collocation_type = collocation_type else: raise Exception('Invalid collocation_type value. Possible values are:\n\n' \ - + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\nExiting.') + + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\n') @T.setter def T(self, T): @@ -227,6 +250,13 @@ class AcadosSimOpts: 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): @@ -280,6 +310,7 @@ class AcadosSim: - :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) @@ -301,9 +332,14 @@ class AcadosSim: 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.cython_include_dirs = '' self.__parameter_values = np.array([]) + self.__problem_class = 'SIM' @property def parameter_values(self): diff --git a/third_party/acados/acados_template/acados_sim_layout.json b/third_party/acados/acados_template/acados_sim_layout.json index 25b149613b..e3ca4b575b 100644 --- a/third_party/acados/acados_template/acados_sim_layout.json +++ b/third_party/acados/acados_template/acados_sim_layout.json @@ -42,6 +42,12 @@ ], "sim_method_newton_iter": [ "int" + ], + "sim_method_newton_tol": [ + "float" + ], + "ext_fun_compile_flags": [ + "str" ] } } diff --git a/third_party/acados/acados_template/acados_sim_solver.py b/third_party/acados/acados_template/acados_sim_solver.py index 3588dd38cd..612f439eaf 100644 --- a/third_party/acados/acados_template/acados_sim_solver.py +++ b/third_party/acados/acados_template/acados_sim_solver.py @@ -1,9 +1,6 @@ # -*- 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 +# Copyright (c) The acados authors. # # This file is part of acados. # @@ -32,56 +29,58 @@ # POSSIBILITY OF SUCH DAMAGE.; # -import sys, os, json +import sys +import os +import json +import importlib import numpy as np -from ctypes import * +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 .generate_c_code_explicit_ode import generate_c_code_explicit_ode -from .generate_c_code_implicit_ode import generate_c_code_implicit_ode -from .generate_c_code_gnsf import generate_c_code_gnsf +from .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 .acados_model import acados_model_strip_casadi_symbolics -from .utils import is_column, render_template, format_class_dict, np_array_to_list,\ - make_model_consistent, set_up_imported_gnsf_model, get_python_interface_path +from .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): +def make_sim_dims_consistent(acados_sim: AcadosSim): dims = acados_sim.dims model = acados_sim.model # nx if is_column(model.x): - dims.nx = model.x.shape[0] + dims.nx = casadi_length(model.x) else: - raise Exception("model.x should be column vector!") + raise Exception('model.x should be column vector!') # nu - if is_column(model.u): - dims.nu = model.u.shape[0] - elif model.u == None or model.u == []: + if is_empty(model.u): dims.nu = 0 else: - raise Exception("model.u should be column vector or None!") + dims.nu = casadi_length(model.u) # nz - if is_column(model.z): - dims.nz = model.z.shape[0] - elif model.z == None or model.z == []: + if is_empty(model.z): dims.nz = 0 else: - raise Exception("model.z should be column vector or None!") + dims.nz = casadi_length(model.z) # np - if is_column(model.p): - dims.np = model.p.shape[0] - elif model.p == None or model.p == []: + if is_empty(model.p): dims.np = 0 else: - raise Exception("model.p should be column vector or None!") + 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(): @@ -92,7 +91,7 @@ def get_sim_layout(): return sim_layout -def sim_formulation_json_dump(acados_sim, json_file='acados_sim.json'): +def sim_formulation_json_dump(acados_sim: AcadosSim, json_file='acados_sim.json'): # Load acados_sim structure description sim_layout = get_sim_layout() @@ -105,11 +104,10 @@ def sim_formulation_json_dump(acados_sim, json_file='acados_sim.json'): # Copy sim object attributes dictionaries sim_dict[key]=dict(getattr(acados_sim, key).__dict__) - sim_dict['model'] = acados_model_strip_casadi_symbolics(sim_dict['model']) sim_json = format_class_dict(sim_dict) with open(json_file, 'w') as f: - json.dump(sim_json, f, default=np_array_to_list, indent=4, sort_keys=True) + json.dump(sim_json, f, default=make_object_json_dumpable, indent=4, sort_keys=True) def sim_get_default_cmake_builder() -> CMakeBuilder: @@ -122,47 +120,49 @@ def sim_get_default_cmake_builder() -> CMakeBuilder: return cmake_builder -def sim_render_templates(json_file, model_name, code_export_dir, cmake_options: CMakeBuilder = None): +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!") - template_dir = code_export_dir - - ## Render templates + # Render templates in_file = 'acados_sim_solver.in.c' out_file = f'acados_sim_solver_{model_name}.c' - render_template(in_file, out_file, template_dir, json_path) + 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, template_dir, json_path) + 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, template_dir, json_path) + 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, template_dir, json_path) + 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, template_dir, json_path) + render_template(in_file, out_file, code_export_dir, json_path) - ## folder model - template_dir = os.path.join(code_export_dir, model_name + '_model') + # 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, template_dir, json_path) + render_template(in_file, out_file, model_dir, json_path) -def sim_generate_casadi_functions(acados_sim): +def sim_generate_external_functions(acados_sim: AcadosSim): model = acados_sim.model model = make_model_consistent(model) @@ -170,7 +170,16 @@ def sim_generate_casadi_functions(acados_sim): 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': @@ -190,62 +199,117 @@ class AcadosSimSolver: the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with `MS Visual Studio`); default: `None` """ - def __init__(self, acados_sim_, json_file='acados_sim.json', build=True, cmake_builder: CMakeBuilder = None): - - self.solver_created = False + if 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] - if isinstance(acados_sim_, AcadosOcp): - # set up acados_sim_ - acados_sim = AcadosSim() - acados_sim.model = acados_sim_.model - acados_sim.dims.nx = acados_sim_.dims.nx - acados_sim.dims.nu = acados_sim_.dims.nu - acados_sim.dims.nz = acados_sim_.dims.nz - acados_sim.dims.np = acados_sim_.dims.np - acados_sim.solver_options.integrator_type = acados_sim_.solver_options.integrator_type - acados_sim.code_export_directory = acados_sim_.code_export_directory - elif isinstance(acados_sim_, AcadosSim): - acados_sim = acados_sim_ + @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.__problem_class = 'SIM' + acados_sim.code_export_directory = os.path.abspath(acados_sim.code_export_directory) - model_name = acados_sim.model.name + # make dims consistent make_sim_dims_consistent(acados_sim) - # reuse existing json and casadi functions, when creating integrator from ocp - if isinstance(acados_sim_, AcadosSim): - if acados_sim.solver_options.integrator_type == 'GNSF': + # 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) - sim_generate_casadi_functions(acados_sim) - sim_formulation_json_dump(acados_sim, json_file) - code_export_dir = acados_sim.code_export_directory - if build: - # render templates - sim_render_templates(json_file, model_name, code_export_dir, cmake_builder) + @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'] - # Compile solver - cwd = os.getcwd() - code_export_dir = os.path.abspath(code_export_dir) - os.chdir(code_export_dir) - if cmake_builder is not None: - cmake_builder.exec(code_export_dir) - else: - os.system('make sim_shared_lib') - os.chdir(cwd) + 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']) - self.sim_struct = acados_sim - model_name = self.sim_struct.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 = 'libacados.so' + 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 @@ -257,19 +321,12 @@ class AcadosSimSolver: 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) - # Ctypes - lib_prefix = 'lib' - lib_ext = '.so' - if os.name == 'nt': - lib_prefix = '' - lib_ext = '' - self.shared_lib_name = os.path.join(code_export_dir, f'{lib_prefix}acados_sim_solver_{model_name}{lib_ext}') - print(f'self.shared_lib_name = "{self.shared_lib_name}"') - + # 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")() @@ -304,23 +361,34 @@ class AcadosSimSolver: getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").restype = c_void_p self.sim_solver = getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver")(self.capsule) - nu = self.sim_struct.dims.nu - nx = self.sim_struct.dims.nx - nz = self.sim_struct.dims.nz - self.gettable = { - 'x': nx, - 'xn': nx, - 'u': nu, - 'z': nz, - 'S_forw': nx*(nx+nu), - 'Sx': nx*nx, - 'Su': nx*nu, - 'S_adj': nx+nu, - 'S_hess': (nx+nu)*(nx+nu), - 'S_algebraic': (nz)*(nx+nu), - } - - self.settable = ['S_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw + 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): @@ -338,55 +406,64 @@ class AcadosSimSolver: """ 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'] + :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_ - field = field.encode('utf-8') + field = field_.encode('utf-8') - if field_ in self.gettable.keys(): + 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 - dims = self.gettable[field_] - out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64) + 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) - if field_ == 'S_forw': - nu = self.sim_struct.dims.nu - nx = self.sim_struct.dims.nx - out = out.reshape(nx, nx+nu, order='F') - elif field_ == 'Sx': - nx = self.sim_struct.dims.nx - out = out.reshape(nx, nx, order='F') - elif field_ == 'Su': - nx = self.sim_struct.dims.nx - nu = self.sim_struct.dims.nu - out = out.reshape(nx, nu, order='F') - elif field_ == 'S_hess': - nx = self.sim_struct.dims.nx - nu = self.sim_struct.dims.nu - out = out.reshape(nx+nu, nx+nu, order='F') - elif field_ == 'S_algebraic': - nx = self.sim_struct.dims.nx - nu = self.sim_struct.dims.nu - nz = self.sim_struct.dims.nz - out = out.reshape(nz, nx+nu, order='F') + 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.keys())}') + f' available fields are {", ".join(self.gettable_vectors+self.gettable_matrices)}, {", ".join(self.gettable_scalars)}') return out - def set(self, field_, value_): + + def set(self, field_: str, value_): """ Set numerical data inside the solver. - :param field: string in ['p', 'S_adj', 'T', 'x', 'u', 'xdot', 'z'] + :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_]) @@ -395,12 +472,11 @@ class AcadosSimSolver: value_data = cast(value_.ctypes.data, POINTER(c_double)) value_data_p = cast((value_data), c_void_p) - field = field_ - field = field.encode('utf-8') + field = field_.encode('utf-8') # treat parameters separately if field_ == 'p': - model_name = self.sim_struct.model.name + 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]) @@ -420,19 +496,46 @@ class AcadosSimSolver: value_shape = (value_shape[0], 0) if value_shape != tuple(dims): - raise Exception('AcadosSimSolver.set(): mismatching dimension' \ - ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape)) + 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 self.settable: + 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(self.settable)}') + 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 @@ -451,4 +554,6 @@ class AcadosSimSolver: 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 diff --git a/third_party/acados/acados_template/acados_sim_solver_common.pxd b/third_party/acados/acados_template/acados_sim_solver_common.pxd new file mode 100644 index 0000000000..cc6a58efd7 --- /dev/null +++ b/third_party/acados/acados_template/acados_sim_solver_common.pxd @@ -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) \ No newline at end of file diff --git a/third_party/acados/acados_template/acados_sim_solver_pyx.pyx b/third_party/acados/acados_template/acados_sim_solver_pyx.pyx new file mode 100644 index 0000000000..be400addc7 --- /dev/null +++ b/third_party/acados/acados_template/acados_sim_solver_pyx.pyx @@ -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, &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, 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, 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, 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, value.data) + elif field_ in settable: + acados_sim_solver_common.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, 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, &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, &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) diff --git a/third_party/acados/acados_template/acados_solver_common.pxd b/third_party/acados/acados_template/acados_solver_common.pxd index fedd7190d9..c6d59d40a5 100644 --- a/third_party/acados/acados_template/acados_solver_common.pxd +++ b/third_party/acados/acados_template/acados_solver_common.pxd @@ -1,9 +1,6 @@ # -*- 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 +# Copyright (c) The acados authors. # # This file is part of acados. # @@ -87,7 +84,7 @@ cdef extern from "acados_c/ocp_nlp_interface.h": int stage, const char *field, int *dims_out) void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field, int *dims_out) - void ocp_nlp_dynamics_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + 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 diff --git a/third_party/acados/acados_template/builders.py b/third_party/acados/acados_template/builders.py index f595033ceb..6f21bfe8cd 100644 --- a/third_party/acados/acados_template/builders.py +++ b/third_party/acados/acados_template/builders.py @@ -34,7 +34,7 @@ import os import sys -from subprocess import call +from subprocess import DEVNULL, call, STDOUT class CMakeBuilder: @@ -78,7 +78,7 @@ class CMakeBuilder: def get_cmd3_install(self): return f'cmake --install "{self._build_dir}"' - def exec(self, code_export_directory): + 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 @@ -96,17 +96,32 @@ class CMakeBuilder: os.chdir(self._build_dir) cmd_str = self.get_cmd1_cmake() print(f'call("{cmd_str})"') - retcode = call(cmd_str, shell=True) + 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) + 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) + 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: diff --git a/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt b/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt index 3d6483b5d2..99bc26f750 100644 --- a/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt +++ b/third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt @@ -1,8 +1,5 @@ # -# 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 +# Copyright (c) The acados authors. # # This file is part of acados. # @@ -121,12 +118,14 @@ {%- 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.10) +cmake_minimum_required(VERSION 3.13) project({{ model.name }}) @@ -139,6 +138,14 @@ option(BUILD_SIM_EXAMPLE "Should the simulation example main_sim_{{ model.name } 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 }}) @@ -146,9 +153,11 @@ 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 %} @@ -176,16 +185,15 @@ set(MODEL_SRC {%- endif %} {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c {%- elif solver_options.integrator_type == "DISCRETE" %} - {%- if model.dyn_ext_fun_type == "casadi" %} {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c {%- if hessian_approx == "EXACT" %} {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c {%- endif %} +{%- endif -%} {%- else %} - {{ model.name }}_model/{{ model.dyn_source_discrete }} + {{ model.name }}_model/{{ model.dyn_generic_source }} {%- endif %} -{%- endif -%} ) add_library(${MODEL_OBJ} OBJECT ${MODEL_SRC} ) @@ -219,6 +227,9 @@ if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMP {{ 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 @@ -232,6 +243,9 @@ if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMP {{ 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 @@ -245,6 +259,9 @@ if(${BUILD_ACADOS_SOLVER_LIB} OR ${BUILD_ACADOS_OCP_SOLVER_LIB} OR ${BUILD_EXAMP {{ 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 @@ -275,7 +292,7 @@ set(EX_SRC main_{{ model.name }}.c) set(EX_EXE main_{{ model.name }}) {%- if model_external_shared_lib_dir and model_external_shared_lib_name %} -set(EXTERNAL_DIR {{ model_external_shared_lib_dir }}) +set(EXTERNAL_DIR {{ model_external_shared_lib_dir | replace(from="\", to="/") }}) set(EXTERNAL_LIB {{ model_external_shared_lib_name }}) {%- else %} set(EXTERNAL_DIR) @@ -283,23 +300,26 @@ set(EXTERNAL_LIB) {%- endif %} # set some search paths for preprocessor and linker -set(ACADOS_INCLUDE_PATH {{ acados_include_path }} CACHE PATH "Define the path which contains the include directory for acados.") -set(ACADOS_LIB_PATH {{ acados_lib_path }} CACHE PATH "Define the path which contains the lib directory for acados.") +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 " +set(CMAKE_C_FLAGS "-fPIC -std=c99 {{ openmp_flag }} {%- if qp_solver == "FULL_CONDENSING_QPOASES" -%} - -DACADOS_WITH_QPOASES + -DACADOS_WITH_QPOASES +{%- endif -%} +{%- if qp_solver == "FULL_CONDENSING_DAQP" -%} + -DACADOS_WITH_DAQP {%- endif -%} {%- if qp_solver == "PARTIAL_CONDENSING_OSQP" -%} - -DACADOS_WITH_OSQP + -DACADOS_WITH_OSQP {%- endif -%} {%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" -%} - -DACADOS_WITH_QPDUNES + -DACADOS_WITH_QPDUNES {%- endif -%} - -fPIC -std=c99 {{ openmp_flag }}") +") #-fno-diagnostics-show-line-numbers -g include_directories( @@ -310,6 +330,9 @@ include_directories( {%- 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 diff --git a/third_party/acados/acados_template/c_templates_tera/CPPLINT.cfg b/third_party/acados/acados_template/c_templates_tera/CPPLINT.cfg deleted file mode 100644 index bbd1caf057..0000000000 --- a/third_party/acados/acados_template/c_templates_tera/CPPLINT.cfg +++ /dev/null @@ -1 +0,0 @@ -exclude_files=[main, acados_solver, acados_solver_sfun, Makefile, model].*\.? diff --git a/third_party/acados/acados_template/c_templates_tera/Makefile.in b/third_party/acados/acados_template/c_templates_tera/Makefile.in index d45be0a9c7..fbefc08e38 100644 --- a/third_party/acados/acados_template/c_templates_tera/Makefile.in +++ b/third_party/acados/acados_template/c_templates_tera/Makefile.in @@ -1,8 +1,5 @@ # -# 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 +# Copyright (c) The acados authors. # # This file is part of acados. # @@ -120,6 +117,8 @@ {%- 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 %} @@ -129,9 +128,11 @@ # model MODEL_SRC= -{%- if solver_options.integrator_type == "ERK" %} + {%- 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 %} @@ -139,9 +140,9 @@ MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c 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" %} + {%- if hessian_approx == "EXACT" %} MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c - {%- endif %} + {%- 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 @@ -159,16 +160,15 @@ MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c {%- endif %} MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c {%- elif solver_options.integrator_type == "DISCRETE" %} - {%- if model.dyn_ext_fun_type == "casadi" %} MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c {%- if hessian_approx == "EXACT" %} MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c {%- endif %} +{%- endif %} {%- else %} -MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_source_discrete }} +MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_generic_source }} {%- endif %} -{%- endif %} MODEL_OBJ := $(MODEL_SRC:.c=.o) # optimal control problem - mostly CasADi exports @@ -200,6 +200,9 @@ OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_z 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 @@ -213,6 +216,9 @@ OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }} 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 @@ -226,6 +232,9 @@ OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }} 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 @@ -235,6 +244,12 @@ OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c 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) @@ -275,6 +290,9 @@ LIB_PATH = {{ acados_lib_path }} {%- 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 %} @@ -288,10 +306,13 @@ 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 }} #-fno-diagnostics-show-line-numbers -g +CFLAGS = -fPIC -std=c99 {{ openmp_flag }} {{ solver_options.ext_fun_compile_flags }}#-fno-diagnostics-show-line-numbers -g # # Debugging # CFLAGS += -g3 @@ -306,9 +327,9 @@ LDLIBS+= -lm LDLIBS+= {{ link_libs }} # libraries -LIBACADOS_SOLVER=libacados_solver_{{ model.name }}.so -LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}.so -LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so) +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 @@ -354,38 +375,73 @@ ocp_cython_o: ocp_cython_c $(CC) $(ACADOS_FLAGS) -c -O2 \ -fPIC \ -o acados_ocp_solver_pyx.o \ - -I /usr/include/python3.8 \ -I $(INCLUDE_PATH)/blasfeo/include/ \ -I $(INCLUDE_PATH)/hpipm/include/ \ -I $(INCLUDE_PATH) \ - -I {{ cython_include_dirs }} \ + {%- 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.so \ + -o acados_ocp_solver_pyx{{ shared_lib_ext }} \ -Wl,-rpath=$(LIB_PATH) \ acados_ocp_solver_pyx.o \ - $(abspath .)/libacados_ocp_solver_{{ model.name }}.so \ + $(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 *.so 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 }}.so 2>nul + 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 }}.so 2>nul + 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.so 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: @@ -398,9 +454,15 @@ clean_ocp_shared_lib: $(RM) $(OCP_OBJ) clean_ocp_cython: - $(RM) libacados_ocp_solver_{{ model.name }}.so + $(RM) libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }} $(RM) acados_solver_{{ model.name }}.o - $(RM) acados_ocp_solver_pyx.so + $(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 %} diff --git a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c index f2e75058c1..0cd098273c 100644 --- a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c +++ b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.c @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -88,18 +85,19 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) double Tsim = {{ solver_options.Tsim }}; {% if solver_options.integrator_type == "IRK" %} - capsule->sim_impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + 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_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_casadi_create(capsule->sim_impl_dae_fun, np); + 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; @@ -107,33 +105,39 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) 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_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np); + external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np); - // external_function_param_casadi impl_dae_jac_x_xdot_u_z; + // 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_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np); + 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_casadi *) malloc(sizeof(external_function_param_casadi)); - // external_function_param_casadi impl_dae_jac_x_xdot_u_z; + 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_casadi_create(capsule->sim_impl_dae_hess, np); + 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_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + 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; @@ -141,7 +145,15 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) 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_casadi_create(capsule->sim_forw_vde_casadi, np); + 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; @@ -149,30 +161,30 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) 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_casadi_create(capsule->sim_expl_ode_fun_casadi, np); + 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_casadi *) malloc(sizeof(external_function_param_casadi)); - // external_function_param_casadi impl_dae_jac_x_xdot_u_z; + 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_casadi_create(capsule->sim_expl_ode_hess, np); + 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_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); - capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + 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_casadi *) malloc(sizeof(external_function_param_casadi)); + 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_casadi *) malloc(sizeof(external_function_param_casadi)); + 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; @@ -181,7 +193,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) 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_casadi_create(capsule->sim_gnsf_phi_fun, np); + 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; @@ -189,7 +201,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) 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_casadi_create(capsule->sim_gnsf_phi_fun_jac_y, np); + 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; @@ -197,7 +209,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) 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_casadi_create(capsule->sim_gnsf_phi_jac_y_uhat, np); + 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; @@ -206,7 +218,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) 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_casadi_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np); + external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np); {%- endif %} {%- endif %} @@ -216,7 +228,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) 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_casadi_create(capsule->sim_gnsf_get_matrices_fun, np); + external_function_param_{{ model.dyn_ext_fun_type }}_create(capsule->sim_gnsf_get_matrices_fun, np); {% endif %} // sim plan & config @@ -252,6 +264,8 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) 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); @@ -307,7 +321,9 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) {%- elif solver_options.integrator_type == "ERK" %} {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model, - "expl_vde_for", capsule->sim_forw_vde_casadi); + "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" %} @@ -408,28 +424,29 @@ int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule) // free external function {%- if solver_options.integrator_type == "IRK" %} - external_function_param_casadi_free(capsule->sim_impl_dae_fun); - external_function_param_casadi_free(capsule->sim_impl_dae_fun_jac_x_xdot_z); - external_function_param_casadi_free(capsule->sim_impl_dae_jac_x_xdot_u_z); + 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_casadi_free(capsule->sim_impl_dae_hess); + external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_hess); {%- endif %} {%- elif solver_options.integrator_type == "ERK" %} - external_function_param_casadi_free(capsule->sim_forw_vde_casadi); - external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi); + 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_casadi_free(capsule->sim_expl_ode_hess); + 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_casadi_free(capsule->sim_gnsf_phi_fun); - external_function_param_casadi_free(capsule->sim_gnsf_phi_fun_jac_y); - external_function_param_casadi_free(capsule->sim_gnsf_phi_jac_y_uhat); + 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_casadi_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z); + external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z); {%- endif %} {%- endif %} - external_function_param_casadi_free(capsule->sim_gnsf_get_matrices_fun); + external_function_param_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_get_matrices_fun); {% endif %} return 0; @@ -449,6 +466,7 @@ int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, doubl {%- 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); diff --git a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h index 7306491baf..59aee62f49 100644 --- a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h +++ b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -60,22 +57,23 @@ typedef struct sim_solver_capsule /* external functions */ // ERK - external_function_param_casadi * sim_forw_vde_casadi; - external_function_param_casadi * sim_expl_ode_fun_casadi; - external_function_param_casadi * sim_expl_ode_hess; + 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_casadi * sim_impl_dae_fun; - external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z; - external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z; - external_function_param_casadi * sim_impl_dae_hess; + 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_casadi * sim_gnsf_phi_fun; - external_function_param_casadi * sim_gnsf_phi_fun_jac_y; - external_function_param_casadi * sim_gnsf_phi_jac_y_uhat; - external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z; - external_function_param_casadi * sim_gnsf_get_matrices_fun; + 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; diff --git a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.pxd b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.pxd new file mode 100644 index 0000000000..153f98d13a --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.pxd @@ -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) diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.c b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.c index 0af8127709..5e36a53d10 100644 --- a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.c +++ b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.c @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -42,34 +39,27 @@ // example specific #include "{{ model.name }}_model/{{ model.name }}_model.h" -{% if constraints.constr_type == "BGP" and dims.nphi %} -#include "{{ model.name }}_constraints/{{ model.name }}_phi_constraint.h" -{% endif %} -{% if constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %} -#include "{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.h" -{% endif %} -{% if constraints.constr_type == "BGH" and dims.nh > 0 %} -#include "{{ model.name }}_constraints/{{ model.name }}_h_constraint.h" -{% endif %} -{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} -#include "{{ model.name }}_constraints/{{ model.name }}_h_e_constraint.h" -{% endif %} -{%- if cost.cost_type == "NONLINEAR_LS" %} -#include "{{ model.name }}_cost/{{ model.name }}_cost_y_fun.h" -{%- elif cost.cost_type == "EXTERNAL" %} -#include "{{ model.name }}_cost/{{ model.name }}_external_cost.h" +#include "{{ model.name }}_constraints/{{ model.name }}_constraints.h" + +{%- if cost.cost_type != "LINEAR_LS" or cost.cost_type_e != "LINEAR_LS" or cost.cost_type_0 != "LINEAR_LS" %} +#include "{{ model.name }}_cost/{{ model.name }}_cost.h" {%- endif %} -{%- if cost.cost_type_0 == "NONLINEAR_LS" %} -#include "{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.h" -{%- elif cost.cost_type_0 == "EXTERNAL" %} -#include "{{ model.name }}_cost/{{ model.name }}_external_cost_0.h" + +{%- if not solver_options.custom_update_filename %} + {%- set custom_update_filename = "" %} +{% else %} + {%- set custom_update_filename = solver_options.custom_update_filename %} {%- endif %} -{%- if cost.cost_type_e == "NONLINEAR_LS" %} -#include "{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.h" -{%- elif cost.cost_type_e == "EXTERNAL" %} -#include "{{ model.name }}_cost/{{ model.name }}_external_cost_e.h" +{%- if not solver_options.custom_update_header_filename %} + {%- set custom_update_header_filename = "" %} +{% else %} + {%- set custom_update_header_filename = solver_options.custom_update_header_filename %} +{%- endif %} +{%- if custom_update_header_filename != "" %} +#include "{{ custom_update_header_filename }}" {%- endif %} + #include "acados_solver_{{ model.name }}.h" #define NX {{ model.name | upper }}_NX @@ -205,7 +195,6 @@ void {{ model.name }}_acados_create_1_set_plan(ocp_nlp_plan_t* nlp_solver_plan, nlp_solver_plan->nlp_constraints[N] = BGH; {%- endif %} -{%- if solver_options.hessian_approx == "EXACT" %} {%- if solver_options.regularize_method == "NO_REGULARIZE" %} nlp_solver_plan->regularization = NO_REGULARIZE; {%- elif solver_options.regularize_method == "MIRROR" %} @@ -217,7 +206,6 @@ void {{ model.name }}_acados_create_1_set_plan(ocp_nlp_plan_t* nlp_solver_plan, {%- elif solver_options.regularize_method == "CONVEXIFY" %} nlp_solver_plan->regularization = CONVEXIFY; {%- endif %} -{%- endif %} } @@ -324,11 +312,11 @@ ocp_nlp_dims* {{ model.name }}_acados_create_2_create_and_set_dimensions({{ mode ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]); } -{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %} +{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" or cost.cost_type_0 == "CONVEX_OVER_NONLINEAR"%} ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]); {%- endif %} -{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %} +{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" or cost.cost_type == "CONVEX_OVER_NONLINEAR"%} for (int i = 1; i < N; i++) ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]); {%- endif %} @@ -353,7 +341,7 @@ ocp_nlp_dims* {{ model.name }}_acados_create_2_create_and_set_dimensions({{ mode ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nphi", &nphi[N]); ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsphi", &nsphi[N]); {%- endif %} -{%- if cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" %} +{%- if cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "CONVEX_OVER_NONLINEAR"%} ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]); {%- endif %} free(intNp1mem); @@ -388,7 +376,6 @@ return nlp_dims; void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_solver_capsule* capsule) { const int N = capsule->nlp_solver_plan->N; - ocp_nlp_config* nlp_config = capsule->nlp_config; /************************************************ * external functions @@ -468,35 +455,50 @@ void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_ {% elif solver_options.integrator_type == "IRK" %} // implicit dae - capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + capsule->impl_dae_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); for (int i = 0; i < N; i++) { + {%- if model.dyn_ext_fun_type == "casadi" %} MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun); + {%- else %} + capsule->impl_dae_fun[i].fun = &{{ model.dyn_impl_dae_fun }}; + external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_fun[i], {{ dims.np }}); + {%- endif %} } - capsule->impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + capsule->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 }})*N); for (int i = 0; i < N; i++) { + {%- if model.dyn_ext_fun_type == "casadi" %} MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_z[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_z); + {%- else %} + capsule->impl_dae_fun_jac_x_xdot_z[i].fun = &{{ model.dyn_impl_dae_fun_jac }}; + external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_fun_jac_x_xdot_z[i], {{ dims.np }}); + {%- endif %} } - capsule->impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + capsule->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 }})*N); for (int i = 0; i < N; i++) { + {%- if model.dyn_ext_fun_type == "casadi" %} MAP_CASADI_FNC(impl_dae_jac_x_xdot_u_z[i], {{ model.name }}_impl_dae_jac_x_xdot_u_z); + {%- else %} + capsule->impl_dae_jac_x_xdot_u_z[i].fun = &{{ model.dyn_impl_dae_jac }}; + external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->impl_dae_jac_x_xdot_u_z[i], {{ dims.np }}); + {%- endif %} } {%- if solver_options.hessian_approx == "EXACT" %} - capsule->impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + capsule->impl_dae_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(impl_dae_hess[i], {{ model.name }}_impl_dae_hess); } {%- endif %} {% elif solver_options.integrator_type == "LIFTED_IRK" %} // external functions (implicit model) - capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + capsule->impl_dae_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun); } - capsule->impl_dae_fun_jac_x_xdot_u = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + capsule->impl_dae_fun_jac_x_xdot_u = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N); for (int i = 0; i < N; i++) { MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_u[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_u); } @@ -574,6 +576,11 @@ void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_ MAP_CASADI_FNC(cost_y_0_fun_jac_ut_xt, {{ model.name }}_cost_y_0_fun_jac_ut_xt); MAP_CASADI_FNC(cost_y_0_hess, {{ model.name }}_cost_y_0_hess); +{%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} + // convex-over-nonlinear cost + MAP_CASADI_FNC(conl_cost_0_fun, {{ model.name }}_conl_cost_0_fun); + MAP_CASADI_FNC(conl_cost_0_fun_jac_hess, {{ model.name }}_conl_cost_0_fun_jac_hess); + {%- elif cost.cost_type_0 == "EXTERNAL" %} // external cost {%- if cost.cost_ext_fun_type_0 == "casadi" %} @@ -619,6 +626,20 @@ void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_ { MAP_CASADI_FNC(cost_y_hess[i], {{ model.name }}_cost_y_hess); } + +{%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} + // convex-over-nonlinear cost + capsule->conl_cost_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N-1; i++) + { + MAP_CASADI_FNC(conl_cost_fun[i], {{ model.name }}_conl_cost_fun); + } + capsule->conl_cost_fun_jac_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N-1; i++) + { + MAP_CASADI_FNC(conl_cost_fun_jac_hess[i], {{ model.name }}_conl_cost_fun_jac_hess); + } + {%- elif cost.cost_type == "EXTERNAL" %} // external cost capsule->ext_cost_fun = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N); @@ -660,6 +681,12 @@ void {{ model.name }}_acados_create_3_create_and_set_functions({{ model.name }}_ MAP_CASADI_FNC(cost_y_e_fun, {{ model.name }}_cost_y_e_fun); MAP_CASADI_FNC(cost_y_e_fun_jac_ut_xt, {{ model.name }}_cost_y_e_fun_jac_ut_xt); MAP_CASADI_FNC(cost_y_e_hess, {{ model.name }}_cost_y_e_hess); + +{%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} + // convex-over-nonlinear cost + MAP_CASADI_FNC(conl_cost_e_fun, {{ model.name }}_conl_cost_e_fun); + MAP_CASADI_FNC(conl_cost_e_fun_jac_hess, {{ model.name }}_conl_cost_e_fun_jac_hess); + {%- elif cost.cost_type_e == "EXTERNAL" %} // external cost - function {%- if cost.cost_ext_fun_type_e == "casadi" %} @@ -808,20 +835,9 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule } /**** Cost ****/ -{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %} - {%- if dims.ny_0 > 0 %} - double* W_0 = calloc(NY0*NY0, sizeof(double)); - // change only the non-zero elements: - {%- for j in range(end=dims.ny_0) %} - {%- for k in range(end=dims.ny_0) %} - {%- if cost.W_0[j][k] != 0 %} - W_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.W_0[j][k] }}; - {%- endif %} - {%- endfor %} - {%- endfor %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0); - free(W_0); +{%- if dims.ny_0 == 0 %} +{%- elif cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" or cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} double* yref_0 = calloc(NY0, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny_0) %} @@ -831,40 +847,91 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0); free(yref_0); - {%- endif %} {%- endif %} -{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %} - {%- if dims.ny > 0 %} - double* W = calloc(NY*NY, sizeof(double)); + +{%- if dims.ny == 0 %} +{%- elif cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" or cost.cost_type == "CONVEX_OVER_NONLINEAR" %} + double* yref = calloc(NY, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny) %} - {%- for k in range(end=dims.ny) %} - {%- if cost.W[j][k] != 0 %} - W[{{ j }}+(NY) * {{ k }}] = {{ cost.W[j][k] }}; + {%- if cost.yref[j] != 0 %} + yref[{{ j }}] = {{ cost.yref[j] }}; + {%- endif %} + {%- endfor %} + + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref); + } + free(yref); +{%- endif %} + + +{%- if dims.ny_e == 0 %} +{%- elif cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} + double* yref_e = calloc(NYN, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_e) %} + {%- if cost.yref_e[j] != 0 %} + yref_e[{{ j }}] = {{ cost.yref_e[j] }}; + {%- endif %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e); + free(yref_e); +{%- endif %} + +{%- if dims.ny_0 == 0 %} +{%- elif cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %} + double* W_0 = calloc(NY0*NY0, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_0) %} + {%- for k in range(end=dims.ny_0) %} + {%- if cost.W_0[j][k] != 0 %} + W_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.W_0[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0); + free(W_0); +{%- endif %} - double* yref = calloc(NY, sizeof(double)); +{%- if dims.ny == 0 %} +{%- elif cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %} + double* W = calloc(NY*NY, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny) %} - {%- if cost.yref[j] != 0 %} - yref[{{ j }}] = {{ cost.yref[j] }}; - {%- endif %} + {%- for k in range(end=dims.ny) %} + {%- if cost.W[j][k] != 0 %} + W[{{ j }}+(NY) * {{ k }}] = {{ cost.W[j][k] }}; + {%- endif %} + {%- endfor %} {%- endfor %} for (int i = 1; i < N; i++) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref); } free(W); - free(yref); - {%- endif %} {%- endif %} -{%- if cost.cost_type_0 == "LINEAR_LS" %} +{%- if dims.ny_e == 0 %} +{%- elif cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" %} + double* W_e = calloc(NYN*NYN, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_e) %} + {%- for k in range(end=dims.ny_e) %} + {%- if cost.W_e[j][k] != 0 %} + W_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.W_e[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); + free(W_e); +{%- endif %} + +{%- if dims.ny_0 == 0 %} +{%- elif cost.cost_type_0 == "LINEAR_LS" %} double* Vx_0 = calloc(NY0*NX, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny_0) %} @@ -877,7 +944,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0); free(Vx_0); - {%- if dims.ny_0 > 0 and dims.nu > 0 %} + {%- if dims.nu > 0 %} double* Vu_0 = calloc(NY0*NU, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny_0) %} @@ -891,7 +958,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule free(Vu_0); {%- endif %} - {%- if dims.ny_0 > 0 and dims.nz > 0 %} + {%- if dims.nz > 0 %} double* Vz_0 = calloc(NY0*NZ, sizeof(double)); // change only the non-zero elements: {% for j in range(end=dims.ny_0) %} @@ -904,10 +971,10 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vz", Vz_0); free(Vz_0); {%- endif %} -{%- endif %}{# LINEAR LS #} - +{%- endif %} -{%- if cost.cost_type == "LINEAR_LS" %} +{%- if dims.ny == 0 %} +{%- elif cost.cost_type == "LINEAR_LS" %} double* Vx = calloc(NY*NX, sizeof(double)); // change only the non-zero elements: {%- for j in range(end=dims.ny) %} @@ -923,7 +990,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule } free(Vx); - {% if dims.ny > 0 and dims.nu > 0 %} + {% if dims.nu > 0 %} double* Vu = calloc(NY*NU, sizeof(double)); // change only the non-zero elements: {% for j in range(end=dims.ny) %} @@ -941,7 +1008,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule free(Vu); {%- endif %} - {%- if dims.ny > 0 and dims.nz > 0 %} + {%- if dims.nz > 0 %} double* Vz = calloc(NY*NZ, sizeof(double)); // change only the non-zero elements: {% for j in range(end=dims.ny) %} @@ -960,11 +1027,28 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule {%- endif %} {%- endif %}{# LINEAR LS #} +{%- if dims.ny_e == 0 %} +{%- elif cost.cost_type_e == "LINEAR_LS" %} + double* Vx_e = calloc(NYN*NX, sizeof(double)); + // change only the non-zero elements: + {% for j in range(end=dims.ny_e) %} + {%- for k in range(end=dims.nx) %} + {%- if cost.Vx_e[j][k] != 0 %} + Vx_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.Vx_e[j][k] }}; + {%- endif %} + {%- endfor %} + {%- endfor %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); + free(Vx_e); +{%- endif %} {%- if cost.cost_type_0 == "NONLINEAR_LS" %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun", &capsule->cost_y_0_fun); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun_jac", &capsule->cost_y_0_fun_jac_ut_xt); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_hess", &capsule->cost_y_0_hess); +{%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "conl_cost_fun", &capsule->conl_cost_0_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "conl_cost_fun_jac_hess", &capsule->conl_cost_0_fun_jac_hess); {%- elif cost.cost_type_0 == "EXTERNAL" %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun", &capsule->ext_cost_0_fun); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun_jac", &capsule->ext_cost_0_fun_jac); @@ -978,6 +1062,12 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun_jac", &capsule->cost_y_fun_jac_ut_xt[i-1]); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_hess", &capsule->cost_y_hess[i-1]); } +{%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "conl_cost_fun", &capsule->conl_cost_fun[i-1]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "conl_cost_fun_jac_hess", &capsule->conl_cost_fun_jac_hess[i-1]); + } {%- elif cost.cost_type == "EXTERNAL" %} for (int i = 1; i < N; i++) { @@ -987,7 +1077,25 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule } {%- endif %} +{%- if cost.cost_type_e == "NONLINEAR_LS" %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess); + +{%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} + + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "conl_cost_fun", &capsule->conl_cost_e_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "conl_cost_fun_jac_hess", &capsule->conl_cost_e_fun_jac_hess); + +{%- elif cost.cost_type_e == "EXTERNAL" %} + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun", &capsule->ext_cost_e_fun); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac", &capsule->ext_cost_e_fun_jac); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac_hess", &capsule->ext_cost_e_fun_jac_hess); +{%- endif %} + + {%- if dims.ns > 0 %} + // slacks double* zlumem = calloc(4*NS, sizeof(double)); double* Zl = zlumem+NS*0; double* Zu = zlumem+NS*1; @@ -1028,59 +1136,8 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule free(zlumem); {%- endif %} - // terminal cost -{%- if cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "NONLINEAR_LS" %} - {%- if dims.ny_e > 0 %} - double* yref_e = calloc(NYN, sizeof(double)); - // change only the non-zero elements: - {%- for j in range(end=dims.ny_e) %} - {%- if cost.yref_e[j] != 0 %} - yref_e[{{ j }}] = {{ cost.yref_e[j] }}; - {%- endif %} - {%- endfor %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e); - free(yref_e); - - double* W_e = calloc(NYN*NYN, sizeof(double)); - // change only the non-zero elements: - {%- for j in range(end=dims.ny_e) %} - {%- for k in range(end=dims.ny_e) %} - {%- if cost.W_e[j][k] != 0 %} - W_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.W_e[j][k] }}; - {%- endif %} - {%- endfor %} - {%- endfor %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); - free(W_e); - - {%- if cost.cost_type_e == "LINEAR_LS" %} - double* Vx_e = calloc(NYN*NX, sizeof(double)); - // change only the non-zero elements: - {% for j in range(end=dims.ny_e) %} - {%- for k in range(end=dims.nx) %} - {%- if cost.Vx_e[j][k] != 0 %} - Vx_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.Vx_e[j][k] }}; - {%- endif %} - {%- endfor %} - {%- endfor %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); - free(Vx_e); - {%- endif %} - - {%- if cost.cost_type_e == "NONLINEAR_LS" %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess); - {%- endif %} - {%- endif %}{# ny_e > 0 #} - -{%- elif cost.cost_type_e == "EXTERNAL" %} - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun", &capsule->ext_cost_e_fun); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac", &capsule->ext_cost_e_fun_jac); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac_hess", &capsule->ext_cost_e_fun_jac_hess); -{%- endif %} - {% if dims.ns_e > 0 %} + // slacks terminal double* zluemem = calloc(4*NSN, sizeof(double)); double* Zl_e = zluemem+NSN*0; double* Zu_e = zluemem+NSN*1; @@ -1185,7 +1242,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule {%- endfor %} for (int i = 1; i < N; i++) - { + { ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbx", idxsbx); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbx", lsbx); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbx", usbx); @@ -1363,7 +1420,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule {%- endif %} {% if dims.ng > 0 %} - // set up general constraints for stage 0 to N-1 + // set up general constraints for stage 0 to N-1 double* D = calloc(NG*NU, sizeof(double)); double* C = calloc(NG*NX, sizeof(double)); double* lug = calloc(2*NG, sizeof(double)); @@ -1427,7 +1484,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule uh[{{ i }}] = {{ constraints.uh[i] }}; {%- endif %} {%- endfor %} - + for (int i = 0; i < N; i++) { // nonlinear constraints for stages 0 to N-1 @@ -1599,16 +1656,16 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule {% endif %} {% if dims.ng_e > 0 %} - // set up general constraints for last stage + // set up general constraints for last stage double* C_e = calloc(NGN*NX, sizeof(double)); double* lug_e = calloc(2*NGN, sizeof(double)); double* lg_e = lug_e; double* ug_e = lug_e + NGN; - {% for j in range(end=dims.ng) %} + {% for j in range(end=dims.ng_e) %} {%- for k in range(end=dims.nx) %} {%- if constraints.C_e[j][k] != 0 %} - C_e[{{ j }}+NG * {{ k }}] = {{ constraints.C_e[j][k] }}; + C_e[{{ j }}+NGN * {{ k }}] = {{ constraints.C_e[j][k] }}; {%- endif %} {%- endfor %} {%- endfor %} @@ -1658,7 +1715,7 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule {%- endif %} {% if dims.nphi_e > 0 and constraints.constr_type_e == "BGP" %} - // set up convex-over-nonlinear constraints for last stage + // set up convex-over-nonlinear constraints for last stage double* luphi_e = calloc(2*NPHIN, sizeof(double)); double* lphi_e = luphi_e; double* uphi_e = luphi_e + NPHIN; @@ -1687,7 +1744,6 @@ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule* { const int N = capsule->nlp_solver_plan->N; ocp_nlp_config* nlp_config = capsule->nlp_config; - ocp_nlp_dims* nlp_dims = capsule->nlp_dims; void *nlp_opts = capsule->nlp_opts; /************************************************ @@ -1695,12 +1751,9 @@ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule* ************************************************/ {% if solver_options.hessian_approx == "EXACT" %} - bool nlp_solver_exact_hessian = true; - // TODO: this if should not be needed! however, calling the setter with false leads to weird behavior. Investigate! - if (nlp_solver_exact_hessian) - { - ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess", &nlp_solver_exact_hessian); - } + int nlp_solver_exact_hessian = 1; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess", &nlp_solver_exact_hessian); + int exact_hess_dyn = {{ solver_options.exact_hess_dyn }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_dyn", &exact_hess_dyn); @@ -1861,6 +1914,8 @@ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule* ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N); {%- endif %} + int nlp_solver_ext_qp_res = {{ solver_options.nlp_solver_ext_qp_res }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res); {%- if solver_options.qp_solver is containing("HPIPM") %} // set HPIPM mode: should be done before setting other QP solver options @@ -1920,6 +1975,15 @@ void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule* int print_level = {{ solver_options.print_level }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level); +{%- if solver_options.qp_solver is containing('PARTIAL_CONDENSING') %} + int qp_solver_cond_ric_alg = {{ solver_options.qp_solver_cond_ric_alg }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_ric_alg", &qp_solver_cond_ric_alg); +{% endif %} + +{%- if solver_options.qp_solver == 'PARTIAL_CONDENSING_HPIPM' %} + int qp_solver_ric_alg = {{ solver_options.qp_solver_ric_alg }}; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_ric_alg", &qp_solver_ric_alg); +{% endif %} int ext_cost_num_hess = {{ solver_options.ext_cost_num_hess }}; {%- if cost.cost_type == "EXTERNAL" %} @@ -2042,6 +2106,12 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c // 9) do precomputations int status = {{ model.name }}_acados_create_9_precompute(capsule); + + {%- if custom_update_filename != "" %} + // Initialize custom update function + custom_update_init_function(capsule); + {%- endif %} + return status; } @@ -2076,7 +2146,7 @@ int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_caps } -int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule) +int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule, int reset_qp_solver_mem) { // set initialization to all zeros @@ -2088,8 +2158,6 @@ int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule) ocp_nlp_in* nlp_in = capsule->nlp_in; ocp_nlp_solver* nlp_solver = capsule->nlp_solver; - int nx, nu, nv, ns, nz, ni, dim; - double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NPHIN+NGN, sizeof(double)); for(int i=0; i reset memory int qp_status; ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "qp_status", &qp_status); - if (qp_status == 3) + if (reset_qp_solver_mem || (qp_status == 3)) { // printf("\nin reset qp_status %d -> resetting QP memory\n", qp_status); ocp_nlp_solver_reset_qp_memory(nlp_solver, nlp_in, nlp_out); @@ -2144,7 +2212,6 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu exit(1); } -{%- if dims.np > 0 %} const int N = capsule->nlp_solver_plan->N; if (stage < N && stage >= 0) { @@ -2201,6 +2268,9 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu capsule->cost_y_0_fun.set_param(&capsule->cost_y_0_fun, p); capsule->cost_y_0_fun_jac_ut_xt.set_param(&capsule->cost_y_0_fun_jac_ut_xt, p); capsule->cost_y_0_hess.set_param(&capsule->cost_y_0_hess, p); + {%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} + capsule->conl_cost_0_fun.set_param(&capsule->conl_cost_0_fun, p); + capsule->conl_cost_0_fun_jac_hess.set_param(&capsule->conl_cost_0_fun_jac_hess, p); {%- elif cost.cost_type_0 == "EXTERNAL" %} capsule->ext_cost_0_fun.set_param(&capsule->ext_cost_0_fun, p); capsule->ext_cost_0_fun_jac.set_param(&capsule->ext_cost_0_fun_jac, p); @@ -2213,6 +2283,9 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu capsule->cost_y_fun[stage-1].set_param(capsule->cost_y_fun+stage-1, p); capsule->cost_y_fun_jac_ut_xt[stage-1].set_param(capsule->cost_y_fun_jac_ut_xt+stage-1, p); capsule->cost_y_hess[stage-1].set_param(capsule->cost_y_hess+stage-1, p); + {%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} + capsule->conl_cost_fun[stage-1].set_param(capsule->conl_cost_fun+stage-1, p); + capsule->conl_cost_fun_jac_hess[stage-1].set_param(capsule->conl_cost_fun_jac_hess+stage-1, p); {%- elif cost.cost_type == "EXTERNAL" %} capsule->ext_cost_fun[stage-1].set_param(capsule->ext_cost_fun+stage-1, p); capsule->ext_cost_fun_jac[stage-1].set_param(capsule->ext_cost_fun_jac+stage-1, p); @@ -2229,6 +2302,9 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu capsule->cost_y_e_fun.set_param(&capsule->cost_y_e_fun, p); capsule->cost_y_e_fun_jac_ut_xt.set_param(&capsule->cost_y_e_fun_jac_ut_xt, p); capsule->cost_y_e_hess.set_param(&capsule->cost_y_e_hess, p); + {%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} + capsule->conl_cost_e_fun.set_param(&capsule->conl_cost_e_fun, p); + capsule->conl_cost_e_fun_jac_hess.set_param(&capsule->conl_cost_e_fun_jac_hess, p); {%- elif cost.cost_type_e == "EXTERNAL" %} capsule->ext_cost_e_fun.set_param(&capsule->ext_cost_e_fun, p); capsule->ext_cost_e_fun_jac.set_param(&capsule->ext_cost_e_fun_jac, p); @@ -2245,16 +2321,149 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsu {%- endif %} {% endif %} } -{% endif %}{# if dims.np #} return solver_status; } +int {{ model.name }}_acados_update_params_sparse({{ model.name }}_solver_capsule * capsule, int stage, int *idx, double *p, int n_update) +{ + int solver_status = 0; + + int casadi_np = {{ dims.np }}; + if (casadi_np < n_update) { + printf("{{ model.name }}_acados_update_params_sparse: trying to set %d parameters for external functions." + " External function has %d parameters. Exiting.\n", n_update, casadi_np); + exit(1); + } + // for (int i = 0; i < n_update; i++) + // { + // if (idx[i] > casadi_np) { + // printf("{{ model.name }}_acados_update_params_sparse: attempt to set parameters with index %d, while" + // " external functions only has %d parameters. Exiting.\n", idx[i], casadi_np); + // exit(1); + // } + // printf("param %d value %e\n", idx[i], p[i]); + // } + +{%- if dims.np > 0 %} + const int N = capsule->nlp_solver_plan->N; + if (stage < N && stage >= 0) + { + {%- if solver_options.integrator_type == "IRK" %} + capsule->impl_dae_fun[stage].set_param_sparse(capsule->impl_dae_fun+stage, n_update, idx, p); + capsule->impl_dae_fun_jac_x_xdot_z[stage].set_param_sparse(capsule->impl_dae_fun_jac_x_xdot_z+stage, n_update, idx, p); + capsule->impl_dae_jac_x_xdot_u_z[stage].set_param_sparse(capsule->impl_dae_jac_x_xdot_u_z+stage, n_update, idx, p); + + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->impl_dae_hess[stage].set_param_sparse(capsule->impl_dae_hess+stage, n_update, idx, p); + {%- endif %} + {% elif solver_options.integrator_type == "LIFTED_IRK" %} + capsule->impl_dae_fun[stage].set_param_sparse(capsule->impl_dae_fun+stage, n_update, idx, p); + capsule->impl_dae_fun_jac_x_xdot_u[stage].set_param_sparse(capsule->impl_dae_fun_jac_x_xdot_u+stage, n_update, idx, p); + {% elif solver_options.integrator_type == "ERK" %} + capsule->forw_vde_casadi[stage].set_param_sparse(capsule->forw_vde_casadi+stage, n_update, idx, p); + capsule->expl_ode_fun[stage].set_param_sparse(capsule->expl_ode_fun+stage, n_update, idx, p); + + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->hess_vde_casadi[stage].set_param_sparse(capsule->hess_vde_casadi+stage, n_update, idx, p); + {%- endif %} + {% elif solver_options.integrator_type == "GNSF" %} + {% if model.gnsf.purely_linear != 1 %} + capsule->gnsf_phi_fun[stage].set_param_sparse(capsule->gnsf_phi_fun+stage, n_update, idx, p); + capsule->gnsf_phi_fun_jac_y[stage].set_param_sparse(capsule->gnsf_phi_fun_jac_y+stage, n_update, idx, p); + capsule->gnsf_phi_jac_y_uhat[stage].set_param_sparse(capsule->gnsf_phi_jac_y_uhat+stage, n_update, idx, p); + {% if model.gnsf.nontrivial_f_LO == 1 %} + capsule->gnsf_f_lo_jac_x1_x1dot_u_z[stage].set_param_sparse(capsule->gnsf_f_lo_jac_x1_x1dot_u_z+stage, n_update, idx, p); + {%- endif %} + {%- endif %} + {% elif solver_options.integrator_type == "DISCRETE" %} + capsule->discr_dyn_phi_fun[stage].set_param_sparse(capsule->discr_dyn_phi_fun+stage, n_update, idx, p); + capsule->discr_dyn_phi_fun_jac_ut_xt[stage].set_param_sparse(capsule->discr_dyn_phi_fun_jac_ut_xt+stage, n_update, idx, p); + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->discr_dyn_phi_fun_jac_ut_xt_hess[stage].set_param_sparse(capsule->discr_dyn_phi_fun_jac_ut_xt_hess+stage, n_update, idx, p); + {% endif %} + {%- endif %}{# integrator_type #} + + // constraints + {% if constraints.constr_type == "BGP" %} + capsule->phi_constraint[stage].set_param_sparse(capsule->phi_constraint+stage, n_update, idx, p); + {% elif constraints.constr_type == "BGH" and dims.nh > 0 %} + capsule->nl_constr_h_fun_jac[stage].set_param_sparse(capsule->nl_constr_h_fun_jac+stage, n_update, idx, p); + capsule->nl_constr_h_fun[stage].set_param_sparse(capsule->nl_constr_h_fun+stage, n_update, idx, p); + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->nl_constr_h_fun_jac_hess[stage].set_param_sparse(capsule->nl_constr_h_fun_jac_hess+stage, n_update, idx, p); + {%- endif %} + {%- endif %} + + // cost + if (stage == 0) + { + {%- if cost.cost_type_0 == "NONLINEAR_LS" %} + capsule->cost_y_0_fun.set_param_sparse(&capsule->cost_y_0_fun, n_update, idx, p); + capsule->cost_y_0_fun_jac_ut_xt.set_param_sparse(&capsule->cost_y_0_fun_jac_ut_xt, n_update, idx, p); + capsule->cost_y_0_hess.set_param_sparse(&capsule->cost_y_0_hess, n_update, idx, p); + {%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} + capsule->conl_cost_0_fun.set_param_sparse(&capsule->conl_cost_0_fun, n_update, idx, p); + capsule->conl_cost_0_fun_jac_hess.set_param_sparse(&capsule->conl_cost_0_fun_jac_hess, n_update, idx, p); + {%- elif cost.cost_type_0 == "EXTERNAL" %} + capsule->ext_cost_0_fun.set_param_sparse(&capsule->ext_cost_0_fun, n_update, idx, p); + capsule->ext_cost_0_fun_jac.set_param_sparse(&capsule->ext_cost_0_fun_jac, n_update, idx, p); + capsule->ext_cost_0_fun_jac_hess.set_param_sparse(&capsule->ext_cost_0_fun_jac_hess, n_update, idx, p); + {% endif %} + } + else // 0 < stage < N + { + {%- if cost.cost_type == "NONLINEAR_LS" %} + capsule->cost_y_fun[stage-1].set_param_sparse(capsule->cost_y_fun+stage-1, n_update, idx, p); + capsule->cost_y_fun_jac_ut_xt[stage-1].set_param_sparse(capsule->cost_y_fun_jac_ut_xt+stage-1, n_update, idx, p); + capsule->cost_y_hess[stage-1].set_param_sparse(capsule->cost_y_hess+stage-1, n_update, idx, p); + {%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} + capsule->conl_cost_fun[stage-1].set_param_sparse(capsule->conl_cost_fun+stage-1, n_update, idx, p); + capsule->conl_cost_fun_jac_hess[stage-1].set_param_sparse(capsule->conl_cost_fun_jac_hess+stage-1, n_update, idx, p); + {%- elif cost.cost_type == "EXTERNAL" %} + capsule->ext_cost_fun[stage-1].set_param_sparse(capsule->ext_cost_fun+stage-1, n_update, idx, p); + capsule->ext_cost_fun_jac[stage-1].set_param_sparse(capsule->ext_cost_fun_jac+stage-1, n_update, idx, p); + capsule->ext_cost_fun_jac_hess[stage-1].set_param_sparse(capsule->ext_cost_fun_jac_hess+stage-1, n_update, idx, p); + {%- endif %} + } + } + + else // stage == N + { + // terminal shooting node has no dynamics + // cost + {%- if cost.cost_type_e == "NONLINEAR_LS" %} + capsule->cost_y_e_fun.set_param_sparse(&capsule->cost_y_e_fun, n_update, idx, p); + capsule->cost_y_e_fun_jac_ut_xt.set_param_sparse(&capsule->cost_y_e_fun_jac_ut_xt, n_update, idx, p); + capsule->cost_y_e_hess.set_param_sparse(&capsule->cost_y_e_hess, n_update, idx, p); + {%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} + capsule->conl_cost_e_fun.set_param_sparse(&capsule->conl_cost_e_fun, n_update, idx, p); + capsule->conl_cost_e_fun_jac_hess.set_param_sparse(&capsule->conl_cost_e_fun_jac_hess, n_update, idx, p); + {%- elif cost.cost_type_e == "EXTERNAL" %} + capsule->ext_cost_e_fun.set_param_sparse(&capsule->ext_cost_e_fun, n_update, idx, p); + capsule->ext_cost_e_fun_jac.set_param_sparse(&capsule->ext_cost_e_fun_jac, n_update, idx, p); + capsule->ext_cost_e_fun_jac_hess.set_param_sparse(&capsule->ext_cost_e_fun_jac_hess, n_update, idx, p); + {% endif %} + // constraints + {% if constraints.constr_type_e == "BGP" %} + capsule->phi_e_constraint.set_param_sparse(&capsule->phi_e_constraint, n_update, idx, p); + {% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + capsule->nl_constr_h_e_fun_jac.set_param_sparse(&capsule->nl_constr_h_e_fun_jac, n_update, idx, p); + capsule->nl_constr_h_e_fun.set_param_sparse(&capsule->nl_constr_h_e_fun, n_update, idx, p); + {%- if solver_options.hessian_approx == "EXACT" %} + capsule->nl_constr_h_e_fun_jac_hess.set_param_sparse(&capsule->nl_constr_h_e_fun_jac_hess, n_update, idx, p); + {%- endif %} + {% endif %} + } +{% endif %}{# if dims.np #} + + return solver_status; +} int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule* capsule) { - // solve NLP + // solve NLP int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); return solver_status; @@ -2265,6 +2474,9 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) { // before destroying, keep some info const int N = capsule->nlp_solver_plan->N; + {%- if custom_update_filename != "" %} + custom_update_terminate_function(capsule); + {%- endif %} // free memory ocp_nlp_solver_opts_destroy(capsule->nlp_opts); ocp_nlp_in_destroy(capsule->nlp_in); @@ -2280,11 +2492,11 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) {%- if solver_options.integrator_type == "IRK" %} for (int i = 0; i < N; i++) { - external_function_param_casadi_free(&capsule->impl_dae_fun[i]); - external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]); - external_function_param_casadi_free(&capsule->impl_dae_jac_x_xdot_u_z[i]); + external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_fun[i]); + external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]); + external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_jac_x_xdot_u_z[i]); {%- if solver_options.hessian_approx == "EXACT" %} - external_function_param_casadi_free(&capsule->impl_dae_hess[i]); + external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_hess[i]); {%- endif %} } free(capsule->impl_dae_fun); @@ -2297,8 +2509,8 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) {%- elif solver_options.integrator_type == "LIFTED_IRK" %} for (int i = 0; i < N; i++) { - external_function_param_casadi_free(&capsule->impl_dae_fun[i]); - external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_u[i]); + external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_fun[i]); + external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_fun_jac_x_xdot_u[i]); } free(capsule->impl_dae_fun); free(capsule->impl_dae_fun_jac_x_xdot_u); @@ -2354,7 +2566,7 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) {%- if solver_options.hessian_approx == "EXACT" %} free(capsule->discr_dyn_phi_fun_jac_ut_xt_hess); {%- endif %} - + {%- endif %} // cost @@ -2362,6 +2574,9 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) external_function_param_casadi_free(&capsule->cost_y_0_fun); external_function_param_casadi_free(&capsule->cost_y_0_fun_jac_ut_xt); external_function_param_casadi_free(&capsule->cost_y_0_hess); +{%- elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} + external_function_param_casadi_free(&capsule->conl_cost_0_fun); + external_function_param_casadi_free(&capsule->conl_cost_0_fun_jac_hess); {%- elif cost.cost_type_0 == "EXTERNAL" %} external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun); external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun_jac); @@ -2377,6 +2592,14 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) free(capsule->cost_y_fun); free(capsule->cost_y_fun_jac_ut_xt); free(capsule->cost_y_hess); +{%- elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} + for (int i = 0; i < N - 1; i++) + { + external_function_param_casadi_free(&capsule->conl_cost_fun[i]); + external_function_param_casadi_free(&capsule->conl_cost_fun_jac_hess[i]); + } + free(capsule->conl_cost_fun); + free(capsule->conl_cost_fun_jac_hess); {%- elif cost.cost_type == "EXTERNAL" %} for (int i = 0; i < N - 1; i++) { @@ -2392,6 +2615,9 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) external_function_param_casadi_free(&capsule->cost_y_e_fun); external_function_param_casadi_free(&capsule->cost_y_e_fun_jac_ut_xt); external_function_param_casadi_free(&capsule->cost_y_e_hess); +{%- elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} + external_function_param_casadi_free(&capsule->conl_cost_e_fun); + external_function_param_casadi_free(&capsule->conl_cost_e_fun_jac_hess); {%- elif cost.cost_type_e == "EXTERNAL" %} external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun); external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun_jac); @@ -2438,15 +2664,6 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule) return 0; } -ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_in; } -ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_out; } -ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule* capsule) { return capsule->sens_out; } -ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver; } -ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_config; } -void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_opts; } -ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_dims; } -ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver_plan; } - void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule* capsule) { @@ -2461,8 +2678,13 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule* capsul int nrow = sqp_iter+1 < stat_m ? sqp_iter+1 : stat_m; + printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha"); + if (stat_n > 8) + printf("\t\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp"); + printf("\n"); + {%- if solver_options.nlp_solver_type == "SQP" %} - printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha\n"); + for (int i = 0; i < nrow; i++) { for (int j = 0; j < stat_n + 1; j++) @@ -2493,3 +2715,27 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule* capsul {%- endif %} } +int {{ model.name }}_acados_custom_update({{ model.name }}_solver_capsule* capsule, double* data, int data_len) +{ +{%- if custom_update_filename == "" %} + (void)capsule; + (void)data; + (void)data_len; + printf("\ndummy function that can be called in between solver calls to update parameters or numerical data efficiently in C.\n"); + printf("nothing set yet..\n"); + return 1; +{% else %} + custom_update_function(capsule, data, data_len); +{%- endif %} +} + + + +ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_in; } +ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_out; } +ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule* capsule) { return capsule->sens_out; } +ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver; } +ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_config; } +void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_opts; } +ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_dims; } +ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule* capsule) { return capsule->nlp_solver_plan; } diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h index 1c82ef3ba0..5cf38aa8c8 100644 --- a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h +++ b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -74,6 +71,12 @@ 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 { @@ -99,15 +102,15 @@ typedef struct {{ model.name }}_solver_capsule external_function_param_casadi *hess_vde_casadi; {%- endif %} {% elif solver_options.integrator_type == "IRK" %} - external_function_param_casadi *impl_dae_fun; - external_function_param_casadi *impl_dae_fun_jac_x_xdot_z; - external_function_param_casadi *impl_dae_jac_x_xdot_u_z; + 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_casadi *impl_dae_hess; + external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_hess; {%- endif %} {% elif solver_options.integrator_type == "LIFTED_IRK" %} - external_function_param_casadi *impl_dae_fun; - external_function_param_casadi *impl_dae_fun_jac_x_xdot_u; + 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; @@ -128,6 +131,9 @@ typedef struct {{ model.name }}_solver_capsule 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; @@ -138,6 +144,9 @@ typedef struct {{ model.name }}_solver_capsule 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; @@ -148,6 +157,9 @@ typedef struct {{ model.name }}_solver_capsule 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; @@ -160,8 +172,10 @@ typedef struct {{ model.name }}_solver_capsule {% 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" %} @@ -169,8 +183,14 @@ typedef struct {{ model.name }}_solver_capsule {% 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; @@ -179,7 +199,7 @@ ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free_capsule({{ model.name }}_s 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); +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 @@ -197,10 +217,14 @@ ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_time_steps({{ model.name */ 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); diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd index 09f8755cbe..233e3f79da 100644 --- a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd +++ b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd @@ -1,8 +1,5 @@ # -# 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 +# Copyright (c) The acados authors. # # This file is part of acados. # @@ -47,11 +44,14 @@ cdef extern from "acados_solver_{{ model.name }}.h": 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 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) diff --git a/third_party/acados/acados_template/c_templates_tera/h_e_constraint.in.h b/third_party/acados/acados_template/c_templates_tera/constraints.in.h similarity index 54% rename from third_party/acados/acados_template/c_templates_tera/h_e_constraint.in.h rename to third_party/acados/acados_template/c_templates_tera/constraints.in.h index a5dd711641..d71ce5cc22 100644 --- a/third_party/acados/acados_template/c_templates_tera/h_e_constraint.in.h +++ b/third_party/acados/acados_template/c_templates_tera/constraints.in.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -31,14 +28,56 @@ * POSSIBILITY OF SUCH DAMAGE.; */ - -#ifndef {{ model.name }}_H_E_CONSTRAINT -#define {{ model.name }}_H_E_CONSTRAINT +#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 *); @@ -68,4 +107,4 @@ int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out(void); } /* extern "C" */ #endif -#endif // {{ model.name }}_H_E_CONSTRAINT +#endif // {{ model.name }}_CONSTRAINTS diff --git a/third_party/acados/acados_template/c_templates_tera/cost.in.h b/third_party/acados/acados_template/c_templates_tera/cost.in.h new file mode 100644 index 0000000000..45eb09c12e --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/cost.in.h @@ -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 diff --git a/third_party/acados/acados_template/c_templates_tera/cost_y_0_fun.in.h b/third_party/acados/acados_template/c_templates_tera/cost_y_0_fun.in.h deleted file mode 100644 index 347446e3f4..0000000000 --- a/third_party/acados/acados_template/c_templates_tera/cost_y_0_fun.in.h +++ /dev/null @@ -1,69 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -#ifndef {{ model.name }}_Y_0_COST -#define {{ model.name }}_Y_0_COST - -#ifdef __cplusplus -extern "C" { -#endif - -{% if cost.cost_type_0 == "NONLINEAR_LS" %} -int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int); -const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int); -int {{ model.name }}_cost_y_0_fun_n_in(void); -int {{ model.name }}_cost_y_0_fun_n_out(void); - -int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int); -const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int); -int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(void); -int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(void); - -int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int); -const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int); -int {{ model.name }}_cost_y_0_hess_n_in(void); -int {{ model.name }}_cost_y_0_hess_n_out(void); -{% endif %} - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // {{ model.name }}_Y_0_COST diff --git a/third_party/acados/acados_template/c_templates_tera/cost_y_e_fun.in.h b/third_party/acados/acados_template/c_templates_tera/cost_y_e_fun.in.h deleted file mode 100644 index acc99009fe..0000000000 --- a/third_party/acados/acados_template/c_templates_tera/cost_y_e_fun.in.h +++ /dev/null @@ -1,69 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -#ifndef {{ model.name }}_Y_E_COST -#define {{ model.name }}_Y_E_COST - -#ifdef __cplusplus -extern "C" { -#endif - -{% if cost.cost_type_e == "NONLINEAR_LS" %} -int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int); -const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int); -int {{ model.name }}_cost_y_e_fun_n_in(void); -int {{ model.name }}_cost_y_e_fun_n_out(void); - -int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int); -const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int); -int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(void); -int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(void); - -int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int); -const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int); -int {{ model.name }}_cost_y_e_hess_n_in(void); -int {{ model.name }}_cost_y_e_hess_n_out(void); -{% endif %} - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // {{ model.name }}_Y_E_COST diff --git a/third_party/acados/acados_template/c_templates_tera/cost_y_fun.in.h b/third_party/acados/acados_template/c_templates_tera/cost_y_fun.in.h deleted file mode 100644 index 1e03780cc1..0000000000 --- a/third_party/acados/acados_template/c_templates_tera/cost_y_fun.in.h +++ /dev/null @@ -1,69 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -#ifndef {{ model.name }}_Y_COST -#define {{ model.name }}_Y_COST - -#ifdef __cplusplus -extern "C" { -#endif - -{% if cost.cost_type == "NONLINEAR_LS" %} -int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_fun_sparsity_in(int); -const int *{{ model.name }}_cost_y_fun_sparsity_out(int); -int {{ model.name }}_cost_y_fun_n_in(void); -int {{ model.name }}_cost_y_fun_n_out(void); - -int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int); -const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int); -int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(void); -int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(void); - -int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_y_hess_sparsity_in(int); -const int *{{ model.name }}_cost_y_hess_sparsity_out(int); -int {{ model.name }}_cost_y_hess_n_in(void); -int {{ model.name }}_cost_y_hess_n_out(void); -{% endif %} - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // {{ model.name }}_Y_COST diff --git a/third_party/acados/acados_template/c_templates_tera/external_cost.in.h b/third_party/acados/acados_template/c_templates_tera/external_cost.in.h deleted file mode 100644 index d200dba45e..0000000000 --- a/third_party/acados/acados_template/c_templates_tera/external_cost.in.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -#ifndef {{ model.name }}_EXT_COST -#define {{ model.name }}_EXT_COST - -#ifdef __cplusplus -extern "C" { -#endif - -{% if cost.cost_ext_fun_type == "casadi" %} -{% if cost.cost_type == "EXTERNAL" %} -int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_fun_n_in(void); -int {{ model.name }}_cost_ext_cost_fun_n_out(void); - -int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(void); -int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(void); - -int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_fun_jac_n_in(void); -int {{ model.name }}_cost_ext_cost_fun_jac_n_out(void); -{% endif %} - -{% else %} -int {{ cost.cost_function_ext_cost }}(void **, void **, void *); -{% endif %} - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // {{ model.name }}_EXT_COST diff --git a/third_party/acados/acados_template/c_templates_tera/external_cost_0.in.h b/third_party/acados/acados_template/c_templates_tera/external_cost_0.in.h deleted file mode 100644 index 8152447e5f..0000000000 --- a/third_party/acados/acados_template/c_templates_tera/external_cost_0.in.h +++ /dev/null @@ -1,75 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -#ifndef {{ model.name }}_EXT_COST_0 -#define {{ model.name }}_EXT_COST_0 - -#ifdef __cplusplus -extern "C" { -#endif - -{% if cost.cost_ext_fun_type_0 == "casadi" %} - -{% if cost.cost_type_0 == "EXTERNAL" %} -int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_0_fun_n_in(void); -int {{ model.name }}_cost_ext_cost_0_fun_n_out(void); - -int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(void); -int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(void); - -int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(void); -int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(void); -{% endif %} - -{% else %} -int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *); -{% endif %} - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // {{ model.name }}_EXT_COST_0 diff --git a/third_party/acados/acados_template/c_templates_tera/external_cost_e.in.h b/third_party/acados/acados_template/c_templates_tera/external_cost_e.in.h deleted file mode 100644 index 56485db4c7..0000000000 --- a/third_party/acados/acados_template/c_templates_tera/external_cost_e.in.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - - -#ifndef {{ model.name }}_EXT_COST_E -#define {{ model.name }}_EXT_COST_E - -#ifdef __cplusplus -extern "C" { -#endif - -{% if cost.cost_ext_fun_type_e == "casadi" %} -{% if cost.cost_type_e == "EXTERNAL" %} -int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_e_fun_n_in(void); -int {{ model.name }}_cost_ext_cost_e_fun_n_out(void); - -int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(void); -int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(void); - -int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *); -const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int); -const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(void); -int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(void); -{% endif %} - -{% else %} -int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *); -{% endif %} - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // {{ model.name }}_EXT_COST_E diff --git a/third_party/acados/acados_template/c_templates_tera/h_constraint.in.h b/third_party/acados/acados_template/c_templates_tera/h_constraint.in.h deleted file mode 100644 index e49176c6ef..0000000000 --- a/third_party/acados/acados_template/c_templates_tera/h_constraint.in.h +++ /dev/null @@ -1,70 +0,0 @@ -/* - * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, - * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, - * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, - * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl - * - * This file is part of acados. - * - * The 2-Clause BSD License - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE.; - */ - -#ifndef {{ model.name }}_H_CONSTRAINT -#define {{ model.name }}_H_CONSTRAINT - -#ifdef __cplusplus -extern "C" { -#endif - -{% if dims.nh > 0 %} -int {{ model.name }}_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *); -const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in(int); -const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out(int); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(void); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(void); - -int {{ model.name }}_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_constr_h_fun_work(int *, int *, int *, int *); -const int *{{ model.name }}_constr_h_fun_sparsity_in(int); -const int *{{ model.name }}_constr_h_fun_sparsity_out(int); -int {{ model.name }}_constr_h_fun_n_in(void); -int {{ model.name }}_constr_h_fun_n_out(void); - -{% if solver_options.hessian_approx == "EXACT" -%} -int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); -const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in(int); -const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out(int); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(void); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(void); -{% endif %} -{% endif %} - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // {{ model.name }}_H_CONSTRAINT diff --git a/third_party/acados/acados_template/c_templates_tera/main.in.c b/third_party/acados/acados_template/c_templates_tera/main.in.c index 99c4f13be1..92a8b33eac 100644 --- a/third_party/acados/acados_template/c_templates_tera/main.in.c +++ b/third_party/acados/acados_template/c_templates_tera/main.in.c @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -31,6 +28,11 @@ * 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 @@ -42,6 +44,9 @@ #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 @@ -191,6 +196,11 @@ int main() 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); diff --git a/third_party/acados/acados_template/c_templates_tera/main_sim.in.c b/third_party/acados/acados_template/c_templates_tera/main_sim.in.c index 743e81d593..8960aa0035 100644 --- a/third_party/acados/acados_template/c_templates_tera/main_sim.in.c +++ b/third_party/acados/acados_template/c_templates_tera/main_sim.in.c @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/acados_template/c_templates_tera/acados_mex_create.in.c b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c similarity index 98% rename from third_party/acados/acados_template/c_templates_tera/acados_mex_create.in.c rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c index e67a51567a..24ae94ac2c 100644 --- a/third_party/acados/acados_template/c_templates_tera/acados_mex_create.in.c +++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_create.in.c @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/acados_template/c_templates_tera/acados_mex_free.in.c b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_free.in.c similarity index 88% rename from third_party/acados/acados_template/c_templates_tera/acados_mex_free.in.c rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_free.in.c index 560adb0b98..bd457969b2 100644 --- a/third_party/acados/acados_template/c_templates_tera/acados_mex_free.in.c +++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_free.in.c @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/acados_template/c_templates_tera/acados_mex_set.in.c b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c similarity index 76% rename from third_party/acados/acados_template/c_templates_tera/acados_mex_set.in.c rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c index f8e1e5e445..78a308df49 100644 --- a/third_party/acados/acados_template/c_templates_tera/acados_mex_set.in.c +++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_set.in.c @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -59,9 +56,6 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) /* RHS */ int min_nrhs = 6; - char *ext_fun_type = mxArrayToString( prhs[0] ); - char *ext_fun_type_e = mxArrayToString( prhs[1] ); - // C ocp const mxArray *C_ocp = prhs[2]; // capsule @@ -378,45 +372,63 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) // } // } // initializations - else if (!strcmp(field, "init_x")) + else if (!strcmp(field, "init_x") || !strcmp(field, "x")) { - if (nrhs!=min_nrhs) - MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) - - acados_size = (N+1) * nx; - MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - for (int ii=0; ii<=N; ii++) + 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) { - ocp_nlp_out_set(config, dims, out, ii, "x", value+ii*nx); + 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")) + else if (!strcmp(field, "init_u") || !strcmp(field, "u")) { - if (nrhs!=min_nrhs) - MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) - - acados_size = N*nu; - MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - for (int ii=0; iisim_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) - MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) - - acados_size = N*nz; - MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - for (int ii=0; iisim_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) - MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) - - acados_size = N*nx; - MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - for (int ii=0; iisim_solver_plan[0]; sim_solver_t type = sim_plan.sim_solver; @@ -454,14 +472,20 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { int nout = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "init_gnsf_phi"); - if (nrhs!=min_nrhs) - MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field) - - acados_size = N*nout; - MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size); - for (int ii=0; ii 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 -%} @@ -177,7 +182,7 @@ static void mdlInitializeSizes (SimStruct *S) {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #} {%- set i_input = i_input + 1 %} // parameters - ssSetInputPortVectorDimension(S, {{ i_input }}, ({{ dims.N }}+1) * {{ dims.np }}); + ssSetInputPortVectorDimension(S, {{ i_input }}, (N+1) * {{ dims.np }}); {%- endif %} {%- if dims.ny > 0 and simulink_opts.inputs.y_ref_0 %} @@ -235,23 +240,34 @@ static void mdlInitializeSizes (SimStruct *S) {%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #} {%- set i_input = i_input + 1 %} // lg - ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ng }}); + 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.ng }}); + 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.nh }}); + 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.nh }}); + 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 #} @@ -312,11 +328,21 @@ static void mdlInitializeSizes (SimStruct *S) 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 @@ -404,7 +430,9 @@ static void mdlOutputs(SimStruct *S, int_T tid) InputRealPtrsType in_sign; - {%- set buffer_sizes = [dims.nbx_0, dims.np, dims.nbx, dims.nbu, dims.ng, dims.nh, dims.nx] -%} + 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)) %} @@ -457,7 +485,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); // update value of parameters - for (int ii = 0; ii <= {{ dims.N }}; ii++) + for (int ii = 0; ii <= N; ii++) { for (int jj = 0; jj < {{ dims.np }}; jj++) buffer[jj] = (double)(*in_sign[ii*{{dims.np}}+jj]); @@ -481,7 +509,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 1; ii < {{ dims.N }}; ii++) + 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]); @@ -497,14 +525,14 @@ static void mdlOutputs(SimStruct *S, int_T tid) 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, {{ dims.N }}, "yref", (void *) buffer); + 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 < {{ dims.N }}; ii++) + 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]); @@ -515,7 +543,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) // ubx {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 1; ii < {{ dims.N }}; ii++) + 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]); @@ -531,7 +559,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) 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, {{ dims.N }}, "lbx", buffer); + 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 @@ -540,7 +568,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) 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, {{ dims.N }}, "ubx", buffer); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", buffer); {%- endif %} @@ -548,7 +576,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) // lbu {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < {{ dims.N }}; ii++) + for (int ii = 0; ii < N; ii++) { for (int jj = 0; jj < {{ dims.nbu }}; jj++) buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]); @@ -559,7 +587,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) // ubu {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < {{ dims.N }}; ii++) + for (int ii = 0; ii < N; ii++) { for (int jj = 0; jj < {{ dims.nbu }}; jj++) buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]); @@ -572,44 +600,67 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int i = 0; i < {{ dims.ng }}; i++) - buffer[i] = (double)(*in_sign[i]); - - for (int ii = 0; ii < {{ dims.N }}; ii++) - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lg", buffer); + 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 i = 0; i < {{ dims.ng }}; i++) - buffer[i] = (double)(*in_sign[i]); - - for (int ii = 0; ii < {{ dims.N }}; ii++) - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ug", buffer); + 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 i = 0; i < {{ dims.nh }}; i++) - buffer[i] = (double)(*in_sign[i]); - - for (int ii = 0; ii < {{ dims.N }}; ii++) - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer); + 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 i = 0; i < {{ dims.nh }}; i++) - buffer[i] = (double)(*in_sign[i]); + 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 -%} + - for (int ii = 0; ii < {{ dims.N }}; ii++) - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer); + {%- 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 #} @@ -629,7 +680,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) for (int i = 0; i < {{ dims.ny * dims.ny }}; i++) buffer[i] = (double)(*in_sign[i]); - for (int ii = 1; ii < {{ dims.N }}; ii++) + for (int ii = 1; ii < N; ii++) ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "W", buffer); {%- endif %} @@ -640,7 +691,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) 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, {{ dims.N }}, "W", buffer); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", buffer); {%- endif %} {%- if simulink_opts.inputs.reset_solver %} {#- reset_solver #} @@ -650,7 +701,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) double reset = (double)(*in_sign[0]); if (reset) { - {{ model.name }}_acados_reset(capsule); + {{ model.name }}_acados_reset(capsule, 1); } {%- endif %} @@ -670,7 +721,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) // u_init {%- set i_input = i_input + 1 %} in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }}); - for (int ii = 0; ii < {{ dims.N }}; ii++) + for (int ii = 0; ii < N; ii++) { for (int jj = 0; jj < {{ dims.nu }}; jj++) buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nu }}+jj]); @@ -686,7 +737,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) /* set outputs */ // assign pointers to output signals - real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin; + 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 #} @@ -699,7 +750,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) {%- if simulink_opts.outputs.utraj == 1 %} {%- set i_output = i_output + 1 %} out_utraj = ssGetOutputPortRealSignal(S, {{ i_output }}); - for (int ii = 0; ii < {{ dims.N }}; ii++) + 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 %} @@ -719,12 +770,32 @@ static void mdlOutputs(SimStruct *S, int_T tid) *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 }}); diff --git a/third_party/acados/acados_template/c_templates_tera/main_mex.in.c b/third_party/acados/acados_template/c_templates_tera/matlab_templates/main_mex.in.c similarity index 95% rename from third_party/acados/acados_template/c_templates_tera/main_mex.in.c rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/main_mex.in.c index 8da5db29a0..851a3cc04f 100644 --- a/third_party/acados/acados_template/c_templates_tera/main_mex.in.c +++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/main_mex.in.c @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/acados_template/c_templates_tera/make_main_mex.in.m b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m similarity index 93% rename from third_party/acados/acados_template/c_templates_tera/make_main_mex.in.m rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m index 9188686a0d..d217948456 100644 --- a/third_party/acados/acados_template/c_templates_tera/make_main_mex.in.m +++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_main_mex.in.m @@ -1,8 +1,5 @@ % -% 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 +% Copyright (c) The acados authors. % % This file is part of acados. % @@ -29,6 +26,7 @@ % CONTRACT, STRICT LIABILITY, OR TORT (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 }}() diff --git a/third_party/acados/acados_template/c_templates_tera/make_mex.in.m b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m similarity index 81% rename from third_party/acados/acados_template/c_templates_tera/make_mex.in.m rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m index cde30f6f41..5e35827137 100644 --- a/third_party/acados/acados_template/c_templates_tera/make_mex.in.m +++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_mex.in.m @@ -1,8 +1,5 @@ % -% 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 +% Copyright (c) The acados authors. % % This file is part of acados. % @@ -29,6 +26,7 @@ % CONTRACT, STRICT LIABILITY, OR TORT (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 }}() @@ -48,6 +46,23 @@ function make_mex_{{ model.name }}() 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 = { ... @@ -94,7 +109,8 @@ function make_mex_{{ model.name }}() if is_octave() % mkoctfile -p CFLAGS mex(acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,... - acados_lib_path, template_lib_path, mex_include, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii}) + 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'; @@ -102,7 +118,8 @@ function make_mex_{{ model.name }}() FLAGS = 'CFLAGS=$CFLAGS -std=c99 -fopenmp'; end mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,... - acados_lib_path, template_lib_path, mex_include, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii}) + template_lib_path, mex_include, acados_lib_path, '-lacados', '-lhpipm', '-lblasfeo',... + acados_lib_extra{:}, mex_files{ii}) end end diff --git a/third_party/acados/acados_template/c_templates_tera/make_sfun.in.m b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m similarity index 74% rename from third_party/acados/acados_template/c_templates_tera/make_sfun.in.m rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m index 77603a78fa..5d74c523f8 100644 --- a/third_party/acados/acados_template/c_templates_tera/make_sfun.in.m +++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun.in.m @@ -1,8 +1,5 @@ % -% 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 +% Copyright (c) The acados authors. % % This file is part of acados. % @@ -29,6 +26,7 @@ % CONTRACT, STRICT LIABILITY, OR TORT (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 = { ... @@ -133,6 +131,9 @@ 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 ' ]; @@ -153,129 +154,176 @@ LIBS{end+1} = '{{ acados_link_libs.osqp }}'; {% 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 %} -mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ... - LIB_PATH, LIBS{:}, SOURCES{:}, ... - '-output', 'acados_solver_sfunction_{{ model.name }}' ); + +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 +%% 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+1,',... +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, size [{{ dims.ng }}]\n '); +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, size [{{ dims.ng }}]\n '); +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, size [{{ dims.nh }}]\n '); +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, size [{{ dims.nh }}]\n '); +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 %} @@ -286,59 +334,99 @@ 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) \ No newline at end of file diff --git a/third_party/acados/acados_template/c_templates_tera/make_sfun_sim.in.m b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m similarity index 70% rename from third_party/acados/acados_template/c_templates_tera/make_sfun_sim.in.m rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m index a0c503e41a..e4c32a8c19 100644 --- a/third_party/acados/acados_template/c_templates_tera/make_sfun_sim.in.m +++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/make_sfun_sim.in.m @@ -1,8 +1,5 @@ % -% 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 +% Copyright (c) The acados authors. % % This file is part of acados. % @@ -29,17 +26,19 @@ % CONTRACT, STRICT LIABILITY, OR TORT (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_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" %} + {%- 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 ', ... @@ -47,15 +46,15 @@ SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ... '{{ 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 ' + {%- 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 ' + '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c ',... {%- endif %} ]; @@ -71,25 +70,42 @@ LIB_PATH = '{{ acados_lib_path }}'; LIBS = '-lacados -lblasfeo -lhpipm'; -eval( [ 'mex -v -output acados_sim_solver_sfunction_{{ model.name }} ', ... - CFLAGS, INCS, ' ', SOURCES, ' -L', LIB_PATH, ' ', LIBS ]); +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 %} @@ -97,7 +113,25 @@ 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) \ No newline at end of file diff --git a/third_party/acados/acados_template/c_templates_tera/mex_solver.in.m b/third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m similarity index 61% rename from third_party/acados/acados_template/c_templates_tera/mex_solver.in.m rename to third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m index 278e0a605c..3743212830 100644 --- a/third_party/acados/acados_template/c_templates_tera/mex_solver.in.m +++ b/third_party/acados/acados_template/c_templates_tera/matlab_templates/mex_solver.in.m @@ -1,8 +1,5 @@ % -% 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 +% Copyright (c) The acados authors. % % This file is part of acados. % @@ -29,6 +26,7 @@ % CONTRACT, STRICT LIABILITY, OR TORT (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 @@ -38,6 +36,9 @@ classdef {{ model.name }}_mex_solver < handle C_ocp_ext_fun cost_ext_fun_type cost_ext_fun_type_e + N + name + code_gen_dir end % properties @@ -52,13 +53,21 @@ classdef {{ model.name }}_mex_solver < handle 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 @@ -112,6 +121,101 @@ classdef {{ model.name }}_mex_solver < handle 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 diff --git a/third_party/acados/acados_template/c_templates_tera/model.in.h b/third_party/acados/acados_template/c_templates_tera/model.in.h index 918e2bc29e..e5059df9ff 100644 --- a/third_party/acados/acados_template/c_templates_tera/model.in.h +++ b/third_party/acados/acados_template/c_templates_tera/model.in.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -47,7 +44,8 @@ extern "C" { {%- endif %} {% if solver_options.integrator_type == "IRK" or solver_options.integrator_type == "LIFTED_IRK" %} -// implicit ODE + {% 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); @@ -55,7 +53,7 @@ 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 +// 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); @@ -63,7 +61,7 @@ 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 +// 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); @@ -79,14 +77,23 @@ 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" %} + {%- 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 %} + {% 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 */ diff --git a/third_party/acados/acados_template/c_templates_tera/phi_e_constraint.in.h b/third_party/acados/acados_template/c_templates_tera/phi_e_constraint.in.h deleted file mode 100644 index dc8e293ad7..0000000000 --- a/third_party/acados/acados_template/c_templates_tera/phi_e_constraint.in.h +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef {{ model.name }}_PHI_E_CONSTRAINT -#define {{ model.name }}_PHI_E_CONSTRAINT - -#ifdef __cplusplus -extern "C" { -#endif - -{% if dims.nphi_e > 0 %} -int {{ model.name }}_phi_e_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_phi_e_constraint_work(int *, int *, int *, int *); -const int *{{ model.name }}_phi_e_constraint_sparsity_in(int); -const int *{{ model.name }}_phi_e_constraint_sparsity_out(int); -int {{ model.name }}_phi_e_constraint_n_in(void); -int {{ model.name }}_phi_e_constraint_n_out(void); -{% endif %} - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif // {{ model.name }}_PHI_E_CONSTRAINT diff --git a/third_party/acados/acados_template/casadi_function_generation.py b/third_party/acados/acados_template/casadi_function_generation.py new file mode 100644 index 0000000000..6373a2809d --- /dev/null +++ b/third_party/acados/acados_template/casadi_function_generation.py @@ -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 + diff --git a/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c b/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c new file mode 100644 index 0000000000..b39ff2e23b --- /dev/null +++ b/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.c @@ -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 +#include +#include +#include + +#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);*/ \ No newline at end of file diff --git a/third_party/acados/acados_template/c_templates_tera/phi_constraint.in.h b/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.h similarity index 59% rename from third_party/acados/acados_template/c_templates_tera/phi_constraint.in.h rename to third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.h index 283ed7f889..9611ea210c 100644 --- a/third_party/acados/acados_template/c_templates_tera/phi_constraint.in.h +++ b/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -31,25 +28,17 @@ * POSSIBILITY OF SUCH DAMAGE.; */ -#ifndef {{ model.name }}_PHI_CONSTRAINT -#define {{ model.name }}_PHI_CONSTRAINT +#include "acados_solver_{{ model.name }}.h" -#ifdef __cplusplus -extern "C" { -#endif +// 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); -{% if dims.nphi > 0 %} -// implicit ODE -int {{ model.name }}_phi_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -int {{ model.name }}_phi_constraint_work(int *, int *, int *, int *); -const int *{{ model.name }}_phi_constraint_sparsity_in(int); -const int *{{ model.name }}_phi_constraint_sparsity_out(int); -int {{ model.name }}_phi_constraint_n_in(void); -int {{ model.name }}_phi_constraint_n_out(void); -{% endif %} -#ifdef __cplusplus -} /* extern "C" */ -#endif +// Custom update function that can be called between solver calls +int custom_update_function({{ model.name }}_solver_capsule* capsule, double* data, int data_len); -#endif // {{ model.name }}_PHI_CONSTRAINT + +// 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); diff --git a/third_party/acados/acados_template/generate_c_code_constraint.py b/third_party/acados/acados_template/generate_c_code_constraint.py deleted file mode 100644 index c79ddc129a..0000000000 --- a/third_party/acados/acados_template/generate_c_code_constraint.py +++ /dev/null @@ -1,180 +0,0 @@ -# -# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, -# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, -# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, -# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -import os -from casadi import * -from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning - -def generate_c_code_constraint( model, con_name, is_terminal, opts ): - - casadi_version = CasadiMeta.version() - casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') - - if casadi_version not in (ALLOWED_CASADI_VERSIONS): - casadi_version_warning(casadi_version) - - # load constraint variables and expression - x = model.x - p = model.p - - if isinstance(x, casadi.MX): - symbol = MX.sym - else: - symbol = SX.sym - - if is_terminal: - con_h_expr = model.con_h_expr_e - con_phi_expr = model.con_phi_expr_e - # create dummy u, z - u = symbol('u', 0, 0) - z = symbol('z', 0, 0) - else: - con_h_expr = model.con_h_expr - con_phi_expr = model.con_phi_expr - u = model.u - z = model.z - - if (not is_empty(con_h_expr)) and (not is_empty(con_phi_expr)): - raise Exception("acados: you can either have constraint_h, or constraint_phi, not both.") - - if not (is_empty(con_h_expr) and is_empty(con_phi_expr)): - if is_empty(con_h_expr): - constr_type = 'BGP' - else: - constr_type = 'BGH' - - if is_empty(p): - p = symbol('p', 0, 0) - - if is_empty(z): - z = symbol('z', 0, 0) - - if not (is_empty(con_h_expr)) and opts['generate_hess']: - # multipliers for hessian - nh = casadi_length(con_h_expr) - lam_h = symbol('lam_h', nh, 1) - - # set up & change directory - code_export_dir = opts["code_export_directory"] - if not os.path.exists(code_export_dir): - os.makedirs(code_export_dir) - - cwd = os.getcwd() - os.chdir(code_export_dir) - gen_dir = con_name + '_constraints' - if not os.path.exists(gen_dir): - os.mkdir(gen_dir) - gen_dir_location = os.path.join('.', gen_dir) - os.chdir(gen_dir_location) - - # export casadi functions - if constr_type == 'BGH': - if is_terminal: - fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt' - else: - fun_name = con_name + '_constr_h_fun_jac_uxt_zt' - - jac_ux_t = transpose(jacobian(con_h_expr, vertcat(u,x))) - jac_z_t = jacobian(con_h_expr, z) - constraint_fun_jac_tran = Function(fun_name, [x, u, z, p], \ - [con_h_expr, jac_ux_t, jac_z_t]) - - constraint_fun_jac_tran.generate(fun_name, casadi_opts) - if opts['generate_hess']: - - if is_terminal: - fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt_hess' - else: - fun_name = con_name + '_constr_h_fun_jac_uxt_zt_hess' - - # adjoint - adj_ux = jtimes(con_h_expr, vertcat(u, x), lam_h, True) - # hessian - hess_ux = jacobian(adj_ux, vertcat(u, x)) - - adj_z = jtimes(con_h_expr, z, lam_h, True) - hess_z = jacobian(adj_z, z) - - # set up functions - constraint_fun_jac_tran_hess = \ - Function(fun_name, [x, u, lam_h, z, p], \ - [con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z]) - - # generate C code - constraint_fun_jac_tran_hess.generate(fun_name, casadi_opts) - - if is_terminal: - fun_name = con_name + '_constr_h_e_fun' - else: - fun_name = con_name + '_constr_h_fun' - h_fun = Function(fun_name, [x, u, z, p], [con_h_expr]) - h_fun.generate(fun_name, casadi_opts) - - else: # BGP constraint - if is_terminal: - fun_name = con_name + '_phi_e_constraint' - r = model.con_r_in_phi_e - con_r_expr = model.con_r_expr_e - else: - fun_name = con_name + '_phi_constraint' - r = model.con_r_in_phi - con_r_expr = model.con_r_expr - - nphi = casadi_length(con_phi_expr) - con_phi_expr_x_u_z = substitute(con_phi_expr, r, con_r_expr) - phi_jac_u = jacobian(con_phi_expr_x_u_z, u) - phi_jac_x = jacobian(con_phi_expr_x_u_z, x) - phi_jac_z = jacobian(con_phi_expr_x_u_z, z) - - hess = hessian(con_phi_expr[0], r)[0] - for i in range(1, nphi): - hess = vertcat(hess, hessian(con_phi_expr[i], r)[0]) - - r_jac_u = jacobian(con_r_expr, u) - r_jac_x = jacobian(con_r_expr, x) - - constraint_phi = \ - Function(fun_name, [x, u, z, p], \ - [con_phi_expr_x_u_z, \ - vertcat(transpose(phi_jac_u), \ - transpose(phi_jac_x)), \ - transpose(phi_jac_z), \ - hess, vertcat(transpose(r_jac_u), \ - transpose(r_jac_x))]) - - constraint_phi.generate(fun_name, casadi_opts) - - # change directory back - os.chdir(cwd) - - return diff --git a/third_party/acados/acados_template/generate_c_code_discrete_dynamics.py b/third_party/acados/acados_template/generate_c_code_discrete_dynamics.py deleted file mode 100644 index c6a245ff81..0000000000 --- a/third_party/acados/acados_template/generate_c_code_discrete_dynamics.py +++ /dev/null @@ -1,98 +0,0 @@ -# -# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, -# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, -# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, -# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES LOSS OF USE, DATA, OR PROFITS OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# - -import os -import casadi as ca -from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning - -def generate_c_code_discrete_dynamics( model, opts ): - - casadi_version = ca.CasadiMeta.version() - casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') - - if casadi_version not in (ALLOWED_CASADI_VERSIONS): - casadi_version_warning(casadi_version) - - # load model - x = model.x - u = model.u - p = model.p - phi = model.disc_dyn_expr - model_name = model.name - nx = casadi_length(x) - - if isinstance(phi, ca.MX): - symbol = ca.MX.sym - elif isinstance(phi, ca.SX): - symbol = ca.SX.sym - else: - Exception("generate_c_code_disc_dyn: disc_dyn_expr must be a CasADi expression, you have type: {}".format(type(phi))) - - # assume nx1 = nx !!! - lam = symbol('lam', nx, 1) - - # generate jacobians - ux = ca.vertcat(u,x) - jac_ux = ca.jacobian(phi, ux) - # generate adjoint - adj_ux = ca.jtimes(phi, ux, lam, True) - # generate hessian - hess_ux = ca.jacobian(adj_ux, ux) - - ## change directory - code_export_dir = opts["code_export_directory"] - if not os.path.exists(code_export_dir): - os.makedirs(code_export_dir) - - cwd = os.getcwd() - os.chdir(code_export_dir) - model_dir = model_name + '_model' - if not os.path.exists(model_dir): - os.mkdir(model_dir) - model_dir_location = os.path.join('.', model_dir) - os.chdir(model_dir_location) - - # set up & generate Functions - fun_name = model_name + '_dyn_disc_phi_fun' - phi_fun = ca.Function(fun_name, [x, u, p], [phi]) - phi_fun.generate(fun_name, casadi_opts) - - fun_name = model_name + '_dyn_disc_phi_fun_jac' - phi_fun_jac_ut_xt = ca.Function(fun_name, [x, u, p], [phi, jac_ux.T]) - phi_fun_jac_ut_xt.generate(fun_name, casadi_opts) - - fun_name = model_name + '_dyn_disc_phi_fun_jac_hess' - phi_fun_jac_ut_xt_hess = ca.Function(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux]) - phi_fun_jac_ut_xt_hess.generate(fun_name, casadi_opts) - - os.chdir(cwd) diff --git a/third_party/acados/acados_template/generate_c_code_explicit_ode.py b/third_party/acados/acados_template/generate_c_code_explicit_ode.py deleted file mode 100644 index 76e6400535..0000000000 --- a/third_party/acados/acados_template/generate_c_code_explicit_ode.py +++ /dev/null @@ -1,124 +0,0 @@ -# -# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, -# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, -# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, -# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -import os -from casadi import * -from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning - -def generate_c_code_explicit_ode( model, opts ): - - casadi_version = CasadiMeta.version() - casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') - if casadi_version not in (ALLOWED_CASADI_VERSIONS): - casadi_version_warning(casadi_version) - - - generate_hess = opts["generate_hess"] - code_export_dir = opts["code_export_directory"] - - # load model - x = model.x - u = model.u - p = model.p - f_expl = model.f_expl_expr - model_name = model.name - - ## get model dimensions - nx = x.size()[0] - nu = u.size()[0] - - if isinstance(f_expl, casadi.MX): - symbol = MX.sym - elif isinstance(f_expl, casadi.SX): - symbol = SX.sym - else: - raise Exception("Invalid type for f_expl! Possible types are 'SX' and 'MX'. Exiting.") - ## set up functions to be exported - Sx = symbol('Sx', nx, nx) - Sp = symbol('Sp', nx, nu) - lambdaX = symbol('lambdaX', nx, 1) - - fun_name = model_name + '_expl_ode_fun' - - ## Set up functions - expl_ode_fun = Function(fun_name, [x, u, p], [f_expl]) - - vdeX = jtimes(f_expl,x,Sx) - vdeP = jacobian(f_expl,u) + jtimes(f_expl,x,Sp) - - fun_name = model_name + '_expl_vde_forw' - - expl_vde_forw = Function(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP]) - - adj = jtimes(f_expl, vertcat(x, u), lambdaX, True) - - fun_name = model_name + '_expl_vde_adj' - expl_vde_adj = Function(fun_name, [x, lambdaX, u, p], [adj]) - - if generate_hess: - S_forw = vertcat(horzcat(Sx, Sp), horzcat(DM.zeros(nu,nx), DM.eye(nu))) - hess = mtimes(transpose(S_forw),jtimes(adj, vertcat(x,u), S_forw)) - hess2 = [] - for j in range(nx+nu): - for i in range(j,nx+nu): - hess2 = vertcat(hess2, hess[i,j]) - - fun_name = model_name + '_expl_ode_hess' - expl_ode_hess = Function(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2]) - - ## generate C code - if not os.path.exists(code_export_dir): - os.makedirs(code_export_dir) - - cwd = os.getcwd() - os.chdir(code_export_dir) - model_dir = model_name + '_model' - if not os.path.exists(model_dir): - os.mkdir(model_dir) - model_dir_location = os.path.join('.', model_dir) - os.chdir(model_dir_location) - fun_name = model_name + '_expl_ode_fun' - expl_ode_fun.generate(fun_name, casadi_opts) - - fun_name = model_name + '_expl_vde_forw' - expl_vde_forw.generate(fun_name, casadi_opts) - - fun_name = model_name + '_expl_vde_adj' - expl_vde_adj.generate(fun_name, casadi_opts) - - if generate_hess: - fun_name = model_name + '_expl_ode_hess' - expl_ode_hess.generate(fun_name, casadi_opts) - os.chdir(cwd) - - return diff --git a/third_party/acados/acados_template/generate_c_code_external_cost.py b/third_party/acados/acados_template/generate_c_code_external_cost.py deleted file mode 100644 index 8396522619..0000000000 --- a/third_party/acados/acados_template/generate_c_code_external_cost.py +++ /dev/null @@ -1,116 +0,0 @@ -# -# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, -# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, -# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, -# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -import os -from casadi import SX, MX, Function, transpose, vertcat, horzcat, hessian, CasadiMeta -from .utils import ALLOWED_CASADI_VERSIONS, casadi_version_warning - - -def generate_c_code_external_cost(model, stage_type, opts): - - casadi_version = CasadiMeta.version() - casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double") - - if casadi_version not in (ALLOWED_CASADI_VERSIONS): - casadi_version_warning(casadi_version) - - x = model.x - p = model.p - - if isinstance(x, MX): - symbol = MX.sym - else: - symbol = SX.sym - - if stage_type == 'terminal': - suffix_name = "_cost_ext_cost_e_fun" - suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess" - suffix_name_jac = "_cost_ext_cost_e_fun_jac" - u = symbol("u", 0, 0) - ext_cost = model.cost_expr_ext_cost_e - custom_hess = model.cost_expr_ext_cost_custom_hess_e - - elif stage_type == 'path': - suffix_name = "_cost_ext_cost_fun" - suffix_name_hess = "_cost_ext_cost_fun_jac_hess" - suffix_name_jac = "_cost_ext_cost_fun_jac" - u = model.u - ext_cost = model.cost_expr_ext_cost - custom_hess = model.cost_expr_ext_cost_custom_hess - - elif stage_type == 'initial': - suffix_name = "_cost_ext_cost_0_fun" - suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess" - suffix_name_jac = "_cost_ext_cost_0_fun_jac" - u = model.u - ext_cost = model.cost_expr_ext_cost_0 - custom_hess = model.cost_expr_ext_cost_custom_hess_0 - - # set up functions to be exported - fun_name = model.name + suffix_name - fun_name_hess = model.name + suffix_name_hess - fun_name_jac = model.name + suffix_name_jac - - # generate expression for full gradient and Hessian - full_hess, grad = hessian(ext_cost, vertcat(u, x)) - - if custom_hess is not None: - full_hess = custom_hess - - ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost]) - ext_cost_fun_jac_hess = Function( - fun_name_hess, [x, u, p], [ext_cost, grad, full_hess] - ) - ext_cost_fun_jac = Function( - fun_name_jac, [x, u, p], [ext_cost, grad] - ) - - # generate C code - code_export_dir = opts["code_export_directory"] - if not os.path.exists(code_export_dir): - os.makedirs(code_export_dir) - - cwd = os.getcwd() - os.chdir(code_export_dir) - gen_dir = model.name + '_cost' - if not os.path.exists(gen_dir): - os.mkdir(gen_dir) - gen_dir_location = "./" + gen_dir - os.chdir(gen_dir_location) - - ext_cost_fun.generate(fun_name, casadi_opts) - ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts) - ext_cost_fun_jac.generate(fun_name_jac, casadi_opts) - - os.chdir(cwd) - return diff --git a/third_party/acados/acados_template/generate_c_code_gnsf.py b/third_party/acados/acados_template/generate_c_code_gnsf.py deleted file mode 100644 index 97acb8e330..0000000000 --- a/third_party/acados/acados_template/generate_c_code_gnsf.py +++ /dev/null @@ -1,131 +0,0 @@ -# -# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, -# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, -# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, -# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -import os -from casadi import * -from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning - -def generate_c_code_gnsf( model, opts ): - - casadi_version = CasadiMeta.version() - casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') - if casadi_version not in (ALLOWED_CASADI_VERSIONS): - casadi_version_warning(casadi_version) - - model_name = model.name - code_export_dir = opts["code_export_directory"] - - # set up directory - if not os.path.exists(code_export_dir): - os.makedirs(code_export_dir) - - cwd = os.getcwd() - os.chdir(code_export_dir) - model_dir = model_name + '_model' - if not os.path.exists(model_dir): - os.mkdir(model_dir) - model_dir_location = os.path.join('.', model_dir) - os.chdir(model_dir_location) - - # obtain gnsf dimensions - get_matrices_fun = model.get_matrices_fun - phi_fun = model.phi_fun - - size_gnsf_A = get_matrices_fun.size_out(0) - gnsf_nx1 = size_gnsf_A[1] - gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] - gnsf_nuhat = max(phi_fun.size_in(1)) - gnsf_ny = max(phi_fun.size_in(0)) - gnsf_nout = max(phi_fun.size_out(0)) - - # set up expressions - # if the model uses MX because of cost/constraints - # the DAE can be exported as SX -> detect GNSF in Matlab - # -> evaluated SX GNSF functions with MX. - u = model.u - - if isinstance(u, casadi.MX): - symbol = MX.sym - else: - symbol = SX.sym - - y = symbol("y", gnsf_ny, 1) - uhat = symbol("uhat", gnsf_nuhat, 1) - p = model.p - x1 = symbol("gnsf_x1", gnsf_nx1, 1) - x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1) - z1 = symbol("gnsf_z1", gnsf_nz1, 1) - dummy = symbol("gnsf_dummy", 1, 1) - empty_var = symbol("gnsf_empty_var", 0, 0) - - ## generate C code - fun_name = model_name + '_gnsf_phi_fun' - phi_fun_ = Function(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)]) - phi_fun_.generate(fun_name, casadi_opts) - - fun_name = model_name + '_gnsf_phi_fun_jac_y' - phi_fun_jac_y = model.phi_fun_jac_y - phi_fun_jac_y_ = Function(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p)) - phi_fun_jac_y_.generate(fun_name, casadi_opts) - - fun_name = model_name + '_gnsf_phi_jac_y_uhat' - phi_jac_y_uhat = model.phi_jac_y_uhat - phi_jac_y_uhat_ = Function(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p)) - phi_jac_y_uhat_.generate(fun_name, casadi_opts) - - fun_name = model_name + '_gnsf_f_lo_fun_jac_x1k1uz' - f_lo_fun_jac_x1k1uz = model.f_lo_fun_jac_x1k1uz - f_lo_fun_jac_x1k1uz_eval = f_lo_fun_jac_x1k1uz(x1, x1dot, z1, u, p) - - # avoid codegeneration issue - if not isinstance(f_lo_fun_jac_x1k1uz_eval, tuple) and is_empty(f_lo_fun_jac_x1k1uz_eval): - f_lo_fun_jac_x1k1uz_eval = [empty_var] - - f_lo_fun_jac_x1k1uz_ = Function(fun_name, [x1, x1dot, z1, u, p], - f_lo_fun_jac_x1k1uz_eval) - f_lo_fun_jac_x1k1uz_.generate(fun_name, casadi_opts) - - fun_name = model_name + '_gnsf_get_matrices_fun' - get_matrices_fun_ = Function(fun_name, [dummy], get_matrices_fun(1)) - get_matrices_fun_.generate(fun_name, casadi_opts) - - # remove fields for json dump - del model.phi_fun - del model.phi_fun_jac_y - del model.phi_jac_y_uhat - del model.f_lo_fun_jac_x1k1uz - del model.get_matrices_fun - - os.chdir(cwd) - - return diff --git a/third_party/acados/acados_template/generate_c_code_implicit_ode.py b/third_party/acados/acados_template/generate_c_code_implicit_ode.py deleted file mode 100644 index f2d50b43ab..0000000000 --- a/third_party/acados/acados_template/generate_c_code_implicit_ode.py +++ /dev/null @@ -1,139 +0,0 @@ -# -# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, -# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, -# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, -# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -import os -from casadi import * -from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning - -def generate_c_code_implicit_ode( model, opts ): - - casadi_version = CasadiMeta.version() - casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') - if casadi_version not in (ALLOWED_CASADI_VERSIONS): - casadi_version_warning(casadi_version) - - generate_hess = opts["generate_hess"] - code_export_dir = opts["code_export_directory"] - - ## load model - x = model.x - xdot = model.xdot - u = model.u - z = model.z - p = model.p - f_impl = model.f_impl_expr - model_name = model.name - - ## get model dimensions - nx = casadi_length(x) - nu = casadi_length(u) - nz = casadi_length(z) - - ## generate jacobians - jac_x = jacobian(f_impl, x) - jac_xdot = jacobian(f_impl, xdot) - jac_u = jacobian(f_impl, u) - jac_z = jacobian(f_impl, z) - - ## generate hessian - x_xdot_z_u = vertcat(x, xdot, z, u) - - if isinstance(x, casadi.MX): - symbol = MX.sym - else: - symbol = SX.sym - - multiplier = symbol('multiplier', nx + nz) - - ADJ = jtimes(f_impl, x_xdot_z_u, multiplier, True) - HESS = jacobian(ADJ, x_xdot_z_u) - - ## Set up functions - p = model.p - fun_name = model_name + '_impl_dae_fun' - impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl]) - - fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' - impl_dae_fun_jac_x_xdot_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z]) - - # fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' - # impl_dae_fun_jac_x_xdot = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z]) - - # fun_name = model_name + '_impl_dae_jac_x_xdot_u' - # impl_dae_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z]) - - fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' - impl_dae_fun_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z]) - - fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' - impl_dae_fun_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u]) - - fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' - impl_dae_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z]) - - - fun_name = model_name + '_impl_dae_hess' - impl_dae_hess = Function(fun_name, [x, xdot, u, z, multiplier, p], [HESS]) - - # generate C code - if not os.path.exists(code_export_dir): - os.makedirs(code_export_dir) - - cwd = os.getcwd() - os.chdir(code_export_dir) - model_dir = model_name + '_model' - if not os.path.exists(model_dir): - os.mkdir(model_dir) - model_dir_location = os.path.join('.', model_dir) - os.chdir(model_dir_location) - - fun_name = model_name + '_impl_dae_fun' - impl_dae_fun.generate(fun_name, casadi_opts) - - fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z' - impl_dae_fun_jac_x_xdot_z.generate(fun_name, casadi_opts) - - fun_name = model_name + '_impl_dae_jac_x_xdot_u_z' - impl_dae_jac_x_xdot_u_z.generate(fun_name, casadi_opts) - - fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' - impl_dae_fun_jac_x_xdot_u_z.generate(fun_name, casadi_opts) - - fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' - impl_dae_fun_jac_x_xdot_u.generate(fun_name, casadi_opts) - - if generate_hess: - fun_name = model_name + '_impl_dae_hess' - impl_dae_hess.generate(fun_name, casadi_opts) - - os.chdir(cwd) diff --git a/third_party/acados/acados_template/generate_c_code_nls_cost.py b/third_party/acados/acados_template/generate_c_code_nls_cost.py deleted file mode 100644 index ffcd78ca7b..0000000000 --- a/third_party/acados/acados_template/generate_c_code_nls_cost.py +++ /dev/null @@ -1,113 +0,0 @@ -# -# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren, -# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor, -# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan, -# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl -# -# This file is part of acados. -# -# The 2-Clause BSD License -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE.; -# - -import os -from casadi import * -from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning - -def generate_c_code_nls_cost( model, cost_name, stage_type, opts ): - - casadi_version = CasadiMeta.version() - casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double') - - if casadi_version not in (ALLOWED_CASADI_VERSIONS): - casadi_version_warning(casadi_version) - - x = model.x - p = model.p - - if isinstance(x, casadi.MX): - symbol = MX.sym - else: - symbol = SX.sym - - if stage_type == 'terminal': - middle_name = '_cost_y_e' - u = symbol('u', 0, 0) - cost_expr = model.cost_y_expr_e - - elif stage_type == 'initial': - middle_name = '_cost_y_0' - u = model.u - cost_expr = model.cost_y_expr_0 - - elif stage_type == 'path': - middle_name = '_cost_y' - u = model.u - cost_expr = model.cost_y_expr - - # set up directory - code_export_dir = opts["code_export_directory"] - if not os.path.exists(code_export_dir): - os.makedirs(code_export_dir) - - cwd = os.getcwd() - os.chdir(code_export_dir) - gen_dir = cost_name + '_cost' - if not os.path.exists(gen_dir): - os.mkdir(gen_dir) - gen_dir_location = os.path.join('.', gen_dir) - os.chdir(gen_dir_location) - - # set up expressions - cost_jac_expr = transpose(jacobian(cost_expr, vertcat(u, x))) - - ny = casadi_length(cost_expr) - - y = symbol('y', ny, 1) - - y_adj = jtimes(cost_expr, vertcat(u, x), y, True) - y_hess = jacobian(y_adj, vertcat(u, x)) - - ## generate C code - suffix_name = '_fun' - fun_name = cost_name + middle_name + suffix_name - y_fun = Function( fun_name, [x, u, p], \ - [ cost_expr ]) - y_fun.generate( fun_name, casadi_opts ) - - suffix_name = '_fun_jac_ut_xt' - fun_name = cost_name + middle_name + suffix_name - y_fun_jac_ut_xt = Function(fun_name, [x, u, p], \ - [ cost_expr, cost_jac_expr ]) - y_fun_jac_ut_xt.generate( fun_name, casadi_opts ) - - suffix_name = '_hess' - fun_name = cost_name + middle_name + suffix_name - y_hess = Function(fun_name, [x, u, y, p], [ y_hess ]) - y_hess.generate( fun_name, casadi_opts ) - - os.chdir(cwd) - - return - diff --git a/third_party/acados/acados_template/gnsf/__init__.py b/third_party/acados/acados_template/gnsf/__init__.py new file mode 100644 index 0000000000..8b13789179 --- /dev/null +++ b/third_party/acados/acados_template/gnsf/__init__.py @@ -0,0 +1 @@ + diff --git a/third_party/acados/acados_template/gnsf/check_reformulation.py b/third_party/acados/acados_template/gnsf/check_reformulation.py new file mode 100644 index 0000000000..2bdfbbc336 --- /dev/null +++ b/third_party/acados/acados_template/gnsf/check_reformulation.py @@ -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 diff --git a/third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py b/third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py new file mode 100644 index 0000000000..ebf1f373a4 --- /dev/null +++ b/third_party/acados/acados_template/gnsf/detect_affine_terms_reduce_nonlinearity.py @@ -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 diff --git a/third_party/acados/acados_template/gnsf/detect_gnsf_structure.py b/third_party/acados/acados_template/gnsf/detect_gnsf_structure.py new file mode 100644 index 0000000000..24ffe643b8 --- /dev/null +++ b/third_party/acados/acados_template/gnsf/detect_gnsf_structure.py @@ -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 diff --git a/third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py b/third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py new file mode 100644 index 0000000000..94aa001c79 --- /dev/null +++ b/third_party/acados/acados_template/gnsf/determine_input_nonlinearity_function.py @@ -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 diff --git a/third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py b/third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py new file mode 100644 index 0000000000..23c2440537 --- /dev/null +++ b/third_party/acados/acados_template/gnsf/determine_trivial_gnsf_transcription.py @@ -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 diff --git a/third_party/acados/acados_template/gnsf/matlab to python.md b/third_party/acados/acados_template/gnsf/matlab to python.md new file mode 100644 index 0000000000..53a0ed971e --- /dev/null +++ b/third_party/acados/acados_template/gnsf/matlab to python.md @@ -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( \ No newline at end of file diff --git a/third_party/acados/acados_template/gnsf/reformulate_with_LOS.py b/third_party/acados/acados_template/gnsf/reformulate_with_LOS.py new file mode 100644 index 0000000000..297a56556c --- /dev/null +++ b/third_party/acados/acados_template/gnsf/reformulate_with_LOS.py @@ -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 diff --git a/third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py b/third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py new file mode 100644 index 0000000000..21ab8ebfd5 --- /dev/null +++ b/third_party/acados/acados_template/gnsf/reformulate_with_invertible_E_mat.py @@ -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 diff --git a/third_party/acados/acados_template/gnsf/structure_detection_print_summary.py b/third_party/acados/acados_template/gnsf/structure_detection_print_summary.py new file mode 100644 index 0000000000..db2d18758e --- /dev/null +++ b/third_party/acados/acados_template/gnsf/structure_detection_print_summary.py @@ -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}) diff --git a/third_party/acados/acados_template/simulink_default_opts.json b/third_party/acados/acados_template/simulink_default_opts.json index 70c939cb88..5d178fef85 100644 --- a/third_party/acados/acados_template/simulink_default_opts.json +++ b/third_party/acados/acados_template/simulink_default_opts.json @@ -4,7 +4,9 @@ "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, @@ -29,6 +31,8 @@ "ug": 1, "lh": 1, "uh": 1, + "lh_e": 1, + "uh_e": 1, "cost_W_0": 0, "cost_W": 0, "cost_W_e": 0, diff --git a/third_party/acados/acados_template/utils.py b/third_party/acados/acados_template/utils.py index 8f44f61e7e..d6f6c02f84 100644 --- a/third_party/acados/acados_template/utils.py +++ b/third_party/acados/acados_template/utils.py @@ -1,9 +1,6 @@ # -*- 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 +# Copyright (c) The acados authors. # # This file is part of acados. # @@ -38,10 +35,17 @@ import shutil import numpy as np from casadi import SX, MX, DM, Function, CasadiMeta -ALLOWED_CASADI_VERSIONS = ('3.5.5', '3.5.4', '3.5.3', '3.5.2', '3.5.1', '3.4.5', '3.4.0') +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: @@ -72,20 +76,17 @@ def get_tera_exec_path(): return TERA_PATH -platform2tera = { - "linux": "linux", - "darwin": "osx", - "win32": "windows" -} - - -def casadi_version_warning(casadi_version): - msg = 'Warning: Please note that the following versions of CasADi are ' - msg += 'officially supported: {}.\n '.format(" or ".join(ALLOWED_CASADI_VERSIONS)) - msg += 'If there is an incompatibility with the CasADi generated code, ' - msg += 'please consider changing your CasADi version.\n' - msg += 'Version {} currently in use.'.format(casadi_version) - print(msg) +def 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): @@ -118,11 +119,16 @@ def is_empty(x): return True else: return False - elif x == None or x == []: + 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. Got: " + str(type(x))) + + "None, numpy array empty list, set. Got: " + str(type(x))) def casadi_length(x): @@ -155,6 +161,14 @@ def make_model_consistent(model): 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() @@ -165,7 +179,7 @@ def get_tera(): 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]) + 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) @@ -200,17 +214,18 @@ def get_tera(): sys.exit(1) -def render_template(in_file, out_file, template_dir, json_path): +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(template_dir): - os.mkdir(template_dir) - os.chdir(template_dir) - tera_path = get_tera() + if not os.path.exists(output_dir): + os.makedirs(output_dir) + os.chdir(output_dir) - # setting up loader and environment - acados_path = os.path.dirname(os.path.abspath(__file__)) - template_glob = os.path.join(acados_path, 'c_templates_tera', '*') + tera_path = get_tera() # call tera as system cmd os_cmd = f"{tera_path} '{template_glob}' '{in_file}' '{json_path}' '{out_file}'" @@ -220,21 +235,24 @@ def render_template(in_file, out_file, template_dir, json_path): status = os.system(os_cmd) if (status != 0): - raise Exception(f'Rendering of {in_file} failed!\n\nAttempted to execute OS command:\n{os_cmd}\n\nExiting.\n') + 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 np_array_to_list(np_array): - if isinstance(np_array, (np.ndarray)): - return np_array.tolist() - elif isinstance(np_array, (SX)): - return DM(np_array).full() - elif isinstance(np_array, (DM)): - return np_array.full() +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(Exception(f"Cannot convert to list type {type(np_array)}")) + raise TypeError(f"Cannot make input of type {type(input)} dumpable.") def format_class_dict(d): @@ -251,7 +269,7 @@ def format_class_dict(d): return out -def get_ocp_nlp_layout(): +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: @@ -259,62 +277,12 @@ def get_ocp_nlp_layout(): return ocp_nlp_layout -def ocp_check_against_layout(ocp_nlp, ocp_dims): - """ - Check dimensions against layout - Parameters - --------- - ocp_nlp : dict - dictionary loaded from JSON to be post-processed. - - ocp_dims : instance of AcadosOcpDims - """ - - ocp_nlp_layout = get_ocp_nlp_layout() - - ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, ocp_nlp_layout) - return - - -def ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, layout): - - for key, item in ocp_nlp.items(): - - try: - layout_of_key = layout[key] - except KeyError: - raise Exception("ocp_check_against_layout_recursion: field" \ - " '{0}' is not in layout but in OCP description.".format(key)) - - if isinstance(item, dict): - ocp_check_against_layout_recursion(item, ocp_dims, layout_of_key) - - if 'ndarray' in layout_of_key: - if isinstance(item, int) or isinstance(item, float): - item = np.array([item]) - if isinstance(item, (list, np.ndarray)) and (layout_of_key[0] != 'str'): - dim_layout = [] - dim_names = layout_of_key[1] - - for dim_name in dim_names: - dim_layout.append(ocp_dims[dim_name]) - - dims = tuple(dim_layout) - - item = np.array(item) - item_dims = item.shape - if len(item_dims) != len(dims): - raise Exception('Mismatching dimensions for field {0}. ' \ - 'Expected {1} dimensional array, got {2} dimensional array.' \ - .format(key, len(dims), len(item_dims))) - - if np.prod(item_dims) != 0 or np.prod(dims) != 0: - if dims != item_dims: - raise Exception('acados -- mismatching dimensions for field {0}. ' \ - 'Provided data has dimensions {1}, ' \ - 'while associated dimensions {2} are {3}' \ - .format(key, item_dims, dim_names, dims)) - return +def 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): @@ -324,9 +292,9 @@ def J_to_idx(J): this_idx = np.nonzero(J[i,:])[0] if len(this_idx) != 1: raise Exception('Invalid J matrix structure detected, ' \ - 'must contain one nonzero element per row. Exiting.') + '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. Exiting.') + raise Exception('J matrices can only contain 1s.') idx[i] = this_idx[0] return idx @@ -342,7 +310,7 @@ def J_to_idx_slack(J): idx[i_idx] = i i_idx = i_idx + 1 elif len(this_idx) > 1: - raise Exception('J_to_idx_slack: Invalid J matrix. Exiting. ' \ + 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, ' \ @@ -376,13 +344,13 @@ def acados_dae_model_json_dump(model): # dump json_file = model_name + '_acados_dae.json' with open(json_file, 'w') as f: - json.dump(dae_dict, f, default=np_array_to_list, indent=4, sort_keys=True) + 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_formulation): +def set_up_imported_gnsf_model(acados_ocp): - gnsf = acados_formulation.gnsf_model + gnsf = acados_ocp.gnsf_model # check CasADi version # dump_casadi_version = gnsf['casadi_version'] @@ -402,39 +370,66 @@ def set_up_imported_gnsf_model(acados_formulation): # obtain gnsf dimensions size_gnsf_A = get_matrices_fun.size_out(0) - acados_formulation.dims.gnsf_nx1 = size_gnsf_A[1] - acados_formulation.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1] - acados_formulation.dims.gnsf_nuhat = max(phi_fun.size_in(1)) - acados_formulation.dims.gnsf_ny = max(phi_fun.size_in(0)) - acados_formulation.dims.gnsf_nout = max(phi_fun.size_out(0)) + 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_formulation.model.phi_fun = phi_fun - acados_formulation.model.phi_fun_jac_y = phi_fun_jac_y - acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat - acados_formulation.model.get_matrices_fun = get_matrices_fun + 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_formulation.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12]) - acados_formulation.model.gnsf['purely_linear'] = int(get_matrices_out[13]) + 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_formulation.model.f_lo_fun_jac_x1k1uz = 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_formulation.dims.gnsf_nx1) - dummy_var_x1dot = SX.sym('dummy_var_x1dot', acados_formulation.dims.gnsf_nx1) - dummy_var_z1 = SX.sym('dummy_var_z1', acados_formulation.dims.gnsf_nz1) - dummy_var_u = SX.sym('dummy_var_z1', acados_formulation.dims.nu) - dummy_var_p = SX.sym('dummy_var_z1', acados_formulation.dims.np) + 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_formulation.model.f_lo_fun_jac_x1k1uz = empty_fun + 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 + - del acados_formulation.gnsf_model +def print_casadi_expression(f): + for ii in range(casadi_length(f)): + print(f[ii,:]) diff --git a/third_party/acados/acados_template/zoro_description.py b/third_party/acados/acados_template/zoro_description.py new file mode 100644 index 0000000000..4d795c1502 --- /dev/null +++ b/third_party/acados/acados_template/zoro_description.py @@ -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__ diff --git a/third_party/acados/build.sh b/third_party/acados/build.sh index 574a577b4d..b45c167b16 100755 --- a/third_party/acados/build.sh +++ b/third_party/acados/build.sh @@ -23,8 +23,8 @@ if [ ! -d acados_repo/ ]; then fi cd acados_repo git fetch --all -git checkout 8ea8827fafb1b23b4c7da1c4cf650de1cbd73584 -git submodule update --recursive --init +git checkout 8af9b0ad180940ef611884574a0b27a43504311d # v0.2.2 +git submodule update --depth=1 --recursive --init # build mkdir -p build @@ -50,7 +50,7 @@ if [[ "$OSTYPE" == "darwin"* ]]; then cargo build --verbose --release --target aarch64-apple-darwin cargo build --verbose --release --target x86_64-apple-darwin lipo -create -output target/release/t_renderer target/x86_64-apple-darwin/release/t_renderer target/aarch64-apple-darwin/release/t_renderer -else +else cargo build --verbose --release fi cp target/release/t_renderer $INSTALL_DIR/ diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_common.h b/third_party/acados/include/acados/dense_qp/dense_qp_common.h index f3809c4294..2a9a974f99 100644 --- a/third_party/acados/include/acados/dense_qp/dense_qp_common.h +++ b/third_party/acados/include/acados/dense_qp/dense_qp_common.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_daqp.h b/third_party/acados/include/acados/dense_qp/dense_qp_daqp.h new file mode 100644 index 0000000000..b262089b4f --- /dev/null +++ b/third_party/acados/include/acados/dense_qp/dense_qp_daqp.h @@ -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_ diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h b/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h index 20eedc26a2..136279d666 100644 --- a/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h +++ b/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h b/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h index 646f11f06f..d051cb15f7 100644 --- a/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h +++ b/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_qore.h b/third_party/acados/include/acados/dense_qp/dense_qp_qore.h index 52606fac5d..392e472918 100644 --- a/third_party/acados/include/acados/dense_qp/dense_qp_qore.h +++ b/third_party/acados/include/acados/dense_qp/dense_qp_qore.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h b/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h index 9f0bb1af68..0e13d3ef64 100644 --- a/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h +++ b/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h index 60d835fc58..ba97db9768 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -419,6 +416,9 @@ void ocp_nlp_embed_initial_value(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp 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); diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h index 7f7a30faf3..9dbf16f6dc 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -82,9 +79,6 @@ acados_size_t ocp_nlp_constraints_bgh_dims_calculate_size(void *config); // void *ocp_nlp_constraints_bgh_dims_assign(void *config, void *raw_memory); // -void ocp_nlp_constraints_bgh_dims_initialize(void *config, void *dims, int nx, int nu, int nz, int nbx, - int nbu, int ng, int nh, int dummy0, int ns); -// void ocp_nlp_constraints_bgh_dims_get(void *config_, void *dims_, const char *field, int* value); // void ocp_nlp_constraints_bgh_dims_set(void *config_, void *dims_, @@ -116,6 +110,9 @@ void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims, void *raw_m 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); /************************************************ diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h index beeec78411..eb05edf7a6 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -63,7 +60,7 @@ typedef struct int nbu; int nbx; int ng; // number of general linear constraints - int nphi; // dimension of convex outer part + 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 @@ -81,9 +78,6 @@ acados_size_t ocp_nlp_constraints_bgp_dims_calculate_size(void *config); // void *ocp_nlp_constraints_bgp_dims_assign(void *config, void *raw_memory); // -void ocp_nlp_constraints_bgp_dims_initialize(void *config, void *dims, int nx, int nu, int nz, - int nbx, int nbu, int ng, int nphi, int nq, int ns); -// void ocp_nlp_constraints_bgp_dims_get(void *config_, void *dims_, const char *field, int* value); @@ -109,6 +103,9 @@ 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 */ diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h index 7cadecab46..bb73c468de 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -60,11 +57,10 @@ typedef struct { acados_size_t (*dims_calculate_size)(void *config); void *(*dims_assign)(void *config, void *raw_memory); - void (*dims_initialize)(void *config, void *dims, int nx, int nu, int nz, int nbx, int nbu, int ng, - int nh, int nq, int ns); acados_size_t (*model_calculate_size)(void *config, void *dims); void *(*model_assign)(void *config, void *dims, void *raw_memory); int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value); + 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); diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h index c9fbbfb404..eb40564036 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -33,8 +30,8 @@ /// -/// \defgroup ocp_nlp_cost ocp_nlp_cost -/// +/// \defgroup ocp_nlp_cost ocp_nlp_cost +/// /// \addtogroup ocp_nlp_cost ocp_nlp_cost /// @{ @@ -62,7 +59,6 @@ typedef struct { acados_size_t (*dims_calculate_size)(void *config); void *(*dims_assign)(void *config, void *raw_memory); - void (*dims_initialize)(void *config, void *dims, int nx, int nu, int ny, int ns, int nz); void (*dims_set)(void *config_, void *dims_, const char *field, int *value); void (*dims_get)(void *config_, void *dims_, const char *field, int *value); acados_size_t (*model_calculate_size)(void *config, void *dims); @@ -91,6 +87,8 @@ typedef struct // 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; // @@ -105,5 +103,5 @@ ocp_nlp_cost_config *ocp_nlp_cost_config_assign(void *raw_memory); #endif #endif // ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_ -/// @} -/// @} +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h new file mode 100644 index 0000000000..2eb3f5d127 --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h @@ -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_ +/// @} +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h index f2196dbee7..78958270de 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -54,6 +51,7 @@ extern "C" { 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; @@ -63,9 +61,6 @@ acados_size_t ocp_nlp_cost_external_dims_calculate_size(void *config); // void *ocp_nlp_cost_external_dims_assign(void *config, void *raw_memory); // -void ocp_nlp_cost_external_dims_initialize(void *config, void *dims, int nx, - int nu, int ny, int ns, int nz); -// void ocp_nlp_cost_external_dims_set(void *config_, void *dims_, const char *field, int* value); // void ocp_nlp_cost_external_dims_get(void *config_, void *dims_, const char *field, int* value); @@ -122,7 +117,7 @@ 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_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 @@ -157,7 +152,10 @@ void ocp_nlp_cost_external_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_ typedef struct { - struct blasfeo_dmat tmp_nv_nv; + 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; @@ -168,6 +166,8 @@ acados_size_t ocp_nlp_cost_external_workspace_calculate_size(void *config, void * 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); // diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h index 3cf759504a..801e9a5b87 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -82,30 +79,14 @@ typedef 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 +/// Assign memory pointed to by raw_memory to ocp_nlp-cost_ls dims struct /// -/// \param[in] config structure containing configuration of ocp_nlp_cost +/// \param[in] config structure containing configuration of ocp_nlp_cost /// module -/// \param[in] raw_memory pointer to memory location +/// \param[in] raw_memory pointer to memory location /// \param[out] [] /// \return dims void *ocp_nlp_cost_ls_dims_assign(void *config, void *raw_memory); - - -/// Initialize the dimensions struct of the -/// ocp_nlp-cost_ls component -/// -/// \param[in] config structure containing configuration ocp_nlp-cost_ls component -/// \param[in] nx number of states -/// \param[in] nu number of inputs -/// \param[in] ny number of residuals -/// \param[in] ns number of slacks -/// \param[in] nz number of algebraic variables -/// \param[out] dims -/// \return size -void ocp_nlp_cost_ls_dims_initialize(void *config, void *dims, int nx, - int nu, int ny, int ns, int nz); - // void ocp_nlp_cost_ls_dims_set(void *config_, void *dims_, const char *field, int* value); // @@ -117,7 +98,7 @@ void ocp_nlp_cost_ls_dims_get(void *config_, void *dims_, const char *field, int //////////////////////////////////////////////////////////////////////////////// -/// structure containing the data describing the linear least-square cost +/// 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 @@ -128,6 +109,8 @@ typedef struct 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; // @@ -170,7 +153,7 @@ void ocp_nlp_cost_ls_opts_set(void *config, void *opts, const char *field, void -/// structure containing the memory associated with cost_ls component +/// structure containing the memory associated with cost_ls component /// of the ocp_nlp module typedef struct { @@ -237,7 +220,8 @@ acados_size_t ocp_nlp_cost_ls_workspace_calculate_size(void *config, void *dims, //////////////////////////////////////////////////////////////////////////////// - +// 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); // diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h index aafb6b3540..5ec68cf580 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -38,8 +35,7 @@ /// @{ /// \addtogroup ocp_nlp_cost_nls ocp_nlp_cost_nls /// \brief This module implements nonlinear-least squares costs of the form -/// \f$\min_{x,u} \| r(x,u) - y_{\text{ref}} \|_W^2\f$. -/// @{ +/// \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_ @@ -65,6 +61,7 @@ extern "C" { 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 @@ -75,8 +72,6 @@ acados_size_t ocp_nlp_cost_nls_dims_calculate_size(void *config); // void *ocp_nlp_cost_nls_dims_assign(void *config, void *raw_memory); // -void ocp_nlp_cost_nls_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz); -// void ocp_nlp_cost_nls_dims_set(void *config_, void *dims_, const char *field, int* value); // void ocp_nlp_cost_nls_dims_get(void *config_, void *dims_, const char *field, int* value); @@ -99,6 +94,7 @@ typedef struct 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; // @@ -144,11 +140,11 @@ 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_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 + 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; // @@ -180,8 +176,11 @@ 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; // temporary vector of dimension ny + struct blasfeo_dvec tmp_2ns; + struct blasfeo_dvec tmp_nz; } ocp_nlp_cost_nls_workspace; // @@ -191,6 +190,8 @@ acados_size_t ocp_nlp_cost_nls_workspace_calculate_size(void *config, void *dims * 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); // diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h index f2128a8574..43fe71b12f 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -68,7 +65,6 @@ typedef struct /* dims */ acados_size_t (*dims_calculate_size)(void *config); void *(*dims_assign)(void *config, void *raw_memory); - void (*dims_initialize)(void *config, void *dims, int nx, int nu, int nx1, int nu1, int nz); void (*dims_set)(void *config_, void *dims_, const char *field, int *value); void (*dims_get)(void *config_, void *dims_, const char *field, int* value); /* model */ diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h index 59a2df4f47..3afdc9f4ed 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -75,10 +72,6 @@ typedef struct acados_size_t ocp_nlp_dynamics_cont_dims_calculate_size(void *config); // void *ocp_nlp_dynamics_cont_dims_assign(void *config, void *raw_memory); -// -void ocp_nlp_dynamics_cont_dims_initialize(void *config, void *dims, int nx, int nu, int nx1, - int nu1, int nz); - // void ocp_nlp_dynamics_cont_dims_set(void *config_, void *dims_, const char *field, int* value); diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h index 8b2a6177bf..6ea26a7010 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -68,10 +65,6 @@ typedef struct acados_size_t ocp_nlp_dynamics_disc_dims_calculate_size(void *config); // void *ocp_nlp_dynamics_disc_dims_assign(void *config, void *raw_memory); -// -void ocp_nlp_dynamics_disc_dims_initialize(void *config, void *dims, int nx, int nu, int nx1, - int nu1, int nz); - // void ocp_nlp_dynamics_disc_dims_set(void *config_, void *dims_, const char *dim, int* value); diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h index cd26788a56..9388f3fd24 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h index df31361680..cb523525e1 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h index f6bd7dcafd..84a023cb69 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h index c085e00d5d..b571f3bac1 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h index 104c297207..682ea206dc 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h index e0b854bc1a..7e12952c15 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h index 74cfb042e3..fdb96417f9 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h index af22c06a17..364d0f4717 100644 --- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -145,12 +142,6 @@ acados_size_t ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, void *dims /************************************************ * functions ************************************************/ - -void ocp_nlp_sqp_rti_preparation_step(void *config_, void *dims_, - void *nlp_in_, void *nlp_out_, void *opts, void *mem_, void *work_); -// -void ocp_nlp_sqp_rti_feedback_step(void *config_, void *dims_, - void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_); // int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_); diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h index caf6caf2f0..d1a45635e4 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h index 50b80850c6..f65f602c15 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h index 14ac97bbf4..d23e658b48 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h index 91690e458d..261606b842 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h index de6ce501b1..8db53a279d 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h index 1a3b7b2fa2..a535503f21 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h index 4ae391a9ff..51df1b1cd6 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h index b95a11114e..844f6048fe 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h index ad4d094516..3b875caeb5 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h index d4b30e007b..a78bc65bb9 100644 --- a/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -84,6 +81,7 @@ 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); diff --git a/third_party/acados/include/acados/sim/sim_collocation_utils.h b/third_party/acados/include/acados/sim/sim_collocation_utils.h index 40a0b1c0cc..045d165cbc 100644 --- a/third_party/acados/include/acados/sim/sim_collocation_utils.h +++ b/third_party/acados/include/acados/sim/sim_collocation_utils.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/sim/sim_common.h b/third_party/acados/include/acados/sim/sim_common.h index 1838d76f81..c4bbd6ed2b 100644 --- a/third_party/acados/include/acados/sim/sim_common.h +++ b/third_party/acados/include/acados/sim/sim_common.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -149,6 +146,8 @@ typedef struct bool jac_reuse; // Newton_scheme *scheme; + double newton_tol; // optinally used in implicit integrators + // workspace void *work; diff --git a/third_party/acados/include/acados/sim/sim_erk_integrator.h b/third_party/acados/include/acados/sim/sim_erk_integrator.h index 24a00c7077..fd46cb4d99 100644 --- a/third_party/acados/include/acados/sim/sim_erk_integrator.h +++ b/third_party/acados/include/acados/sim/sim_erk_integrator.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/sim/sim_gnsf.h b/third_party/acados/include/acados/sim/sim_gnsf.h index 5524b384e0..404532a732 100644 --- a/third_party/acados/include/acados/sim/sim_gnsf.h +++ b/third_party/acados/include/acados/sim/sim_gnsf.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -46,12 +43,12 @@ extern "C" { #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" +// #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 diff --git a/third_party/acados/include/acados/sim/sim_irk_integrator.h b/third_party/acados/include/acados/sim/sim_irk_integrator.h index 6851bacb3a..5090aa0bb5 100644 --- a/third_party/acados/include/acados/sim/sim_irk_integrator.h +++ b/third_party/acados/include/acados/sim/sim_irk_integrator.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h b/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h index 9ec2d97bed..e60bb80ebf 100644 --- a/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h +++ b/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/utils/external_function_generic.h b/third_party/acados/include/acados/utils/external_function_generic.h index 021363f26e..1e68dc155d 100644 --- a/third_party/acados/include/acados/utils/external_function_generic.h +++ b/third_party/acados/include/acados/utils/external_function_generic.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -137,8 +134,8 @@ typedef struct int (*casadi_work)(int *, int *, int *, int *); const int *(*casadi_sparsity_in)(int); const int *(*casadi_sparsity_out)(int); - int (*casadi_n_in)(); - int (*casadi_n_out)(); + int (*casadi_n_in)(void); + int (*casadi_n_out)(void); double **args; double **res; double *w; @@ -195,8 +192,8 @@ typedef struct int (*casadi_work)(int *, int *, int *, int *); const int *(*casadi_sparsity_in)(int); const int *(*casadi_sparsity_out)(int); - int (*casadi_n_in)(); - int (*casadi_n_out)(); + int (*casadi_n_in)(void); + int (*casadi_n_out)(void); double **args; double **res; double *w; diff --git a/third_party/acados/include/acados/utils/math.h b/third_party/acados/include/acados/utils/math.h index fe1da875f6..7156a82084 100644 --- a/third_party/acados/include/acados/utils/math.h +++ b/third_party/acados/include/acados/utils/math.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -42,6 +39,7 @@ extern "C" { #if defined(__MABX2__) double fmax(double a, double b); +int isnan(double x); #endif #define MIN(a,b) (((a)<(b))?(a):(b)) diff --git a/third_party/acados/include/acados/utils/mem.h b/third_party/acados/include/acados/utils/mem.h index 7b9efc5ed8..681a371e36 100644 --- a/third_party/acados/include/acados/utils/mem.h +++ b/third_party/acados/include/acados/utils/mem.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/utils/print.h b/third_party/acados/include/acados/utils/print.h index f8568afb26..824d3cee22 100644 --- a/third_party/acados/include/acados/utils/print.h +++ b/third_party/acados/include/acados/utils/print.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/utils/strsep.h b/third_party/acados/include/acados/utils/strsep.h index 02f1835593..62bdfb4891 100644 --- a/third_party/acados/include/acados/utils/strsep.h +++ b/third_party/acados/include/acados/utils/strsep.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/utils/timing.h b/third_party/acados/include/acados/utils/timing.h index fe561d3891..b0955932da 100644 --- a/third_party/acados/include/acados/utils/timing.h +++ b/third_party/acados/include/acados/utils/timing.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados/utils/types.h b/third_party/acados/include/acados/utils/types.h index d3da0a86b7..a27ef9e552 100644 --- a/third_party/acados/include/acados/utils/types.h +++ b/third_party/acados/include/acados/utils/types.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -78,7 +75,7 @@ typedef int (*casadi_function_t)(const double** arg, double** res, int* iw, doub enum return_values { ACADOS_SUCCESS, - ACADOS_FAILURE, + ACADOS_NAN_DETECTED, ACADOS_MAXITER, ACADOS_MINSTEP, ACADOS_QP_FAILURE, diff --git a/third_party/acados/include/acados_c/condensing_interface.h b/third_party/acados/include/acados_c/condensing_interface.h index 51fe827127..b4302078d6 100644 --- a/third_party/acados/include/acados_c/condensing_interface.h +++ b/third_party/acados/include/acados_c/condensing_interface.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados_c/dense_qp_interface.h b/third_party/acados/include/acados_c/dense_qp_interface.h index 4ecc77381d..b3af4bf682 100644 --- a/third_party/acados/include/acados_c/dense_qp_interface.h +++ b/third_party/acados/include/acados_c/dense_qp_interface.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -41,7 +38,7 @@ extern "C" { #include "acados/dense_qp/dense_qp_common.h" -typedef enum { DENSE_QP_HPIPM, DENSE_QP_QORE, DENSE_QP_QPOASES, DENSE_QP_OOQP } dense_qp_solver_t; +typedef enum { DENSE_QP_HPIPM, DENSE_QP_QORE, DENSE_QP_QPOASES, DENSE_QP_OOQP, DENSE_QP_DAQP } dense_qp_solver_t; typedef struct { diff --git a/third_party/acados/include/acados_c/external_function_interface.h b/third_party/acados/include/acados_c/external_function_interface.h index 6838975071..d4f52db850 100644 --- a/third_party/acados/include/acados_c/external_function_interface.h +++ b/third_party/acados/include/acados_c/external_function_interface.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/acados_c/ocp_nlp_interface.h b/third_party/acados/include/acados_c/ocp_nlp_interface.h index 50bc0cf1af..dd3e596f8b 100644 --- a/third_party/acados/include/acados_c/ocp_nlp_interface.h +++ b/third_party/acados/include/acados_c/ocp_nlp_interface.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -66,6 +63,7 @@ typedef enum { LINEAR_LS, NONLINEAR_LS, + CONVEX_OVER_NONLINEAR, EXTERNAL, INVALID_COST, } ocp_nlp_cost_t; @@ -246,6 +244,10 @@ ACADOS_SYMBOL_EXPORT int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_ ACADOS_SYMBOL_EXPORT 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); +/// +ACADOS_SYMBOL_EXPORT void ocp_nlp_constraints_model_get(ocp_nlp_config *config, ocp_nlp_dims *dims, + ocp_nlp_in *in, int stage, const char *field, void *value); + /* out */ /// Constructs an output struct for the non-linear program. @@ -297,7 +299,7 @@ ACADOS_SYMBOL_EXPORT void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config * ACADOS_SYMBOL_EXPORT void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field, int *dims_out); -void ocp_nlp_dynamics_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, +ACADOS_SYMBOL_EXPORT 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 */ @@ -386,8 +388,6 @@ ACADOS_SYMBOL_EXPORT int ocp_nlp_precompute(ocp_nlp_solver *solver, ocp_nlp_in * /// \param nlp_out The output struct. ACADOS_SYMBOL_EXPORT void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out); -// -void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out); /// Computes the residuals. /// diff --git a/third_party/acados/include/acados_c/ocp_qp_interface.h b/third_party/acados/include/acados_c/ocp_qp_interface.h index 2582f142da..3dc3f1a532 100644 --- a/third_party/acados/include/acados_c/ocp_qp_interface.h +++ b/third_party/acados/include/acados_c/ocp_qp_interface.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * @@ -87,6 +84,11 @@ typedef enum { #else FULL_CONDENSING_QPOASES_NOT_AVAILABLE, #endif +#ifdef ACADOS_WITH_DAQP + FULL_CONDENSING_DAQP, +#else + FULL_CONDENSING_DAQP_NOT_AVAILABLE, +#endif #ifdef ACADOS_WITH_QORE FULL_CONDENSING_QORE, #else diff --git a/third_party/acados/include/acados_c/sim_interface.h b/third_party/acados/include/acados_c/sim_interface.h index 5dce6f153a..09a05d6995 100644 --- a/third_party/acados/include/acados_c/sim_interface.h +++ b/third_party/acados/include/acados_c/sim_interface.h @@ -1,8 +1,5 @@ /* - * 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 + * Copyright (c) The acados authors. * * This file is part of acados. * diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h index ff9e9d4c16..2eab1e4094 100644 --- a/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h @@ -50,18 +50,20 @@ #define BLASFEO_CBLAS_ENUM #ifdef FORTRAN_BLAS_API #ifndef CBLAS_H -enum CBLAS_ORDER {CblasRowMajor=101, CblasColMajor=102}; +enum CBLAS_LAYOUT {CblasRowMajor=101, CblasColMajor=102}; enum CBLAS_TRANSPOSE {CblasNoTrans=111, CblasTrans=112, CblasConjTrans=113}; enum CBLAS_UPLO {CblasUpper=121, CblasLower=122}; enum CBLAS_DIAG {CblasNonUnit=131, CblasUnit=132}; enum CBLAS_SIDE {CblasLeft=141, CblasRight=142}; +#define CBLAS_ORDER CBLAS_LAYOUT /* this for backward compatibility with CBLAS_ORDER */ #endif // CBLAS_H #else // FORTRAN_BLAS_API -enum BLASFEO_CBLAS_ORDER {BlasfeoCblasRowMajor=101, BlasfeoCblasColMajor=102}; +enum BLASFEO_CBLAS_LAYOUT {BlasfeoCblasRowMajor=101, BlasfeoCblasColMajor=102}; enum BLASFEO_CBLAS_TRANSPOSE {BlasfeoCblasNoTrans=111, BlasfeoCblasTrans=112, BlasfeoCblasConjTrans=113}; enum BLASFEO_CBLAS_UPLO {BlasfeoCblasUpper=121, BlasfeoCblasLower=122}; enum BLASFEO_CBLAS_DIAG {BlasfeoCblasNonUnit=131, BlasfeoCblasUnit=132}; enum BLASFEO_CBLAS_SIDE {BlasfeoCblasLeft=141, BlasfeoCblasRight=142}; +#define BLASFEO_CBLAS_ORDER BLASFEO_CBLAS_LAYOUT /* this for backward compatibility with BLASFEO_CBLAS_ORDER */ #endif // FORTRAN_BLAS_API #endif // BLASFEO_CBLAS_ENUM #endif // CBLAS_API @@ -151,15 +153,19 @@ void cblas_dswap(const int N, double *X, const int incX, double *Y, const int in // void cblas_dcopy(const int N, const double *X, const int incX, double *Y, const int incY); +// CBLAS 2 +// +void cblas_dgemv(const enum CBLAS_LAYOUT layout, const enum CBLAS_TRANSPOSE TransA, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *X, const int incX, const double beta, double *Y, const int incY); + // CBLAS 3 // -void cblas_dgemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc); +void cblas_dgemm(const enum CBLAS_LAYOUT layout, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc); // -void cblas_dsyrk(const enum CBLAS_ORDER Order, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc); +void cblas_dsyrk(const enum CBLAS_LAYOUT layout, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc); // -void cblas_dtrmm(const enum CBLAS_ORDER Order, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); +void cblas_dtrmm(const enum CBLAS_LAYOUT layout, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); // -void cblas_dtrsm(const enum CBLAS_ORDER Order, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); +void cblas_dtrsm(const enum CBLAS_LAYOUT layout, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); @@ -240,15 +246,19 @@ void blasfeo_cblas_dswap(const int N, double *X, const int incX, double *Y, cons // void blasfeo_cblas_dcopy(const int N, const double *X, const int incX, double *Y, const int incY); +// CBLAS 2 +// +void blasfeo_cblas_dgemv(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const int M, const int N, const double alpha, const double *A, const int lda, const double *X, const int incX, const double beta, double *Y, const int incY); + // CBLAS 3 // -void blasfeo_cblas_dgemm(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc); +void blasfeo_cblas_dgemm(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc); // -void blasfeo_cblas_dsyrk(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc); +void blasfeo_cblas_dsyrk(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc); // -void blasfeo_cblas_dtrmm(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); +void blasfeo_cblas_dtrmm(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); // -void blasfeo_cblas_dtrsm(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); +void blasfeo_cblas_dtrsm(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); diff --git a/third_party/acados/include/blasfeo/include/blasfeo_target.h b/third_party/acados/include/blasfeo/include/blasfeo_target.h index 4ff4f307b3..51f617a649 100644 --- a/third_party/acados/include/blasfeo/include/blasfeo_target.h +++ b/third_party/acados/include/blasfeo/include/blasfeo_target.h @@ -1,13 +1,13 @@ -#ifndef TARGET_ARMV8A_ARM_CORTEX_A57 -#define TARGET_ARMV8A_ARM_CORTEX_A57 +#ifndef TARGET_X64_INTEL_HASWELL +#define TARGET_X64_INTEL_HASWELL #endif #ifndef TARGET_NEED_FEATURE_AVX2 -/* #undef TARGET_NEED_FEATURE_AVX2 */ +#define TARGET_NEED_FEATURE_AVX2 1 #endif #ifndef TARGET_NEED_FEATURE_FMA -/* #undef TARGET_NEED_FEATURE_FMA */ +#define TARGET_NEED_FEATURE_FMA 1 #endif #ifndef TARGET_NEED_FEATURE_SSE3 @@ -27,11 +27,11 @@ #endif #ifndef TARGET_NEED_FEATURE_VFPv4 -#define TARGET_NEED_FEATURE_VFPv4 1 +/* #undef TARGET_NEED_FEATURE_VFPv4 */ #endif #ifndef TARGET_NEED_FEATURE_NEONv2 -#define TARGET_NEED_FEATURE_NEONv2 1 +/* #undef TARGET_NEED_FEATURE_NEONv2 */ #endif #ifndef LA_HIGH_PERFORMANCE diff --git a/third_party/acados/larch64/lib/libacados.so b/third_party/acados/larch64/lib/libacados.so index 956578fe8b..a081e31ab1 100644 --- a/third_party/acados/larch64/lib/libacados.so +++ b/third_party/acados/larch64/lib/libacados.so @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:d320459a896c765a75456bc7cfb89bacfd692709ccbf5e261051d37eb0cb11e2 -size 493656 +oid sha256:091ed10ea2b52866b826ef6867b3413d6fd61af66fcedf3d932c9adf3fb329de +size 512912 diff --git a/third_party/acados/larch64/lib/libblasfeo.so b/third_party/acados/larch64/lib/libblasfeo.so index d1724f881b..e2191897a1 100644 --- a/third_party/acados/larch64/lib/libblasfeo.so +++ b/third_party/acados/larch64/lib/libblasfeo.so @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3ddffc8ed63d42b4301ce0f7f599c39177298cc598a2cb5e1d26990460ea59be +oid sha256:d40b25de85318c8349c9894a3cbeaa713efae4158d8cc2c5c77af9f1bfbbb28e size 1150320 diff --git a/third_party/acados/larch64/lib/libhpipm.so b/third_party/acados/larch64/lib/libhpipm.so index 506dd75fc3..002eda375a 100644 --- a/third_party/acados/larch64/lib/libhpipm.so +++ b/third_party/acados/larch64/lib/libhpipm.so @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:c7494826f51ccbbe43da2df236216dad90bbd2f378accee9a1828f63fac80599 +oid sha256:91b9276e74c7d7750288d76087d586ed27932939bc7bca2ad1494b6558fc43e3 size 1367352 diff --git a/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 b/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 index fcf27d1114..d64e40ec60 100644 --- a/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 +++ b/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9e936002deb7bb95b38b61a496dd1cf8afbd03588289436851fee56137094cd1 +oid sha256:c0a84042e5315af18e6e33b9b9c5e96573248e41ff165657eb9d83da4da2889f size 223152 diff --git a/third_party/acados/larch64/t_renderer b/third_party/acados/larch64/t_renderer index d5256621e6..f9e7d16601 100755 --- a/third_party/acados/larch64/t_renderer +++ b/third_party/acados/larch64/t_renderer @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:0c4eb7ca23af7909bec464fd63e7d7dde83f9f089ac4bee5fd42b2e4cc6daf1a -size 7419856 +oid sha256:4660fe00e03e29d90459ac2fd46a6e10982577d0ba558b9f05e5712df66814b4 +size 17915048 diff --git a/third_party/acados/x86_64/lib/libacados.so b/third_party/acados/x86_64/lib/libacados.so index 069e4f8f41..4e80f7c76b 100644 --- a/third_party/acados/x86_64/lib/libacados.so +++ b/third_party/acados/x86_64/lib/libacados.so @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:ab9cb6079675937ddb09fcc77133a14ebdc29dd9338e4d79a3e61af1efa271aa -size 534416 +oid sha256:821ce18f417d211c4845b60482d465b809f90dc7d04f023d652d8221e87679b1 +size 553544 diff --git a/third_party/acados/x86_64/lib/libblasfeo.so b/third_party/acados/x86_64/lib/libblasfeo.so index b65d6c7dbb..26d5a3dbe9 100644 --- a/third_party/acados/x86_64/lib/libblasfeo.so +++ b/third_party/acados/x86_64/lib/libblasfeo.so @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:e64a78c320f1ed03c086506ba400149edaa33b3c9a71f3547d2593ee169751b9 +oid sha256:3feea7927d004064bbc5a13c3287467669ce801cb0a3c616cf9e089816da5a0b size 2155088 diff --git a/third_party/acados/x86_64/lib/libhpipm.so b/third_party/acados/x86_64/lib/libhpipm.so index 7799316d94..40e2e4e7d4 100644 --- a/third_party/acados/x86_64/lib/libhpipm.so +++ b/third_party/acados/x86_64/lib/libhpipm.so @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:0cabd3d3b9ceffbe1f7b783089b54c68e4cf635053a0f1cec96ffaf5f68589c1 +oid sha256:a042716f515913786581dff39799eb71fc66caddfa18b1c9f0d54f00c1568fd2 size 1572648 diff --git a/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 b/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 index cb6d4878e4..cf5e550faa 100644 --- a/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 +++ b/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:67661c8be478b34680e093df1d7221679bdbf20c5abd39cfaadd74cac11e44f5 +oid sha256:a6abea4815e3f03cff06fe8a9602e97f9acf102f18f803571460a94595b93be4 size 262824 diff --git a/third_party/acados/x86_64/t_renderer b/third_party/acados/x86_64/t_renderer index 24be571f30..e995a209b7 100755 --- a/third_party/acados/x86_64/t_renderer +++ b/third_party/acados/x86_64/t_renderer @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:f39cc2f561109c4d311a6503cd03883a132ae7a49dbb2e5ef457aaf176df390b -size 7286616 +oid sha256:7a360d4b53826b91ada3358156d44a14d497bdd8ace88707fd4b386ed6d194c7 +size 17503920