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

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,

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();
-void init_weights(double pathCost, double headingCost, double steerRateCost);
+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]);

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:

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 *")
 

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

selfdrive/debug/mpc/tune_lateral.py

@@ -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()

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 x_std_arr[TRAJECTORY_SIZE];
-  //float y_std_arr[TRAJECTORY_SIZE];
+  float y_std_arr[TRAJECTORY_SIZE];
-  //float z_std_arr[TRAJECTORY_SIZE];
+  float z_std_arr[TRAJECTORY_SIZE];
   float t_arr[TRAJECTORY_SIZE];
   for (int i=0; i<TRAJECTORY_SIZE; i++) {
     // column_offset == -1 means this data is X indexed not T indexed
     if (column_offset >= 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<const float> x_std(x_std_arr, TRAJECTORY_SIZE);
-  //kj::ArrayPtr<const float> y_std(y_std_arr, TRAJECTORY_SIZE);
-  //kj::ArrayPtr<const float> 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.initPosition(), best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr, true);
-  fill_xyzt(framed.initVelocity(), best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr);
+  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);
+  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);
+  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);

selfdrive/test/process_replay/model_ref_commit

@@ -1 +1 @@
-6829c5c76f3527af06e1c2b685f98a5e1bbef00a
+1d17a97b34258507720b39cdf059a3c769aaf998

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-6c2409d2b1a93b675e4cd4ae7e67fc56ec3824dc
+15390f9a445a1fd775079d1938ed14b0d6afacc9

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-724ca5ef28a601d5c78e63fb59890c6c93bd07d7
+305c7a50812c20094998975b2966a7a5ad768e96