Lateral torque-based control with roll on TSS2 corolla and TSSP rav4 (#24260)

* Initial commit * Fix bugs * Need more torque rate * Cleanup cray cray control * Write nicely * Chiiil * Not relevant for cray cray control * Do some logging * Seems like it has more torque than I thought * Bit more feedforward * Tune change * Retune * Retune * Little more chill * Add coroll * Add corolla * Give craycray a good name * Update to proper logging * D to the PI * Should be in radians * Add d * Start oscillations * Add D term * Only change torque rate limits for new tune * Add d logging * Should be enough * Wrong sign in D * Downtune a little * Needed to prevent faults * Add lqr rav4 to tune * Try derivative again * Data based retune * Data based retune * add friction compensation * Doesnt need too much P with friction comp * remove lqr * Remove kd * Fix tests * fix tests * Too much error * Get roll induced error under 1cm/deg * Too much jitter * Do roll comp * Add ki * Final update * Update refs * Cleanup latcontrol_torque a little more
2026-02-18 22:23:56 +08:00 · 2022-04-19 19:34:31 -07:00
parent 6877059b45
commit fe0bcdaef6
16 changed files with 120 additions and 123 deletions
--- a/release/files_common
+++ b/release/files_common
@@ -238,7 +238,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/controls/lib/lateral_planner.py
--- a/selfdrive/car/tests/test_car_interfaces.py
+++ b/selfdrive/car/tests/test_car_interfaces.py
@@ -38,8 +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':
        self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV))

--- a/selfdrive/car/tests/test_models.py
+++ b/selfdrive/car/tests/test_models.py
@@ -115,8 +115,8 @@ class TestCarModel(unittest.TestCase):
      tuning = self.CP.lateralTuning.which()
      if tuning == 'pid':
        self.assertTrue(len(self.CP.lateralTuning.pid.kpV))
-      elif tuning == 'lqr':
-        self.assertTrue(len(self.CP.lateralTuning.lqr.a))
+      elif tuning == 'torque':
+        self.assertTrue(self.CP.lateralTuning.torque.kf > 0)
      elif tuning == 'indi':
        self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV))
      else:
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -14,6 +14,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
 class CarController:
  def __init__(self, dbc_name, CP, VM):
    self.CP = CP
+    self.torque_rate_limits = CarControllerParams(self.CP)
    self.frame = 0
    self.last_steer = 0
    self.alert_active = False
@@ -50,7 +51,7 @@ class CarController:

    # steer torque
    new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
-    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams)
+    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits)
    self.steer_rate_limited = new_steer != apply_steer

    # Cut steering while we're in a known fault state (2s)
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -34,7 +34,6 @@ class CarInterface(CarInterfaceBase):
      ret.steerRatio = 15.74   # unknown end-to-end spec
      tire_stiffness_factor = 0.6371   # hand-tune
      ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
-
      set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS)
      ret.steerActuatorDelay = 0.3

@@ -44,7 +43,7 @@ class CarInterface(CarInterfaceBase):
      ret.steerRatio = 17.4
      tire_stiffness_factor = 0.5533
      ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
-      set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE)

    elif candidate in (CAR.RAV4, CAR.RAV4H):
      stop_and_go = True if (candidate in CAR.RAV4H) else False
@@ -52,7 +51,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.LQR_RAV4)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_TORQUE=2.5, FRICTION=0.06)

    elif candidate == CAR.COROLLA:
      ret.wheelbase = 2.70
@@ -133,7 +132,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.PID_D)
+      set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_TORQUE=3.0, FRICTION=0.08)

    elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH):
      stop_and_go = True
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -24,6 +24,7 @@ class LatTunes(Enum):
  PID_L = 13
  PID_M = 14
  PID_N = 15
+  TORQUE = 16


 ###### LONG ######
@@ -49,8 +50,15 @@ def set_long_tune(tune, name):


 ###### LAT ######
