Revert "Rocket league model (commaai#24869)"

echo1001 · echo1001 · commit 0c6fd3d2a2ca · 2022-06-19T12:19:56.000-05:00
This reverts commit 9283040.
diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py
@@ -19,6 +19,7 @@ def get_params(candidate, fingerprint=None, car_fw=None, disable_radar=False):
     ret.minSteerSpeed = -math.inf
     ret.maxLateralAccel = math.inf  # TODO: set to a reasonable value
     ret.steerRatio = 0.5
+    ret.steerRateCost = 0.5
     ret.steerLimitTimer = 1.0
     ret.steerActuatorDelay = 0.
 
diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py
@@ -13,6 +13,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disa
     ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.chrysler)]
 
     ret.steerActuatorDelay = 0.1
+    ret.steerRateCost = 0.7
     ret.steerLimitTimer = 0.4
 
     ret.minSteerSpeed = 3.8  # m/s
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
@@ -59,6 +59,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disa
     # LCA can steer down to zero
     ret.minSteerSpeed = 0.
 
+    ret.steerRateCost = 1.0
     ret.centerToFront = ret.wheelbase * 0.44
 
     ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
@@ -63,6 +63,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disa
     ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
     ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
     ret.lateralTuning.pid.kf = 0.00004   # full torque for 20 deg at 80mph means 0.00007818594
+    ret.steerRateCost = 1.0
     ret.steerActuatorDelay = 0.1  # Default delay, not measured yet
 
     ret.longitudinalTuning.kpBP = [5., 35.]
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
@@ -319,6 +319,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
                                                                          tire_stiffness_factor=tire_stiffness_factor)
 
     ret.steerActuatorDelay = 0.1
+    ret.steerRateCost = 0.5
     ret.steerLimitTimer = 0.8
 
     return ret
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
@@ -40,6 +40,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.dashcamOnly = candidate in {CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30} or candidate in HDA2_CAR
 
     ret.steerActuatorDelay = 0.1  # Default delay
+    ret.steerRateCost = 0.5
     ret.steerLimitTimer = 0.4
     tire_stiffness_factor = 1.
 
diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py
@@ -25,6 +25,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disa
     ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021)
 
     ret.steerActuatorDelay = 0.1
+    ret.steerRateCost = 1.0
     ret.steerLimitTimer = 0.8
     tire_stiffness_factor = 0.70   # not optimized yet
 
diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py
@@ -14,6 +14,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disa
     ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)]
 
     ret.steerLimitTimer = 1.0
+    ret.steerRateCost = 0.5
 
     ret.steerActuatorDelay = 0.1
 
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
@@ -22,6 +22,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disa
 
     ret.dashcamOnly = candidate in PREGLOBAL_CARS
 
+    ret.steerRateCost = 0.7
     ret.steerLimitTimer = 0.4
 
     if candidate == CAR.ASCENT:
diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py
@@ -42,6 +42,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disa
 
     ret.steerLimitTimer = 1.0
     ret.steerActuatorDelay = 0.25
+    ret.steerRateCost = 0.5
 
     if candidate in (CAR.AP2_MODELS, CAR.AP1_MODELS):
       ret.mass = 2100. + STD_CARGO_KG
diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py
@@ -33,6 +33,7 @@ def test_car_interfaces(self, car_name):
     assert car_interface
 
     self.assertGreater(car_params.mass, 1)
+    self.assertGreater(car_params.steerRateCost, 1e-3)
 
     if car_params.steerControlType != car.CarParams.SteerControlType.angle:
       tuning = car_params.lateralTuning.which()
diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py
@@ -121,6 +121,7 @@ def test_car_params(self):
 
     # make sure car params are within a valid range
     self.assertGreater(self.CP.mass, 1)
+    self.assertGreater(self.CP.steerRateCost, 1e-3)
 
     if self.CP.steerControlType != car.CarParams.SteerControlType.angle:
       tuning = self.CP.lateralTuning.which()
diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py
@@ -45,6 +45,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disa
     # Global lateral tuning defaults, can be overridden per-vehicle
 
     ret.steerActuatorDelay = 0.1
+    ret.steerRateCost = 1.0
     ret.steerLimitTimer = 0.4
     ret.steerRatio = 15.6  # Let the params learner figure this out
     tire_stiffness_factor = 1.0  # Let the params learner figure this out
diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py
@@ -11,12 +11,13 @@
 
 
 class LateralPlanner:
-  def __init__(self, use_lanelines=True, wide_camera=False):
+  def __init__(self, CP, use_lanelines=True, wide_camera=False):
     self.use_lanelines = use_lanelines
     self.LP = LanePlanner(wide_camera)
     self.DH = DesireHelper()
 
     self.last_cloudlog_t = 0
+    self.steer_rate_cost = CP.steerRateCost
     self.solution_invalid_cnt = 0
 
     self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
@@ -58,12 +59,12 @@ def update(self, sm):
     # Calculate final driving path and set MPC costs
     if self.use_lanelines:
       d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
