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
old-commit-hash: 66c275b711

phonelibs/acados/build.sh

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:56eac2b5de0c9b1eccc31f072f315a17f3e116fd31acfb8720026ff551bc4f79
+oid sha256:3adaf7cd3c6bc6bb6e7c8bc35c9107ce618467c46e54e051785e786241281846
 size 2053

pyextra/acados_template/acados_ocp_solver.py

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:7cf452d4fc7959bc2b2dac312550b9ef41640c9762515c6a1b904aefbd304e23
+oid sha256:b406ef368856f2e70e02a56b42361d7c5937383529654489318e5831a1d23926
-size 61689
+size 60258

selfdrive/controls/lib/fcw.py

@@ -44,8 +44,7 @@ class FCWChecker():
       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):
+  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):
-    mpc_solution_a = list(mpc_solution[0].a_ego)
 
     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 @@ from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 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)):
+  def reset(self, x0=np.zeros(X_DIM)):
-    self.x_sol = np.zeros((N+1, 4))
+    self.x_sol = np.zeros((N+1, X_DIM))
-    self.u_sol = np.zeros((N))
+    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 @@ class LateralMpc():
 
     # 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 @@ class LateralMpc():
     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.solver.fill_in_slice(0, N+1, 'x', self.x_sol)
-    self.u_sol = self.solver.get_slice(0, N, 'u')
+    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
+acados_ocp_lead.json
-lib_qp/
+c_generated_code/

selfdrive/controls/lib/lead_mpc_lib/SConscript

@@ -1,48 +1,58 @@
 Import('env', 'arch')
 
+gen = "c_generated_code"
 
