restored INDI and LQR controller (disabled by default)

use dp_lat_controller params to force enable them

Author: eFiniLan

README.md

@@ -30,7 +30,7 @@ I have decided to make this side project open source for users who wish to:
 
 By making this project open source, I hope to alleviate some of the frustration and complaints about not being able to access the dragonpilot source code.
 
-I encourage users to consider purchasing a [comma 3](https://shop.comma.ai) for the best and up-to-date openpilot experience.
+I encourage users to consider purchasing a [comma 3x](https://shop.comma.ai) for the best and up-to-date openpilot experience.
 
 
 ## Limitations
@@ -43,13 +43,14 @@ I encourage users to consider purchasing a [comma 3](https://shop.comma.ai) for
 * Services are not optimized for resource usage, and using all services may result in overheating issues.
 * Language files can only be generated in a PC due to missing Qt5 tools.
 * webjoystick is disabled as it requires additional python modules. (aiohttp and others)
-* Starting from August 7th, 2023, comma has removed ESP/GPS support from Pandas. You can find more details about this change in this [link](https://github.com/commaai/panda/commit/c66b98b2a67441faa4cfcd36c3c9d9f90474cd08).
+* Starting from August 7th, 2023, comma has removed ESP/GPS support from Panda. You can find more details about this change in this [link](https://github.com/commaai/panda/commit/c66b98b2a67441faa4cfcd36c3c9d9f90474cd08).
   * Going forward, I will focus solely on maintaining the safety aspects of the code, ensuring that vehicle support and safety declarations remain up to date.
 * For safety concern, End-to-End / vision only longitudinal control only available in 0.8.16 driving model.
 
 
 ## Configuration
 
+* For research purposes, the INDI and LQR lateral controllers have been restored. Please use the `dp_lat_controller` parameter to override the default controller (0 = DEFAULT, 1 = INDI, 2 = LQR).
 * If you are not a Comma Two device, you can use the `dp_no_fan_ctrl` parameter to disable fan-related detection and control.
 
 =======================

common/params.cc

@@ -212,6 +212,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"dp_no_gps_ctrl", PERSISTENT},
     {"dp_no_fan_ctrl", PERSISTENT},
     {"dp_logging", PERSISTENT},
+    {"dp_lat_controller", PERSISTENT},
 };
 
 } // namespace

selfdrive/car/interfaces.py

@@ -15,6 +15,7 @@
 from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction
 from openpilot.selfdrive.controls.lib.events import Events
 from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
+from openpilot.common.params import Params
 
 ButtonType = car.CarState.ButtonEvent.Type
 GearShifter = car.CarState.GearShifter
@@ -101,6 +102,13 @@ def get_params(cls, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_
     ret = CarInterfaceBase.get_std_params(candidate)
     ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs)
 
+    # rick - override lat controller
+    dp_lat_controller = int(Params().get("dp_lat_controller"))
+    if dp_lat_controller == 1: # indi
+      ret = cls.configure_indi_tune(ret.lateralTuning)
+    elif dp_lat_controller == 2: # lqr
+      ret = cls.configure_lqr_tune(ret.lateralTuning)
+
     # Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload
     if not ret.notCar:
       ret.mass = ret.mass + STD_CARGO_KG
@@ -191,6 +199,30 @@ def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_
     tune.torque.latAccelOffset = 0.0
     tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
 
+  @staticmethod
+  def configure_lqr_tune(tune):
+    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
+
+  @staticmethod
+  def configure_indi_tune(tune):
+    tune.init('indi')
+    tune.indi.innerLoopGainBP = [0.]
+    tune.indi.innerLoopGainV = [4.0]
+    tune.indi.outerLoopGainBP = [0.]
+    tune.indi.outerLoopGainV = [3.0]
+    tune.indi.timeConstantBP = [0.]
+    tune.indi.timeConstantV = [1.0]
+    tune.indi.actuatorEffectivenessBP = [0.]
+    tune.indi.actuatorEffectivenessV = [1.0]
+
   @abstractmethod
   def _update(self, c: car.CarControl) -> car.CarState:
     pass

selfdrive/controls/controlsd.py

@@ -22,6 +22,8 @@
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
+from openpilot.selfdrive.controls.lib.latcontrol_indi import LatControlINDI
+from openpilot.selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
 from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
 from openpilot.selfdrive.controls.lib.events import Events, ET
@@ -157,6 +159,10 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None):
       self.LaC = LatControlAngle(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'pid':
       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)
 

selfdrive/controls/lib/latcontrol_indi.py

@@ -0,0 +1,120 @@
+import math
+import numpy as np
+
+from cereal import log
+from common.filter_simple import FirstOrderFilter
+from common.numpy_fast import clip, interp
+from common.realtime import DT_CTRL
+from selfdrive.controls.lib.latcontrol import LatControl
+
+
+class LatControlINDI(LatControl):
+  def __init__(self, CP, CI):
+    super().__init__(CP, CI)
+    self.angle_steers_des = 0.
+
+    A = np.array([[1.0, DT_CTRL, 0.0],
+                  [0.0, 1.0, DT_CTRL],
+                  [0.0, 0.0, 1.0]])
+    C = np.array([[1.0, 0.0, 0.0],
+                  [0.0, 1.0, 0.0]])
+
+    # Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]])
+    # R = np.matrix([[1e-2, 0.0], [0.0, 1e3]])
+
+    # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
+    # K = np.transpose(K)
+    K = np.array([[7.30262179e-01, 2.07003658e-04],
+                  [7.29394177e+00, 1.39159419e-02],
+                  [1.71022442e+01, 3.38495381e-02]])
+
+    self.speed = 0.
+
+    self.K = K
+    self.A_K = A - np.dot(K, C)
+    self.x = np.array([[0.], [0.], [0.]])
+
+    self._RC = (CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV)
+    self._G = (CP.lateralTuning.indi.actuatorEffectivenessBP, CP.lateralTuning.indi.actuatorEffectivenessV)
+    self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV)
+    self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV)
+
+    self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL)
+    self.reset()
+
+  @property
+  def RC(self):
+    return interp(self.speed, self._RC[0], self._RC[1])
+
+  @property
+  def G(self):
+    return interp(self.speed, self._G[0], self._G[1])
+
+  @property
+  def outer_loop_gain(self):
+    return interp(self.speed, self._outer_loop_gain[0], self._outer_loop_gain[1])
+
+  @property
+  def inner_loop_gain(self):
+    return interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1])
+
+  def reset(self):
+    super().reset()
+    self.steer_filter.x = 0.
+    self.speed = 0.
+
+  def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk):
+    self.speed = CS.vEgo
+    # Update Kalman filter
+    y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
+    self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
+
+    indi_log = log.ControlsState.LateralINDIState.new_message()
+    indi_log.steeringAngleDeg = math.degrees(self.x[0])
+    indi_log.steeringRateDeg = math.degrees(self.x[1])
+    indi_log.steeringAccelDeg = math.degrees(self.x[2])
+
+    steers_des = VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)
+    steers_des += math.radians(params.angleOffsetDeg)
+    indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)
+
+    # desired rate is the desired rate of change in the setpoint, not the absolute desired curvature
+    rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0)
+    indi_log.steeringRateDesiredDeg = math.degrees(rate_des)
+
+    if not active:
+      indi_log.active = False
+      self.steer_filter.x = 0.0
+      output_steer = 0
+    else:
+      # Expected actuator value
+      self.steer_filter.update_alpha(self.RC)
+      self.steer_filter.update(last_actuators.steer)
+
+      # Compute acceleration error
+      rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des
+      accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
+      accel_error = accel_sp - self.x[2]
+
+      # Compute change in actuator
+      g_inv = 1. / self.G
+      delta_u = g_inv * accel_error
+
+      # If steering pressed, only allow wind down
+      if CS.steeringPressed and (delta_u * last_actuators.steer > 0):
+        delta_u = 0
+
+      output_steer = self.steer_filter.x + delta_u
+
+      output_steer = clip(output_steer, -self.steer_max, self.steer_max)
+
+      indi_log.active = True
+      indi_log.rateSetPoint = float(rate_sp)
+      indi_log.accelSetPoint = float(accel_sp)
+      indi_log.accelError = float(accel_error)
+      indi_log.delayedOutput = float(self.steer_filter.x)
+      indi_log.delta = float(delta_u)
+      indi_log.output = float(output_steer)
+      indi_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)
+
+    return float(output_steer), float(steers_des), indi_log

selfdrive/controls/lib/latcontrol_lqr.py

@@ -0,0 +1,85 @@
+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
+
+
+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, steer_limited, desired_curvature, desired_curvature_rate, llk):
+    # 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 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, steer_limited)
+    return output_steer, desired_angle, lqr_log

selfdrive/manager/manager.py

@@ -51,6 +51,7 @@ def manager_init() -> None:
     ("dp_no_fan_ctrl", "0"),
     ("dp_logging", "1"),
     ("dp_0813", "1"),
+    ("dp_lat_controller", "0"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')))