Less QP iterations for lateral acados (#22231)

* Less QP iterations for lateral acados

* update ref
old-commit-hash: 96a8f8a831a2e72e195d5b384723644e8499c531

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -98,7 +98,7 @@ def gen_lat_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_iter_max = 10
+  ocp.solver_options.qp_solver_iter_max = 1
 
   # set prediction horizon
   ocp.solver_options.tf = Tf
@@ -143,7 +143,8 @@ class LateralMpc():
     x0_cp = np.copy(x0)
     self.solver.constraints_set(0, "lbx", x0_cp)
     self.solver.constraints_set(0, "ubx", x0_cp)
-    self.yref = np.column_stack([y_pts, heading_pts*(v_ego+5.0), np.zeros(N+1)])
+    self.yref[:,0] = y_pts
+    self.yref[:,1] = heading_pts*(v_ego+5.0)
     self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
     self.solver.cost_set(N, "yref", self.yref[N][:2])
 

selfdrive/controls/tests/test_lateral_mpc.py

@@ -4,10 +4,11 @@ from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
 from selfdrive.controls.lib.drive_helpers import LAT_MPC_N, CAR_ROTATION_RADIUS
 
 
-def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
+def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
             lane_width=3.6, poly_shift=0.):
 
-  lat_mpc = LateralMpc()
+  if lat_mpc is None:
+    lat_mpc = LateralMpc()
   lat_mpc.set_weights(1., 1., 1.)
 
   y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
@@ -15,8 +16,8 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
 
   x0 = np.array([x_init, y_init, psi_init, curvature_init, v_ref, CAR_ROTATION_RADIUS])
 
-  # converge in no more than 20 iterations
-  for _ in range(20):
+  # converge in no more than 10 iterations
+  for _ in range(10):
     lat_mpc.run(x0, v_ref,
                 CAR_ROTATION_RADIUS,
                 y_pts, heading_pts)
@@ -72,6 +73,14 @@ class TestLateralMpc(unittest.TestCase):
     for y in list(sol[:,1]):
       self.assertGreaterEqual(y_init, abs(y))
 
+  def test_switch_convergence(self):
+    lat_mpc = LateralMpc()
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=30.0, v_ref=7.0)
+    right_psi_deg = np.degrees(sol[:,2])
+    sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-30.0, v_ref=7.0)
+    left_psi_deg = np.degrees(sol[:,2])
+    np.testing.assert_almost_equal(right_psi_deg, -left_psi_deg, decimal=3)
+
 
 if __name__ == "__main__":
   unittest.main()

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-b631f006877b2576163e66435d800fc0a565189f
+eb150cfb34f825d0db2dc30c1077357b0a66952e