diff --git a/pyextra/acados_template/acados_layout.json b/pyextra/acados_template/acados_layout.json
index 16fd45233..59aa13aa8 100644
--- a/pyextra/acados_template/acados_layout.json
+++ b/pyextra/acados_template/acados_layout.json
@@ -5,6 +5,9 @@
     "acados_include_path": [
         "str"
     ],
+    "cython_include_dirs": [
+        "str"
+    ],
     "model": {
          "name" : [
             "str"
@@ -23,7 +26,15 @@
         ],
         "dyn_disc_fun" : [
             "str"
-        ]
+        ],
+        "gnsf" : {
+            "nontrivial_f_LO": [
+                "int"
+            ],
+            "purely_linear": [
+                "int"
+            ]
+        }
     },
     "parameter_values": [
         "ndarray",
@@ -693,6 +704,18 @@
         "alpha_reduction": [
             "float"
         ],
+        "line_search_use_sufficient_descent": [
+            "int"
+        ],
+        "globalization_use_SOC": [
+            "int"
+        ],
+        "full_step_dual": [
+            "int"
+        ],
+        "eps_sufficient_descent": [
+            "float"
+        ],
         "sim_method_num_stages": [
             "ndarray",
             [
diff --git a/pyextra/acados_template/acados_model.py b/pyextra/acados_template/acados_model.py
index 4871a2c0d..e292cc747 100644
--- a/pyextra/acados_template/acados_model.py
+++ b/pyextra/acados_template/acados_model.py
@@ -78,6 +78,13 @@ class AcadosModel():
         self.dyn_disc_fun_jac = None  #: name of function discrete dyanamics + jacobian; Default: :code:`None`
         self.dyn_disc_fun = None  #: name of function discrete dyanamics; Default: :code:`None`
 
+        # for GNSF models
+        self.gnsf = {'nontrivial_f_LO': 1, 'purely_linear': 0}
+        """
+        dictionary containing information on GNSF structure needed when rendering templates.
+        Contains integers `nontrivial_f_LO`, `purely_linear`.
+        """
+
         ## for OCP
         # constraints
         self.con_h_expr = None  #: CasADi expression for the constraint :math:`h`; Default: :code:`None`
diff --git a/pyextra/acados_template/acados_ocp.py b/pyextra/acados_template/acados_ocp.py
index 198ab033d..8ea129936 100644
--- a/pyextra/acados_template/acados_ocp.py
+++ b/pyextra/acados_template/acados_ocp.py
@@ -270,28 +270,28 @@ class AcadosOcpDims:
 
     @nx.setter
     def nx(self, nx):
-        if type(nx) == int and nx > 0:
+        if isinstance(nx, int) and nx > 0:
             self.__nx = nx
         else:
             raise Exception('Invalid nx value, expected positive integer. Exiting.')
 
     @nz.setter
     def nz(self, nz):
-        if type(nz) == int and nz > -1:
+        if isinstance(nz, int) and nz > -1:
             self.__nz = nz
         else:
             raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
 
     @nu.setter
     def nu(self, nu):
-        if type(nu) == int and nu > -1:
+        if isinstance(nu, int) and nu > -1:
             self.__nu = nu
         else:
             raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
 
     @np.setter
     def np(self, np):
-        if type(np) == int and np > -1:
+        if isinstance(np, int) and np > -1:
             self.__np = np
         else:
             raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
@@ -312,49 +312,49 @@ class AcadosOcpDims:
 
     @ny_e.setter
     def ny_e(self, ny_e):
-        if type(ny_e) == int and ny_e > -1:
+        if isinstance(ny_e, int) and ny_e > -1:
             self.__ny_e = ny_e
         else:
             raise Exception('Invalid ny_e value, expected nonnegative integer. Exiting.')
 
     @nr.setter
     def nr(self, nr):
-        if type(nr) == int and nr > -1:
+        if isinstance(nr, int) and nr > -1:
             self.__nr = nr
         else:
             raise Exception('Invalid nr value, expected nonnegative integer. Exiting.')
 
     @nr_e.setter
     def nr_e(self, nr_e):
-        if type(nr_e) == int and nr_e > -1:
+        if isinstance(nr_e, int) and nr_e > -1:
             self.__nr_e = nr_e
         else:
             raise Exception('Invalid nr_e value, expected nonnegative integer. Exiting.')
 
     @nh.setter
     def nh(self, nh):
-        if type(nh) == int and nh > -1:
+        if isinstance(nh, int) and nh > -1:
             self.__nh = nh
         else:
             raise Exception('Invalid nh value, expected nonnegative integer. Exiting.')
 
     @nh_e.setter
     def nh_e(self, nh_e):
-        if type(nh_e) == int and nh_e > -1:
+        if isinstance(nh_e, int) and nh_e > -1:
             self.__nh_e = nh_e
         else:
             raise Exception('Invalid nh_e value, expected nonnegative integer. Exiting.')
 
     @nphi.setter
     def nphi(self, nphi):
-        if type(nphi) == int and nphi > -1:
+        if isinstance(nphi, int) and nphi > -1:
             self.__nphi = nphi
         else:
             raise Exception('Invalid nphi value, expected nonnegative integer. Exiting.')
 
     @nphi_e.setter
     def nphi_e(self, nphi_e):
-        if type(nphi_e) == int and nphi_e > -1:
+        if isinstance(nphi_e, int) and nphi_e > -1:
             self.__nphi_e = nphi_e
         else:
             raise Exception('Invalid nphi_e value, expected nonnegative integer. Exiting.')
@@ -375,42 +375,42 @@ class AcadosOcpDims:
 
     @nbx_0.setter
     def nbx_0(self, nbx_0):
-        if type(nbx_0) == int and nbx_0 > -1:
+        if isinstance(nbx_0, int) and nbx_0 > -1:
             self.__nbx_0 = nbx_0
         else:
             raise Exception('Invalid nbx_0 value, expected nonnegative integer. Exiting.')
 
     @nbx_e.setter
     def nbx_e(self, nbx_e):
-        if type(nbx_e) == int and nbx_e > -1:
+        if isinstance(nbx_e, int) and nbx_e > -1:
             self.__nbx_e = nbx_e
         else:
             raise Exception('Invalid nbx_e value, expected nonnegative integer. Exiting.')
 
     @nbu.setter
     def nbu(self, nbu):
-        if type(nbu) == int and nbu > -1:
+        if isinstance(nbu, int) and nbu > -1:
             self.__nbu = nbu
         else:
             raise Exception('Invalid nbu value, expected nonnegative integer. Exiting.')
 
     @nsbx.setter
     def nsbx(self, nsbx):
-        if type(nsbx) == int and nsbx > -1:
+        if isinstance(nsbx, int) and nsbx > -1:
             self.__nsbx = nsbx
         else:
             raise Exception('Invalid nsbx value, expected nonnegative integer. Exiting.')
 
     @nsbx_e.setter
     def nsbx_e(self, nsbx_e):
-        if type(nsbx_e) == int and nsbx_e > -1:
+        if isinstance(nsbx_e, int) and nsbx_e > -1:
             self.__nsbx_e = nsbx_e
         else:
             raise Exception('Invalid nsbx_e value, expected nonnegative integer. Exiting.')
 
     @nsbu.setter
     def nsbu(self, nsbu):
-        if type(nsbu) == int and nsbu > -1:
+        if isinstance(nsbu, int) and nsbu > -1:
             self.__nsbu = nsbu
         else:
             raise Exception('Invalid nsbu value, expected nonnegative integer. Exiting.')
@@ -1592,14 +1592,14 @@ class AcadosOcpConstraints:
     # initial x
     @lbx_0.setter
     def lbx_0(self, lbx_0):
-        if type(lbx_0) == np.ndarray:
+        if isinstance(lbx_0, np.ndarray):
             self.__lbx_0 = lbx_0
         else:
             raise Exception('Invalid lbx_0 value. Exiting.')
 
     @ubx_0.setter
     def ubx_0(self, ubx_0):
-        if type(ubx_0) == np.ndarray:
+        if isinstance(ubx_0, np.ndarray):
             self.__ubx_0 = ubx_0
         else:
             raise Exception('Invalid ubx_0 value. Exiting.')
@@ -1613,7 +1613,7 @@ class AcadosOcpConstraints:
 
     @Jbx_0.setter
     def Jbx_0(self, Jbx_0):
-        if type(Jbx_0) == np.ndarray:
+        if isinstance(Jbx_0, np.ndarray):
             self.__idxbx_0 = J_to_idx(Jbx_0)
         else:
             raise Exception('Invalid Jbx_0 value. Exiting.')
@@ -1639,28 +1639,28 @@ class AcadosOcpConstraints:
     # bounds on x
     @lbx.setter
     def lbx(self, lbx):
-        if type(lbx) == np.ndarray:
+        if isinstance(lbx, np.ndarray):
             self.__lbx = lbx
         else:
             raise Exception('Invalid lbx value. Exiting.')
 
     @ubx.setter
     def ubx(self, ubx):
-        if type(ubx) == np.ndarray:
+        if isinstance(ubx, np.ndarray):
             self.__ubx = ubx
         else:
             raise Exception('Invalid ubx value. Exiting.')
 
     @idxbx.setter
     def idxbx(self, idxbx):
-        if type(idxbx) == np.ndarray:
+        if isinstance(idxbx, np.ndarray):
             self.__idxbx = idxbx
         else:
             raise Exception('Invalid idxbx value. Exiting.')
 
     @Jbx.setter
     def Jbx(self, Jbx):
-        if type(Jbx) == np.ndarray:
+        if isinstance(Jbx, np.ndarray):
             self.__idxbx = J_to_idx(Jbx)
         else:
             raise Exception('Invalid Jbx value. Exiting.')
@@ -1668,28 +1668,28 @@ class AcadosOcpConstraints:
     # bounds on u
     @lbu.setter
     def lbu(self, lbu):
-        if type(lbu) == np.ndarray:
+        if isinstance(lbu, np.ndarray):
             self.__lbu = lbu
         else:
             raise Exception('Invalid lbu value. Exiting.')
 
     @ubu.setter
     def ubu(self, ubu):
-        if type(ubu) == np.ndarray:
+        if isinstance(ubu, np.ndarray):
             self.__ubu = ubu
         else:
             raise Exception('Invalid ubu value. Exiting.')
 
     @idxbu.setter
     def idxbu(self, idxbu):
-        if type(idxbu) == np.ndarray:
+        if isinstance(idxbu, np.ndarray):
             self.__idxbu = idxbu
         else:
             raise Exception('Invalid idxbu value. Exiting.')
 
     @Jbu.setter
     def Jbu(self, Jbu):
-        if type(Jbu) == np.ndarray:
+        if isinstance(Jbu, np.ndarray):
             self.__idxbu = J_to_idx(Jbu)
         else:
             raise Exception('Invalid Jbu value. Exiting.')
@@ -1697,28 +1697,28 @@ class AcadosOcpConstraints:
     # bounds on x at shooting node N
     @lbx_e.setter
     def lbx_e(self, lbx_e):
-        if type(lbx_e) == np.ndarray:
+        if isinstance(lbx_e, np.ndarray):
             self.__lbx_e = lbx_e
         else:
             raise Exception('Invalid lbx_e value. Exiting.')
 
     @ubx_e.setter
     def ubx_e(self, ubx_e):
-        if type(ubx_e) == np.ndarray:
+        if isinstance(ubx_e, np.ndarray):
             self.__ubx_e = ubx_e
         else:
             raise Exception('Invalid ubx_e value. Exiting.')
 
     @idxbx_e.setter
     def idxbx_e(self, idxbx_e):
-        if type(idxbx_e) == np.ndarray:
+        if isinstance(idxbx_e, np.ndarray):
             self.__idxbx_e = idxbx_e
         else:
             raise Exception('Invalid idxbx_e value. Exiting.')
 
     @Jbx_e.setter
     def Jbx_e(self, Jbx_e):
-        if type(Jbx_e) == np.ndarray:
+        if isinstance(Jbx_e, np.ndarray):
             self.__idxbx_e = J_to_idx(Jbx_e)
         else:
             raise Exception('Invalid Jbx_e value. Exiting.')
@@ -1742,14 +1742,14 @@ class AcadosOcpConstraints:
 
     @lg.setter
     def lg(self, lg):
-        if type(lg) == np.ndarray:
+        if isinstance(lg, np.ndarray):
             self.__lg = lg
         else:
             raise Exception('Invalid lg value. Exiting.')
 
     @ug.setter
     def ug(self, ug):
-        if type(ug) == np.ndarray:
+        if isinstance(ug, np.ndarray):
             self.__ug = ug
         else:
             raise Exception('Invalid ug value. Exiting.')
@@ -1765,14 +1765,14 @@ class AcadosOcpConstraints:
 
     @lg_e.setter
     def lg_e(self, lg_e):
-        if type(lg_e) == np.ndarray:
+        if isinstance(lg_e, np.ndarray):
             self.__lg_e = lg_e
         else:
             raise Exception('Invalid lg_e value. Exiting.')
 
     @ug_e.setter
     def ug_e(self, ug_e):
-        if type(ug_e) == np.ndarray:
+        if isinstance(ug_e, np.ndarray):
             self.__ug_e = ug_e
         else:
             raise Exception('Invalid ug_e value. Exiting.')
@@ -1780,14 +1780,14 @@ class AcadosOcpConstraints:
     # nonlinear constraints
     @lh.setter
     def lh(self, lh):
-        if type(lh) == np.ndarray:
+        if isinstance(lh, np.ndarray):
             self.__lh = lh
         else:
             raise Exception('Invalid lh value. Exiting.')
 
     @uh.setter
     def uh(self, uh):
-        if type(uh) == np.ndarray:
+        if isinstance(uh, np.ndarray):
             self.__uh = uh
         else:
             raise Exception('Invalid uh value. Exiting.')
@@ -1795,14 +1795,14 @@ class AcadosOcpConstraints:
     # convex-over-nonlinear constraints
     @lphi.setter
     def lphi(self, lphi):
-        if type(lphi) == np.ndarray:
+        if isinstance(lphi, np.ndarray):
             self.__lphi = lphi
         else:
             raise Exception('Invalid lphi value. Exiting.')
 
     @uphi.setter
     def uphi(self, uphi):
-        if type(uphi) == np.ndarray:
+        if isinstance(uphi, np.ndarray):
             self.__uphi = uphi
         else:
             raise Exception('Invalid uphi value. Exiting.')
@@ -1810,14 +1810,14 @@ class AcadosOcpConstraints:
     # nonlinear constraints at shooting node N
     @lh_e.setter
     def lh_e(self, lh_e):
-        if type(lh_e) == np.ndarray:
+        if isinstance(lh_e, np.ndarray):
             self.__lh_e = lh_e
         else:
             raise Exception('Invalid lh_e value. Exiting.')
 
     @uh_e.setter
     def uh_e(self, uh_e):
-        if type(uh_e) == np.ndarray:
+        if isinstance(uh_e, np.ndarray):
             self.__uh_e = uh_e
         else:
             raise Exception('Invalid uh_e value. Exiting.')
@@ -1825,14 +1825,14 @@ class AcadosOcpConstraints:
     # convex-over-nonlinear constraints at shooting node N
     @lphi_e.setter
     def lphi_e(self, lphi_e):
-        if type(lphi_e) == np.ndarray:
+        if isinstance(lphi_e, np.ndarray):
             self.__lphi_e = lphi_e
         else:
             raise Exception('Invalid lphi_e value. Exiting.')
 
     @uphi_e.setter
     def uphi_e(self, uphi_e):
-        if type(uphi_e) == np.ndarray:
+        if isinstance(uphi_e, np.ndarray):
             self.__uphi_e = uphi_e
         else:
             raise Exception('Invalid uphi_e value. Exiting.')
@@ -1841,21 +1841,21 @@ class AcadosOcpConstraints:
     # soft bounds on x
     @lsbx.setter
     def lsbx(self, lsbx):
-        if type(lsbx) == np.ndarray:
+        if isinstance(lsbx, np.ndarray):
             self.__lsbx = lsbx
         else:
             raise Exception('Invalid lsbx value. Exiting.')
 
     @usbx.setter
     def usbx(self, usbx):
-        if type(usbx) == np.ndarray:
+        if isinstance(usbx, np.ndarray):
             self.__usbx = usbx
         else:
             raise Exception('Invalid usbx value. Exiting.')
 
     @idxsbx.setter
     def idxsbx(self, idxsbx):
-        if type(idxsbx) == np.ndarray:
+        if isinstance(idxsbx, np.ndarray):
             self.__idxsbx = idxsbx
         else:
             raise Exception('Invalid idxsbx value. Exiting.')
@@ -1870,28 +1870,28 @@ class AcadosOcpConstraints:
     # soft bounds on u
     @lsbu.setter
     def lsbu(self, lsbu):
-        if type(lsbu) == np.ndarray:
+        if isinstance(lsbu, np.ndarray):
             self.__lsbu = lsbu
         else:
             raise Exception('Invalid lsbu value. Exiting.')
 
     @usbu.setter
     def usbu(self, usbu):
-        if type(usbu) == np.ndarray:
+        if isinstance(usbu, np.ndarray):
             self.__usbu = usbu
         else:
             raise Exception('Invalid usbu value. Exiting.')
 
     @idxsbu.setter
     def idxsbu(self, idxsbu):
-        if type(idxsbu) == np.ndarray:
+        if isinstance(idxsbu, np.ndarray):
             self.__idxsbu = idxsbu
         else:
             raise Exception('Invalid idxsbu value. Exiting.')
 
     @Jsbu.setter
     def Jsbu(self, Jsbu):
-        if type(Jsbu) == np.ndarray:
+        if isinstance(Jsbu, np.ndarray):
             self.__idxsbu = J_to_idx_slack(Jsbu)
         else:
             raise Exception('Invalid Jsbu value. Exiting.')
@@ -1899,28 +1899,28 @@ class AcadosOcpConstraints:
     # soft bounds on x at shooting node N
     @lsbx_e.setter
     def lsbx_e(self, lsbx_e):
-        if type(lsbx_e) == np.ndarray:
+        if isinstance(lsbx_e, np.ndarray):
             self.__lsbx_e = lsbx_e
         else:
             raise Exception('Invalid lsbx_e value. Exiting.')
 
     @usbx_e.setter
     def usbx_e(self, usbx_e):
-        if type(usbx_e) == np.ndarray:
+        if isinstance(usbx_e, np.ndarray):
             self.__usbx_e = usbx_e
         else:
             raise Exception('Invalid usbx_e value. Exiting.')
 
     @idxsbx_e.setter
     def idxsbx_e(self, idxsbx_e):
-        if type(idxsbx_e) == np.ndarray:
+        if isinstance(idxsbx_e, np.ndarray):
             self.__idxsbx_e = idxsbx_e
         else:
             raise Exception('Invalid idxsbx_e value. Exiting.')
 
     @Jsbx_e.setter
     def Jsbx_e(self, Jsbx_e):
-        if type(Jsbx_e) == np.ndarray:
+        if isinstance(Jsbx_e, np.ndarray):
             self.__idxsbx_e = J_to_idx_slack(Jsbx_e)
         else:
             raise Exception('Invalid Jsbx_e value. Exiting.')
@@ -1959,21 +1959,21 @@ class AcadosOcpConstraints:
     # soft bounds on nonlinear constraints
     @lsh.setter
     def lsh(self, lsh):
-        if type(lsh) == np.ndarray:
+        if isinstance(lsh, np.ndarray):
             self.__lsh = lsh
         else:
             raise Exception('Invalid lsh value. Exiting.')
 
     @ush.setter
     def ush(self, ush):
-        if type(ush) == np.ndarray:
+        if isinstance(ush, np.ndarray):
             self.__ush = ush
         else:
             raise Exception('Invalid ush value. Exiting.')
 
     @idxsh.setter
     def idxsh(self, idxsh):
-        if type(idxsh) == np.ndarray:
+        if isinstance(idxsh, np.ndarray):
             self.__idxsh = idxsh
         else:
             raise Exception('Invalid idxsh value. Exiting.')
@@ -1989,21 +1989,21 @@ class AcadosOcpConstraints:
     # soft bounds on convex-over-nonlinear constraints
     @lsphi.setter
     def lsphi(self, lsphi):
-        if type(lsphi) == np.ndarray:
+        if isinstance(lsphi, np.ndarray):
             self.__lsphi = lsphi
         else:
             raise Exception('Invalid lsphi value. Exiting.')
 
     @usphi.setter
     def usphi(self, usphi):
-        if type(usphi) == np.ndarray:
+        if isinstance(usphi, np.ndarray):
             self.__usphi = usphi
         else:
             raise Exception('Invalid usphi value. Exiting.')
 
     @idxsphi.setter
     def idxsphi(self, idxsphi):
-        if type(idxsphi) == np.ndarray:
+        if isinstance(idxsphi, np.ndarray):
             self.__idxsphi = idxsphi
         else:
             raise Exception('Invalid idxsphi value. Exiting.')
@@ -2151,6 +2151,10 @@ class AcadosOcpOptions:
         self.__ext_cost_num_hess = 0
         self.__alpha_min = 0.05
         self.__alpha_reduction = 0.7
+        self.__line_search_use_sufficient_descent = 0
+        self.__globalization_use_SOC = 0
+        self.__full_step_dual = 0
+        self.__eps_sufficient_descent = 1e-4
 
 
     @property
@@ -2367,6 +2371,43 @@ class AcadosOcpOptions:
         """Step size reduction factor for globalization MERIT_BACKTRACKING, default: 0.7."""
         return self.__alpha_reduction
 
+    @property
+    def line_search_use_sufficient_descent(self):
+        """
+        Determines if sufficient descent (Armijo) condition is used in line search.
+        Type: int; 0 or 1;
+        default: 0.
+        """
+        return self.__line_search_use_sufficient_descent
+
+    @property
+    def eps_sufficient_descent(self):
+        """
+        Factor for sufficient descent (Armijo) conditon, see line_search_use_sufficient_descent.
+        Type: float,
+        default: 1e-4.
+        """
+        return self.__eps_sufficient_descent
+
+    @property
+    def globalization_use_SOC(self):
+        """
+        Determines if second order correction (SOC) is done when using MERIT_BACKTRACKING.
+        SOC is done if preliminary line search does not return full step.
+        Type: int; 0 or 1;
+        default: 0.
+        """
+        return self.__globalization_use_SOC
+
+    @property
+    def full_step_dual(self):
+        """
+        Determines if dual variables are updated with full steps (alpha=1.0) when primal variables are updated with smaller step.
+        Type: int; 0 or 1;
+        default: 0.
+        """
+        return self.__full_step_dual
+
     @property
     def nlp_solver_tol_ineq(self):
         """NLP solver inequality tolerance"""
@@ -2524,12 +2565,23 @@ class AcadosOcpOptions:
 
     @time_steps.setter
     def time_steps(self, time_steps):
-        self.__time_steps = time_steps
+        if isinstance(time_steps, np.ndarray):
+            if len(time_steps.shape) == 1:
+                    self.__time_steps = time_steps
+            else:
+                raise Exception('Invalid time_steps, expected np.ndarray of shape (N,).')
+        else:
+            raise Exception('Invalid time_steps, expected np.ndarray.')
 
     @shooting_nodes.setter
     def shooting_nodes(self, shooting_nodes):
-        self.__shooting_nodes = shooting_nodes
-
+        if isinstance(shooting_nodes, np.ndarray):
+            if len(shooting_nodes.shape) == 1:
+                self.__shooting_nodes = shooting_nodes
+            else:
+                raise Exception('Invalid shooting_nodes, expected np.ndarray of shape (N+1,).')
+        else:
+            raise Exception('Invalid shooting_nodes, expected np.ndarray.')
 
     @Tsim.setter
     def Tsim(self, Tsim):
@@ -2537,7 +2589,12 @@ class AcadosOcpOptions:
 
     @globalization.setter
     def globalization(self, globalization):
-        self.__globalization = globalization
+        globalization_types = ('MERIT_BACKTRACKING', 'FIXED_STEP')
+        if globalization in globalization_types:
+            self.__globalization = globalization
+        else:
+            raise Exception('Invalid globalization value. Possible values are:\n\n' \
+                    + ',\n'.join(globalization_types) + '.\n\nYou have: ' + globalization + '.\n\nExiting.')
 
     @alpha_min.setter
     def alpha_min(self, alpha_min):
@@ -2547,10 +2604,38 @@ class AcadosOcpOptions:
     def alpha_reduction(self, alpha_reduction):
         self.__alpha_reduction = alpha_reduction
 
+    @line_search_use_sufficient_descent.setter
+    def line_search_use_sufficient_descent(self, line_search_use_sufficient_descent):
+        if line_search_use_sufficient_descent in [0, 1]:
+            self.__line_search_use_sufficient_descent = line_search_use_sufficient_descent
+        else:
+            raise Exception(f'Invalid value for line_search_use_sufficient_descent. Possible values are 0, 1, got {line_search_use_sufficient_descent}')
+
+    @globalization_use_SOC.setter
+    def globalization_use_SOC(self, globalization_use_SOC):
+        if globalization_use_SOC in [0, 1]:
+            self.__globalization_use_SOC = globalization_use_SOC
+        else:
+            raise Exception(f'Invalid value for globalization_use_SOC. Possible values are 0, 1, got {globalization_use_SOC}')
+
+    @full_step_dual.setter
+    def full_step_dual(self, full_step_dual):
+        if full_step_dual in [0, 1]:
+            self.__full_step_dual = full_step_dual
+        else:
+            raise Exception(f'Invalid value for full_step_dual. Possible values are 0, 1, got {full_step_dual}')
+
+    @eps_sufficient_descent.setter
+    def eps_sufficient_descent(self, eps_sufficient_descent):
+        if isinstance(eps_sufficient_descent, float) and eps_sufficient_descent > 0:
+            self.__eps_sufficient_descent = eps_sufficient_descent
+        else:
+            raise Exception('Invalid eps_sufficient_descent value. eps_sufficient_descent must be a positive float. Exiting')
+
     @sim_method_num_stages.setter
     def sim_method_num_stages(self, sim_method_num_stages):
 
-        # if type(sim_method_num_stages) == int:
+        # if isinstance(sim_method_num_stages, int):
         #     self.__sim_method_num_stages = sim_method_num_stages
         # else:
         #     raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer. Exiting.')
@@ -2560,7 +2645,7 @@ class AcadosOcpOptions:
     @sim_method_num_steps.setter
     def sim_method_num_steps(self, sim_method_num_steps):
 
-        # if type(sim_method_num_steps) == int:
+        # if isinstance(sim_method_num_steps, int):
         #     self.__sim_method_num_steps = sim_method_num_steps
         # else:
         #     raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer. Exiting.')
@@ -2570,7 +2655,7 @@ class AcadosOcpOptions:
     @sim_method_newton_iter.setter
     def sim_method_newton_iter(self, sim_method_newton_iter):
 
-        if type(sim_method_newton_iter) == int:
+        if isinstance(sim_method_newton_iter, int):
             self.__sim_method_newton_iter = sim_method_newton_iter
         else:
             raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer. Exiting.')
@@ -2593,7 +2678,7 @@ class AcadosOcpOptions:
 
     @nlp_solver_step_length.setter
     def nlp_solver_step_length(self, nlp_solver_step_length):
-        if type(nlp_solver_step_length) == float and nlp_solver_step_length > 0:
+        if isinstance(nlp_solver_step_length, float) and nlp_solver_step_length > 0:
             self.__nlp_solver_step_length = nlp_solver_step_length
         else:
             raise Exception('Invalid nlp_solver_step_length value. nlp_solver_step_length must be a positive float. Exiting')
@@ -2614,7 +2699,7 @@ class AcadosOcpOptions:
 
     @qp_solver_cond_N.setter
     def qp_solver_cond_N(self, qp_solver_cond_N):
-        if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N > 0:
+        if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N >= 0:
             self.__qp_solver_cond_N = qp_solver_cond_N
         else:
             raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int. Exiting')
@@ -2705,21 +2790,21 @@ class AcadosOcpOptions:
     @nlp_solver_max_iter.setter
     def nlp_solver_max_iter(self, nlp_solver_max_iter):
 
-        if type(nlp_solver_max_iter) == int and nlp_solver_max_iter > 0:
+        if isinstance(nlp_solver_max_iter, int) and nlp_solver_max_iter > 0:
             self.__nlp_solver_max_iter = nlp_solver_max_iter
         else:
             raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a positive int. Exiting')
 
     @print_level.setter
     def print_level(self, print_level):
-        if type(print_level) == int and print_level >= 0:
+        if isinstance(print_level, int) and print_level >= 0:
             self.__print_level = print_level
         else:
             raise Exception('Invalid print_level value. print_level takes one of the values >=0. Exiting')
 
     @model_external_shared_lib_dir.setter
     def model_external_shared_lib_dir(self, model_external_shared_lib_dir):
-        if type(model_external_shared_lib_dir) == str :
+        if isinstance(model_external_shared_lib_dir, str) :
             self.__model_external_shared_lib_dir = model_external_shared_lib_dir
         else:
             raise Exception('Invalid model_external_shared_lib_dir value. Str expected.' \
@@ -2727,7 +2812,7 @@ class AcadosOcpOptions:
 
     @model_external_shared_lib_name.setter
     def model_external_shared_lib_name(self, model_external_shared_lib_name):
-        if type(model_external_shared_lib_name) == str :
+        if isinstance(model_external_shared_lib_name, str) :
             if model_external_shared_lib_name[-3:] == '.so' : 
                 raise Exception('Invalid model_external_shared_lib_name value. Remove the .so extension.' \
             + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.')
@@ -2810,6 +2895,9 @@ class AcadosOcp:
         self.acados_lib_path = f'{acados_path}/lib'
         """Path to where acados library is located, type: string"""
 
+        import numpy
+        self.cython_include_dirs = numpy.get_include()
+
         self.__parameter_values = np.array([])
         self.__problem_class = 'OCP'
 
diff --git a/pyextra/acados_template/acados_ocp_solver.py b/pyextra/acados_template/acados_ocp_solver.py
index b34b8fa84..6d1ff9869 100644
--- a/pyextra/acados_template/acados_ocp_solver.py
+++ b/pyextra/acados_template/acados_ocp_solver.py
@@ -37,7 +37,7 @@ import os
 import json
 import numpy as np
 from datetime import datetime
-import ctypes
+import importlib
 from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_int64, byref
 
 from copy import deepcopy
@@ -51,9 +51,9 @@ from .generate_c_code_nls_cost import generate_c_code_nls_cost
 from .generate_c_code_external_cost import generate_c_code_external_cost
 from .acados_ocp import AcadosOcp
 from .acados_model import acados_model_strip_casadi_symbolics
-from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
+from .utils import is_column, is_empty, casadi_length, render_template,\
      format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
-     set_up_imported_gnsf_model, get_acados_path, get_ocp_nlp_layout, get_python_interface_path
+     set_up_imported_gnsf_model, get_ocp_nlp_layout, get_python_interface_path
 
 
 def make_ocp_dims_consistent(acados_ocp):
@@ -90,7 +90,7 @@ def make_ocp_dims_consistent(acados_ocp):
         raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \
             f'\nGot np = {dims.np}, acados_ocp.parameter_values.shape = {acados_ocp.parameter_values.shape[0]}\n')
 
-    # cost
+    ## cost
     # initial stage - if not set, copy fields from path constraints
     if cost.cost_type_0 is None:
         cost.cost_type_0 = cost.cost_type
@@ -434,18 +434,14 @@ def make_ocp_dims_consistent(acados_ocp):
         if np.shape(opts.shooting_nodes)[0] != dims.N+1:
             raise Exception('inconsistent dimension N, regarding shooting_nodes.')
 
-        # time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1]
-        # # identify constant time-steps: due to numerical reasons the content of time_steps might vary a bit
-        # delta_time_steps = time_steps[1:] - time_steps[0:-1]
-        # avg_time_steps = np.average(time_steps)
-        # # criterion for constant time-step detection: the min/max difference in values normalized by the average
-        # check_const_time_step = np.max(delta_time_steps)-np.min(delta_time_steps) / avg_time_steps
-        # # if the criterion is small, we have a constant time-step
-        # if check_const_time_step < 1e-9:
-        #     time_steps[:] = avg_time_steps  # if we have a constant time-step: apply the average time-step
-        time_steps = np.zeros((dims.N,))
-        for i in range(dims.N):
-            time_steps[i] = opts.shooting_nodes[i+1] - opts.shooting_nodes[i] # TODO use commented code above
+        time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1]
+        # identify constant time_steps: due to numerical reasons the content of time_steps might vary a bit
+        avg_time_steps = np.average(time_steps)
+        # criterion for constant time step detection: the min/max difference in values normalized by the average
+        check_const_time_step = (np.max(time_steps)-np.min(time_steps)) / avg_time_steps
+        # if the criterion is small, we have a constant time_step
+        if check_const_time_step < 1e-9:
+            time_steps[:] = avg_time_steps  # if we have a constant time_step: apply the average time_step
 
         opts.time_steps = time_steps
 
@@ -525,8 +521,7 @@ def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_n
 
     # strip shooting_nodes
     ocp_nlp_dict['solver_options'].pop('shooting_nodes', None)
-
-    dims_dict = acados_class2dict(acados_ocp.dims)
+    dims_dict = format_class_dict(acados_ocp.dims.__dict__)
 
     ocp_check_against_layout(ocp_nlp_dict, dims_dict)
 
@@ -782,8 +777,15 @@ class AcadosOcpSolver:
         dlclose.argtypes = [c_void_p]
 
     @classmethod
-    def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True):
+    def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None):
+        """
+        Generates the code for an acados OCP solver, given the description in acados_ocp.
+            :param acados_ocp: type AcadosOcp - description of the OCP for acados
+            :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json
+            :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs
+        """
         model = acados_ocp.model
+        acados_ocp.code_export_directory = os.path.abspath(acados_ocp.code_export_directory)
 
         if simulink_opts is None:
             simulink_opts = get_simulink_default_opts()
@@ -807,24 +809,91 @@ class AcadosOcpSolver:
         # dump to json
         ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file)
 