-      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, MPC_COST_LAT.STEER_RATE)
+      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
     else:
       d_path_xyz = self.path_xyz
       # Heading cost is useful at low speed, otherwise end of plan can be off-heading
       heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.15])
-      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, MPC_COST_LAT.STEER_RATE)
+      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, heading_cost, self.steer_rate_cost)
 
     y_pts = np.interp(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(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
@@ -78,7 +79,7 @@ def update(self, sm):
                      y_pts,
                      heading_pts)
     # init state for next
-    # mpc.u_sol is the desired curvature rate given x0 curv state.
+    # mpc.u_sol is the desired curvature rate given x0 curv state. 
     # with x0[3] = measured_curvature, this would be the actual desired rate.
     # instead, interpolate x_sol so that x0[3] is the desired curvature for lat_control.
     self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
@@ -22,7 +22,7 @@ def plannerd_thread(sm=None, pm=None):
   cloudlog.event("e2e mode", on=use_lanelines)
 
   longitudinal_planner = Planner(CP)
-  lateral_planner = LateralPlanner(use_lanelines=use_lanelines, wide_camera=wide_camera)
+  lateral_planner = LateralPlanner(CP, use_lanelines=use_lanelines, wide_camera=wide_camera)
 
   if sm is None:
     sm = messaging.SubMaster(['carState', 'controlsState', 'radarState', 'modelV2'],
diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h
@@ -245,7 +245,7 @@ struct ModelOutput {
 
 constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);
 #ifdef TEMPORAL
-  constexpr int TEMPORAL_SIZE = 512+256;
+  constexpr int TEMPORAL_SIZE = 512;
 #else
   constexpr int TEMPORAL_SIZE = 0;
 #endif
diff --git a/selfdrive/modeld/models/supercombo.dlc b/selfdrive/modeld/models/supercombo.dlc
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:4c2cb3a3054f3292bbe538d6b793908dc2e234c200802d41b6766d3cb51b0b44
-size 101662751
+oid sha256:027cbb1fabae369878271cb0e3505071a8bdaa07473fad9a0b2e8d695c5dc1ff
+size 76725611
diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:96b60d0bfd1386c93b4f79195aa1c5e77b23e0250578a308ee2c58857ed5eb49
-size 102570834
+oid sha256:484976ea5bd4ddcabc82e95faf30d7311a27802c1e337472558699fa2395a499
+size 77472267
diff --git a/selfdrive/modeld/thneed/compile.cc b/selfdrive/modeld/thneed/compile.cc
@@ -5,7 +5,7 @@
 #include "selfdrive/modeld/thneed/thneed.h"
 #include "system/hardware/hw.h"
 
-#define TEMPORAL_SIZE 512+256
+#define TEMPORAL_SIZE 512
 #define DESIRE_LEN 8
 #define TRAFFIC_CONVENTION_LEN 2
 
diff --git a/selfdrive/modeld/thneed/optimizer.cc b/selfdrive/modeld/thneed/optimizer.cc
@@ -9,7 +9,7 @@
 
 extern map<cl_program, string> g_program_source;
 
-/*static int is_same_size_image(cl_mem a, cl_mem b) {
+static int is_same_size_image(cl_mem a, cl_mem b) {
   size_t a_width, a_height, a_depth, a_array_size, a_row_pitch, a_slice_pitch;
   clGetImageInfo(a, CL_IMAGE_WIDTH, sizeof(a_width), &a_width, NULL);
   clGetImageInfo(a, CL_IMAGE_HEIGHT, sizeof(a_height), &a_height, NULL);
@@ -29,7 +29,7 @@ extern map<cl_program, string> g_program_source;
   return (a_width == b_width) && (a_height == b_height) &&
     (a_depth == b_depth) && (a_array_size == b_array_size) &&
     (a_row_pitch == b_row_pitch) && (a_slice_pitch == b_slice_pitch);
-}*/
+}
 
 static cl_mem make_image_like(cl_context context, cl_mem val) {
   cl_image_format format;
@@ -138,7 +138,7 @@ int Thneed::optimize() {
 
       // delete useless copy layers
       // saves ~0.7 ms
-      /*if (kq[i]->name == "concatenation" || kq[i]->name == "flatten") {
+      if (kq[i]->name == "concatenation" || kq[i]->name == "flatten") {
         string in = kq[i]->args[kq[i]->get_arg_num("input")];
         string out = kq[i]->args[kq[i]->get_arg_num("output")];
         if (is_same_size_image(*(cl_mem*)in.data(), *(cl_mem*)out.data())) {
@@ -148,7 +148,7 @@ int Thneed::optimize() {
 
           kq.erase(kq.begin()+i); --i;
         }
-      }*/
+      }
 
       // NOTE: if activations/accumulation are done in the wrong order, this will be wrong
 
diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit
@@ -1 +1 @@
-629eaa7b26d1721a71547f9de880f99732cb27f3
+512a9d4596c8faba304d6f7ded2ce77837357b65
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-ed1dfb8b155ebcd8fdad4e06462b3bb7869fc67b
+1d66eed104dbc124c4e5679f5dddf40197b86ce9
