Acados long fast (#22233)

* acados long

* new ref

* SPPEEEEEDDD

* less iterations

* this shouldn't be so high

* reset only essentials

* minimal reset for long mpc

* more cpu usage plannerd

* Use lead mpc even when going to crash

* reset to current state

* Use open loop speed for lead mpc

* 1 iteration is too little for cruise mpc

* add whitespace

* update refs

phonelibs/acados/build.sh

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

pyextra/acados_template/acados_ocp_solver.py

@@ -881,6 +881,15 @@ def __init__(self, model_name, N, code_export_dir):
         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_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
+        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_get_at_stage.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+
+
     def solve(self):
         """
         Solve the ocp with current input.
@@ -893,24 +902,13 @@ def solve(self):
 
 
     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]
+        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)
+        self.fill_in_slice(start_stage_, end_stage_, field_, out)
+        return out
 
-            .. 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
-        """
+    def fill_in_slice(self, start_stage_, end_stage_, field_, arr):
         out_fields = ['x', 'u', 'z', 'pi', 'lam', 't']
         mem_fields = ['sl', 'su']
         field = field_
@@ -931,30 +929,16 @@ def get_slice(self, start_stage_, end_stage_, field_):
 
         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))
+        out_data = cast(arr.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(self, stage_, field_):
         return self.get_slice(stage_, stage_ + 1, field_)[0]
 
@@ -1255,13 +1239,10 @@ def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
         """
         # cast value_ to avoid conversion issues
         field = field_.encode('utf-8')
-        dim = np.product(value_.shape[1:])
-
-        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)
+        if len(value_.shape) > 2:
+          dim = value_.shape[1]*value_.shape[2]
+        else:
+          dim = value_.shape[1]
 
         self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config, \
             self.nlp_dims, self.nlp_in, start_stage_, end_stage_, field,
@@ -1280,18 +1261,12 @@ def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='w
             :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_])
 
         field = field_.encode('utf-8')
-        dim = np.product(value_.shape[1:])
-
-        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)
+        if len(value_.shape) > 2:
+          dim = value_.shape[1]*value_.shape[2]
+        else:
+          dim = value_.shape[1]
 
         self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \
             self.nlp_dims, self.nlp_in, start_stage_, end_stage_, field,

selfdrive/controls/lib/fcw.py

@@ -44,8 +44,7 @@ def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead):
       ttc = min(2 * x_lead / (math.sqrt(delta) + v_rel), max_ttc)
     return ttc
 
-  def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
-    mpc_solution_a = list(mpc_solution[0].a_ego)
+  def update(self, mpc_solution_a, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
 
     self.last_min_a = min(mpc_solution_a)
     self.v_lead_max = max(self.v_lead_max, v_lead)

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -11,7 +11,7 @@
 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"
-
+X_DIM = 6
 
 def gen_lat_model():
   model = AcadosModel()
@@ -56,7 +56,6 @@ def gen_lat_mpc_solver():
   ocp = AcadosOcp()
   ocp.model = gen_lat_model()
 
-  N = 16
   Tf = np.array(T_IDXS)[N]
 
   # set dimensions
@@ -109,13 +108,13 @@ def gen_lat_mpc_solver():
 
 
 class LateralMpc():
-  def __init__(self, x0=np.zeros(6)):
+  def __init__(self, x0=np.zeros(X_DIM)):
     self.solver = AcadosOcpSolver('lat', N, EXPORT_DIR)
     self.reset(x0)
 
-  def reset(self, x0=np.zeros(6)):
-    self.x_sol = np.zeros((N+1, 4))
-    self.u_sol = np.zeros((N))
+  def reset(self, x0=np.zeros(X_DIM)):
+    self.x_sol = np.zeros((N+1, X_DIM))
+    self.u_sol = np.zeros((N, 1))
     self.yref = np.zeros((N+1, 3))
     self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
     self.solver.cost_set(N, "yref", self.yref[N][:2])
@@ -124,7 +123,7 @@ def reset(self, x0=np.zeros(6)):
 
     # Somehow needed for stable init
     for i in range(N+1):
-      self.solver.set(i, 'x', np.zeros(6))
+      self.solver.set(i, 'x', np.zeros(X_DIM))
     self.solver.constraints_set(0, "lbx", x0)
     self.solver.constraints_set(0, "ubx", x0)
     self.solver.solve()
@@ -149,8 +148,8 @@ def run(self, x0, v_ego, car_rotation_radius, y_pts, heading_pts):
     self.solver.cost_set(N, "yref", self.yref[N][:2])
 
     self.solution_status = self.solver.solve()
-    self.x_sol = self.solver.get_slice(0, N+1, 'x')
-    self.u_sol = self.solver.get_slice(0, N, 'u')
+    self.solver.fill_in_slice(0, N+1, 'x', self.x_sol)
+    self.solver.fill_in_slice(0, N, 'u', self.u_sol)
     self.cost = self.solver.get_cost()
 
 

selfdrive/controls/lib/lead_mpc_lib/.gitignore

@@ -1,2 +1,2 @@
-generator
-lib_qp/
+acados_ocp_lead.json
+c_generated_code/

selfdrive/controls/lib/lead_mpc_lib/SConscript

@@ -1,48 +1,58 @@
 Import('env', 'arch')
 
+gen = "c_generated_code"
 
-cpp_path = [
-    "#phonelibs/acado/include",
-    "#phonelibs/acado/include/acado",
-    "#phonelibs/qpoases/INCLUDE",
-    "#phonelibs/qpoases/INCLUDE/EXTRAS",
-    "#phonelibs/qpoases/SRC/",
-    "#phonelibs/qpoases",
-    "lib_mpc_export",
+casadi_model = [
+  f'{gen}/lead_model/lead_expl_ode_fun.c',
+  f'{gen}/lead_model/lead_expl_vde_forw.c',
 ]
 
-generated_c = [
-    'lib_mpc_export/acado_auxiliary_functions.c',
-    'lib_mpc_export/acado_qpoases_interface.cpp',
-    'lib_mpc_export/acado_integrator.c',
-    'lib_mpc_export/acado_solver.c',
+casadi_cost_y = [
+  f'{gen}/lead_cost/lead_cost_y_fun.c',
+  f'{gen}/lead_cost/lead_cost_y_fun_jac_ut_xt.c',
+  f'{gen}/lead_cost/lead_cost_y_hess.c',
 ]
 
-generated_h = [
-    'lib_mpc_export/acado_common.h',
-    'lib_mpc_export/acado_auxiliary_functions.h',
-    'lib_mpc_export/acado_qpoases_interface.hpp',
+casadi_cost_e = [
+  f'{gen}/lead_cost/lead_cost_y_e_fun.c',
+  f'{gen}/lead_cost/lead_cost_y_e_fun_jac_ut_xt.c',
+  f'{gen}/lead_cost/lead_cost_y_e_hess.c',
 ]
 
+casadi_cost_0 = [
+  f'{gen}/lead_cost/lead_cost_y_0_fun.c',
+  f'{gen}/lead_cost/lead_cost_y_0_fun_jac_ut_xt.c',
+  f'{gen}/lead_cost/lead_cost_y_0_hess.c',
+]
 
-interface_dir = Dir('lib_mpc_export')
+build_files = [f'{gen}/acados_solver_lead.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0
 
-SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir'])
+# extra generated files used to trigger a rebuild
+generated_files = [
+  f'{gen}/Makefile',
 
-if GetOption('mpc_generate'):
-    generator_cpp = File('generator.cpp')
+  f'{gen}/main_lead.c',
+  f'{gen}/acados_solver_lead.h',
 
-    acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"),
-                  File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"),
-                  File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")]
+  f'{gen}/lead_model/lead_expl_vde_adj.c',
 
-    generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path,
-                            CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"])
+  f'{gen}/lead_model/lead_model.h',
+  f'{gen}/lead_cost/lead_cost_y_fun.h',
+  f'{gen}/lead_cost/lead_cost_y_e_fun.h',
+  f'{gen}/lead_cost/lead_cost_y_0_fun.h',
+] + build_files
 
-    cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}"
-    env.Command(generated_c + generated_h, generator, cmd)
+lenv = env.Clone()
+lenv.Clean(generated_files, Dir(gen))
 
+lenv.Command(generated_files,
+             ["lead_mpc.py"],
+             f"cd {Dir('.').abspath} && python lead_mpc.py")
 
-mpc_files = ["longitudinal_mpc.c"] + generated_c
-env.SharedLibrary('mpc0', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
-env.SharedLibrary('mpc1', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
+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_lead",
+                   build_files,
+                   LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])