-        code_export_dir = acados_ocp.code_export_directory
         # render templates
         ocp_render_templates(acados_ocp, json_file)
+        acados_ocp.json_file = json_file
 
-        if build:
-          ## Compile solver
-          cwd=os.getcwd()
-          os.chdir(code_export_dir)
-          os.system('make clean_ocp_shared_lib')
-          os.system('make ocp_shared_lib')
-          os.chdir(cwd)
 
-    def __init__(self, model_name, N, code_export_dir):
-        self.model_name = model_name
-        self.N = N
+    @classmethod
+    def build(cls, code_export_dir, with_cython=False):
+        """
+        Builds the code for an acados OCP solver, that has been generated in code_export_dir
+            :param code_export_dir: directory in which acados OCP solver has been generated, see generate()
+            :param with_cython: option indicating if the cython interface is build, default: False.
+        """
+        cwd=os.getcwd()
+        os.chdir(code_export_dir)
+        if with_cython:
+            os.system('make clean_ocp_cython')
+            os.system('make ocp_cython')
+        else:
+            os.system('make clean_ocp_shared_lib')
+            os.system('make ocp_shared_lib')
+        os.chdir(cwd)
+
+
+    @classmethod
+    def create_cython_solver(cls, json_file):
+        """
+        Returns an `AcadosOcpSolverCython` object.
+
+        This is an alternative Cython based Python wrapper to the acados OCP solver in C.
+        This offers faster interaction with the solver, because getter and setter calls, which include shape checking are done in compiled C code.
+
+        The default wrapper `AcadosOcpSolver` is based on ctypes.
+        """
+        with open(json_file, 'r') as f:
+            acados_ocp_json = json.load(f)
+        code_export_directory = acados_ocp_json['code_export_directory']
+
+        importlib.invalidate_caches()
+        rel_code_export_directory = os.path.relpath(code_export_directory)
+        acados_ocp_solver_pyx = importlib.import_module(f'{rel_code_export_directory}.acados_ocp_solver_pyx')
+
+        AcadosOcpSolverCython = getattr(acados_ocp_solver_pyx, 'AcadosOcpSolverCython')
+        return AcadosOcpSolverCython(acados_ocp_json['model']['name'],
+                    acados_ocp_json['solver_options']['nlp_solver_type'],
+                    acados_ocp_json['dims']['N'])
+
+
+    def __init__(self, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True, generate=True):
 
         self.solver_created = False
-        self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{self.model_name}.so'
+        if generate:
+            self.generate(acados_ocp, json_file=json_file, simulink_opts=simulink_opts)
+
+        # load json, store options in object
+        with open(json_file, 'r') as f:
+            acados_ocp_json = json.load(f)
+        self.N = acados_ocp_json['dims']['N']
+        self.model_name = acados_ocp_json['model']['name']
+        self.solver_options = acados_ocp_json['solver_options']
+
+        acados_lib_path = acados_ocp_json['acados_lib_path']
+        code_export_directory = acados_ocp_json['code_export_directory']
+
+        if build:
+            self.build(code_export_directory, with_cython=False)
+
+        # Load acados library to avoid unloading the library.
+        # This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed.
+        # Unloading a library which uses OpenMP results in a segfault (on any platform?).
+        # see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp]
+        # or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html]
+        libacados_name = 'libacados.so'
+        libacados_filepath = os.path.join(acados_lib_path, libacados_name)
+        self.__acados_lib = CDLL(libacados_filepath)
+        # find out if acados was compiled with OpenMP
+        try:
+            self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None
+        except AttributeError as e:
+            self.__acados_lib_uses_omp = False
+        if self.__acados_lib_uses_omp:
+            print('acados was compiled with OpenMP.')
+        else:
+            print('acados was compiled without OpenMP.')
+
+        self.shared_lib_name = f'{code_export_directory}/libacados_ocp_solver_{self.model_name}.so'
 
         # get shared_lib
         self.shared_lib = CDLL(self.shared_lib_name)
@@ -842,6 +911,8 @@ class AcadosOcpSolver:
         # get pointers solver
         self.__get_pointers_solver()
 
+        self.status = 0
+
 
     def __get_pointers_solver(self):
         """
@@ -864,6 +935,10 @@ class AcadosOcpSolver:
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out").restype = c_void_p
         self.nlp_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out")(self.capsule)
 
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out").restype = c_void_p
+        self.sens_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_sens_out")(self.capsule)
+
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").argtypes = [c_void_p]
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").restype = c_void_p
         self.nlp_in = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in")(self.capsule)
@@ -872,46 +947,26 @@ class AcadosOcpSolver:
         getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").restype = c_void_p
         self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver")(self.capsule)
 
-        # treat parameters separately
-        getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
-        getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int
-        self._set_param = getattr(self.shared_lib, f"{self.model_name}_acados_update_params")
-
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
-
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
-
-        self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-        self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-        self.shared_lib.ocp_nlp_out_set.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-        self.shared_lib.ocp_nlp_set.argtypes = \
-            [c_void_p, c_void_p, c_int, c_char_p, c_void_p]
 
     def solve(self):
         """
         Solve the ocp with current input.
         """
-
         getattr(self.shared_lib, f"{self.model_name}_acados_solve").argtypes = [c_void_p]
         getattr(self.shared_lib, f"{self.model_name}_acados_solve").restype = c_int
-        status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule)
-        return status
+        self.status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule)
+
+        return self.status
 
 
     def set_new_time_steps(self, new_time_steps):
         """
-        Set new time steps before solving. Only reload library without code generation but with new time steps.
+        Set new time steps.
+        Recreates the solver if N changes.
 
-            :param new_time_steps: vector of new time steps for the solver
+            :param new_time_steps: 1 dimensional np array of new time steps for the solver
 
-            .. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
+            .. note:: This allows for different use-cases: either set a new size of time_steps or a new distribution of
                       the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
                       do not require a new code export and compilation.
         """
@@ -921,15 +976,14 @@ class AcadosOcpSolver:
             raise Exception('Solver was not yet created!')
 
         # check if time steps really changed in value
-        if np.array_equal(self.acados_ocp.solver_options.time_steps, new_time_steps):
+        if np.array_equal(self.solver_options['time_steps'], new_time_steps):
             return
 
         N = new_time_steps.size
-        model = self.acados_ocp.model
         new_time_steps_data = cast(new_time_steps.ctypes.data, POINTER(c_double))
 
         # check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
-        if self.acados_ocp.solver_options.time_steps.size == N:
+        if len(self.solver_options['time_steps']) == N:
             getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").argtypes = [c_void_p, c_int, c_void_p]
             getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").restype = c_int
             assert getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps")(self.capsule, N, new_time_steps_data) == 0
@@ -941,11 +995,6 @@ class AcadosOcpSolver:
             getattr(self.shared_lib, f"{self.model_name}_acados_free").restype = c_int
             getattr(self.shared_lib, f"{self.model_name}_acados_free")(self.capsule)
 
-            # store N and new time steps
-            self.N = self.acados_ocp.dims.N = N
-            self.acados_ocp.solver_options.time_steps = new_time_steps
-            self.acados_ocp.solver_options.Tsim = self.acados_ocp.solver_options.time_steps[0]
-
             # create solver with new time steps
             getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").argtypes = [c_void_p, c_int, c_void_p]
             getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").restype = c_int
@@ -956,6 +1005,75 @@ class AcadosOcpSolver:
             # get pointers solver
             self.__get_pointers_solver()
 
+        # store time_steps, N
+        self.solver_options['time_steps'] = new_time_steps
+        self.N = N
+        self.solver_options['Tsim'] = self.solver_options['time_steps'][0]
+
+
+    def update_qp_solver_cond_N(self, qp_solver_cond_N: int):
+        """
+        Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver.
+        This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or
+        the influence of a different `qp_solver_cond_N` is studied without code export and compilation.
+            :param qp_solver_cond_N: new number of condensing stages for the solver
+
+            .. note:: This function can only be used in combination with a partial condensing QP solver.
+
+            .. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be
+                      necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically
+                      `qp_solver_cond_N < N`.
+        """
+        # unlikely but still possible
+        if not self.solver_created:
+            raise Exception('Solver was not yet created!')
+        if self.N < qp_solver_cond_N:
+            raise Exception('Setting qp_solver_cond_N to be larger than N does not work!')
+        if self.solver_options['qp_solver_cond_N'] != qp_solver_cond_N:
+            self.solver_created = False
+
+            # recreate the solver
+            fun_name = f'{self.model_name}_acados_update_qp_solver_cond_N'
+            getattr(self.shared_lib, fun_name).argtypes = [c_void_p, c_int]
+            getattr(self.shared_lib, fun_name).restype = c_int
+            assert getattr(self.shared_lib, fun_name)(self.capsule, qp_solver_cond_N) == 0
+
+            # store the new value
+            self.solver_options['qp_solver_cond_N'] = qp_solver_cond_N
+            self.solver_created = True
+
+            # get pointers solver
+            self.__get_pointers_solver()
+
+
+    def eval_param_sens(self, index, stage=0, field="ex"):
+        """
+        Calculate the sensitivity of the curent solution with respect to the initial state component of index
+
+            :param index: integer corresponding to initial state index in range(nx)
+        """
+
+        field_ = field
+        field = field_.encode('utf-8')
+
+        # checks
+        if not isinstance(index, int):
+            raise Exception('AcadosOcpSolver.eval_param_sens(): index must be Integer.')
+
+        self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
+        self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
+        nx = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8'))
+
+        if index < 0 or index > nx:
+            raise Exception(f'AcadosOcpSolver.eval_param_sens(): index must be in [0, nx-1], got: {index}.')
+
+        # actual eval_param
+        self.shared_lib.ocp_nlp_eval_param_sens.argtypes = [c_void_p, c_char_p, c_int, c_int, c_void_p]
+        self.shared_lib.ocp_nlp_eval_param_sens.restype = None
+        self.shared_lib.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out)
+
+        return
+
 
     def get(self, stage_, field_):
         """
@@ -978,23 +1096,30 @@ class AcadosOcpSolver:
 
         out_fields = ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su']
         # mem_fields = ['sl', 'su']
-        field = field_
-        field = field.encode('utf-8')
+        sens_fields = ['sens_u', "sens_x"]
+        all_fields = out_fields + sens_fields
 
-        if (field_ not in out_fields):
+        field = field_
+
+        if (field_ not in all_fields):
             raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
-                    \n Possible values are {}. Exiting.'.format(field_, out_fields))
+                    \n Possible values are {}. Exiting.'.format(field_, all_fields))
 
         if not isinstance(stage_, int):
             raise Exception('AcadosOcpSolver.get(): stage index must be Integer.')
 
         if stage_ < 0 or stage_ > self.N:
-            raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))
+            raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(stage_))
 
         if stage_ == self.N and field_ == 'pi':
             raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
                 .format(field_, stage_))
 