-cpp_path = [
+casadi_model = [
-    "#phonelibs/acado/include",
+  f'{gen}/lead_model/lead_expl_ode_fun.c',
-    "#phonelibs/acado/include/acado",
+  f'{gen}/lead_model/lead_expl_vde_forw.c',
-    "#phonelibs/qpoases/INCLUDE",
-    "#phonelibs/qpoases/INCLUDE/EXTRAS",
-    "#phonelibs/qpoases/SRC/",
-    "#phonelibs/qpoases",
-    "lib_mpc_export",
 ]
 
-generated_c = [
+casadi_cost_y = [
-    'lib_mpc_export/acado_auxiliary_functions.c',
+  f'{gen}/lead_cost/lead_cost_y_fun.c',
-    'lib_mpc_export/acado_qpoases_interface.cpp',
+  f'{gen}/lead_cost/lead_cost_y_fun_jac_ut_xt.c',
-    'lib_mpc_export/acado_integrator.c',
+  f'{gen}/lead_cost/lead_cost_y_hess.c',
-    'lib_mpc_export/acado_solver.c',
 ]
 
-generated_h = [
+casadi_cost_e = [
-    'lib_mpc_export/acado_common.h',
+  f'{gen}/lead_cost/lead_cost_y_e_fun.c',
-    'lib_mpc_export/acado_auxiliary_functions.h',
+  f'{gen}/lead_cost/lead_cost_y_e_fun_jac_ut_xt.c',
-    'lib_mpc_export/acado_qpoases_interface.hpp',
+  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'):
+  f'{gen}/main_lead.c',
-    generator_cpp = File('generator.cpp')
+  f'{gen}/acados_solver_lead.h',
 
-    acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"),
+  f'{gen}/lead_model/lead_expl_vde_adj.c',
-                  File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"),
-                  File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")]
 
-    generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path,
+  f'{gen}/lead_model/lead_model.h',
-                            CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"])
+  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()}"
+lenv = env.Clone()
-    env.Command(generated_c + generated_h, generator, cmd)
+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
+lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
-env.SharedLibrary('mpc0', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
+lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
-env.SharedLibrary('mpc1', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
+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'])

selfdrive/controls/lib/lead_mpc_lib/lead_mpc.py

@@ -0,0 +1,265 @@
+#!/usr/bin/env python3
+import os
+import math
+import numpy as np
+
+from common.realtime import sec_since_boot
+from selfdrive.swaglog import cloudlog
+from selfdrive.modeld.constants import T_IDXS
+from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG, CONTROL_N
+from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
+
+from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+from casadi import SX, vertcat, sqrt, exp
+
+LEAD_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
+EXPORT_DIR = os.path.join(LEAD_MPC_DIR, "c_generated_code")
+JSON_FILE = "acados_ocp_lead.json"
+
+MPC_T = list(np.arange(0,1.,.2)) + list(np.arange(1.,10.6,.6))
+N = len(MPC_T) - 1
+
+
+def RW(v_ego, v_l):
+  TR = 1.8
+  G = 9.81
+  return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G))
+
+
+def gen_lead_model():
+  model = AcadosModel()
+  model.name = 'lead'
+
+  # set up states & controls
+  x_ego = SX.sym('x_ego')
+  v_ego = SX.sym('v_ego')
+  a_ego = SX.sym('a_ego')
+  model.x = vertcat(x_ego, v_ego, a_ego)
+
+  # controls
+  j_ego = SX.sym('j_ego')
+  model.u = vertcat(j_ego)
+
+  # xdot
+  x_ego_dot = SX.sym('x_ego_dot')
+  v_ego_dot = SX.sym('v_ego_dot')
+  a_ego_dot = SX.sym('a_ego_dot')
+  model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
+
+  # live parameters
+  x_lead = SX.sym('x_lead')
+  v_lead = SX.sym('v_lead')
+  model.p = vertcat(x_lead, v_lead)
+
+  # dynamics model
+  f_expl = vertcat(v_ego, a_ego, j_ego)
+  model.f_impl_expr = model.xdot - f_expl
+  model.f_expl_expr = f_expl
+  return model
+
+
+def gen_lead_mpc_solver():
+  ocp = AcadosOcp()
+  ocp.model = gen_lead_model()
+
+  Tf = np.array(MPC_T)[-1]
+
+  # set dimensions
+  ocp.dims.N = N
+
+  # set cost module
+  ocp.cost.cost_type = 'NONLINEAR_LS'
+  ocp.cost.cost_type_e = 'NONLINEAR_LS'
+
+  QR = np.diag([0.0, 0.0, 0.0, 0.0])
+  Q = np.diag([0.0, 0.0, 0.0])
+
+  ocp.cost.W = QR
+  ocp.cost.W_e = Q
+
+  x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2]
+  j_ego = ocp.model.u[0]
+
+  ocp.cost.yref = np.zeros((4, ))
+  ocp.cost.yref_e = np.zeros((3, ))
+
+  x_lead, v_lead = ocp.model.p[0], ocp.model.p[1]
+  G = 9.81
+  TR = 1.8
+  desired_dist = (v_ego * TR
+                  - (v_lead - v_ego) * TR
+                  + v_ego*v_ego/(2*G)
+                  - v_lead * v_lead / (2*G))
+  dist_err = (desired_dist + 4.0 - (x_lead - x_ego))/(sqrt(v_ego + 0.5) + 0.1)
+
+  # TODO hacky weights to keep behavior the same
+  ocp.model.cost_y_expr = vertcat(exp(.3 * dist_err) - 1.,
+                                  ((x_lead - x_ego) - (desired_dist + 4.0)) / (0.05 * v_ego + 0.5),
+                                  a_ego * (.1 * v_ego + 1.0),
+                                  j_ego * (.1 * v_ego + 1.0))
+  ocp.model.cost_y_expr_e = vertcat(exp(.3 * dist_err) - 1.,
+                                  ((x_lead - x_ego) - (desired_dist + 4.0)) / (0.05 * v_ego + 0.5),
+                                  a_ego * (.1 * v_ego + 1.0))
+  ocp.parameter_values = np.array([0., .0])
+
+  # set constraints
+  ocp.constraints.constr_type = 'BGH'
+  ocp.constraints.idxbx = np.array([1,])
+  ocp.constraints.lbx = np.array([0,])
+  ocp.constraints.ubx = np.array([100.,])
+  x0 = np.array([0.0, 0.0, 0.0])
+  ocp.constraints.x0 = x0
+
+
+  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_tol_stat = 1e-3
+  #ocp.solver_options.tol = 1e-3
+
+  ocp.solver_options.qp_solver_iter_max = 10
+  #ocp.solver_options.qp_tol = 1e-3
+
+  # set prediction horizon
+  ocp.solver_options.tf = Tf
+  ocp.solver_options.shooting_nodes = np.array(MPC_T)
+
+  ocp.code_export_directory = EXPORT_DIR
+  return ocp
+
+
+class LeadMpc():
+  def __init__(self, lead_id):
+    self.lead_id = lead_id
+    self.solver = AcadosOcpSolver('lead', N, EXPORT_DIR)
+    self.v_solution = [0.0 for i in range(N)]
+    self.a_solution = [0.0 for i in range(N)]
+    self.j_solution = [0.0 for i in range(N-1)]
+    yref = np.zeros((N+1,4))
+    self.solver.cost_set_slice(0, N, "yref", yref[:N])
+    self.solver.set(N, "yref", yref[N][:3])
+    self.x_sol = np.zeros((N+1, 3))
+    self.u_sol = np.zeros((N,1))
+    self.lead_xv = np.zeros((N+1,2))
+    self.reset()
+    self.set_weights()
+
+  def reset(self):
+    for i in range(N+1):
+      self.solver.set(i, 'x', np.zeros(3))
+    self.last_cloudlog_t = 0
+    self.status = False
+    self.new_lead = False
+    self.prev_lead_status = False
+    self.crashing = False
+    self.prev_lead_x = 10
+    self.solution_status = 0
+    self.x0 = np.zeros(3)
+
+  def set_weights(self):
+    W = np.diag([MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
+                 MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK])
+    Ws = np.tile(W[None], reps=(N,1,1))
+    self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
+    #TODO hacky weights to keep behavior the same
+    self.solver.cost_set(N, 'W', (3./5.)*W[:3,:3])
+
+  def set_cur_state(self, v, a):
+    self.x0[1] = v
+    self.x0[2] = a
+
+  def extrapolate_lead(self, x_lead, v_lead, a_lead_0, a_lead_tau):
+    dt =.2
+    t = .0
+    for i in range(N+1):
+      if i > 4:
+        dt = .6
+      self.lead_xv[i, 0], self.lead_xv[i, 1] = x_lead, v_lead
+      a_lead = a_lead_0 * math.exp(-a_lead_tau * (t**2)/2.)
+      x_lead += v_lead * dt
+      v_lead += a_lead * dt
+      if v_lead < 0.0:
+        a_lead = 0.0
+        v_lead = 0.0
+      t += dt
+
+  def init_with_sim(self, v_ego, lead_xv, a_lead_0):
+    a_ego = min(0.0, -(v_ego - lead_xv[0,1]) * (v_ego - lead_xv[0,1]) / (2.0 * lead_xv[0,0] + 0.01) + a_lead_0)
+    dt =.2
+    t = .0
+    x_ego = 0.0
+    for i in range(N+1):
+      if i > 4:
+        dt = .6
+      v_ego += a_ego * dt
+      if v_ego <= 0.0:
+        v_ego = 0.0
+        a_ego = 0.0
+      x_ego += v_ego * dt
+      t += dt
+      self.solver.set(i, 'x', np.array([x_ego, v_ego, a_ego]))
+
+  def update(self, carstate, radarstate, v_cruise):
+    v_ego = self.x0[1]
+    if self.lead_id == 0:
+      lead = radarstate.leadOne
+    else:
+      lead = radarstate.leadTwo
+    self.status = lead.status
+    if lead is not None and lead.status:
+      x_lead = lead.dRel
+      v_lead = max(0.0, lead.vLead)
+      a_lead = lead.aLeadK
+
+      if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
+        v_lead = 0.0
+        a_lead = 0.0
+
+      self.a_lead_tau = lead.aLeadTau
+      self.new_lead = False
+      self.extrapolate_lead(x_lead, v_lead, a_lead, self.a_lead_tau)
+      if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
+        self.init_with_sim(v_ego, self.lead_xv, a_lead)
+        self.new_lead = True
+
+      self.prev_lead_status = True
+      self.prev_lead_x = x_lead
+    else:
+      self.prev_lead_status = False
+      # Fake a fast lead car, so mpc keeps running
+      x_lead = 50.0
+      v_lead = v_ego + 10.0
+      a_lead = 0.0
+      self.a_lead_tau = _LEAD_ACCEL_TAU
+      self.extrapolate_lead(x_lead, v_lead, a_lead, self.a_lead_tau)
+    self.solver.constraints_set(0, "lbx", self.x0)
+    self.solver.constraints_set(0, "ubx", self.x0)
+    for i in range(N+1):
+      self.solver.set_param(i, self.lead_xv[i])
+
+    self.solution_status = self.solver.solve()
+    self.solver.fill_in_slice(0, N+1, 'x', self.x_sol)
+    self.solver.fill_in_slice(0, N, 'u', self.u_sol)
+    #self.solver.print_statistics()
+
+    self.v_solution = np.interp(T_IDXS[:CONTROL_N], MPC_T, list(self.x_sol[:,1]))
+    self.a_solution = np.interp(T_IDXS[:CONTROL_N], MPC_T, list(self.x_sol[:,2]))
+    self.j_solution = np.interp(T_IDXS[:CONTROL_N], MPC_T[:-1], list(self.u_sol[:,0]))
+
+    # Reset if goes through lead car
+    self.crashing = np.sum(self.lead_xv[:,0] - self.x_sol[:,0] < 0) > 0
+
+    t = sec_since_boot()
+    if self.solution_status != 0:
+      if t > self.last_cloudlog_t + 5.0:
+        self.last_cloudlog_t = t
+        cloudlog.warning("Lead mpc %d reset, solution_status: %s" % (
+                          self.lead_id, self.solution_status))
+      self.prev_lead_status = False
+      self.reset()
+
+
+if __name__ == "__main__":
+  ocp = gen_lead_mpc_solver()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)

selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore

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

selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -1,47 +1,58 @@
 Import('env', 'arch')
 
-cpp_path = [
+gen = "c_generated_code"
-    "#",
+
-    "#selfdrive",
+casadi_model = [
-    "#phonelibs/acado/include",
+  f'{gen}/long_model/long_expl_ode_fun.c',
-    "#phonelibs/acado/include/acado",
+  f'{gen}/long_model/long_expl_vde_forw.c',
-    "#phonelibs/qpoases/INCLUDE",
+]
-    "#phonelibs/qpoases/INCLUDE/EXTRAS",
+
-    "#phonelibs/qpoases/SRC/",
+casadi_cost_y = [
-    "#phonelibs/qpoases",
+  f'{gen}/long_cost/long_cost_y_fun.c',
-    "lib_mpc_export",
+  f'{gen}/long_cost/long_cost_y_fun_jac_ut_xt.c',
+  f'{gen}/long_cost/long_cost_y_hess.c',
 ]
 
-generated_c = [
+casadi_cost_e = [
-    'lib_mpc_export/acado_auxiliary_functions.c',
+  f'{gen}/long_cost/long_cost_y_e_fun.c',
-    'lib_mpc_export/acado_qpoases_interface.cpp',
+  f'{gen}/long_cost/long_cost_y_e_fun_jac_ut_xt.c',
-    'lib_mpc_export/acado_integrator.c',
+  f'{gen}/long_cost/long_cost_y_e_hess.c',
-    'lib_mpc_export/acado_solver.c',
 ]
 
-generated_h = [
+casadi_cost_0 = [
-    'lib_mpc_export/acado_common.h',
+  f'{gen}/long_cost/long_cost_y_0_fun.c',
-    'lib_mpc_export/acado_auxiliary_functions.h',
+  f'{gen}/long_cost/long_cost_y_0_fun_jac_ut_xt.c',
-    'lib_mpc_export/acado_qpoases_interface.hpp',
+  f'{gen}/long_cost/long_cost_y_0_hess.c',
 ]
 
-interface_dir = Dir('lib_mpc_export')
+build_files = [f'{gen}/acados_solver_long.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'):
+  f'{gen}/main_long.c',
-  generator_cpp = File('generator.cpp')
+  f'{gen}/acados_solver_long.h',
 
-  acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"),
+  f'{gen}/long_model/long_expl_vde_adj.c',
-                File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"),
-                File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")]
 
-  generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path,
+  f'{gen}/long_model/long_model.h',
-                          CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"])
+  f'{gen}/long_cost/long_cost_y_fun.h',
+  f'{gen}/long_cost/long_cost_y_e_fun.h',
+  f'{gen}/long_cost/long_cost_y_0_fun.h',
+] + build_files
 
-  cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}"
+lenv = env.Clone()
-  env.Command(generated_c + generated_h, generator, cmd)
+lenv.Clean(generated_files, Dir(gen))
 
+lenv.Command(generated_files,
+             ["long_mpc.py"],
+             f"cd {Dir('.').abspath} && python long_mpc.py")
 
-mpc_files = ["longitudinal_mpc.c"] + generated_c
+lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
-env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
+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",
+                   build_files,
+                   LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -0,0 +1,183 @@
+#!/usr/bin/env python3
+import os
+import numpy as np
+
+from common.numpy_fast import clip
+from common.realtime import sec_since_boot
+from selfdrive.swaglog import cloudlog
+from selfdrive.controls.lib.drive_helpers import LON_MPC_N as N
+from selfdrive.modeld.constants import T_IDXS
+
+from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+from casadi import SX, vertcat
+
+
+
+
+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"
+
+
+def gen_long_model():
+  model = AcadosModel()
+  model.name = 'long'
+
+  # set up states & controls
+  x_ego = SX.sym('x_ego')
+  v_ego = SX.sym('v_ego')
+  a_ego = SX.sym('a_ego')
+  model.x = vertcat(x_ego, v_ego, a_ego)
+
+  # controls
+  j_ego = SX.sym('j_ego')
+  model.u = vertcat(j_ego)
+
+  # xdot
+  x_ego_dot = SX.sym('x_ego_dot')
+  v_ego_dot = SX.sym('v_ego_dot')
+  a_ego_dot = SX.sym('a_ego_dot')
+  model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
+
+  # dynamics model
+  f_expl = vertcat(v_ego, a_ego, j_ego)
+  model.f_impl_expr = model.xdot - f_expl
+  model.f_expl_expr = f_expl
+  return model
+
+
+def gen_long_mpc_solver():
+  ocp = AcadosOcp()
+  ocp.model = gen_long_model()
+
+  Tf = np.array(T_IDXS)[N]
+
+  # set dimensions
+  ocp.dims.N = N
+
+  # set cost module
+  ocp.cost.cost_type = 'NONLINEAR_LS'
+  ocp.cost.cost_type_e = 'NONLINEAR_LS'
+
+  QR = np.diag([0.0, 0.0, 0.0, 0.0])
+  Q = np.diag([0.0, 0.0, 0.0])
+
+  ocp.cost.W = QR
+  ocp.cost.W_e = Q
+
+  x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2]
+  j_ego = ocp.model.u[0]
+
+  ocp.cost.yref = np.zeros((4, ))
+  ocp.cost.yref_e = np.zeros((3, ))
+  # TODO hacky weights to keep behavior the same
+  ocp.model.cost_y_expr = vertcat(x_ego, v_ego, a_ego, j_ego)
+  ocp.model.cost_y_expr_e = vertcat(x_ego, v_ego, a_ego)
+
+  # set constraints
+  ocp.constraints.constr_type = 'BGH'
+  ocp.constraints.idxbx = np.array([0, 1,2])
+  ocp.constraints.lbx = np.array([0., 0, -1.2])
+  ocp.constraints.ubx = np.array([10000, 100., 1.2])
+  ocp.constraints.Jsbx = np.eye(3)
+  x0 = np.array([0.0, 0.0, 0.0])
+  ocp.constraints.x0 = x0
+
+  l2_penalty = 1.0
+  l1_penalty = 0.0
+  weights = np.array([0.0, 1e4, 1e4])
+  ocp.cost.Zl = l2_penalty * weights
+  ocp.cost.Zu = l2_penalty * weights
+  ocp.cost.zl = l1_penalty * weights
+  ocp.cost.zu = l1_penalty * weights
+
+  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.qp_solver_iter_max = 2
+
+  # set prediction horizon
+  ocp.solver_options.tf = Tf
+  ocp.solver_options.shooting_nodes = np.array(T_IDXS)[:N+1]
+
+  ocp.code_export_directory = EXPORT_DIR
+  return ocp
+
+
+class LongitudinalMpc():
+  def __init__(self):
+    self.solver = AcadosOcpSolver('long', N, EXPORT_DIR)
+    self.x_sol = np.zeros((N+1, 3))
+    self.u_sol = np.zeros((N, 1))
+    self.set_weights()
+
+    self.v_solution = [0.0 for i in range(len(T_IDXS))]
+    self.a_solution = [0.0 for i in range(len(T_IDXS))]
+    self.j_solution = [0.0 for i in range(len(T_IDXS)-1)]
+    self.yref = np.ones((N+1, 4))
+    self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
+    self.solver.cost_set(N, "yref", self.yref[N][:3])
+    self.T_IDXS = np.array(T_IDXS[:N+1])
+    self.min_a = -1.2
+    self.max_a = 1.2
+    self.mins = np.tile(np.array([0.0, 0.0, self.min_a])[None], reps=(N-1,1))
+    self.maxs = np.tile(np.array([0.0, 100.0, self.max_a])[None], reps=(N-1,1))
+    self.x0 = np.zeros(3)
+    self.reset()
+
+  def reset(self):
+    self.last_cloudlog_t = 0
+    self.status = True
+    self.solution_status = 0
+    for i in range(N+1):
+      self.solver.set(i, 'x', self.x0)
+
+  def set_weights(self):
+    W = np.diag([0.0, 1.0, 0.0, 50.0])
+    Ws = np.tile(W[None], reps=(N,1,1))
+    self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
+    #TODO hacky weights to keep behavior the same
+    self.solver.cost_set(N, 'W', (3/20.)*W[:3,:3])
+
+  def set_accel_limits(self, min_a, max_a):
+    self.min_a = min_a
+    self.max_a = max_a
+    self.mins[:,2] = self.min_a
+    self.maxs[:,2] = self.max_a
+    self.solver.constraints_set_slice(1, N, "lbx", self.mins, api='old')
+    self.solver.constraints_set_slice(1, N, "ubx", self.maxs, api='old')
+
+  def set_cur_state(self, v, a):
+    self.x0[1] = v
+    self.x0[2] = a
+    self.solver.constraints_set(0, "lbx", self.x0)
+    self.solver.constraints_set(0, "ubx", self.x0)
+
+  def update(self, carstate, model, v_cruise):
+    v_cruise_clipped = clip(v_cruise, self.x0[1] - 10., self.x0[1] + 10.0)
+    self.yref[:,0] = v_cruise_clipped * self.T_IDXS  # position
+    self.yref[:,1] = v_cruise_clipped * np.ones(N+1)  # speed
+    self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
+    self.solver.cost_set(N, "yref", self.yref[N][:3])
+
+    self.solution_status = self.solver.solve()
+    self.solver.fill_in_slice(0, N+1, 'x', self.x_sol)
+    self.solver.fill_in_slice(0, N, 'u', self.u_sol)
+    #self.solver.print_statistics()
+
+    self.v_solution = list(self.x_sol[:,1])
+    self.a_solution = list(self.x_sol[:,2])
+    self.j_solution = list(self.u_sol[:,0])
+
+    t = sec_since_boot()
+    if self.solution_status != 0:
+      if t > self.last_cloudlog_t + 5.0:
+        self.last_cloudlog_t = t
+        cloudlog.warning(f'Longitudinal model mpc reset, solution status: {self.solution_status}')
+      self.reset()
+
+
+if __name__ == "__main__":
+  ocp = gen_long_mpc_solver()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)

