add latcontrol_steer_model

selfdrive/car/toyota/tunes.py

@@ -24,6 +24,7 @@ class LatTunes(Enum):
   PID_L = 13
   PID_M = 14
   PID_N = 15
+  STEER_MODEL = 16
 
 
 ###### LONG ######
@@ -72,6 +73,13 @@ def set_lat_tune(tune, name):
     tune.lqr.l = [0.3233671, 0.3185757]
     tune.lqr.dcGain = 0.002237852961363602
 
+  elif name == LatTunes.STEER_MODEL:
+    tune.init('steerModel')
+    tune.steerModel.modelparam = [-1.49630374e-01, -1.36092176e+00, 7.05508965e-01, -2.59304574e+00,
+                                    2.78625170e+00, 2.53506728e+01, -2.00322650e-01, -1.82360024e+00,
+                                    7.23304857e-03, 6.56997797e-02, -2.13578545e-04, -1.93872059e-03,
+                                    5.05738269e-03, 4.60615905e-02]
+
   elif 'PID' in str(name):
     tune.init('pid')
     tune.pid.kiBP = [0.0]

selfdrive/controls/controlsd.py

@@ -21,6 +21,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_angle import LatControlAngle
+from selfdrive.controls.lib.latcontrol_steer_model import LatControlSteerModel
 from selfdrive.controls.lib.events import Events, ET
 from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
 from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -133,6 +134,8 @@ class Controls:
       self.LaC = LatControlINDI(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'lqr':
       self.LaC = LatControlLQR(self.CP, self.CI)
+    elif self.CP.lateralTuning.which() == 'steerModel':
+      self.LaC = LatControlSteerModel(self.CP, self.CI)
 
     self.initialized = False
     self.state = State.disabled

selfdrive/controls/lib/latcontrol_steer_model.py

@@ -0,0 +1,80 @@
+import math
+import numpy as np
+
+from selfdrive.controls.lib.drive_helpers import get_steer_max
+from cereal import log
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
+from common.realtime import DT_CTRL
+
+NX = 2
+NU = 1
+N_live_param = 4
+
+# TODO:
+# - update model structure?
+# - get angle measurements sometimes?
+
+
+class LatControlSteerModel(LatControl):
+  def __init__(self, CP, CI):
+    super().__init__(CP, CI)
+
+    self.xcurrent = np.zeros((NX, ))
+    self.torque = 0.0
+
+    model_param = np.asarray(list(CP.lateralTuning.steerModel.modelparam))
+
+    self.A = model_param[0:NX*NX].reshape((NX, NX), order='F')
+    self.B = model_param[NX*NX:NX*(NX+NU)].reshape((NX, NU), order='F')
+    self.R = model_param[NX*(NX+NU):].reshape((NX, N_live_param), order='F')
+    self.M = - np.linalg.solve(self.B.T @ self.B, self.B.T)
+    self.B = self.B.reshape((NX,))
+    # CP.lateralTuning.steerModel.?
+    self.get_steer_feedforward = CI.get_steer_feedforward_function()
+
+
+  def reset(self):
+    # when is this called? only in if below?
+    super().reset()
+    self.xcurrent = np.zeros((NX, ))
+
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+    model_log = log.ControlsState.LateralSteerModelState.new_message()
+
+    angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
+    angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
+
+    # live_param_list = ['roll', 'speeds', 'speed_squared', 'speed_times_roll']
+    live_param = np.array([params.roll, CS.vEgo, CS.vEgo ** 2, CS.vEgo * params.roll])
+
+    model_log.steeringAngleDesiredDeg = angle_steers_des
+
+    if CS.vEgo < MIN_STEER_SPEED or not active:
+      output_steer = 0.0
+      model_log.active = False
+      self.reset()
+    else:
+      steers_max = get_steer_max(CP, CS.vEgo)
+      # self.pid.pos_limit = steers_max
+      # self.pid.neg_limit = -steers_max
+
+      # torque = argmin norm(xcurrent + DT_CTRL * (A*x + R*live_param + B*u)
+      # analytical solution similar to steady state.
+      # offset does not contribute to resistive torque
+      output_steer = self.M @ (self.A @ self.xcurrent + self.R @ live_param +
+                (self.xcurrent - np.array([angle_steers_des_no_offset, self.xcurrent[1,]]))/DT_CTRL )
+
+      # TODO: needed?
+      # steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
+
+      # update state estimate with forward simulation
+      self.xcurrent = self.xcurrent + DT_CTRL * ((self.A @ self.xcurrent) + (self.B * output_steer) + (self.R @ live_param))
+      self.torque = float(output_steer[0])
+
+      model_log.active = True
+
+    model_log.steeringAngleDeg = float(self.xcurrent[0])
+    model_log.output = self.torque
+    model_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
+
+    return output_steer, angle_steers_des, model_log