+        if field_ in sens_fields:
+            field = field_.replace('sens_', '')
+
+        field = field.encode('utf-8')
+
         self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
             [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
         self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
@@ -1015,6 +1140,11 @@ class AcadosOcpSolver:
         #         [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
         #     self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
         #         self.nlp_dims, self.nlp_solver, stage_, field, out_data)
+        elif field_ in sens_fields:
+            self.shared_lib.ocp_nlp_out_get.argtypes = \
+                [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_out_get(self.nlp_config, \
+                self.nlp_dims, self.sens_out, stage_, field, out_data)
 
         return out
 
@@ -1029,6 +1159,7 @@ class AcadosOcpSolver:
             - res_comp: residual wrt complementarity conditions
             - qp_stat: status of QP solver
             - qp_iter: number of QP iterations
+            - alpha: SQP step size
             - qp_res_stat: stationarity residual of the last QP solution
             - qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution
             - qp_res_ineq: residual wrt inequality constraints (constraints)  of the last QP solution
@@ -1036,19 +1167,18 @@ class AcadosOcpSolver:
         """
         stat = self.get_stats("statistics")
 
-        if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
-            print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter')
-            if stat.shape[0]>7:
+        if self.solver_options['nlp_solver_type'] == 'SQP':
+            print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha')
+            if stat.shape[0]>8:
                 print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
             for jj in range(stat.shape[1]):
-                print('{:d}\t{:e}\t{:e}\t{:e}\t{:e}\t{:d}\t{:d}'.format( \
-                     int(stat[0][jj]), stat[1][jj], stat[2][jj], \
-                     stat[3][jj], stat[4][jj], int(stat[5][jj]), int(stat[6][jj])))
-                if stat.shape[0]>7:
+                print(f'{int(stat[0][jj]):d}\t{stat[1][jj]:e}\t{stat[2][jj]:e}\t{stat[3][jj]:e}\t' +
+                      f'{stat[4][jj]:e}\t{int(stat[5][jj]):d}\t{int(stat[6][jj]):d}\t{stat[7][jj]:e}\t')
+                if stat.shape[0]>8:
                     print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \
-                        stat[7][jj], stat[8][jj], stat[9][jj], stat[10][jj]))
+                        stat[8][jj], stat[9][jj], stat[10][jj], stat[11][jj]))
             print('\n')
-        elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
+        elif self.solver_options['nlp_solver_type'] == 'SQP_RTI':
             print('\niter\tqp_stat\tqp_iter')
             if stat.shape[0]>3:
                 print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
@@ -1108,6 +1238,7 @@ class AcadosOcpSolver:
         with open(filename, 'r') as f:
             solution = json.load(f)
 
+        print(f"loading iterate {filename}")
         for key in solution.keys():
             (field, stage) = key.split('_')
             self.set(int(stage), field, np.array(solution[key]))
@@ -1117,62 +1248,99 @@ class AcadosOcpSolver:
         """
         Get the information of the last solver call.
 
-            :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
+            :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter', 'residuals', 'qp_iter', 'alpha']
+
+        Available fileds:
+            - time_tot: total CPU time previous call
+            - time_lin: CPU time for linearization
+            - time_sim: CPU time for integrator
+            - time_sim_ad: CPU time for integrator contribution of external function calls
+            - time_sim_la: CPU time for integrator contribution of linear algebra
+            - time_qp: CPU time qp solution
+            - time_qp_solver_call: CPU time inside qp solver (without converting the QP)
+            - time_qp_xcond: time_glob: CPU time globalization
+            - time_solution_sensitivities: CPU time for previous call to eval_param_sens
+            - time_reg: CPU time regularization
+            - sqp_iter: number of SQP iterations
+            - qp_iter: vector of QP iterations for last SQP call
+            - statistics: table with info about last iteration
+            - stat_m: number of rows in statistics matrix
+            - stat_n: number of columns in statistics matrix
+            - residuals: residuals of last iterate
+            - alpha: step sizes of SQP iterations
         """
 
-        fields = ['time_tot',  # total cpu time previous call
-                  'time_lin',  # cpu time for linearization
-                  'time_sim',  # cpu time for integrator
-                  'time_sim_ad',  # cpu time for integrator contribution of external function calls
-                  'time_sim_la',  # cpu time for integrator contribution of linear algebra
-                  'time_qp',   # cpu time qp solution
-                  'time_qp_solver_call',  # cpu time inside qp solver (without converting the QP)
+        double_fields = ['time_tot',
+                  'time_lin',
+                  'time_sim',
+                  'time_sim_ad',
+                  'time_sim_la',
+                  'time_qp',
+                  'time_qp_solver_call',
                   'time_qp_xcond',
-                  'time_glob',  # cpu time globalization
-                  'time_reg',  # cpu time regularization
-                  'sqp_iter',  # number of SQP iterations
-                  'qp_iter',  # vector of QP iterations for last SQP call
-                  'statistics',  # table with info about last iteration
+                  'time_glob',
+                  'time_solution_sensitivities',
+                  'time_reg'
+        ]
+        fields = double_fields + [
+                  'sqp_iter',
+                  'qp_iter',
+                  'statistics',
                   'stat_m',
-                  'stat_n',]
+                  'stat_n',
+                  'residuals',
+                  'alpha',
+                ]
+        field = field_.encode('utf-8')
 
-        field = field_
-        field = field.encode('utf-8')
-        if (field_ not in fields):
-            raise Exception('AcadosOcpSolver.get_stats(): {} is not a valid argument.\
-                    \n Possible values are {}. Exiting.'.format(fields, fields))
 
         if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
             out = np.ascontiguousarray(np.zeros((1,)), dtype=np.int64)
             out_data = cast(out.ctypes.data, POINTER(c_int64))
+            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+            return out
+
+        # TODO: just return double instead of np.
+        elif field_ in double_fields:
+            out = np.zeros((1,))
+            out_data = cast(out.ctypes.data, POINTER(c_double))
+            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+            return out
 
         elif field_ == 'statistics':
             sqp_iter = self.get_stats("sqp_iter")
             stat_m = self.get_stats("stat_m")
             stat_n = self.get_stats("stat_n")
-
             min_size = min([stat_m, sqp_iter+1])
-
             out = np.ascontiguousarray(
                         np.zeros((stat_n[0]+1, min_size[0])), dtype=np.float64)
             out_data = cast(out.ctypes.data, POINTER(c_double))
+            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+            return out
 
         elif field_ == 'qp_iter':
             full_stats = self.get_stats('statistics')
-            if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
-                out = full_stats[6, :]
-            elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
-                out = full_stats[2, :]
+            if self.solver_options['nlp_solver_type'] == 'SQP':
+                return full_stats[6, :]
+            elif self.solver_options['nlp_solver_type'] == 'SQP_RTI':
+                return full_stats[2, :]
+
+        elif field_ == 'alpha':
+            full_stats = self.get_stats('statistics')
+            if self.solver_options['nlp_solver_type'] == 'SQP':
+                return full_stats[7, :]
+            else: # self.solver_options['nlp_solver_type'] == 'SQP_RTI':
+                raise Exception("alpha values are not available for SQP_RTI")
+
+        elif field_ == 'residuals':
+            return self.get_residuals()
 
         else:
-            out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
-            out_data = cast(out.ctypes.data, POINTER(c_double))
-
-        if not field_ == 'qp_iter':
-            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
-            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
-
-        return out
+            raise Exception(f'AcadosOcpSolver.get_stats(): {field} is not a valid argument.'
+                    + f'\n Possible values are {fields}.')
 
 
     def get_cost(self):
@@ -1201,7 +1369,7 @@ class AcadosOcpSolver:
         Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
         """
         # compute residuals if RTI
-        if self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
+        if self.solver_options['nlp_solver_type'] == 'SQP_RTI':
             self.shared_lib.ocp_nlp_eval_residuals.argtypes = [c_void_p, c_void_p, c_void_p]
             self.shared_lib.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
 
@@ -1230,9 +1398,7 @@ class AcadosOcpSolver:
 
 
     # Note: this function should not be used anymore, better use cost_set, constraints_set
-
     def set(self, stage_, field_, value_):
-
         """
         Set numerical data inside the solver.
 
@@ -1253,6 +1419,7 @@ class AcadosOcpSolver:
         cost_fields = ['y_ref', 'yref']
         constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
         out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
+        mem_fields = ['xdot_guess']
 
         # cast value_ to avoid conversion issues
         if isinstance(value_, (float, int)):
@@ -1294,18 +1461,25 @@ class AcadosOcpSolver:
             value_data_p = cast((value_data), c_void_p)
 
             if field_ in constraints_fields:
+                self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
+                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
                 self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
                     self.nlp_dims, self.nlp_in, stage, field, value_data_p)
             elif field_ in cost_fields:
+                self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
+                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
                 self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
                     self.nlp_dims, self.nlp_in, stage, field, value_data_p)
             elif field_ in out_fields:
+                self.shared_lib.ocp_nlp_out_set.argtypes = \
+                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
                 self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
                     self.nlp_dims, self.nlp_out, stage, field, value_data_p)
-            # elif field_ in mem_fields:
-            #     self.shared_lib.ocp_nlp_set(self.nlp_config, \
-            #         self.nlp_solver, stage, field, value_data_p)
-
+            elif field_ in mem_fields:
+                self.shared_lib.ocp_nlp_set.argtypes = \
+                    [c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+                self.shared_lib.ocp_nlp_set(self.nlp_config, \
+                    self.nlp_solver, stage, field, value_data_p)
         return
 
 
@@ -1364,9 +1538,8 @@ class AcadosOcpSolver:
                 raise Exception("Unknown api: '{}'".format(api))
 
         if value_shape != tuple(dims):
-            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
-                ' for field "{}" with dimension {} (you have {})'.format( \
-                field_, tuple(dims), value_shape))
+            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension' +
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
 
         value_data = cast(value_.ctypes.data, POINTER(c_double))
         value_data_p = cast((value_data), c_void_p)
@@ -1433,8 +1606,8 @@ class AcadosOcpSolver:
                 raise Exception("Unknown api: '{}'".format(api))
 
         if value_shape != tuple(dims):
-            raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
-                ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
+            raise Exception(f'AcadosOcpSolver.constraints_set(): mismatching dimension' +
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
 
         value_data = cast(value_.ctypes.data, POINTER(c_double))
         value_data_p = cast((value_data), c_void_p)
@@ -1490,11 +1663,11 @@ class AcadosOcpSolver:
         """
         Set options of the solver.
 
-            :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
+            :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC'
             :param value: of type int, float
         """
-        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
-        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
+        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC']
+        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent']
         string_fields = ['globalization']
 
         # check field availability and type
@@ -1522,10 +1695,10 @@ class AcadosOcpSolver:
 
         if field_ == 'rti_phase':
             if value_ < 0 or value_ > 2:
-                raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+                raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can '
                     'take only values 0, 1, 2 for SQP-RTI-type solvers')
-            if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
-                raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+            if self.solver_options['nlp_solver_type'] != 'SQP_RTI' and value_ > 0:
+                raise Exception('AcadosOcpSolver.options_set(): argument \'rti_phase\' can '
                     'take only value 0 for SQP-type solvers')
 
         # encode
diff --git a/pyextra/acados_template/acados_ocp_solver_fast.py b/pyextra/acados_template/acados_ocp_solver_fast.py
deleted file mode 100644
index 656d288f1..000000000
--- a/pyextra/acados_template/acados_ocp_solver_fast.py
+++ /dev/null
@@ -1,402 +0,0 @@
-import sys
-import os
-import json
-import numpy as np
-from datetime import datetime
-
-from ctypes import POINTER, CDLL, c_void_p, c_int, cast, c_double, c_char_p
-
-from copy import deepcopy
-
-from .generate_c_code_explicit_ode import generate_c_code_explicit_ode
-from .generate_c_code_implicit_ode import generate_c_code_implicit_ode
-from .generate_c_code_gnsf import generate_c_code_gnsf
-from .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics
-from .generate_c_code_constraint import generate_c_code_constraint
-from .generate_c_code_nls_cost import generate_c_code_nls_cost
-from .generate_c_code_external_cost import generate_c_code_external_cost
-from .acados_ocp import AcadosOcp
-from .acados_model import acados_model_strip_casadi_symbolics
-from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
-     format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
-     set_up_imported_gnsf_model, get_acados_path
-
-
-class AcadosOcpSolverFast:
-    dlclose = CDLL(None).dlclose
-    dlclose.argtypes = [c_void_p]
-
-    def __init__(self, model_name, N, code_export_dir):
-
-        self.solver_created = False
-        self.N = N
-        self.model_name = model_name
-
-        self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{model_name}.so'
-
-        # get shared_lib
-        self.shared_lib = CDLL(self.shared_lib_name)
-
-        # create capsule
-        getattr(self.shared_lib, f"{model_name}_acados_create_capsule").restype = c_void_p
-        self.capsule = getattr(self.shared_lib, f"{model_name}_acados_create_capsule")()
-
-        # create solver
-        getattr(self.shared_lib, f"{model_name}_acados_create").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_create").restype = c_int
-        assert getattr(self.shared_lib, f"{model_name}_acados_create")(self.capsule)==0
-        self.solver_created = True
-
-        # get pointers solver
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").restype = c_void_p
-        self.nlp_opts = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").restype = c_void_p
-        self.nlp_dims = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").restype = c_void_p
-        self.nlp_config = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").restype = c_void_p
-        self.nlp_out = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").restype = c_void_p
-        self.nlp_in = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in")(self.capsule)
-
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").restype = c_void_p
-        self.nlp_solver = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver")(self.capsule)
-
-
-    def solve(self):
-        """
-        Solve the ocp with current input.
-        """
-        model_name = self.model_name
-
-        getattr(self.shared_lib, f"{model_name}_acados_solve").argtypes = [c_void_p]
-        getattr(self.shared_lib, f"{model_name}_acados_solve").restype = c_int
-        status = getattr(self.shared_lib, f"{model_name}_acados_solve")(self.capsule)
-        return status
-
-    def cost_set(self, start_stage_, field_, value_, api='warn'):
-      self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
-      return
-
-    def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
-        """
-        Set numerical data in the cost module of the solver.
-
-            :param stage: integer corresponding to shooting node
-            :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess'
-            :param value: of appropriate size
-        """
-        # cast value_ to avoid conversion issues
-        if isinstance(value_, (float, int)):
-            value_ = np.array([value_])
-        value_ = np.ascontiguousarray(np.copy(value_), dtype=np.float64)
-        field = field_
-        field = field.encode('utf-8')
-        dim = np.product(value_.shape[1:])
-
-        start_stage = c_int(start_stage_)
-        end_stage = c_int(end_stage_)
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
-
-        dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
-        dims_data = cast(dims.ctypes.data, POINTER(c_int))
-
-        self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config,
-            self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
-
-        value_shape = value_.shape
-        expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
-        if len(value_shape) == 2:
-            value_shape = (value_shape[0], value_shape[1], 0)
-
-        elif len(value_shape) == 3:
-            if api=='old':
-                pass
-            elif api=='warn':
-                if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
-                    raise Exception("Ambiguity in API detected.\n"
-                                    "Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n"
-                                    "Are you seeing this error suddenly in previously running code? Read on.\n"
-                                    "  You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) +
-                                    "  acados_template now correctly passes on any matrices to acados in column major format.\n" +
-                                    "  Two options to fix this error: \n" +
-                                    "   * Add api='old' to cost_set to restore old incorrect behaviour\n" +
-                                    "   * Add api='new' to cost_set and remove any unnatural manipulation of the value argument " +
-                                    "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
-                                    "If there is no such manipulation, then you have probably been getting an incorrect solution before.")
-                # Get elements in column major order
-                value_ = np.ravel(value_, order='F')
-            elif api=='new':
-                # Get elements in column major order
-                value_ = np.ravel(value_, order='F')
-            else:
-                raise Exception("Unknown api: '{}'".format(api))
-
-        if value_shape != expected_shape:
-            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension',
-                            ' for field "{}" with dimension {} (you have {})'.format(
-                               field_, expected_shape, value_shape))
-
-
-        value_data = cast(value_.ctypes.data, POINTER(c_double))
-        value_data_p = cast((value_data), c_void_p)
-
-        self.shared_lib.ocp_nlp_cost_model_set_slice.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
-        self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config,
-            self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim)
-        return
-
-    def constraints_set(self, start_stage_, field_, value_, api='warn'):
-      self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
-      return
-
-    def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
-        """
-        Set numerical data in the constraint module of the solver.
-
-            :param stage: integer corresponding to shooting node
-            :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi']
-            :param value: of appropriate size
-        """
-        # cast value_ to avoid conversion issues
-        if isinstance(value_, (float, int)):
-            value_ = np.array([value_])
-        value_ = value_.astype(float)
-
-        field = field_
-        field = field.encode('utf-8')
-        dim = np.product(value_.shape[1:])
-
-        start_stage = c_int(start_stage_)
-        end_stage = c_int(end_stage_)
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
-
-        dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
-        dims_data = cast(dims.ctypes.data, POINTER(c_int))
-
-        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
-            self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
-
-        value_shape = value_.shape
-        expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
-        if len(value_shape) == 2:
-            value_shape = (value_shape[0], value_shape[1], 0)
-        elif len(value_shape) == 3:
-            if api=='old':
-                pass
-            elif api=='warn':
-                if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
-                    raise Exception("Ambiguity in API detected.\n"
-                                    "Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n"
-                                    "Are you seeing this error suddenly in previously running code? Read on.\n"
-                                    "  You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) +
-                                    "  acados_template now correctly passes on any matrices to acados in column major format.\n" +
-                                    "  Two options to fix this error: \n" +
-                                    "   * Add api='old' to constraints_set to restore old incorrect behaviour\n" +
-                                    "   * Add api='new' to constraints_set and remove any unnatural manipulation of the value argument " +
-                                    "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
-                                    "If there is no such manipulation, then you have probably been getting an incorrect solution before.")
-                # Get elements in column major order
-                value_ = np.ravel(value_, order='F')
-            elif api=='new':
-                # Get elements in column major order
-                value_ = np.ravel(value_, order='F')
-            else:
-                raise Exception("Unknown api: '{}'".format(api))
-        if value_shape != expected_shape:
-            raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
-                ' for field "{}" with dimension {} (you have {})'.format(field_, expected_shape, value_shape))
-
-        value_data = cast(value_.ctypes.data, POINTER(c_double))
-        value_data_p = cast((value_data), c_void_p)
-
-        self.shared_lib.ocp_nlp_constraints_model_set_slice.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
-        self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \
-            self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim)
-        return
-
-    # Note: this function should not be used anymore, better use cost_set, constraints_set
-    def set(self, stage_, field_, value_):
-        """
-        Set numerical data inside the solver.
-
-            :param stage: integer corresponding to shooting node
-            :param field: string in ['x', 'u', 'pi', 'lam', 't', 'p']
-
-            .. note:: regarding lam, t: \n
-                    the inequalities are internally organized in the following order: \n
-                    [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
-                      lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
-
-            .. note:: pi: multipliers for dynamics equality constraints \n
-                      lam: multipliers for inequalities \n
-                      t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
-                      sl: slack variables of soft lower inequality constraints \n
-                      su: slack variables of soft upper inequality constraints \n
-        """
-        cost_fields = ['y_ref', 'yref']
-        constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
-        out_fields = ['x', 'u', 'pi', 'lam', 't', 'z']
-        mem_fields = ['sl', 'su']
-
-        # cast value_ to avoid conversion issues
-        if isinstance(value_, (float, int)):
-            value_ = np.array([value_])
-        value_ = value_.astype(float)
-
-        model_name = self.model_name
-
-        field = field_
-        field = field.encode('utf-8')
-
-        stage = c_int(stage_)
-
-        # treat parameters separately
-        if field_ == 'p':
-            getattr(self.shared_lib, f"{model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
-            getattr(self.shared_lib, f"{model_name}_acados_update_params").restype = c_int
-
-            value_data = cast(value_.ctypes.data, POINTER(c_double))
-
-            assert getattr(self.shared_lib, f"{model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0
-        else:
-            if field_ not in constraints_fields + cost_fields + out_fields + mem_fields:
-                raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
-                    \nPossible values are {}. Exiting.".format(field, \
-                    constraints_fields + cost_fields + out_fields + ['p']))
-
-            self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
-                [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
-            self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
-
-            dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
-                self.nlp_dims, self.nlp_out, stage_, field)
-
-            if value_.shape[0] != dims:
-                msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
-                msg += 'with dimension {} (you have {})'.format(dims, value_.shape)
-                raise Exception(msg)
-
-            value_data = cast(value_.ctypes.data, POINTER(c_double))
-            value_data_p = cast((value_data), c_void_p)
-
-            if field_ in constraints_fields:
-                self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
-                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-                self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
-                    self.nlp_dims, self.nlp_in, stage, field, value_data_p)
-            elif field_ in cost_fields:
-                self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
-                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-                self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
-                    self.nlp_dims, self.nlp_in, stage, field, value_data_p)
-            elif field_ in out_fields:
-                self.shared_lib.ocp_nlp_out_set.argtypes = \
-                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-                self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
-                    self.nlp_dims, self.nlp_out, stage, field, value_data_p)
-            elif field_ in mem_fields:
-                self.shared_lib.ocp_nlp_set.argtypes = \
-                    [c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-                self.shared_lib.ocp_nlp_set(self.nlp_config, \
-                    self.nlp_solver, stage, field, value_data_p)
-        return
-
-
-    def get_slice(self, start_stage_, end_stage_, field_):
-        """
-        Get the last solution of the solver:
-
-            :param start_stage: integer corresponding to shooting node that indicates start of slice
-            :param end_stage: integer corresponding to shooting node that indicates end of slice
-            :param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',]
-
-            .. note:: regarding lam, t: \n
-                    the inequalities are internally organized in the following order: \n
-                    [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
-                      lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
-
-            .. note:: pi: multipliers for dynamics equality constraints \n
-                      lam: multipliers for inequalities \n
-                      t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
-                      sl: slack variables of soft lower inequality constraints \n
-                      su: slack variables of soft upper inequality constraints \n
-        """
-        out_fields = ['x', 'u', 'z', 'pi', 'lam', 't']
-        mem_fields = ['sl', 'su']
-        field = field_
-        field = field.encode('utf-8')
-
-        if (field_ not in out_fields + mem_fields):
-            raise Exception('AcadosOcpSolver.get_slice(): {} is an invalid argument.\
-                    \n Possible values are {}. Exiting.'.format(field_, out_fields))
-
-        if not isinstance(start_stage_, int):
-            raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
-
-        if not isinstance(end_stage_, int):
-            raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
-
-        if start_stage_ >= end_stage_:
-            raise Exception('AcadosOcpSolver.get_slice(): end stage index must be larger than start stage index')
-
-        if start_stage_ < 0 or end_stage_ > self.N + 1:
-            raise Exception('AcadosOcpSolver.get_slice(): stage index must be in [0, N], got: {}.'.format(self.N))
-        self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
-            [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
-        self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
-
-        dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
-            self.nlp_dims, self.nlp_out, start_stage_, field)
-
-        out = np.ascontiguousarray(np.zeros((end_stage_ - start_stage_, dims)), dtype=np.float64)
-        out_data = cast(out.ctypes.data, POINTER(c_double))
-
-        if (field_ in out_fields):
-            self.shared_lib.ocp_nlp_out_get_slice.argtypes = \
-                [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p]
-            self.shared_lib.ocp_nlp_out_get_slice(self.nlp_config, \
-                self.nlp_dims, self.nlp_out, start_stage_, end_stage_, field, out_data)
-        elif field_ in mem_fields:
-            self.shared_lib.ocp_nlp_get_at_stage.argtypes = \
-                [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
-            self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
-                self.nlp_dims, self.nlp_solver, start_stage_, end_stage_, field, out_data)
-
-        return out
-
-    def get_cost(self):
-        """
-        Returns the cost value of the current solution.
-        """
-        # compute cost internally
-        self.shared_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p]
-        self.shared_lib.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out)
-
-        # create output array
-        out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
-        out_data = cast(out.ctypes.data, POINTER(c_double))
-
-        # call getter
-        self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
-
-        field = "cost_value".encode('utf-8')
-        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
-
-        return out[0]
diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx
index cab033a7a..97d5b2855 100644
--- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx
+++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx
@@ -39,22 +39,19 @@ cimport cython
 from libc cimport string
 
 cimport acados_solver_common
+# TODO: make this import more clear? it is not a general solver, but problem specific.
 cimport acados_solver
 
 cimport numpy as cnp
 
 import os
-import json
 from datetime import datetime
 import numpy as np
 
 
-cdef class AcadosOcpSolverFast:
+cdef class AcadosOcpSolverCython:
     """
     Class to interact with the acados ocp solver C object.
-
-        :param model_name:
-        :param N:
     """
 
     cdef acados_solver.nlp_solver_capsule *capsule
@@ -62,19 +59,26 @@ cdef class AcadosOcpSolverFast:
     cdef acados_solver_common.ocp_nlp_dims *nlp_dims
     cdef acados_solver_common.ocp_nlp_config *nlp_config
     cdef acados_solver_common.ocp_nlp_out *nlp_out
+    cdef acados_solver_common.ocp_nlp_out *sens_out
     cdef acados_solver_common.ocp_nlp_in *nlp_in
     cdef acados_solver_common.ocp_nlp_solver *nlp_solver
 
-    cdef str model_name
-    cdef int N
+    cdef int status
     cdef bint solver_created
 
-    def __cinit__(self, str model_name, int N):
-        self.model_name = model_name
-        self.N = N
+    cdef str model_name
+    cdef int N
+
+    cdef str nlp_solver_type
+
+    def __cinit__(self, model_name, nlp_solver_type, N):
 
         self.solver_created = False
 
+        self.N = N
+        self.model_name = model_name
+        self.nlp_solver_type = nlp_solver_type
+
         # create capsule
         self.capsule = acados_solver.acados_create_capsule()
 
@@ -82,11 +86,21 @@ cdef class AcadosOcpSolverFast:
         assert acados_solver.acados_create(self.capsule) == 0
         self.solver_created = True
 
+        # get pointers solver
+        self.__get_pointers_solver()
+        self.status = 0
+
+
+    def __get_pointers_solver(self):
+        """
+        Private function to get the pointers for solver
+        """
         # get pointers solver
         self.nlp_opts = acados_solver.acados_get_nlp_opts(self.capsule)
         self.nlp_dims = acados_solver.acados_get_nlp_dims(self.capsule)
         self.nlp_config = acados_solver.acados_get_nlp_config(self.capsule)
         self.nlp_out = acados_solver.acados_get_nlp_out(self.capsule)
+        self.sens_out = acados_solver.acados_get_sens_out(self.capsule)
         self.nlp_in = acados_solver.acados_get_nlp_in(self.capsule)
         self.nlp_solver = acados_solver.acados_get_nlp_solver(self.capsule)
 
@@ -100,15 +114,112 @@ cdef class AcadosOcpSolverFast:
 
     def set_new_time_steps(self, new_time_steps):
         """
-        Set new time steps before solving. Only reload library without code generation but with new time steps.
+        Set new time steps.
+        Recreates the solver if N changes.
 
-            :param new_time_steps: vector of new time steps for the solver
+            :param new_time_steps: 1 dimensional np array of new time steps for the solver
 
             .. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
                       the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
                       do not require a new code export and compilation.
         """
-        raise NotImplementedError()
+
+        raise NotImplementedError("AcadosOcpSolverCython: does not support set_new_time_steps() since it is only a prototyping feature")
+        # # unlikely but still possible
+        # if not self.solver_created:
+        #     raise Exception('Solver was not yet created!')
+
+        # ## check if time steps really changed in value
+        # # get time steps
+        # cdef cnp.ndarray[cnp.float64_t, ndim=1] old_time_steps = np.ascontiguousarray(np.zeros((self.N,)), dtype=np.float64)
+        # assert acados_solver.acados_get_time_steps(self.capsule, self.N, <double *> old_time_steps.data)
+
+        # if np.array_equal(old_time_steps, new_time_steps):
+        #     return
+
+        # N = new_time_steps.size
+        # cdef cnp.ndarray[cnp.float64_t, ndim=1] value = np.ascontiguousarray(new_time_steps, dtype=np.float64)
+
+        # # check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
+        # if len(old_time_steps) == N:
+        #     assert acados_solver.acados_update_time_steps(self.capsule, N, <double *> value.data) == 0
+
+        # else:  # recreate the solver with the new time steps
+        #     self.solver_created = False
+
+        #     # delete old memory (analog to __del__)
+        #     acados_solver.acados_free(self.capsule)
+
+        #     # create solver with new time steps
+        #     assert acados_solver.acados_create_with_discretization(self.capsule, N, <double *> value.data) == 0
+
+        #     self.solver_created = True
+
+        #     # get pointers solver
+        #     self.__get_pointers_solver()
+
+        # # store time_steps, N
+        # self.time_steps = new_time_steps
+        # self.N = N
+
+
+    def update_qp_solver_cond_N(self, qp_solver_cond_N: int):
+        """
+        Recreate solver with new value `qp_solver_cond_N` with a partial condensing QP solver.
+        This function is relevant for code reuse, i.e., if either `set_new_time_steps(...)` is used or
+        the influence of a different `qp_solver_cond_N` is studied without code export and compilation.
+            :param qp_solver_cond_N: new number of condensing stages for the solver
+
+            .. note:: This function can only be used in combination with a partial condensing QP solver.
+
+            .. note:: After `set_new_time_steps(...)` is used and depending on the new number of time steps it might be
+                      necessary to change `qp_solver_cond_N` as well (using this function), i.e., typically
+                      `qp_solver_cond_N < N`.
+        """
+        raise NotImplementedError("AcadosOcpSolverCython: does not support update_qp_solver_cond_N() since it is only a prototyping feature")
+
+        # # unlikely but still possible
+        # if not self.solver_created:
+        #     raise Exception('Solver was not yet created!')
+        # if self.N < qp_solver_cond_N:
+        #     raise Exception('Setting qp_solver_cond_N to be larger than N does not work!')
+        # if self.qp_solver_cond_N != qp_solver_cond_N:
+        #     self.solver_created = False
+
+        #     # recreate the solver
+        #     acados_solver.acados_update_qp_solver_cond_N(self.capsule, qp_solver_cond_N)
+
+        #     # store the new value
+        #     self.qp_solver_cond_N = qp_solver_cond_N
+        #     self.solver_created = True
+
+        #     # get pointers solver
+        #     self.__get_pointers_solver()
+
+
+    def eval_param_sens(self, index, stage=0, field="ex"):
+        """
+        Calculate the sensitivity of the curent solution with respect to the initial state component of index
+
+            :param index: integer corresponding to initial state index in range(nx)
+        """
+
+        field_ = field
+        field = field_.encode('utf-8')
+
+        # checks
+        if not isinstance(index, int):
+            raise Exception('AcadosOcpSolverCython.eval_param_sens(): index must be Integer.')
+
+        cdef int nx = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, 0, "x".encode('utf-8'))
+
+        if index < 0 or index > nx:
+            raise Exception(f'AcadosOcpSolverCython.eval_param_sens(): index must be in [0, nx-1], got: {index}.')
+
+        # actual eval_param
+        acados_solver_common.ocp_nlp_eval_param_sens(self.nlp_solver, field, stage, index, self.sens_out)
+
+        return
 
 
     def get(self, int stage, str field_):
@@ -134,14 +245,14 @@ cdef class AcadosOcpSolverFast:
         field = field_.encode('utf-8')
 
         if field_ not in out_fields:
-            raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
+            raise Exception('AcadosOcpSolverCython.get(): {} is an invalid argument.\
                     \n Possible values are {}. Exiting.'.format(field_, out_fields))
 
         if stage < 0 or stage > self.N:
-            raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))
+            raise Exception('AcadosOcpSolverCython.get(): stage index must be in [0, N], got: {}.'.format(self.N))
 
         if stage == self.N and field_ == 'pi':
-            raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
+            raise Exception('AcadosOcpSolverCython.get(): field {} does not exist at final stage {}.'\
                 .format(field_, stage))
 
         cdef int dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config,
@@ -179,6 +290,7 @@ cdef class AcadosOcpSolverFast:
             :param filename: if not set, use model_name + timestamp + '.json'
             :param overwrite: if false and filename exists add timestamp to filename
         """
+        import json
         if filename == '':
             filename += self.model_name + '_' + 'iterate' + '.json'
 
@@ -212,6 +324,7 @@ cdef class AcadosOcpSolverFast:
         """
         Loads the iterate stored in json file with filename into the ocp solver.
         """
+        import json
         if not os.path.isfile(filename):
             raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename))
 
@@ -228,34 +341,53 @@ cdef class AcadosOcpSolverFast:
         Get the information of the last solver call.
 
             :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
+        Available fileds:
+            - time_tot: total CPU time previous call
+            - time_lin: CPU time for linearization
+            - time_sim: CPU time for integrator
+            - time_sim_ad: CPU time for integrator contribution of external function calls
+            - time_sim_la: CPU time for integrator contribution of linear algebra
+            - time_qp: CPU time qp solution
+            - time_qp_solver_call: CPU time inside qp solver (without converting the QP)
+            - time_qp_xcond: time_glob: CPU time globalization
+            - time_solution_sensitivities: CPU time for previous call to eval_param_sens
+            - time_reg: CPU time regularization
+            - sqp_iter: number of SQP iterations
+            - qp_iter: vector of QP iterations for last SQP call
+            - statistics: table with info about last iteration
+            - stat_m: number of rows in statistics matrix
+            - stat_n: number of columns in statistics matrix
+            - residuals: residuals of last iterate
+            - alpha: step sizes of SQP iterations
         """
-        fields = ['time_tot',  # total cpu time previous call
-                  'time_lin',  # cpu time for linearization
-                  'time_sim',  # cpu time for integrator
-                  'time_sim_ad',  # cpu time for integrator contribution of external function calls
-                  'time_sim_la',  # cpu time for integrator contribution of linear algebra
-                  'time_qp',   # cpu time qp solution
-                  'time_qp_solver_call',  # cpu time inside qp solver (without converting the QP)
+
+        double_fields = ['time_tot',
+                  'time_lin',
+                  'time_sim',
+                  'time_sim_ad',
+                  'time_sim_la',
+                  'time_qp',
+                  'time_qp_solver_call',
                   'time_qp_xcond',
-                  'time_glob',  # cpu time globalization
-                  'time_reg',  # cpu time regularization
-                  'sqp_iter',  # number of SQP iterations
-                  'qp_iter',  # vector of QP iterations for last SQP call
-                  'statistics',  # table with info about last iteration
+                  'time_glob',
+                  'time_solution_sensitivities',
+                  'time_reg'
+        ]
+        fields = double_fields + [
+                  'sqp_iter',
+                  'qp_iter',
+                  'statistics',
                   'stat_m',
-                  'stat_n',]
-
-        field = field_
-        field = field.encode('utf-8')
-
-        if (field_ not in fields):
-            raise Exception('AcadosOcpSolver.get_stats(): {} is not a valid argument.\
-                    \n Possible values are {}. Exiting.'.format(fields, fields))
+                  'stat_n',
+                  'residuals',
+                  'alpha',
+                ]
+        field = field_.encode('utf-8')
 
         if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
             return self.__get_stat_int(field)
 
-        elif field_.startswith('time'):
+        elif field_ in double_fields:
             return self.__get_stat_double(field)
 
         elif field_ == 'statistics':
@@ -266,14 +398,24 @@ cdef class AcadosOcpSolverFast:
             return self.__get_stat_matrix(field, stat_n+1, min_size)
 
         elif field_ == 'qp_iter':
-            NotImplementedError("TODO! Cython not aware if SQP or SQP_RTI")
             full_stats = self.get_stats('statistics')
-            if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
-                out = full_stats[6, :]
-            elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
-                out = full_stats[2, :]
+            if self.nlp_solver_type == 'SQP':
+                return full_stats[6, :]
+            elif self.nlp_solver_type == 'SQP_RTI':
+                return full_stats[2, :]
+
+        elif field_ == 'alpha':
+            full_stats = self.get_stats('statistics')
+            if self.nlp_solver_type == 'SQP':
+                return full_stats[7, :]
+            else: # self.nlp_solver_type == 'SQP_RTI':
+                raise Exception("alpha values are not available for SQP_RTI")
+
+        elif field_ == 'residuals':
+            return self.get_residuals()
+
         else:
-            NotImplementedError("TODO!")
+            raise NotImplementedError("TODO!")
 
 
     def __get_stat_int(self, field):
@@ -312,10 +454,9 @@ cdef class AcadosOcpSolverFast:
         """
         Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
         """
-        # TODO: check if RTI, only eval then
-        # if self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
         # compute residuals if RTI
-        acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
+        if self.nlp_solver_type == 'SQP_RTI':
+            acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
 
         # create output array
         cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.ascontiguousarray(np.zeros((4,), dtype=np.float64))
@@ -363,6 +504,7 @@ cdef class AcadosOcpSolverFast:
         cost_fields = ['y_ref', 'yref']
         constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
         out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
+        mem_fields = ['xdot_guess']
 
         field = field_.encode('utf-8')
 
@@ -373,7 +515,7 @@ cdef class AcadosOcpSolverFast:
             assert acados_solver.acados_update_params(self.capsule, stage, <double *> value.data, value.shape[0]) == 0
         else:
             if field_ not in constraints_fields + cost_fields + out_fields:
-                raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
+                raise Exception("AcadosOcpSolverCython.set(): {} is not a valid argument.\
                     \nPossible values are {}. Exiting.".format(field, \
                     constraints_fields + cost_fields + out_fields + ['p']))
 
@@ -381,7 +523,7 @@ cdef class AcadosOcpSolverFast:
                 self.nlp_dims, self.nlp_out, stage, field)
 
             if value_.shape[0] != dims:
-                msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
+                msg = 'AcadosOcpSolverCython.set(): mismatching dimension for field "{}" '.format(field_)
                 msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0])
                 raise Exception(msg)
 
@@ -394,6 +536,9 @@ cdef class AcadosOcpSolverFast:
             elif field_ in out_fields:
                 acados_solver_common.ocp_nlp_out_set(self.nlp_config,
                     self.nlp_dims, self.nlp_out, stage, field, <void *> value.data)
+            elif field_ in mem_fields:
+                acados_solver_common.ocp_nlp_set(self.nlp_config, \
+                    self.nlp_solver, stage, field, <void *> value.data)
 
 
     def cost_set(self, int stage, str field_, value_):
@@ -422,9 +567,8 @@ cdef class AcadosOcpSolverFast:
             value = np.asfortranarray(value_)
 
         if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
-            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
-                ' for field "{}" with dimension {} (you have {})'.format( \
-                field_, tuple(dims), value_shape))
+            raise Exception('AcadosOcpSolverCython.cost_set(): mismatching dimension' +
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
 
         acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, \
             self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
@@ -456,8 +600,8 @@ cdef class AcadosOcpSolverFast:
             value = np.asfortranarray(value_)
 
         if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
-            raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
-                ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
+            raise Exception(f'AcadosOcpSolverCython.constraints_set(): mismatching dimension' +
+                f' for field "{field_}" at stage {stage} with dimension {tuple(dims)} (you have {value_shape})')
 
         acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \
             self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
@@ -494,8 +638,8 @@ cdef class AcadosOcpSolverFast:
             :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
             :param value: of type int, float
         """
-        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
-        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
+        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC']
+        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent']
         string_fields = ['globalization']
 
         # encode
@@ -512,10 +656,10 @@ cdef class AcadosOcpSolverFast:
 
             if field_ == 'rti_phase':
                 if value_ < 0 or value_ > 2:
-                    raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+                    raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
                         'take only values 0, 1, 2 for SQP-RTI-type solvers')
-                if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
-                    raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+                if self.nlp_solver_type != 'SQP_RTI' and value_ > 0:
+                    raise Exception('AcadosOcpSolverCython.solve(): argument \'rti_phase\' can '
                         'take only value 0 for SQP-type solvers')
 
             int_value = value_
@@ -536,7 +680,7 @@ cdef class AcadosOcpSolverFast:
             acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &string_value[0])
 
         else:
