(New) Lemon Pie Model 🍋 (#30209)

* 6f6e3749-1b7c-42e8-a33b-03929b7fc476/700

* oops deleted too much

* 1b4308b7-a659-4ebd-b4c6-c81c1c3890f8/700

* 1be192f3-f407-4217-9757-78b9ad92750a/700

* remove some todos

* more cleanup in lat planner

* vego > min_speed

* regen and update process replay refs

* update model replay ref

* update model replay ref commit again

* Revert "update model replay ref commit again"

This reverts commit 922cb796b8.

* update again

* bump cereal

cereal

@@ -1 +1 @@
-Subproject commit 24a522f6ba47aba12a9baea53020a8323673c79c
+Subproject commit 416c3d531c90ce16498d782bf383625a857ee74c

selfdrive/controls/lib/lateral_planner.py

@@ -1,10 +1,4 @@
-import time
 import numpy as np
-from openpilot.common.realtime import DT_MDL
-from openpilot.common.numpy_fast import interp
-from openpilot.system.swaglog import cloudlog
-from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
-from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
 from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED, get_speed_error
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 import cereal.messaging as messaging
@@ -13,18 +7,6 @@ from cereal import log
 TRAJECTORY_SIZE = 33
 CAMERA_OFFSET = 0.04
 
-
-PATH_COST = 1.0
-LATERAL_MOTION_COST = 0.11
-LATERAL_ACCEL_COST = 0.0
-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 = 700.0
-
-
 class LateralPlanner:
   def __init__(self, CP, debug=False):
     self.DH = DesireHelper()
@@ -37,42 +19,26 @@ class LateralPlanner:
 
     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)
-    self.y_pts = np.zeros((TRAJECTORY_SIZE,))
     self.v_plan = np.zeros((TRAJECTORY_SIZE,))
-    self.v_ego = 0.0
+    self.x_sol = np.zeros((TRAJECTORY_SIZE, 4), dtype=np.float32)
+    self.v_ego = MIN_SPEED
     self.l_lane_change_prob = 0.0
     self.r_lane_change_prob = 0.0
 
     self.debug_mode = debug
 
-    self.lat_mpc = LateralMpc()
-    self.reset_mpc(np.zeros(4))
-
-  def reset_mpc(self, x0=None):
-    if x0 is None:
-      x0 = np.zeros(4)
-    self.x0 = x0
-    self.lat_mpc.reset(x0=self.x0)
-
   def update(self, sm):
-    # clip speed , lateral planning is not possible at 0 speed
-    measured_curvature = sm['controlsState'].curvature
     v_ego_car = sm['carState'].vEgo
 
     # Parse model predictions
     md = sm['modelV2']
-    if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
+    if len(md.position.x) == TRAJECTORY_SIZE and len(md.velocity.x) == TRAJECTORY_SIZE and len(md.lateralPlannerSolution.x) == TRAJECTORY_SIZE:
       self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
-      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) - get_speed_error(md, v_ego_car)
       self.v_plan = np.clip(car_speed, MIN_SPEED, np.inf)
       self.v_ego = self.v_plan[0]
+      self.x_sol = np.column_stack([md.lateralPlannerSolution.x, md.lateralPlannerSolution.y, md.lateralPlannerSolution.yaw, md.lateralPlannerSolution.yawRate])
 
     # Lane change logic
     desire_state = md.meta.desireState
@@ -82,66 +48,23 @@ 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)
 
-    self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
-                             LATERAL_ACCEL_COST, LATERAL_JERK_COST,
-                             STEERING_RATE_COST)
-
-    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 = 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,
-                     heading_pts,
-                     yaw_rate_pts)
-    # init state for next iteration
-    # mpc.u_sol is the desired second derivative of psi given x0 curv state.
-    # with x0[3] = measured_yaw_rate, this would be the actual desired yaw rate.
-    # instead, interpolate x_sol so that x0[3] is the desired yaw rate for lat_control.
-    self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
-
-    #  Check for infeasible MPC solution
-    mpc_nans = np.isnan(self.lat_mpc.x_sol[:, 3]).any()
-    t = time.monotonic()
-    if mpc_nans or self.lat_mpc.solution_status != 0:
-      self.reset_mpc()
-      self.x0[3] = measured_curvature * self.v_ego
-      if t > self.last_cloudlog_t + 5.0:
-        self.last_cloudlog_t = t
-        cloudlog.warning("Lateral mpc - nan: True")
-
-    if self.lat_mpc.cost > 1e6 or mpc_nans:
-      self.solution_invalid_cnt += 1
-    else:
-      self.solution_invalid_cnt = 0
-
   def publish(self, sm, pm):
