Torque control with roll #24260

commaai/openpilot#24260

Update interface.py

Update interface.py

Update interface.py

CHANGELOGS.md

@@ -1,3 +1,7 @@
+0.8.12-4-subaru-MADS-dev
+========================
+* ❗❗DO NOT UPDATE ❗❗
+
 sunnypilot - Version 0.8.12-4 (2022-03-XX)
 ========================
 * NEW❗: Enable M.A.D.S with LKAS button on the steering wheel

release/files_common

@@ -232,7 +232,7 @@ selfdrive/controls/lib/events.py
 selfdrive/controls/lib/drive_helpers.py
 selfdrive/controls/lib/latcontrol_pid.py
 selfdrive/controls/lib/latcontrol_indi.py
-selfdrive/controls/lib/latcontrol_lqr.py
+selfdrive/controls/lib/latcontrol_torque.py
 selfdrive/controls/lib/latcontrol_angle.py
 selfdrive/controls/lib/longcontrol.py
 selfdrive/controls/lib/lateral_planner.py

selfdrive/car/subaru/interface.py

@@ -7,6 +7,9 @@
 EventName = car.CarEvent.EventName
 ButtonType = car.CarState.ButtonEvent.Type
 
+MAX_TORQUE = 3.0
+FRICTION = 0.06
+
 class CarInterface(CarInterfaceBase):
   def __init__(self, CP, CarController, CarState):
     super().__init__(CP, CarController, CarState)
@@ -53,17 +56,15 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.5
       ret.steerActuatorDelay = 0.18   # end-to-end angle controller
-      ret.lateralTuning.pid.kpBP = [0.0]
-      ret.lateralTuning.pid.kiBP = [0.0]
-
-      ret.lateralTuning.pid.kpV = [0.3] # too high = the car overshoots and undershoots center, too low = the car doesn't turn enough
-
-      ret.lateralTuning.pid.kiV = [0.025] # too high = it gets to center without oscillations, but it takes too long to center. If the wheel oscillates forever (critically damped), then your Kp or Ki or both are too high.
-
       ret.steerRatio = 14  # too high = car ping pongs on straights and turns, too low = car doesn't turn enough on curves.
 
-      ret.lateralTuning.pid.kf = 0.00003 # lower = reduce oscillilations and you've done everything else. It can be lowered to 0
-
+      ret.lateralTuning.init('torque')
+      ret.lateralTuning.torque.useSteeringAngle = True
+      ret.lateralTuning.torque.kp = 2.0 / MAX_TORQUE
+      ret.lateralTuning.torque.kf = 1.0 / MAX_TORQUE
+      ret.lateralTuning.torque.ki = 0.5 / MAX_TORQUE
+      ret.lateralTuning.torque.friction = FRICTION
+
     if candidate == CAR.FORESTER:
       ret.mass = 1568. + STD_CARGO_KG
       ret.wheelbase = 2.67

selfdrive/car/subaru/values.py

@@ -16,8 +16,11 @@ def __init__(self, CP):
       self.STEER_MAX = 1439         # torque increase per refresh, 0.8s to max
     elif CP.carFingerprint == CAR.IMPREZA or CP.carFingerprint == CAR.CROSSTREK_2020H:
       self.STEER_MAX = 3071
-      self.STEER_DELTA_UP = 60           # torque increase per refresh, 0.8s to max
-      self.STEER_DELTA_DOWN = 60         # torque decrease per refresh
+      #self.STEER_DELTA_UP = 60           # torque increase per refresh, 0.8s to max
+      #self.STEER_DELTA_DOWN = 60         # torque decrease per refresh
+
+      self.STEER_DELTA_UP = 15           # TORQUE TUNE ONLY
+      self.STEER_DELTA_DOWN = 25         # TORQUE TUNE ONLY
     else:
       self.STEER_MAX = 2047
 

selfdrive/car/tests/test_car_interfaces.py