-            raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
+            raise Exception('AcadosOcpSolverCython.options_set() does not support field {}.'\
                 '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
 
 
diff --git a/pyextra/acados_template/acados_sim.py b/pyextra/acados_template/acados_sim.py
index d7ad1487d..c63cffc31 100644
--- a/pyextra/acados_template/acados_sim.py
+++ b/pyextra/acados_template/acados_sim.py
@@ -70,28 +70,28 @@ class AcadosSimDims:
 
     @nx.setter
     def nx(self, nx):
-        if type(nx) == int and nx > 0:
+        if isinstance(nx, int) and nx > 0:
             self.__nx = nx
         else:
             raise Exception('Invalid nx value, expected positive integer. Exiting.')
 
     @nz.setter
     def nz(self, nz):
-        if type(nz) == int and nz > -1:
+        if isinstance(nz, int) and nz > -1:
             self.__nz = nz
         else:
             raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
 
     @nu.setter
     def nu(self, nu):
-        if type(nu) == int and nu > -1:
+        if isinstance(nu, int) and nu > -1:
             self.__nu = nu
         else:
             raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
 
     @np.setter
     def np(self, np):
-        if type(np) == int and np > -1:
+        if isinstance(np, int) and np > -1:
             self.__np = np
         else:
             raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
@@ -302,6 +302,7 @@ class AcadosSim:
         self.code_export_directory = 'c_generated_code'
         """Path to where code will be exported. Default: `c_generated_code`."""
 
+        self.cython_include_dirs = ''
         self.__parameter_values = np.array([])
 
     @property
diff --git a/pyextra/acados_template/acados_sim_solver.py b/pyextra/acados_template/acados_sim_solver.py
index 145f90293..a12ecb176 100644
--- a/pyextra/acados_template/acados_sim_solver.py
+++ b/pyextra/acados_template/acados_sim_solver.py
@@ -215,6 +215,24 @@ class AcadosSimSolver:
         model_name = self.sim_struct.model.name
         self.model_name = model_name
 
+        # Load acados library to avoid unloading the library.
+        # This is necessary if acados was compiled with OpenMP, since the OpenMP threads can't be destroyed.
+        # Unloading a library which uses OpenMP results in a segfault (on any platform?).
+        # see [https://stackoverflow.com/questions/34439956/vc-crash-when-freeing-a-dll-built-with-openmp]
+        # or [https://python.hotexamples.com/examples/_ctypes/-/dlclose/python-dlclose-function-examples.html]
+        libacados_name = 'libacados.so'
+        libacados_filepath = os.path.join(acados_sim.acados_lib_path, libacados_name)
+        self.__acados_lib = CDLL(libacados_filepath)
+        # find out if acados was compiled with OpenMP
+        try:
+            self.__acados_lib_uses_omp = getattr(self.__acados_lib, 'omp_get_thread_num') is not None
+        except AttributeError as e:
+            self.__acados_lib_uses_omp = False
+        if self.__acados_lib_uses_omp:
+            print('acados was compiled with OpenMP.')
+        else:
+            print('acados was compiled without OpenMP.')
+
         # Ctypes
         shared_lib = f'{code_export_dir}/libacados_sim_solver_{model_name}.so'
         self.shared_lib = CDLL(shared_lib)
diff --git a/pyextra/acados_template/acados_solver_common.pxd b/pyextra/acados_template/acados_solver_common.pxd
index 9314802e6..fedd7190d 100644
--- a/pyextra/acados_template/acados_solver_common.pxd
+++ b/pyextra/acados_template/acados_solver_common.pxd
@@ -95,6 +95,7 @@ cdef extern from "acados_c/ocp_nlp_interface.h":
 
     # solver
     void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out)
+    void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out)
     void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in_, ocp_nlp_out *nlp_out)
 
     # get/set
diff --git a/pyextra/acados_template/c_templates_tera/Makefile.in b/pyextra/acados_template/c_templates_tera/Makefile.in
index 487e66ab0..d45be0a9c 100644
--- a/pyextra/acados_template/c_templates_tera/Makefile.in
+++ b/pyextra/acados_template/c_templates_tera/Makefile.in
@@ -125,134 +125,134 @@
 	{%- endif %}
 {%- endif %}
 