-    plan_solution_valid = self.solution_invalid_cnt < 2
     plan_send = messaging.new_message('lateralPlan')
     plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
 
     lateralPlan = plan_send.lateralPlan
     lateralPlan.modelMonoTime = sm.logMonoTime['modelV2']
-    lateralPlan.dPathPoints = self.y_pts.tolist()
+    lateralPlan.dPathPoints = self.path_xyz[:,1].tolist()
-    lateralPlan.psis = self.lat_mpc.x_sol[0:CONTROL_N, 2].tolist()
+    lateralPlan.psis = self.x_sol[0:CONTROL_N, 2].tolist()
 
-    lateralPlan.curvatures = (self.lat_mpc.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist()
+    lateralPlan.curvatures = (self.x_sol[0:CONTROL_N, 3]/self.v_ego).tolist()
-    lateralPlan.curvatureRates = [float(x.item() / self.v_ego) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
+    lateralPlan.curvatureRates = [float(0) for _ in range(CONTROL_N-1)] # TODO: unused
 
-    lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
+    lateralPlan.mpcSolutionValid = bool(1)
-    lateralPlan.solverExecutionTime = self.lat_mpc.solve_time
+    lateralPlan.solverExecutionTime = 0.0
     if self.debug_mode:
-      lateralPlan.solverCost = self.lat_mpc.cost
       lateralPlan.solverState = log.LateralPlan.SolverState.new_message()
-      lateralPlan.solverState.x = self.lat_mpc.x_sol.tolist()
+      lateralPlan.solverState.x = self.x_sol.tolist()
-      lateralPlan.solverState.u = self.lat_mpc.u_sol.flatten().tolist()
 
     lateralPlan.desire = self.DH.desire
     lateralPlan.useLaneLines = False

selfdrive/controls/plannerd.py

@@ -21,8 +21,8 @@ def publish_ui_plan(sm, pm, lateral_planner, longitudinal_planner):
   ui_send.valid = sm.all_checks(service_list=['carState', 'controlsState', 'modelV2'])
   uiPlan = ui_send.uiPlan
   uiPlan.frameId = sm['modelV2'].frameId
-  uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,0]).tolist()
+  uiPlan.position.x = np.interp(plan_odo, model_odo, lateral_planner.x_sol[:,0]).tolist()
-  uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.lat_mpc.x_sol[:,1]).tolist()
+  uiPlan.position.y = np.interp(plan_odo, model_odo, lateral_planner.x_sol[:,1]).tolist()
   uiPlan.position.z = np.interp(plan_odo, model_odo, lateral_planner.path_xyz[:,2]).tolist()
   uiPlan.accel = longitudinal_planner.a_desired_trajectory_full.tolist()
   pm.send('uiPlan', ui_send)

selfdrive/modeld/constants.py

@@ -21,6 +21,7 @@ class ModelConstants:
   NAV_FEATURE_LEN = 256
   NAV_INSTRUCTION_LEN = 150
   DRIVING_STYLE_LEN = 12
+  LAT_PLANNER_STATE_LEN = 4
 
   # model outputs constants
   FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
@@ -37,6 +38,7 @@ class ModelConstants:
   ROAD_EDGES_WIDTH = 2
   PLAN_WIDTH = 15
   DESIRE_PRED_WIDTH = 8
+  LAT_PLANNER_SOLUTION_WIDTH = 4
 
   NUM_LANE_LINES = 4
   NUM_ROAD_EDGES = 2

selfdrive/modeld/fill_model_msg.py

@@ -71,6 +71,11 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str,
   orientation_rate = modelV2.orientationRate
   fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
 
+  # lateral planning
+  solution = modelV2.lateralPlannerSolution
+  solution.x, solution.y, solution.yaw, solution.yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)]
+  solution.xStd, solution.yStd, solution.yawStd, solution.yawRateStd = [net_output_data['lat_planner_solution_stds'][0,:,i].tolist() for i in range(4)]
+
   # times at X_IDXS according to model plan
   PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
   PLAN_T_IDXS[0] = 0.0

selfdrive/modeld/modeld.py

@@ -11,6 +11,8 @@ from cereal.messaging import PubMaster, SubMaster
 from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
 from openpilot.system.swaglog import cloudlog
 from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.numpy_fast import interp
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import config_realtime_process
 from openpilot.common.transformations.model import get_warp_matrix
@@ -54,6 +56,7 @@ class ModelState:
     self.inputs = {
       'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
       'traffic_convention': np.zeros(ModelConstants.TRAFFIC_CONVENTION_LEN, dtype=np.float32),
+      'lat_planner_state': np.zeros(ModelConstants.LAT_PLANNER_STATE_LEN, dtype=np.float32),
       'nav_features': np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32),
       'nav_instructions': np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32),
       'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
@@ -105,6 +108,8 @@ class ModelState:
 
     self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:]
     self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :]