selfdrive/controls/lib/longitudinal_planner.py

@@ -11,8 +11,8 @@ from selfdrive.modeld.constants import T_IDXS
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.fcw import FCWChecker
 from selfdrive.controls.lib.longcontrol import LongCtrlState
-from selfdrive.controls.lib.lead_mpc import LeadMpc
+from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import LeadMpc
-from selfdrive.controls.lib.long_mpc import LongitudinalMpc
+from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
 from selfdrive.swaglog import cloudlog
 
@@ -64,6 +64,7 @@ class Planner():
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
+    self.j_desired_trajectory = np.zeros(CONTROL_N)
 
 
   def update(self, sm, CP):
@@ -97,10 +98,9 @@ class Planner():
       accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
       accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
     # clip limits, cannot init MPC outside of bounds
-    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired)
+    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
-    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired)
+    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
     self.mpcs['cruise'].set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
-
     next_a = np.inf
     for key in self.mpcs:
       self.mpcs[key].set_cur_state(self.v_desired, self.a_desired)
@@ -116,7 +116,7 @@ class Planner():
     if self.mpcs['lead0'].new_lead:
       self.fcw_checker.reset_lead(cur_time)
     blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
-    self.fcw = self.fcw_checker.update(self.mpcs['lead0'].mpc_solution, cur_time,
+    self.fcw = self.fcw_checker.update(self.mpcs['lead0'].x_sol[:,2], cur_time,
                                        sm['controlsState'].active,
                                        v_ego, sm['carState'].aEgo,
                                        self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,

selfdrive/controls/tests/test_following_distance.py

@@ -5,13 +5,7 @@ import numpy as np
 from cereal import log
 import cereal.messaging as messaging
 from selfdrive.config import Conversions as CV
-from selfdrive.controls.lib.lead_mpc import LeadMpc
+from selfdrive.controls.lib.lead_mpc_lib.lead_mpc import RW, LeadMpc
-
-
-def RW(v_ego, v_l):
-  TR = 1.8
-  G = 9.81
-  return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G))
 
 
 class FakePubMaster():
