Los Angeles Model (#31037)

* 1c888f5b-c213-4c1c-9eba-c587afd047fc/700

* Move to action

* Updates

* Add steer delay

* Update curvature grep

* clip speed

* No car params for now

* Add delay back

* Update

* fix lint

* fix lint

* update model regf

selfdrive/controls/lib/drive_helpers.py

@@ -3,8 +3,7 @@ import math
 from cereal import car, log
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import clip, interp
-from openpilot.common.realtime import DT_MDL, DT_CTRL
+from openpilot.common.realtime import DT_CTRL
-from openpilot.selfdrive.modeld.constants import ModelConstants
 
 # WARNING: this value was determined based on the model's training distribution,
 #          model predictions above this speed can be unpredictable
@@ -22,7 +21,6 @@ CAR_ROTATION_RADIUS = 0.0
 
 # EU guidelines
 MAX_LATERAL_JERK = 5.0
-
 MAX_VEL_ERR = 5.0
 
 ButtonEvent = car.CarState.ButtonEvent
@@ -169,28 +167,6 @@ def clip_curvature(v_ego, prev_curvature, new_curvature):
   safe_desired_curvature = clip(new_curvature,
                                 prev_curvature - max_curvature_rate * DT_CTRL,
                                 prev_curvature + max_curvature_rate * DT_CTRL)
-  return safe_desired_curvature
-
-
-def get_lag_adjusted_curvature(steer_delay, v_ego, psis, curvatures):
-  if len(psis) != CONTROL_N:
-    psis = [0.0]*CONTROL_N
-    curvatures = [0.0]*CONTROL_N
-  v_ego = max(MIN_SPEED, v_ego)
-
-  # MPC can plan to turn the wheel and turn back before t_delay. This means
-  # in high delay cases some corrections never even get commanded. So just use
-  # psi to calculate a simple linearization of desired curvature
-  current_curvature_desired = curvatures[0]
-  psi = interp(steer_delay, ModelConstants.T_IDXS[:CONTROL_N], psis)
-  average_curvature_desired = psi / (v_ego * steer_delay)
-  desired_curvature = 2 * average_curvature_desired - current_curvature_desired
-
-  # This is the "desired rate of the setpoint" not an actual desired rate
-  max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
-  safe_desired_curvature = clip(desired_curvature,
-                                current_curvature_desired - max_curvature_rate * DT_MDL,
-                                current_curvature_desired + max_curvature_rate * DT_MDL)
 
   return safe_desired_curvature
 

selfdrive/modeld/constants.py

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

selfdrive/modeld/fill_model_msg.py

@@ -3,7 +3,6 @@ import capnp
 import numpy as np
 from typing import Dict
 from cereal import log
-from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_lag_adjusted_curvature, MIN_SPEED
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
 
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -73,14 +72,8 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str,
   fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
 
   # lateral planning
-  x, y, yaw, yawRate = [net_output_data['lat_planner_solution'][0,:,i].tolist() for i in range(4)]
-  x_sol = np.column_stack([x, y, yaw, yawRate])
-  v_ego = max(MIN_SPEED, v_ego)
-  psis = x_sol[0:CONTROL_N, 2].tolist()
-  curvatures = (x_sol[0:CONTROL_N, 3]/v_ego).tolist()
-
   action = modelV2.action
-  action.desiredCurvature = get_lag_adjusted_curvature(steer_delay, v_ego, psis, curvatures)
+  action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
 
   # times at X_IDXS according to model plan
   PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N

selfdrive/modeld/modeld.py

@@ -12,8 +12,6 @@ from cereal.messaging import PubMaster, SubMaster
 from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
 from openpilot.common.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
@@ -59,7 +57,8 @@ 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),
+      'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32),
+      'prev_desired_curvs': np.zeros(ModelConstants.PREV_DESIRED_CURVS_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),
@@ -94,6 +93,7 @@ class ModelState:
     self.prev_desire[:] = inputs['desire']
 
     self.inputs['traffic_convention'][:] = inputs['traffic_convention']
+    self.inputs['lateral_control_params'][:] = inputs['lateral_control_params']
     self.inputs['nav_features'][:] = inputs['nav_features']
     self.inputs['nav_instructions'][:] = inputs['nav_instructions']
 
@@ -110,8 +110,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['prev_desired_curvs'][:-1] = self.inputs['prev_desired_curvs'][1:]
-    self.inputs['lat_planner_state'][3] = interp(DT_MDL, ModelConstants.T_IDXS, outputs['lat_planner_solution'][0, :, 3])
+    self.inputs['prev_desired_curvs'][-1] = outputs['desired_curvature'][0, 0]
     return outputs
 
 
@@ -152,8 +152,12 @@ def main(demo=False):
   pm = PubMaster(["modelV2", "cameraOdometry"])
   sm = SubMaster(["carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl"])
 
+
   publish_state = PublishState()
   params = Params()
+  with car.CarParams.from_bytes(params.get("CarParams", block=True)) as msg:
+    steer_delay = msg.steerActuatorDelay + .2
+  #steer_delay = 0.4
 
   # setup filter to track dropped frames
   frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ)
@@ -221,6 +225,8 @@ def main(demo=False):
     v_ego = sm["carState"].vEgo
     is_rhd = sm["driverMonitoringState"].isRHD
     frame_id = sm["roadCameraState"].frameId
+    # TODO add lag
+    lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32)
     if sm.updated["liveCalibration"]:
       device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
       model_transform_main = get_warp_matrix(device_from_calib_euler, main_wide_camera, False).astype(np.float32)
@@ -274,6 +280,7 @@ def main(demo=False):
     inputs:Dict[str, np.ndarray] = {
       'desire': vec_desire,
       'traffic_convention': traffic_convention,
+      'lateral_control_params': lateral_control_params,
       'nav_features': nav_features,
       'nav_instructions': nav_instructions}
 

selfdrive/modeld/models/navmodel.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:4971931accb5ba2e534bb3e0c591826ee507e2988df2eccf1fe862c303ddf9c5
+oid sha256:8254b569878b7472e3f63ed9f3527a87bde785c9037aee3ed66f972e072b5899
-size 14221074
+size 14166696

selfdrive/modeld/models/navmodel_q.dlc

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:fa346ada6f8c6326a5ee5fcd27e45e3e710049358079413c6a4624b20c6e1e47
+oid sha256:89fda8380efa3e421fbcdb6bb204c36a4991f137ee01d47f3d0380895aa7c036
 size 3630942

selfdrive/modeld/models/supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:ae44fe832fe48b89998f09cebb1bcd129864a8f51497b636cd38e66e46d69a89
+oid sha256:4b085c1ba231bc381f78462bda136172787371d5d83b6e1bcd340aad17290ebc
-size 48457850
+size 48197170

selfdrive/modeld/parse_model_outputs.py

@@ -93,7 +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))
+    self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_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 @@
-0513d29764980f512710cc2ebd7c14f91ae0351d
+cfdad3a695e3562ca32accce043b358291f0eef2