Expand lateral MPC to 10s

selfdrive/controls/lib/drive_helpers.py

@@ -17,8 +17,6 @@
 IMPERIAL_INCREMENT = 1.6  # should be CV.MPH_TO_KPH, but this causes rounding errors
 
 MIN_SPEED = 1.0
-LAT_MPC_N = 16
-LON_MPC_N = 32
 CONTROL_N = 17
 CAR_ROTATION_RADIUS = 0.0
 

selfdrive/controls/lib/lateral_mpc_lib/SConscript

@@ -46,7 +46,7 @@ generated_files = [
 acados_dir = '#third_party/acados'
 acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera'
 
-source_list = ['lat_mpc.py',
+source_list = ['lat_mpc.py', '../../../modeld/constants.py',
   f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
   f'{acados_dir}/x86_64/lib/libacados.so',
   f'{acados_dir}/larch64/lib/libacados.so',

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -3,7 +3,6 @@
 import numpy as np
 
 from casadi import SX, vertcat, sin, cos
-
 from common.realtime import sec_since_boot
 from selfdrive.modeld.constants import T_IDXS
 
@@ -17,12 +16,12 @@
 JSON_FILE = os.path.join(LAT_MPC_DIR, "acados_ocp_lat.json")
 X_DIM = 4
 P_DIM = 2
-N = 16
 COST_E_DIM = 3
 COST_DIM = COST_E_DIM + 2
 SPEED_OFFSET = 10.0
 MODEL_NAME = 'lat'
 ACADOS_SOLVER_TYPE = 'SQP_RTI'
+N = 32
 
 def gen_lat_model():
   model = AcadosModel()
@@ -168,14 +167,14 @@ def run(self, x0, p, y_pts, heading_pts, yaw_rate_pts):
     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]
+    v_ego = p_cp[0, 0]
     # rotation_radius = p_cp[1]
     self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET)
     self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET)
     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.set(i, "p", p_cp[i])
+    self.solver.set(N, "p", p_cp[N])
     self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
 
     t = sec_since_boot()

selfdrive/controls/lib/lateral_planner.py

@@ -16,12 +16,12 @@
 PATH_COST = 1.0
 LATERAL_MOTION_COST = 0.11
 LATERAL_ACCEL_COST = 0.0
-LATERAL_JERK_COST = 0.05
+LATERAL_JERK_COST = 0.04
 # Extreme steering rate is unpleasant, even
 # when it does not cause bad jerk.
 # TODO this cost should be lowered when low
 # speed lateral control is stable on all cars
-STEERING_RATE_COST = 800.0
+STEERING_RATE_COST = 700.0
 
 
 class LateralPlanner:
@@ -35,6 +35,7 @@ def __init__(self, CP):
     self.solution_invalid_cnt = 0
 
     self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
+    self.velocity_xyz = np.zeros((TRAJECTORY_SIZE, 3))
     self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
     self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
     self.t_idxs = np.arange(TRAJECTORY_SIZE)
@@ -49,7 +50,6 @@ def reset_mpc(self, x0=np.zeros(4)):
 
   def update(self, sm):
     # clip speed , lateral planning is not possible at 0 speed
-    self.v_ego = max(MIN_SPEED, sm['carState'].vEgo)
     measured_curvature = sm['controlsState'].curvature
 
     # Parse model predictions
@@ -59,6 +59,10 @@ def update(self, sm):
       self.t_idxs = np.array(md.position.t)
       self.plan_yaw = np.array(md.orientation.z)
       self.plan_yaw_rate = np.array(md.orientationRate.z)
+      self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z])
+      car_speed = np.linalg.norm(self.velocity_xyz, axis=1)
+      self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf)
+      self.v_ego = self.v_plan[0]
 
     # Lane change logic
     desire_state = md.meta.desireState
@@ -68,21 +72,20 @@ def update(self, sm):
     lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob
     self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
 
-    d_path_xyz = self.path_xyz
     self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
                              LATERAL_ACCEL_COST, LATERAL_JERK_COST,
                              STEERING_RATE_COST)
 
-    y_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
-    heading_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
-    yaw_rate_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate)
+    y_pts = self.path_xyz[:LAT_MPC_N+1, 1]
+    heading_pts = self.plan_yaw[:LAT_MPC_N+1]
+    yaw_rate_pts = self.plan_yaw_rate[:LAT_MPC_N+1]
     self.y_pts = y_pts
 
     assert len(y_pts) == LAT_MPC_N + 1
     assert len(heading_pts) == LAT_MPC_N + 1
     assert len(yaw_rate_pts) == LAT_MPC_N + 1
-    lateral_factor = max(0, self.factor1 - (self.factor2 * self.v_ego**2))
-    p = np.array([self.v_ego, lateral_factor])
+    lateral_factor = np.clip(self.factor1 - (self.factor2 * self.v_plan**2), 0.0, np.inf)
+    p = np.column_stack([self.v_plan, lateral_factor])
     self.lat_mpc.run(self.x0,
                      p,
                      y_pts,

selfdrive/controls/lib/longitudinal_mpc_lib/SConscript

@@ -53,7 +53,7 @@ generated_files = [
 acados_dir = '#third_party/acados'
 acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera'
 
-source_list = ['long_mpc.py',
+source_list = ['long_mpc.py', '../../../modeld/constants.py',
   f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
   f'{acados_dir}/x86_64/lib/libacados.so',
   f'{acados_dir}/larch64/lib/libacados.so',

selfdrive/controls/tests/test_lateral_mpc.py

@@ -17,7 +17,8 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur
   curv_rate_pts = np.zeros(LAT_MPC_N + 1)
 
   x0 = np.array([x_init, y_init, psi_init, curvature_init])
-  p = np.array([v_ref, CAR_ROTATION_RADIUS])
+  p = np.column_stack([v_ref * np.ones(LAT_MPC_N + 1),
+                      CAR_ROTATION_RADIUS * np.ones(LAT_MPC_N + 1)])
 
   # converge in no more than 10 iterations
   for _ in range(10):