E2e long model: calibrate model speed to wheel speed (#26395)

* calibrate!

* Fix test

* Fix proc replay

* check len

* get v_ego from model 8501d20-bb59-4193-aa82-82b2737dedd6/449 609d90f3-65e6-4617-a60c-d6d99eead408/700

* bump cereal

* initialize v_model_error

* typo

* better names

* cleanup

* bump cereal

* update model replay ref commit

* bump to cereal master

Co-authored-by: Yassine Yousfi <yyousfi1@binghamton.edu>

cereal

@@ -1 +1 @@
-Subproject commit 1d25fc3f202d5ddeee97848480323e9b14f9bdfa
+Subproject commit cdba1aafec5e36505ef6ace675568e1f15003c47

selfdrive/controls/lib/longitudinal_planner.py

@@ -58,6 +58,7 @@ class LongitudinalPlanner:
 
     self.a_desired = init_a
     self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
+    self.v_model_error = 0.0
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
@@ -68,12 +69,12 @@ class LongitudinalPlanner:
     e2e = self.params.get_bool('EndToEndLong') and self.CP.openpilotLongitudinalControl
     self.mpc.mode = 'blended' if e2e else 'acc'
 
-  def parse_model(self, model_msg):
+  def parse_model(self, model_msg, model_error):
     if (len(model_msg.position.x) == 33 and
        len(model_msg.velocity.x) == 33 and
        len(model_msg.acceleration.x) == 33):
-      x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x)
-      v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x)
+      x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC
+      v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) - model_error
       a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x)
       j = np.zeros(len(T_IDXS_MPC))
     else:
@@ -112,6 +113,9 @@ class LongitudinalPlanner:
 
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
+    # Compute model v_ego error
+    if len(sm['modelV2'].temporalPose.trans):
+      self.v_model_error = sm['modelV2'].temporalPose.trans[0] - v_ego
 
     if force_slow_decel:
       # if required so, force a smooth deceleration
@@ -124,7 +128,7 @@ class LongitudinalPlanner:
     self.mpc.set_weights(prev_accel_constraint)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    x, v, a, j = self.parse_model(sm['modelV2'])
+    x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
     self.mpc.update(sm['carState'], sm['radarState'], v_cruise, x, v, a, j)
 
     self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)

selfdrive/modeld/models/driving.cc

@@ -337,6 +337,17 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_out
   for (int i=0; i<LEAD_MHP_SELECTION; i++) {
     fill_lead(leads[i], net_outputs.leads, i, t_offsets[i]);
   }
+
+  // temporal pose
+  const auto &v_mean = net_outputs.temporal_pose.velocity_mean;
+  const auto &r_mean = net_outputs.temporal_pose.rotation_mean;
+  const auto &v_std = net_outputs.temporal_pose.velocity_std;
+  const auto &r_std = net_outputs.temporal_pose.rotation_std;
+  auto temporal_pose = framed.initTemporalPose();
+  temporal_pose.setTrans({v_mean.x, v_mean.y, v_mean.z});
+  temporal_pose.setRot({r_mean.x, r_mean.y, r_mean.z});
+  temporal_pose.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)});
+  temporal_pose.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
 }
 
 void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,

selfdrive/modeld/models/driving.h

@@ -167,6 +167,14 @@ struct ModelOutputWideFromDeviceEuler {
 };
 static_assert(sizeof(ModelOutputWideFromDeviceEuler) == sizeof(ModelOutputXYZ)*2);
 
+struct ModelOutputTemporalPose {
+  ModelOutputXYZ velocity_mean;
+  ModelOutputXYZ rotation_mean;
+  ModelOutputXYZ velocity_std;
+  ModelOutputXYZ rotation_std;
+};
+static_assert(sizeof(ModelOutputTemporalPose) == sizeof(ModelOutputXYZ)*4);
+
 struct ModelOutputDisengageProb {
   float gas_disengage;
   float brake_disengage;
@@ -225,6 +233,7 @@ struct ModelOutput {
   const ModelOutputMeta meta;
   const ModelOutputPose pose;
   const ModelOutputWideFromDeviceEuler wide_from_device_euler;
+  const ModelOutputTemporalPose temporal_pose;
 };
 
 constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);

selfdrive/modeld/models/supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:dcfad22cecf37275d01a339d96174800c109e9a70f853fdef3e4ef62ed3f4bbe
-size 45922983
+oid sha256:db746e3753de84367595fedd089c2bd41b06bd401ea28e085663533d0e63d74b
+size 45962473

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-49ea844254883ac61caa2ac425f453799aeb28a6
+30efb4238327d723e17a3bda7e7c19c18f8a3b18