Revert torque control (#24565)

* torque reversal start

* Fix carmodel tests

* Update ref

* update ref

* Elif is better than if

release/files_common

@@ -168,6 +168,7 @@ selfdrive/controls/lib/events.py
 selfdrive/controls/lib/lane_planner.py
 selfdrive/controls/lib/latcontrol_angle.py
 selfdrive/controls/lib/latcontrol_indi.py
+selfdrive/controls/lib/latcontrol_lqr.py
 selfdrive/controls/lib/latcontrol_torque.py
 selfdrive/controls/lib/latcontrol_pid.py
 selfdrive/controls/lib/latcontrol.py

selfdrive/car/tests/test_car_interfaces.py

@@ -38,6 +38,8 @@ class TestCarInterfaces(unittest.TestCase):
       tuning = car_params.lateralTuning.which()
       if tuning == 'pid':
         self.assertTrue(len(car_params.lateralTuning.pid.kpV))
+      elif tuning == 'lqr':
+        self.assertTrue(len(car_params.lateralTuning.lqr.a))
       elif tuning == 'torque':
         self.assertTrue(car_params.lateralTuning.torque.kf > 0)
       elif tuning == 'indi':

selfdrive/car/tests/test_models.py

@@ -116,6 +116,8 @@ class TestCarModel(unittest.TestCase):
         self.assertTrue(len(self.CP.lateralTuning.pid.kpV))
       elif tuning == 'torque':
         self.assertTrue(self.CP.lateralTuning.torque.kf > 0)
+      elif tuning == 'lqr':
+        self.assertTrue(len(self.CP.lateralTuning.lqr.a))
       elif tuning == 'indi':
         self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV))
       else:

selfdrive/car/toyota/interface.py

@@ -47,7 +47,7 @@ class CarInterface(CarInterfaceBase):
       ret.steerRatio = 17.4
       tire_stiffness_factor = 0.5533
       ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG
-      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06)
+      set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
 
     elif candidate in (CAR.RAV4, CAR.RAV4H):
       stop_and_go = True if (candidate in CAR.RAV4H) else False
@@ -55,7 +55,7 @@ class CarInterface(CarInterfaceBase):
       ret.steerRatio = 16.88   # 14.5 is spec end-to-end
       tire_stiffness_factor = 0.5533
       ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
-      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06)
+      set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
 
     elif candidate == CAR.COROLLA:
       ret.wheelbase = 2.70
@@ -87,9 +87,6 @@ class CarInterface(CarInterfaceBase):
       ret.steerRatio = 13.7
       tire_stiffness_factor = 0.7933
       ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG  # mean between normal and hybrid
-      if candidate in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2):
-        set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.4, FRICTION=0.05)
-      else:
       set_lat_tune(ret.lateralTuning, LatTunes.PID_C)
 
     elif candidate in (CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2):
@@ -139,7 +136,7 @@ class CarInterface(CarInterfaceBase):
       ret.steerRatio = 13.9
       tire_stiffness_factor = 0.444  # not optimized yet
       ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
-      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.0, FRICTION=0.07)
+      set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
 
     elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH):
       stop_and_go = True

selfdrive/car/toyota/tunes.py

@@ -58,6 +58,16 @@ def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1):
     tune.torque.kf = 1.0 / MAX_LAT_ACCEL
     tune.torque.ki = 0.1 / MAX_LAT_ACCEL
     tune.torque.friction = FRICTION
+  elif name == LatTunes.LQR_RAV4:
+    tune.init('lqr')
+    tune.lqr.scale = 1500.0
+    tune.lqr.ki = 0.05
+    tune.lqr.a = [0., 1., -0.22619643, 1.21822268]
+    tune.lqr.b = [-1.92006585e-04, 3.95603032e-05]
+    tune.lqr.c = [1., 0.]
+    tune.lqr.k = [-110.73572306, 451.22718255]
+    tune.lqr.l = [0.3233671, 0.3185757]
+    tune.lqr.dcGain = 0.002237852961363602
   elif name == LatTunes.INDI_PRIUS:
     tune.init('indi')
     tune.indi.innerLoopGainBP = [0.]

selfdrive/controls/controlsd.py

