Add new ML-based lateral controller (commaai#447)

* add model steering controller

* add comment

* add model name field

* add parameter to enable the model

* update releases

* update releases

* fixes

SA_RELEASES.md

@@ -2,6 +2,7 @@ Stock Additions Update 1 - 2021-08-XX (0.8.7)
 ===
  * Update SA to 0.8.7!
  * Variable longitudinal lag by speed
+ * Toyota: add ML-based steering controller enabled by parameter `use_steering_model`
 
 Stock Additions Update 3 - 2021-05-03 (0.8.2)
 ===

common/op_params.py

@@ -131,6 +131,8 @@ def __init__(self):
                         'prius_use_pid': Param(False, bool, 'This enables the PID lateral controller with new a experimental derivative tune\n'
                                                             'False: stock INDI, True: TSS2-tuned PID', static=True),
                         'use_lqr': Param(False, bool, 'Enable this to use LQR as your lateral controller over default with any car', static=True),
+                        'use_steering_model': Param(False, bool, 'Enable this to use an experimental ML-based lateral controller trained on the TSSP Corolla\n'
+                                                                 'Warning: the model may behave unexpectedly at any time, so always pay attention', static=True),
                         'corollaTSS2_use_indi': Param(False, bool, 'Enable this to use INDI for lat with your TSS2 Corolla', static=True),
                         'rav4TSS2_use_indi': Param(False, bool, 'Enable this to use INDI for lat with your TSS2 RAV4', static=True),
                         'standstill_hack': Param(False, bool, 'Some cars support stop and go, you just need to enable this', static=True)}

selfdrive/car/toyota/interface.py

@@ -20,6 +20,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
 
     op_params = opParams()
     use_lqr = op_params.get('use_lqr')
+    use_steering_model = op_params.get('use_steering_model')
     prius_use_pid = op_params.get('prius_use_pid')
     corollaTSS2_use_indi = op_params.get('corollaTSS2_use_indi')
     rav4TSS2_use_indi = op_params.get('rav4TSS2_use_indi')
@@ -34,11 +35,11 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
 
     ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
 
-    CARS_NOT_PID = [CAR.RAV4, CAR.RAV4H]
+    CARS_NOT_PID = [CAR.RAV4, CAR.RAV4H, CAR.COROLLA]
     if not prius_use_pid:
       CARS_NOT_PID.append(CAR.PRIUS)
 
-    if candidate not in CARS_NOT_PID and not use_lqr:  # These cars use LQR/INDI
+    if candidate not in CARS_NOT_PID and not use_lqr and not use_steering_model:  # These cars use LQR/INDI
       ret.lateralTuning.init('pid')
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
 
@@ -90,12 +91,10 @@ 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.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
+
+      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
 
     elif candidate == CAR.LEXUS_RX:
       stop_and_go = True
@@ -350,6 +349,11 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # py
       ret.lateralTuning.lqr.l = [0.3233671, 0.3185757]
       ret.lateralTuning.lqr.dcGain = 0.002237852961363602
 
+    elif use_steering_model:
+      ret.lateralTuning.init('model')
+      ret.lateralTuning.model.name = "corolla_model_v5"
+      ret.lateralTuning.model.useRates = False
+
     ret.centerToFront = ret.wheelbase * 0.44
 
     # TODO: get actual value, for now starting with reasonable value for

selfdrive/controls/controlsd.py

@@ -17,6 +17,7 @@
 from selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
+from selfdrive.controls.lib.latcontrol_model import LatControlModel
 from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from selfdrive.controls.lib.events import Events, ET
 from selfdrive.controls.lib.alertmanager import AlertManager
@@ -133,6 +134,8 @@ def __init__(self, sm=None, pm=None, can_sock=None):
       self.LaC = LatControlINDI(self.CP)
     elif self.CP.lateralTuning.which() == 'lqr':
       self.LaC = LatControlLQR(self.CP)
+    elif self.CP.lateralTuning.which() == 'model':
+      self.LaC = LatControlModel(self.CP)
 
     self.initialized = False
     self.state = State.disabled
@@ -667,6 +670,8 @@ def publish_logs(self, CS, start_time, actuators, lac_log):
       controlsState.lateralControlState.lqrState = lac_log
     elif self.CP.lateralTuning.which() == 'indi':
       controlsState.lateralControlState.indiState = lac_log
+    elif self.CP.lateralTuning.which() == 'model':
+      controlsState.lateralControlState.modelState = lac_log
     self.pm.send('controlsState', dat)
 
     # carState

selfdrive/controls/lib/latcontrol_model.py

@@ -0,0 +1,79 @@
+import math
+import numpy as np
+from common.basedir import BASEDIR
+from selfdrive.controls.lib.drive_helpers import get_steer_max
+from cereal import log
+from common.numpy_fast import clip
+from common.realtime import DT_CTRL
+
+
+class LatControlModel:
+  def __init__(self, CP):
+    # Model generated using Konverter: https://github.com/ShaneSmiskol/Konverter
+    model_weights_file = f'{BASEDIR}/torque_models/{CP.lateralTuning.model.name}_weights.npz'
+    self.w, self.b = np.load(model_weights_file, allow_pickle=True)['wb']
+
+    self.use_rates = CP.lateralTuning.model.useRates
+
+    self.sat_count_rate = 1.0 * DT_CTRL
+    self.sat_limit = CP.steerLimitTimer
+
+  def reset(self):
+    pass
+
+  def _check_saturation(self, control, check_saturation, limit):
+    saturated = abs(control) == limit
+
+    if saturated and check_saturation:
+      self.sat_count += self.sat_count_rate
+    else:
+      self.sat_count -= self.sat_count_rate
+
+    self.sat_count = clip(self.sat_count, 0.0, 1.0)
+
+    return self.sat_count > self.sat_limit
+
+  def predict(self, x):
+    x = np.array(x, dtype=np.float32)
+    l0 = np.dot(x, self.w[0]) + self.b[0]
+    l0 = np.where(l0 > 0, l0, l0 * 0.3)
+    l1 = np.dot(l0, self.w[1]) + self.b[1]
+    l1 = np.where(l1 > 0, l1, l1 * 0.3)
+    l2 = np.dot(l1, self.w[2]) + self.b[2]
+    return l2
+
+  def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate):
+    model_log = log.ControlsState.LateralModelState.new_message()
+    model_log.steeringAngleDeg = float(CS.steeringAngleDeg)
+    model_log.useRates = self.use_rates
+
+    angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
+    angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
+
+    if CS.vEgo < 0.15 or not active:
+      output_steer = 0.0
+      model_log.active = False
+    else:
+      steers_max = get_steer_max(CP, CS.vEgo)
+      pos_limit = steers_max
+      neg_limit = -steers_max
+
+      steering_rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo)
+
+      # TODO: Can be sluggish when model is given rates, the issue is probably with the training data,
+      # specifically the disturbances/perturbations fed into the model to recover from large errors
+      # Basically, figure out a better way to train the model to recover without random samples and using a PF controller as the output
+      rate_des = steering_rate_des if self.use_rates else 0
+      rate = CS.steeringRateDeg if self.use_rates else 0
+      model_input = [angle_steers_des, CS.steeringAngleDeg, rate_des, rate, CS.vEgo]
+
+      output_steer = self.predict(model_input)[0]
+      output_steer = float(clip(output_steer, neg_limit, pos_limit))
+
+      model_log.active = True
+      model_log.output = output_steer
+
+      check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
+      model_log.saturated = self._check_saturation(output_steer, check_saturation, steers_max)
+
+    return output_steer, angle_steers_des, model_log