-def set_lat_tune(tune, name):
-  if name == LatTunes.INDI_PRIUS:
+def set_lat_tune(tune, name, MAX_TORQUE=2.5, FRICTION=.1):
+  if name == LatTunes.TORQUE:
+    tune.init('torque')
+    tune.torque.useSteeringAngle = True
+    tune.torque.kp = 2.0 / MAX_TORQUE
+    tune.torque.kf = 1.0 / MAX_TORQUE
+    tune.torque.ki = 0.5 / MAX_TORQUE
+    tune.torque.friction = FRICTION
+  elif name == LatTunes.INDI_PRIUS:
    tune.init('indi')
    tune.indi.innerLoopGainBP = [0.]
    tune.indi.innerLoopGainV = [4.0]
@@ -60,18 +68,6 @@ def set_lat_tune(tune, name):
    tune.indi.timeConstantV = [1.0]
    tune.indi.actuatorEffectivenessBP = [0.]
    tune.indi.actuatorEffectivenessV = [1.0]
-
-  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 'PID' in str(name):
    tune.init('pid')
    tune.pid.kiBP = [0.0]
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -18,10 +18,16 @@ class CarControllerParams:
  ACCEL_MIN = -3.5  # m/s2

  STEER_MAX = 1500
-  STEER_DELTA_UP = 10       # 1.5s time to peak torque
-  STEER_DELTA_DOWN = 25     # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
  STEER_ERROR_MAX = 350     # max delta between torque cmd and torque motor

+  def __init__(self, CP):
+    if CP.lateralTuning.which == 'torque':
+      self.STEER_DELTA_UP = 15       # 1.0s time to peak torque
+      self.STEER_DELTA_DOWN = 25     # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
+    else:
+      self.STEER_DELTA_UP = 10       # 1.5s time to peak torque
+      self.STEER_DELTA_DOWN = 25     # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
+

 class ToyotaFlags(IntFlag):
  HYBRID = 1
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -20,8 +20,8 @@ from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature
 from selfdrive.controls.lib.longcontrol import LongControl
 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_torque import LatControlTorque
 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
@@ -144,8 +144,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)

    self.initialized = False
    self.state = State.disabled
@@ -566,7 +566,7 @@ class Controls:
                                                                             lat_plan.curvatureRates)
      actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM,
                                                                             params, self.last_actuators, desired_curvature,
-                                                                             desired_curvature_rate)
+                                                                             desired_curvature_rate, self.sm['liveLocationKalman'])
    else:
      lac_log = log.ControlsState.LateralDebugState.new_message()
      if self.sm.rcv_frame['testJoystick'] > 0:
@@ -730,8 +730,8 @@ class Controls:
      controlsState.lateralControlState.angleState = lac_log
    elif lat_tuning == 'pid':
      controlsState.lateralControlState.pidState = lac_log
-    elif lat_tuning == 'lqr':
-      controlsState.lateralControlState.lqrState = lac_log
+    elif lat_tuning == 'torque':
+      controlsState.lateralControlState.torqueState = lac_log
    elif lat_tuning == 'indi':
      controlsState.lateralControlState.indiState = lac_log

--- a/selfdrive/controls/lib/latcontrol.py
+++ b/selfdrive/controls/lib/latcontrol.py
@@ -16,7 +16,7 @@ class LatControl(ABC):
    self.steer_max = 1.0

  @abstractmethod
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
    pass

  def reset(self):
--- a/selfdrive/controls/lib/latcontrol_angle.py
+++ b/selfdrive/controls/lib/latcontrol_angle.py
@@ -7,7 +7,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees


 class LatControlAngle(LatControl):
-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
    angle_log = log.ControlsState.LateralAngleState.new_message()

    if CS.vEgo < MIN_STEER_SPEED or not active:
--- a/selfdrive/controls/lib/latcontrol_indi.py
+++ b/selfdrive/controls/lib/latcontrol_indi.py
@@ -63,7 +63,7 @@ class LatControlINDI(LatControl):
    self.steer_filter.x = 0.
    self.speed = 0.

