diff --git a/release/files_common b/release/files_common index 52f65a066d6c9a..2ff94a06bb6a8f 100644 --- a/release/files_common +++ b/release/files_common @@ -239,7 +239,6 @@ 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 diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 4679aa7f635228..d97b16e682147a 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -38,8 +38,6 @@ 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': diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index e5fc217edb3f71..b9c36db07ab065 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -117,8 +117,6 @@ def test_car_params(self): 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: diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 7f01b7deadcfcb..12658a9875727b 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -43,7 +43,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl 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.LQR_RAV4) + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06) elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False @@ -51,7 +51,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl 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_LAT_ACCEL=1.8, FRICTION=0.06) elif candidate == CAR.COROLLA: ret.wheelbase = 2.70 @@ -83,7 +83,10 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl ret.steerRatio = 13.7 tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - set_lat_tune(ret.lateralTuning, LatTunes.PID_C) + 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): stop_and_go = True @@ -132,7 +135,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl 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_LAT_ACCEL=2.0, FRICTION=0.07) elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH): stop_and_go = True diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py index c200e39c0c4cbe..2cfdd316441974 100644 --- a/selfdrive/car/toyota/tunes.py +++ b/selfdrive/car/toyota/tunes.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 from enum import Enum - +from selfdrive.controls.lib.latcontrol_torque import set_torque_tune class LongTunes(Enum): PEDAL = 0 @@ -52,22 +52,7 @@ def set_long_tune(tune, name): ###### LAT ###### def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1): if name == LatTunes.TORQUE: - tune.init('torque') - tune.torque.useSteeringAngle = True - tune.torque.kp = 1.0 / MAX_LAT_ACCEL - 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 + set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION) elif name == LatTunes.INDI_PRIUS: tune.init('indi') tune.indi.innerLoopGainBP = [0.] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6c5d3b9c7205f7..0bccb404fb0e13 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -22,7 +22,6 @@ 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,8 +144,6 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None): 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) @@ -739,8 +736,6 @@ def publish_logs(self, CS, start_time, CC, lac_log): 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 diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py deleted file mode 100644 index 5b4f65a03c24c3..00000000000000 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ /dev/null @@ -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, 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 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 64c9880831eed5..a58270764dbe60 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -22,6 +22,15 @@ JERK_THRESHOLD = 0.2 +def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=.1): + tune.init('torque') + tune.torque.useSteeringAngle = True + tune.torque.kp = 1.0 / MAX_LAT_ACCEL + tune.torque.kf = 1.0 / MAX_LAT_ACCEL + tune.torque.ki = 0.1 / MAX_LAT_ACCEL + tune.torque.friction = FRICTION + + class LatControlTorque(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py index 8345840eca6063..503eaaa6a465a2 100755 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -9,7 +9,7 @@ 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 @@ 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) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 276905a4329c98..55ce9832cc2575 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -b8c35486e8354713221d4237e97e5abced6f5228 +336d77ad17b90af17b7eb24cc832e80b62d05a24