From d72d433ec7a3c9f0bb2f90d196b3826d48d9b1a0 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Tue, 6 Apr 2021 23:49:29 -0700 Subject: [PATCH] no heading cost (#20594) * no heading cost * live mpc weight config * need to add stds * make work on empty data * no divide by 0 * update refs * update model replay * update proc replat * new model replay ref --- .../controls/lib/lateral_mpc/lateral_mpc.c | 6 +- .../controls/lib/lateral_mpc/libmpc_py.py | 4 +- selfdrive/controls/lib/lateral_planner.py | 10 +- selfdrive/controls/tests/test_lateral_mpc.py | 4 +- selfdrive/debug/mpc/test_mpc_wobble.py | 3 +- selfdrive/debug/mpc/tune_lateral.py | 187 ------------------ selfdrive/modeld/models/driving.cc | 39 ++-- .../test/process_replay/model_ref_commit | 2 +- .../process_replay/model_replay_ref_commit | 2 +- selfdrive/test/process_replay/ref_commit | 2 +- 10 files changed, 39 insertions(+), 220 deletions(-) delete mode 100755 selfdrive/debug/mpc/tune_lateral.py diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c index 8da44f4f1..6765fe57b 100644 --- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c +++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c @@ -29,7 +29,7 @@ typedef struct { double cost; } log_t; -void init_weights(double pathCost, double headingCost, double steerRateCost){ +void set_weights(double pathCost, double headingCost, double steerRateCost){ int i; const int STEP_MULTIPLIER = 3.0; @@ -44,7 +44,7 @@ void init_weights(double pathCost, double headingCost, double steerRateCost){ acadoVariables.WN[(NYN+1)*1] = headingCost * STEP_MULTIPLIER; } -void init(double pathCost, double headingCost, double steerRateCost){ +void init(){ acado_initializeSolver(); int i; @@ -58,8 +58,6 @@ void init(double pathCost, double headingCost, double steerRateCost){ /* MPC: initialize the current state feedback. */ for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; - - init_weights(pathCost, headingCost, steerRateCost); } int run_mpc(state_t * x0, log_t * solution, double v_ego, diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py index 7ef94fc59..2ca3db3a2 100644 --- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -22,8 +22,8 @@ typedef struct { double cost; } log_t; -void init(double pathCost, double headingCost, double steerRateCost); -void init_weights(double pathCost, double headingCost, double steerRateCost); +void init(); +void set_weights(double pathCost, double headingCost, double steerRateCost); int run_mpc(state_t * x0, log_t * solution, double v_ego, double rotation_radius, double target_y[N+1], double target_psi[N+1]); diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index cc81ae8b3..0ea902f6a 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -63,13 +63,14 @@ class LateralPlanner(): self.desire = log.LateralPlan.Desire.none 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.t_idxs = np.arange(TRAJECTORY_SIZE) self.y_pts = np.zeros(TRAJECTORY_SIZE) def setup_mpc(self): self.libmpc = libmpc_py.libmpc - self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost) + self.libmpc.init() self.mpc_solution = libmpc_py.ffi.new("log_t *") self.cur_state = libmpc_py.ffi.new("state_t *") @@ -94,6 +95,8 @@ class LateralPlanner(): 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) + if len(md.orientation.xStd) == TRAJECTORY_SIZE: + self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd]) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker @@ -161,8 +164,10 @@ class LateralPlanner(): self.LP.lll_prob *= self.lane_change_ll_prob self.LP.rll_prob *= self.lane_change_ll_prob if self.use_lanelines: + std_cost_mult = np.clip(abs(self.path_xyz[0,1]/self.path_xyz_stds[0,1]), 0.5, 5.0) d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) else: + std_cost_mult = 1.0 d_path_xyz = self.path_xyz y_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1]) heading_pts = np.interp(v_ego * self.t_idxs[:MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) @@ -170,6 +175,7 @@ class LateralPlanner(): assert len(y_pts) == MPC_N + 1 assert len(heading_pts) == MPC_N + 1 + self.libmpc.set_weights(std_cost_mult*MPC_COST_LAT.PATH, 0.0, CP.steerRateCost) self.libmpc.run_mpc(self.cur_state, self.mpc_solution, float(v_ego), CAR_ROTATION_RADIUS, @@ -207,7 +213,7 @@ class LateralPlanner(): mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature) t = sec_since_boot() if mpc_nans: - self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost) + self.libmpc.init() self.cur_state.curvature = measured_curvature if t > self.last_cloudlog_t + 5.0: diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index f6c3fbc07..c27bdf2bd 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -8,7 +8,9 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0., lane_width=3.6, poly_shift=0.): libmpc = libmpc_py.libmpc - libmpc.init(1.0, 1.0, 1.0) + libmpc.init() + libmpc.set_weights(1., 1., 1.) + mpc_solution = libmpc_py.ffi.new("log_t *") diff --git a/selfdrive/debug/mpc/test_mpc_wobble.py b/selfdrive/debug/mpc/test_mpc_wobble.py index 6fa960627..ee74c2cf1 100755 --- a/selfdrive/debug/mpc/test_mpc_wobble.py +++ b/selfdrive/debug/mpc/test_mpc_wobble.py @@ -6,7 +6,8 @@ from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT, MPC_N, CAR_ROTATI import math libmpc = libmpc_py.libmpc -libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, 1.) +libmpc.init() +libmpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, 1.) cur_state = libmpc_py.ffi.new("state_t *") cur_state[0].x = 0.0 diff --git a/selfdrive/debug/mpc/tune_lateral.py b/selfdrive/debug/mpc/tune_lateral.py deleted file mode 100755 index f01865574..000000000 --- a/selfdrive/debug/mpc/tune_lateral.py +++ /dev/null @@ -1,187 +0,0 @@ -#! /usr/bin/env python -# type: ignore - -import numpy as np -from collections import OrderedDict -import matplotlib.pyplot as plt -from selfdrive.car.honda.interface import CarInterface -from selfdrive.controls.lib.lateral_mpc import libmpc_py -from selfdrive.controls.lib.vehicle_model import VehicleModel - -# plot lateral MPC trajectory by defining boundary conditions: -# lane lines, p_poly and vehicle states. Use this script to tune MPC costs - -libmpc = libmpc_py.libmpc - -mpc_solution = libmpc_py.ffi.new("log_t *") - -points_l = np.array([1.1049711, 1.1053879, 1.1073375, 1.1096942, 1.1124474, 1.1154714, 1.1192677, 1.1245866, 1.1321017, 1.1396152, 1.146443, 1.1555313, 1.1662073, 1.1774249, 1.1888939, 1.2009926, 1.2149779, 1.2300836, 1.2450289, 1.2617753, 1.2785473, 1.2974714, 1.3151019, 1.3331807, 1.3545501, 1.3763691, 1.3983455, 1.4215056, 1.4446729, 1.4691089, 1.4927692, 1.5175346, 1.5429921, 1.568854, 1.5968665, 1.6268958, 1.657122, 1.6853137, 1.7152609, 1.7477539, 1.7793678, 1.8098511, 1.8428392, 1.8746407, 1.9089606, 1.9426043, 1.9775689, 2.0136933, 2.0520134, 2.0891454]) - -points_r = np.array([-2.4442139, -2.4449506, -2.4448867, -2.44377, -2.4422617, -2.4393811, -2.4374201, -2.4334245, -2.4286852, -2.4238286, -2.4177458, -2.4094386, -2.3994849, -2.3904033, -2.380136, -2.3699453, -2.3594661, -2.3474073, -2.3342307, -2.3194637, -2.3046403, -2.2881098, -2.2706163, -2.2530098, -2.235604, -2.2160542, -2.1967411, -2.1758952, -2.1544619, -2.1325269, -2.1091819, -2.0850561, -2.0621953, -2.0364127, -2.0119917, -1.9851667, -1.9590458, -1.9306552, -1.9024918, -1.8745357, -1.8432863, -1.8131843, -1.7822732, -1.7507075, -1.7180918, -1.6845931, -1.650871, -1.6157099, -1.5787286, -1.5418037]) - - -points_c = (points_l + points_r) / 2.0 - -def compute_path_pinv(): - deg = 3 - x = np.arange(50.0) - X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T - pinv = np.linalg.pinv(X) - return pinv - - -def model_polyfit(points): - path_pinv = compute_path_pinv() - return np.dot(path_pinv, map(float, points)) - - -xx = [] -yy = [] -deltas = [] -psis = [] -times = [] - -CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") -VM = VehicleModel(CP) - -v_ref = 32.00 # 45 mph -curvature_factor = VM.curvature_factor(v_ref) -print(curvature_factor) - -LANE_WIDTH = 3.9 -p_l = map(float, model_polyfit(points_l)) -p_r = map(float, model_polyfit(points_r)) -p_p = map(float, model_polyfit(points_c)) - -l_poly = libmpc_py.ffi.new("double[4]", p_l) -r_poly = libmpc_py.ffi.new("double[4]", p_r) -p_poly = libmpc_py.ffi.new("double[4]", p_p) -l_prob = 1.0 -r_prob = 1.0 -p_prob = 1.0 # This is always 1 - - -mpc_x_points = np.linspace(0., 2.5*v_ref, num=50) -points_poly_l = np.polyval(p_l, mpc_x_points) -points_poly_r = np.polyval(p_r, mpc_x_points) -points_poly_p = np.polyval(p_p, mpc_x_points) -print(points_poly_l) - -lanes_x = np.linspace(0, 49) - -cur_state = libmpc_py.ffi.new("state_t *") -cur_state[0].x = 0.0 -cur_state[0].y = 0.5 -cur_state[0].psi = 0.0 -cur_state[0].delta = 0.0 - -xs = [] -ys = [] -deltas = [] -titles = [ - 'Steer rate cost', - 'Heading cost', - 'Lane cost', - 'Path cost', -] - -# Steer rate cost -sol_x = OrderedDict() -sol_y = OrderedDict() -delta = OrderedDict() -for cost in np.logspace(-1, 1.0, 5): - libmpc.init(1.0, 3.0, 1.0, cost) - for _ in range(10): - libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, - curvature_factor, v_ref, LANE_WIDTH) - sol_x[cost] = map(float, list(mpc_solution[0].x)) - sol_y[cost] = map(float, list(mpc_solution[0].y)) - delta[cost] = map(float, list(mpc_solution[0].delta)) -xs.append(sol_x) -ys.append(sol_y) -deltas.append(delta) - -# Heading cost -sol_x = OrderedDict() -sol_y = OrderedDict() -delta = OrderedDict() -for cost in np.logspace(-1, 1.0, 5): - libmpc.init(1.0, 3.0, cost, 1.0) - for _ in range(10): - libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, - curvature_factor, v_ref, LANE_WIDTH) - sol_x[cost] = map(float, list(mpc_solution[0].x)) - sol_y[cost] = map(float, list(mpc_solution[0].y)) - delta[cost] = map(float, list(mpc_solution[0].delta)) -xs.append(sol_x) -ys.append(sol_y) -deltas.append(delta) - -# Lane cost -sol_x = OrderedDict() -sol_y = OrderedDict() -delta = OrderedDict() -for cost in np.logspace(-1, 2.0, 5): - libmpc.init(1.0, cost, 1.0, 1.0) - for _ in range(10): - libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, - curvature_factor, v_ref, LANE_WIDTH) - sol_x[cost] = map(float, list(mpc_solution[0].x)) - sol_y[cost] = map(float, list(mpc_solution[0].y)) - delta[cost] = map(float, list(mpc_solution[0].delta)) -xs.append(sol_x) -ys.append(sol_y) -deltas.append(delta) - - -# Path cost -sol_x = OrderedDict() -sol_y = OrderedDict() -delta = OrderedDict() -for cost in np.logspace(-1, 1.0, 5): - libmpc.init(cost, 3.0, 1.0, 1.0) - for _ in range(10): - libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, - curvature_factor, v_ref, LANE_WIDTH) - sol_x[cost] = map(float, list(mpc_solution[0].x)) - sol_y[cost] = map(float, list(mpc_solution[0].y)) - delta[cost] = map(float, list(mpc_solution[0].delta)) -xs.append(sol_x) -ys.append(sol_y) -deltas.append(delta) - - -plt.figure() - -for i in range(len(xs)): - ax = plt.subplot(2, 2, i + 1) - sol_x = xs[i] - sol_y = ys[i] - for cost in sol_x.keys(): - plt.plot(sol_x[cost], sol_y[cost]) - - plt.plot(lanes_x, points_r, '.b') - plt.plot(lanes_x, points_l, '.b') - plt.plot(lanes_x, (points_l + points_r) / 2.0, '--g') - plt.plot(mpc_x_points, points_poly_l, 'b') - plt.plot(mpc_x_points, points_poly_r, 'b') - plt.plot(mpc_x_points, (points_poly_l + points_poly_r) / 2.0, 'g') - plt.legend(map(lambda x: str(round(x, 2)), sol_x.keys()) + ['right', 'left', 'center'], loc=3) - plt.title(titles[i]) - plt.grid(True) - # ax.set_aspect('equal', 'datalim') - - -plt.figure() -for i in range(len(xs)): - plt.subplot(2, 2, i + 1) - sol_x = xs[i] - delta = deltas[i] - - for cost in sol_x.keys(): - plt.plot(delta[cost]) - plt.title(titles[i]) - plt.legend(map(lambda x: str(round(x, 2)), sol_x.keys()), loc=3) - plt.grid(True) - -plt.show() diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index c0b7de3d3..0bd6534d1 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -180,40 +180,39 @@ void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_da } void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data, - int columns, int column_offset, float * plan_t_arr) { + int columns, int column_offset, float * plan_t_arr, bool fill_std) { float x_arr[TRAJECTORY_SIZE] = {}; float y_arr[TRAJECTORY_SIZE] = {}; float z_arr[TRAJECTORY_SIZE] = {}; - //float x_std_arr[TRAJECTORY_SIZE]; - //float y_std_arr[TRAJECTORY_SIZE]; - //float z_std_arr[TRAJECTORY_SIZE]; + float x_std_arr[TRAJECTORY_SIZE]; + float y_std_arr[TRAJECTORY_SIZE]; + float z_std_arr[TRAJECTORY_SIZE]; float t_arr[TRAJECTORY_SIZE]; for (int i=0; i= 0) { t_arr[i] = T_IDXS[i]; x_arr[i] = data[i*columns + 0 + column_offset]; - //x_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 0 + column_offset]; + x_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 0 + column_offset]; } else { t_arr[i] = plan_t_arr[i]; x_arr[i] = X_IDXS[i]; - //x_std_arr[i] = NAN; + x_std_arr[i] = NAN; } y_arr[i] = data[i*columns + 1 + column_offset]; - //y_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 1 + column_offset]; + y_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 1 + column_offset]; z_arr[i] = data[i*columns + 2 + column_offset]; - //z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset]; + z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset]; } - //kj::ArrayPtr x_std(x_std_arr, TRAJECTORY_SIZE); - //kj::ArrayPtr y_std(y_std_arr, TRAJECTORY_SIZE); - //kj::ArrayPtr z_std(z_std_arr, TRAJECTORY_SIZE); xyzt.setX(x_arr); xyzt.setY(y_arr); xyzt.setZ(z_arr); - //xyzt.setXStd(x_std); - //xyzt.setYStd(y_std); - //xyzt.setZStd(z_std); xyzt.setT(t_arr); + if (fill_std) { + xyzt.setXStd(x_std_arr); + xyzt.setYStd(y_std_arr); + xyzt.setZStd(z_std_arr); + } } void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_outputs) { @@ -224,17 +223,17 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou plan_t_arr[i] = best_plan[i*PLAN_MHP_COLUMNS + 15]; } - fill_xyzt(framed.initPosition(), best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr); - fill_xyzt(framed.initVelocity(), best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr); - fill_xyzt(framed.initOrientation(), best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr); - fill_xyzt(framed.initOrientationRate(), best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr); + fill_xyzt(framed.initPosition(), best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr, true); + fill_xyzt(framed.initVelocity(), best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr, false); + fill_xyzt(framed.initOrientation(), best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr, false); + fill_xyzt(framed.initOrientationRate(), best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr, false); // lane lines auto lane_lines = framed.initLaneLines(4); float lane_line_probs_arr[4]; float lane_line_stds_arr[4]; for (int i = 0; i < 4; i++) { - fill_xyzt(lane_lines[i], &net_outputs.lane_lines[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr); + fill_xyzt(lane_lines[i], &net_outputs.lane_lines[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr, false); lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i]); lane_line_stds_arr[i] = exp(net_outputs.lane_lines[2*TRAJECTORY_SIZE*(4 + i)]); } @@ -245,7 +244,7 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou auto road_edges = framed.initRoadEdges(2); float road_edge_stds_arr[2]; for (int i = 0; i < 2; i++) { - fill_xyzt(road_edges[i], &net_outputs.road_edges[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr); + fill_xyzt(road_edges[i], &net_outputs.road_edges[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr, false); road_edge_stds_arr[i] = exp(net_outputs.road_edges[2*TRAJECTORY_SIZE*(2 + i)]); } framed.setRoadEdgeStds(road_edge_stds_arr); diff --git a/selfdrive/test/process_replay/model_ref_commit b/selfdrive/test/process_replay/model_ref_commit index c44a660c4..03099ac10 100644 --- a/selfdrive/test/process_replay/model_ref_commit +++ b/selfdrive/test/process_replay/model_ref_commit @@ -1 +1 @@ -6829c5c76f3527af06e1c2b685f98a5e1bbef00a \ No newline at end of file +1d17a97b34258507720b39cdf059a3c769aaf998 diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index a4c305cd2..41fd2020c 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -6c2409d2b1a93b675e4cd4ae7e67fc56ec3824dc +15390f9a445a1fd775079d1938ed14b0d6afacc9 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 646026b0a..84709e350 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -724ca5ef28a601d5c78e63fb59890c6c93bd07d7 \ No newline at end of file +305c7a50812c20094998975b2966a7a5ad768e96 \ No newline at end of file