Tomb Raider Model (#33629)

* 69acff08-5383-4103-beea-822f0d228c76/160

* 53f39907-4763-4d19-ba26-e757527c2b61/200

* 21afb89f-1397-4652-b423-abc2da32417a/200

* 21afb89f-1397-4652-b423-abc2da32417a/400

* 53f39907-4763-4d19-ba26-e757527c2b61/400

* 3fb967fd-16a8-4569-ba57-359e54deeab3/395

* Try other policy again

* 3fb967fd-16a8-4569-ba57-359e54deeab3/400

* 0136cabf-539a-4a43-bc7f-06c3654a493c/4400

* 473c0686-1ac9-4c05-9b0b-d1f1afdb6cc3/400

* 83b7993d-51a9-4e3f-904e-3fcd5763c231/400

* 021566c7-cff3-431d-8da2-17a96c888c5f/400

* Fix bugs

* Update longitudinal_planner.py

* Update fill_model_msg.py

* 8be6b59d-4449-42fd-b8e0-93c48387070f/400

* f670a748-3591-4489-a0b3-215118ddad01/400

* 790a2950-c713-4eec-838b-4f55f4fe0ccb/400

* Update model ref

* Plans start at ego

* Update ref

selfdrive/controls/lib/drive_helpers.py

@@ -22,7 +22,7 @@ def clip_curvature(v_ego, prev_curvature, new_curvature):
 
 def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
   # ToDo: Try relative error, and absolute speed
-  if len(modelV2.temporalPose.trans):
-    vel_err = clip(modelV2.temporalPose.trans[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
+  if len(modelV2.velocity.x):
+    vel_err = clip(modelV2.velocity.x[0] - v_ego, -MAX_VEL_ERR, MAX_VEL_ERR)
     return float(vel_err)
   return 0.0

selfdrive/controls/radard.py

@@ -243,8 +243,8 @@ class RadarD:
     self.radar_state.radarErrors = list(rr.errors)
     self.radar_state.carStateMonoTime = sm.logMonoTime['carState']
 
-    if len(sm['modelV2'].temporalPose.trans):
-      model_v_ego = sm['modelV2'].temporalPose.trans[0]
+    if len(sm['modelV2'].velocity.x):
+      model_v_ego = sm['modelV2'].velocity.x[0]
     else:
       model_v_ego = self.v_ego
     leads_v3 = sm['modelV2'].leadsV3

selfdrive/modeld/fill_model_msg.py

@@ -168,13 +168,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
     (publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all()
   meta.hardBrakePredicted = hard_brake_predicted.item()
 
-  # temporal pose
-  temporal_pose = modelV2.temporalPose
-  temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
-  temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
-  temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
-  temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
-
   # confidence
   if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
     # any disengage prob

selfdrive/modeld/models/supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:39786068cae1ed8c0dc34ef80c281dfcc67ed18a50e06b90765c49bcfdbf7db4
-size 51453312
+oid sha256:d42319e42e8ba1a818c845ee0c687ece6afd98e59fef142895be33dd230974c2
+size 62481231

selfdrive/modeld/parse_model_outputs.py

@@ -87,7 +87,6 @@ class Parser:
     self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
     self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
     self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
-    self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
     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))

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -107,6 +107,7 @@ class Plant:
     model.modelV2.position = position
     velocity = log.XYZTData.new_message()
     velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
+    velocity.x[0] = float(self.speed) # always start at current speed
     model.modelV2.velocity = velocity
     acceleration = log.XYZTData.new_message()
     acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-32fe8cf4a0daa8d10a689c9ae2e51a879151c87c
+f0504b66f3f736b5bea39cc01e1a073e3be95659

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-96c2e2ab2e3a11476f1207c531893cc8e45d2b3c
+d8a02c23f530dd236553b47ff7f0355929fe15fc