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);
 }