longitudinal MPC: use reset() function instead of recreating the solver in (commaai#24091)

* scons: add acados_template as dependency for lat and long mpc

* long MPC: use acados reset instead of recreating the solver

* long MPC: print timings and reset commented

* update acados x86_64

* update acados include folder

* update acados Python interface

* update acados reference commit to latest acados/master

* update x86 libs

* update comma two

* update acados again with commit 8ea8827fafb1b23b4c7da1c4cf650de1cbd73584

* update comma two

* update comma three

* update x86

Co-authored-by: Adeeb Shihadeh <[email protected]>
Co-authored-by: Comma Device <[email protected]>

Author: 3 people

pyextra/acados_template/acados_layout.json

@@ -668,6 +668,9 @@
         "hessian_approx": [
             "str"
         ],
+        "hpipm_mode": [
+            "str"
+        ],
         "regularize_method": [
             "str"
         ],

pyextra/acados_template/acados_ocp.py

@@ -2155,6 +2155,7 @@ def __init__(self):
         self.__globalization_use_SOC = 0
         self.__full_step_dual = 0
         self.__eps_sufficient_descent = 1e-4
+        self.__hpipm_mode = 'BALANCE'
 
 
     @property
@@ -2165,6 +2166,21 @@ def qp_solver(self):
         """
         return self.__qp_solver
 
+    @property
+    def hpipm_mode(self):
+        """
+        Mode of HPIPM to be used,
+
+        String in ('BALANCE', 'SPEED_ABS', 'SPEED', 'ROBUST').
+
+        Default: 'BALANCE'.
+
+        see https://cdn.syscop.de/publications/Frison2020a.pdf
+        and the HPIPM code:
+        https://github.com/giaf/hpipm/blob/master/ocp_qp/x_ocp_qp_ipm.c#L69
+        """
+        return self.__hpipm_mode
+
     @property
     def hessian_approx(self):
         """Hessian approximation.
@@ -2541,6 +2557,15 @@ def collocation_type(self, collocation_type):
             raise Exception('Invalid collocation_type value. Possible values are:\n\n' \
                     + ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\nExiting.')
 
+    @hpipm_mode.setter
+    def hpipm_mode(self, hpipm_mode):
+        hpipm_modes = ('BALANCE', 'SPEED_ABS', 'SPEED', 'ROBUST')
+        if hpipm_mode in hpipm_modes:
+            self.__hpipm_mode = hpipm_mode
+        else:
+            raise Exception('Invalid hpipm_mode value. Possible values are:\n\n' \
+                    + ',\n'.join(hpipm_modes) + '.\n\nYou have: ' + hpipm_mode + '.\n\nExiting.')
+
     @hessian_approx.setter
     def hessian_approx(self, hessian_approx):
         hessian_approxs = ('GAUSS_NEWTON', 'EXACT')
@@ -2890,10 +2915,10 @@ def __init__(self, acados_path=''):
         self.solver_options = AcadosOcpOptions()
         """Solver Options, type :py:class:`acados_template.acados_ocp.AcadosOcpOptions`"""
 
-        self.acados_include_path = f'{acados_path}/include'
-        """Path to acados include directory, type: string"""
-        self.acados_lib_path = f'{acados_path}/lib'
-        """Path to where acados library is located, type: string"""
+        self.acados_include_path = os.path.join(acados_path, 'include').replace(os.sep, '/') # the replace part is important on Windows for CMake
+        """Path to acados include directory (set automatically), type: `string`"""
+        self.acados_lib_path = os.path.join(acados_path, 'lib').replace(os.sep, '/') # the replace part is important on Windows for CMake
+        """Path to where acados library is located, type: `string`"""
 
         import numpy
         self.cython_include_dirs = numpy.get_include()

pyextra/acados_template/acados_ocp_solver.py

@@ -112,6 +112,13 @@ cdef class AcadosOcpSolverCython:
         return acados_solver.acados_solve(self.capsule)
 
 
+    def reset(self):
+        """
+        Sets current iterate to all zeros.
+        """
+        return acados_solver.acados_reset(self.capsule)
+
+
     def set_new_time_steps(self, new_time_steps):
         """
         Set new time steps.
@@ -450,12 +457,12 @@ cdef class AcadosOcpSolverCython:
         return out
 
 
-    def get_residuals(self):
+    def get_residuals(self, recompute=False):
         """
         Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
         """
         # compute residuals if RTI
-        if self.nlp_solver_type == 'SQP_RTI':
+        if self.nlp_solver_type == 'SQP_RTI' or recompute:
             acados_solver_common.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
 
         # create output array
@@ -504,7 +511,7 @@ cdef class AcadosOcpSolverCython:
         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']
+        mem_fields = ['xdot_guess', 'z_guess']
 
         field = field_.encode('utf-8')
 
@@ -635,11 +642,21 @@ cdef class AcadosOcpSolverCython:
         """
         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
+            :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0'
+
+            :param value: of type int, float, string
+
+            - qp_tol_stat: QP solver tolerance stationarity
+            - qp_tol_eq: QP solver tolerance equalities
+            - qp_tol_ineq: QP solver tolerance inequalities
+            - qp_tol_comp: QP solver tolerance complementarity
+            - qp_tau_min: for HPIPM QP solvers: minimum value of barrier parameter in HPIPM
+            - qp_mu0: for HPIPM QP solvers: initial value for complementarity slackness
+            - warm_start_first_qp: indicates if first QP in SQP is warm_started
         """
-        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC']
-        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent']
+        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks', 'qp_warm_start', 'line_search_use_sufficient_descent', 'full_step_dual', 'globalization_use_SOC', 'warm_start_first_qp']
+        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction', 'eps_sufficient_descent',
+        'qp_tol_stat', 'qp_tol_eq', 'qp_tol_ineq', 'qp_tol_comp', 'qp_tau_min', 'qp_mu0']
         string_fields = ['globalization']
 
         # encode

pyextra/acados_template/acados_ocp_solver_pyx.pyx

@@ -294,9 +294,9 @@ def __init__(self, acados_path=''):
         self.solver_options = AcadosSimOpts()
         """Solver Options, type :py:class:`acados_template.acados_sim.AcadosSimOpts`"""
 
-        self.acados_include_path = f'{acados_path}/include'
-        """Path to acados include directors (set automatically), type: `string`"""
-        self.acados_lib_path = f'{acados_path}/lib'
+        self.acados_include_path = os.path.join(acados_path, 'include').replace(os.sep, '/') # the replace part is important on Windows for CMake
+        """Path to acados include directory (set automatically), type: `string`"""
+        self.acados_lib_path = os.path.join(acados_path, 'lib').replace(os.sep, '/') # the replace part is important on Windows for CMake
         """Path to where acados library is located (set automatically), type: `string`"""
 
         self.code_export_directory = 'c_generated_code'

pyextra/acados_template/acados_sim.py

@@ -47,6 +47,7 @@
 from .acados_model import acados_model_strip_casadi_symbolics
 from .utils import is_column, render_template, format_class_dict, np_array_to_list,\
      make_model_consistent, set_up_imported_gnsf_model, get_python_interface_path
+from .builders import CMakeBuilder
 
 
 def make_sim_dims_consistent(acados_sim):
@@ -111,7 +112,17 @@ def sim_formulation_json_dump(acados_sim, json_file='acados_sim.json'):
         json.dump(sim_json, f, default=np_array_to_list, indent=4, sort_keys=True)
 
 
-def sim_render_templates(json_file, model_name, code_export_dir):
+def sim_get_default_cmake_builder() -> CMakeBuilder:
+    """
+    If :py:class:`~acados_template.acados_sim_solver.AcadosSimSolver` is used with `CMake` this function returns a good first setting.
+    :return: default :py:class:`~acados_template.builders.CMakeBuilder`
+    """
+    cmake_builder = CMakeBuilder()
+    cmake_builder.options_on = ['BUILD_ACADOS_SIM_SOLVER_LIB']
+    return cmake_builder
+
+
+def sim_render_templates(json_file, model_name, code_export_dir, cmake_options: CMakeBuilder = None):
     # setting up loader and environment
     json_path = os.path.join(os.getcwd(), json_file)
 
@@ -129,9 +140,15 @@ def sim_render_templates(json_file, model_name, code_export_dir):
     out_file = f'acados_sim_solver_{model_name}.h'
     render_template(in_file, out_file, template_dir, json_path)
 
-    in_file = 'Makefile.in'
-    out_file = f'Makefile'
-    render_template(in_file, out_file, template_dir, json_path)
+    # Builder
+    if cmake_options is not None:
+        in_file = 'CMakeLists.in.txt'
+        out_file = 'CMakeLists.txt'
+        render_template(in_file, out_file, template_dir, json_path)
+    else:
+        in_file = 'Makefile.in'
+        out_file = 'Makefile'
+        render_template(in_file, out_file, template_dir, json_path)
 
     in_file = 'main_sim.in.c'
     out_file = f'main_sim_{model_name}.c'
@@ -161,15 +178,19 @@ def sim_generate_casadi_functions(acados_sim):
     elif integrator_type == 'GNSF':
         generate_c_code_gnsf(model, opts)
 
+
 class AcadosSimSolver:
     """
     Class to interact with the acados integrator C object.
 
-    :param acados_sim: type :py:class:`acados_template.acados_ocp.AcadosOcp` (takes values to generate an instance :py:class:`acados_template.acados_sim.AcadosSim`) or :py:class:`acados_template.acados_sim.AcadosSim`
-    :param json_file: Default: 'acados_sim.json'
-    :param build: Default: True
+        :param acados_sim: type :py:class:`~acados_template.acados_ocp.AcadosOcp` (takes values to generate an instance :py:class:`~acados_template.acados_sim.AcadosSim`) or :py:class:`~acados_template.acados_sim.AcadosSim`
+        :param json_file: Default: 'acados_sim.json'
+        :param build: Default: True
+        :param cmake_builder: type :py:class:`~acados_template.utils.CMakeBuilder` generate a `CMakeLists.txt` and use
+            the `CMake` pipeline instead of a `Makefile` (`CMake` seems to be the better option in conjunction with
+            `MS Visual Studio`); default: `None`
     """
-    def __init__(self, acados_sim_, json_file='acados_sim.json', build=True):
+    def __init__(self, acados_sim_, json_file='acados_sim.json', build=True, cmake_builder: CMakeBuilder = None):
 
         self.solver_created = False
 
@@ -203,12 +224,16 @@ def __init__(self, acados_sim_, json_file='acados_sim.json', build=True):
         code_export_dir = acados_sim.code_export_directory
         if build:
             # render templates
-            sim_render_templates(json_file, model_name, code_export_dir)
+            sim_render_templates(json_file, model_name, code_export_dir, cmake_builder)
 
-            ## Compile solver
+            # Compile solver
             cwd = os.getcwd()
+            code_export_dir = os.path.abspath(code_export_dir)
             os.chdir(code_export_dir)
-            os.system('make sim_shared_lib')
+            if cmake_builder is not None:
+                cmake_builder.exec(code_export_dir)
+            else:
+                os.system('make sim_shared_lib')
             os.chdir(cwd)
 
         self.sim_struct = acados_sim
@@ -234,8 +259,15 @@ def __init__(self, acados_sim_, json_file='acados_sim.json', build=True):
             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)
+        lib_prefix = 'lib'
+        lib_ext = '.so'
+        if os.name == 'nt':
+            lib_prefix = ''
+            lib_ext = ''
+        self.shared_lib_name = os.path.join(code_export_dir, f'{lib_prefix}acados_sim_solver_{model_name}{lib_ext}')
+        print(f'self.shared_lib_name = "{self.shared_lib_name}"')
+        
+        self.shared_lib = CDLL(self.shared_lib_name)
 
 
         # create capsule

pyextra/acados_template/acados_sim_solver.py

@@ -0,0 +1,116 @@
+# -*- coding: future_fstrings -*-
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import os
+import sys
+from subprocess import call
+
+
+class CMakeBuilder:
+    """
+    Class to work with the `CMake` build system.
+    """
+    def __init__(self):
+        self._source_dir = None  # private source directory, this is set to code_export_dir
+        self.build_dir = 'build'
+        self._build_dir = None  # private build directory, usually rendered to abspath(build_dir)
+        self.generator = None
+        """Defines the generator, options can be found via `cmake --help` under 'Generator'. Type: string. Linux default 'Unix Makefiles', Windows 'Visual Studio 15 2017 Win64'; default value: `None`."""
+        # set something for Windows
+        if os.name == 'nt':
+            self.generator = 'Visual Studio 15 2017 Win64'
+        self.build_targets = None
+        """A comma-separated list of the build targets, if `None` then all targets will be build; type: List of strings; default: `None`."""
+        self.options_on = None
+        """List of strings as CMake options which are translated to '-D Opt[0]=ON -D Opt[1]=ON ...'; default: `None`."""
+
+    # Generate the command string for handling the cmake command.
+    def get_cmd1_cmake(self):
+        defines_str = ''
+        if self.options_on is not None:
+            defines_arr = [f' -D{opt}=ON' for opt in self.options_on]
+            defines_str = ' '.join(defines_arr)
+        generator_str = ''
+        if self.generator is not None:
+            generator_str = f' -G"{self.generator}"'
+        return f'cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX="{self._source_dir}"{defines_str}{generator_str} -Wdev -S"{self._source_dir}" -B"{self._build_dir}"'
+
+    # Generate the command string for handling the build.
+    def get_cmd2_build(self):
+        import multiprocessing
+        cmd = f'cmake --build "{self._build_dir}" --config Release -j{multiprocessing.cpu_count()}'
+        if self.build_targets is not None:
+            cmd += f' -t {self.build_targets}'
+        return cmd
+
+    # Generate the command string for handling the install command.
+    def get_cmd3_install(self):
+        return f'cmake --install "{self._build_dir}"'
+
+    def exec(self, code_export_directory):
+        """
+        Execute the compilation using `CMake` with the given settings.
+        :param code_export_directory: must be the absolute path to the directory where the code was exported to
+        """
+        if(os.path.isabs(code_export_directory) is False):
+            print(f'(W) the code export directory "{code_export_directory}" is not an absolute path!')
+        self._source_dir = code_export_directory
+        self._build_dir = os.path.abspath(self.build_dir)
+        try:
+            os.mkdir(self._build_dir)
+        except FileExistsError as e:
+            pass
+
+        try:
+            os.chdir(self._build_dir)
+            cmd_str = self.get_cmd1_cmake()
+            print(f'call("{cmd_str})"')
+            retcode = call(cmd_str, shell=True)
+            if retcode != 0:
+                raise RuntimeError(f'CMake command "{cmd_str}" was terminated by signal {retcode}')
+            cmd_str = self.get_cmd2_build()
+            print(f'call("{cmd_str}")')
+            retcode = call(cmd_str, shell=True)
+            if retcode != 0:
+                raise RuntimeError(f'Build command "{cmd_str}" was terminated by signal {retcode}')
+            cmd_str = self.get_cmd3_install()
+            print(f'call("{cmd_str}")')
+            retcode = call(cmd_str, shell=True)
+            if retcode != 0:
+                raise RuntimeError(f'Install command "{cmd_str}" was terminated by signal {retcode}')
+        except OSError as e:
+            print("Execution failed:", e, file=sys.stderr)
+        except Exception as e:
+            print("Execution failed:", e, file=sys.stderr)
+            exit(1)