ACADOS fix non-convergence when long_plan changes (#22495)

* debug commit

* cleanup

* some indexing bugs

* need more its

* BALANCE is way better it seems

* fix test

* this converges in 2000segs

* new ref

* less cpu

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -5,8 +5,7 @@ import numpy as np
 from common.realtime import sec_since_boot
 from common.numpy_fast import clip, interp
 from selfdrive.swaglog import cloudlog
-from selfdrive.modeld.constants import T_IDXS as T_IDXS_LST
+from selfdrive.modeld.constants import index_function
-from selfdrive.controls.lib.drive_helpers import LON_MPC_N as N
 from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
 
 from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
@@ -32,9 +31,15 @@ DANGER_ZONE_COST = 100.
 CRASH_DISTANCE = .5
 LIMIT_COST = 1e6
 
+
+# Less timestamps doesn't hurt performance and leads to
+# much better convergence of the MPC with low iterations
+N = 12
+MAX_T = 10.0
+T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N+1) for idx in range(N+1)]
+
 T_IDXS = np.array(T_IDXS_LST)
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
-
 MIN_ACCEL = -3.5
 T_REACT = 1.8
 MAX_BRAKE = 9.81
@@ -162,7 +167,7 @@ def gen_long_mpc_solver():
 
   # More iterations take too much time and less lead to inaccurate convergence in
   # some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
-  ocp.solver_options.qp_solver_iter_max = 4
+  ocp.solver_options.qp_solver_iter_max = 10
 
   # set prediction horizon
   ocp.solver_options.tf = Tf

selfdrive/controls/lib/longitudinal_planner.py

@@ -9,6 +9,7 @@ from selfdrive.modeld.constants import T_IDXS
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.longcontrol import LongCtrlState
 from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
+from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
 from selfdrive.swaglog import cloudlog
 
@@ -88,9 +89,9 @@ class Planner():
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired, self.a_desired)
     self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
-    self.v_desired_trajectory = self.mpc.v_solution[:CONTROL_N]
+    self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
-    self.a_desired_trajectory = self.mpc.a_solution[:CONTROL_N]
+    self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)
-    self.j_desired_trajectory = self.mpc.j_solution[:CONTROL_N]
+    self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
 
     #TODO counter is only needed because radar is glitchy, remove once radar is gone
     self.fcw = self.mpc.crash_cnt > 5

selfdrive/modeld/constants.py

@@ -1,7 +1,7 @@
 IDX_N = 33
 
-def index_function(idx, max_val=192):
+def index_function(idx, max_val=192, max_idx=32):
-  return (max_val/1024)*(idx**2)
+  return (max_val) * ((idx/max_idx)**2)
 
 
 T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -54,7 +54,7 @@ maneuvers = [
     breakpoints=[0., 15., 21.66],
   ),
   Maneuver(
-    'steady state following a car at 20m/s, then lead decel to 0mph at 3.5m/s^2',
+    'steady state following a car at 20m/s, then lead decel to 0mph at 3+m/s^2',
     duration=40.,
     initial_speed=20.,
     lead_relevancy=True,
@@ -62,7 +62,7 @@ maneuvers = [
     speed_lead_values=[20., 20., 0.],
     prob_lead_values=[0., 1., 1.],
     cruise_values=[20., 20., 20.],
-    breakpoints=[2., 2.01, 8.01],
+    breakpoints=[2., 2.01, 8.51],
   ),
   Maneuver(
     "approach stopped car at 20m/s",

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-22f78d143d7e17e9ac7d0be8e29d5d7b5477ecd3
+1704bf0294610dfe380fbdf601b03f1387ddec9d

selfdrive/test/test_onroad.py

@@ -23,7 +23,7 @@ PROCS = {
   "selfdrive.controls.controlsd": 50.0,
   "./loggerd": 45.0,
   "./locationd": 9.1,
-  "selfdrive.controls.plannerd": 27.0,
+  "selfdrive.controls.plannerd": 22.6,
   "./_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": 14.0,
+    "selfdrive.controls.plannerd": 11.7,
     "selfdrive.locationd.paramsd": 5.0,
     "./_dmonitoringmodeld": 10.0,
     "selfdrive.thermald.thermald": 1.5,

third_party/acados/build.sh

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