+    self.inputs['lat_planner_state'][2] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 2])
+    self.inputs['lat_planner_state'][3] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 3])
     return outputs
 
 

selfdrive/modeld/models/supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:18e53fa5d0f9a185833d50fa0d777725ca715460062f3a2c050847588525d87c
+oid sha256:6330383805f2e64e750f61c47660d2ec8522f55d84217c0520584d2e0d7c3faf
-size 52272467
+size 52939093

selfdrive/modeld/parse_model_outputs.py

@@ -93,6 +93,7 @@ class Parser:
     self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
     self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
                    out_shape=(ModelConstants.LEAD_TRAJ_LEN,ModelConstants.LEAD_WIDTH))
+    self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N,ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
     for k in ['lead_prob', 'lane_lines_prob', 'meta']:
       self.parse_binary_crossentropy(k, outs)
     self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-0e0f55cf3bb2cf79b44adf190e6387a83deb6646
+eec02217169e6f5d71b8a376368ea96e83b10e4a

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-6e27d5c97fe6554a86e9ee8bb9259e0cc6df5bb1
+dbea36698ba48429b201b138846165eb4c329b92

selfdrive/test/process_replay/test_processes.py

@@ -41,23 +41,23 @@ source_segments = [
 ]
 
 segments = [
-  ("BODY", "regen7FE9F3C7CE3|2023-10-25--23-56-32--0"),
+  ("BODY", "regen997DF2697CB|2023-10-30--23-14-29--0"),
-  ("HYUNDAI", "regen7519EF9EE71|2023-10-25--23-53-59--0"),
+  ("HYUNDAI", "regen2A9D2A8E0B4|2023-10-30--23-13-34--0"),
-  ("HYUNDAI2", "regenF68B9F1B286|2023-10-25--23-56-31--0"),
+  ("HYUNDAI2", "regen6CA24BC3035|2023-10-30--23-14-28--0"),
-  ("TOYOTA", "regen56DC072FA51|2023-10-25--23-53-51--0"),
+  ("TOYOTA", "regen5C019D76307|2023-10-30--23-13-31--0"),
-  ("TOYOTA2", "regen78130056536|2023-10-25--23-53-58--0"),
+  ("TOYOTA2", "regen5DCADA88A96|2023-10-30--23-14-57--0"),
-  ("TOYOTA3", "regenC554B250909|2023-10-25--23-58-53--0"),
+  ("TOYOTA3", "regen7204CA3A498|2023-10-30--23-15-55--0"),
-  ("HONDA", "regen3ED625586FB|2023-10-25--23-56-29--0"),
+  ("HONDA", "regen048F8FA0B24|2023-10-30--23-15-53--0"),
-  ("HONDA2", "regen9F1A8F44FD5|2023-10-25--23-56-34--0"),
+  ("HONDA2", "regen7D2D3F82D5B|2023-10-30--23-15-55--0"),
-  ("CHRYSLER", "regen60CE93181EA|2023-10-25--23-59-01--0"),
+  ("CHRYSLER", "regen7125C42780C|2023-10-30--23-16-21--0"),
-  ("RAM", "regen9E2B62E8E9A|2023-10-26--00-00-41--0"),
+  ("RAM", "regen2731F3213D2|2023-10-30--23-18-11--0"),
-  ("SUBARU", "regenEEBF379E0ED|2023-10-26--00-01-37--0"),
+  ("SUBARU", "regen86E4C1B4DDD|2023-10-30--23-18-14--0"),
-  ("GM", "regen0B0EE5D6E0D|2023-10-25--23-58-57--0"),
+  ("GM", "regenF6393D64745|2023-10-30--23-17-18--0"),
-  ("GM2", "regen043B44E4FBD|2023-10-26--00-03-51--0"),
+  ("GM2", "regen220F830C05B|2023-10-30--23-18-39--0"),
-  ("NISSAN", "regen14F35E327BC|2023-10-26--00-01-22--0"),
+  ("NISSAN", "regen4F671F7C435|2023-10-30--23-18-40--0"),
-  ("VOLKSWAGEN", "regen63A052AE7D7|2023-10-26--00-01-36--0"),
+  ("VOLKSWAGEN", "regen8BDFE7307A0|2023-10-30--23-19-36--0"),
-  ("MAZDA", "regenF9047685121|2023-10-26--00-05-02--0"),
+  ("MAZDA", "regen2E9F1A15FD5|2023-10-30--23-20-36--0"),
-  ("FORD", "regen5115F2AE4FE|2023-10-26--00-06-17--0"),
+  ("FORD", "regen6D39E54606E|2023-10-30--23-20-54--0"),
   ]
 
 # dashcamOnly makes don't need to be tested until a full port is done