-{# acados flags #}
-ACADOS_FLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g
-{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-ACADOS_FLAGS += -DACADOS_WITH_QPOASES
-{%- endif %}
-{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
-ACADOS_FLAGS += -DACADOS_WITH_OSQP
-{%- endif %}
-{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %}
-ACADOS_FLAGS += -DACADOS_WITH_QPDUNES
-{%- endif %}
-# # Debugging
-# ACADOS_FLAGS += -g3
+# define sources and use make's implicit rules to generate object files (*.o)
 
-MODEL_OBJ=
+# model
+MODEL_SRC=
 {%- if  solver_options.integrator_type == "ERK" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.o
-{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.o
-{%- endif %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.c
+	{%- if hessian_approx == "EXACT" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.c
+	{%- endif %}
 {%- elif solver_options.integrator_type == "IRK" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.o
-{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
-{%- endif %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c
+	{%- if hessian_approx == "EXACT" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
+	{%- endif %}
 {%- elif solver_options.integrator_type == "LIFTED_IRK" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.o
-{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
-{%- endif %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
+	{%- if hessian_approx == "EXACT" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.c
+	{%- endif %}
 {%- elif solver_options.integrator_type == "GNSF" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.o
+	{% if model.gnsf.purely_linear != 1 %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c
+		{% if model.gnsf.nontrivial_f_LO == 1 %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
+		{%- endif %}
+	{%- endif %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c
 {%- elif solver_options.integrator_type == "DISCRETE" %}
-{%- if model.dyn_ext_fun_type == "casadi" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.o
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.o
-{%- if hessian_approx == "EXACT" %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.o
-{%- endif %}
-{%- else %}
-MODEL_OBJ+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
-{%- endif %}
+	{%- if model.dyn_ext_fun_type == "casadi" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c
+		{%- if hessian_approx == "EXACT" %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c
+		{%- endif %}
+	{%- else %}
+MODEL_SRC+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
+	{%- endif %}
 {%- endif %}
+MODEL_OBJ := $(MODEL_SRC:.c=.o)
 
-
-OCP_OBJ=
+# optimal control problem - mostly CasADi exports
+OCP_SRC=
 {%- if constr_type == "BGP" and dims_nphi > 0 %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.o
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.c
 {%- endif %}
 {%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.o
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c
 {%- endif %}
 
 {%- if constr_type == "BGH" and dims_nh > 0 %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.o
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.o
-{%- if hessian_approx == "EXACT" %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.o
-{%- endif %}
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c
+	{%- if hessian_approx == "EXACT" %}
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
+	{%- endif %}
 {%- endif %}
 
 {%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.o
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.o
-{%- if hessian_approx == "EXACT" %}
-OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.o
-{%- endif %}
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c
+	{%- if hessian_approx == "EXACT" %}
+OCP_SRC+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
+	{%- endif %}
 {%- endif %}
 
 {%- if cost_type_0 == "NONLINEAR_LS" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
 {%- elif cost_type_0 == "EXTERNAL" %}
-{% if cost.cost_ext_fun_type_0 == "casadi" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
-{% else %}
-OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
-{% endif %}
+	{%- if cost.cost_ext_fun_type_0 == "casadi" %}
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
+	{%- else %}
+OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
+	{%- endif %}
 {%- endif %}
 {%- if cost_type == "NONLINEAR_LS" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
 {%- elif cost_type == "EXTERNAL" %}
-{% if cost.cost_ext_fun_type == "casadi" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
-{% elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
-OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
-{% endif %}
+	{%- if cost.cost_ext_fun_type == "casadi" %}
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
+	{%- elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
+OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
+	{%- endif %}
 {%- endif %}
 {%- if cost_type_e == "NONLINEAR_LS" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
 {%- elif cost_type_e == "EXTERNAL" %}
-{% if cost.cost_ext_fun_type_e == "casadi" %}
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
-OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
-{% elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
-OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
-{% endif %}
+	{%- if cost.cost_ext_fun_type_e == "casadi" %}
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
+OCP_SRC+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
+	{%- elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
+OCP_SRC+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
+	{%- endif %}
 {%- endif %}
-OCP_OBJ+= acados_solver_{{ model.name }}.o
+OCP_SRC+= acados_solver_{{ model.name }}.c
+OCP_OBJ := $(OCP_SRC:.c=.o)
 
+# for sim solver
+SIM_SRC= acados_sim_solver_{{ model.name }}.c
+SIM_OBJ := $(SIM_SRC:.c=.o)
 
-SIM_OBJ=
-SIM_OBJ+= acados_sim_solver_{{ model.name }}.o
+# for target example
+EX_SRC= main_{{ model.name }}.c
+EX_OBJ := $(EX_SRC:.c=.o)
+EX_EXE := $(EX_SRC:.c=)
 
-EX_OBJ=
-EX_OBJ+= main_{{ model.name }}.o
-
-EX_SIM_OBJ=
-EX_SIM_OBJ+= main_sim_{{ model.name }}.o
+# for target example_sim
+EX_SIM_SRC= main_sim_{{ model.name }}.c
+EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o)
+EX_SIM_EXE := $(EX_SIM_SRC:.c=)
 
+# combine model, sim and ocp object files
 OBJ=
 OBJ+= $(MODEL_OBJ)
 {%- if solver_options.integrator_type != "DISCRETE" %}
@@ -271,233 +271,103 @@ EXTERNAL_LIB+= {{ model_external_shared_lib_name }}
 INCLUDE_PATH = {{ acados_include_path }}
 LIB_PATH = {{ acados_lib_path }}
 
-{%- if solver_options.integrator_type == "DISCRETE" %}
-all: clean casadi_fun example
+# preprocessor flags for make's implicit rules
+{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
+CPPFLAGS += -DACADOS_WITH_QPOASES
+{%- endif %}
+{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
+CPPFLAGS += -DACADOS_WITH_OSQP
+{%- endif %}
+{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %}
+CPPFLAGS += -DACADOS_WITH_QPDUNES
+{%- endif %}
+CPPFLAGS+= -I$(INCLUDE_PATH)
+CPPFLAGS+= -I$(INCLUDE_PATH)/acados
+CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include
+CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include
+ {%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
+CPPFLAGS+= -I $(INCLUDE_PATH)/qpOASES_e/
+ {%- endif %}
+
+{# c-compiler flags #}
+# define the c-compiler flags for make's implicit rules
+CFLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g
+# # Debugging
+# CFLAGS += -g3
+
+# linker flags
+LDFLAGS+= -L$(LIB_PATH)
+
+# link to libraries
+LDLIBS+= -lacados
+LDLIBS+= -lhpipm
+LDLIBS+= -lblasfeo
+LDLIBS+= -lm
+LDLIBS+= {{ link_libs }}
+
+# libraries
+LIBACADOS_SOLVER=libacados_solver_{{ model.name }}.so
+LIBACADOS_OCP_SOLVER=libacados_ocp_solver_{{ model.name }}.so
+LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so)
+
+# virtual targets
+.PHONY : all clean
+
+#all: clean example_sim example shared_lib
+{% if solver_options.integrator_type == "DISCRETE" -%}
+all: clean example
 shared_lib: ocp_shared_lib
 {%- else %}
-all: clean casadi_fun example_sim example
+all: clean example_sim example
 shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
 {%- endif %}
 
-CASADI_MODEL_SOURCE=
-{%- if solver_options.integrator_type == "ERK" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_fun.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_expl_vde_forw.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_hess.c
-{%- endif %}
-{%- elif solver_options.integrator_type == "IRK" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c
-{%- endif %}
-{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
-# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
-# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c
-{%- endif %}
-{%- elif solver_options.integrator_type == "GNSF" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun_jac_y.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_jac_y_uhat.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_get_matrices_fun.c
-{%- elif solver_options.integrator_type == "DISCRETE" and model.dyn_ext_fun_type == "casadi" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun.c
-CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac_hess.c
-{%- endif %}
-{%- endif %}
-{%- if constr_type == "BGP" and dims_nphi > 0 %}
-CASADI_CON_PHI_SOURCE=
-CASADI_CON_PHI_SOURCE+= {{ model.name }}_phi_constraint.c
-{%- endif %}
-{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
-CASADI_CON_PHI_E_SOURCE=
-CASADI_CON_PHI_E_SOURCE+= {{ model.name }}_phi_e_constraint.c
-{%- endif %}
-{%- if constr_type == "BGH" and dims_nh > 0 %}
-CASADI_CON_H_SOURCE=
-CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt.c
-CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
-{%- endif %}
+# some linker targets
+example: $(EX_OBJ) $(OBJ)
+	$(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS)
+
+example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ)
+	$(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS)
+
+{% if solver_options.integrator_type != "DISCRETE" -%}
+bundled_shared_lib: $(OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS)
 {%- endif %}
 
-{%- if dims_nh_e > 0 %}
-CASADI_CON_H_E_SOURCE=
-CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
-CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun.c
-{%- if hessian_approx == "EXACT" %}
-CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
-{%- endif %}
-{%- endif %}
+ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \
+	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB)
 
-{%- if cost_type == "NONLINEAR_LS" %}
-CASADI_COST_Y_SOURCE=
-CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun.c
-CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun_jac_ut_xt.c
-CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_hess.c
-{%- endif %}
-{%- if cost_type_e == "NONLINEAR_LS" %}
-CASADI_COST_Y_E_SOURCE=
-CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun.c
-CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun_jac_ut_xt.c
-CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_hess.c
-{%- endif %}
-{%- if cost_type_0 == "NONLINEAR_LS" %}
-CASADI_COST_Y_0_SOURCE=
-CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun.c
-CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun_jac_ut_xt.c
-CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_hess.c
-{%- endif %}
-
-casadi_fun:
-	{%- if model.dyn_ext_fun_type == "casadi" %}
-	( cd {{ model.name }}_model {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_MODEL_SOURCE))
-	{%- endif %}
-	{%- if constr_type == "BGP" and dims_nphi > 0 %}
-	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_SOURCE))
-	{%- endif %}
-	{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
-	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_E_SOURCE))
-	{%- endif %}
-	{%- if constr_type == "BGH" and dims_nh > 0 %}
-	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_SOURCE))
-	{%- endif %}
-	{%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
-	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_E_SOURCE))
-	{%- endif %}
-	{%- if cost_type == "NONLINEAR_LS" %}
-	( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_SOURCE))
-	{%- endif %}
-	{%- if cost_type_e == "NONLINEAR_LS" %}
-	( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_E_SOURCE))
-	{%- endif %}
-	{%- if cost_type_0 == "NONLINEAR_LS" %}
-	( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_0_SOURCE))
-	{%- endif %}
-
-main:
-	gcc $(ACADOS_FLAGS) -c main_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
-
-main_sim:
-	gcc $(ACADOS_FLAGS) -c main_sim_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/
-
-ocp_solver:
-	gcc $(ACADOS_FLAGS) -c acados_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
-
-sim_solver:
-	gcc $(ACADOS_FLAGS) -c acados_sim_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
-
-example: ocp_solver main
-	gcc $(ACADOS_FLAGS) -o main_{{ model.name }} $(EX_OBJ) $(OBJ) -L $(LIB_PATH) \
-	-lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) \
-	-I $(INCLUDE_PATH)/acados/ \
-	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
-	-I $(INCLUDE_PATH)/qpOASES_e/
-	{%- endif %}
+sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ)
+	$(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS)
 
 
-example_sim: sim_solver main_sim
-	gcc $(ACADOS_FLAGS) -o main_sim_{{ model.name }} $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) -L $(LIB_PATH) \
-	-lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/acados/ \
-
-{%- if solver_options.integrator_type != "DISCRETE" %}
-
-bundled_shared_lib: casadi_fun ocp_solver sim_solver
-	gcc $(ACADOS_FLAGS) -shared -o libacados_solver_{{ model.name }}.so $(OBJ) \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) \
-	-L $(LIB_PATH) \
-	-lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
-
-ocp_shared_lib: casadi_fun ocp_solver
-	gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) \
-	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
-	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
-
-{%- else %}
-
-ocp_shared_lib: casadi_fun ocp_solver
-	gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
-	-I $(INCLUDE_PATH)/blasfeo/include/ \
-	-I $(INCLUDE_PATH)/hpipm/include/ \
-	-I $(INCLUDE_PATH) \
-	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
-	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
-
-{%- endif %}
-
+# Cython targets
 ocp_cython_c: ocp_shared_lib
 	cython \
 	-o acados_ocp_solver_pyx.c \
 	-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
 	$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
+	-I {{ code_export_directory }} \
 
 ocp_cython_o: ocp_cython_c
-	clang $(ACADOS_FLAGS) -c -O2 \
+	$(CC) $(ACADOS_FLAGS) -c -O2 \
+	-fPIC \
 	-o acados_ocp_solver_pyx.o \
 	-I /usr/include/python3.8 \
 	-I $(INCLUDE_PATH)/blasfeo/include/ \
 	-I $(INCLUDE_PATH)/hpipm/include/ \
 	-I $(INCLUDE_PATH) \
+	-I {{ cython_include_dirs }} \
 	acados_ocp_solver_pyx.c \
 
 ocp_cython: ocp_cython_o
-	clang $(ACADOS_FLAGS) -shared \
+	$(CC) $(ACADOS_FLAGS) -shared \
 	-o acados_ocp_solver_pyx.so \
 	-Wl,-rpath=$(LIB_PATH) \
 	acados_ocp_solver_pyx.o \
 	$(abspath .)/libacados_ocp_solver_{{ model.name }}.so \
-	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo -lqpOASES_e \
-	{{ link_libs }} \
-	-lm \
-
-sim_shared_lib: casadi_fun sim_solver
-	gcc $(ACADOS_FLAGS) -shared -o libacados_sim_solver_{{ model.name }}.so $(SIM_OBJ) $(MODEL_OBJ) -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
-	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
-	{{ link_libs }} \
-	-lm \
+	$(LDFLAGS) $(LDLIBS)
 
 {%- if os and os == "pc" %}
 
@@ -510,15 +380,27 @@ clean_ocp_shared_lib:
 	del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
 	del \Q acados_solver_{{ model.name }}.o 2>nul
 
+clean_ocp_cython:
+	del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
+	del \Q acados_solver_{{ model.name }}.o 2>nul
+	del \Q acados_ocp_solver_pyx.so 2>nul
+	del \Q acados_ocp_solver_pyx.o 2>nul
+
 {%- else %}
 
 clean:
-	rm -f *.o
-	rm -f *.so
-	rm -f main_{{ model.name }}
+	$(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ)
+	$(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER)
+	$(RM) $(EX_EXE) $(EX_SIM_EXE)
 
 clean_ocp_shared_lib:
-	rm -f libacados_ocp_solver_{{ model.name }}.so
-	rm -f acados_solver_{{ model.name }}.o
+	$(RM) $(LIBACADOS_OCP_SOLVER)
+	$(RM) $(OCP_OBJ)
+
+clean_ocp_cython:
+	$(RM) libacados_ocp_solver_{{ model.name }}.so
+	$(RM) acados_solver_{{ model.name }}.o
+	$(RM) acados_ocp_solver_pyx.so
+	$(RM) acados_ocp_solver_pyx.o
 
 {%- endif %}
diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c
index 251b249b2..e67a51567 100644
--- a/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c
@@ -63,7 +63,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     mexPrintf("{{ model.name }}_acados_create() -> success!\n");
 
     // get pointers to nlp solver related objects
-    ocp_nlp_plan *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
+    ocp_nlp_plan_t *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
     ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule);
     ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule);
     ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule);
@@ -238,14 +238,18 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_hess;
 {%- endif %}
 {% elif solver_options.integrator_type == "GNSF" %}
+    {% if model.gnsf.purely_linear != 1 %}
     l_ptr = mxGetData(gnsf_phi_fun_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun;
     l_ptr = mxGetData(gnsf_phi_fun_jac_y_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun_jac_y;
     l_ptr = mxGetData(gnsf_phi_jac_y_uhat_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_jac_y_uhat;
+    {% if model.gnsf.nontrivial_f_LO == 1 %}
     l_ptr = mxGetData(gnsf_f_lo_jac_x1_x1dot_u_z_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_f_lo_jac_x1_x1dot_u_z;
+    {%- endif %}
+    {%- endif %}
     l_ptr = mxGetData(gnsf_get_matrices_fun_mat);
     l_ptr[0] = (long long) acados_ocp_capsule->gnsf_get_matrices_fun;
 {% elif solver_options.integrator_type == "DISCRETE" %}
diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c
index ae8c31155..f8e1e5e44 100644
--- a/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c
@@ -69,7 +69,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     {{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
     // plan
     ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "plan" ) );
-    ocp_nlp_plan *plan = (ocp_nlp_plan *) ptr[0];
+    ocp_nlp_plan_t *plan = (ocp_nlp_plan_t *) ptr[0];
     // config
     ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "config" ) );
     ocp_nlp_config *config = (ocp_nlp_config *) ptr[0];
@@ -404,7 +404,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     }
     else if (!strcmp(field, "init_z"))
     {
-        sim_solver_plan sim_plan = plan->sim_solver_plan[0];
+        sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
         if (type == IRK)
         {
@@ -426,7 +426,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     }
     else if (!strcmp(field, "init_xdot"))
     {
-        sim_solver_plan sim_plan = plan->sim_solver_plan[0];
+        sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
         if (type == IRK)
         {
@@ -448,7 +448,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
     }
     else if (!strcmp(field, "init_gnsf_phi"))
     {
-        sim_solver_plan sim_plan = plan->sim_solver_plan[0];
+        sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
         sim_solver_t type = sim_plan.sim_solver;
         if (type == GNSF)
         {
diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c
index 971fb8a78..f2e75058c 100644
--- a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c
@@ -164,12 +164,17 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
 {%- endif %}
 
     {% elif solver_options.integrator_type == "GNSF" -%}
+  {% if model.gnsf.purely_linear != 1 %}
     capsule->sim_gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
     capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
     capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+  {% 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));
+  {%- endif %}
+  {%- endif %}
     capsule->sim_gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
 
+  {% if model.gnsf.purely_linear != 1 %}
     capsule->sim_gnsf_phi_fun->casadi_fun = &{{ model.name }}_gnsf_phi_fun;
     capsule->sim_gnsf_phi_fun->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in;
     capsule->sim_gnsf_phi_fun->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out;
@@ -194,6 +199,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     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);
 
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out;
@@ -201,6 +207,8 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     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);
+  {%- endif %}
+  {%- endif %}
 
     capsule->sim_gnsf_get_matrices_fun->casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
     capsule->sim_gnsf_get_matrices_fun->casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
@@ -212,7 +220,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
     {% endif %}
 
     // sim plan & config
-    sim_solver_plan plan;
+    sim_solver_plan_t plan;
     plan.sim_solver = {{ solver_options.integrator_type }};
 
     // create correct config based on plan
@@ -307,14 +315,18 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
                 "expl_ode_hess", capsule->sim_expl_ode_hess);
 {%- endif %}
 {%- elif solver_options.integrator_type == "GNSF" %}
+  {% if model.gnsf.purely_linear != 1 %}
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "phi_fun", capsule->sim_gnsf_phi_fun);
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "phi_fun_jac_y", capsule->sim_gnsf_phi_fun_jac_y);
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "phi_jac_y_uhat", capsule->sim_gnsf_phi_jac_y_uhat);
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "f_lo_jac_x1_x1dot_u_z", capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
+  {%- endif %}
+  {%- endif %}
     {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
                  "gnsf_get_matrices_fun", capsule->sim_gnsf_get_matrices_fun);
 {%- endif %}
@@ -409,10 +421,14 @@ int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule)
     external_function_param_casadi_free(capsule->sim_expl_ode_hess);
 {%- endif %}
 {%- elif solver_options.integrator_type == "GNSF" %}
+  {% if model.gnsf.purely_linear != 1 %}
     external_function_param_casadi_free(capsule->sim_gnsf_phi_fun);
     external_function_param_casadi_free(capsule->sim_gnsf_phi_fun_jac_y);
     external_function_param_casadi_free(capsule->sim_gnsf_phi_jac_y_uhat);
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     external_function_param_casadi_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
+  {%- endif %}
+  {%- endif %}
     external_function_param_casadi_free(capsule->sim_gnsf_get_matrices_fun);
 {% endif %}
 
@@ -445,10 +461,14 @@ int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, doubl
     capsule->sim_impl_dae_hess[0].set_param(capsule->sim_impl_dae_hess, p);
 {%- endif %}
 {%- elif solver_options.integrator_type == "GNSF" %}
+  {% if model.gnsf.purely_linear != 1 %}
     capsule->sim_gnsf_phi_fun[0].set_param(capsule->sim_gnsf_phi_fun, p);
     capsule->sim_gnsf_phi_fun_jac_y[0].set_param(capsule->sim_gnsf_phi_fun_jac_y, p);
     capsule->sim_gnsf_phi_jac_y_uhat[0].set_param(capsule->sim_gnsf_phi_jac_y_uhat, p);
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p);
+  {%- endif %}
+  {%- endif %}
     capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p);
 {% endif %}
 
diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c
index 234295fa2..68a6a3f80 100644
--- a/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c
@@ -37,7 +37,7 @@
 #define MDL_START
 
 // acados
-#include "acados/utils/print.h"
+// #include "acados/utils/print.h"
 #include "acados_c/ocp_nlp_interface.h"
 #include "acados_c/external_function_interface.h"
 
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_solver.in.c
index 6e13242a4..a42937ed2 100644
--- a/pyextra/acados_template/c_templates_tera/acados_solver.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.c
@@ -34,8 +34,9 @@
 // standard
 #include <stdio.h>
 #include <stdlib.h>
+#include <assert.h>
 // acados
-#include "acados/utils/print.h"
+// #include "acados/utils/print.h"
 #include "acados_c/ocp_nlp_interface.h"
 #include "acados_c/external_function_interface.h"
 
@@ -121,14 +122,15 @@ int {{ model.name }}_acados_free_capsule({{ model.name }}_solver_capsule *capsul
 }
 
 
-int {{ model.name }}_acados_create({{ model.name }}_solver_capsule * capsule)
+int {{ model.name }}_acados_create({{ model.name }}_solver_capsule* capsule)
 {
     int N_shooting_intervals = {{ model.name | upper }}_N;
     double* new_time_steps = NULL; // NULL -> don't alter the code generated time-steps
     return {{ model.name }}_acados_create_with_discretization(capsule, N_shooting_intervals, new_time_steps);
 }
 
-int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps)
+
+int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule* capsule, int N, double* new_time_steps)
 {
     if (N != capsule->nlp_solver_plan->N) {
         fprintf(stderr, "{{ model.name }}_acados_update_time_steps: given number of time steps (= %d) " \
@@ -151,30 +153,20 @@ int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule *
     return 0;
 }
 
-int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps)
+/**
+ * Internal function for {{ model.name }}_acados_create: step 1
+ */
+void {{ model.name }}_acados_create_1_set_plan(ocp_nlp_plan_t* nlp_solver_plan, const int N)
 {
-    int status = 0;
-    // If N does not match the number of shooting intervals used for code generation, new_time_steps must be given.
-    if (N != {{ model.name | upper }}_N && !new_time_steps) {
-        fprintf(stderr, "{{ model.name }}_acados_create_with_discretization: new_time_steps is NULL " \
-            "but the number of shooting intervals (= %d) differs from the number of " \
-            "shooting intervals (= %d) during code generation! Please provide a new vector of time_stamps!\n", \
-             N, {{ model.name | upper }}_N);
-        return 1;
-    }
-
-    // number of expected runtime parameters
-    capsule->nlp_np = NP;
+    assert(N == nlp_solver_plan->N);
 
     /************************************************
-    *  plan & config
+    *  plan
     ************************************************/
-    ocp_nlp_plan * nlp_solver_plan = ocp_nlp_plan_create(N);
-    capsule->nlp_solver_plan = nlp_solver_plan;
 
     {%- if solver_options.nlp_solver_type == "SQP" %}
     nlp_solver_plan->nlp_solver = SQP;
-    {% else %}
+    {%- else %}
     nlp_solver_plan->nlp_solver = SQP_RTI;
     {%- endif %}
 
@@ -188,11 +180,11 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 
     for (int i = 0; i < N; i++)
     {
-        {% if solver_options.integrator_type == "DISCRETE" %}
+        {%- if solver_options.integrator_type == "DISCRETE" %}
         nlp_solver_plan->nlp_dynamics[i] = DISCRETE_MODEL;
         // discrete dynamics does not need sim solver option, this field is ignored
         nlp_solver_plan->sim_solver_plan[i].sim_solver = INVALID_SIM_SOLVER;
-        {% else %}
+        {%- else %}
         nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL;
         nlp_solver_plan->sim_solver_plan[i].sim_solver = {{ solver_options.integrator_type }};
         {%- endif %}
@@ -200,7 +192,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 
     for (int i = 0; i < N; i++)
     {
-        {% if constraints.constr_type == "BGP" %}
+        {%- if constraints.constr_type == "BGP" %}
         nlp_solver_plan->nlp_constraints[i] = BGP;
         {%- else -%}
         nlp_solver_plan->nlp_constraints[i] = BGH;
@@ -209,7 +201,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 
     {%- if constraints.constr_type_e == "BGP" %}
     nlp_solver_plan->nlp_constraints[N] = BGP;
-    {% else %}
+    {%- else %}
     nlp_solver_plan->nlp_constraints[N] = BGH;
     {%- endif %}
 
@@ -226,10 +218,18 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     nlp_solver_plan->regularization = CONVEXIFY;
     {%- endif %}
 {%- endif %}
-    ocp_nlp_config * nlp_config = ocp_nlp_config_create(*nlp_solver_plan);
-    capsule->nlp_config = nlp_config;
+}
 
 
+/**
+ * Internal function for {{ model.name }}_acados_create: step 2
+ */
+ocp_nlp_dims* {{ model.name }}_acados_create_2_create_and_set_dimensions({{ model.name }}_solver_capsule* capsule)
+{
+    ocp_nlp_plan_t* nlp_solver_plan = capsule->nlp_solver_plan;
+    const int N = nlp_solver_plan->N;
+    ocp_nlp_config* nlp_config = capsule->nlp_config;
+
     /************************************************
     *  dimensions
     ************************************************/
@@ -307,7 +307,6 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 
     /* create and set ocp_nlp_dims */
     ocp_nlp_dims * nlp_dims = ocp_nlp_dims_create(nlp_config);
-    capsule->nlp_dims = nlp_dims;
 
     ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nx", nx);
     ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nu", nu);
@@ -325,14 +324,14 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         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" %}
     ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]);
-    {%- endif %}
+{%- 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" %}
     for (int i = 1; i < N; i++)
         ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]);
-    {%- endif %}
+{%- endif %}
 
     for (int i = 0; i < N; i++)
     {
@@ -346,21 +345,20 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         {%- endif %}
     }
 
-    {%- if constraints.constr_type_e == "BGH" %}
+{%- if constraints.constr_type_e == "BGH" %}
     ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]);
     ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsh", &nsh[N]);
-    {%- elif constraints.constr_type_e == "BGP" %}
+{%- elif constraints.constr_type_e == "BGP" %}
     ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nr", &nr[N]);
     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" %}
+{%- endif %}
+{%- if cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" %}
     ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]);
-    {%- endif %}
-
+{%- endif %}
     free(intNp1mem);
 
-{% if solver_options.integrator_type == "GNSF" -%}
+{%- if solver_options.integrator_type == "GNSF" -%}
     // GNSF specific dimensions
     int gnsf_nx1 = {{ dims.gnsf_nx1 }};
     int gnsf_nz1 = {{ dims.gnsf_nz1 }};
@@ -380,136 +378,91 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         }
     }
 {%- endif %}
+return nlp_dims;
+}
+
+
+/**
+ * Internal function for {{ model.name }}_acados_create: step 3
+ */
+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
     ************************************************/
-    {%- if constraints.constr_type == "BGP" %}
+
+#define MAP_CASADI_FNC(__CAPSULE_FNC__, __MODEL_BASE_FNC__) do{ \
+        capsule->__CAPSULE_FNC__.casadi_fun = & __MODEL_BASE_FNC__ ;\
+        capsule->__CAPSULE_FNC__.casadi_n_in = & __MODEL_BASE_FNC__ ## _n_in; \
+        capsule->__CAPSULE_FNC__.casadi_n_out = & __MODEL_BASE_FNC__ ## _n_out; \
+        capsule->__CAPSULE_FNC__.casadi_sparsity_in = & __MODEL_BASE_FNC__ ## _sparsity_in; \
+        capsule->__CAPSULE_FNC__.casadi_sparsity_out = & __MODEL_BASE_FNC__ ## _sparsity_out; \
+        capsule->__CAPSULE_FNC__.casadi_work = & __MODEL_BASE_FNC__ ## _work; \
+        external_function_param_casadi_create(&capsule->__CAPSULE_FNC__ , {{ dims.np }}); \
+    }while(false)
+
+{% if constraints.constr_type == "BGP" %}
+    // constraints.constr_type == "BGP"
     capsule->phi_constraint = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++)
     {
         // nonlinear part of convex-composite constraint
-        capsule->phi_constraint[i].casadi_fun = &{{ model.name }}_phi_constraint;
-        capsule->phi_constraint[i].casadi_n_in = &{{ model.name }}_phi_constraint_n_in;
-        capsule->phi_constraint[i].casadi_n_out = &{{ model.name }}_phi_constraint_n_out;
-        capsule->phi_constraint[i].casadi_sparsity_in = &{{ model.name }}_phi_constraint_sparsity_in;
-        capsule->phi_constraint[i].casadi_sparsity_out = &{{ model.name }}_phi_constraint_sparsity_out;
-        capsule->phi_constraint[i].casadi_work = &{{ model.name }}_phi_constraint_work;
-
-        external_function_param_casadi_create(&capsule->phi_constraint[i], {{ dims.np }});
+        MAP_CASADI_FNC(phi_constraint[i], {{ model.name }}_phi_constraint);
     }
-    {%- endif %}
+{%- endif %}
 
-    {%- if constraints.constr_type_e == "BGP" %}
+{%- if constraints.constr_type_e == "BGP" %}
+    // constraints.constr_type_e == "BGP"
     // nonlinear part of convex-composite constraint
-    capsule->phi_e_constraint.casadi_fun = &{{ model.name }}_phi_e_constraint;
-    capsule->phi_e_constraint.casadi_n_in = &{{ model.name }}_phi_e_constraint_n_in;
-    capsule->phi_e_constraint.casadi_n_out = &{{ model.name }}_phi_e_constraint_n_out;
-    capsule->phi_e_constraint.casadi_sparsity_in = &{{ model.name }}_phi_e_constraint_sparsity_in;
-    capsule->phi_e_constraint.casadi_sparsity_out = &{{ model.name }}_phi_e_constraint_sparsity_out;
-    capsule->phi_e_constraint.casadi_work = &{{ model.name }}_phi_e_constraint_work;
+    MAP_CASADI_FNC(phi_e_constraint, {{ model.name }}_phi_e_constraint);
+{%- endif %}
 
-    external_function_param_casadi_create(&capsule->phi_e_constraint, {{ dims.np }});
-    {% endif %}
-
-    {%- if constraints.constr_type == "BGH" and dims.nh > 0  %}
+{%- if constraints.constr_type == "BGH" and dims.nh > 0  %}
+    // constraints.constr_type == "BGH" and dims.nh > 0
     capsule->nl_constr_h_fun_jac = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->nl_constr_h_fun_jac[i].casadi_fun = &{{ model.name }}_constr_h_fun_jac_uxt_zt;
-        capsule->nl_constr_h_fun_jac[i].casadi_n_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_n_in;
-        capsule->nl_constr_h_fun_jac[i].casadi_n_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_n_out;
-        capsule->nl_constr_h_fun_jac[i].casadi_sparsity_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in;
-        capsule->nl_constr_h_fun_jac[i].casadi_sparsity_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out;
-        capsule->nl_constr_h_fun_jac[i].casadi_work = &{{ model.name }}_constr_h_fun_jac_uxt_zt_work;
-        external_function_param_casadi_create(&capsule->nl_constr_h_fun_jac[i], {{ dims.np }});
+        MAP_CASADI_FNC(nl_constr_h_fun_jac[i], {{ model.name }}_constr_h_fun_jac_uxt_zt);
     }
     capsule->nl_constr_h_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->nl_constr_h_fun[i].casadi_fun = &{{ model.name }}_constr_h_fun;
-        capsule->nl_constr_h_fun[i].casadi_n_in = &{{ model.name }}_constr_h_fun_n_in;
-        capsule->nl_constr_h_fun[i].casadi_n_out = &{{ model.name }}_constr_h_fun_n_out;
-        capsule->nl_constr_h_fun[i].casadi_sparsity_in = &{{ model.name }}_constr_h_fun_sparsity_in;
-        capsule->nl_constr_h_fun[i].casadi_sparsity_out = &{{ model.name }}_constr_h_fun_sparsity_out;
-        capsule->nl_constr_h_fun[i].casadi_work = &{{ model.name }}_constr_h_fun_work;
-        external_function_param_casadi_create(&capsule->nl_constr_h_fun[i], {{ dims.np }});
+        MAP_CASADI_FNC(nl_constr_h_fun[i], {{ model.name }}_constr_h_fun);
     }
     {% if solver_options.hessian_approx == "EXACT" %}
     capsule->nl_constr_h_fun_jac_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->nl_constr_h_fun_jac_hess[i].casadi_fun = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess;
-        capsule->nl_constr_h_fun_jac_hess[i].casadi_n_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in;
-        capsule->nl_constr_h_fun_jac_hess[i].casadi_n_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out;
-        capsule->nl_constr_h_fun_jac_hess[i].casadi_sparsity_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in;
-        capsule->nl_constr_h_fun_jac_hess[i].casadi_sparsity_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out;
-        capsule->nl_constr_h_fun_jac_hess[i].casadi_work = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work;
-
-        external_function_param_casadi_create(&capsule->nl_constr_h_fun_jac_hess[i], {{ dims.np }});
+        MAP_CASADI_FNC(nl_constr_h_fun_jac_hess[i], {{ model.name }}_constr_h_fun_jac_uxt_zt_hess);
     }
     {% endif %}
+{% endif %}
+
+{%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
+    MAP_CASADI_FNC(nl_constr_h_e_fun_jac, {{ model.name }}_constr_h_e_fun_jac_uxt_zt);
+    MAP_CASADI_FNC(nl_constr_h_e_fun, {{ model.name }}_constr_h_e_fun);
+
+    {%- if solver_options.hessian_approx == "EXACT" %}
+    MAP_CASADI_FNC(nl_constr_h_e_fun_jac_hess, {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess);
     {% endif %}
-
-    {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
-    capsule->nl_constr_h_e_fun_jac.casadi_fun = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt;
-    capsule->nl_constr_h_e_fun_jac.casadi_n_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in;
-    capsule->nl_constr_h_e_fun_jac.casadi_n_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out;
-    capsule->nl_constr_h_e_fun_jac.casadi_sparsity_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_in;
-    capsule->nl_constr_h_e_fun_jac.casadi_sparsity_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_out;
-    capsule->nl_constr_h_e_fun_jac.casadi_work = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_work;
-    external_function_param_casadi_create(&capsule->nl_constr_h_e_fun_jac, {{ dims.np }});
-
-    capsule->nl_constr_h_e_fun.casadi_fun = &{{ model.name }}_constr_h_e_fun;
-    capsule->nl_constr_h_e_fun.casadi_n_in = &{{ model.name }}_constr_h_e_fun_n_in;
-    capsule->nl_constr_h_e_fun.casadi_n_out = &{{ model.name }}_constr_h_e_fun_n_out;
-    capsule->nl_constr_h_e_fun.casadi_sparsity_in = &{{ model.name }}_constr_h_e_fun_sparsity_in;
-    capsule->nl_constr_h_e_fun.casadi_sparsity_out = &{{ model.name }}_constr_h_e_fun_sparsity_out;
-    capsule->nl_constr_h_e_fun.casadi_work = &{{ model.name }}_constr_h_e_fun_work;
-    external_function_param_casadi_create(&capsule->nl_constr_h_e_fun, {{ dims.np }});
-
-    {% if solver_options.hessian_approx == "EXACT" %}
-    capsule->nl_constr_h_e_fun_jac_hess.casadi_fun = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess;
-    capsule->nl_constr_h_e_fun_jac_hess.casadi_n_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in;
-    capsule->nl_constr_h_e_fun_jac_hess.casadi_n_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out;
-    capsule->nl_constr_h_e_fun_jac_hess.casadi_sparsity_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_in;
-    capsule->nl_constr_h_e_fun_jac_hess.casadi_sparsity_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_out;
-    capsule->nl_constr_h_e_fun_jac_hess.casadi_work = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_work;
-    external_function_param_casadi_create(&capsule->nl_constr_h_e_fun_jac_hess, {{ dims.np }});
-    {% endif %}
-    {%- endif %}
+{%- endif %}
 
 {% if solver_options.integrator_type == "ERK" %}
     // explicit ode
     capsule->forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->forw_vde_casadi[i].casadi_fun = &{{ model.name }}_expl_vde_forw;
-        capsule->forw_vde_casadi[i].casadi_n_in = &{{ model.name }}_expl_vde_forw_n_in;
-        capsule->forw_vde_casadi[i].casadi_n_out = &{{ model.name }}_expl_vde_forw_n_out;
-        capsule->forw_vde_casadi[i].casadi_sparsity_in = &{{ model.name }}_expl_vde_forw_sparsity_in;
-        capsule->forw_vde_casadi[i].casadi_sparsity_out = &{{ model.name }}_expl_vde_forw_sparsity_out;
-        capsule->forw_vde_casadi[i].casadi_work = &{{ model.name }}_expl_vde_forw_work;
-        external_function_param_casadi_create(&capsule->forw_vde_casadi[i], {{ dims.np }});
+        MAP_CASADI_FNC(forw_vde_casadi[i], {{ model.name }}_expl_vde_forw);
     }
 
     capsule->expl_ode_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->expl_ode_fun[i].casadi_fun = &{{ model.name }}_expl_ode_fun;
-        capsule->expl_ode_fun[i].casadi_n_in = &{{ model.name }}_expl_ode_fun_n_in;
-        capsule->expl_ode_fun[i].casadi_n_out = &{{ model.name }}_expl_ode_fun_n_out;
-        capsule->expl_ode_fun[i].casadi_sparsity_in = &{{ model.name }}_expl_ode_fun_sparsity_in;
-        capsule->expl_ode_fun[i].casadi_sparsity_out = &{{ model.name }}_expl_ode_fun_sparsity_out;
-        capsule->expl_ode_fun[i].casadi_work = &{{ model.name }}_expl_ode_fun_work;
-        external_function_param_casadi_create(&capsule->expl_ode_fun[i], {{ dims.np }});
+        MAP_CASADI_FNC(expl_ode_fun[i], {{ model.name }}_expl_ode_fun);
     }
 
     {%- if solver_options.hessian_approx == "EXACT" %}
     capsule->hess_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->hess_vde_casadi[i].casadi_fun = &{{ model.name }}_expl_ode_hess;
-        capsule->hess_vde_casadi[i].casadi_n_in = &{{ model.name }}_expl_ode_hess_n_in;
-        capsule->hess_vde_casadi[i].casadi_n_out = &{{ model.name }}_expl_ode_hess_n_out;
-        capsule->hess_vde_casadi[i].casadi_sparsity_in = &{{ model.name }}_expl_ode_hess_sparsity_in;
-        capsule->hess_vde_casadi[i].casadi_sparsity_out = &{{ model.name }}_expl_ode_hess_sparsity_out;
-        capsule->hess_vde_casadi[i].casadi_work = &{{ model.name }}_expl_ode_hess_work;
-        external_function_param_casadi_create(&capsule->hess_vde_casadi[i], {{ dims.np }});
+        MAP_CASADI_FNC(hess_vde_casadi[i], {{ model.name }}_expl_ode_hess);
     }
     {%- endif %}
 
@@ -517,126 +470,64 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     // implicit dae
     capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->impl_dae_fun[i].casadi_fun = &{{ model.name }}_impl_dae_fun;
-        capsule->impl_dae_fun[i].casadi_work = &{{ model.name }}_impl_dae_fun_work;
-        capsule->impl_dae_fun[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in;
-        capsule->impl_dae_fun[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out;
-        capsule->impl_dae_fun[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in;
-        capsule->impl_dae_fun[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out;
-        external_function_param_casadi_create(&capsule->impl_dae_fun[i], {{ dims.np }});
+        MAP_CASADI_FNC(impl_dae_fun[i], {{ model.name }}_impl_dae_fun);
     }
 
     capsule->impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z;
-        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_work;
-        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in;
-        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out;
-        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in;
-        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out;
-        external_function_param_casadi_create(&capsule->impl_dae_fun_jac_x_xdot_z[i], {{ dims.np }});
+        MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_z[i], {{ model.name }}_impl_dae_fun_jac_x_xdot_z);
     }
 
     capsule->impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_fun = &{{ model.name }}_impl_dae_jac_x_xdot_u_z;
-        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_work = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_work;
-        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in;
-        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out;
-        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_n_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in;
-        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_n_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out;
-        external_function_param_casadi_create(&capsule->impl_dae_jac_x_xdot_u_z[i], {{ dims.np }});
+        MAP_CASADI_FNC(impl_dae_jac_x_xdot_u_z[i], {{ model.name }}_impl_dae_jac_x_xdot_u_z);
     }
 
     {%- if solver_options.hessian_approx == "EXACT" %}
     capsule->impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->impl_dae_hess[i].casadi_fun = &{{ model.name }}_impl_dae_hess;
-        capsule->impl_dae_hess[i].casadi_work = &{{ model.name }}_impl_dae_hess_work;
-        capsule->impl_dae_hess[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_hess_sparsity_in;
-        capsule->impl_dae_hess[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_hess_sparsity_out;
-        capsule->impl_dae_hess[i].casadi_n_in = &{{ model.name }}_impl_dae_hess_n_in;
-        capsule->impl_dae_hess[i].casadi_n_out = &{{ model.name }}_impl_dae_hess_n_out;
-        external_function_param_casadi_create(&capsule->impl_dae_hess[i], {{ dims.np }});
+        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);
     for (int i = 0; i < N; i++) {
-        capsule->impl_dae_fun[i].casadi_fun = &{{ model.name }}_impl_dae_fun;
-        capsule->impl_dae_fun[i].casadi_work = &{{ model.name }}_impl_dae_fun_work;
-        capsule->impl_dae_fun[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in;
-        capsule->impl_dae_fun[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out;
-        capsule->impl_dae_fun[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in;
-        capsule->impl_dae_fun[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out;
-        external_function_param_casadi_create(&capsule->impl_dae_fun[i], {{ dims.np }});
+        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);    for (int i = 0; i < N; i++) {
-        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u;
-        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_work;
-        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_in;
-        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out;
-        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in;
-        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out;
-        external_function_param_casadi_create(&capsule->impl_dae_fun_jac_x_xdot_u[i], {{ dims.np }});
+    capsule->impl_dae_fun_jac_x_xdot_u = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*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);
     }
 
 {% elif solver_options.integrator_type == "GNSF" %}
+    {% if model.gnsf.purely_linear != 1 %}
     capsule->gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->gnsf_phi_fun[i].casadi_fun = &{{ model.name }}_gnsf_phi_fun;
-        capsule->gnsf_phi_fun[i].casadi_work = &{{ model.name }}_gnsf_phi_fun_work;
-        capsule->gnsf_phi_fun[i].casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_sparsity_in;
-        capsule->gnsf_phi_fun[i].casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_sparsity_out;
-        capsule->gnsf_phi_fun[i].casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in;
-        capsule->gnsf_phi_fun[i].casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out;
-        external_function_param_casadi_create(&capsule->gnsf_phi_fun[i], {{ dims.np }});
+        MAP_CASADI_FNC(gnsf_phi_fun[i], {{ model.name }}_gnsf_phi_fun);
     }
 
     capsule->gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->gnsf_phi_fun_jac_y[i].casadi_fun = &{{ model.name }}_gnsf_phi_fun_jac_y;
-        capsule->gnsf_phi_fun_jac_y[i].casadi_work = &{{ model.name }}_gnsf_phi_fun_jac_y_work;
-        capsule->gnsf_phi_fun_jac_y[i].casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in;
-        capsule->gnsf_phi_fun_jac_y[i].casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out;
-        capsule->gnsf_phi_fun_jac_y[i].casadi_n_in = &{{ model.name }}_gnsf_phi_fun_jac_y_n_in;
-        capsule->gnsf_phi_fun_jac_y[i].casadi_n_out = &{{ model.name }}_gnsf_phi_fun_jac_y_n_out;
-        external_function_param_casadi_create(&capsule->gnsf_phi_fun_jac_y[i], {{ dims.np }});
+        MAP_CASADI_FNC(gnsf_phi_fun_jac_y[i], {{ model.name }}_gnsf_phi_fun_jac_y);
     }
 
     capsule->gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->gnsf_phi_jac_y_uhat[i].casadi_fun = &{{ model.name }}_gnsf_phi_jac_y_uhat;
-        capsule->gnsf_phi_jac_y_uhat[i].casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work;
-        capsule->gnsf_phi_jac_y_uhat[i].casadi_sparsity_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in;
-        capsule->gnsf_phi_jac_y_uhat[i].casadi_sparsity_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out;
-        capsule->gnsf_phi_jac_y_uhat[i].casadi_n_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_in;
-        capsule->gnsf_phi_jac_y_uhat[i].casadi_n_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_out;
-        external_function_param_casadi_create(&capsule->gnsf_phi_jac_y_uhat[i], {{ dims.np }});
+        MAP_CASADI_FNC(gnsf_phi_jac_y_uhat[i], {{ model.name }}_gnsf_phi_jac_y_uhat);
     }
 
+    {% if model.gnsf.nontrivial_f_LO == 1 %}
     capsule->gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work;
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_sparsity_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in;
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out;
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out;
-        external_function_param_casadi_create(&capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i], {{ dims.np }});
+        MAP_CASADI_FNC(gnsf_f_lo_jac_x1_x1dot_u_z[i], {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz);
     }
-
+    {%- endif %}
+    {%- endif %}
     capsule->gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N; i++) {
-        capsule->gnsf_get_matrices_fun[i].casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
-        capsule->gnsf_get_matrices_fun[i].casadi_work = &{{ model.name }}_gnsf_get_matrices_fun_work;
-        capsule->gnsf_get_matrices_fun[i].casadi_sparsity_in = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_in;
-        capsule->gnsf_get_matrices_fun[i].casadi_sparsity_out = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_out;
-        capsule->gnsf_get_matrices_fun[i].casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
-        capsule->gnsf_get_matrices_fun[i].casadi_n_out = &{{ model.name }}_gnsf_get_matrices_fun_n_out;
-        external_function_param_casadi_create(&capsule->gnsf_get_matrices_fun[i], {{ dims.np }});
+        MAP_CASADI_FNC(gnsf_get_matrices_fun[i], {{ model.name }}_gnsf_get_matrices_fun);
     }
 {% elif solver_options.integrator_type == "DISCRETE" %}
     // discrete dynamics
@@ -644,32 +535,22 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     for (int i = 0; i < N; i++)
     {
         {%- if model.dyn_ext_fun_type == "casadi" %}
-        capsule->discr_dyn_phi_fun[i].casadi_fun = &{{ model.name }}_dyn_disc_phi_fun;
-        capsule->discr_dyn_phi_fun[i].casadi_n_in = &{{ model.name }}_dyn_disc_phi_fun_n_in;
-        capsule->discr_dyn_phi_fun[i].casadi_n_out = &{{ model.name }}_dyn_disc_phi_fun_n_out;
-        capsule->discr_dyn_phi_fun[i].casadi_sparsity_in = &{{ model.name }}_dyn_disc_phi_fun_sparsity_in;
-        capsule->discr_dyn_phi_fun[i].casadi_sparsity_out = &{{ model.name }}_dyn_disc_phi_fun_sparsity_out;
-        capsule->discr_dyn_phi_fun[i].casadi_work = &{{ model.name }}_dyn_disc_phi_fun_work;
+        MAP_CASADI_FNC(discr_dyn_phi_fun[i], {{ model.name }}_dyn_disc_phi_fun);
         {%- else %}
         capsule->discr_dyn_phi_fun[i].fun = &{{ model.dyn_disc_fun }};
-        {%- endif %}
         external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun[i], {{ dims.np }});
+        {%- endif %}
     }
 
     capsule->discr_dyn_phi_fun_jac_ut_xt = (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" %}
-        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_fun = &{{ model.name }}_dyn_disc_phi_fun_jac;
-        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_n_in = &{{ model.name }}_dyn_disc_phi_fun_jac_n_in;
-        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_n_out = &{{ model.name }}_dyn_disc_phi_fun_jac_n_out;
-        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_sparsity_in = &{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_in;
-        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_sparsity_out = &{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_out;
-        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_work = &{{ model.name }}_dyn_disc_phi_fun_jac_work;
+        MAP_CASADI_FNC(discr_dyn_phi_fun_jac_ut_xt[i], {{ model.name }}_dyn_disc_phi_fun_jac);
         {%- else %}
         capsule->discr_dyn_phi_fun_jac_ut_xt[i].fun = &{{ model.dyn_disc_fun_jac }};
-        {%- endif %}
         external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt[i], {{ dims.np }});
+        {%- endif %}
     }
 
   {%- if solver_options.hessian_approx == "EXACT" %}
@@ -677,86 +558,46 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     for (int i = 0; i < N; i++)
     {
         {%- if model.dyn_ext_fun_type == "casadi" %}
-        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_fun = &{{ model.name }}_dyn_disc_phi_fun_jac_hess;
-        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_n_in = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_n_in;
-        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_n_out = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_n_out;
-        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_sparsity_in = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_in;
-        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_sparsity_out = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_out;
-        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_work = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_work;
+        MAP_CASADI_FNC(discr_dyn_phi_fun_jac_ut_xt_hess[i], {{ model.name }}_dyn_disc_phi_fun_jac_hess);
         {%- else %}
         capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].fun = &{{ model.dyn_disc_fun_jac_hess }};
-        {%- endif %}
         external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i], {{ dims.np }});
+        {%- endif %}
     }
   {%- endif %}
 {%- endif %}
 
 
 {%- if cost.cost_type_0 == "NONLINEAR_LS" %}
-    // nonlinear least square function
-    capsule->cost_y_0_fun.casadi_fun = &{{ model.name }}_cost_y_0_fun;
-    capsule->cost_y_0_fun.casadi_n_in = &{{ model.name }}_cost_y_0_fun_n_in;
-    capsule->cost_y_0_fun.casadi_n_out = &{{ model.name }}_cost_y_0_fun_n_out;
-    capsule->cost_y_0_fun.casadi_sparsity_in = &{{ model.name }}_cost_y_0_fun_sparsity_in;
-    capsule->cost_y_0_fun.casadi_sparsity_out = &{{ model.name }}_cost_y_0_fun_sparsity_out;
-    capsule->cost_y_0_fun.casadi_work = &{{ model.name }}_cost_y_0_fun_work;
-    external_function_param_casadi_create(&capsule->cost_y_0_fun, {{ dims.np }});
-
-    capsule->cost_y_0_fun_jac_ut_xt.casadi_fun = &{{ model.name }}_cost_y_0_fun_jac_ut_xt;
-    capsule->cost_y_0_fun_jac_ut_xt.casadi_n_in = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in;
-    capsule->cost_y_0_fun_jac_ut_xt.casadi_n_out = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out;
-    capsule->cost_y_0_fun_jac_ut_xt.casadi_sparsity_in = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in;
-    capsule->cost_y_0_fun_jac_ut_xt.casadi_sparsity_out = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out;
-    capsule->cost_y_0_fun_jac_ut_xt.casadi_work = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_work;
-    external_function_param_casadi_create(&capsule->cost_y_0_fun_jac_ut_xt, {{ dims.np }});
-
-    capsule->cost_y_0_hess.casadi_fun = &{{ model.name }}_cost_y_0_hess;
-    capsule->cost_y_0_hess.casadi_n_in = &{{ model.name }}_cost_y_0_hess_n_in;
-    capsule->cost_y_0_hess.casadi_n_out = &{{ model.name }}_cost_y_0_hess_n_out;
-    capsule->cost_y_0_hess.casadi_sparsity_in = &{{ model.name }}_cost_y_0_hess_sparsity_in;
-    capsule->cost_y_0_hess.casadi_sparsity_out = &{{ model.name }}_cost_y_0_hess_sparsity_out;
-    capsule->cost_y_0_hess.casadi_work = &{{ model.name }}_cost_y_0_hess_work;
-    external_function_param_casadi_create(&capsule->cost_y_0_hess, {{ dims.np }});
+    // nonlinear least squares function
+    MAP_CASADI_FNC(cost_y_0_fun, {{ model.name }}_cost_y_0_fun);
+    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 == "EXTERNAL" %}
     // external cost
-    {% if cost.cost_ext_fun_type_0 == "casadi" %}
-    capsule->ext_cost_0_fun.casadi_fun = &{{ model.name }}_cost_ext_cost_0_fun;
-    capsule->ext_cost_0_fun.casadi_n_in = &{{ model.name }}_cost_ext_cost_0_fun_n_in;
-    capsule->ext_cost_0_fun.casadi_n_out = &{{ model.name }}_cost_ext_cost_0_fun_n_out;
-    capsule->ext_cost_0_fun.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_0_fun_sparsity_in;
-    capsule->ext_cost_0_fun.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_0_fun_sparsity_out;
-    capsule->ext_cost_0_fun.casadi_work = &{{ model.name }}_cost_ext_cost_0_fun_work;
-    {% else %}
+    {%- if cost.cost_ext_fun_type_0 == "casadi" %}
+    MAP_CASADI_FNC(ext_cost_0_fun, {{ model.name }}_cost_ext_cost_0_fun);
+    {%- else %}
     capsule->ext_cost_0_fun.fun = &{{ cost.cost_function_ext_cost_0 }};
-    {% endif %}
     external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun, {{ dims.np }});
+    {%- endif %}
 
     // external cost
-    {% if cost.cost_ext_fun_type_0 == "casadi" %}
-    capsule->ext_cost_0_fun_jac.casadi_fun = &{{ model.name }}_cost_ext_cost_0_fun_jac;
-    capsule->ext_cost_0_fun_jac.casadi_n_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_n_in;
-    capsule->ext_cost_0_fun_jac.casadi_n_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_n_out;
-    capsule->ext_cost_0_fun_jac.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in;
-    capsule->ext_cost_0_fun_jac.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out;
-    capsule->ext_cost_0_fun_jac.casadi_work = &{{ model.name }}_cost_ext_cost_0_fun_jac_work;
-    {% else %}
+    {%- if cost.cost_ext_fun_type_0 == "casadi" %}
+    MAP_CASADI_FNC(ext_cost_0_fun_jac, {{ model.name }}_cost_ext_cost_0_fun_jac);
+    {%- else %}
     capsule->ext_cost_0_fun_jac.fun = &{{ cost.cost_function_ext_cost_0 }};
-    {% endif %}
     external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac, {{ dims.np }});
+    {%- endif %}
 
     // external cost
-    {% if cost.cost_ext_fun_type_0 == "casadi" %}
-    capsule->ext_cost_0_fun_jac_hess.casadi_fun = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess;
-    capsule->ext_cost_0_fun_jac_hess.casadi_n_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in;
-    capsule->ext_cost_0_fun_jac_hess.casadi_n_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out;
-    capsule->ext_cost_0_fun_jac_hess.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in;
-    capsule->ext_cost_0_fun_jac_hess.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out;
-    capsule->ext_cost_0_fun_jac_hess.casadi_work = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_work;
-    {% else %}
+    {%- if cost.cost_ext_fun_type_0 == "casadi" %}
+    MAP_CASADI_FNC(ext_cost_0_fun_jac_hess, {{ model.name }}_cost_ext_cost_0_fun_jac_hess);
+    {%- else %}
     capsule->ext_cost_0_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_0 }};
