From 166f50036ef25ed4b493d9039d143f84432ce0e1 Mon Sep 17 00:00:00 2001 From: sshane Date: Thu, 12 Aug 2021 22:52:42 -0700 Subject: [PATCH] Add new ML-based lateral controller (#447) * add model steering controller * add comment * add model name field * add parameter to enable the model * update releases * update releases * fixes --- SA_RELEASES.md | 1 + cereal | 2 +- common/op_params.py | 2 + selfdrive/car/toyota/interface.py | 20 +++--- selfdrive/controls/controlsd.py | 5 ++ selfdrive/controls/lib/latcontrol_model.py | 79 +++++++++++++++++++++ torque_models/corolla_model_v5_weights.npz | Bin 0 -> 1356 bytes 7 files changed, 100 insertions(+), 9 deletions(-) create mode 100644 selfdrive/controls/lib/latcontrol_model.py create mode 100644 torque_models/corolla_model_v5_weights.npz diff --git a/SA_RELEASES.md b/SA_RELEASES.md index 6d8239777bae25..f1c3b86bddd790 100644 --- a/SA_RELEASES.md +++ b/SA_RELEASES.md @@ -2,6 +2,7 @@ Stock Additions Update 1 - 2021-08-XX (0.8.7) === * Update SA to 0.8.7! * Variable longitudinal lag by speed + * Toyota: add ML-based steering controller enabled by parameter `use_steering_model` Stock Additions Update 3 - 2021-05-03 (0.8.2) === diff --git a/cereal b/cereal index 9de6cd3ffc2419..43b86bff9b52e8 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 9de6cd3ffc2419dbc922832fbc1ac308df50248a +Subproject commit 43b86bff9b52e8954ee3959553cf170770daaa1d diff --git a/common/op_params.py b/common/op_params.py index 3ae4e1601df2c0..240c598d7d40ae 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -131,6 +131,8 @@ def __init__(self): 'prius_use_pid': Param(False, bool, 'This enables the PID lateral controller with new a experimental derivative tune\n' 'False: stock INDI, True: TSS2-tuned PID', static=True), 'use_lqr': Param(False, bool, 'Enable this to use LQR as your lateral controller over default with any car', static=True), + 'use_steering_model': Param(False, bool, 'Enable this to use an experimental ML-based lateral controller trained on the TSSP Corolla\n' + 'Warning: the model may behave unexpectedly at any time, so always pay attention', static=True), 'corollaTSS2_use_indi': Param(False, bool, 'Enable this to use INDI for lat with your TSS2 Corolla', static=True), 'rav4TSS2_use_indi': Param(False, bool, 'Enable this to use INDI for lat with your TSS2 RAV4', static=True), 'standstill_hack': Param(False, bool, 'Some cars support stop and go, you just need to enable this', static=True)} diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index e8c1c274a7748b..af727e55cd8c6d 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -20,6 +20,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py op_params = opParams() use_lqr = op_params.get('use_lqr') + use_steering_model = op_params.get('use_steering_model') prius_use_pid = op_params.get('prius_use_pid') corollaTSS2_use_indi = op_params.get('corollaTSS2_use_indi') rav4TSS2_use_indi = op_params.get('rav4TSS2_use_indi') @@ -34,11 +35,11 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop - CARS_NOT_PID = [CAR.RAV4, CAR.RAV4H] + CARS_NOT_PID = [CAR.RAV4, CAR.RAV4H, CAR.COROLLA] if not prius_use_pid: CARS_NOT_PID.append(CAR.PRIUS) - if candidate not in CARS_NOT_PID and not use_lqr: # These cars use LQR/INDI + if candidate not in CARS_NOT_PID and not use_lqr and not use_steering_model: # These cars use LQR/INDI ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] @@ -90,12 +91,10 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.minSpeedCan = 0.1 * CV.KPH_TO_MS tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kpV = [[20, 31], [0.06, 0.12]] # 45 to 70 mph - ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[20, 31], [0.001, 0.02]] - ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[20, 31], [0.1, 0.2]] - # ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 - ret.lateralTuning.pid.kf = 0.00006908923778520113 # full torque for 20 deg at 80mph means 0.00007818594 - ret.lateralTuning.pid.newKfTuned = True + + ret.lateralTuning.init('model') + ret.lateralTuning.model.name = "corolla_model_v5" + ret.lateralTuning.model.useRates = False # TODO: makes model sluggish, see comments in latcontrol_model.py elif candidate == CAR.LEXUS_RX: stop_and_go = True @@ -350,6 +349,11 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # py ret.lateralTuning.lqr.l = [0.3233671, 0.3185757] ret.lateralTuning.lqr.dcGain = 0.002237852961363602 + elif use_steering_model: + ret.lateralTuning.init('model') + ret.lateralTuning.model.name = "corolla_model_v5" + ret.lateralTuning.model.useRates = False + ret.centerToFront = ret.wheelbase * 0.44 # TODO: get actual value, for now starting with reasonable value for diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 989c26d3053b79..c2a155b20c930a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -17,6 +17,7 @@ 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_model import LatControlModel from selfdrive.controls.lib.latcontrol_angle import LatControlAngle from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager @@ -133,6 +134,8 @@ def __init__(self, sm=None, pm=None, can_sock=None): self.LaC = LatControlINDI(self.CP) elif self.CP.lateralTuning.which() == 'lqr': self.LaC = LatControlLQR(self.CP) + elif self.CP.lateralTuning.which() == 'model': + self.LaC = LatControlModel(self.CP) self.initialized = False self.state = State.disabled @@ -667,6 +670,8 @@ def publish_logs(self, CS, start_time, actuators, lac_log): controlsState.lateralControlState.lqrState = lac_log elif self.CP.lateralTuning.which() == 'indi': controlsState.lateralControlState.indiState = lac_log + elif self.CP.lateralTuning.which() == 'model': + controlsState.lateralControlState.modelState = lac_log self.pm.send('controlsState', dat) # carState diff --git a/selfdrive/controls/lib/latcontrol_model.py b/selfdrive/controls/lib/latcontrol_model.py new file mode 100644 index 00000000000000..52077960b7b5b9 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_model.py @@ -0,0 +1,79 @@ +import math +import numpy as np +from common.basedir import BASEDIR +from selfdrive.controls.lib.drive_helpers import get_steer_max +from cereal import log +from common.numpy_fast import clip +from common.realtime import DT_CTRL + + +class LatControlModel: + def __init__(self, CP): + # Model generated using Konverter: https://github.com/ShaneSmiskol/Konverter + model_weights_file = f'{BASEDIR}/torque_models/{CP.lateralTuning.model.name}_weights.npz' + self.w, self.b = np.load(model_weights_file, allow_pickle=True)['wb'] + + self.use_rates = CP.lateralTuning.model.useRates + + self.sat_count_rate = 1.0 * DT_CTRL + self.sat_limit = CP.steerLimitTimer + + def reset(self): + pass + + def _check_saturation(self, control, check_saturation, limit): + saturated = abs(control) == limit + + if saturated and check_saturation: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate + + self.sat_count = clip(self.sat_count, 0.0, 1.0) + + return self.sat_count > self.sat_limit + + def predict(self, x): + x = np.array(x, dtype=np.float32) + l0 = np.dot(x, self.w[0]) + self.b[0] + l0 = np.where(l0 > 0, l0, l0 * 0.3) + l1 = np.dot(l0, self.w[1]) + self.b[1] + l1 = np.where(l1 > 0, l1, l1 * 0.3) + l2 = np.dot(l1, self.w[2]) + self.b[2] + return l2 + + def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate): + model_log = log.ControlsState.LateralModelState.new_message() + model_log.steeringAngleDeg = float(CS.steeringAngleDeg) + model_log.useRates = self.use_rates + + angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo)) + angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg + + if CS.vEgo < 0.15 or not active: + output_steer = 0.0 + model_log.active = False + else: + steers_max = get_steer_max(CP, CS.vEgo) + pos_limit = steers_max + neg_limit = -steers_max + + steering_rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo) + + # TODO: Can be sluggish when model is given rates, the issue is probably with the training data, + # specifically the disturbances/perturbations fed into the model to recover from large errors + # Basically, figure out a better way to train the model to recover without random samples and using a PF controller as the output + rate_des = steering_rate_des if self.use_rates else 0 + rate = CS.steeringRateDeg if self.use_rates else 0 + model_input = [angle_steers_des, CS.steeringAngleDeg, rate_des, rate, CS.vEgo] + + output_steer = self.predict(model_input)[0] + output_steer = float(clip(output_steer, neg_limit, pos_limit)) + + model_log.active = True + model_log.output = output_steer + + check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed + model_log.saturated = self._check_saturation(output_steer, check_saturation, steers_max) + + return output_steer, angle_steers_des, model_log diff --git a/torque_models/corolla_model_v5_weights.npz b/torque_models/corolla_model_v5_weights.npz new file mode 100644 index 0000000000000000000000000000000000000000..6781e595aaba3ed05cbd637e81041766cfbe1187 GIT binary patch literal 1356 zcmV-S1+)54O9KQH000080000X0C5F}(#iw?0CWWa00saQ0C!?8Zg6=401yCl1poj5 z0002W1ONa40001;l4($rR}_X511{haWRX>(2nmazK*Z798w8grF;P=wuqsPJFjfiN z@P&%iQ9FcGfl&fMpX9AX!T&#Qn|W%TA+Mk~TAi=gMCTdSGOM%o`s`x2Oua^(uPbEq z1~ubGID=hox}3pZ!j*B1WLPp+MR+FSq9m>&8CNz;#w#Vm|831-iVHMuWJLNf4u`X7 zCK)N?$hamlN=e*AGG02@QQbh?6$(X4zvB#CGATnwtG2ErV?)?-5626n~ zDjQi!JhX$z+;}p65b3ETULTMW;{9JFhaoJmgIFvM5&VQoL>+Wtxs;9Ps8j9*FuwO(73IF7~T zH*D5#!doGYUk7y_?Xan)3|~xE;=EZ(me<#c?(A!!O-_zJ(uWLPgb}0tGzgq$ws(e_S5P|aE%mv1+7L*Bg`ccVA+W*1_BD=)! zy74f)UmAo{yPJ_`@kwmStOkq3j^9aosj>P?{4Rjv$`?DW;?4yyp|%KhdS<|?v%c(= zsb={5Vk9nq$pfDs_rlC8`|*yJ2HJCZGk)$bhk%|?NLVAJouYf-omPs@#7~3ty9#>i ze5}+X+28u?i40$kEw|z$E1>7nL%b*>T)JDZA1lMQKrk3_ZzH-{0wzXfuA z*V9zkG^;#LO?xa&l9l(n@j>}2yv?&2*G%S1UdQ-JmSrr#bK1p_Hs20c-=Bch?Y_{E z`GA^i2{=_;h(jW`<0XN<;A&}rtCN2~ua4<(#qEWZDQ|__9lzk9L-#@cNf}K%Z9qqk zxl8RGxw!U3G0J!U71nKR1ZKx~w6b75beKZf^LyG*b?|kX_4X8=*yjl~E4~KX9y$K> zLNBxju4TLHU!jcDCTy!8j~f^GflJa2dH}K@{5b>iQ9J2&8(+F5;sAYQu7v$#y&%E+ zI*=fSOdJYIurnwEC7I+53SY)8C6gUNNlr91r{ES#8m`y1)2c8dp52;^t2T7gYlpW% z249W%KL=y?IhDwJ-%Ir1co+2&l|n_yO>}%=AhgV&h=ed?%8(`0X$eyj;XoXXMC5b` zwvSgyAJJ+&ujeiF*Ugqj75cG^_XSq$D*XP?u5m00XGp{lBhtx;Qj)0ylNw3%z?=sS z)!=?)I=fSz2Axwzvvn0#T>JDqJe%9gHoYFs@*Z0tbcxw&{%H-A2R;W|+wWFI+j1<( z^pdt!aaj_>kZD7X=}t$ilEgV3TqB8hI$V6pc!-D@GDD?dNP;T*A5cpH0u%rg00008 z0000X0C5F}(#iw?0CWWa00sa600000000000Du7i0001YVlHlQc~DCQ1^@s600962 O05kvq00IR70001kF=wm* literal 0 HcmV?d00001