lateral: online feedforward learning

SA_RELEASES.md

@@ -1,6 +1,7 @@
 Stock Additions Update 2 - 2021-08-15 (0.8.8)
 ===
  * Update SA to 0.8.8 with new model from upstream!
+ * Experimental model cruise control button is back with a new model
 
 Stock Additions Update 1 - 2021-08-15 (0.8.7)
 ===

selfdrive/car/toyota/interface.py

@@ -92,11 +92,12 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       ret.minSpeedCan = 0.1 * CV.KPH_TO_MS
       tire_stiffness_factor = 0.444  # not optimized yet
       ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
-
-      ret.lateralTuning.init('model')
-      ret.lateralTuning.model.name = "corolla_model_v5"
-      ret.lateralTuning.model.useRates = False  # TODO: makes model sluggish, see comments in latcontrol_model.py
-      ret.lateralTuning.model.multiplier = 1.
+      ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kpV = [[20, 31], [0.06, 0.12]]  # 45 to 70 mph
+      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[20, 31], [0.001, 0.02]]
+      ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[20, 31], [0.1, 0.2]]
+      ret.lateralTuning.pid.kf = 0.00003  # full torque for 20 deg at 80mph means 0.00007818594
+      # ret.lateralTuning.pid.kf = 0.00006908923778520113  # full torque for 20 deg at 80mph means 0.00007818594
+      ret.lateralTuning.pid.newKfTuned = True
 
     elif candidate == CAR.LEXUS_RX:
       stop_and_go = True

selfdrive/controls/lib/latcontrol_pid.py

@@ -1,5 +1,8 @@
+import importlib
 import math
 
+from common.filter_simple import FirstOrderFilter
+from common.realtime import DT_CTRL
 from selfdrive.controls.lib.pid import LatPIDController
 from selfdrive.controls.lib.drive_helpers import get_steer_max
 from cereal import log
@@ -11,7 +14,12 @@ def __init__(self, CP):
                                 (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
                                 (CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV),
                                 k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, sat_limit=CP.steerLimitTimer)
-    self.new_kf_tuned = CP.lateralTuning.pid.newKfTuned
+    self.new_kf_tuned = True  # CP.lateralTuning.pid.newKfTuned
+    self.kf_filter = FirstOrderFilter(CP.lateralTuning.pid.kf, 10, DT_CTRL)
+
+    self.CarControllerParams = importlib.import_module('selfdrive.car.{}.values'.format(CP.carName)).CarControllerParams
+    assert self.CarControllerParams, 'Missing CarControllerParams!'
+    assert self.CarControllerParams.STEER_MAX != 0, 'Can\'t be 0'
 
   def reset(self):
     self.pid.reset()
@@ -28,6 +36,13 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur
       output_steer = 0.0
       pid_log.active = False
       self.pid.reset()
+
+      if abs(CS.steeringRateDeg) < 20 and 5 < abs(CS.steeringAngleDeg) < 90 and CS.vEgo > 5:
+        torque = CS.steeringTorqueEps / self.CarControllerParams.STEER_MAX
+        predicted_kf = torque / (CS.steeringAngleDeg * CS.vEgo ** 2)
+        self.pid.k_f = self.kf_filter.update(predicted_kf)
+
+        print('PREDICTED KF: {}'.format(self.kf_filter.x))
     else:
       steers_max = get_steer_max(CP, CS.vEgo)
       self.pid.pos_limit = steers_max

selfdrive/controls/lib/lead_mpc.py

@@ -49,7 +49,7 @@ def set_cur_state(self, v, a):
     self.cur_state[0].v_ego = v_safe
     self.cur_state[0].a_ego = a_safe
 
-  def update(self, CS, radarstate, v_cruise):
+  def update(self, CS, radarstate, modelstate, v_cruise):
     v_ego = CS.vEgo
     if self.lead_id == 0:
       lead = radarstate.leadOne

selfdrive/controls/lib/long_mpc.py

@@ -9,18 +9,23 @@
 
 
 class LongitudinalMpc():
-  def __init__(self):
+  def __init__(self, mpc_id):
+    self.mpc_id = mpc_id
+
     self.reset_mpc()
     self.last_cloudlog_t = 0.0
     self.ts = list(range(10))
     self.status = True
-    self.min_a = -1.2
+    self.min_a = -1.2 if self.mpc_id == 0 else -3.5
     self.max_a = 1.2
 
 
   def reset_mpc(self):
     self.libmpc = libmpc_py.libmpc