-    {% endif %}
     external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac_hess, {{ dims.np }});
+    {%- endif %}
 {%- endif %}
 
 {%- if cost.cost_type == "NONLINEAR_LS" %}
@@ -764,164 +605,130 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     capsule->cost_y_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N-1; i++)
     {
-        capsule->cost_y_fun[i].casadi_fun = &{{ model.name }}_cost_y_fun;
-        capsule->cost_y_fun[i].casadi_n_in = &{{ model.name }}_cost_y_fun_n_in;
-        capsule->cost_y_fun[i].casadi_n_out = &{{ model.name }}_cost_y_fun_n_out;
-        capsule->cost_y_fun[i].casadi_sparsity_in = &{{ model.name }}_cost_y_fun_sparsity_in;
-        capsule->cost_y_fun[i].casadi_sparsity_out = &{{ model.name }}_cost_y_fun_sparsity_out;
-        capsule->cost_y_fun[i].casadi_work = &{{ model.name }}_cost_y_fun_work;
-
-        external_function_param_casadi_create(&capsule->cost_y_fun[i], {{ dims.np }});
+        MAP_CASADI_FNC(cost_y_fun[i], {{ model.name }}_cost_y_fun);
     }
 
     capsule->cost_y_fun_jac_ut_xt = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N-1; i++)
     {
-        capsule->cost_y_fun_jac_ut_xt[i].casadi_fun = &{{ model.name }}_cost_y_fun_jac_ut_xt;
-        capsule->cost_y_fun_jac_ut_xt[i].casadi_n_in = &{{ model.name }}_cost_y_fun_jac_ut_xt_n_in;
-        capsule->cost_y_fun_jac_ut_xt[i].casadi_n_out = &{{ model.name }}_cost_y_fun_jac_ut_xt_n_out;
-        capsule->cost_y_fun_jac_ut_xt[i].casadi_sparsity_in = &{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in;
-        capsule->cost_y_fun_jac_ut_xt[i].casadi_sparsity_out = &{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out;
-        capsule->cost_y_fun_jac_ut_xt[i].casadi_work = &{{ model.name }}_cost_y_fun_jac_ut_xt_work;
-
-        external_function_param_casadi_create(&capsule->cost_y_fun_jac_ut_xt[i], {{ dims.np }});
+        MAP_CASADI_FNC(cost_y_fun_jac_ut_xt[i], {{ model.name }}_cost_y_fun_jac_ut_xt);
     }
 
     capsule->cost_y_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
     for (int i = 0; i < N-1; i++)
     {
-        capsule->cost_y_hess[i].casadi_fun = &{{ model.name }}_cost_y_hess;
-        capsule->cost_y_hess[i].casadi_n_in = &{{ model.name }}_cost_y_hess_n_in;
-        capsule->cost_y_hess[i].casadi_n_out = &{{ model.name }}_cost_y_hess_n_out;
-        capsule->cost_y_hess[i].casadi_sparsity_in = &{{ model.name }}_cost_y_hess_sparsity_in;
-        capsule->cost_y_hess[i].casadi_sparsity_out = &{{ model.name }}_cost_y_hess_sparsity_out;
-        capsule->cost_y_hess[i].casadi_work = &{{ model.name }}_cost_y_hess_work;
-
-        external_function_param_casadi_create(&capsule->cost_y_hess[i], {{ dims.np }});
+        MAP_CASADI_FNC(cost_y_hess[i], {{ model.name }}_cost_y_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);
     for (int i = 0; i < N-1; i++)
     {
-        {% if cost.cost_ext_fun_type == "casadi" %}
-        capsule->ext_cost_fun[i].casadi_fun = &{{ model.name }}_cost_ext_cost_fun;
-        capsule->ext_cost_fun[i].casadi_n_in = &{{ model.name }}_cost_ext_cost_fun_n_in;
-        capsule->ext_cost_fun[i].casadi_n_out = &{{ model.name }}_cost_ext_cost_fun_n_out;
-        capsule->ext_cost_fun[i].casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_fun_sparsity_in;
-        capsule->ext_cost_fun[i].casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_fun_sparsity_out;
-        capsule->ext_cost_fun[i].casadi_work = &{{ model.name }}_cost_ext_cost_fun_work;
-        {% else %}
+        {%- if cost.cost_ext_fun_type == "casadi" %}
+        MAP_CASADI_FNC(ext_cost_fun[i], {{ model.name }}_cost_ext_cost_fun);
+        {%- else %}
         capsule->ext_cost_fun[i].fun = &{{ cost.cost_function_ext_cost }};
-        {% endif %}
         external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun[i], {{ dims.np }});
+        {%- endif %}
     }
 
     capsule->ext_cost_fun_jac = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N);
     for (int i = 0; i < N-1; i++)
     {
-        {% if cost.cost_ext_fun_type == "casadi" %}
-        capsule->ext_cost_fun_jac[i].casadi_fun = &{{ model.name }}_cost_ext_cost_fun_jac;
-        capsule->ext_cost_fun_jac[i].casadi_n_in = &{{ model.name }}_cost_ext_cost_fun_jac_n_in;
-        capsule->ext_cost_fun_jac[i].casadi_n_out = &{{ model.name }}_cost_ext_cost_fun_jac_n_out;
-        capsule->ext_cost_fun_jac[i].casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in;
-        capsule->ext_cost_fun_jac[i].casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out;
-        capsule->ext_cost_fun_jac[i].casadi_work = &{{ model.name }}_cost_ext_cost_fun_jac_work;
-        {% else %}
+        {%- if cost.cost_ext_fun_type == "casadi" %}
+        MAP_CASADI_FNC(ext_cost_fun_jac[i], {{ model.name }}_cost_ext_cost_fun_jac);
+        {%- else %}
         capsule->ext_cost_fun_jac[i].fun = &{{ cost.cost_function_ext_cost }};
-        {% endif %}
         external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac[i], {{ dims.np }});
+        {%- endif %}
     }
 
     capsule->ext_cost_fun_jac_hess = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N);
     for (int i = 0; i < N-1; i++)
     {
-        {% if cost.cost_ext_fun_type == "casadi" %}
-        capsule->ext_cost_fun_jac_hess[i].casadi_fun = &{{ model.name }}_cost_ext_cost_fun_jac_hess;
-        capsule->ext_cost_fun_jac_hess[i].casadi_n_in = &{{ model.name }}_cost_ext_cost_fun_jac_hess_n_in;
-        capsule->ext_cost_fun_jac_hess[i].casadi_n_out = &{{ model.name }}_cost_ext_cost_fun_jac_hess_n_out;
-        capsule->ext_cost_fun_jac_hess[i].casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in;
-        capsule->ext_cost_fun_jac_hess[i].casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out;
-        capsule->ext_cost_fun_jac_hess[i].casadi_work = &{{ model.name }}_cost_ext_cost_fun_jac_hess_work;
-        {% else %}
+        {%- if cost.cost_ext_fun_type == "casadi" %}
+        MAP_CASADI_FNC(ext_cost_fun_jac_hess[i], {{ model.name }}_cost_ext_cost_fun_jac_hess);
+        {%- else %}
         capsule->ext_cost_fun_jac_hess[i].fun = &{{ cost.cost_function_ext_cost }};
-        {% endif %}
         external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac_hess[i], {{ dims.np }});
+        {%- endif %}
     }
 {%- endif %}
 
 {%- if cost.cost_type_e == "NONLINEAR_LS" %}
     // nonlinear least square function
-    capsule->cost_y_e_fun.casadi_fun = &{{ model.name }}_cost_y_e_fun;
-    capsule->cost_y_e_fun.casadi_n_in = &{{ model.name }}_cost_y_e_fun_n_in;
-    capsule->cost_y_e_fun.casadi_n_out = &{{ model.name }}_cost_y_e_fun_n_out;
-    capsule->cost_y_e_fun.casadi_sparsity_in = &{{ model.name }}_cost_y_e_fun_sparsity_in;
-    capsule->cost_y_e_fun.casadi_sparsity_out = &{{ model.name }}_cost_y_e_fun_sparsity_out;
-    capsule->cost_y_e_fun.casadi_work = &{{ model.name }}_cost_y_e_fun_work;
-    external_function_param_casadi_create(&capsule->cost_y_e_fun, {{ dims.np }});
-
-    capsule->cost_y_e_fun_jac_ut_xt.casadi_fun = &{{ model.name }}_cost_y_e_fun_jac_ut_xt;
-    capsule->cost_y_e_fun_jac_ut_xt.casadi_n_in = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in;
-    capsule->cost_y_e_fun_jac_ut_xt.casadi_n_out = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out;
-    capsule->cost_y_e_fun_jac_ut_xt.casadi_sparsity_in = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in;
-    capsule->cost_y_e_fun_jac_ut_xt.casadi_sparsity_out = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out;
-    capsule->cost_y_e_fun_jac_ut_xt.casadi_work = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_work;
-    external_function_param_casadi_create(&capsule->cost_y_e_fun_jac_ut_xt, {{ dims.np }});
-
-    capsule->cost_y_e_hess.casadi_fun = &{{ model.name }}_cost_y_e_hess;
-    capsule->cost_y_e_hess.casadi_n_in = &{{ model.name }}_cost_y_e_hess_n_in;
-    capsule->cost_y_e_hess.casadi_n_out = &{{ model.name }}_cost_y_e_hess_n_out;
-    capsule->cost_y_e_hess.casadi_sparsity_in = &{{ model.name }}_cost_y_e_hess_sparsity_in;
-    capsule->cost_y_e_hess.casadi_sparsity_out = &{{ model.name }}_cost_y_e_hess_sparsity_out;
-    capsule->cost_y_e_hess.casadi_work = &{{ model.name }}_cost_y_e_hess_work;
-    external_function_param_casadi_create(&capsule->cost_y_e_hess, {{ dims.np }});
-
+    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 == "EXTERNAL" %}
-    // external cost
-    {% if cost.cost_ext_fun_type_e == "casadi" %}
-    capsule->ext_cost_e_fun.casadi_fun = &{{ model.name }}_cost_ext_cost_e_fun;
-    capsule->ext_cost_e_fun.casadi_n_in = &{{ model.name }}_cost_ext_cost_e_fun_n_in;
-    capsule->ext_cost_e_fun.casadi_n_out = &{{ model.name }}_cost_ext_cost_e_fun_n_out;
-    capsule->ext_cost_e_fun.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_e_fun_sparsity_in;
-    capsule->ext_cost_e_fun.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_e_fun_sparsity_out;
-    capsule->ext_cost_e_fun.casadi_work = &{{ model.name }}_cost_ext_cost_e_fun_work;
+    // external cost - function
+    {%- if cost.cost_ext_fun_type_e == "casadi" %}
+    MAP_CASADI_FNC(ext_cost_e_fun, {{ model.name }}_cost_ext_cost_e_fun);
     {% else %}
     capsule->ext_cost_e_fun.fun = &{{ cost.cost_function_ext_cost_e }};
-    {% endif %}
     external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun, {{ dims.np }});
+    {%- endif %}
 
-    // external cost
-    {% if cost.cost_ext_fun_type_e == "casadi" %}
-    capsule->ext_cost_e_fun_jac.casadi_fun = &{{ model.name }}_cost_ext_cost_e_fun_jac;
-    capsule->ext_cost_e_fun_jac.casadi_n_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_n_in;
-    capsule->ext_cost_e_fun_jac.casadi_n_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_n_out;
-    capsule->ext_cost_e_fun_jac.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in;
-    capsule->ext_cost_e_fun_jac.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out;
-    capsule->ext_cost_e_fun_jac.casadi_work = &{{ model.name }}_cost_ext_cost_e_fun_jac_work;
-    {% else %}
+    // external cost - jacobian
+    {%- if cost.cost_ext_fun_type_e == "casadi" %}
+    MAP_CASADI_FNC(ext_cost_e_fun_jac, {{ model.name }}_cost_ext_cost_e_fun_jac);
+    {%- else %}
     capsule->ext_cost_e_fun_jac.fun = &{{ cost.cost_function_ext_cost_e }};
-    {% endif %}
     external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun_jac, {{ dims.np }});
+    {%- endif %}
 
-    // external cost
-    {% if cost.cost_ext_fun_type_e == "casadi" %}
-    capsule->ext_cost_e_fun_jac_hess.casadi_fun = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess;
-    capsule->ext_cost_e_fun_jac_hess.casadi_n_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in;
-    capsule->ext_cost_e_fun_jac_hess.casadi_n_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out;
-    capsule->ext_cost_e_fun_jac_hess.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in;
-    capsule->ext_cost_e_fun_jac_hess.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out;
-    capsule->ext_cost_e_fun_jac_hess.casadi_work = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_work;
-    {% else %}
+    // external cost - hessian
+    {%- if cost.cost_ext_fun_type_e == "casadi" %}
+    MAP_CASADI_FNC(ext_cost_e_fun_jac_hess, {{ model.name }}_cost_ext_cost_e_fun_jac_hess);
+    {%- else %}
     capsule->ext_cost_e_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_e }};
-    {% endif %}
     external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun_jac_hess, {{ dims.np }});
+    {%- endif %}
 {%- endif %}
 
+#undef MAP_CASADI_FNC
+}
+
+
+/**
+ * Internal function for {{ model.name }}_acados_create: step 4
+ */
+void {{ model.name }}_acados_create_4_set_default_parameters({{ model.name }}_solver_capsule* capsule) {
+{%- if dims.np > 0 %}
+    const int N = capsule->nlp_solver_plan->N;
+    // initialize parameters to nominal value
+    double* p = calloc(NP, sizeof(double));
+    {%- for item in parameter_values %}
+        {%- if item != 0 %}
+    p[{{ loop.index0 }}] = {{ item }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 0; i <= N; i++) {
+        {{ model.name }}_acados_update_params(capsule, i, p, NP);
+    }
+    free(p);
+{%- else %}
+    // no parameters defined
+{%- endif %}{# if dims.np #}
+}
+
+
+/**
+ * Internal function for {{ model.name }}_acados_create: step 5
+ */
+void {{ model.name }}_acados_create_5_set_nlp_in({{ model.name }}_solver_capsule* capsule, const int N, double* new_time_steps)
+{
+    assert(N == capsule->nlp_solver_plan->N);
+    ocp_nlp_config* nlp_config = capsule->nlp_config;
+    ocp_nlp_dims* nlp_dims = capsule->nlp_dims;
+
     /************************************************
     *  nlp_in
     ************************************************/
-    ocp_nlp_in * nlp_in = ocp_nlp_in_create(nlp_config, nlp_dims);
-    capsule->nlp_in = nlp_in;
+//    ocp_nlp_in * nlp_in = ocp_nlp_in_create(nlp_config, nlp_dims);
+//    capsule->nlp_in = nlp_in;
+    ocp_nlp_in * nlp_in = capsule->nlp_in;
 
     // set up time_steps
     {% set all_equal = true -%}
@@ -978,11 +785,15 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i,
                                    "impl_dae_fun_jac_x_xdot_u", &capsule->impl_dae_fun_jac_x_xdot_u[i]);
     {% elif solver_options.integrator_type == "GNSF" %}
+        {% if model.gnsf.purely_linear != 1 %}
         ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun", &capsule->gnsf_phi_fun[i]);
         ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun_jac_y", &capsule->gnsf_phi_fun_jac_y[i]);
         ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_jac_y_uhat", &capsule->gnsf_phi_jac_y_uhat[i]);
+            {% if model.gnsf.nontrivial_f_LO == 1 %}
         ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "f_lo_jac_x1_x1dot_u_z",
                                    &capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i]);
+            {%- endif %}
+        {%- endif %}
         ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "gnsf_get_matrices_fun",
                                    &capsule->gnsf_get_matrices_fun[i]);
     {% elif solver_options.integrator_type == "DISCRETE" %}
@@ -996,10 +807,9 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endif %}
     }
 
-
     /**** Cost ****/
 {%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %}
-{% if dims.ny_0 > 0 %}
+    {%- 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) %}
@@ -1021,14 +831,14 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0);
     free(yref_0);
-{% endif %}
-{% endif %}
+    {%- 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));
     // change only the non-zero elements:
-    {% for j in range(end=dims.ny) %}
+    {%- for j in range(end=dims.ny) %}
         {%- for k in range(end=dims.ny) %}
             {%- if cost.W[j][k] != 0 %}
     W[{{ j }}+(NY) * {{ k }}] = {{ cost.W[j][k] }};
@@ -1051,13 +861,13 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(W);
     free(yref);
-{% endif %}
-{% endif %}
+    {%- endif %}
+{%- endif %}
 
 {%- if 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) %}
+    {%- for j in range(end=dims.ny_0) %}
         {%- for k in range(end=dims.nx) %}
             {%- if cost.Vx_0[j][k] != 0 %}
     Vx_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vx_0[j][k] }};
@@ -1067,10 +877,10 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     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.ny_0 > 0 and 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) %}
+    {%- for j in range(end=dims.ny_0) %}
         {%- for k in range(end=dims.nu) %}
             {%- if cost.Vu_0[j][k] != 0 %}
     Vu_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vu_0[j][k] }};
@@ -1079,8 +889,9 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vu", Vu_0);
     free(Vu_0);
-{% endif %}
-{% if dims.ny_0 > 0 and dims.nz > 0 %}
+    {%- endif %}
+
+    {%- if dims.ny_0 > 0 and 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) %}
@@ -1092,14 +903,14 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vz", Vz_0);
     free(Vz_0);
-{%- endif %}
+    {%- endif %}
 {%- endif %}{# LINEAR LS #}
 
 
 {%- if 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) %}
+    {%- for j in range(end=dims.ny) %}
         {%- for k in range(end=dims.nx) %}
             {%- if cost.Vx[j][k] != 0 %}
     Vx[{{ j }}+(NY) * {{ k }}] = {{ cost.Vx[j][k] }};
@@ -1112,7 +923,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(Vx);
 
-{% if dims.ny > 0 and dims.nu > 0 %}
+    {% if dims.ny > 0 and dims.nu > 0 %}
     double* Vu = calloc(NY*NU, sizeof(double));
     // change only the non-zero elements:
     {% for j in range(end=dims.ny) %}
@@ -1128,9 +939,9 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vu", Vu);
     }
     free(Vu);
-{% endif %}
+    {%- endif %}
 
-{% if dims.ny > 0 and dims.nz > 0 %}
+    {%- if dims.ny > 0 and dims.nz > 0 %}
     double* Vz = calloc(NY*NZ, sizeof(double));
     // change only the non-zero elements:
     {% for j in range(end=dims.ny) %}
@@ -1146,7 +957,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vz", Vz);
     }
     free(Vz);
-{%- endif %}
+    {%- endif %}
 {%- endif %}{# LINEAR LS #}
 
 
@@ -1176,8 +987,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
 {%- endif %}
 
-
-{% if dims.ns > 0 %}
+{%- if dims.ns > 0 %}
     double* zlumem = calloc(4*NS, sizeof(double));
     double* Zl = zlumem+NS*0;
     double* Zu = zlumem+NS*1;
@@ -1216,14 +1026,14 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zu", zu);
     }
     free(zlumem);
-{% endif %}
+{%- endif %}
 
     // terminal cost
-{% if cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "NONLINEAR_LS" %}
-{% if dims.ny_e > 0 %}
+{%- 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) %}
+    {%- for j in range(end=dims.ny_e) %}
         {%- if cost.yref_e[j] != 0 %}
     yref_e[{{ j }}] = {{ cost.yref_e[j] }};
         {%- endif %}
@@ -1233,7 +1043,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 
     double* W_e = calloc(NYN*NYN, sizeof(double));
     // change only the non-zero elements:
-    {% for j in range(end=dims.ny_e) %}
+    {%- 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] }};
@@ -1243,7 +1053,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     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" %}
+        {%- 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) %}
@@ -1255,14 +1065,14 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
     ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e);
     free(Vx_e);
-    {%- endif %}
+        {%- endif %}
 
-    {%- if cost.cost_type_e == "NONLINEAR_LS" %}
+        {%- 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 #}
+        {%- 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);
@@ -1312,7 +1122,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     /**** Constraints ****/
 
     // bounds for initial stage
-{% if dims.nbx_0 > 0 %}
+{%- if dims.nbx_0 > 0 %}
     // x0
     int* idxbx0 = malloc(NBX0 * sizeof(int));
     {%- for i in range(end=dims.nbx_0) %}
@@ -1337,8 +1147,9 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
     free(idxbx0);
     free(lubx0);
-{% endif %}
-{% if dims.nbxe_0 > 0 %}
+{%- endif %}
+
+{%- if dims.nbxe_0 > 0 %}
     // idxbxe_0
     int* idxbxe_0 = malloc({{ dims.nbxe_0 }} * sizeof(int));
     {% for i in range(end=dims.nbxe_0) %}
@@ -1346,7 +1157,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
     ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0);
     free(idxbxe_0);
-{% endif %}
+{%- endif %}
 
     /* constraints that are the same for initial and intermediate */
 {%- if dims.nsbx > 0 %}
@@ -1357,14 +1168,14 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 
     // soft bounds on x
     int* idxsbx = malloc(NSBX * sizeof(int));
-    {% for i in range(end=dims.nsbx) %}
+    {%- for i in range(end=dims.nsbx) %}
     idxsbx[{{ i }}] = {{ constraints.idxsbx[i] }};
     {%- endfor %}
 
     double* lusbx = calloc(2*NSBX, sizeof(double));
     double* lsbx = lusbx;
     double* usbx = lusbx + NSBX;
-    {% for i in range(end=dims.nsbx) %}
+    {%- for i in range(end=dims.nsbx) %}
         {%- if constraints.lsbx[i] != 0 %}
     lsbx[{{ i }}] = {{ constraints.lsbx[i] }};
         {%- endif %}
@@ -1384,7 +1195,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 {%- endif %}
 
 
-{% if dims.nbu > 0 %}
+{%- if dims.nbu > 0 %}
     // u
     int* idxbu = malloc(NBU * sizeof(int));
     {% for i in range(end=dims.nbu) %}
@@ -1410,9 +1221,9 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(idxbu);
     free(lubu);
-{% endif %}
+{%- endif %}
 
-{% if dims.nsbu > 0 %}
+{%- if dims.nsbu > 0 %}
     // set up soft bounds for u
     int* idxsbu = malloc(NSBU * sizeof(int));
     {% for i in range(end=dims.nsbu) %}
@@ -1437,7 +1248,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(idxsbu);
     free(lusbu);
-{% endif %}
+{%- endif %}
 
 {% if dims.nsg > 0 %}
     // set up soft bounds for general linear constraints
@@ -1465,7 +1276,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(idxsg);
     free(lusg);