@@ -23,6 +23,7 @@ from selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
 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
@@ -145,6 +146,8 @@ class Controls:
       self.LaC = LatControlPID(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'indi':
       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() == 'torque':
       self.LaC = LatControlTorque(self.CP, self.CI)
 
@@ -747,6 +750,8 @@ class Controls:
       controlsState.lateralControlState.pidState = lac_log
     elif lat_tuning == 'torque':
       controlsState.lateralControlState.torqueState = lac_log
+    elif lat_tuning == 'lqr':
+      controlsState.lateralControlState.lqrState = lac_log
     elif lat_tuning == 'indi':
       controlsState.lateralControlState.indiState = lac_log
 

selfdrive/controls/lib/latcontrol_lqr.py

@@ -0,0 +1,84 @@
+import math
+import numpy as np
+
+from common.numpy_fast import clip
+from common.realtime import DT_CTRL
+from cereal import log
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
+
+
+class LatControlLQR(LatControl):
+  def __init__(self, CP, CI):
+    super().__init__(CP, CI)
+    self.scale = CP.lateralTuning.lqr.scale
+    self.ki = CP.lateralTuning.lqr.ki
+
+    self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2))
+    self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1))
+    self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2))
+    self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2))
+    self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1))
+    self.dc_gain = CP.lateralTuning.lqr.dcGain
+
+    self.x_hat = np.array([[0], [0]])
+    self.i_unwind_rate = 0.3 * DT_CTRL
+    self.i_rate = 1.0 * DT_CTRL
+
+    self.reset()
+
+  def reset(self):
+    super().reset()
+    self.i_lqr = 0.0
+
+  def update(self, active, CS, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
+    lqr_log = log.ControlsState.LateralLQRState.new_message()
+
+    torque_scale = (0.45 + CS.vEgo / 60.0)**2  # Scale actuator model with speed
+
+    # Subtract offset. Zero angle should correspond to zero torque
+    steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg
+
+    desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
+
+    instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
+    desired_angle += instant_offset  # Only add offset that originates from vehicle model errors
+    lqr_log.steeringAngleDesiredDeg = desired_angle
+
+    # Update Kalman filter
+    angle_steers_k = float(self.C.dot(self.x_hat))
+    e = steering_angle_no_offset - angle_steers_k
+    self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
+
+    if CS.vEgo < MIN_STEER_SPEED or not active:
+      lqr_log.active = False
+      lqr_output = 0.
+      output_steer = 0.
+      self.reset()
+    else:
+      lqr_log.active = True
+
+      # LQR
+      u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat))
+      lqr_output = torque_scale * u_lqr / self.scale
+
+      # Integrator
+      if CS.steeringPressed:
+        self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
+      else:
+        error = desired_angle - angle_steers_k
+        i = self.i_lqr + self.ki * self.i_rate * error
+        control = lqr_output + i
+
+        if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \
+           (error <= 0 and (control >= -self.steer_max or i > 0.0)):
+          self.i_lqr = i
+
+      output_steer = lqr_output + self.i_lqr
+      output_steer = clip(output_steer, -self.steer_max, self.steer_max)
+
+    lqr_log.steeringAngleDeg = angle_steers_k
+    lqr_log.i = self.i_lqr
+    lqr_log.output = output_steer
+    lqr_log.lqrOutput = lqr_output
+    lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS)
+    return output_steer, desired_angle, lqr_log

selfdrive/controls/lib/tests/test_latcontrol.py

@@ -9,7 +9,7 @@ from selfdrive.car.honda.values import CAR as HONDA
 from selfdrive.car.toyota.values import CAR as TOYOTA
 from selfdrive.car.nissan.values import CAR as NISSAN
 from selfdrive.controls.lib.latcontrol_pid import LatControlPID
-from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -17,7 +17,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel
 
 class TestLatControl(unittest.TestCase):
 
-  @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlTorque), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
+  @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
   def test_saturation(self, car_name, controller):
     CarInterface, CarController, CarState = interfaces[car_name]
     CP = CarInterface.get_params(car_name)

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-16a3e2f9ef2bf013bc71bed6085a5296eb276f85
+b8c35486e8354713221d4237e97e5abced6f5228