@@ -36,8 +30,8 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
 
     # Setup CarState
     CS = messaging.new_message('carState')
-    CS.carState.vEgo = v_ego
+    CS.carState.vEgo = float(v_ego)
-    CS.carState.aEgo = a_ego
+    CS.carState.aEgo = float(a_ego)
 
     # Setup model packet
     radarstate = messaging.new_message('radarState')
@@ -57,7 +51,7 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
     mpc.update(CS.carState, radarstate.radarState, 0)
 
     # Choose slowest of two solutions
-    v_ego, a_ego = mpc.mpc_solution.v_ego[5], mpc.mpc_solution.a_ego[5]
+    v_ego, a_ego = float(mpc.v_solution[5]), float(mpc.a_solution[5])
 
     # Update state
     x_lead += v_lead * dt
@@ -76,7 +70,7 @@ class TestFollowingDistance(unittest.TestCase):
       simulation_steady_state = run_following_distance_simulation(v_lead)
       correct_steady_state = RW(v_lead, v_lead) + 4.0
 
-      self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=.1)
+      self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=.2)
 
 
 if __name__ == "__main__":

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-c967d67902bd36e82c8b9e70c5538475b448ac49
+c66c5414f85cd98ad2dd7a83989e05990851da74

selfdrive/test/test_onroad.py