-{% endif %}
+{%- endif %}
 
 {% if dims.nsh > 0 %}
     // set up soft bounds for nonlinear constraints
@@ -1493,7 +1304,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(idxsh);
     free(lush);
-{% endif %}
+{%- endif %}
 
 {% if dims.nsphi > 0 %}
     // set up soft bounds for convex-over-nonlinear constraints
@@ -1521,7 +1332,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(idxsphi);
     free(lusphi);
-{% endif %}
+{%- endif %}
 
 {% if dims.nbx > 0 %}
     // x
@@ -1549,7 +1360,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     free(idxbx);
     free(lubx);
-{% endif %}
+{%- endif %}
 
 {% if dims.ng > 0 %}
     // set up general constraints for stage 0 to N-1 
@@ -1597,7 +1408,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     free(D);
     free(C);
     free(lug);
-{% endif %}
+{%- endif %}
 
 {% if dims.nh > 0 %}
     // set up nonlinear constraints for stage 0 to N-1
@@ -1632,7 +1443,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uh", uh);
     }
     free(luh);
-{% endif %}
+{%- endif %}
 
 {% if dims.nphi > 0 and constraints.constr_type == "BGP" %}
     // set up convex-over-nonlinear constraints for stage 0 to N-1
@@ -1659,7 +1470,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
         ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uphi", uphi);
     }
     free(luphi);
-{% endif %}
+{%- endif %}
 
     /* terminal constraints */
 {% if dims.nbx_e > 0 %}
@@ -1866,42 +1677,62 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
                                   "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_e_constraint);
     free(luphi_e);
 {% endif %}
+}
 
 
+/**
+ * Internal function for {{ model.name }}_acados_create: step 6
+ */
+void {{ model.name }}_acados_create_6_set_opts({{ model.name }}_solver_capsule* 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;
+
     /************************************************
     *  opts
     ************************************************/
 
-    capsule->nlp_opts = ocp_nlp_solver_opts_create(nlp_config, nlp_dims);
-
 {% if solver_options.hessian_approx == "EXACT" %}
     bool nlp_solver_exact_hessian = true;
     // TODO: this if should not be needed! however, calling the setter with false leads to weird behavior. Investigate!
     if (nlp_solver_exact_hessian)
     {
-        ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess", &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, capsule->nlp_opts, "exact_hess_dyn", &exact_hess_dyn);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_dyn", &exact_hess_dyn);
 
     int exact_hess_cost = {{ solver_options.exact_hess_cost }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess_cost", &exact_hess_cost);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_cost", &exact_hess_cost);
 
     int exact_hess_constr = {{ solver_options.exact_hess_constr }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess_constr", &exact_hess_constr);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "exact_hess_constr", &exact_hess_constr);
 {%- endif -%}
 
 {%- if solver_options.globalization == "FIXED_STEP" %}
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization", "fixed_step");
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization", "fixed_step");
 {%- elif solver_options.globalization == "MERIT_BACKTRACKING" %}
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization", "merit_backtracking");
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization", "merit_backtracking");
 
     double alpha_min = {{ solver_options.alpha_min }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "alpha_min", &alpha_min);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "alpha_min", &alpha_min);
 
     double alpha_reduction = {{ solver_options.alpha_reduction }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "alpha_reduction", &alpha_reduction);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "alpha_reduction", &alpha_reduction);
+
+    int line_search_use_sufficient_descent = {{ solver_options.line_search_use_sufficient_descent }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "line_search_use_sufficient_descent", &line_search_use_sufficient_descent);
+
+    int globalization_use_SOC = {{ solver_options.globalization_use_SOC }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_use_SOC", &globalization_use_SOC);
+
+    double eps_sufficient_descent = {{ solver_options.eps_sufficient_descent }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "eps_sufficient_descent", &eps_sufficient_descent);
 {%- endif -%}
+    int full_step_dual = {{ solver_options.full_step_dual }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "full_step_dual", &full_step_dual);
 
 {%- if dims.nz > 0 %}
     // TODO: these options are lower level -> should be encapsulated! maybe through hessian approx option.
@@ -1909,9 +1740,9 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     bool sens_algebraic_val = true;
 
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_output_z", &output_z_val);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_output_z", &output_z_val);
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_sens_algebraic", &sens_algebraic_val);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_sens_algebraic", &sens_algebraic_val);
 {%- endif %}
 
 {%- if solver_options.integrator_type != "DISCRETE" %}
@@ -1919,7 +1750,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     // set collocation type (relevant for implicit integrators)
     sim_collocation_type collocation_type = {{ solver_options.collocation_type }};
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_collocation_type", &collocation_type);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_collocation_type", &collocation_type);
 
     // set up sim_method_num_steps
     {%- set all_equal = true %}
@@ -1935,7 +1766,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     // all sim_method_num_steps are identical
     int sim_method_num_steps = {{ solver_options.sim_method_num_steps[0] }};
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps);
     {%- else %}
     // sim_method_num_steps are different
     int* sim_method_num_steps = malloc(N*sizeof(int));
@@ -1944,7 +1775,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
 
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps[i]);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps[i]);
     free(sim_method_num_steps);
     {%- endif %}
 
@@ -1962,7 +1793,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     // all sim_method_num_stages are identical
     int sim_method_num_stages = {{ solver_options.sim_method_num_stages[0] }};
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages);
   {%- else %}
     int* sim_method_num_stages = malloc(N*sizeof(int));
     {%- for j in range(end=dims.N) %}
@@ -1970,13 +1801,13 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
 
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages[i]);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages[i]);
     free(sim_method_num_stages);
   {%- endif %}
 
     int newton_iter_val = {{ solver_options.sim_method_newton_iter }};
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_newton_iter", &newton_iter_val);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_iter", &newton_iter_val);
 
 
     // set up sim_method_jac_reuse
@@ -1991,7 +1822,7 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
   {%- if all_equal == true %}
     bool tmp_bool = (bool) {{ solver_options.sim_method_jac_reuse[0] }};
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_jac_reuse", &tmp_bool);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool);
   {%- else %}
     bool* sim_method_jac_reuse = malloc(N*sizeof(bool));
     {%- for j in range(end=dims.N) %}
@@ -1999,104 +1830,114 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     {%- endfor %}
 
     for (int i = 0; i < N; i++)
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_jac_reuse", &sim_method_jac_reuse[i]);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &sim_method_jac_reuse[i]);
     free(sim_method_jac_reuse);
   {%- endif %}
 
 {%- endif %}
 
     double nlp_solver_step_length = {{ solver_options.nlp_solver_step_length }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "step_length", &nlp_solver_step_length);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "step_length", &nlp_solver_step_length);
 
     {%- if solver_options.nlp_solver_warm_start_first_qp %}
     int nlp_solver_warm_start_first_qp = {{ solver_options.nlp_solver_warm_start_first_qp }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "warm_start_first_qp", &nlp_solver_warm_start_first_qp);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "warm_start_first_qp", &nlp_solver_warm_start_first_qp);
     {%- endif %}
 
     double levenberg_marquardt = {{ solver_options.levenberg_marquardt }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "levenberg_marquardt", &levenberg_marquardt);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt);
 
     /* options QP solver */
 {%- if solver_options.qp_solver is starting_with("PARTIAL_CONDENSING") %}
     int qp_solver_cond_N;
 
-    {%- if solver_options.qp_solver_cond_N %}
-    qp_solver_cond_N = {{ solver_options.qp_solver_cond_N }};
-    {% else %}
+    {% if solver_options.qp_solver_cond_N -%}
+    const int qp_solver_cond_N_ori = {{ solver_options.qp_solver_cond_N }};
+    qp_solver_cond_N = N < qp_solver_cond_N_ori ? N : qp_solver_cond_N_ori; // use the minimum value here
+    {%- else %}
     // NOTE: there is no condensing happening here!
     qp_solver_cond_N = N;
     {%- endif %}
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_cond_N", &qp_solver_cond_N);
-{% endif %}
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_cond_N", &qp_solver_cond_N);
+{%- endif %}
+
+
+{% if solver_options.nlp_solver_type == "SQP" %}
+    // set SQP specific options
+    double nlp_solver_tol_stat = {{ solver_options.nlp_solver_tol_stat }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_stat", &nlp_solver_tol_stat);
+
+    double nlp_solver_tol_eq = {{ solver_options.nlp_solver_tol_eq }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_eq", &nlp_solver_tol_eq);
+
+    double nlp_solver_tol_ineq = {{ solver_options.nlp_solver_tol_ineq }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_ineq", &nlp_solver_tol_ineq);
+
+    double nlp_solver_tol_comp = {{ solver_options.nlp_solver_tol_comp }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_comp", &nlp_solver_tol_comp);
+
+    int nlp_solver_max_iter = {{ solver_options.nlp_solver_max_iter }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "max_iter", &nlp_solver_max_iter);
+
+    int initialize_t_slacks = {{ solver_options.initialize_t_slacks }};
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "initialize_t_slacks", &initialize_t_slacks);
+{%- endif %}
 
     int qp_solver_iter_max = {{ solver_options.qp_solver_iter_max }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_iter_max", &qp_solver_iter_max);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max);
 
+{# NOTE: qp_solver tolerances must be set after NLP ones, since the setter for NLP tolerances sets the QP tolerances to the sam values. #}
     {%- if solver_options.qp_solver_tol_stat %}
     double qp_solver_tol_stat = {{ solver_options.qp_solver_tol_stat }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_stat", &qp_solver_tol_stat);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_stat", &qp_solver_tol_stat);
     {%- endif -%}
 
     {%- if solver_options.qp_solver_tol_eq %}
     double qp_solver_tol_eq = {{ solver_options.qp_solver_tol_eq }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_eq", &qp_solver_tol_eq);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_eq", &qp_solver_tol_eq);
     {%- endif -%}
 
     {%- if solver_options.qp_solver_tol_ineq %}
     double qp_solver_tol_ineq = {{ solver_options.qp_solver_tol_ineq }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_ineq", &qp_solver_tol_ineq);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_ineq", &qp_solver_tol_ineq);
     {%- endif -%}
 
     {%- if solver_options.qp_solver_tol_comp %}
     double qp_solver_tol_comp = {{ solver_options.qp_solver_tol_comp }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_comp", &qp_solver_tol_comp);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_tol_comp", &qp_solver_tol_comp);
     {%- endif -%}
 
     {%- if solver_options.qp_solver_warm_start %}
     int qp_solver_warm_start = {{ solver_options.qp_solver_warm_start }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_warm_start", &qp_solver_warm_start);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_warm_start", &qp_solver_warm_start);
     {%- endif -%}
-    
-{% if solver_options.nlp_solver_type == "SQP" %}
-    // set SQP specific options
-    double nlp_solver_tol_stat = {{ solver_options.nlp_solver_tol_stat }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_stat", &nlp_solver_tol_stat);
-
-    double nlp_solver_tol_eq = {{ solver_options.nlp_solver_tol_eq }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_eq", &nlp_solver_tol_eq);
-
-    double nlp_solver_tol_ineq = {{ solver_options.nlp_solver_tol_ineq }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_ineq", &nlp_solver_tol_ineq);
-
-    double nlp_solver_tol_comp = {{ solver_options.nlp_solver_tol_comp }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_comp", &nlp_solver_tol_comp);
-
-    int nlp_solver_max_iter = {{ solver_options.nlp_solver_max_iter }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "max_iter", &nlp_solver_max_iter);
-
-    int initialize_t_slacks = {{ solver_options.initialize_t_slacks }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "initialize_t_slacks", &initialize_t_slacks);
-{%- endif %}
 
     int print_level = {{ solver_options.print_level }};
-    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "print_level", &print_level);
+    ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level);
 
 
     int ext_cost_num_hess = {{ solver_options.ext_cost_num_hess }};
 {%- if cost.cost_type == "EXTERNAL" %}
     for (int i = 0; i < N; i++)
     {
-        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "cost_numerical_hessian", &ext_cost_num_hess);
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "cost_numerical_hessian", &ext_cost_num_hess);
     }
 {%- endif %}
 {%- if cost.cost_type_e == "EXTERNAL" %}
-    ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, N, "cost_numerical_hessian", &ext_cost_num_hess);
+    ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, N, "cost_numerical_hessian", &ext_cost_num_hess);
 {%- endif %}
+}
 
 
-    /* out */
-    ocp_nlp_out * nlp_out = ocp_nlp_out_create(nlp_config, nlp_dims);
-    capsule->nlp_out = nlp_out;
+/**
+ * Internal function for {{ model.name }}_acados_create: step 7
+ */
+void {{ model.name }}_acados_create_7_set_nlp_out({{ model.name }}_solver_capsule* 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;
+    ocp_nlp_out* nlp_out = capsule->nlp_out;
 
     // initialize primal solution
     double* xu0 = calloc(NX+NU, sizeof(double));
@@ -2123,31 +1964,25 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
     }
     ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x0);
     free(xu0);
-    
-    capsule->nlp_solver = ocp_nlp_solver_create(nlp_config, nlp_dims, capsule->nlp_opts);
+}
 
 
-{% if dims.np > 0 %}
-    // initialize parameters to nominal value
-    double* p = calloc(NP, sizeof(double));
-    {% for item in parameter_values %}
-        {%- if item != 0 %}
-    p[{{ loop.index0 }}] = {{ item }};
-        {%- endif %}
-    {%- endfor %}
+/**
+ * Internal function for {{ model.name }}_acados_create: step 8
+ */
+//void {{ model.name }}_acados_create_8_create_solver({{ model.name }}_solver_capsule* capsule)
+//{
+//    capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts);
+//}
 
-    for (int i = 0; i <= N; i++)
-    {
-        {{ model.name }}_acados_update_params(capsule, i, p, NP);
-    }
-    free(p);
-{%- endif %}{# if dims.np #}
+/**
+ * Internal function for {{ model.name }}_acados_create: step 9
+ */
+int {{ model.name }}_acados_create_9_precompute({{ model.name }}_solver_capsule* capsule) {
+    int status = ocp_nlp_precompute(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
 
-    status = ocp_nlp_precompute(capsule->nlp_solver, nlp_in, nlp_out);
-
-    if (status != ACADOS_SUCCESS)
-    {
-        printf("\nocp_precompute failed!\n\n");
+    if (status != ACADOS_SUCCESS) {
+        printf("\nocp_nlp_precompute failed!\n\n");
         exit(1);
     }
 
@@ -2155,7 +1990,88 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
 }
 
 
-int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *p, int np)
+int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule* capsule, int N, double* new_time_steps)
+{
+    // If N does not match the number of shooting intervals used for code generation, new_time_steps must be given.
+    if (N != {{ model.name | upper }}_N && !new_time_steps) {
+        fprintf(stderr, "{{ model.name }}_acados_create_with_discretization: new_time_steps is NULL " \
+            "but the number of shooting intervals (= %d) differs from the number of " \
+            "shooting intervals (= %d) during code generation! Please provide a new vector of time_stamps!\n", \
+             N, {{ model.name | upper }}_N);
+        return 1;
+    }
+
+    // number of expected runtime parameters
+    capsule->nlp_np = NP;
+
+    // 1) create and set nlp_solver_plan; create nlp_config
+    capsule->nlp_solver_plan = ocp_nlp_plan_create(N);
+    {{ model.name }}_acados_create_1_set_plan(capsule->nlp_solver_plan, N);
+    capsule->nlp_config = ocp_nlp_config_create(*capsule->nlp_solver_plan);
+
+    // 3) create and set dimensions
+    capsule->nlp_dims = {{ model.name }}_acados_create_2_create_and_set_dimensions(capsule);
+    {{ model.name }}_acados_create_3_create_and_set_functions(capsule);
+
+    // 4) set default parameters in functions
+    {{ model.name }}_acados_create_4_set_default_parameters(capsule);
+
+    // 5) create and set nlp_in
+    capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims);
+    {{ model.name }}_acados_create_5_set_nlp_in(capsule, N, new_time_steps);
+
+    // 6) create and set nlp_opts
+    capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims);
+    {{ model.name }}_acados_create_6_set_opts(capsule);
+
+    // 7) create and set nlp_out
+    // 7.1) nlp_out
+    capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims);
+    // 7.2) sens_out
+    capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims);
+    {{ model.name }}_acados_create_7_set_nlp_out(capsule);
+
+    // 8) create solver
+    capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts);
+    //{{ model.name }}_acados_create_8_create_solver(capsule);
+
+    // 9) do precomputations
+    int status = {{ model.name }}_acados_create_9_precompute(capsule);
+    return status;
+}
+
+/**
+ * This function is for updating an already initialized solver with a different number of qp_cond_N. It is useful for code reuse after code export.
+ */
+int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule* capsule, int qp_solver_cond_N)
+{
+{%- if solver_options.qp_solver is starting_with("PARTIAL_CONDENSING") %}
+    // 1) destroy solver
+    ocp_nlp_solver_destroy(capsule->nlp_solver);
+
+    // 2) set new value for "qp_cond_N"
+    const int N = capsule->nlp_solver_plan->N;
+    if(qp_solver_cond_N > N)
+        printf("Warning: qp_solver_cond_N = %d > N = %d\n", qp_solver_cond_N, N);
+    ocp_nlp_solver_opts_set(capsule->nlp_config, capsule->nlp_opts, "qp_cond_N", &qp_solver_cond_N);
+
+    // 3) continue with the remaining steps from {{ model.name }}_acados_create_with_discretization(...):
+    // -> 8) create solver
+    capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts);
+
+    // -> 9) do precomputations
+    int status = {{ model.name }}_acados_create_9_precompute(capsule);
+    return status;
+{%- else %}
+    printf("\nacados_update_qp_solver_cond_N() failed, since no partial condensing solver is used!\n\n");
+    // Todo: what is an adequate behavior here?
+    exit(1);
+    return -1;
+{%- endif %}
+}
+
+
+int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule* capsule, int stage, double *p, int np)
 {
     int solver_status = 0;
 
@@ -2189,11 +2105,14 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * caps
         capsule->hess_vde_casadi[stage].set_param(capsule->hess_vde_casadi+stage, p);
         {%- endif %}
     {% elif solver_options.integrator_type == "GNSF" %}
+    {% if model.gnsf.purely_linear != 1 %}
         capsule->gnsf_phi_fun[stage].set_param(capsule->gnsf_phi_fun+stage, p);
         capsule->gnsf_phi_fun_jac_y[stage].set_param(capsule->gnsf_phi_fun_jac_y+stage, p);
         capsule->gnsf_phi_jac_y_uhat[stage].set_param(capsule->gnsf_phi_jac_y_uhat+stage, p);
-
-        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[stage].set_param(capsule->gnsf_f_lo_jac_x1_x1dot_u_z+stage, p);
+        {% if model.gnsf.nontrivial_f_LO == 1 %}
+            capsule->gnsf_f_lo_jac_x1_x1dot_u_z[stage].set_param(capsule->gnsf_f_lo_jac_x1_x1dot_u_z+stage, p);
+        {%- endif %}
+    {%- endif %}
     {% elif solver_options.integrator_type == "DISCRETE" %}
         capsule->discr_dyn_phi_fun[stage].set_param(capsule->discr_dyn_phi_fun+stage, p);
         capsule->discr_dyn_phi_fun_jac_ut_xt[stage].set_param(capsule->discr_dyn_phi_fun_jac_ut_xt+stage, p);
@@ -2271,7 +2190,7 @@ int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * caps
 
 
 
-int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule)
+int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule* capsule)
 {
     // solve NLP 
     int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
@@ -2280,7 +2199,7 @@ int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule)
 }
 
 
-int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule)
+int {{ model.name }}_acados_free({{ model.name }}_solver_capsule* capsule)
 {
     // before destroying, keep some info
     const int N = capsule->nlp_solver_plan->N;
@@ -2288,6 +2207,7 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule)
     ocp_nlp_solver_opts_destroy(capsule->nlp_opts);
     ocp_nlp_in_destroy(capsule->nlp_in);
     ocp_nlp_out_destroy(capsule->nlp_out);
+    ocp_nlp_out_destroy(capsule->sens_out);
     ocp_nlp_solver_destroy(capsule->nlp_solver);
     ocp_nlp_dims_destroy(capsule->nlp_dims);
     ocp_nlp_config_destroy(capsule->nlp_config);
@@ -2339,16 +2259,24 @@ int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule)
 {%- elif solver_options.integrator_type == "GNSF" %}
     for (int i = 0; i < N; i++)
     {
+        {% if model.gnsf.purely_linear != 1 %}
         external_function_param_casadi_free(&capsule->gnsf_phi_fun[i]);
         external_function_param_casadi_free(&capsule->gnsf_phi_fun_jac_y[i]);
         external_function_param_casadi_free(&capsule->gnsf_phi_jac_y_uhat[i]);
+        {% if model.gnsf.nontrivial_f_LO == 1 %}
         external_function_param_casadi_free(&capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i]);
+        {%- endif %}
+        {%- endif %}
         external_function_param_casadi_free(&capsule->gnsf_get_matrices_fun[i]);
     }
+  {% if model.gnsf.purely_linear != 1 %}
     free(capsule->gnsf_phi_fun);
     free(capsule->gnsf_phi_fun_jac_y);
     free(capsule->gnsf_phi_jac_y_uhat);
+  {% if model.gnsf.nontrivial_f_LO == 1 %}
     free(capsule->gnsf_f_lo_jac_x1_x1dot_u_z);
+  {%- endif %}
+  {%- endif %}
     free(capsule->gnsf_get_matrices_fun);
 {%- elif solver_options.integrator_type == "DISCRETE" %}
     for (int i = 0; i < N; i++)
@@ -2448,34 +2376,36 @@ 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_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 *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_solver_plan; }
+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)
+void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule* capsule)
 {
     int sqp_iter, stat_m, stat_n, tmp_int;
     ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "sqp_iter", &sqp_iter);
     ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_n", &stat_n);
     ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_m", &stat_m);
 
-    {% set stat_n_max = 10 %}
+    {% set stat_n_max = 12 %}
     double stat[{{ solver_options.nlp_solver_max_iter * stat_n_max }}];
     ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "statistics", stat);
 
     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\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++)
         {
-            if (j == 0 || j > 4)
+            if (j == 0 || j == 5 || j == 6)
             {
                 tmp_int = (int) stat[i + j * nrow];
                 printf("%d\t", tmp_int);
@@ -2487,5 +2417,17 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsu
         }
         printf("\n");
     }
+{% else %}
+    printf("iter\tqp_stat\tqp_iter\n");
+    for (int i = 0; i < nrow; i++)
+    {
+        for (int j = 0; j < stat_n + 1; j++)
+        {
+            tmp_int = (int) stat[i + j * nrow];
+            printf("%d\t", tmp_int);
+        }
+        printf("\n");
+    }
+{%- endif %}
 }
 
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_solver.in.h
index e8c0a38ca..2efc9e515 100644
--- a/pyextra/acados_template/c_templates_tera/acados_solver.in.h
+++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.h
@@ -78,9 +78,10 @@ typedef struct {{ model.name }}_solver_capsule
     // acados objects
     ocp_nlp_in *nlp_in;
     ocp_nlp_out *nlp_out;
+    ocp_nlp_out *sens_out;
     ocp_nlp_solver *nlp_solver;
     void *nlp_opts;
-    ocp_nlp_plan *nlp_solver_plan;
+    ocp_nlp_plan_t *nlp_solver_plan;
     ocp_nlp_config *nlp_config;
     ocp_nlp_dims *nlp_dims;
 
@@ -186,6 +187,10 @@ int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_c
  * nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0.
  */
 int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps);
+/**
+ * This function is used for updating an already initialized solver with a different number of qp_cond_N.
+ */
+int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule * capsule, int qp_solver_cond_N);
 int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *value, int np);
 int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule);
 int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule);
@@ -193,11 +198,12 @@ void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsu
 
 ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule);
 ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule);
+ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule * capsule);
 ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule);
 ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule);
 void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule);
 ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule);
-ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
+ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
 
 #ifdef __cplusplus
 } /* extern "C" */
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd b/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd
index 8387980c2..2cc5ee391 100644
--- a/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd
+++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.pxd
@@ -1,3 +1,36 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
 cimport acados_solver_common
 
 cdef extern from "acados_solver_{{ model.name }}.h":
@@ -8,6 +41,11 @@ cdef extern from "acados_solver_{{ model.name }}.h":
     int acados_free_capsule "{{ model.name }}_acados_free_capsule"(nlp_solver_capsule *capsule)
 
     int acados_create "{{ model.name }}_acados_create"(nlp_solver_capsule * capsule)
+
+    int acados_create_with_discretization "{{ model.name }}_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps)
+    int acados_update_time_steps "{{ model.name }}_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps)
+    int acados_update_qp_solver_cond_N "{{ model.name }}_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N)
+
     int acados_update_params "{{ model.name }}_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
     int acados_solve "{{ model.name }}_acados_solve"(nlp_solver_capsule * capsule)
     int acados_free "{{ model.name }}_acados_free"(nlp_solver_capsule * capsule)
@@ -15,6 +53,7 @@ cdef extern from "acados_solver_{{ model.name }}.h":
 
     acados_solver_common.ocp_nlp_in *acados_get_nlp_in "{{ model.name }}_acados_get_nlp_in"(nlp_solver_capsule * capsule)
     acados_solver_common.ocp_nlp_out *acados_get_nlp_out "{{ model.name }}_acados_get_nlp_out"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_out *acados_get_sens_out "{{ model.name }}_acados_get_sens_out"(nlp_solver_capsule * capsule)
     acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "{{ model.name }}_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
     acados_solver_common.ocp_nlp_config *acados_get_nlp_config "{{ model.name }}_acados_get_nlp_config"(nlp_solver_capsule * capsule)
     void *acados_get_nlp_opts "{{ model.name }}_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c
index a6cd1faa8..a27ba8837 100644
--- a/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c
+++ b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c
@@ -37,7 +37,7 @@
 #define MDL_START
 
 // acados
-#include "acados/utils/print.h"
+// #include "acados/utils/print.h"
 #include "acados_c/sim_interface.h"
 #include "acados_c/external_function_interface.h"
 
diff --git a/pyextra/acados_template/c_templates_tera/main.in.c b/pyextra/acados_template/c_templates_tera/main.in.c
index 3348eea5c..99c4f13be 100644
--- a/pyextra/acados_template/c_templates_tera/main.in.c
+++ b/pyextra/acados_template/c_templates_tera/main.in.c
@@ -156,11 +156,12 @@ int main()
     for (int ii = 0; ii < NTIMINGS; ii++)
     {
         // initialize solution
-        for (int i = 0; i <= nlp_dims->N; i++)
+        for (int i = 0; i < N; i++)
         {
             ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
             ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
         }
+        ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x_init);
         ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
         status = {{ model.name }}_acados_solve(acados_ocp_capsule);
         ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
