Cleanup pathplanner (#19827)

* no divide by 0

* misc cleanup

* final fixes

* remove last polys

* new ref

* fix test

* update again
old-commit-hash: 0e49919ab9

cereal

@@ -1 +1 @@
-Subproject commit a85bf58bb595ad2507186e81d4b2cf2b975a5140
+Subproject commit bbf05d546a6074f269accdf44e58a6d4232a961b

selfdrive/controls/controlsd.py

@@ -394,8 +394,8 @@ class Controls:
     if (lac_log.saturated and not CS.steeringPressed) or \
        (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
       # Check if we deviated from the path
-      left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1
-      right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1
+      left_deviation = actuators.steer > 0 and path_plan.dPathPoints[0] < -0.1
+      right_deviation = actuators.steer < 0 and path_plan.dPathPoints[0] > 0.1
 
       if left_deviation or right_deviation:
         self.events.add(EventName.steerSaturated)
@@ -437,8 +437,8 @@ class Controls:
     if len(meta.desirePrediction) and ldw_allowed:
       l_lane_change_prob = meta.desirePrediction[Desire.laneChangeLeft - 1]
       r_lane_change_prob = meta.desirePrediction[Desire.laneChangeRight - 1]
-      l_lane_close = left_lane_visible and (self.sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET))
-      r_lane_close = right_lane_visible and (self.sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET))
+      l_lane_close = left_lane_visible and (self.sm['model'].leftLane.poly[3] < (1.08 - CAMERA_OFFSET))
+      r_lane_close = right_lane_visible and (self.sm['model'].rightLane.poly[3] > -(1.08 + CAMERA_OFFSET))
 
       CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close)
       CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close)

selfdrive/controls/lib/lane_planner.py

@@ -19,6 +19,7 @@ class LanePlanner:
 
     self.lll_prob = 0.
     self.rll_prob = 0.
+    self.d_prob = 0.
 
     self.lll_std = 0.
     self.rll_std = 0.
@@ -75,8 +76,8 @@ class LanePlanner:
     path_from_left_lane = self.lll_y + clipped_lane_width / 2.0
     path_from_right_lane = self.rll_y - clipped_lane_width / 2.0
 
-    lr_prob = l_prob + r_prob - l_prob * r_prob
+    self.d_prob = l_prob + r_prob - l_prob * r_prob
     lane_path_y = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
     lane_path_y_interp = np.interp(path_t, self.ll_t, lane_path_y)
-    path_xyz[:,1] = lr_prob * lane_path_y_interp + (1.0 - lr_prob) * path_xyz[:,1]
+    path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
     return path_xyz

selfdrive/controls/lib/lateral_mpc/generator.cpp

@@ -17,21 +17,20 @@ int main( )
   DifferentialState xx; // x position
   DifferentialState yy; // y position
   DifferentialState psi; // vehicle heading
-  DifferentialState delta;
+  DifferentialState tire_angle;
 
   OnlineData curvature_factor;
-  OnlineData v_poly_r0, v_poly_r1, v_poly_r2, v_poly_r3;
+  OnlineData v_ego;
   OnlineData rotation_radius;
 
-  Control t;
+  Control tire_angle_rate;
   
-  auto poly_v = v_poly_r0*(xx*xx*xx) + v_poly_r1*(xx*xx) + v_poly_r2*xx + v_poly_r3;
 
   // Equations of motion
-  f << dot(xx) == poly_v * cos(psi) - rotation_radius * sin(psi) * (poly_v * delta *curvature_factor);
-  f << dot(yy) == poly_v * sin(psi) + rotation_radius * cos(psi) * (poly_v * delta *curvature_factor);
-  f << dot(psi) == poly_v * delta * curvature_factor;
-  f << dot(delta) == t;
+  f << dot(xx) == v_ego * cos(psi) - rotation_radius * sin(psi) * (v_ego * tire_angle *curvature_factor);
+  f << dot(yy) == v_ego * sin(psi) + rotation_radius * cos(psi) * (v_ego * tire_angle *curvature_factor);
+  f << dot(psi) == v_ego * tire_angle * curvature_factor;
+  f << dot(tire_angle) == tire_angle_rate;
 
   // Running cost
   Function h;