-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
    self.speed = CS.vEgo
    # Update Kalman filter
    y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
--- a/selfdrive/controls/lib/latcontrol_lqr.py
+++ b/selfdrive/controls/lib/latcontrol_lqr.py
@@ -1,84 +0,0 @@
-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, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
-    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
--- a/selfdrive/controls/lib/latcontrol_pid.py
+++ b/selfdrive/controls/lib/latcontrol_pid.py
@@ -17,7 +17,7 @@ class LatControlPID(LatControl):
    super().reset()
    self.pid.reset()

-  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
    pid_log = log.ControlsState.LateralPIDState.new_message()
    pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
    pid_log.steeringRateDeg = float(CS.steeringRateDeg)
--- a/selfdrive/controls/lib/latcontrol_torque.py
+++ b/selfdrive/controls/lib/latcontrol_torque.py
@@ -0,0 +1,79 @@
+import math
+from selfdrive.controls.lib.pid import PIDController
+from common.numpy_fast import interp
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
+from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
+from cereal import log
+
+# At higher speeds (25+mph) we can assume:
+# Lateral acceleration achieved by a specific car correlates to
+# torque applied to the steering rack. It does not correlate to
+# wheel slip, or to speed.
+
+# This controller applies torque to achieve desired lateral
+# accelerations. To compensate for the low speed effects we
+# use a LOW_SPEED_FACTOR in the error. Additionally there is
+# friction in the steering wheel that needs to be overcome to
+# move it at all, this is compensated for too.
+
+
+LOW_SPEED_FACTOR = 200
+JERK_THRESHOLD = 0.2
+
+
+class LatControlTorque(LatControl):
+  def __init__(self, CP, CI):
+    super().__init__(CP, CI)
+    self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki,
+                            k_f=CP.lateralTuning.torque.kf, pos_limit=1.0, neg_limit=-1.0)
+    self.get_steer_feedforward = CI.get_steer_feedforward_function()
+    self.steer_max = 1.0
+    self.pid.pos_limit = self.steer_max
+    self.pid.neg_limit = -self.steer_max
+    self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
+    self.friction = CP.lateralTuning.torque.friction
+
+  def reset(self):
+    super().reset()
+    self.pid.reset()
+
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
+    pid_log = log.ControlsState.LateralTorqueState.new_message()
+
+    if CS.vEgo < MIN_STEER_SPEED or not active:
+      output_torque = 0.0
+      pid_log.active = False
+      self.pid.reset()
+    else:
+      if self.use_steering_angle:
+        actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+      else:
+        actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo
+      desired_lateral_accel = desired_curvature * CS.vEgo**2
+      desired_lateral_jerk = desired_curvature_rate * CS.vEgo**2
+      actual_lateral_accel = actual_curvature * CS.vEgo**2
+
+      setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature
+      measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature
+      error = setpoint - measurement
+      pid_log.error = error
+
+      ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
+      output_torque = self.pid.update(error,
+                                      override=CS.steeringPressed, feedforward=ff,
+                                      speed=CS.vEgo,
+                                      freeze_integrator=CS.steeringRateLimited)
+
+      friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction])
+      output_torque += friction_compensation
+
+      pid_log.active = True
+      pid_log.p = self.pid.p
+      pid_log.i = self.pid.i
+      pid_log.d = self.pid.d
+      pid_log.f = self.pid.f
+      pid_log.output = -output_torque
+      pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS)
+
+    #TODO left is positive in this convention
+    return -output_torque, 0.0, pid_log
--- a/selfdrive/controls/lib/tests/test_latcontrol.py
+++ b/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_lqr import LatControlLQR
+from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 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, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
+  @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlTorque), (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)
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-22356d49a926a62c01d698d77c1f323016b68fd8
+185f5f9c8d878ad4b98664afc7147400476208cc