diff --git a/pyextra/acados_template/c_templates_tera/make_sfun.in.m b/pyextra/acados_template/c_templates_tera/make_sfun.in.m
index 172da654e..30339bf5d 100644
--- a/pyextra/acados_template/c_templates_tera/make_sfun.in.m
+++ b/pyextra/acados_template/c_templates_tera/make_sfun.in.m
@@ -46,10 +46,14 @@ SOURCES = { ...
             '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c',...
             {%- endif %}
         {%- elif solver_options.integrator_type == "GNSF" %}
+            {% if model.gnsf.purely_linear != 1 %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c',...
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c',...
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c',...
+            {% if model.gnsf.nontrivial_f_LO == 1 %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c',...
+            {%- endif %}
+            {%- endif %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c',...
         {%- elif solver_options.integrator_type == "DISCRETE" %}
             '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c',...
diff --git a/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m b/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m
index 1c5cf0b12..a0c503e41 100644
--- a/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m
+++ b/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m
@@ -47,10 +47,14 @@ SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ...
             '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c ',...
                 {%- endif %}
             {%- elif solver_options.integrator_type == "GNSF" %}
+                {% if model.gnsf.purely_linear != 1 %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c '
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c '
             '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c '
+                {% if model.gnsf.nontrivial_f_LO == 1 %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c '
+                {%- endif %}
+                {%- endif %}
             '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c '
             {%- endif %}
           ];
diff --git a/pyextra/acados_template/c_templates_tera/mex_solver.in.m b/pyextra/acados_template/c_templates_tera/mex_solver.in.m
index 728741a46..278e0a605 100644
--- a/pyextra/acados_template/c_templates_tera/mex_solver.in.m
+++ b/pyextra/acados_template/c_templates_tera/mex_solver.in.m
@@ -125,15 +125,15 @@ classdef {{ model.name }}_mex_solver < handle
             if strcmp(field, 'stat')
                 stat = obj.get('stat');
                 {%- if solver_options.nlp_solver_type == "SQP" %}
-                fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter');
-                if size(stat,2)>7
+                fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha');
+                if size(stat,2)>8
                     fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp');
                 end
                 fprintf('\n');
                 for jj=1:size(stat,1)
-                    fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7));
-                    if size(stat,2)>7
-                        fprintf('\t%e\t%e\t%e\t%e', stat(jj,8), stat(jj,9), stat(jj,10), stat(jj,11));
+                    fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d\t%e', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7), stat(jj, 8));
+                    if size(stat,2)>8
+                        fprintf('\t%e\t%e\t%e\t%e', stat(jj,9), stat(jj,10), stat(jj,11), stat(jj,12));
                     end
                     fprintf('\n');
                 end
diff --git a/pyextra/acados_template/c_templates_tera/model.in.h b/pyextra/acados_template/c_templates_tera/model.in.h
index 661811232..918e2bc29 100644
--- a/pyextra/acados_template/c_templates_tera/model.in.h
+++ b/pyextra/acados_template/c_templates_tera/model.in.h
@@ -90,14 +90,7 @@ int {{ model.name }}_impl_dae_hess_n_out(void);
 
 {% elif solver_options.integrator_type == "GNSF" %}
 /* GNSF Functions */
-// used to import model matrices
-int        {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem);
-int        {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *);
-const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int);
-const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int);
-int        {{ model.name }}_gnsf_get_matrices_fun_n_in(void);
-int        {{ model.name }}_gnsf_get_matrices_fun_n_out(void);
-
+	{% if model.gnsf.purely_linear != 1 %}
 // phi_fun
 int        {{ model.name }}_gnsf_phi_fun(const double** arg, double** res, int* iw, double* w, void *mem);
 int        {{ model.name }}_gnsf_phi_fun_work(int *, int *, int *, int *);
@@ -121,7 +114,7 @@ const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in(int);
 const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out(int);
 int        {{ model.name }}_gnsf_phi_jac_y_uhat_n_in(void);
 int        {{ model.name }}_gnsf_phi_jac_y_uhat_n_out(void);
-
+	{% if model.gnsf.nontrivial_f_LO == 1 %}
 // f_lo_fun_jac_x1k1uz
 int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz(const double** arg, double** res, int* iw, double* w, void *mem);
 int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work(int *, int *, int *, int *);
@@ -129,6 +122,15 @@ const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in(int);
 const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out(int);
 int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in(void);
 int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out(void);
+	{%- endif %}
+	{%- endif %}
+// used to import model matrices
+int        {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem);
+int        {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int);
+const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int);
+int        {{ model.name }}_gnsf_get_matrices_fun_n_in(void);
+int        {{ model.name }}_gnsf_get_matrices_fun_n_out(void);
 
 {% elif solver_options.integrator_type == "ERK" %}
 /* explicit ODE */
diff --git a/pyextra/acados_template/utils.py b/pyextra/acados_template/utils.py
index bf8ae4d5d..a60e99fc7 100644
--- a/pyextra/acados_template/utils.py
+++ b/pyextra/acados_template/utils.py
@@ -254,22 +254,6 @@ def format_class_dict(d):
     return out
 
 
-def acados_class2dict(class_instance):
-    """
-    removes the __ artifact from class to dict conversion
-    """
-
-    d = dict(class_instance.__dict__)
-    out = {}
-    for k, v in d.items():
-        if isinstance(v, dict):
-            v = format_class_dict(v)
-
-        out_key = k.split('__', 1)[-1]
-        out[k.replace(k, out_key)] = v
-    return out
-
-
 def get_ocp_nlp_layout():
     python_interface_path = get_python_interface_path()
     abs_path = os.path.join(python_interface_path, 'acados_layout.json')
@@ -433,6 +417,13 @@ def set_up_imported_gnsf_model(acados_formulation):
     acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat
     acados_formulation.model.get_matrices_fun = get_matrices_fun
 
+    # get_matrices_fun = Function([model_name,'_gnsf_get_matrices_fun'], {dummy},...
+    #  {A, B, C, E, L_x, L_xdot, L_z, L_u, A_LO, c, E_LO, B_LO,...
+    #   nontrivial_f_LO, purely_linear, ipiv_x, ipiv_z, c_LO});
+    get_matrices_out = get_matrices_fun(0)
+    acados_formulation.model.gnsf['nontrivial_f_LO'] = int(get_matrices_out[12])
+    acados_formulation.model.gnsf['purely_linear'] = int(get_matrices_out[13])
+
     if "f_lo_fun_jac_x1k1uz" in gnsf:
         f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz'])
         acados_formulation.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/SConscript b/selfdrive/controls/lib/lateral_mpc_lib/SConscript
index f402e6e15..64eada95c 100644
--- a/selfdrive/controls/lib/lateral_mpc_lib/SConscript
+++ b/selfdrive/controls/lib/lateral_mpc_lib/SConscript
@@ -43,11 +43,19 @@ generated_files = [
   f'{gen}/lat_cost/lat_cost_y_0_fun.h',
 ] + build_files
 
+acados_dir = '#third_party/acados'
+source_list = ['lat_mpc.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_dir}/aarch64/lib/libacados.so',
+  f'{acados_dir}/x86_64/lib/libacados.so',
+  f'{acados_dir}/larch64/lib/libacados.so',
+]
+
 lenv = env.Clone()
 lenv.Clean(generated_files, Dir(gen))
 
 lenv.Command(generated_files,
-             ["lat_mpc.py"],
+             source_list,
              f"cd {Dir('.').abspath} && python3 lat_mpc.py")
 
 lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
index 1b4d5283a..a1037f040 100755
--- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
+++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
@@ -11,18 +11,19 @@ from selfdrive.modeld.constants import T_IDXS
 if __name__ == '__main__':  # generating code
   from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
-  # from pyextra.acados_template import AcadosOcpSolverFast
-  from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast  # pylint: disable=no-name-in-module, import-error
+  from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython  # pylint: disable=no-name-in-module, import-error
 
 LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
 EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
-JSON_FILE = "acados_ocp_lat.json"
+JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json")
 X_DIM = 4
 P_DIM = 2
+MODEL_NAME = 'lat'
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
 
 def gen_lat_model():
   model = AcadosModel()
-  model.name = 'lat'
+  model.name = MODEL_NAME
 
   # set up states & controls
   x_ego = SX.sym('x_ego')
@@ -58,7 +59,7 @@ def gen_lat_model():
   return model
 
 
-def gen_lat_mpc_solver():
+def gen_lat_ocp():
   ocp = AcadosOcp()
   ocp.model = gen_lat_model()
 
@@ -103,7 +104,7 @@ def gen_lat_mpc_solver():
   ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
   ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
   ocp.solver_options.integrator_type = 'ERK'
-  ocp.solver_options.nlp_solver_type = 'SQP_RTI'
+  ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
   ocp.solver_options.qp_solver_iter_max = 1
   ocp.solver_options.qp_solver_cond_N = 1
 
@@ -117,7 +118,7 @@ def gen_lat_mpc_solver():
 
 class LateralMpc():
   def __init__(self, x0=np.zeros(X_DIM)):
-    self.solver = AcadosOcpSolverFast('lat', N)
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset(x0)
 
   def reset(self, x0=np.zeros(X_DIM)):
@@ -173,5 +174,6 @@ class LateralMpc():
 
 
 if __name__ == "__main__":
-  ocp = gen_lat_mpc_solver()
-  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)
+  ocp = gen_lat_ocp()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
index b19401339..cd764114c 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
@@ -50,11 +50,19 @@ generated_files = [
   f'{gen}/long_cost/long_cost_y_0_fun.h',
 ] + build_files
 
+acados_dir = '#third_party/acados'
+source_list = ['long_mpc.py',
+  f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
+  f'{acados_dir}/aarch64/lib/libacados.so',
+  f'{acados_dir}/x86_64/lib/libacados.so',
+  f'{acados_dir}/larch64/lib/libacados.so',
+]
+
 lenv = env.Clone()
 lenv.Clean(generated_files, Dir(gen))
 
 lenv.Command(generated_files,
-             ["long_mpc.py"],
+             source_list,
              f"cd {Dir('.').abspath} && python3 long_mpc.py")
 
 lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index 094e4ce1b..94d7ae80e 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -11,14 +11,14 @@ from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
 if __name__ == '__main__':  # generating code
   from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
-  # from pyextra.acados_template import AcadosOcpSolver as AcadosOcpSolverFast
-  from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast  # pylint: disable=no-name-in-module, import-error
+  from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython  # pylint: disable=no-name-in-module, import-error
 
 from casadi import SX, vertcat
 
+MODEL_NAME = 'long'
 LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
 EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
-JSON_FILE = "acados_ocp_long.json"
+JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")
 
 SOURCES = ['lead0', 'lead1', 'cruise']
 
@@ -38,6 +38,7 @@ A_CHANGE_COST = 200.
 DANGER_ZONE_COST = 100.
 CRASH_DISTANCE = .5
 LIMIT_COST = 1e6
+ACADOS_SOLVER_TYPE = 'SQP_RTI'
 
 
 # Fewer timestamps don't hurt performance and lead to
@@ -65,7 +66,7 @@ def desired_follow_distance(v_ego, v_lead):
 
 def gen_long_model():
   model = AcadosModel()
-  model.name = 'long'
+  model.name = MODEL_NAME
 
   # set up states & controls
   x_ego = SX.sym('x_ego')
@@ -97,7 +98,7 @@ def gen_long_model():
   return model
 
 
-def gen_long_mpc_solver():
+def gen_long_ocp():
   ocp = AcadosOcp()
   ocp.model = gen_long_model()
 
@@ -172,7 +173,7 @@ def gen_long_mpc_solver():
   ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
   ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
   ocp.solver_options.integrator_type = 'ERK'
-  ocp.solver_options.nlp_solver_type = 'SQP_RTI'
+  ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
   ocp.solver_options.qp_solver_cond_N = 1
 
   # More iterations take too much time and less lead to inaccurate convergence in
@@ -195,7 +196,7 @@ class LongitudinalMpc:
     self.source = SOURCES[2]
 
   def reset(self):
-    self.solver = AcadosOcpSolverFast('long', N)
+    self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.v_solution = np.zeros(N+1)
     self.a_solution = np.zeros(N+1)
     self.prev_a = np.array(self.a_solution)
@@ -388,5 +389,6 @@ class LongitudinalMpc:
 
 
 if __name__ == "__main__":
-  ocp = gen_long_mpc_solver()
-  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)
+  ocp = gen_long_ocp()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE)
+  # AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True)
diff --git a/third_party/acados/aarch64/lib/libacados.so b/third_party/acados/aarch64/lib/libacados.so
index fe961d8c5..640de2c2a 100755
Binary files a/third_party/acados/aarch64/lib/libacados.so and b/third_party/acados/aarch64/lib/libacados.so differ
diff --git a/third_party/acados/aarch64/lib/libblasfeo.so b/third_party/acados/aarch64/lib/libblasfeo.so
index a59f3f79f..fae141e9f 100755
Binary files a/third_party/acados/aarch64/lib/libblasfeo.so and b/third_party/acados/aarch64/lib/libblasfeo.so differ
diff --git a/third_party/acados/build.sh b/third_party/acados/build.sh
index 888a067a4..96aa23419 100755
--- a/third_party/acados/build.sh
+++ b/third_party/acados/build.sh
@@ -13,19 +13,19 @@ elif [ -f /EON ]; then
 fi
 
 if [ ! -d acados_repo/ ]; then
-  #git clone https://github.com/acados/acados.git $DIR/acados
-  git clone https://github.com/commaai/acados.git $DIR/acados_repo
+  git clone https://github.com/acados/acados.git $DIR/acados_repo
+  # git clone https://github.com/commaai/acados.git $DIR/acados_repo
 fi
 cd acados_repo
 git fetch
-git checkout 92b85c61f7358a1b08b7cd30aeb9d32ad15942e8
+git checkout 105e06df87f06ea02df4af825867c946b31defdd
 git submodule update --recursive --init
 
 # build
 mkdir -p build
 cd build
 cmake -DACADOS_WITH_QPOASES=ON -UBLASFEO_TARGET -DBLASFEO_TARGET=$BLAS_TARGET ..
-make -j4 install
+make -j20 install
 
 INSTALL_DIR="$DIR/$ARCHNAME"
 rm -rf $INSTALL_DIR
@@ -33,8 +33,10 @@ mkdir -p $INSTALL_DIR
 
 rm $DIR/acados_repo/lib/*.json
 
+rm -rf $DIR/include
 cp -r $DIR/acados_repo/include $DIR
 cp -r $DIR/acados_repo/lib $INSTALL_DIR
+rm -rf $DIR/../../pyextra/acados_template
 cp -r $DIR/acados_repo/interfaces/acados_template/acados_template $DIR/../../pyextra
 #pip3 install -e $DIR/acados/interfaces/acados_template
 
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h
index 64939b9ed..4a7f19a07 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h
@@ -234,11 +234,7 @@ typedef struct ocp_nlp_out
 
     // NOTE: the inequalities are internally organized in the following order:
     // [ lbu lbx lg lh lphi ubu ubx ug uh uphi; lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
-
-    int sqp_iter;
-    int qp_iter;
     double inf_norm_res;
-    double total_time;
 
     void *raw_memory; // Pointer to allocated memory, to be used for freeing
 
@@ -274,11 +270,16 @@ typedef struct ocp_nlp_opts
     double levenberg_marquardt;  // LM factor to be added to the hessian before regularization
     int reuse_workspace;
     int num_threads;
+    int print_level;
 
     // TODO: move to separate struct?
     ocp_nlp_globalization_t globalization;
+    int full_step_dual;
+    int line_search_use_sufficient_descent;
+    int globalization_use_SOC;
     double alpha_min;
     double alpha_reduction;
+    double eps_sufficient_descent;
 } ocp_nlp_opts;
 
 //
@@ -316,6 +317,8 @@ typedef struct ocp_nlp_res
 acados_size_t ocp_nlp_res_calculate_size(ocp_nlp_dims *dims);
 //
 ocp_nlp_res *ocp_nlp_res_assign(ocp_nlp_dims *dims, void *raw_memory);
+//
+void ocp_nlp_res_get_inf_norm(ocp_nlp_res *res, double *out);
 
 /************************************************
  * memory
@@ -351,7 +354,7 @@ typedef struct ocp_nlp_memory
     bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables
     struct blasfeo_dvec *sim_guess;
 
-	int *sqp_iter; // pointer to iteration number
+    int *sqp_iter; // pointer to iteration number
 
 } ocp_nlp_memory;
 
@@ -375,8 +378,12 @@ typedef struct ocp_nlp_workspace
     void **cost;         // cost_workspace
     void **constraints;  // constraints_workspace
 
-	ocp_nlp_out *tmp_nlp_out;
-	ocp_nlp_out *weight_merit_fun;
+    // for globalization: -> move to module?!
+    ocp_nlp_out *tmp_nlp_out;
+    ocp_nlp_out *weight_merit_fun;
+    struct blasfeo_dvec tmp_nxu;
+    struct blasfeo_dvec tmp_ni;
+    struct blasfeo_dvec dxnext_dy;
 
 } ocp_nlp_workspace;
 
@@ -392,6 +399,8 @@ ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims
  * function
  ************************************************/
 
+void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
 //
 void ocp_nlp_initialize_qp(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);
@@ -409,7 +418,8 @@ void ocp_nlp_update_variables_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, oc
            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, double alpha);
 //
 double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
-            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work,
+            int check_early_termination);
 //
 double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
           ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h
index 45dac20a2..f2128a857 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h
@@ -84,6 +84,7 @@ typedef struct
     /* memory */
     acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
     void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
+    // get shooting node gap x_next(x_n, u_n) - x_{n+1}
     struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory_);
     struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory_);
     void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory_);
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h
index 8d2b6ecc1..c085e00d5 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h
@@ -65,11 +65,6 @@ extern "C" {
  * options
  ************************************************/
 
-typedef struct
-{
-    int dummy;
-} ocp_nlp_reg_noreg_opts;
-
 //
 acados_size_t ocp_nlp_reg_noreg_opts_calculate_size(void);
 //
@@ -84,12 +79,6 @@ void ocp_nlp_reg_noreg_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opt
 /************************************************
  * memory
  ************************************************/
-
-typedef struct
-{
-    int dummy;
-} ocp_nlp_reg_noreg_memory;
-
 //
 acados_size_t ocp_nlp_reg_noreg_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
 //
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h
index 81a4afd03..5e4fc810d 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h
@@ -68,7 +68,6 @@ typedef struct
     int qp_warm_start;   // qp_warm_start in all but the first sqp iterations
     bool warm_start_first_qp; // to set qp_warm_start in first iteration
     int rti_phase;       // only phase 0 at the moment 
-    int print_level;     // verbosity
     int initialize_t_slacks;  // 0-false or 1-true
 
 } ocp_nlp_sqp_opts;
@@ -107,6 +106,7 @@ typedef struct
     double time_sim;
     double time_sim_la;
     double time_sim_ad;
+    double time_solution_sensitivities;
 
     // statistics
     double *stat;
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h
index 6f16594d2..af22c06a1 100644
--- a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h
@@ -64,7 +64,6 @@ typedef struct
     int qp_warm_start;        // NOTE: this is not actually setting the warm_start! Just for compatibility with sqp.
     bool warm_start_first_qp; // to set qp_warm_start in first iteration
     int rti_phase;            // phase of RTI. Possible values 1 (preparation), 2 (feedback) 0 (both)
-    int print_level;     // verbosity
 
 } ocp_nlp_sqp_rti_opts;
 
@@ -100,6 +99,7 @@ typedef struct
     double time_reg;
     double time_tot;
     double time_glob;
+    double time_solution_sensitivities;
 
     // statistics
     double *stat;
diff --git a/third_party/acados/include/acados/utils/print.h b/third_party/acados/include/acados/utils/print.h
index 2993447db..f8568afb2 100644
--- a/third_party/acados/include/acados/utils/print.h
+++ b/third_party/acados/include/acados/utils/print.h
@@ -69,28 +69,32 @@ void ocp_nlp_out_print(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out);
 void ocp_nlp_res_print(ocp_nlp_dims *dims, ocp_nlp_res *nlp_res);
 
 // ocp qp
+// TODO: move printing routines below that print qp structures to HPIPM!
 void print_ocp_qp_dims(ocp_qp_dims *dims);
 
 // void print_dense_qp_dims(dense_qp_dims *dims);
 
 void print_ocp_qp_in(ocp_qp_in *qp_in);
 
+void print_ocp_qp_in_to_file(FILE *file, ocp_qp_in *qp_in);
+
 void print_ocp_qp_out(ocp_qp_out *qp_out);
 
+void print_ocp_qp_out_to_file(FILE *file, ocp_qp_out *qp_out);
+
+void print_ocp_qp_res(ocp_qp_res *qp_res);
+
+void print_dense_qp_in(dense_qp_in *qp_in);
 // void print_ocp_qp_in_to_string(char string_out[], ocp_qp_in *qp_in);
 
 // void print_ocp_qp_out_to_string(char string_out[], ocp_qp_out *qp_out);
 
-void print_ocp_qp_res(ocp_qp_res *qp_res);
-
 // void print_colmaj_ocp_qp_in(colmaj_ocp_qp_in *qp);
 
 // void print_colmaj_ocp_qp_in_to_file(colmaj_ocp_qp_in *qp);
 
 // void print_colmaj_ocp_qp_out(char *filename, colmaj_ocp_qp_in *qp, colmaj_ocp_qp_out *out);
 
-void print_dense_qp_in(dense_qp_in *qp_in);
-
 void print_qp_info(qp_info *info);
 
 // void acados_warning(char warning_string[]);
diff --git a/third_party/acados/include/acados_c/ocp_nlp_interface.h b/third_party/acados/include/acados_c/ocp_nlp_interface.h
index b6b893024..0ec82a437 100644
--- a/third_party/acados/include/acados_c/ocp_nlp_interface.h
+++ b/third_party/acados/include/acados_c/ocp_nlp_interface.h
@@ -107,13 +107,13 @@ typedef enum
 
 
 /// Structure to store the configuration of a non-linear program
-typedef struct ocp_nlp_plan
+typedef struct ocp_nlp_plan_t
 {
     /// QP solver configuration.
-    ocp_qp_solver_plan ocp_qp_solver_plan;
+    ocp_qp_solver_plan_t ocp_qp_solver_plan;
 
     /// Simulation solver configuration for each stage.
-    sim_solver_plan *sim_solver_plan;
+    sim_solver_plan_t *sim_solver_plan;
 
     /// Nlp solver type.
     ocp_nlp_solver_t nlp_solver;
@@ -133,7 +133,7 @@ typedef struct ocp_nlp_plan
     /// Horizon length.
     int N;
 
-} ocp_nlp_plan;
+} ocp_nlp_plan_t;
 
 
 /// Structure to store the state/configuration for the non-linear programming solver
@@ -151,7 +151,7 @@ typedef struct ocp_nlp_solver
 /// default/invalid state.
 ///
 /// \param N Horizon length
-ocp_nlp_plan *ocp_nlp_plan_create(int N);
+ocp_nlp_plan_t *ocp_nlp_plan_create(int N);
 
 /// Destructor for plan struct, frees memory.
 ///
@@ -162,7 +162,7 @@ void ocp_nlp_plan_destroy(void* plan_);
 /// Constructs an nlp configuration struct from a plan.
 ///
 /// \param plan The plan (user nlp configuration).
-ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan plan);
+ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan_t plan);
 
 /// Desctructor of the nlp configuration.
 ///
@@ -372,6 +372,14 @@ void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *
 //
 void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
 
+/// Computes the residuals.
+///
+/// \param solver The solver struct.
+/// \param nlp_in The inputs struct.
+/// \param nlp_out The output struct.
+void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
+
+
 //
 void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out);
 
diff --git a/third_party/acados/include/acados_c/ocp_qp_interface.h b/third_party/acados/include/acados_c/ocp_qp_interface.h
index a567ebeb3..2582f142d 100644
--- a/third_party/acados/include/acados_c/ocp_qp_interface.h
+++ b/third_party/acados/include/acados_c/ocp_qp_interface.h
@@ -105,7 +105,7 @@ typedef enum {
 typedef struct
 {
     ocp_qp_solver_t qp_solver;
-} ocp_qp_solver_plan;
+} ocp_qp_solver_plan_t;
 
 
 /// Linear ocp configuration.
@@ -127,7 +127,7 @@ void ocp_qp_xcond_solver_config_initialize_from_plan(
 /// Constructs a qp solver config and Initializes with default values.
 ///
 /// \param plan The qp solver plan struct.
-ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_create(ocp_qp_solver_plan plan);
+ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_create(ocp_qp_solver_plan_t plan);
 
 /// Destructor for config struct, frees memory.
 ///
diff --git a/third_party/acados/include/acados_c/sim_interface.h b/third_party/acados/include/acados_c/sim_interface.h
index cc47b62ed..92969e801 100644
--- a/third_party/acados/include/acados_c/sim_interface.h
+++ b/third_party/acados/include/acados_c/sim_interface.h
@@ -57,7 +57,7 @@ typedef enum
 typedef struct
 {
     sim_solver_t sim_solver;
-} sim_solver_plan;
+} sim_solver_plan_t;
 
 
 
@@ -74,7 +74,7 @@ typedef struct
 
 /* config */
 //
-sim_config *sim_config_create(sim_solver_plan plan);
+sim_config *sim_config_create(sim_solver_plan_t plan);
 //
 void sim_config_destroy(void *config);
 
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h
index 7a9415a59..d4d805c3e 100644
--- a/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h
@@ -185,9 +185,14 @@ void blasfeo_dveccp(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec
 void blasfeo_dvecsc(int m, double alpha, struct blasfeo_dvec *sx, int xi);
 // y <= alpha*x
 void blasfeo_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi);
+// z[idx] += alpha * x
 void blasfeo_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
+// z[idx] <= alpha * x
 void blasfeo_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
-void blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi);
+// z <= alpha * x[idx]
+void blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z += alpha * x[idx]
+void blasfeo_dvecexad_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
 
 void blasfeo_dveccl(int m,
 	struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi,
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h
index 448234044..1e007fa89 100644
--- a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h
@@ -182,6 +182,8 @@ void blasfeo_ref_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi,
 void blasfeo_ref_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
 void blasfeo_ref_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
 void blasfeo_ref_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi);
+// z += alpha * x[idx]
+void blasfeo_ref_dvecexad_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
 
 void blasfeo_ref_dveccl(int m,
 	struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi,
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h
index 539268849..f43f5e83c 100644
--- a/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h
@@ -127,6 +127,8 @@ float blasfeo_svecex1(struct blasfeo_svec *sx, int xi);
 void blasfeo_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
 void blasfeo_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
 void blasfeo_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi);
+// z += alpha * x[idx]
+void blasfeo_svecexad_sp(int m, double alpha, int *idx, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
 void blasfeo_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi);
 void blasfeo_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi);
 void blasfeo_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei);
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h
index f6e16a957..998e1d899 100644
--- a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h
@@ -128,6 +128,8 @@ float blasfeo_ref_svecex1(struct blasfeo_svec *sx, int xi);
 void blasfeo_ref_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
 void blasfeo_ref_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
 void blasfeo_ref_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi);
+// z += alpha * x[idx]
+void blasfeo_ref_svecexad_sp(int m, double alpha, int *idx, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
 void blasfeo_ref_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi);
 void blasfeo_ref_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi);
 void blasfeo_ref_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei);
diff --git a/third_party/acados/larch64/lib/libacados.so b/third_party/acados/larch64/lib/libacados.so
index 1f2b1f5c0..c9448d963 100644
Binary files a/third_party/acados/larch64/lib/libacados.so and b/third_party/acados/larch64/lib/libacados.so differ
diff --git a/third_party/acados/larch64/lib/libblasfeo.so b/third_party/acados/larch64/lib/libblasfeo.so
index 790282fa1..583769e98 100644
Binary files a/third_party/acados/larch64/lib/libblasfeo.so and b/third_party/acados/larch64/lib/libblasfeo.so differ
diff --git a/third_party/acados/x86_64/lib/libacados.so b/third_party/acados/x86_64/lib/libacados.so
index 00a5e2371..5c3203b55 100644
Binary files a/third_party/acados/x86_64/lib/libacados.so and b/third_party/acados/x86_64/lib/libacados.so differ
diff --git a/third_party/acados/x86_64/lib/libblasfeo.so b/third_party/acados/x86_64/lib/libblasfeo.so
index 5a8399287..262cf9f12 100644
Binary files a/third_party/acados/x86_64/lib/libblasfeo.so and b/third_party/acados/x86_64/lib/libblasfeo.so differ
diff --git a/third_party/acados/x86_64/lib/libhpipm.so b/third_party/acados/x86_64/lib/libhpipm.so
index f52091d93..48421183f 100644
Binary files a/third_party/acados/x86_64/lib/libhpipm.so and b/third_party/acados/x86_64/lib/libhpipm.so differ
diff --git a/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 b/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1
index 6b553554f..6aa7b4f8b 100644
Binary files a/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 and b/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 differ
diff --git a/third_party/acados/x86_64/t_renderer b/third_party/acados/x86_64/t_renderer
index 181ef9c3d..5e7871bf8 100755
Binary files a/third_party/acados/x86_64/t_renderer and b/third_party/acados/x86_64/t_renderer differ