@@ -40,10 +39,10 @@ int main( )
   h << yy;
 
   // Heading error
-  h << (v_poly_r3 + 1.0 ) * psi;
+  h << (v_ego + 1.0 ) * psi;
 
   // Angular rate error
-  h << (v_poly_r3 + 1.0 ) * t;
+  h << (v_ego + 1.0 ) * tire_angle_rate;
 
   BMatrix Q(3,3); Q.setAll(true);
   // Q(0,0) = 1.0;
@@ -59,7 +58,7 @@ int main( )
   hN << yy;
 
   // Heading errors
-  hN << (2.0 * v_poly_r3 + 1.0 ) * psi;
+  hN << (2.0 * v_ego + 1.0 ) * psi;
 
   BMatrix QN(2,2); QN.setAll(true);
   // QN(0,0) = 1.0;
@@ -79,8 +78,8 @@ int main( )
   // car can't go backward to avoid "circles"
   ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
   // more than absolute max steer angle
-  ocp.subjectTo( deg2rad(-50) <= delta <= deg2rad(50));
-  ocp.setNOD(6);
+  ocp.subjectTo( deg2rad(-50) <= tire_angle <= deg2rad(50));
+  ocp.setNOD(3);
 
   OCPexport mpc(ocp);
   mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );

selfdrive/controls/lib/lateral_mpc/lateral_mpc.c

@@ -17,15 +17,15 @@ ACADOvariables acadoVariables;
 ACADOworkspace acadoWorkspace;
 
 typedef struct {
-  double x, y, psi, delta, t;
+  double x, y, psi, tire_angle, tire_angle_rate;
 } state_t;
 
 typedef struct {
   double x[N+1];
   double y[N+1];
   double psi[N+1];
-  double delta[N+1];
-  double rate[N];
+  double tire_angle[N+1];
+  double tire_angle_rate[N];
   double cost;
 } log_t;
 
@@ -62,34 +62,28 @@ void init(double pathCost, double headingCost, double steerRateCost){
   init_weights(pathCost, headingCost, steerRateCost);
 }
 