@@ -39,8 +39,8 @@ def test_car_interfaces(self, car_name):
       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))
 

selfdrive/controls/controlsd.py

@@ -19,7 +19,7 @@
 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_torque import LatControlTorque
 from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
 from selfdrive.controls.lib.events import Events, ET
 from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
@@ -137,8 +137,8 @@ def __init__(self, sm=None, pm=None, can_sock=None):
       self.LaC = LatControlPID(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'indi':
       self.LaC = LatControlINDI(self.CP)
-    elif self.CP.lateralTuning.which() == 'lqr':
-      self.LaC = LatControlLQR(self.CP)
+    elif self.CP.lateralTuning.which() == 'torque':
+      self.LaC = LatControlTorque(self.CP, self.CI)
 
     self.initialized = False
     self.state = State.disabled
@@ -531,8 +531,9 @@ def state_control(self, CS):
                                                                              lat_plan.psis,
                                                                              lat_plan.curvatures,
                                                                              lat_plan.curvatureRates)
-      actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.VM, params,
-                                                                             desired_curvature, desired_curvature_rate)
+      actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(lat_active, CS, self.CP, self.VM,
+                                                                             params, desired_curvature,
+                                                                             desired_curvature_rate, self.sm['liveLocationKalman'])
     else:
       lac_log = log.ControlsState.LateralDebugState.new_message()
       if self.sm.rcv_frame['testJoystick'] > 0 and self.active:
@@ -698,8 +699,8 @@ def publish_logs(self, CS, start_time, actuators, lac_log):
       controlsState.lateralControlState.angleState = lac_log
     elif self.CP.lateralTuning.which() == 'pid':
       controlsState.lateralControlState.pidState = lac_log
-    elif self.CP.lateralTuning.which() == 'lqr':
-      controlsState.lateralControlState.lqrState = lac_log
+    elif self.CP.lateralTuning.which() == 'torque':
+      controlsState.lateralControlState.torqueState = lac_log
     elif self.CP.lateralTuning.which() == 'indi':
       controlsState.lateralControlState.indiState = lac_log
     self.pm.send('controlsState', dat)

selfdrive/controls/lib/latcontrol.py

@@ -0,0 +1,31 @@
+from abc import abstractmethod, ABC
+
+from common.realtime import DT_CTRL
+from common.numpy_fast import clip
+
+MIN_STEER_SPEED = 0.3
+
+
+class LatControl(ABC):
+  def __init__(self, CP, CI):
+    self.sat_count_rate = 1.0 * DT_CTRL
+    self.sat_limit = CP.steerLimitTimer
+    self.sat_count = 0.
+
+    # we define the steer torque scale as [-1.0...1.0]
+    self.steer_max = 1.0
+
+  @abstractmethod
+  def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
+    pass
+
+  def reset(self):
+    self.sat_count = 0.
+
+  def _check_saturation(self, saturated, CS):
+    if saturated and CS.vEgo > 10. and not CS.steeringRateLimited and not CS.steeringPressed:
+      self.sat_count += self.sat_count_rate
+    else:
+      self.sat_count -= self.sat_count_rate
+    self.sat_count = clip(self.sat_count, 0.0, self.sat_limit)
+    return self.sat_count > (self.sat_limit - 1e-3)

selfdrive/controls/lib/latcontrol_angle.py

@@ -10,7 +10,7 @@ def __init__(self, CP):
   def reset(self):
     pass
 
-  def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate, llk):
     angle_log = log.ControlsState.LateralAngleState.new_message()
 
     if CS.vEgo < 0.3 or not active:

selfdrive/controls/lib/latcontrol_pid.py

@@ -16,7 +16,7 @@ def __init__(self, CP, CI):
   def reset(self):
     self.pid.reset()
 
-  def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate):
+  def update(self, active, CS, CP, VM, params, 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)

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

selfdrive/test/test_models.py

@@ -110,8 +110,8 @@ def test_car_params(self):
       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: