From fad4604b2c8fedf3a55d1c4f26faceea9cb9fb58 Mon Sep 17 00:00:00 2001 From: Edison-CBS Date: Thu, 8 Feb 2024 03:55:43 +0000 Subject: [PATCH] car: remove lat lqr control --- common/params.cc | 2 - release/files_common | 1 - selfdrive/car/tests/test_car_interfaces.py | 3 - selfdrive/car/tests/test_models.py | 2 - selfdrive/car/toyota/interface.py | 2 - selfdrive/car/toyota/tunes.py | 22 ------ selfdrive/controls/controlsd.py | 5 -- selfdrive/controls/lib/latcontrol_lqr.py | 84 ---------------------- selfdrive/manager/manager.py | 1 - selfdrive/ui/qt/offroad/settings.cc | 6 -- selfdrive/ui/translations/main_ar.ts | 8 --- selfdrive/ui/translations/main_de.ts | 8 --- selfdrive/ui/translations/main_fr.ts | 8 --- selfdrive/ui/translations/main_ja.ts | 8 --- selfdrive/ui/translations/main_ko.ts | 8 --- selfdrive/ui/translations/main_pt-BR.ts | 8 --- selfdrive/ui/translations/main_th.ts | 8 --- selfdrive/ui/translations/main_tr.ts | 8 --- selfdrive/ui/translations/main_zh-CHS.ts | 8 --- selfdrive/ui/translations/main_zh-CHT.ts | 8 --- 20 files changed, 208 deletions(-) delete mode 100755 selfdrive/car/toyota/tunes.py delete mode 100644 selfdrive/controls/lib/latcontrol_lqr.py diff --git a/common/params.cc b/common/params.cc index 515d94d8891ef71..30f5cbf68855604 100644 --- a/common/params.cc +++ b/common/params.cc @@ -112,7 +112,6 @@ std::unordered_map keys = { {"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"DisablePowerDown", PERSISTENT}, {"DisableUpdates", PERSISTENT}, - {"LQR", PERSISTENT}, {"DisengageOnAccelerator", PERSISTENT}, {"DmModelInitialized", CLEAR_ON_ONROAD_TRANSITION}, {"DongleId", PERSISTENT}, @@ -210,7 +209,6 @@ std::unordered_map keys = { {"VisionRadarToggle", PERSISTENT}, {"WheeledBody", PERSISTENT}, // edison params - {"LQR", PERSISTENT}, {"ScreenOffTimer", PERSISTENT}, {"CruiseSpeedRewrite", PERSISTENT}, {"DriverCamera", PERSISTENT}, diff --git a/release/files_common b/release/files_common index ceb07d11264a39b..1158d2c552e5d91 100644 --- a/release/files_common +++ b/release/files_common @@ -139,7 +139,6 @@ selfdrive/controls/lib/desire_helper.py selfdrive/controls/lib/drive_helpers.py selfdrive/controls/lib/events.py selfdrive/controls/lib/latcontrol_angle.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 a73518f59860f3e..2306cbf456e6d54 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -87,9 +87,6 @@ def test_car_interfaces(self, car_name, data): self.assertTrue(len(tune.pid.kpV) > 0 and len(tune.pid.kpV) == len(tune.pid.kpBP)) self.assertTrue(len(tune.pid.kiV) > 0 and len(tune.pid.kiV) == len(tune.pid.kiBP)) - elif tune.which() == 'lqr': - self.assertTrue(len(tune.lqr.a)) - elif tune.which() == 'torque': self.assertTrue(not math.isnan(tune.torque.kf) and tune.torque.kf > 0) self.assertTrue(not math.isnan(tune.torque.friction) and tune.torque.friction > 0) diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 6f7b43705ec50ee..85472c756d49b2e 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -209,8 +209,6 @@ 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) else: diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 63da0cad4281760..108ec852496db96 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -69,8 +69,6 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.steerRatio = 16.8 ret.tireStiffnessFactor = 0.5533 ret.mass = 3340. * CV.LB_TO_KG - if Params().get_bool("LQR"): - set_lat_tune(ret.lateralTuning, LatTunes.LQR_PV) elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py deleted file mode 100755 index 21d4b0c114daafe..000000000000000 --- a/selfdrive/car/toyota/tunes.py +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python3 -from enum import Enum - - -class LatTunes(Enum): - LQR_PV = 0 - - -###### LAT ###### -def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, steering_angle_deadzone_deg=0.0, use_steering_angle=True): - if name == LatTunes.LQR_PV: - tune.init('lqr') - tune.lqr.scale = 1650.0 - tune.lqr.ki = 0.028 - 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.0028 - else: - raise NotImplementedError('This lateral tune does not exist') diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4221e27c39d4817..7660e5bd3a07939 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -21,7 +21,6 @@ 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_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 @@ -149,8 +148,6 @@ def __init__(self, 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() == 'lqr': - self.LaC = LatControlLQR(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'torque': self.LaC = LatControlTorque(self.CP, self.CI) @@ -806,8 +803,6 @@ def publish_logs(self, CS, start_time, CC, lac_log): 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 diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py deleted file mode 100644 index c692b00bcb04bab..000000000000000 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ /dev/null @@ -1,84 +0,0 @@ -import math -import numpy as np - -from openpilot.common.numpy_fast import clip -from openpilot.common.realtime import DT_CTRL -from cereal import log -from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_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, steer_limited, 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_LATERAL_CONTROL_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, steer_limited) - return output_steer, desired_angle, lqr_log diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 9815ea0ef696fbf..0146292e05339af 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -35,7 +35,6 @@ def manager_init() -> None: default_params: List[Tuple[str, Union[str, bytes]]] = [ ("CompletedTrainingVersion", "0"), - ("LQR", "0"), ("DisengageOnAccelerator", "0"), ("GsmMetered", "1"), ("HasAcceptedTerms", "0"), diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 0b6977d5eba59d9..7c8f70bf5f041ad 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -65,12 +65,6 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { tr("Upload data from the driver facing camera and help improve the driver monitoring algorithm."), "../assets/offroad/icon_monitoring.png", }, - { - "LQR", - tr("Use LQR on Lat Control for PA"), - tr("When enabled, using LQR on lat control for prius alpha."), - "../assets/offroad/icon_openpilot.png", - }, // screen off timer { "ScreenOffTimer", diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts index bbf9ed6d5ac5beb..8f9961eed3fac58 100644 --- a/selfdrive/ui/translations/main_ar.ts +++ b/selfdrive/ui/translations/main_ar.ts @@ -1139,14 +1139,6 @@ This may take up to a minute. Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. تمكين التحكم الطولي من openpilot (ألفا) للسماح بالوضع التجريبي. - - Use LQR on Lat Control for PA - - - - When enabled, using LQR on lat control for prius alpha. - - Turn Off Display After 30 Seconds diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index 2a2a4504c8e789d..712e1adcdedb712 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -1033,14 +1033,6 @@ This may take up to a minute. Upload data from the driver facing camera and help improve the driver monitoring algorithm. - - Use LQR on Lat Control for PA - - - - When enabled, using LQR on lat control for prius alpha. - - Turn Off Display After 30 Seconds diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts index ecc76350a864aab..c036e6ba2e16543 100644 --- a/selfdrive/ui/translations/main_fr.ts +++ b/selfdrive/ui/translations/main_fr.ts @@ -1123,14 +1123,6 @@ Cela peut prendre jusqu'à une minute. Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. Activer le contrôle longitudinal d'openpilot (en alpha) pour autoriser le mode expérimental. - - Use LQR on Lat Control for PA - - - - When enabled, using LQR on lat control for prius alpha. - - Turn Off Display After 30 Seconds diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 58160d88633b920..c7bdc9771b2b50d 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -1117,14 +1117,6 @@ This may take up to a minute. Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. - - Use LQR on Lat Control for PA - - - - When enabled, using LQR on lat control for prius alpha. - - Turn Off Display After 30 Seconds diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index d787802ca5ca32e..13c5309460ee5e4 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -1119,14 +1119,6 @@ This may take up to a minute. The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green. 주행 시각화는 저속으로 주행 시 도로를 향한 광각 카메라로 자동 전환되어 일부 곡선 경로를 더 잘 보여줍니다. 실험 모드 로고는 우측 상단에 표시됩니다. 내비게이션 목적지가 설정되고 주행 모델에 입력되면 지도의 주행 경로가 녹색으로 바뀝니다. - - Use LQR on Lat Control for PA - - - - When enabled, using LQR on lat control for prius alpha. - - Turn Off Display After 30 Seconds diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index 67ec5f2c033d56c..de1448c8cd12748 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -1033,14 +1033,6 @@ This may take up to a minute. Upload data from the driver facing camera and help improve the driver monitoring algorithm. - - Use LQR on Lat Control for PA - - - - When enabled, using LQR on lat control for prius alpha. - - Turn Off Display After 30 Seconds diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index a8c428bd4638fa7..82a38dc080dad8e 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -1119,14 +1119,6 @@ This may take up to a minute. Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. เปิดระบบควบคุมการเร่ง/เบรคโดย openpilot (alpha) เพื่อเปิดใช้งานโหมดทดลอง - - Use LQR on Lat Control for PA - - - - When enabled, using LQR on lat control for prius alpha. - - Turn Off Display After 30 Seconds diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts index 53e688e653221f5..23fc3f1653a45db 100644 --- a/selfdrive/ui/translations/main_tr.ts +++ b/selfdrive/ui/translations/main_tr.ts @@ -1117,14 +1117,6 @@ This may take up to a minute. Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. - - Use LQR on Lat Control for PA - - - - When enabled, using LQR on lat control for prius alpha. - - Turn Off Display After 30 Seconds diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 80fe8d53e3afdb0..cf5f3ff3f3551a3 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -1119,14 +1119,6 @@ This may take up to a minute. The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green. 行驶画面将在低速时切换到道路朝向的广角摄像头，以更好地显示一些转弯。实验模式标志也将显示在右上角。当设置了导航目的地并且驾驶模型正在使用它作为输入时，地图上的驾驶路径将变为绿色。 - - Use LQR on Lat Control for PA - - - - When enabled, using LQR on lat control for prius alpha. - - Turn Off Display After 30 Seconds diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 25390ade9b6e04a..8c45c39f84f40ef 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -1119,14 +1119,6 @@ This may take up to a minute. The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green. 行駛畫面將在低速時切換至道路朝向的廣角鏡頭，以更好地顯示一些轉彎。實驗模式圖示也將顯示在右上角。當設定了導航目的地並且行駛模型正在將其作為輸入時，地圖上的行駛路徑將變為綠色。 - - Use LQR on Lat Control for PA - - - - When enabled, using LQR on lat control for prius alpha. - - Turn Off Display After 30 Seconds