-int run_mpc(state_t * x0, log_t * solution, double v_poly[4],
+int run_mpc(state_t * x0, log_t * solution, double v_ego,
              double curvature_factor, double rotation_radius, double target_y[N+1], double target_psi[N+1]){
 
   int    i;
 
   for (i = 0; i <= NOD * N; i+= NOD){
     acadoVariables.od[i] = curvature_factor;
-    
-    acadoVariables.od[i+1] = v_poly[0];
-    acadoVariables.od[i+2] = v_poly[1];
-    acadoVariables.od[i+3] = v_poly[2];
-    acadoVariables.od[i+4] = v_poly[3];
-    
-    acadoVariables.od[i+5] = rotation_radius;
-
+    acadoVariables.od[i+1] = v_ego;
+    acadoVariables.od[i+2] = rotation_radius;
   }
   for (i = 0; i < N; i+= 1){
     acadoVariables.y[NY*i + 0] = target_y[i];
-    acadoVariables.y[NY*i + 1] = (v_poly[3] + 1.0) * target_psi[i];
+    acadoVariables.y[NY*i + 1] = (v_ego + 1.0) * target_psi[i];
     acadoVariables.y[NY*i + 2] = 0.0;
   }
   acadoVariables.yN[0] = target_y[N];
-  acadoVariables.yN[1] = (2.0 * v_poly[3] + 1.0) * target_psi[N];
+  acadoVariables.yN[1] = (2.0 * v_ego + 1.0) * target_psi[N];
 
   acadoVariables.x0[0] = x0->x;
   acadoVariables.x0[1] = x0->y;
   acadoVariables.x0[2] = x0->psi;
-  acadoVariables.x0[3] = x0->delta;
+  acadoVariables.x0[3] = x0->tire_angle;
 
 
   acado_preparationStep();
@@ -102,9 +96,9 @@ int run_mpc(state_t * x0, log_t * solution, double v_poly[4],
     solution->x[i] = acadoVariables.x[i*NX];
     solution->y[i] = acadoVariables.x[i*NX+1];
     solution->psi[i] = acadoVariables.x[i*NX+2];
-    solution->delta[i] = acadoVariables.x[i*NX+3];
+    solution->tire_angle[i] = acadoVariables.x[i*NX+3];
     if (i < N){
-      solution->rate[i] = acadoVariables.u[i];
+      solution->tire_angle_rate[i] = acadoVariables.u[i];
     }
   }
   solution->cost = acado_getObjective();

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:e15604230fe8c48c3448ec978b3b5a0c80b21cade787931acce50602190fca7b
-size 8755
+oid sha256:16f6549e3f9731d8154541f11f8e663df3e0fe1d6b095bf01dd0cf16e0bf9267
+size 8752

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:2bd358ab623df9fbf4182ff955f04505df4abd83c2a0afd21a66d71f34aeda08
-size 25742
+oid sha256:454572df20b2ec7e4c0ec463f832f8b22c5735db5e3fd1f6302a7d101251c0c1
+size 19322

selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:ee16cb2f641439c28e352ac0fe967a5cea95e7807074e40523d2e1f259fe84f5
-size 245177
+oid sha256:3f8f5554059ca8472ec625771c7060ad93be05ba51963c5d4c4c577e6c765dbf
+size 243936

selfdrive/controls/lib/lateral_mpc/libmpc_py.py

@@ -9,7 +9,7 @@ libmpc_fn = os.path.join(mpc_dir, "libmpc"+suffix())
 ffi = FFI()
 ffi.cdef("""
 typedef struct {
-    double x, y, psi, delta, t;
+    double x, y, psi, tire_angle, tire_angle_rate;
 } state_t;
 int N = 16;
 
@@ -17,15 +17,15 @@ typedef struct {
     double x[N+1];
     double y[N+1];
     double psi[N+1];
-    double delta[N+1];
-    double rate[N];
+    double tire_angle[N+1];
+    double tire_angle_rate[N];
     double cost;
 } log_t;
 
 void init(double pathCost, double headingCost, double steerRateCost);
 void init_weights(double pathCost, double headingCost, double steerRateCost);
 int run_mpc(state_t * x0, log_t * solution,
-             double v_poly[4], double curvature_factor, double rotation_radius,
+             double v_ego, double curvature_factor, double rotation_radius,
              double target_y[N+1], double target_psi[N+1]);
 """)
 

selfdrive/controls/lib/pathplanner.py

@@ -71,35 +71,32 @@ class PathPlanner():
     self.cur_state[0].x = 0.0
     self.cur_state[0].y = 0.0
     self.cur_state[0].psi = 0.0
-    self.cur_state[0].delta = 0.0
+    self.cur_state[0].tire_angle = 0.0
 
     self.angle_steers_des = 0.0
     self.angle_steers_des_mpc = 0.0
-    self.angle_steers_des_prev = 0.0
     self.angle_steers_des_time = 0.0
 
   def update(self, sm, pm, CP, VM):
     v_ego = sm['carState'].vEgo
-    angle_steers = sm['carState'].steeringAngle
     active = sm['controlsState'].active
+    steering_wheel_angle_offset_deg = sm['liveParameters'].angleOffset
+    steering_wheel_angle_deg = sm['carState'].steeringAngle
+    measured_tire_angle = -math.radians(steering_wheel_angle_deg - steering_wheel_angle_offset_deg) / VM.sR
 
-    angle_offset = sm['liveParameters'].angleOffset
-
-    # Run MPC
-    self.angle_steers_des_prev = self.angle_steers_des_mpc
 
     # Update vehicle model
     x = max(sm['liveParameters'].stiffnessFactor, 0.1)
     sr = max(sm['liveParameters'].steerRatio, 0.1)
     VM.update_params(x, sr)
-
     curvature_factor = VM.curvature_factor(v_ego)
 
+
     md = sm['modelV2']
     self.LP.parse_model(sm['modelV2'])
     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 = list(md.position.t)
+      self.t_idxs = np.array(md.position.t)
       self.plan_yaw = list(md.orientation.z)
 
     # Lane change logic
@@ -168,16 +165,14 @@ class PathPlanner():
       self.LP.lll_prob *= self.lane_change_ll_prob
       self.LP.rll_prob *= self.lane_change_ll_prob
     d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
-    y_pts = np.interp(self.t_idxs[:MPC_N+1], np.linalg.norm(d_path_xyz, axis=1)/v_ego, d_path_xyz[:,1])
-    heading_pts = np.interp(self.t_idxs[:MPC_N+1], np.linalg.norm(self.path_xyz, axis=1)/v_ego, self.plan_yaw)
+    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)
 
     v_ego_mpc = max(v_ego, 5.0)  # avoid mpc roughness due to low speed
-    v_poly = [0.0, 0.0, 0.0, v_ego_mpc]
-    assert len(v_poly) == 4
     assert len(y_pts) == MPC_N + 1
     assert len(heading_pts) == MPC_N + 1
     self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
-                        v_poly,
+                        float(v_ego_mpc),
                         curvature_factor,
                         CAR_ROTATION_RADIUS,
                         list(y_pts),
@@ -186,30 +181,31 @@ class PathPlanner():
     self.cur_state.x = 0.0
     self.cur_state.y = 0.0
     self.cur_state.psi = 0.0
-    self.cur_state.delta = interp(DT_MDL, self.t_idxs[:MPC_N+1], self.mpc_solution.delta)
+    self.cur_state.tire_angle = interp(DT_MDL, self.t_idxs[:MPC_N+1], self.mpc_solution.tire_angle)
 
     # TODO this needs more thought, use .2s extra for now to estimate other delays
     delay = CP.steerActuatorDelay + .2
-    # TODO negative sign, still run mpc in ENU, make NED
-    next_delta = -interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.delta)
-    next_rate = -interp(delay, self.t_idxs[:MPC_N], self.mpc_solution.rate)
+    next_tire_angle = interp(DT_MDL + delay, self.t_idxs[:MPC_N+1], self.mpc_solution.tire_angle)
+    next_tire_angle_rate = interp(delay, self.t_idxs[:MPC_N], self.mpc_solution.tire_angle_rate)
 
     # reset to current steer angle if not active or overriding
     if active:
-      delta_desired = next_delta
-      rate_desired = math.degrees(next_rate * VM.sR)
+      tire_angle_desired = next_tire_angle
+      desired_tire_angle_rate = next_tire_angle_rate
     else:
-      delta_desired = math.radians(angle_steers - angle_offset) / VM.sR
-      rate_desired = 0.0
+      tire_angle_desired = measured_tire_angle
+      desired_tire_angle_rate = 0.0
 
-    self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset)
+    # negative sign, controls uses different convention
+    self.desired_steering_wheel_angle_deg = -float(math.degrees(tire_angle_desired * VM.sR)) + steering_wheel_angle_offset_deg
+    self.desired_steering_wheel_angle_rate_deg = -float(math.degrees(desired_tire_angle_rate * VM.sR))
 
     #  Check for infeasable MPC solution
-    mpc_nans = any(math.isnan(x) for x in self.mpc_solution.delta)
+    mpc_nans = any(math.isnan(x) for x in self.mpc_solution.tire_angle)
     t = sec_since_boot()
     if mpc_nans:
       self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost)
-      self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR
+      self.cur_state.tire_angle = measured_tire_angle
 
       if t > self.last_cloudlog_t + 5.0:
         self.last_cloudlog_t = t
@@ -223,14 +219,13 @@ class PathPlanner():
     plan_send = messaging.new_message('pathPlan')
     plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'modelV2'])
     plan_send.pathPlan.laneWidth = float(self.LP.lane_width)