-    self.libmpc.init(0.0, 1.0, 0.0, 50.0, 10000.0)
+    if self.mpc_id == 0:
+      self.libmpc.init(0.0, 1.0, 0.0, 50.0, 10000.0)
+    else:
+      self.libmpc.init(1.0, 1.0, 0.0, 5.0, 10000.0)
 
     self.mpc_solution = libmpc_py.ffi.new("log_t *")
     self.cur_state = libmpc_py.ffi.new("state_t *")
@@ -45,10 +50,14 @@ def set_cur_state(self, v, a):
     self.cur_state[0].v_ego = v_safe
     self.cur_state[0].a_ego = a_safe
 
-  def update(self, carstate, radarstate, v_cruise):
+  def update(self, carstate, radarstate, modelstate, v_cruise):
     v_cruise_clipped = np.clip(v_cruise, self.cur_state[0].v_ego - 10., self.cur_state[0].v_ego + 10.0)
-    poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1])
-    speeds = v_cruise_clipped * np.ones(LON_MPC_N+1)
+    if self.mpc_id == 0:
+      poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1])
+      speeds = v_cruise_clipped * np.ones(LON_MPC_N+1)
+    else:
+      poss = np.minimum(np.array(modelstate.position.x)[:LON_MPC_N+1], v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1]))
+      speeds = np.minimum(np.array(modelstate.velocity.x)[:LON_MPC_N+1], v_cruise_clipped)
     accels = np.zeros(LON_MPC_N+1)
     self.update_with_xva(poss, speeds, accels)
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -15,8 +15,6 @@
 from selfdrive.controls.lib.long_mpc import LongitudinalMpc
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
 from selfdrive.swaglog import cloudlog
-# from selfdrive.controls.lib.long_mpc_model import LongitudinalMpcModel
-# from common.op_params import opParams
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
 AWARENESS_DECEL = -0.2     # car smoothly decel at .2m/s^2 when user is distracted
@@ -46,44 +44,17 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   return [a_target[0], min(a_target[1], a_x_allowed)]
 
 
-class ModelMpcHelper:
-  def __init__(self):
-    self.model_t = [i ** 2 / 102.4 for i in range(33)]  # the timesteps of the model predictions
-    self.mpc_t = list(range(10))  # the timesteps of what the LongMpcModel class takes in, 1 sec intervels to 10
-    self.model_t_idx = [sorted(range(len(self.model_t)), key=[abs(idx - t) for t in self.model_t].__getitem__)[0] for idx in self.mpc_t]  # matches 0 to 9 interval to idx from t
-    assert len(self.model_t_idx) == 10, 'Needs to be length 10 for mpc'
-
-  def convert_data(self, sm):
-    modelV2 = sm['modelV2']
-    distances, speeds, accelerations = [], [], []
-    if not sm.alive['modelV2'] or len(modelV2.position.x) == 0:
-      return distances, speeds, accelerations
-
-    speeds = [modelV2.velocity.x[t] for t in self.model_t_idx]
-    distances = [modelV2.position.x[t] for t in self.model_t_idx]
-    for t in self.mpc_t:  # todo these three in one loop
-      if 0 < t < 9:
-        accelerations.append((speeds[t + 1] - speeds[t - 1]) / 2)
-
-    # Extrapolate forward and backward at edges
-    accelerations.append(accelerations[-1] - (accelerations[-2] - accelerations[-1]))
-    accelerations.insert(0, accelerations[0] - (accelerations[1] - accelerations[0]))
-    return distances, speeds, accelerations
-
-
 class Planner():
   def __init__(self, CP):
     self.CP = CP
     self.mpcs = {}
     self.mpcs['lead0'] = LeadMpc(0)
     self.mpcs['lead1'] = LeadMpc(1)
-    self.mpcs['cruise'] = LongitudinalMpc()
-    # self.mpcs['model'] = LongitudinalMpcModel()
+    self.mpcs['cruise'] = LongitudinalMpc(0)
+    self.mpcs['e2e'] = LongitudinalMpc(1)
 
     self.fcw = False
     self.fcw_checker = FCWChecker()
-    # self.op_params = opParams()
-    self.model_mpc_helper = ModelMpcHelper()
 
     self.v_desired = 0.0
     self.a_desired = 0.0
@@ -134,9 +105,9 @@ def update(self, sm, CP):
     next_a = np.inf
     for key in self.mpcs:
       self.mpcs[key].set_cur_state(self.v_desired, self.a_desired)
