Cython acados and minor (#23835)

* acados_ocp_solver_pyx.pyx: implement get_stats for timings and ints

* long_mpc: use acados timers

* acados_ocp_solver_pyx.pyx: fix dynamics_get

* acados_ocp_solver_pyx.pyx: get statistics

* use acados_ocp_solver_pyx.pyx from commaai/cython2 branch

* acados_ocp_solver_pyx.pyx: implement store_iterate

* acados_ocp_solver_pyx.pyx: implement get_residuals

* acados_ocp_solver_pyx.pyx: fix set() for empty fields

* acados_ocp_solver_pyx.pyx: load_iterate

* cython acados: add print_statistics

* test_following_distance: fix typo

* test_longitudinal: unique names for test maneuvers

* longitudinal MPC: comments for evaluation

* longitudinal MPC: add comments to eval acados residuals

* long_mpc: use qp_solver_cond_N = 1

* long MPC: comments, simplify set_cur_state

* update acados version in build script

* longitudinal mpc: weigh a_change in 1 place only

* update ref

* Update ref

Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com>
old-commit-hash: d09dffb7cd

pyextra/acados_template/acados_ocp_solver_pyx.pyx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:9df0412c0f77fbf72ceb0ba8186e683f6b467521b7707156d7e2baa1f5d88430
-size 17649
+oid sha256:da810ba4a48d1be64fbad09946672779f5d5401de56aa38ea9da8fdbe719e9e4
+size 22704

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -117,7 +117,7 @@ def gen_lat_mpc_solver():
 
 class LateralMpc():
   def __init__(self, x0=np.zeros(X_DIM)):
-    self.solver = AcadosOcpSolverFast('lat', N, EXPORT_DIR)
+    self.solver = AcadosOcpSolverFast('lat', N)
     self.reset(x0)
 
   def reset(self, x0=np.zeros(X_DIM)):

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -34,7 +34,7 @@ X_EGO_COST = 0.
 V_EGO_COST = 0.
 A_EGO_COST = 0.
 J_EGO_COST = 5.0
-A_CHANGE_COST = .5
+A_CHANGE_COST = 200.
 DANGER_ZONE_COST = 100.
 CRASH_DISTANCE = .5
 LIMIT_COST = 1e6
@@ -136,7 +136,7 @@ def gen_long_mpc_solver():
            x_ego,
            v_ego,
            a_ego,
-           20*(a_ego - prev_a),
+           a_ego - prev_a,
            j_ego]
   ocp.model.cost_y_expr = vertcat(*costs)
   ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
@@ -176,7 +176,7 @@ def gen_long_mpc_solver():
   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_cond_N = N//4
+  ocp.solver_options.qp_solver_cond_N = 1
 
   # 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.
@@ -197,7 +197,7 @@ class LongitudinalMpc:
     self.source = SOURCES[2]
 
   def reset(self):
-    self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR)
+    self.solver = AcadosOcpSolverFast('long', N)
     self.v_solution = np.zeros(N+1)
     self.a_solution = np.zeros(N+1)
     self.prev_a = np.array(self.a_solution)
@@ -215,7 +215,11 @@ class LongitudinalMpc:
     self.status = False
     self.crash_cnt = 0.0
     self.solution_status = 0
+    # timers
     self.solve_time = 0.0
+    self.time_qp_solution = 0.0
+    self.time_linearization = 0.0
+    self.time_integrator = 0.0
     self.x0 = np.zeros(X_DIM)
     self.set_weights()
 
@@ -232,6 +236,7 @@ class LongitudinalMpc:
     a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
     W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, J_EGO_COST]))
     for i in range(N):
+      # reduce the cost on (a-a_prev) later in the horizon.
       W[4,4] = a_change_cost * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
       self.solver.cost_set(i, 'W', W)
     # Setting the slice without the copy make the array not contiguous,
@@ -257,14 +262,12 @@ class LongitudinalMpc:
       self.solver.cost_set(i, 'Zl', Zl)
 
   def set_cur_state(self, v, a):
