cython wrapper for acados

pyextra/acados_template/.gitignore

@@ -1 +1,6 @@
 __pycache__/
+
+# Cython intermediates
+*_pyx.c
+*_pyx.o
+*_pyx.so

pyextra/acados_template/acados_ocp_solver.py

@@ -645,6 +645,10 @@ def ocp_render_templates(acados_ocp, json_file):
     out_file = f'acados_solver_{name}.h'
     render_template(in_file, out_file, template_dir, json_path)
 
+    in_file = 'acados_solver.in.pxd'
+    out_file = f'acados_solver.pxd'
+    render_template(in_file, out_file, template_dir, json_path)
+
     in_file = 'Makefile.in'
     out_file = 'Makefile'
     render_template(in_file, out_file, template_dir, json_path)

pyextra/acados_template/acados_ocp_solver_pyx.pyx

@@ -0,0 +1,545 @@
+# cython: language_level=3
+# cython: profile=False
+# distutils: language = c
+
+# -*- coding: future_fstrings -*-
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+cimport cython
+
+from libc cimport string
+from libc cimport bool
+
+cimport numpy as np
+from cpython cimport array
+
+cimport acados_solver_common
+cimport acados_solver
+
+import os
+import json
+import numpy as np
+import array
+from datetime import datetime
+
+# from .utils import np_array_to_list
+
+
+cdef class AcadosOcpSolverFast:
+    """
+    Class to interact with the acados ocp solver C object.
+
+        :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
+    """
+
+    cdef acados_solver.nlp_solver_capsule *capsule
+    cdef void *nlp_opts
+    cdef acados_solver_common.ocp_nlp_dims *nlp_dims
+    cdef acados_solver_common.ocp_nlp_config *nlp_config
+    cdef acados_solver_common.ocp_nlp_out *nlp_out
+    cdef acados_solver_common.ocp_nlp_in *nlp_in
+    cdef acados_solver_common.ocp_nlp_solver *nlp_solver
+
+    cdef str model_name
+    cdef int N
+    cdef bint solver_created
+
+    def __cinit__(self, str model_name, int N, str code_export_dir):
+        self.model_name = model_name
+        self.N = N
+
+        self.solver_created = False
+
+        # create capsule
+        self.capsule = acados_solver.acados_create_capsule()
+
+        # create solver
+        assert acados_solver.acados_create(self.capsule) == 0
+        self.solver_created = True
+
+        # get pointers solver
+        self.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.nlp_in = acados_solver.acados_get_nlp_in(self.capsule)
+        self.nlp_solver = acados_solver.acados_get_nlp_solver(self.capsule)
+
+    def solve(self):
+        """
+        Solve the ocp with current input.
+        """
+
+        status = acados_solver.acados_solve(self.capsule)
+        return status
+
+    def get_slice(self, int start_stage_, int end_stage_, str field_):
+        field = field_.encode('utf-8')
+        dims = acados_solver_common.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)
+        self.fill_in_slice(start_stage_, end_stage_, field_, out)
+        return out
+
+    def fill_in_slice(self, int start_stage_, int end_stage_, str field_, np.ndarray[np.float64_t, ndim=2, mode='c'] arr_):
+        out_fields = ['x', 'u', 'z', 'pi', 'lam', 't']
+        mem_fields = ['sl', 'su']
+
+        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 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))
+
+        cdef np.ndarray[np.float64_t, ndim=2, mode='c'] arr = np.ascontiguousarray(arr_, dtype=np.double)
+
+        if (field_ in out_fields):
+            acados_solver_common.ocp_nlp_out_get_slice(self.nlp_config, self.nlp_dims, self.nlp_out, start_stage_, end_stage_,
+                field, <double *> arr.data)
+        elif field_ in mem_fields:
+            raise NotImplementedError()
+        #     acados_solver_common.ocp_nlp_get_at_stage(self.nlp_config, self.nlp_dims, self.nlp_solver, start_stage_, end_stage_,
+        #         field, <double *> arr.data)
+
+    def get(self, stage_, field_):
+        return self.get_slice(stage_, stage_ + 1, field_)[0]
+
+
+    def print_statistics(self):
+        """
+        prints statistics of previous solver run as a table:
+            - iter: iteration number
+            - res_stat: stationarity residual
+            - res_eq: residual wrt equality constraints (dynamics)
+            - res_ineq: residual wrt inequality constraints (constraints)
+            - res_comp: residual wrt complementarity conditions
+            - qp_stat: status of QP solver
+            - qp_iter: number of QP iterations
+            - qp_res_stat: stationarity residual of the last QP solution
+            - qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution
+            - qp_res_ineq: residual wrt inequality constraints (constraints)  of the last QP solution
+            - qp_res_comp: residual wrt complementarity conditions of the last QP solution
+        """
+        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:
+                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('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \
+                        stat[7][jj], stat[8][jj], stat[9][jj], stat[10][jj]))
+            print('\n')
+        elif self.acados_ocp.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')
+            for jj in range(stat.shape[1]):
+                print('{:d}\t{:d}\t{:d}'.format( int(stat[0][jj]), int(stat[1][jj]), int(stat[2][jj])))
+                if stat.shape[0]>3:
+                    print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \
+                         stat[3][jj], stat[4][jj], stat[5][jj], stat[6][jj]))
+            print('\n')
+
+        return
+
+
+    def store_iterate(self, filename='', overwrite=False):
+        """
+        Stores the current iterate of the ocp solver in a json file.
+
+            :param filename: if not set, use model_name + timestamp + '.json'
+            :param overwrite: if false and filename exists add timestamp to filename
+        """
+        if filename == '':
+            filename += self.model_name + '_' + 'iterate' + '.json'
+
+        if not overwrite:
+            # append timestamp
+            if os.path.isfile(filename):
+                filename = filename[:-5]
+                filename += datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f') + '.json'
+
+        # get iterate:
+        solution = dict()
+
+        for i in range(self.N+1):
+            solution['x_'+str(i)] = self.get(i,'x')
+            solution['u_'+str(i)] = self.get(i,'u')
+            solution['z_'+str(i)] = self.get(i,'z')
+            solution['lam_'+str(i)] = self.get(i,'lam')
+            solution['t_'+str(i)] = self.get(i, 't')
+            solution['sl_'+str(i)] = self.get(i, 'sl')
+            solution['su_'+str(i)] = self.get(i, 'su')
+        for i in range(self.N):
+            solution['pi_'+str(i)] = self.get(i,'pi')
+
+        # save
+        raise NotImplementedError()
+        # with open(filename, 'w') as f:
+        #     json.dump(solution, f, default=np_array_to_list, indent=4, sort_keys=True)
+        # print("stored current iterate in ", os.path.join(os.getcwd(), filename))
+
+
+    def load_iterate(self, filename):
+        """
+        Loads the iterate stored in json file with filename into the ocp solver.
+        """
+        if not os.path.isfile(filename):
+            raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename))
+
+        with open(filename, 'r') as f:
+            solution = json.load(f)
+
+        for key in solution.keys():
+            (field, stage) = key.split('_')
+            self.set(int(stage), field, np.array(solution[key]))
+
+
+    def get_stats(self, field_):
+        """
+        Get the information of the last solver call.
+
+            :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
+        """
+
+        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)
+                  '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
+                  '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))
+
+        if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
+            out = np.ascontiguousarray(np.zeros((1,)), dtype=np.int64)
+            out_data = out.data
+
+        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 = out.data
+
+        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, :]
+
+        else:
+            out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
+            out_data = out.data
+
+        if not field_ == 'qp_iter':
+            acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out_data)
+
+        return out
+
+
+    def get_cost(self):
+        """
+        Returns the cost value of the current solution.
+        """
+        # compute cost internally
+        acados_solver_common.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out)
+
+        # create output array
+        out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
+
+        # call getter
+        field = "cost_value".encode('utf-8')
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out.data)
+
+        return out[0]
+
+
+    def get_residuals(self):
+        """
+        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':
+            acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
+
+        # create output array
+        out = np.ascontiguousarray(np.zeros((4, 1)), dtype=np.float64)
+
+        # call getters
+        field = "res_stat".encode('utf-8')
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out.data)
+
+        field = "res_eq".encode('utf-8')
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out[1].data)
+
+        field = "res_ineq".encode('utf-8')
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out[2].data)
+
+        field = "res_comp".encode('utf-8')
+        acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, <void *> out[3].data)
+        return out.flatten()
+
+
+    # Note: this function should not be used anymore, better use cost_set, constraints_set
+
+    def set(self, int stage, str field_, value_):
+
+        """
+        Set numerical data inside the solver.
+
+            :param stage: integer corresponding to shooting node
+            :param field: string in ['x', 'u', 'pi', 'lam', 't', 'p']
+
+            .. note:: regarding lam, t: \n
+                    the inequalities are internally organized in the following order: \n
+                    [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
+                      lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
+
+            .. note:: pi: multipliers for dynamics equality constraints \n
+                      lam: multipliers for inequalities \n
+                      t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
+                      sl: slack variables of soft lower inequality constraints \n
+                      su: slack variables of soft upper inequality constraints \n
+        """
+        cost_fields = ['y_ref', 'yref']
+        constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
+        out_fields = ['x', 'u', 'pi', 'lam', 't', 'z']
+        mem_fields = ['sl', 'su']
+
+        # cast value_ to avoid conversion issues
+        if isinstance(value_, (float, int)):
+            value_ = np.array([value_])
+        value_ = value_.astype(np.double)
+
+        field = field_
+        field = field.encode('utf-8')
+
+        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']))
+
+        dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field)
+
+        if value_.shape[0] != dims:
+            msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
+            msg += 'with dimension {} (you have {})'.format(dims, value_.shape)
+            raise Exception(msg)
+
+        cdef np.ndarray[np.float64_t, ndim=1, mode='c'] value = np.ascontiguousarray(value_, dtype=np.double)
+
+        value_data_p = <void *> value.data
+        if field_ in constraints_fields:
+            acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, self.nlp_dims, self.nlp_in, stage, field, value_data_p)
+        elif field_ in cost_fields:
+            acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, self.nlp_dims, self.nlp_in, stage, field, value_data_p)
+        elif field_ in out_fields:
+            acados_solver_common.ocp_nlp_out_set(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, value_data_p)
+        elif field_ in mem_fields:
+            acados_solver_common.ocp_nlp_set(self.nlp_config, self.nlp_solver, stage, field, value_data_p)
+        return
+
+
+    def set_param(self, stage_, np.ndarray[np.float64_t, ndim=1] value_):
+        cdef np.ndarray[np.float64_t, ndim=1, mode='c'] value = np.ascontiguousarray(value_, dtype=np.double)
+        acados_solver.acados_update_params(self.capsule, stage_, <double *> value_.data, value_.shape[0])
+
+    def cost_set(self, start_stage_, field_, value_, api='warn'):
+      self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
+
+    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
+        field = field_.encode('utf-8')
+        if len(value_.shape) > 2:
+          dim = value_.shape[1]*value_.shape[2]
+        else:
+          dim = value_.shape[1]
+          value_ = value_[None,:,:]
+
+        cdef np.ndarray[np.float64_t, ndim=3, mode='c'] value = np.ascontiguousarray(value_, dtype=np.double)
+
+        acados_solver_common.ocp_nlp_cost_model_set_slice(self.nlp_config, self.nlp_dims, self.nlp_in, start_stage_, end_stage_,
+            field, <void *> value.data, dim)
+
+
+    def constraints_set(self, start_stage_, field_, value_, api='warn'):
+      self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
+
+
+    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
+        """
+
+        field = field_.encode('utf-8')
+        if len(value_.shape) > 2:
+          dim = value_.shape[1]*value_.shape[2]
+        else:
+          dim = value_.shape[1]
+          value_ = value_[None,:,:]
+
+        cdef np.ndarray[np.float64_t, ndim=3, mode='c'] value = np.ascontiguousarray(value_, dtype=np.double)
+
+        acados_solver_common.ocp_nlp_constraints_model_set_slice(self.nlp_config, self.nlp_dims, self.nlp_in, start_stage_, end_stage_,
+            field, <void*> value.data, dim)
+
+
+    def dynamics_get(self, int stage, field_):
+        """
+        Get numerical data from the dynamics module of the solver:
+
+            :param stage: integer corresponding to shooting node
+            :param field: string, e.g. 'A'
+        """
+
+        field = field_
+        field = field.encode('utf-8')
+
+        # get dims
+        cdef int[2] dims
+        acados_solver_common.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0])
+
+        # create output data
+        out = np.ascontiguousarray(np.zeros((dims[0]*dims[1],)), dtype=np.float64)
+        out = out.reshape(dims[0], dims[1], order='F')
+
+        # call getter
+        acados_solver_common.ocp_nlp_get_at_stage(self.nlp_config, self.nlp_dims, self.nlp_solver, stage, field, <void *> out.data)
+
+        return out
+
+
+    def options_set(self, bytes field_, value_):
+        """
+        Set options of the solver.
+
+            :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
+            :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']
+        string_fields = ['globalization']
+
+        # encode
+        field = field_
+        field = field.encode('utf-8')
+
+        cdef int int_value
+        cdef double double_value
+        cdef unsigned char[:] string_value
+
+        # check field availability and type
+        if field_ in int_fields:
+            if not isinstance(value_, int):
+                raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_)))
+
+            if field_ == 'rti_phase':
+                if value_ < 0 or value_ > 2:
+                    raise Exception('AcadosOcpSolver.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 '
+                        'take only value 0 for SQP-type solvers')
+
+            int_value = value_
+            acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &int_value)
+
+        elif field_ in double_fields:
+            if not isinstance(value_, float):
+                raise Exception('solver option {} must be of type float. You have {}.'.format(field_, type(value_)))
+
+            double_value = value_
+            acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &double_value)
+
+        elif field_ in string_fields:
+            if not isinstance(value_, bytes):
+                raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_)))
+
+            string_value = value_.encode('utf-8')
+            acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &string_value[0])
+
+        raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
+            '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
+
+
+    def __del__(self):
+        if self.solver_created:
+            acados_solver.acados_free(self.capsule)
+            acados_solver.acados_free_capsule(self.capsule)
+
+            # try:
+            #     self.dlclose(self.shared_lib._handle)
+            # except:
+            #     pass

pyextra/acados_template/acados_solver_common.pxd

@@ -0,0 +1,106 @@
+# cython: language_level=3
+# cython: profile=True
+# distutils: language = c
+
+# -*- coding: future_fstrings -*-
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+cdef extern from "acados/ocp_nlp/ocp_nlp_common.h":
+    ctypedef struct ocp_nlp_config:
+        pass
+
+    ctypedef struct ocp_nlp_dims:
+        pass
+
+    ctypedef struct ocp_nlp_in:
+        pass
+
+    ctypedef struct ocp_nlp_out:
+        pass
+
+
+cdef extern from "acados_c/ocp_nlp_interface.h":
+    ctypedef enum ocp_nlp_solver_t:
+        pass
+
+    ctypedef enum ocp_nlp_cost_t:
+        pass
+
+    ctypedef enum ocp_nlp_dynamics_t:
+        pass
+
+    ctypedef enum ocp_nlp_constraints_t:
+        pass
+
+    ctypedef enum ocp_nlp_reg_t:
+        pass
+
+    ctypedef struct ocp_nlp_plan:
+        pass
+
+    ctypedef struct ocp_nlp_solver:
+        pass
+
+    int ocp_nlp_cost_model_set_slice(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in_,
+        int start_stage, int end_stage, const char *field, void *value, int dim)
+    int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in_,
+        int start_stage, const char *field, void *value)
+    int ocp_nlp_constraints_model_set_slice(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in_,
+        int start_stage, int end_stage, const char *field, void *value, int dim)
+    int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims,
+        ocp_nlp_in *in_, int stage, const char *field, void *value)
+
+    # out
+    void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+        int stage, const char *field, void *value)
+    void ocp_nlp_out_get_slice(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+        int start_stage, int end_stage, const char *field, void *value)
+    void ocp_nlp_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_solver *solver,
+        int stage, const char *field, void *value)
+    int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+        int stage, const char *field)
+    void ocp_nlp_dynamics_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+        int stage, const char *field, int *dims_out)
+
+    # opts
+    void ocp_nlp_solver_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void* value)
+
+    # solver
+    void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out)
+    void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in_, ocp_nlp_out *nlp_out)
+
+    # get/set
+    void ocp_nlp_get(ocp_nlp_config *config, ocp_nlp_solver *solver, const char *field, void *return_value_)
+    void ocp_nlp_set(ocp_nlp_config *config, ocp_nlp_solver *solver, int stage, const char *field, void *value)

pyextra/acados_template/c_templates_tera/acados_solver.in.pxd

@@ -0,0 +1,22 @@
+cimport acados_solver_common
+
+cdef extern from "acados_solver_{{ model.name }}.h":
+    ctypedef struct nlp_solver_capsule:
+        pass
+
+    nlp_solver_capsule * acados_create_capsule "{{ model.name }}_acados_create_capsule"()
+    int acados_free_capsule "{{ model.name }}_acados_free_capsule"(nlp_solver_capsule *capsule)
+
+    int acados_create "{{ model.name }}_acados_create"(nlp_solver_capsule * capsule)
+    int acados_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)
+    void acados_print_stats "{{ model.name }}_acados_print_stats"(nlp_solver_capsule * capsule)
+
+    acados_solver_common.ocp_nlp_in *acados_get_nlp_in "{{ model.name }}_acados_get_nlp_in"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_out *acados_get_nlp_out "{{ model.name }}_acados_get_nlp_out"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "{{ model.name }}_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_config *acados_get_nlp_config "{{ model.name }}_acados_get_nlp_config"(nlp_solver_capsule * capsule)
+    void *acados_get_nlp_opts "{{ model.name }}_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "{{ model.name }}_acados_get_nlp_dims"(nlp_solver_capsule * capsule)
+    acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "{{ model.name }}_acados_get_nlp_plan"(nlp_solver_capsule * capsule)

selfdrive/controls/lib/lateral_mpc_lib/SConscript

@@ -1,4 +1,4 @@
-Import('env', 'arch')
+Import('env', 'envCython', 'arch', 'common')
 
 gen = "c_generated_code"
 
@@ -53,6 +53,25 @@ lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
 lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
 lenv["CCFLAGS"].append("-Wno-unused")
 lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
-lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat",
+lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat",
-                   build_files,
+                                build_files,
-                   LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
+                                LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
+
+# generate cython stuff
+acados_ocp_solver_pyx = File("#pyextra/acados_template/acados_ocp_solver_pyx.pyx")
+libacados_ocp_solver_lat = File('#selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/libacados_ocp_solver_lat.so')
+libacados_ocp_solver_lat_pxd = File('#selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_solver.pxd')
+libacados_ocp_solver_lat_c = File('#selfdrive/controls/lib/lateral_mpc_lib/c_generated_code/acados_ocp_solver_pyx.c')
+
+lenv2 = envCython.Clone()
+lenv2["LINKFLAGS"] += [libacados_ocp_solver_lat.get_labspath()]
+lenv2.Command(libacados_ocp_solver_lat_c,
+  [acados_ocp_solver_pyx, libacados_ocp_solver_lat_pxd],  # TODO fix
+  f'cython' + \
+  f' -o {libacados_ocp_solver_lat_c.get_labspath()}' + \
+  f' -I {libacados_ocp_solver_lat_pxd.get_dir().get_labspath()}' + \
+  f' -I {acados_ocp_solver_pyx.get_dir().get_labspath()}' + \
+  f' {acados_ocp_solver_pyx.get_labspath()}')
+lib_cython = lenv2.Program(f'{libacados_ocp_solver_lat_c.get_dir().get_labspath()}/acados_ocp_solver_pyx.so',
+  [libacados_ocp_solver_lat_c])
+lenv2.Depends(lib_cython, lib_solver)

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -5,8 +5,11 @@ import numpy as np
 from casadi import SX, vertcat, sin, cos
 from selfdrive.controls.lib.drive_helpers import LAT_MPC_N as N
 from selfdrive.controls.lib.drive_helpers import T_IDXS
-from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 
+if __name__ == '__main__':  # generating code
+  from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+else:
+  from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast  # pylint: disable=no-name-in-module
 
 LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
 EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
@@ -110,7 +113,7 @@ def gen_lat_mpc_solver():
 
 class LateralMpc():
   def __init__(self, x0=np.zeros(X_DIM)):
-    self.solver = AcadosOcpSolver('lat', N, EXPORT_DIR)
+    self.solver = AcadosOcpSolverFast('lat', N, EXPORT_DIR)
     self.reset(x0)
 
   def reset(self, x0=np.zeros(X_DIM)):

selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -1,4 +1,4 @@
-Import('env', 'arch')
+Import('env', 'envCython', 'arch', 'common')
 
 gen = "c_generated_code"
 
@@ -63,6 +63,25 @@ lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
 lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
 lenv["CCFLAGS"].append("-Wno-unused")
 lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
-lenv.SharedLibrary(f"{gen}/acados_ocp_solver_long",
+lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_long",
-                   build_files,
+                                build_files,
-                   LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
+                                LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
+
+# generate cython stuff
+acados_ocp_solver_pyx = File("#pyextra/acados_template/acados_ocp_solver_pyx.pyx")
+libacados_ocp_solver_long = File('#selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/libacados_ocp_solver_long.so')
+libacados_ocp_solver_long_pxd = File('#selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_solver.pxd')
+libacados_ocp_solver_long_c = File('#selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code/acados_ocp_solver_pyx.c')
+
+lenv2 = envCython.Clone()
+lenv2["LINKFLAGS"] += [libacados_ocp_solver_long.get_labspath()]
+lenv2.Command(libacados_ocp_solver_long_c,
+  [acados_ocp_solver_pyx, libacados_ocp_solver_long_pxd],  # TODO fix
+  f'cython' + \
+  f' -o {libacados_ocp_solver_long_c.get_labspath()}' + \
+  f' -I {libacados_ocp_solver_long_pxd.get_dir().get_labspath()}' + \
+  f' -I {acados_ocp_solver_pyx.get_dir().get_labspath()}' + \
+  f' {acados_ocp_solver_pyx.get_labspath()}')
+lib_cython = lenv2.Program(f'{libacados_ocp_solver_long_c.get_dir().get_labspath()}/acados_ocp_solver_pyx.so',
+  [libacados_ocp_solver_long_c])
+lenv2.Depends(lib_cython, lib_solver)

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -8,7 +8,11 @@ from selfdrive.swaglog import cloudlog
 from selfdrive.modeld.constants import index_function
 from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
 
-from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+if __name__ == '__main__':  # generating code
+  from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+else:
+  from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast  # pylint: disable=no-name-in-module
+
 from casadi import SX, vertcat
 
 LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
@@ -190,7 +194,7 @@ class LongitudinalMpc():
     self.source = SOURCES[2]
 
   def reset(self):
-    self.solver = AcadosOcpSolver('long', N, EXPORT_DIR)
+    self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR)
     self.v_solution = [0.0 for i in range(N+1)]
     self.a_solution = [0.0 for i in range(N+1)]
     self.j_solution = [0.0 for i in range(N)]

third_party/acados/build.sh

@@ -18,7 +18,7 @@ if [ ! -d acados_repo/ ]; then
 fi
 cd acados_repo
 git fetch
-git checkout 43ba28e95062f9ac9b48facd3b45698d57666fa3
+git checkout 08b4ad0b290ea0825c8ddc28111f5071e40d6b43
 git submodule update --recursive --init
 
 # build

third_party/acados/include/acados_c/ocp_nlp_interface.h

@@ -300,6 +300,8 @@ void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims
 void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
         int stage, const char *field, int *dims_out);
 
+void ocp_nlp_dynamics_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+        int stage, const char *field, int *dims_out);
 
 /* opts */
 
@@ -374,6 +376,8 @@ int ocp_nlp_precompute(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *
 /// \param nlp_out The output struct.
 void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
 
+//
+void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
 
 //
 void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out);