-      self.mpcs[key].update(sm['carState'], sm['radarState'], v_cruise)
-      # TODO: handle model long enabled check
-      if self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a:  # picks slowest solution from accel in ~0.2 seconds
+      self.mpcs[key].update(sm['carState'], sm['radarState'], sm['modelV2'], v_cruise)
+      if (self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a and  # picks slowest solution from accel in ~0.2 seconds
+              ((key == 'e2e' and sm['modelLongButton'].enabled) or key != 'e2e')):
         self.longitudinalPlanSource = key
         self.v_desired_trajectory = self.mpcs[key].v_solution[:CONTROL_N]
         self.a_desired_trajectory = self.mpcs[key].a_solution[:CONTROL_N]

selfdrive/controls/plannerd.py

@@ -38,7 +38,6 @@ def plannerd_thread(sm=None, pm=None):
     if sm.updated['modelV2']:
       lateral_planner.update(sm, CP)
       lateral_planner.publish(sm, pm)
-    if sm.updated['radarState']:
       longitudinal_planner.update(sm, CP)
       longitudinal_planner.publish(sm, pm)
 

selfdrive/controls/tests/test_following_distance.py

@@ -6,6 +6,8 @@
 import cereal.messaging as messaging
 from selfdrive.config import Conversions as CV
 from selfdrive.controls.lib.lead_mpc import LeadMpc
+from selfdrive.controls.lib.drive_helpers import LON_MPC_N
+from selfdrive.modeld.constants import T_IDXS
 
 
 def RW(v_ego, v_l):
@@ -39,7 +41,7 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
     CS.carState.vEgo = v_ego
     CS.carState.aEgo = a_ego
 
-    # Setup model packet
+    # Setup radarstate packet
     radarstate = messaging.new_message('radarState')
     lead = log.RadarState.LeadData.new_message()
     lead.modelProb = .75
@@ -49,12 +51,21 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
     lead.status = True
     radarstate.radarState.leadOne = lead
 
+    # Setup model packet
+    modelstate = messaging.new_message('modelV2')
+    positions = log.ModelDataV2.XYZTData.new_message()
+    velocities = log.ModelDataV2.XYZTData.new_message()
+    positions.x = (v_ego * np.array(T_IDXS[:LON_MPC_N+1])).tolist()
+    velocities.x = (v_ego * np.ones(LON_MPC_N+1)).tolist()
+    modelstate.modelV2.position = positions
+    modelstate.modelV2.velocity = velocities
+
     # Run MPC
     mpc.set_cur_state(v_ego, a_ego)
     if first:  # Make sure MPC is converged on first timestep
       for _ in range(20):
-        mpc.update(CS.carState, radarstate.radarState, 0)
-    mpc.update(CS.carState, radarstate.radarState, 0)
+        mpc.update(CS.carState, radarstate.radarState, modelstate.modelV2, 0)
+    mpc.update(CS.carState, radarstate.radarState, modelstate.modelV2, 0)
 
     # Choose slowest of two solutions
     v_ego, a_ego = mpc.mpc_solution.v_ego[5], mpc.mpc_solution.a_ego[5]

selfdrive/manager/manager.py

@@ -150,7 +150,7 @@ def manager_thread():
 
     running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
                     for p in managed_processes.values() if p.proc]
-    cloudlog.debug(' '.join(running_list))
+    # cloudlog.debug(' '.join(running_list))
 
     # send managerState
     msg = messaging.new_message('managerState')

selfdrive/test/process_replay/process_replay.py

