Expand lateral MPC to 10s (#27343)

* 10s lat

* Full length MPC

* redfine N

* Leave controls the same for now

* Updates

* use long plan in lat plan

* interp plan

* add new interp

* simplergit add selfdrive/controls/plannerd.py selfdrive/controls/

* expand to 10s

* revert this

* fix linter

* Update sconscripts

* fix test

* fix test

* fix test

* Revert "Update sconscripts"

This reverts commit 6e23c69dcebd5ed003e37e01921f6af7c31de0db.

* Dont import drive helpers

* better compile deps

* fix compile

* comment

* update replay

* Update plannerd time

selfdrive/controls/lib/drive_helpers.py

@@ -17,8 +17,6 @@ V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105
 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

@@ -47,6 +47,7 @@ acados_dir = '#third_party/acados'
 acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera'
 
 source_list = ['lat_mpc.py',
+  '#/selfdrive/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,8 +3,8 @@ import os
 import numpy as np
 
 from casadi import SX, vertcat, sin, cos
-
 from common.realtime import sec_since_boot
+# WARNING: imports outside of constants will not trigger a rebuild
 from selfdrive.modeld.constants import T_IDXS
 
 if __name__ == '__main__':  # generating code
@@ -17,12 +17,12 @@ EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
 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 +168,14 @@ class LateralMpc():
     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 @@ CAMERA_OFFSET = 0.04
 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 @@ class LateralPlanner:
     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 @@ class LateralPlanner:
 
   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 @@ class LateralPlanner:
       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 @@ class LateralPlanner:
     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

@@ -54,6 +54,7 @@ acados_dir = '#third_party/acados'
 acados_templates_dir = '#third_party/acados/acados_template/c_templates_tera'
 
 source_list = ['long_mpc.py',
+  '#/selfdrive/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/longitudinal_mpc_lib/long_mpc.py

@@ -5,6 +5,7 @@ import numpy as np
 from common.realtime import sec_since_boot
 from common.numpy_fast import clip
 from system.swaglog import cloudlog
+# WARNING: imports outside of constants will not trigger a rebuild
 from selfdrive.modeld.constants import index_function
 from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
 

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):

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-8883c476d5abc12b4b2949e04c6d7c0cd7c8b9fa
+086896986a3fcdb1a03d9afd00a9abc928f9ef25

selfdrive/test/test_onroad.py

@@ -26,7 +26,7 @@ PROCS = {
   "./encoderd": 17.0,
   "./camerad": 14.5,
   "./locationd": 9.1,
-  "selfdrive.controls.plannerd": 11.7,
+  "selfdrive.controls.plannerd": 16.5,
   "./_ui": 19.2,
   "selfdrive.locationd.paramsd": 9.0,
   "./_sensord": 12.0,