-    plan_send.pathPlan.dPoly = [0,0,0,0]
-    plan_send.pathPlan.lPoly = [0,0,0,0]
-    plan_send.pathPlan.rPoly = [0,0,0,0]
+    plan_send.pathPlan.dPathPoints = [float(x) for x in y_pts]
     plan_send.pathPlan.lProb = float(self.LP.lll_prob)
     plan_send.pathPlan.rProb = float(self.LP.rll_prob)
+    plan_send.pathPlan.dProb = float(self.LP.d_prob)
 
-    plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
-    plan_send.pathPlan.rateSteers = float(rate_desired)
+    plan_send.pathPlan.angleSteers = float(self.desired_steering_wheel_angle_deg)
+    plan_send.pathPlan.rateSteers = float(self.desired_steering_wheel_angle_rate_deg)
     plan_send.pathPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage)
     plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid)
     plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid)
@@ -246,6 +241,6 @@ class PathPlanner():
       dat.liveMpc.x = list(self.mpc_solution[0].x)
       dat.liveMpc.y = list(self.mpc_solution[0].y)
       dat.liveMpc.psi = list(self.mpc_solution[0].psi)
-      dat.liveMpc.delta = list(self.mpc_solution[0].delta)
+      dat.liveMpc.tire_angle = list(self.mpc_solution[0].tire_angle)
       dat.liveMpc.cost = self.mpc_solution[0].cost
       pm.send('liveMpc', dat)

selfdrive/controls/tests/test_lateral_mpc.py

@@ -6,7 +6,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel
 from selfdrive.controls.lib.drive_helpers import MPC_N, CAR_ROTATION_RADIUS
 
 
-def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
+def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., tire_angle_init=0.,
             lane_width=3.6, poly_shift=0.):
 
   libmpc = libmpc_py.libmpc
@@ -26,11 +26,11 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
   cur_state.x = x_init
   cur_state.y = y_init
   cur_state.psi = psi_init
-  cur_state.delta = delta_init
+  cur_state.tire_angle = tire_angle_init
 
   # converge in no more than 20 iterations
   for _ in range(20):
-    libmpc.run_mpc(cur_state, mpc_solution, [0,0,0,v_ref],
+    libmpc.run_mpc(cur_state, mpc_solution, v_ref,
                    curvature_factor, CAR_ROTATION_RADIUS,
                    list(y_pts), list(heading_pts))
 
@@ -39,26 +39,26 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0.,
 
 class TestLateralMpc(unittest.TestCase):
 
-  def _assert_null(self, sol, delta=1e-6):
+  def _assert_null(self, sol, tire_angle=1e-6):
     for i in range(len(sol[0].y)):
-      self.assertAlmostEqual(sol[0].y[i], 0., delta=delta)
-      self.assertAlmostEqual(sol[0].psi[i], 0., delta=delta)
-      self.assertAlmostEqual(sol[0].delta[i], 0., delta=delta)
+      self.assertAlmostEqual(sol[0].y[i], 0., delta=tire_angle)
+      self.assertAlmostEqual(sol[0].psi[i], 0., delta=tire_angle)
+      self.assertAlmostEqual(sol[0].tire_angle[i], 0., delta=tire_angle)
 
-  def _assert_simmetry(self, sol, delta=1e-6):
+  def _assert_simmetry(self, sol, tire_angle=1e-6):
     for i in range(len(sol[0][0].y)):
-      self.assertAlmostEqual(sol[0][0].y[i], -sol[1][0].y[i], delta=delta)
-      self.assertAlmostEqual(sol[0][0].psi[i], -sol[1][0].psi[i], delta=delta)
-      self.assertAlmostEqual(sol[0][0].delta[i], -sol[1][0].delta[i], delta=delta)
-      self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=delta)
+      self.assertAlmostEqual(sol[0][0].y[i], -sol[1][0].y[i], delta=tire_angle)
+      self.assertAlmostEqual(sol[0][0].psi[i], -sol[1][0].psi[i], delta=tire_angle)
+      self.assertAlmostEqual(sol[0][0].tire_angle[i], -sol[1][0].tire_angle[i], delta=tire_angle)
+      self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=tire_angle)
 