@@ -264,8 +264,8 @@ def ublox_rcv_callback(msg):
   ProcessConfig(
     proc_name="plannerd",
     pub_sub={
-      "modelV2": ["lateralPlan"], "radarState": ["longitudinalPlan"],
-      "carState": [], "controlsState": [], 'modelLongButton': [],
+      "modelV2": ["lateralPlan", "longitudinalPlan"],
+      "carState": [], "controlsState": [], "radarState": [],
     },
     ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay"],
     init_callback=get_car_params,

selfdrive/ui/qt/onroad.cc

@@ -113,27 +113,26 @@ ButtonsWindow::ButtonsWindow(QWidget *parent) : QWidget(parent) {
   QWidget *btns_wrapper = new QWidget;
   QHBoxLayout *btns_layout  = new QHBoxLayout(btns_wrapper);
   btns_layout->setSpacing(0);
-  btns_layout->setContentsMargins(0, 0, 30, 30);
+  btns_layout->setContentsMargins(30, 0, 30, 30);
 
   main_layout->addWidget(btns_wrapper, 0, Qt::AlignBottom);
 
-//  mlButton = new QPushButton("Toggle Model Long");
-//  mlButton->setStyleSheet("font-size: 50px; border-radius: 25px; border-color: #b83737;");
-//  QObject::connect(mlButton, &QPushButton::clicked, [=]() {
-//    mlButton->setStyleSheet("font-size: 50px; border-radius: 25px; border-color: #37b868;");
-//  });
-//  mlButton->setFixedWidth(525);
-//  mlButton->setFixedHeight(150);
-//  btns_layout->addStretch();
-//  btns_layout->addWidget(mlButton, 0, Qt::AlignCenter);
+  mlButton = new QPushButton("Model Cruise Control");
+  QObject::connect(mlButton, &QPushButton::clicked, [=]() {
+    QUIState::ui_state.scene.mlButtonEnabled = !mlEnabled;
+  });
+  mlButton->setFixedWidth(575);
+  mlButton->setFixedHeight(150);
+  btns_layout->addStretch(4);
+  btns_layout->addWidget(mlButton, 0, Qt::AlignHCenter | Qt::AlignBottom);
+  btns_layout->addStretch(3);
 
   dfButton = new QPushButton("DF\nprofile");
   QObject::connect(dfButton, &QPushButton::clicked, [=]() {
     QUIState::ui_state.scene.dfButtonStatus = dfStatus < 3 ? dfStatus + 1 : 0;  // wrap back around
   });
   dfButton->setFixedWidth(200);
   dfButton->setFixedHeight(200);
-//  btns_layout->addStretch();
   btns_layout->addWidget(dfButton, 0, Qt::AlignRight);
 
   setStyleSheet(R"(
@@ -149,20 +148,25 @@ ButtonsWindow::ButtonsWindow(QWidget *parent) : QWidget(parent) {
 }
 
 void ButtonsWindow::updateState(const UIState &s) {
-  updateDfButton(s.scene.dfButtonStatus);  // update dynamic follow profile button
-//  updateMlButton(s.scene.dfButtonStatus);  // update model longitudinal button  // TODO: add model long back first
-}
-
-void ButtonsWindow::updateDfButton(int status) {
-  if (dfStatus != status) {  // updates (resets) on car start, or on button press
-    dfStatus = status;
+  if (dfStatus != s.scene.dfButtonStatus) {  // update dynamic follow profile button
+    dfStatus = s.scene.dfButtonStatus;
     dfButton->setStyleSheet(QString("font-size: 45px; border-radius: 100px; border-color: %1").arg(dfButtonColors.at(dfStatus)));
 
     MessageBuilder msg;
     auto dfButtonStatus = msg.initEvent().initDynamicFollowButton();
     dfButtonStatus.setStatus(dfStatus);
     QUIState::ui_state.pm->send("dynamicFollowButton", msg);
   }
+
+  if (mlEnabled != s.scene.mlButtonEnabled) {  // update model longitudinal button
+    mlEnabled = s.scene.mlButtonEnabled;
+    mlButton->setStyleSheet(QString("font-size: 50px; border-radius: 25px; border-color: %1").arg(mlButtonColors.at(mlEnabled)));
+
+    MessageBuilder msg;
+    auto mlButtonEnabled = msg.initEvent().initModelLongButton();
+    mlButtonEnabled.setEnabled(mlEnabled);
+    QUIState::ui_state.pm->send("modelLongButton", msg);
+  }
 }
 
 void OnroadAlerts::updateAlert(const Alert &a, const QColor &color) {

selfdrive/ui/qt/onroad.h

@@ -21,11 +21,15 @@ class ButtonsWindow : public QWidget {
 
 private:
   QPushButton *dfButton;
-//  QPushButton *mlButton;
+  QPushButton *mlButton;
 
+  // dynamic follow button
   int dfStatus = -1;  // always initialize style sheet and send msg
   const QStringList dfButtonColors = {"#044389", "#24a8bc", "#fcff4b", "#37b868"};
-  void updateDfButton(int status);
+
+  // model long button
+  bool mlEnabled = true;  // triggers initialization
+  const QStringList mlButtonColors = {"#b83737", "#37b868"};
 
 public slots:
   void updateState(const UIState &s);

selfdrive/ui/ui.h

@@ -102,7 +102,7 @@ typedef struct UIScene {
 
   int dfButtonStatus = 0;
   int lsButtonStatus = 0;
-  bool mlButtonEnabled;
+  bool mlButtonEnabled = false;
 
   mat3 view_from_calib;
   bool world_objects_visible;