Falcon Punch Model: turn cutting improvements (#25413)

* simplified change to mpc dynamics

* add jerk pts

* increase jerk cost

* increase jerk pts multipler to master value

* Add final commit

* 1456d261-d232-4654-8885-4d9fde883894/440 ac1a6744-85b0-4ec6-8ba7-608d0717b8f1/750

* some copies are useful

* update model replay ref

* less frames in model replay onnx cpu

* 1456d261-d232-4654-8885-4d9fde883894/440 264b67f5-3f52-4b58-b11f-58dd8aaf08bf/950

* 1456d261-d232-4654-8885-4d9fde883894/440 236fc556-fba3-4255-8ccf-684b22637160/950

* c9d10c64-bea4-41ec-8ca3-d8c886fda172/440 26d73dd2-862a-44ae-bbdd-32cc4f397ad7/900

* Fix couple tests

* Update ref

* Unused for now

* Add lateral factor comment

* Unused variable

Co-authored-by: nuwandavek <vivekaithal44@gmail.com>
Co-authored-by: Bruce Wayne <yassine@comma.ai>
Co-authored-by: Yassine Yousfi <yyousfi1@binghamton.edu>
Co-authored-by: Bruce Wayne <batman@gpu06.internal>

selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py

@@ -89,7 +89,7 @@ def gen_lat_ocp():
   # TODO hacky weights to keep behavior the same
   ocp.model.cost_y_expr = vertcat(y_ego,
                                   ((v_ego +5.0) * psi_ego),
-                                  ((v_ego +5.0) * 4 * curv_rate))
+                                  ((v_ego + 5.0) * 4.0 * curv_rate))
   ocp.model.cost_y_expr_e = vertcat(y_ego,
                                     ((v_ego +5.0) * psi_ego))
 
@@ -147,7 +147,7 @@ class LateralMpc():
     #TODO hacky weights to keep behavior the same
     self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2])
 
-  def run(self, x0, p, y_pts, heading_pts):
+  def run(self, x0, p, y_pts, heading_pts, curv_rate_pts):
     x0_cp = np.copy(x0)
     p_cp = np.copy(p)
     self.solver.constraints_set(0, "lbx", x0_cp)
@@ -156,6 +156,7 @@ class LateralMpc():
     v_ego = p_cp[0]
     # rotation_radius = p_cp[1]
     self.yref[:,1] = heading_pts*(v_ego+5.0)
+    self.yref[:,2] = curv_rate_pts * (v_ego+5.0) * 4.0
     for i in range(N):
       self.solver.cost_set(i, "yref", self.yref[i])
       self.solver.set(i, "p", p_cp)

selfdrive/controls/lib/lateral_planner.py

@@ -3,7 +3,7 @@ from common.realtime import sec_since_boot, DT_MDL
 from common.numpy_fast import interp
 from system.swaglog import cloudlog
 from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
-from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N, CAR_ROTATION_RADIUS
+from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N
 from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE
 from selfdrive.controls.lib.desire_helper import DesireHelper
 import cereal.messaging as messaging
@@ -11,17 +11,21 @@ from cereal import log
 
 
 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()
 
+    # Vehicle model parameters used to calculate lateral movement of car
+    self.factor1 = CP.wheelbase - CP.centerToFront
+    self.factor2 = (CP.centerToFront * CP.mass) / (CP.wheelbase * CP.tireStiffnessRear)
     self.last_cloudlog_t = 0
     self.solution_invalid_cnt = 0
 
     self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
     self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
     self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
+    self.plan_curv_rate = np.zeros((TRAJECTORY_SIZE,))
     self.t_idxs = np.arange(TRAJECTORY_SIZE)
     self.y_pts = np.zeros(TRAJECTORY_SIZE)
 
@@ -42,7 +46,7 @@ class LateralPlanner:
     if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.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 = list(md.orientation.z)
+      self.plan_yaw = np.array(md.orientation.z)
     if len(md.position.xStd) == TRAJECTORY_SIZE:
       self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])
 
@@ -67,16 +71,19 @@ class LateralPlanner:
 
     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)
+    curv_rate_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_curv_rate)
     self.y_pts = y_pts
 
     assert len(y_pts) == LAT_MPC_N + 1
     assert len(heading_pts) == LAT_MPC_N + 1
-    # self.x0[4] = v_ego
+    assert len(curv_rate_pts) == LAT_MPC_N + 1
-    p = np.array([v_ego, CAR_ROTATION_RADIUS])
+    lateral_factor = max(0, self.factor1 - (self.factor2 * v_ego**2))
+    p = np.array([v_ego, lateral_factor])
     self.lat_mpc.run(self.x0,
                      p,
                      y_pts,
-                     heading_pts)
+                     heading_pts,
+                     curv_rate_pts)
     # init state for next
     # mpc.u_sol is the desired curvature rate given x0 curv state.
     # with x0[3] = measured_curvature, this would be the actual desired rate.

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'],

selfdrive/controls/tests/test_lateral_mpc.py

@@ -13,6 +13,7 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur
 
   y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
   heading_pts = np.zeros(LAT_MPC_N + 1)
+  curv_rate_pts = np.zeros(LAT_MPC_N + 1)
 
   x0 = np.array([x_init, y_init, psi_init, curvature_init])
   p = np.array([v_ref, CAR_ROTATION_RADIUS])
@@ -20,7 +21,7 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur
   # converge in no more than 10 iterations
   for _ in range(10):
     lat_mpc.run(x0, p,
-                y_pts, heading_pts)
+                y_pts, heading_pts, curv_rate_pts)
   return lat_mpc.x_sol
 
 

selfdrive/modeld/models/supercombo.dlc

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:027cbb1fabae369878271cb0e3505071a8bdaa07473fad9a0b2e8d695c5dc1ff
+oid sha256:3c5c8d71a8a1434ef79073362e608b9fe02f22ce7478f11bc71c6806c1e00091
-size 76725611
+size 94302331

selfdrive/modeld/models/supercombo.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:484976ea5bd4ddcabc82e95faf30d7311a27802c1e337472558699fa2395a499
+oid sha256:15d9eb01edd98998abceaa092d33fab149ff4a8942646b20a7c1403f999f4eca
-size 77472267
+size 95165081

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 @@ static int is_same_size_image(cl_mem a, cl_mem b) {
   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
 

selfdrive/test/process_replay/model_replay.py

@@ -22,7 +22,7 @@ from tools.lib.logreader import LogReader
 
 TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"
 SEGMENT = 0
-MAX_FRAMES = 20 if PC else 1300
+MAX_FRAMES = 10 if PC else 1300
 
 SEND_EXTRA_INPUTS = bool(os.getenv("SEND_EXTRA_INPUTS", "0"))
 

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-5c2cb8fb9787584a1eb553968cb87e7e6782dac5
+507241e0a41ee757cca4eb6ff12cff3f02c0b98a

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-c556add24a92c9d8593d9d3ebbeed9434b751d66
+461333164e531e3ffdac05b63b529aa5dce1186f