@@ -23,7 +23,7 @@ PROCS = {
   "selfdrive.controls.controlsd": 50.0,
   "./loggerd": 45.0,
   "./locationd": 9.1,
-  "selfdrive.controls.plannerd": 26.0,
+  "selfdrive.controls.plannerd": 33.0,
   "./_ui": 15.0,
   "selfdrive.locationd.paramsd": 9.1,
   "./camerad": 7.07,
@@ -55,7 +55,7 @@ if TICI:
     "selfdrive.controls.controlsd": 28.0,
     "./camerad": 31.0,
     "./_ui": 21.0,
-    "selfdrive.controls.plannerd": 12.0,
+    "selfdrive.controls.plannerd": 15.0,
     "selfdrive.locationd.paramsd": 5.0,
     "./_dmonitoringmodeld": 10.0,
     "selfdrive.thermald.thermald": 1.5,
@@ -198,8 +198,8 @@ class TestOnroad(unittest.TestCase):
       ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s]
       self.assertLess(min(ts), instant_max, f"high '{s}' execution time: {min(ts)}")
       self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}")
-      result += f"'{s}' execution time: {min(ts)}'"
+      result += f"'{s}' execution time: {min(ts)}\n"
-      result += f"'{s}' avg execution time: {np.mean(ts)}"
+      result += f"'{s}' avg execution time: {np.mean(ts)}\n"
     print(result)
 
   def test_timings(self):