e2e long rebased

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -3,7 +3,7 @@
 import numpy as np
 
 from common.realtime import sec_since_boot
-from common.numpy_fast import clip, interp
+from common.numpy_fast import interp, clip
 from selfdrive.swaglog import cloudlog
 from selfdrive.modeld.constants import index_function
 from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
@@ -35,7 +35,8 @@
 A_EGO_COST = 0.
 J_EGO_COST = 5.0
 A_CHANGE_COST = .5
-DANGER_ZONE_COST = 100.
+J_EGO_COST = 5.
+DANGER_ZONE_COST = 10.
 CRASH_DISTANCE = .5
 LIMIT_COST = 1e6
 
@@ -228,12 +229,10 @@ def set_weights(self):
       self.set_weights_for_lead_policy()
 
   def set_weights_for_lead_policy(self):
-    W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, A_CHANGE_COST, J_EGO_COST]))
+    W = np.diag([0., .03, .0, 10., 0.0, 1.])
     for i in range(N):
-      W[4,4] = A_CHANGE_COST * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
+      W[4,4] = .1 * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
       self.solver.cost_set(i, 'W', W)
-    # Setting the slice without the copy make the array not contiguous,
-    # causing issues with the C interface.
     self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
 
     # Set L2 slack cost on lower bound constraints
@@ -242,11 +241,9 @@ def set_weights_for_lead_policy(self):
       self.solver.cost_set(i, 'Zl', Zl)
 
   def set_weights_for_xva_policy(self):
-    W = np.asfortranarray(np.diag([0., 10., 1., 10., 0.0, 1.]))
+    W = np.diag([0., 0., .0, 1., 0.0, 1.])
     for i in range(N):
       self.solver.cost_set(i, 'W', W)
-    # Setting the slice without the copy make the array not contiguous,
-    # causing issues with the C interface.
     self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
 
     # Set L2 slack cost on lower bound constraints
@@ -299,15 +296,18 @@ def set_accel_limits(self, min_a, max_a):
     self.cruise_max_a = max_a
 
   def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False):
-    v_ego = self.x0[1]
+    #v_ego = self.x0[1]
     a_ego = self.x0[2]
+    self.yref[:,1] = x
+    self.yref[:,2] = v
+    self.yref[:,3] = a
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
     lead_xv_0 = self.process_lead(radarstate.leadOne)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)
 
     # set accel limits in params
-    self.params[:,0] = interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL])
+    self.params[:,0] = self.cruise_min_a
     self.params[:,1] = self.cruise_max_a
 
     # To estimate a safe distance from a moving lead, we calculate how much stopping
@@ -316,22 +316,23 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False):
     lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
     lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
 
-    # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
-    # when the leads are no factor.
-    v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05)
-    v_upper = v_ego + (T_IDXS * self.cruise_max_a * 1.05)
-    v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
-                               v_lower,
-                               v_upper)
-    cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped)
-
-    x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
-    self.source = SOURCES[np.argmin(x_obstacles[0])]
-    self.params[:,2] = np.min(x_obstacles, axis=1)
+    cruise_target = T_IDXS * v_cruise + x[0]
+
+    x_targets = np.column_stack([x,
+                                lead_0_obstacle - (3/4) * get_safe_obstacle_distance(v),
+                                lead_1_obstacle - (3/4) * get_safe_obstacle_distance(v),
+                                cruise_target])
+    #self.source = SOURCES[np.argmin(x_obstacles[0])]
+    self.params[:,2] = 1e3
     if prev_accel_constraint:
-      self.params[:,3] = np.copy(self.prev_a)
+      self.params[:, 3] = np.copy(self.prev_a)
     else:
-      self.params[:,3] = a_ego
+      self.params[:, 3] = a_ego
+
+    self.yref[:,1] = np.min(x_targets, axis=1)
+    for i in range(N):
+      self.solver.set(i, "yref", self.yref[i])
+    self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
 
     self.run()
     if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and

selfdrive/controls/lib/longitudinal_planner.py

@@ -87,17 +87,27 @@ def update(self, sm):
     # clip limits, cannot init MPC outside of bounds
     accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