-    if abs(self.x0[1] - v) > 2.:
+    v_prev = self.x0[1]
     self.x0[1] = v
     self.x0[2] = a
+    if abs(v_prev - v) > 2.: # probably only helps if v < v_prev
       for i in range(0, N+1):
         self.solver.set(i, 'x', self.x0)
-    else:
-      self.x0[1] = v
-      self.x0[2] = a
 
   @staticmethod
   def extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau):
@@ -355,9 +358,17 @@ class LongitudinalMpc:
     self.solver.constraints_set(0, "lbx", self.x0)
     self.solver.constraints_set(0, "ubx", self.x0)
 
-    t = sec_since_boot()
     self.solution_status = self.solver.solve()
-    self.solve_time = sec_since_boot() - t
+    self.solve_time = float(self.solver.get_stats('time_tot')[0])
+    self.time_qp_solution = float(self.solver.get_stats('time_qp')[0])
+    self.time_linearization = float(self.solver.get_stats('time_lin')[0])
+    self.time_integrator = float(self.solver.get_stats('time_sim')[0])
+
+    # qp_iter = self.solver.get_stats('statistics')[-1][-1] # SQP_RTI specific
+    # print(f"long_mpc timings: tot {self.solve_time:.2e}, qp {self.time_qp_solution:.2e}, lin {self.time_linearization:.2e}, integrator {self.time_integrator:.2e}, qp_iter {qp_iter}")
+    # res = self.solver.get_residuals()
+    # print(f"long_mpc residuals: {res[0]:.2e}, {res[1]:.2e}, {res[2]:.2e}, {res[3]:.2e}")
+    # self.solver.print_statistics()
 
     for i in range(N+1):
       self.x_sol[i] = self.solver.get(i, 'x')
@@ -370,6 +381,7 @@ class LongitudinalMpc:
 
     self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution)
 
+    t = sec_since_boot()
     if self.solution_status != 0:
       if t > self.last_cloudlog_t + 5.0:
         self.last_cloudlog_t = t

selfdrive/controls/tests/test_following_distance.py

@@ -21,7 +21,7 @@ def run_following_distance_simulation(v_lead, t_end=100.0):
 
 
 class TestFollowingDistance(unittest.TestCase):
-  def test_following_distanc(self):
+  def test_following_distance(self):
     for speed in np.arange(0, 40, 5):
       print(f'Testing {speed} m/s')
       v_lead = float(speed)

selfdrive/test/longitudinal_maneuvers/test_longitudinal.py

@@ -9,7 +9,7 @@ from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver
 # TODO: make new FCW tests
 maneuvers = [
   Maneuver(
-    'approach stopped car at 20m/s',
+    'approach stopped car at 20m/s, initial distance: 120m',
     duration=20.,
     initial_speed=25.,
     lead_relevancy=True,
@@ -18,7 +18,7 @@ maneuvers = [
     breakpoints=[0., 1.],
   ),
   Maneuver(
-    'approach stopped car at 20m/s',
+    'approach stopped car at 20m/s, initial distance 90m',
     duration=20.,
     initial_speed=20.,
     lead_relevancy=True,
@@ -65,7 +65,7 @@ maneuvers = [
     breakpoints=[2., 2.01, 8.8],
   ),
   Maneuver(
-    "approach stopped car at 20m/s",
+    "approach stopped car at 20m/s, with prob_lead_values",
     duration=30.,
     initial_speed=20.,
     lead_relevancy=True,

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-67c8f283858998b75ac28879e1350a589a968e5d
+7e6072a254791e4106a15ecbf94c16f40d54b459

third_party/acados/build.sh

@@ -18,7 +18,7 @@ if [ ! -d acados_repo/ ]; then
 fi
 cd acados_repo
 git fetch
-git checkout 79e9e3e76f2751198858adf382c97837833ad31f
+git checkout 92b85c61f7358a1b08b7cd30aeb9d32ad15942e8
 git submodule update --recursive --init
 
 # build