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
old-commit-hash: 04cf12cb0064f42e507fd6ae907564cecf140d28

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.controls.lib.drive_helpers import LON_MPC_N as N
+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
@@ -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.a_desired_trajectory = self.mpc.a_solution[:CONTROL_N]
-    self.j_desired_trajectory = self.mpc.j_solution[:CONTROL_N]
+    self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
+    self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)
+    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):
-  return (max_val/1024)*(idx**2)
+def index_function(idx, max_val=192, max_idx=32):
+  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

third_party/acados/x86_64/lib/libacados.so

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:13aa7b28a5d778b82c65e88ec3c45a216a29f0ab62b3c989ab4d2ec24abb9ce9
+oid sha256:ad96e016d31a7195786b9dc011e94ad78d01199da77ec4288a17bf4b69f33021
 size 525968

third_party/acados/x86_64/lib/libhpipm.so

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:c4bf98739484235afe35e159da5338301232257c4cdd514eff981bf26ae1c3fe
+oid sha256:8d15b89d07c01f24e668961797f9e163d3aab1fb17006164fa8d1c850908fc49
 size 1572648

third_party/acados/x86_64/lib/libqpOASES_e.so.3.1

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:40542540a6094b831d459b2f74fed9149251578d20eff243528526c0e4a63675
+oid sha256:f8b684fe7a70461124d492e99ffe8e7858c9d6708ffeefec121653ac3aaa945c
 size 262824

third_party/acados/x86_64/t_renderer

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:60b5961f1425745dbf81e79cc4e8a98e8b892938211bc133eac1c1f8e1c1b058
-size 7379920
+oid sha256:2545ad0a8dc9c92e60ac21d3209d62b94fdd3d91c5634ed81189844059b1d4df
+size 7377848