-  def _assert_identity(self, sol, ignore_y=False, delta=1e-6):
+  def _assert_identity(self, sol, ignore_y=False, tire_angle=1e-6):
     for i in range(len(sol[0][0].y)):
-      self.assertAlmostEqual(sol[0][0].psi[i], sol[1][0].psi[i], delta=delta)
-      self.assertAlmostEqual(sol[0][0].delta[i], sol[1][0].delta[i], delta=delta)
-      self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=delta)
+      self.assertAlmostEqual(sol[0][0].psi[i], sol[1][0].psi[i], delta=tire_angle)
+      self.assertAlmostEqual(sol[0][0].tire_angle[i], sol[1][0].tire_angle[i], delta=tire_angle)
+      self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=tire_angle)
       if not ignore_y:
-        self.assertAlmostEqual(sol[0][0].y[i], sol[1][0].y[i], delta=delta)
+        self.assertAlmostEqual(sol[0][0].y[i], sol[1][0].y[i], delta=tire_angle)
 
   def test_straight(self):
     sol = run_mpc()
@@ -76,10 +76,10 @@ class TestLateralMpc(unittest.TestCase):
       sol.append(run_mpc(poly_shift=poly_shift))
     self._assert_simmetry(sol)
 
-  def test_delta_symmetry(self):
+  def test_tire_angle_symmetry(self):
     sol = []
-    for delta_init in [-0.1, 0.1]:
-      sol.append(run_mpc(delta_init=delta_init))
+    for tire_angle_init in [-0.1, 0.1]:
+      sol.append(run_mpc(tire_angle_init=tire_angle_init))
     self._assert_simmetry(sol)
 
   def test_psi_symmetry(self):
@@ -93,9 +93,9 @@ class TestLateralMpc(unittest.TestCase):
     sol = []
     sol.append(run_mpc(y_init=shift))
     sol.append(run_mpc(poly_shift=-shift))
-    # need larger delta than standard, otherwise it false triggers.
+    # need larger tire_angle than standard, otherwise it false triggers.
     # this is acceptable because the 2 cases are very different from the optimizer standpoint
-    self._assert_identity(sol, ignore_y=True, delta=1e-5)
+    self._assert_identity(sol, ignore_y=True, tire_angle=1e-5)
 
   def test_no_overshoot(self):
     y_init = 1.

selfdrive/debug/mpc/live_lateral_mpc.py

@@ -75,7 +75,7 @@ def mpc_vwr_thread(addr="127.0.0.1"):
       r_path_y = np.polyval(l_poly, path_x)
 
     if pp is not None:
-      p_path_y = np.polyval(pp.pathPlan.dPoly, path_x)
+      p_path_y = np.polyval(pp.pathPlan.dPolyDEPRECATED, path_x)
       lineP.set_xdata(p_path_y)
       lineP.set_ydata(path_x)
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-02806ead12deaceb9379bfcd94d79d0346450e5b
+9ffa2b9927f188bdc07be62b2cc4ee00e5632522

tools/replay/ui.py

@@ -184,7 +184,7 @@ def ui_thread(addr, frame_address):
     if len(sm['model'].path.poly) > 0:
       model_data = extract_model_data(sm['model'])
       plot_model(model_data, VM, sm['controlsState'].vEgo, sm['controlsState'].curvature, imgw, calibration,
-                  top_down, np.array(sm['pathPlan'].dPoly))
+                  top_down, np.array(sm['pathPlan'].dPolyDEPRECATED))
 
     # MPC
     if sm.updated['liveMpc']: