From 48522500ea14d90bfd565afbcf835cb6467ad84e Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 12 Aug 2022 00:47:59 -0700 Subject: [PATCH] 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 Co-authored-by: Bruce Wayne Co-authored-by: Yassine Yousfi Co-authored-by: Bruce Wayne old-commit-hash: 041458f632f197de3c954ed4a08650fbcb24690a --- .../controls/lib/lateral_mpc_lib/lat_mpc.py | 5 +++-- selfdrive/controls/lib/lateral_planner.py | 19 +++++++++++++------ selfdrive/controls/plannerd.py | 2 +- selfdrive/controls/tests/test_lateral_mpc.py | 3 ++- selfdrive/modeld/models/supercombo.dlc | 4 ++-- selfdrive/modeld/models/supercombo.onnx | 4 ++-- selfdrive/modeld/thneed/optimizer.cc | 8 ++++---- selfdrive/test/process_replay/model_replay.py | 2 +- .../process_replay/model_replay_ref_commit | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- 10 files changed, 30 insertions(+), 21 deletions(-) diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index a1037f040b..c0e7358160 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/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) diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 019a19fb1d..dda4f18a49 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/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 - p = np.array([v_ego, CAR_ROTATION_RADIUS]) + assert len(curv_rate_pts) == LAT_MPC_N + 1 + 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. diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 9356a55d84..06807b2a5c 100755 --- 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/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 4630e28a61..4864dbdc06 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/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 diff --git a/selfdrive/modeld/models/supercombo.dlc b/selfdrive/modeld/models/supercombo.dlc index 23f6d904fb..fe3ad51021 100644 --- 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:027cbb1fabae369878271cb0e3505071a8bdaa07473fad9a0b2e8d695c5dc1ff -size 76725611 +oid sha256:3c5c8d71a8a1434ef79073362e608b9fe02f22ce7478f11bc71c6806c1e00091 +size 94302331 diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 9023c18dd7..39e8743642 100644 --- 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:484976ea5bd4ddcabc82e95faf30d7311a27802c1e337472558699fa2395a499 -size 77472267 +oid sha256:15d9eb01edd98998abceaa092d33fab149ff4a8942646b20a7c1403f999f4eca +size 95165081 diff --git a/selfdrive/modeld/thneed/optimizer.cc b/selfdrive/modeld/thneed/optimizer.cc index 03d20ff386..39737d3d76 100644 --- a/selfdrive/modeld/thneed/optimizer.cc +++ b/selfdrive/modeld/thneed/optimizer.cc @@ -9,7 +9,7 @@ extern map 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 diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 8b41d6df1b..12f3583ee4 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/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")) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 8b94834193..e5bbfa74c2 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -5c2cb8fb9787584a1eb553968cb87e7e6782dac5 +507241e0a41ee757cca4eb6ff12cff3f02c0b98a diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 27bea55740..8c8f5d10fe 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -c556add24a92c9d8593d9d3ebbeed9434b751d66 \ No newline at end of file +461333164e531e3ffdac05b63b529aa5dce1186f