work on acados lateral MPC (#23558)

* lat_mpc: make v_ego, rotation_radius parameters instead of states

* lat_mpc: remove rotation_radius argument, since it is part of the parameters

* lat_mpc: use qp_solver_cond_N = 1

slightly faster and in line with case study in Fig. 2/ 3 in Frison2016 - https://cdn.syscop.de/publications/Frison2016.pdf
An Efficient Implementation of Partial Condensing for Nonlinear Model Predictive Control

* adapt test_lateral_mpc to formulation with parameters

* lat_mpc: set parameters in reset() and copy values

* acados_ocp_solver_pyx: make options_set useable

* update ref

Co-authored-by: Willem Melching <willem.melching@gmail.com>

pyextra/acados_template/acados_ocp_solver_pyx.pyx

@@ -417,8 +417,9 @@ cdef class AcadosOcpSolverFast:
             string_value = value_.encode('utf-8')
             acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &string_value[0])
 
-        raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
-            '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
+        else:
+            raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
+                '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
 
 
     def __del__(self):

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -17,7 +17,8 @@ else:
 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
+X_DIM = 4
+P_DIM = 2
 
 def gen_lat_model():
   model = AcadosModel()
@@ -28,9 +29,12 @@ def gen_lat_model():
   y_ego = SX.sym('y_ego')
   psi_ego = SX.sym('psi_ego')
   curv_ego = SX.sym('curv_ego')
+  model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego)
+
+  # parameters
   v_ego = SX.sym('v_ego')
   rotation_radius = SX.sym('rotation_radius')
-  model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego, v_ego, rotation_radius)
+  model.p = vertcat(v_ego, rotation_radius)
 
   # controls
   curv_rate = SX.sym('curv_rate')
@@ -41,18 +45,14 @@ def gen_lat_model():
   y_ego_dot = SX.sym('y_ego_dot')
   psi_ego_dot = SX.sym('psi_ego_dot')
   curv_ego_dot = SX.sym('curv_ego_dot')
-  v_ego_dot = SX.sym('v_ego_dot')
-  rotation_radius_dot = SX.sym('rotation_radius_dot')
-  model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot,
-                       v_ego_dot, rotation_radius_dot)
+
+  model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot)
 
   # dynamics model
   f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego),
                    v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego),
                    v_ego * curv_ego,
-                   curv_rate,
-                   0.0,
-                   0.0)
+                   curv_rate)
   model.f_impl_expr = model.xdot - f_expl
   model.f_expl_expr = f_expl
   return model
@@ -79,8 +79,9 @@ def gen_lat_mpc_solver():
 
   y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2]
   curv_rate = ocp.model.u[0]
-  v_ego = ocp.model.x[4]
+  v_ego = ocp.model.p[0]
 
+  ocp.parameter_values = np.zeros((P_DIM, ))
 
   ocp.cost.yref = np.zeros((3, ))
   ocp.cost.yref_e = np.zeros((2, ))
@@ -96,7 +97,7 @@ def gen_lat_mpc_solver():
   ocp.constraints.idxbx = np.array([2,3])
   ocp.constraints.ubx = np.array([np.radians(90), np.radians(50)])
   ocp.constraints.lbx = np.array([-np.radians(90), -np.radians(50)])
-  x0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+  x0 = np.zeros((X_DIM,))
   ocp.constraints.x0 = x0
 
   ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
@@ -104,7 +105,7 @@ def gen_lat_mpc_solver():
   ocp.solver_options.integrator_type = 'ERK'
   ocp.solver_options.nlp_solver_type = 'SQP_RTI'
   ocp.solver_options.qp_solver_iter_max = 1
-  ocp.solver_options.qp_solver_cond_N = N//4
+  ocp.solver_options.qp_solver_cond_N = 1
 
   # set prediction horizon
   ocp.solver_options.tf = Tf
@@ -130,6 +131,7 @@ class LateralMpc():
     # Somehow needed for stable init
     for i in range(N+1):
       self.solver.set(i, 'x', np.zeros(X_DIM))
+      self.solver.set(i, 'p', np.zeros(P_DIM))
     self.solver.constraints_set(0, "lbx", x0)
     self.solver.constraints_set(0, "ubx", x0)
     self.solver.solve()
@@ -144,14 +146,19 @@ class LateralMpc():
     #TODO hacky weights to keep behavior the same
     self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2])
 
-  def run(self, x0, v_ego, car_rotation_radius, y_pts, heading_pts):
+  def run(self, x0, p, y_pts, heading_pts):
     x0_cp = np.copy(x0)
+    p_cp = np.copy(p)
     self.solver.constraints_set(0, "lbx", x0_cp)
     self.solver.constraints_set(0, "ubx", x0_cp)
     self.yref[:,0] = y_pts
+    v_ego = p_cp[0]
+    # rotation_radius = p_cp[1]
     self.yref[:,1] = heading_pts*(v_ego+5.0)
     for i in range(N):
       self.solver.cost_set(i, "yref", self.yref[i])
+      self.solver.set(i, "p", p_cp)
+    self.solver.set(N, "p", p_cp)
     self.solver.cost_set(N, "yref", self.yref[N][:2])
 
     t = sec_since_boot()

selfdrive/controls/lib/lateral_planner.py

@@ -61,9 +61,9 @@ class LateralPlanner:
     self.y_pts = np.zeros(TRAJECTORY_SIZE)
 
     self.lat_mpc = LateralMpc()
-    self.reset_mpc(np.zeros(6))
+    self.reset_mpc(np.zeros(4))
 
-  def reset_mpc(self, x0=np.zeros(6)):
+  def reset_mpc(self, x0=np.zeros(4)):
     self.x0 = x0
     self.lat_mpc.reset(x0=self.x0)
 
@@ -175,10 +175,10 @@ class LateralPlanner:
 
     assert len(y_pts) == LAT_MPC_N + 1
     assert len(heading_pts) == LAT_MPC_N + 1
-    self.x0[4] = v_ego
+    # self.x0[4] = v_ego
+    p = np.array([v_ego, CAR_ROTATION_RADIUS])
     self.lat_mpc.run(self.x0,
-                     v_ego,
-                     CAR_ROTATION_RADIUS,
+                     p,
                      y_pts,
                      heading_pts)
     # init state for next

selfdrive/controls/tests/test_lateral_mpc.py

@@ -14,12 +14,12 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur
   y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
   heading_pts = np.zeros(LAT_MPC_N + 1)
 
-  x0 = np.array([x_init, y_init, psi_init, curvature_init, v_ref, CAR_ROTATION_RADIUS])
+  x0 = np.array([x_init, y_init, psi_init, curvature_init])
+  p = np.array([v_ref, CAR_ROTATION_RADIUS])
 
   # converge in no more than 10 iterations
   for _ in range(10):
-    lat_mpc.run(x0, v_ref,
-                CAR_ROTATION_RADIUS,
+    lat_mpc.run(x0, p,
                 y_pts, heading_pts)
   return lat_mpc.x_sol
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-280a712ece99c231ea036c3b66d6aafa55548211
+ef8a69dd1e52e2441d5c6155836a676ff98950a6