-    self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
+    self.mpc.set_accel_limits(-3.5, 2.)
     self.mpc.set_cur_state(self.v_desired, self.a_desired)
-    self.mpc.update(sm['carState'], sm['radarState'], v_cruise, prev_accel_constraint=prev_accel_constraint)
+    if (len(sm['modelV2'].position.x) == 33 and
+         len(sm['modelV2'].velocity.x) == 33 and
+          len(sm['modelV2'].acceleration.x) == 33):
+      x = np.interp(T_IDXS_MPC, T_IDXS, sm['modelV2'].position.x)
+      v = np.interp(T_IDXS_MPC, T_IDXS, sm['modelV2'].velocity.x)
+      a = np.interp(T_IDXS_MPC, T_IDXS, sm['modelV2'].acceleration.x)
+    else:
+      x = np.zeros(len(T_IDXS_MPC))
+      v = np.zeros(len(T_IDXS_MPC))
+      a = np.zeros(len(T_IDXS_MPC))
+    self.mpc.update(sm['carState'], sm['radarState'], v_cruise, x, v, a)
     self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)
     self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
 
-    # TODO counter is only needed because radar is glitchy, remove once radar is gone
-    self.fcw = self.mpc.crash_cnt > 5
-    if self.fcw:
-      cloudlog.info("FCW triggered")
+    #TODO counter is only needed because radar is glitchy, remove once radar is gone
+    self.fcw = False #self.mpc.crash_cnt > 5
+    #if self.fcw:
+    #  cloudlog.info("FCW triggered")
 
     # Interpolate 0.05 seconds and save as starting point for next iteration
     a_prev = self.a_desired
@@ -118,7 +128,7 @@ def publish(self, sm, pm):
     longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory]
 
     longitudinalPlan.hasLead = sm['radarState'].leadOne.status
-    longitudinalPlan.longitudinalPlanSource = self.mpc.source
+    #longitudinalPlan.longitudinalPlanSource = self.mpc.source
     longitudinalPlan.fcw = self.fcw
 
     pm.send('longitudinalPlan', plan_send)

selfdrive/modeld/models/driving.cc

@@ -182,6 +182,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic
   std::array<float, TRAJECTORY_SIZE> pos_x, pos_y, pos_z;
   std::array<float, TRAJECTORY_SIZE> pos_x_std, pos_y_std, pos_z_std;
   std::array<float, TRAJECTORY_SIZE> vel_x, vel_y, vel_z;
+  std::array<float, TRAJECTORY_SIZE> accel_x, accel_y, accel_z;
   std::array<float, TRAJECTORY_SIZE> rot_x, rot_y, rot_z;
   std::array<float, TRAJECTORY_SIZE> rot_rate_x, rot_rate_y, rot_rate_z;
 
@@ -195,6 +196,9 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic
     vel_x[i] = plan.mean[i].velocity.x;
     vel_y[i] = plan.mean[i].velocity.y;
     vel_z[i] = plan.mean[i].velocity.z;
+    accel_x[i] = plan.mean[i].acceleration.x;
+    accel_y[i] = plan.mean[i].acceleration.y;
+    accel_z[i] = plan.mean[i].acceleration.z;
     rot_x[i] = plan.mean[i].rotation.x;
     rot_y[i] = plan.mean[i].rotation.y;
     rot_z[i] = plan.mean[i].rotation.z;
@@ -205,6 +209,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPredic
 
   fill_xyzt(framed.initPosition(), T_IDXS_FLOAT, pos_x, pos_y, pos_z, pos_x_std, pos_y_std, pos_z_std);
   fill_xyzt(framed.initVelocity(), T_IDXS_FLOAT, vel_x, vel_y, vel_z);
+  fill_xyzt(framed.initAcceleration(), T_IDXS_FLOAT, accel_x, accel_y, accel_z);
   fill_xyzt(framed.initOrientation(), T_IDXS_FLOAT, rot_x, rot_y, rot_z);
   fill_xyzt(framed.initOrientationRate(), T_IDXS_FLOAT, rot_rate_x, rot_rate_y, rot_rate_z);
 }