acados: update to 0.2.2 (#28821)

* use 0.2.2

* Add mac binaries

* Replace libqpOASES with symlink on larch64

* Add comment to build script

---------

Co-authored-by: Kacper Rączy <gfw.kra@gmail.com>
old-commit-hash: 4c1b8b71d0

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,

third_party/acados/Darwin/lib/libacados.dylib

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:2926c48d96e20086823c87899e5d6d834fada0f616916c0edfe0f58c2aa4b555
+oid sha256:620195ff61aaccf9adf00a66b8acf4e9915a0895d0a86f9f51ec952e44fad70a
-size 834016
+size 853776

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

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

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

third_party/acados/Darwin/t_renderer

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:f2304f73cd27c821a8cc03cf990b74ca246a6a95bef46d28e2b5d5e6a3132fdb
+oid sha256:c6c78791cfdd841da14266c62a6099d0af53cec348051118f1a70518729aa5ba
-size 7776232
+size 8483768

third_party/acados/acados_template/__init__.py

@@ -1,8 +1,5 @@
 #
-# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Copyright (c) The acados authors.
-# 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.
 #
@@ -31,13 +28,13 @@
 # POSSIBILITY OF SUCH DAMAGE.;
 #
 
-from .acados_model import *
+from .acados_model import AcadosModel
-from .generate_c_code_explicit_ode import *
+from .acados_ocp import AcadosOcp, AcadosOcpConstraints, AcadosOcpCost, AcadosOcpDims, AcadosOcpOptions
-from .generate_c_code_implicit_ode import *
+from .acados_sim import AcadosSim, AcadosSimDims, AcadosSimOpts
-from .generate_c_code_constraint import *
+from .acados_ocp_solver import AcadosOcpSolver, get_simulink_default_opts, ocp_get_default_cmake_builder
-from .generate_c_code_nls_cost import *
+from .acados_sim_solver import AcadosSimSolver, sim_get_default_cmake_builder
-from .acados_ocp import *
+from .utils import print_casadi_expression, get_acados_path, get_python_interface_path, \
-from .acados_sim import *
+    get_tera_exec_path, get_tera, check_casadi_version, acados_dae_model_json_dump, \
-from .acados_ocp_solver import *
+    casadi_length, make_object_json_dumpable, J_to_idx, get_default_simulink_opts
-from .acados_sim_solver import *
+
-from .utils import *
+from .zoro_description import ZoroDescription, process_zoro_description

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"
         ],

third_party/acados/acados_template/acados_model.py

@@ -1,8 +1,5 @@
 #
-# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Copyright (c) The acados authors.
-# 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.
 #
@@ -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`
 
-
+        ## CONVEX_OVER_NONLINEAR convex-over-nonlinear cost: psi(y(x, u, p) - y_ref; p)
-def acados_model_strip_casadi_symbolics(model):
+        self.cost_psi_expr_0 = None
-    out = model
+        """
-    if 'f_impl_expr' in out.keys():
+        CasADi expression for the outer loss function :math:`\psi(r, p)`, initial; Default: :code:`None`
-        del out['f_impl_expr']
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'.
-    if 'f_expl_expr' in out.keys():
+        """
-        del out['f_expl_expr']
+        self.cost_psi_expr = None
-    if 'disc_dyn_expr' in out.keys():
+        """
-        del out['disc_dyn_expr']
+        CasADi expression for the outer loss function :math:`\psi(r, p)`; Default: :code:`None`
-    if 'x' in out.keys():
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'.
-        del out['x']
+        """
-    if 'xdot' in out.keys():
+        self.cost_psi_expr_e = None
-        del out['xdot']
+        """
-    if 'u' in out.keys():
+        CasADi expression for the outer loss function :math:`\psi(r, p)`, terminal; Default: :code:`None`
-        del out['u']
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'.
-    if 'z' in out.keys():
+        """
-        del out['z']
+        self.cost_r_in_psi_expr_0 = None
-    if 'p' in out.keys():
+        """
-        del out['p']
+        CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`, initial; Default: :code:`None`
-    # constraints
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'.
-    if 'con_phi_expr' in out.keys():
+        """
-        del out['con_phi_expr']
+        self.cost_r_in_psi_expr = None
-    if 'con_h_expr' in out.keys():
+        """
-        del out['con_h_expr']
+        CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`; Default: :code:`None`
-    if 'con_r_expr' in out.keys():
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'.
-        del out['con_r_expr']
+        """
-    if 'con_r_in_phi' in out.keys():
+        self.cost_r_in_psi_expr_e = None
-        del out['con_r_in_phi']
+        """
-    # terminal
+        CasADi expression for the argument :math:`r`; to the outer loss function :math:`\psi(r, p)`, terminal; Default: :code:`None`
-    if 'con_phi_expr_e' in out.keys():
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'.
-        del out['con_phi_expr_e']
+        """
-    if 'con_h_expr_e' in out.keys():
+        self.cost_conl_custom_outer_hess_0 = None
-        del out['con_h_expr_e']
+        """
-    if 'con_r_expr_e' in out.keys():
+        CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), initial; Default: :code:`None`
-        del out['con_r_expr_e']
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_0` == 'CONVEX_OVER_NONLINEAR'.
-    if 'con_r_in_phi_e' in out.keys():
+        """
-        del out['con_r_in_phi_e']
+        self.cost_conl_custom_outer_hess = None
-    # cost
+        """
-    if 'cost_y_expr' in out.keys():
+        CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost); Default: :code:`None`
-        del out['cost_y_expr']
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type` == 'CONVEX_OVER_NONLINEAR'.
-    if 'cost_y_expr_e' in out.keys():
+        """
-        del out['cost_y_expr_e']
+        self.cost_conl_custom_outer_hess_e = None
-    if 'cost_y_expr_0' in out.keys():
+        """
-        del out['cost_y_expr_0']
+        CasADi expression for the custom hessian of the outer loss function (only for convex-over-nonlinear cost), terminal; Default: :code:`None`
-    if 'cost_expr_ext_cost' in out.keys():
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.cost_type_e` == 'CONVEX_OVER_NONLINEAR'.
-        del out['cost_expr_ext_cost']
+        """
-    if 'cost_expr_ext_cost_e' in out.keys():
-        del out['cost_expr_ext_cost_e']
-    if 'cost_expr_ext_cost_0' in out.keys():
-        del out['cost_expr_ext_cost_0']
-    if 'cost_expr_ext_cost_custom_hess' in out.keys():
-        del out['cost_expr_ext_cost_custom_hess']
-    if 'cost_expr_ext_cost_custom_hess_e' in out.keys():
-        del out['cost_expr_ext_cost_custom_hess_e']
-    if 'cost_expr_ext_cost_custom_hess_0' in out.keys():
-        del out['cost_expr_ext_cost_custom_hess_0']
-
-    return out

third_party/acados/acados_template/acados_ocp_solver_pyx.pyx

@@ -1,9 +1,6 @@
 # -*- coding: future_fstrings -*-
 #
-# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Copyright (c) The acados authors.
-# 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.
 #
@@ -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, <double *> 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')
+            i_string = f'{i:0{lN}d}'
-            solution['u_'+str(i)] = self.get(i,'u')
+            solution['x_'+i_string] = self.get(i,'x')
-            solution['z_'+str(i)] = self.get(i,'z')
+            solution['u_'+i_string] = self.get(i,'u')
-            solution['lam_'+str(i)] = self.get(i,'lam')
+            solution['z_'+i_string] = self.get(i,'z')
-            solution['t_'+str(i)] = self.get(i, 't')
+            solution['lam_'+i_string] = self.get(i,'lam')
-            solution['sl_'+str(i)] = self.get(i, 'sl')
+            solution['t_'+i_string] = self.get(i, 't')
-            solution['su_'+str(i)] = self.get(i, 'su')
+            solution['sl_'+i_string] = self.get(i, 'sl')
-        for i in range(self.N):
+            solution['su_'+i_string] = self.get(i, 'su')
-            solution['pi_'+str(i)] = self.get(i,'pi')
+            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, <void *> value.data)
 
+            if field_ == 'z':
+                field = 'z_guess'.encode('utf-8')
+                acados_solver_common.ocp_nlp_set(self.nlp_config, \
+                    self.nlp_solver, stage, field, <void *> value.data)
+        return
 
     def cost_set(self, int stage, str field_, value_):
         """
@@ -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, <int *> idx.data, <double *> value.data, n_update) == 0
+        return
+
+
     def __del__(self):
         if self.solver_created:
             acados_solver.acados_free(self.capsule)

third_party/acados/acados_template/acados_sim.py

@@ -1,9 +1,6 @@
 # -*- coding: future_fstrings -*-
 #
-# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Copyright (c) The acados authors.
-# 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.
 #
@@ -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):

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"
         ]
     }
 }

third_party/acados/acados_template/acados_sim_solver.py

@@ -1,9 +1,6 @@
 # -*- coding: future_fstrings -*-
 #
-# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Copyright (c) The acados authors.
-# 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.
 #
@@ -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 .casadi_function_generation import generate_c_code_implicit_ode, generate_c_code_gnsf, generate_c_code_explicit_ode
-from .generate_c_code_implicit_ode import generate_c_code_implicit_ode
-from .generate_c_code_gnsf import generate_c_code_gnsf
 from .acados_sim import AcadosSim
 from .acados_ocp import AcadosOcp
-from .acados_model import acados_model_strip_casadi_symbolics
+from .utils import is_column, render_template, format_class_dict, make_object_json_dumpable,\
-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, get_lib_ext,\
-     make_model_consistent, set_up_imported_gnsf_model, get_python_interface_path
+     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):
+    if is_empty(model.u):
-        dims.nu = model.u.shape[0]
-    elif model.u == None or model.u == []:
         dims.nu = 0
     else:
-        raise Exception("model.u should be column vector or None!")
+        dims.nu = casadi_length(model.u)
 
     # nz
-    if is_column(model.z):
+    if is_empty(model.z):
-        dims.nz = model.z.shape[0]
-    elif model.z == None or model.z == []:
         dims.nz = 0
     else:
-        raise Exception("model.z should be column vector or None!")
+        dims.nz = casadi_length(model.z)
 
     # np
-    if is_column(model.p):
+    if is_empty(model.p):
-        dims.np = model.p.shape[0]
-    elif model.p == None or model.p == []:
         dims.np = 0
     else:
-        raise Exception("model.p should be column vector or None!")
+        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
+    # folder model
-    template_dir = os.path.join(code_export_dir, model_name + '_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):
+    if sys.platform=="win32":
-
+        from ctypes import wintypes
-        self.solver_created = False
+        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):
+    @classmethod
-            acados_sim = acados_sim_
+    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
+        # module dependent post processing
-        if isinstance(acados_sim_, AcadosSim):
         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)
 
-            sim_generate_casadi_functions(acados_sim)
+        # dump to json
         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)
+        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()
-            code_export_dir = os.path.abspath(code_export_dir)
         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)
+                cmake_builder.exec(code_export_dir, verbose=verbose)
             else:
-                os.system('make sim_shared_lib')
+                call(
+                    ['make', 'sim_shared_lib'],
+                    stdout=None if verbose else DEVNULL,
+                    stderr=None if verbose else STDOUT
+                )
         os.chdir(cwd)
 
-        self.sim_struct = acados_sim
+
-        model_name = self.sim_struct.model.name
+    @classmethod
+    def create_cython_solver(cls, json_file):
+        """
+        """
+        with open(json_file, 'r') as f:
+            acados_sim_json = json.load(f)
+        code_export_directory = acados_sim_json['code_export_directory']
+
+        importlib.invalidate_caches()
+        rel_code_export_directory = os.path.relpath(code_export_directory)
+        acados_sim_solver_pyx = importlib.import_module(f'{rel_code_export_directory}.acados_sim_solver_pyx')
+
+        AcadosSimSolverCython = getattr(acados_sim_solver_pyx, 'AcadosSimSolverCython')
+        return AcadosSimSolverCython(acados_sim_json['model']['name'])
+
+    def __init__(self, acados_sim, json_file='acados_sim.json', generate=True, build=True, cmake_builder: CMakeBuilder = None, verbose: bool = True):
+
+        self.solver_created = False
+        self.acados_sim = acados_sim
+        model_name = acados_sim.model.name
         self.model_name = model_name
 
+        code_export_dir = os.path.abspath(acados_sim.code_export_directory)
+
+        # reuse existing json and casadi functions, when creating integrator from ocp
+        if generate and not isinstance(acados_sim, AcadosOcp):
+            self.generate(acados_sim, json_file=json_file, cmake_builder=cmake_builder)
+
+        if build:
+            self.build(code_export_dir, cmake_builder=cmake_builder, verbose=True)
+
+        # prepare library loading
+        lib_prefix = 'lib'
+        lib_ext = get_lib_ext()
+        if os.name == 'nt':
+            lib_prefix = ''
+
         # Load acados library to avoid unloading the library.
         # This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed.
         # Unloading a library which uses OpenMP results in a segfault (on any platform?).
         # see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp]
         # or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html]
-        libacados_name = '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
+        # get shared_lib
-        lib_prefix = 'lib'
-        lib_ext = '.so'
-        if os.name == 'nt':
-            lib_prefix = ''
-            lib_ext = ''
-        self.shared_lib_name = os.path.join(code_export_dir, f'{lib_prefix}acados_sim_solver_{model_name}{lib_ext}')
-        print(f'self.shared_lib_name = "{self.shared_lib_name}"')
-        
         self.shared_lib = CDLL(self.shared_lib_name)
 
-
         # create capsule
         getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule").restype = c_void_p
         self.capsule = getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule")()
@@ -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
+        self.gettable_vectors = ['x', 'u', 'z', 'S_adj']
-        nx = self.sim_struct.dims.nx
+        self.gettable_matrices = ['S_forw', 'Sx', 'Su', 'S_hess', 'S_algebraic']
-        nz = self.sim_struct.dims.nz
+        self.gettable_scalars = ['CPUtime', 'time_tot', 'ADtime', 'time_ad', 'LAtime', 'time_la']
-        self.gettable = {
+
-            'x': nx,
+
-            'xn': nx,
+    def simulate(self, x=None, u=None, z=None, p=None):
-            'u': nu,
+        """
-            'z': nz,
+        Simulate the system forward for the given x, u, z, p and return x_next.
-            'S_forw': nx*(nx+nu),
+        Wrapper around `solve()` taking care of setting/getting inputs/outputs.
-            'Sx': nx*nx,
+        """
-            'Su': nx*nu,
+        if x is not None:
-            'S_adj': nx+nu,
+            self.set('x', x)
-            'S_hess': (nx+nu)*(nx+nu),
+        if u is not None:
-            'S_algebraic': (nz)*(nx+nu),
+            self.set('u', u)
-        }
+        if z is not None:
-
+            self.set('z', z)
-        self.settable = ['S_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw
+        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[0],)), dtype=np.float64)
-            out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64)
             out_data = cast(out.ctypes.data, POINTER(c_double))
 
             self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
             self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data)
 
-            if field_ == 'S_forw':
+        elif field_ in self.gettable_matrices:
-                nu = self.sim_struct.dims.nu
+            # get dims
-                nx = self.sim_struct.dims.nx
+            dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
-                out = out.reshape(nx, nx+nu, order='F')
+            dims_data = cast(dims.ctypes.data, POINTER(c_int))
-            elif field_ == 'Sx':
+
-                nx = self.sim_struct.dims.nx
+            self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)]
-                out = out.reshape(nx, nx, order='F')
+            self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data)
-            elif field_ == 'Su':
+
-                nx = self.sim_struct.dims.nx
+            out = np.zeros((dims[0], dims[1]), order='F')
-                nu = self.sim_struct.dims.nu
+            out_data = cast(out.ctypes.data, POINTER(c_double))
-                out = out.reshape(nx, nu, order='F')
+
-            elif field_ == 'S_hess':
+            self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
-                nx = self.sim_struct.dims.nx
+            self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data)
-                nu = self.sim_struct.dims.nu
+
-                out = out.reshape(nx+nu, nx+nu, order='F')
+        elif field_ in self.gettable_scalars:
-            elif field_ == 'S_algebraic':
+            scalar = c_double()
-                nx = self.sim_struct.dims.nx
+            scalar_data = byref(scalar)
-                nu = self.sim_struct.dims.nu
+            self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
-                nz = self.sim_struct.dims.nz
+            self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, scalar_data)
-                out = out.reshape(nz, nx+nu, order='F')
+
+            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' \
+                raise Exception(f'AcadosSimSolver.set(): mismatching dimension' \
-                    ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
+                    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

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)

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, <void *> &scalar)
+        return scalar
+
+
+    def __get_vector(self, field):
+        cdef int[2] dims
+        acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0])
+        # cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((dims[0],), dtype=np.float64))
+        cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((dims[0]),)
+        acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> out.data)
+        return out
+
+
+    def __get_matrix(self, field):
+        cdef int[2] dims
+        acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0])
+        cdef cnp.ndarray[cnp.float64_t, ndim=2] out = np.zeros((dims[0], dims[1]), order='F', dtype=np.float64)
+        acados_sim_solver_common.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, <void *> out.data)
+        return out
+
+
+    def set(self, field_: str, value_):
+        """
+        Set numerical data inside the solver.
+
+            :param field: string in ['p', 'seed_adj', 'T', 'x', 'u', 'xdot', 'z']
+            :param value: the value with appropriate size.
+        """
+        settable = ['seed_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw
+
+        # cast value_ to avoid conversion issues
+        if isinstance(value_, (float, int)):
+            value_ = np.array([value_])
+        # if len(value_.shape) > 1:
+            # raise RuntimeError('AcadosSimSolverCython.set(): value_ should be 1 dimensional')
+
+        cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(value_, dtype=np.float64).flatten()
+
+        field = field_.encode('utf-8')
+        cdef int[2] dims
+
+        # treat parameters separately
+        if field_ == 'p':
+            assert acados_sim_solver.acados_sim_update_params(self.capsule, <double *> value.data, value.shape[0]) == 0
+            return
+        else:
+            acados_sim_solver_common.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, &dims[0])
+
+            value_ = np.ravel(value_, order='F')
+
+            value_shape = value_.shape
+            if len(value_shape) == 1:
+                value_shape = (value_shape[0], 0)
+
+            if value_shape != tuple(dims):
+                raise Exception(f'AcadosSimSolverCython.set(): mismatching dimension' \
+                    f' for field "{field_}" with dimension {tuple(dims)} (you have {value_shape}).')
+
+        # set
+        if field_ in ['xdot', 'z']:
+            acados_sim_solver_common.sim_solver_set(self.sim_solver, field, <void *> value.data)
+        elif field_ in settable:
+            acados_sim_solver_common.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, <void *> value.data)
+        else:
+            raise Exception(f'AcadosSimSolverCython.set(): Unknown field {field_},' \
+                f' available fields are {", ".join(settable)}')
+
+
+    def options_set(self, field_: str, value_: bool):
+        """
+        Set solver options
+
+            :param field: string in ['sens_forw', 'sens_adj', 'sens_hess']
+            :param value: Boolean
+        """
+        fields = ['sens_forw', 'sens_adj', 'sens_hess']
+        if field_ not in fields:
+            raise Exception(f"field {field_} not supported. Supported values are {', '.join(fields)}.\n")
+
+        field = field_.encode('utf-8')
+
+        if not isinstance(value_, bool):
+            raise TypeError("options_set: expected boolean for value")
+
+        cdef bint bool_value = value_
+        acados_sim_solver_common.sim_opts_set(self.sim_config, self.sim_opts, field, <void *> &bool_value)
+        # TODO: only allow setting
+        # if getattr(self.acados_sim.solver_options, field_) or value_ == False:
+        #     acados_sim_solver_common.sim_opts_set(self.sim_config, self.sim_opts, field, <void *> &bool_value)
+        # else:
+        #     raise RuntimeError(f"Cannot set option {field_} to True, because it was False in original solver options.\n")
+
+        return
+
+
+    def __del__(self):
+        if self.solver_created:
+            acados_sim_solver.acados_sim_free(self.capsule)
+            acados_sim_solver.acados_sim_solver_free_capsule(self.capsule)

third_party/acados/acados_template/acados_solver_common.pxd

@@ -1,9 +1,6 @@
 # -*- coding: future_fstrings -*-
 #
-# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Copyright (c) The acados authors.
-# 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.
 #
@@ -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

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:

third_party/acados/acados_template/c_templates_tera/CMakeLists.in.txt

@@ -1,8 +1,5 @@
 #
-# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Copyright (c) The acados authors.
-# 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.
 #
@@ -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_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 }} CACHE PATH "Define the path which contains the lib 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
 {%- endif -%}
+{%- if qp_solver == "FULL_CONDENSING_DAQP" -%}
+    -DACADOS_WITH_DAQP
+{%- endif -%}
 {%- if qp_solver == "PARTIAL_CONDENSING_OSQP" -%}
     -DACADOS_WITH_OSQP
 {%- endif -%}
 {%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" -%}
     -DACADOS_WITH_QPDUNES
 {%- endif -%}
-    -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

third_party/acados/acados_template/c_templates_tera/CPPLINT.cfg

@@ -1 +0,0 @@
-exclude_files=[main, acados_solver, acados_solver_sfun, Makefile, model].*\.?

third_party/acados/acados_template/c_templates_tera/Makefile.in

@@ -1,8 +1,5 @@
 #
-# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Copyright (c) The acados authors.
-# 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.
 #
@@ -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 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 %}
@@ -159,15 +160,14 @@ 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 %}
-	{%- else %}
-MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
 {%- endif %}
+	{%- else %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_generic_source }}
 	{%- endif %}
 MODEL_OBJ := $(MODEL_SRC:.c=.o)
 
@@ -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_SOLVER=libacados_solver_{{ model.name }}{{ shared_lib_ext }}
-LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}.so
+LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}{{ shared_lib_ext }}
-LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
+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 %}

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,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -88,10 +85,11 @@ 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 = (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_casadi *) malloc(sizeof(external_function_param_casadi));
+    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_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }}));
 
+  {%- if model.dyn_ext_fun_type == "casadi" %}
     // external functions (implicit model)
     capsule->sim_impl_dae_fun->casadi_fun = &{{ model.name }}_impl_dae_fun;
     capsule->sim_impl_dae_fun->casadi_work = &{{ model.name }}_impl_dae_fun_work;
@@ -99,7 +97,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     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));
+    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_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_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_forw_vde_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_casadi *) malloc(sizeof(external_function_param_casadi));
+    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));
+    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_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_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 = (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_casadi *) malloc(sizeof(external_function_param_casadi));
+    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_casadi *) malloc(sizeof(external_function_param_casadi));
+    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_{{ model.dyn_ext_fun_type }}_free(capsule->sim_impl_dae_fun);
-    external_function_param_casadi_free(capsule->sim_impl_dae_fun_jac_x_xdot_z);
+    external_function_param_{{ model.dyn_ext_fun_type }}_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_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_{{ model.dyn_ext_fun_type }}_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_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_{{ model.dyn_ext_fun_type }}_free(capsule->sim_gnsf_phi_fun);
-    external_function_param_casadi_free(capsule->sim_gnsf_phi_fun_jac_y);
+    external_function_param_{{ model.dyn_ext_fun_type }}_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_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);

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,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -60,22 +57,23 @@ typedef struct sim_solver_capsule
 
     /* external functions */
     // ERK
-    external_function_param_casadi * sim_forw_vde_casadi;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_forw_vde_casadi;
-    external_function_param_casadi * sim_expl_ode_fun_casadi;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_vde_adj_casadi;
-    external_function_param_casadi * sim_expl_ode_hess;
+    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_{{ model.dyn_ext_fun_type }} * sim_impl_dae_fun;
-    external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_fun_jac_x_xdot_z;
-    external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
+    external_function_param_{{ model.dyn_ext_fun_type }} * 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_hess;
 
     // GNSF
-    external_function_param_casadi * sim_gnsf_phi_fun;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_fun;
-    external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_fun_jac_y;
-    external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
+    external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_jac_y_uhat;
-    external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
+    external_function_param_{{ model.dyn_ext_fun_type }} * 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_get_matrices_fun;
 
 } sim_solver_capsule;
 

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)

third_party/acados/acados_template/c_templates_tera/acados_solver.in.c

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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 }}_constraints.h"
-#include "{{ model.name }}_constraints/{{ model.name }}_phi_constraint.h"
+
-{% endif %}
+{%- if cost.cost_type != "LINEAR_LS" or cost.cost_type_e != "LINEAR_LS" or cost.cost_type_0 != "LINEAR_LS" %}
-{% if constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %}
+#include "{{ model.name }}_cost/{{ model.name }}_cost.h"
-#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"
 {%- endif %}
-{%- if cost.cost_type_0 == "NONLINEAR_LS" %}
+
-#include "{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.h"
+{%- if not solver_options.custom_update_filename %}
-{%- elif cost.cost_type_0 == "EXTERNAL" %}
+    {%- set custom_update_filename = "" %}
-#include "{{ model.name }}_cost/{{ model.name }}_external_cost_0.h"
+{% else %}
+    {%- set custom_update_filename = solver_options.custom_update_filename %}
 {%- endif %}
-{%- if cost.cost_type_e == "NONLINEAR_LS" %}
+{%- if not solver_options.custom_update_header_filename %}
-#include "{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.h"
+    {%- set custom_update_header_filename = "" %}
-{%- elif cost.cost_type_e == "EXTERNAL" %}
+{% else %}
-#include "{{ model.name }}_cost/{{ model.name }}_external_cost_e.h"
+    {%- 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) %}
@@ -832,39 +848,90 @@ 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, "yref", yref_0);
     free(yref_0);
 {%- endif %}
-{%- endif %}
 
-{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %}
+
-    {%- if dims.ny > 0 %}
+{%- if dims.ny == 0 %}
-    double* W = calloc(NY*NY, sizeof(double));
+{%- 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.yref[j] != 0 %}
-            {%- if cost.W[j][k] != 0 %}
+    yref[{{ j }}] = {{ cost.yref[j] }};
-    W[{{ j }}+(NY) * {{ k }}] = {{ cost.W[j][k] }};
         {%- 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 %}
 
-    double* yref = calloc(NY, sizeof(double));
+{%- 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 %}
+
+{%- 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 %}
+        {%- for k in range(end=dims.ny) %}
-    yref[{{ j }}] = {{ cost.yref[j] }};
+            {%- 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 %}
+
+{%- 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 cost.cost_type_0 == "LINEAR_LS" %}
+{%- 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;
@@ -1605,10 +1662,10 @@ void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule
     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 %}
@@ -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;
+    int nlp_solver_exact_hessian = 1;
-    // 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 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<N+1; i++)
@@ -2119,7 +2187,7 @@ int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule)
     // get qp_status: if NaN -> 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,12 +2321,145 @@ 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)
 {
@@ -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_{{ model.dyn_ext_fun_type }}_free(&capsule->impl_dae_fun[i]);
-        external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]);
+        external_function_param_{{ model.dyn_ext_fun_type }}_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_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_{{ model.dyn_ext_fun_type }}_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_jac_x_xdot_u[i]);
     }
     free(capsule->impl_dae_fun);
     free(capsule->impl_dae_fun_jac_x_xdot_u);
@@ -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; }

third_party/acados/acados_template/c_templates_tera/acados_solver.in.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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_{{ model.dyn_ext_fun_type }} *impl_dae_fun;
-    external_function_param_casadi *impl_dae_fun_jac_x_xdot_z;
+    external_function_param_{{ model.dyn_ext_fun_type }} *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_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_{{ model.dyn_ext_fun_type }} *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_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,9 +217,13 @@ 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);

third_party/acados/acados_template/c_templates_tera/acados_solver.in.pxd

@@ -1,8 +1,5 @@
 #
-# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Copyright (c) The acados authors.
-# 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.
 #
@@ -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)

third_party/acados/acados_template/c_templates_tera/constraints.in.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -31,14 +28,56 @@
  * POSSIBILITY OF SUCH DAMAGE.;
  */
 
-
+#ifndef {{ model.name }}_CONSTRAINTS
-#ifndef {{ model.name }}_H_E_CONSTRAINT
+#define {{ model.name }}_CONSTRAINTS
-#define {{ model.name }}_H_E_CONSTRAINT
 
 #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

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

third_party/acados/acados_template/c_templates_tera/cost_y_0_fun.in.h

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

third_party/acados/acados_template/c_templates_tera/cost_y_e_fun.in.h

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

third_party/acados/acados_template/c_templates_tera/cost_y_fun.in.h

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

third_party/acados/acados_template/c_templates_tera/external_cost.in.h

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

third_party/acados/acados_template/c_templates_tera/external_cost_0.in.h

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

third_party/acados/acados_template/c_templates_tera/external_cost_e.in.h

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

third_party/acados/acados_template/c_templates_tera/h_constraint.in.h

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

third_party/acados/acados_template/c_templates_tera/main.in.c

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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 <stdio.h>
@@ -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);

third_party/acados/acados_template/c_templates_tera/main_sim.in.c

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *

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,
+ * Copyright (c) The acados authors.
- * 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.
  *

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,
+ * Copyright (c) The acados authors.
- * 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.
  *

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,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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,11 +372,10 @@ 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)
         {
-        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++)
@@ -390,11 +383,17 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
                 ocp_nlp_out_set(config, dims, out, ii, "x", value+ii*nx);
             }
         }
-    else if (!strcmp(field, "init_u"))
+        else // (nrhs == min_nrhs + 1)
+        {
+            acados_size = nx;
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, "x", value);
+        }
+    }
+    else if (!strcmp(field, "init_u") || !strcmp(field, "u"))
+    {
+        if (nrhs==min_nrhs)
         {
-        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; ii<N; ii++)
@@ -402,16 +401,22 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
                 ocp_nlp_out_set(config, dims, out, ii, "u", value+ii*nu);
             }
         }
-    else if (!strcmp(field, "init_z"))
+        else // (nrhs == min_nrhs + 1)
+        {
+            acados_size = nu;
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, "u", value);
+        }
+    }
+    else if (!strcmp(field, "init_z")||!strcmp(field, "z"))
     {
         sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
         if (type == IRK)
         {
             int nz = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "z");
-            if (nrhs!=min_nrhs)
+            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; ii<N; ii++)
@@ -419,21 +424,27 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
                     ocp_nlp_set(config, solver, ii, "z_guess", value+ii*nz);
                 }
             }
+            else // (nrhs==min_nrhs+1)
+            {
+                acados_size = nz;
+                MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+                ocp_nlp_set(config, solver, s0, "z_guess", value);
+            }
+        }
         else
         {
             MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk")
         }
     }
-    else if (!strcmp(field, "init_xdot"))
+    else if (!strcmp(field, "init_xdot")||!strcmp(field, "xdot"))
     {
         sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
         if (type == IRK)
         {
             int nx = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "x");
-            if (nrhs!=min_nrhs)
+            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; ii<N; ii++)
@@ -441,12 +452,19 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
                     ocp_nlp_set(config, solver, ii, "xdot_guess", value+ii*nx);
                 }
             }
+            else // nrhs==min_nrhs+1)
+            {
+                acados_size = nx;
+                MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+                ocp_nlp_set(config, solver, s0, "xdot_guess", value);
+            }
+        }
         else
         {
             MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk")
         }
     }
-    else if (!strcmp(field, "init_gnsf_phi"))
+    else if (!strcmp(field, "init_gnsf_phi")||!strcmp(field, "gnsf_phi"))
     {
         sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
@@ -454,9 +472,8 @@ 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)
+            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<N; ii++)
@@ -464,16 +481,22 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
                     ocp_nlp_set(config, solver, ii, "gnsf_phi_guess", value+ii*nx);
                 }
             }
+            else // (nrhs==min_nrhs+1)
+            {
+                acados_size = nout;
+                MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+                ocp_nlp_set(config, solver, s0, "gnsf_phi_guess", value);
+            }
+        }
         else
         {
             MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_gnsf_phi", "irk_gnsf")
         }
     }
-    else if (!strcmp(field, "init_pi"))
+    else if (!strcmp(field, "init_pi")||!strcmp(field, "pi"))
+    {
+        if (nrhs==min_nrhs)
         {
-        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; ii<N; ii++)
@@ -481,24 +504,63 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
                 ocp_nlp_out_set(config, dims, out, ii, "pi", value+ii*nx);
             }
         }
-    else if (!strcmp(field, "init_lam"))
+        else // (nrhs==min_nrhs+1)
         {
-        for (int ii=s0; ii<se; ii++)
+            acados_size = nx;
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, "pi", value);
+        }
+    }
+    else if (!strcmp(field, "init_lam")||!strcmp(field, "lam"))
     {
-            int nlam = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lam");
+        if (nrhs==min_nrhs)
-            MEX_DIM_CHECK_VEC(fun_name, "lam", nrow*ncol, nlam);
+        {
-
+            MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
-            ocp_nlp_out_set(config, dims, out, ii, "lam", value);
+        }
+        else //(nrhs==min_nrhs+1)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "lam");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, "lam", value);
         }
     }
-    else if (!strcmp(field, "init_t"))
+    else if (!strcmp(field, "init_t")||!strcmp(field, "t"))
     {
-        for (int ii=s0; ii<se; ii++)
+        if (nrhs==min_nrhs)
         {
-            int nt = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "t");
+            MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
-            MEX_DIM_CHECK_VEC(fun_name, "t", nrow*ncol, nt);
+        }
-
+        else //(nrhs==min_nrhs+1)
-            ocp_nlp_out_set(config, dims, out, ii, "t", value);
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "t");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, "t", value);
+        }
+    }
+    else if (!strcmp(field, "init_sl")||!strcmp(field, "sl"))
+    {
+        if (nrhs==min_nrhs)
+        {
+            MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
+        }
+        else //(nrhs==min_nrhs+1)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "sl");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, field, value);
+        }
+    }
+    else if (!strcmp(field, "init_su")||!strcmp(field, "su"))
+    {
+        if (nrhs==min_nrhs)
+        {
+            MEX_SETTER_NO_ALL_STAGES_SUPPORT(fun_name, field)
+        }
+        else //(nrhs==min_nrhs+1)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "su");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            ocp_nlp_out_set(config, dims, out, s0, field, value);
         }
     }
     else if (!strcmp(field, "p"))
@@ -558,8 +620,8 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     else
     {
         MEX_FIELD_NOT_SUPPORTED_SUGGEST(fun_name, field, "p, constr_x0,\
- constr_lbx, constr_ubx, constr_C, constr_D, constr_lg, constr_ug, constr_lh, constr_uh\
+ constr_lbx, constr_ubx, constr_C, constr_D, constr_lg, constr_ug, constr_lh, constr_uh,\
- constr_lbu, constr_ubu, cost_y_ref[_e],\
+ constr_lbu, constr_ubu, cost_y_ref[_e], sl, su, x, xdot, u, pi, lam, z, \
  cost_Vu, cost_Vx, cost_Vz, cost_W, cost_Z, cost_Zl, cost_Zu, cost_z,\
  cost_zl, cost_zu, init_x, init_u, init_z, init_xdot, init_gnsf_phi,\
  init_pi, nlp_solver_max_iter, qp_warm_start, warm_start_first_qp, print_level");

third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_mex_solve.in.c

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *

third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_sim_solver_sfun.in.c

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *

third_party/acados/acados_template/c_templates_tera/matlab_templates/acados_solver_sfun.in.c

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -61,6 +58,8 @@ static void mdlInitializeSizes (SimStruct *S)
     ssSetNumContStates(S, 0);
     ssSetNumDiscStates(S, 0);
 
+    int N = {{ model.name | upper }}_N;
+
   {%- for key, val in simulink_opts.inputs -%}
     {%- if val != 0 and val != 1 -%}
       {{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }}
@@ -117,6 +116,12 @@ static void mdlInitializeSizes (SimStruct *S)
   {%- if dims.nh > 0 and simulink_opts.inputs.uh -%}  {#- uh #}
     {%- set n_inputs = n_inputs + 1 -%}
   {%- endif -%}
+  {%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%}  {#- lh_e #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.nh_e > 0 and simulink_opts.inputs.uh_e -%}  {#- uh_e #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
 
   {%- for key, val in simulink_opts.inputs -%}
     {%- if val != 0 and val != 1 -%}
@@ -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++)
+    for (int ii = 0; ii < N; ii++)
-        buffer[i] = (double)(*in_sign[i]);
+    {
-
+        for (int jj = 0; jj < {{ dims.ng }}; jj++)
-    for (int ii = 0; ii < {{ dims.N }}; ii++)
+            buffer[jj] = (double)(*in_sign[ii*{{ dims.ng }}+jj]);
-        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lg", buffer);
+        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++)
+    for (int ii = 0; ii < N; ii++)
-        buffer[i] = (double)(*in_sign[i]);
+    {
-
+        for (int jj = 0; jj < {{ dims.ng }}; jj++)
-    for (int ii = 0; ii < {{ dims.N }}; ii++)
+            buffer[jj] = (double)(*in_sign[ii*{{ dims.ng }}+jj]);
-        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ug", buffer);
+        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++)
+    for (int ii = 0; ii < N; ii++)
-        buffer[i] = (double)(*in_sign[i]);
+    {
-
+        for (int jj = 0; jj < {{ dims.nh }}; jj++)
-    for (int ii = 0; ii < {{ dims.N }}; ii++)
+            buffer[jj] = (double)(*in_sign[ii*{{ dims.nh }}+jj]);
-        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer);
+        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++)
+    for (int ii = 0; ii < N; ii++)
-        buffer[i] = (double)(*in_sign[i]);
+    {
+        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++)
+  {%- if dims.nh_e > 0 and simulink_opts.inputs.lh_e -%}  {#- lh_e #}
-        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer);
+    // 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 }});

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,
+ * Copyright (c) The acados authors.
- * 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.
  *

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,
+% Copyright (c) The acados authors.
-% 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.
 %
@@ -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 }}()

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,
+% Copyright (c) The acados authors.
-% 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.
 %
@@ -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
 

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,
+% Copyright (c) The acados authors.
-% 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.
 %
@@ -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{:}, ...
+
+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)

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,
+% Copyright (c) The acados authors.
-% 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.
 %
@@ -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 = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ...
@@ -36,6 +34,7 @@ SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ...
             {%- if  solver_options.integrator_type == 'ERK' %}
             '{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c ',...
             '{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c ',...
+            '{{ model.name }}_model/{{ model.name }}_expl_vde_adj.c ',...
                 {%- if solver_options.hessian_approx == 'EXACT' %}
             '{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c ',...
                 {%- endif %}
@@ -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 %}
+                {%- if model.gnsf.purely_linear != 1 %}
-            '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c '
+            '{{ 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_fun_jac_y.c ',...
-            '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c '
+            '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c ',...
-                {% if model.gnsf.nontrivial_f_LO == 1 %}
+                {%- if model.gnsf.nontrivial_f_LO == 1 %}
-            '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c '
+            '{{ 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 }} ', ...
+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)

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,
+% Copyright (c) The acados authors.
-% 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.
 %
@@ -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

third_party/acados/acados_template/c_templates_tera/model.in.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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);
@@ -80,13 +78,22 @@ int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in(void);
 int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out(void);
 
 	{%- if hessian_approx == "EXACT" %}
+// implicit ODE - hessian
 int {{ model.name }}_impl_dae_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
 int {{ model.name }}_impl_dae_hess_work(int *, int *, int *, int *);
 const int *{{ model.name }}_impl_dae_hess_sparsity_in(int);
 const int *{{ model.name }}_impl_dae_hess_sparsity_out(int);
 int {{ model.name }}_impl_dae_hess_n_in(void);
 int {{ model.name }}_impl_dae_hess_n_out(void);
-{%- endif %}
+	{% 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 */

third_party/acados/acados_template/c_templates_tera/phi_e_constraint.in.h

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

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
+

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

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,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -31,25 +28,17 @@
  * POSSIBILITY OF SUCH DAMAGE.;
  */
 
-#ifndef {{ model.name }}_PHI_CONSTRAINT
+#include "acados_solver_{{ model.name }}.h"
-#define {{ model.name }}_PHI_CONSTRAINT
 
-#ifdef __cplusplus
+// Called at the end of solver creation.
-extern "C" {
+// This is allowed to allocate memory and store the pointer to it into capsule->custom_update_memory.
-#endif
+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
+// Custom update function that can be called between solver calls
-} /* extern "C" */
+int custom_update_function({{ model.name }}_solver_capsule* capsule, double* data, int data_len);
-#endif
 
-#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);

third_party/acados/acados_template/generate_c_code_constraint.py

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

third_party/acados/acados_template/generate_c_code_discrete_dynamics.py

@@ -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)

third_party/acados/acados_template/generate_c_code_explicit_ode.py

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

third_party/acados/acados_template/generate_c_code_external_cost.py

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

third_party/acados/acados_template/generate_c_code_gnsf.py

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

third_party/acados/acados_template/generate_c_code_implicit_ode.py

@@ -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)

third_party/acados/acados_template/generate_c_code_nls_cost.py

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

third_party/acados/acados_template/gnsf/__init__.py

@@ -0,0 +1 @@
+

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

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

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

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

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

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(

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

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

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})

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,

third_party/acados/acados_template/utils.py

@@ -1,9 +1,6 @@
 # -*- coding: future_fstrings -*-
 #
-# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Copyright (c) The acados authors.
-# 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.
 #
@@ -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,14 +76,11 @@ def get_tera_exec_path():
     return TERA_PATH
 
 
-platform2tera = {
+def check_casadi_version():
-    "linux": "linux",
+    casadi_version = CasadiMeta.version()
-    "darwin": "osx",
+    if casadi_version in ALLOWED_CASADI_VERSIONS:
-    "win32": "windows"
+        return
-}
+    else:
-
-
-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, '
@@ -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
+    tera_path = get_tera()
-    acados_path = os.path.dirname(os.path.abspath(__file__))
-    template_glob = os.path.join(acados_path, 'c_templates_tera', '*')
 
     # call tera as system cmd
     os_cmd = f"{tera_path} '{template_glob}' '{in_file}' '{json_path}' '{out_file}'"
@@ -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):
+def make_object_json_dumpable(input):
-    if isinstance(np_array, (np.ndarray)):
+    if isinstance(input, (np.ndarray)):
-        return np_array.tolist()
+        return input.tolist()
-    elif isinstance(np_array, (SX)):
+    elif isinstance(input, (SX)):
-        return DM(np_array).full()
+        return input.serialize()
-    elif isinstance(np_array, (DM)):
+    elif isinstance(input, (MX)):
-        return np_array.full()
+        # 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):
+def get_default_simulink_opts() -> dict:
-    """
+    python_interface_path = get_python_interface_path()
-    Check dimensions against layout
+    abs_path = os.path.join(python_interface_path, 'simulink_default_opts.json')
-    Parameters
+    with open(abs_path, 'r') as f:
-    ---------
+        simulink_opts = json.load(f)
-    ocp_nlp : dict
+    return simulink_opts
-        dictionary loaded from JSON to be post-processed.
-
-    ocp_dims : instance of AcadosOcpDims
-    """
-
-    ocp_nlp_layout = get_ocp_nlp_layout()
-
-    ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, ocp_nlp_layout)
-    return
-
-
-def ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, layout):
-
-    for key, item in ocp_nlp.items():
-
-        try:
-            layout_of_key = layout[key]
-        except KeyError:
-            raise Exception("ocp_check_against_layout_recursion: field" \
-                            " '{0}' is not in layout but in OCP description.".format(key))
-
-        if isinstance(item, dict):
-            ocp_check_against_layout_recursion(item, ocp_dims, layout_of_key)
-
-        if 'ndarray' in layout_of_key:
-            if isinstance(item, int) or isinstance(item, float):
-                item = np.array([item])
-        if isinstance(item, (list, np.ndarray)) and (layout_of_key[0] != 'str'):
-            dim_layout = []
-            dim_names = layout_of_key[1]
-
-            for dim_name in dim_names:
-                dim_layout.append(ocp_dims[dim_name])
-
-            dims = tuple(dim_layout)
-
-            item = np.array(item)
-            item_dims = item.shape
-            if len(item_dims) != len(dims):
-                raise Exception('Mismatching dimensions for field {0}. ' \
-                    'Expected {1} dimensional array, got {2} dimensional array.' \
-                        .format(key, len(dims), len(item_dims)))
-
-            if np.prod(item_dims) != 0 or np.prod(dims) != 0:
-                if dims != item_dims:
-                    raise Exception('acados -- mismatching dimensions for field {0}. ' \
-                        'Provided data has dimensions {1}, ' \
-                        'while associated dimensions {2} are {3}' \
-                            .format(key, item_dims, dim_names, dims))
-    return
 
 
 def J_to_idx(J):
@@ -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_ocp.dims.gnsf_nx1 = size_gnsf_A[1]
-    acados_formulation.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
+    acados_ocp.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
-    acados_formulation.dims.gnsf_nuhat = max(phi_fun.size_in(1))
+    acados_ocp.dims.gnsf_nuhat = max(phi_fun.size_in(1))
-    acados_formulation.dims.gnsf_ny = max(phi_fun.size_in(0))
+    acados_ocp.dims.gnsf_ny = max(phi_fun.size_in(0))
-    acados_formulation.dims.gnsf_nout = max(phi_fun.size_out(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_ocp.model.phi_fun = phi_fun
-    acados_formulation.model.phi_fun_jac_y = phi_fun_jac_y
+    acados_ocp.model.phi_fun_jac_y = phi_fun_jac_y
-    acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat
+    acados_ocp.model.phi_jac_y_uhat = phi_jac_y_uhat
-    acados_formulation.model.get_matrices_fun = get_matrices_fun
+    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_ocp.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['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_x1 = SX.sym('dummy_var_x1', acados_ocp.dims.gnsf_nx1)
-        dummy_var_x1dot = SX.sym('dummy_var_x1dot', acados_formulation.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_formulation.dims.gnsf_nz1)
+        dummy_var_z1 = SX.sym('dummy_var_z1', acados_ocp.dims.gnsf_nz1)
-        dummy_var_u = SX.sym('dummy_var_z1', acados_formulation.dims.nu)
+        dummy_var_u = SX.sym('dummy_var_z1', acados_ocp.dims.nu)
-        dummy_var_p = SX.sym('dummy_var_z1', acados_formulation.dims.np)
+        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,:])

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__

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 checkout 8af9b0ad180940ef611884574a0b27a43504311d # v0.2.2
-git submodule update --recursive --init
+git submodule update --depth=1 --recursive --init
 
 # build
 mkdir -p build

third_party/acados/include/acados/dense_qp/dense_qp_common.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *

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_

third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *

third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *

third_party/acados/include/acados/dense_qp/dense_qp_qore.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *

third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *

third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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);

third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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);
 
 
 /************************************************

third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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 */
 

third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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);

third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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;
 
 //

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_
+/// @}
+/// @}
+/// @}

third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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);
@@ -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);
 //

third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -90,22 +87,6 @@ acados_size_t ocp_nlp_cost_ls_dims_calculate_size(void *config);
 ///  \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);
 //
@@ -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;
 
 //
@@ -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);
 //

third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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,10 +140,10 @@ 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
+    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);
 //

third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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 */

third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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);
 

third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *
@@ -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);
 

third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *

third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *

third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *

third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *

third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h

@@ -1,8 +1,5 @@
 /*
- * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Copyright (c) The acados authors.
- * 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.
  *