From ac3290c0e7e1fbae660cefa0ac69441c6f84e84f Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 12 Jan 2026 17:03:23 -0600 Subject: [PATCH] Revert "Torque Model" This reverts commit f25410c6e9bffa7525ff1c42c71aca29d2cd841f. --- selfdrive/controls/lib/latcontrol_torque.py | 70 +----------------- .../controls/lib/torque_ml_artifacts/norm.pkl | Bin 526 -> 0 bytes .../lib/torque_ml_artifacts/weights.npz | Bin 6529 -> 0 bytes selfdrive/controls/lib/torque_ml_model.py | 45 ----------- 4 files changed, 1 insertion(+), 114 deletions(-) delete mode 100644 selfdrive/controls/lib/torque_ml_artifacts/norm.pkl delete mode 100644 selfdrive/controls/lib/torque_ml_artifacts/weights.npz delete mode 100644 selfdrive/controls/lib/torque_ml_model.py diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d2f0e35b3..ee918b166 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -1,7 +1,6 @@ import math import numpy as np from collections import deque -import os from cereal import log from openpilot.selfdrive.car.interfaces import FRICTION_THRESHOLD, get_friction_threshold @@ -10,8 +9,6 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED from openpilot.selfdrive.controls.lib.pid import PIDController from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY -from openpilot.common.basedir import BASEDIR -from openpilot.selfdrive.controls.lib.torque_ml_model import TorqueMLModel # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to @@ -40,19 +37,6 @@ JERK_GAIN = 0.22 LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 VERSION = 2 -# ==== ML torque override config (hardcoded globals) ==== -# Toggle this to enable/disable steering on the ML model. -TORQUE_ML_ENABLED = True - -# Trained model artifacts vendored into openpilot (so the device only needs openpilot) -_ARTIFACTS_DIR = os.path.join(BASEDIR, "selfdrive", "controls", "lib", "torque_ml_artifacts") -TORQUE_ML_WEIGHTS_PATH = os.path.join(_ARTIFACTS_DIR, "weights.npz") -TORQUE_ML_NORM_PATH = os.path.join(_ARTIFACTS_DIR, "norm.pkl") - -# 1.0 = full ML torque, 0.0 = stock torque controller -TORQUE_ML_BLEND = 1.0 -# ====================================================== - class LatControlTorque(LatControl): def __init__(self, CP, CI, dt): super().__init__(CP, CI, dt) @@ -70,15 +54,6 @@ class LatControlTorque(LatControl): self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) self.low_speed_reset_threshold = max(CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) - # ML torque override (enabled via env vars) - self._ml = None - if TORQUE_ML_ENABLED: - self._ml = TorqueMLModel(TORQUE_ML_WEIGHTS_PATH, TORQUE_ML_NORM_PATH) - self._ml_blend = float(TORQUE_ML_BLEND) - self._prev_des_lat_accel = 0.0 - self._prev_meas_lat_accel = 0.0 - self._prev_ml_torque = 0.0 - def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor self.torque_params.latAccelOffset = latAccelOffset @@ -149,48 +124,5 @@ class LatControlTorque(LatControl): pid_log.desiredLateralJerk = float(desired_lateral_jerk) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) - # ML override uses a different "desired" definition than the PID loop: - # it matches the dataset (controlsState.desiredCurvature * v^2), not the delayed setpoint. - steer_cmd = -output_torque - if self._ml is not None and active: - desired_lat_accel = float(desired_curvature * CS.vEgo ** 2) - meas_lat_accel = float(measurement) - lat_accel_error = desired_lat_accel - meas_lat_accel - - desired_lat_jerk = (desired_lat_accel - self._prev_des_lat_accel) / self.dt - meas_lat_jerk = (meas_lat_accel - self._prev_meas_lat_accel) / self.dt - - prev_des_mag = abs(self._prev_des_lat_accel) - cur_des_mag = abs(desired_lat_accel) - unwind = float((cur_des_mag < prev_des_mag) and (abs(meas_lat_accel) > 0.5)) - - # Build features in the same order as training (cmd_torque_dataset.py) - features = [ - desired_lat_accel, - meas_lat_accel, - lat_accel_error, - float(desired_lat_jerk), - float(meas_lat_jerk), - float(CS.vEgo), - float(CS.aEgo), - float(CS.steeringAngleDeg), - float(CS.steeringRateDeg), - unwind, - float(self._prev_ml_torque), - ] - - ml_torque = float(self._ml.predict(features)) - ml_torque = float(np.clip(ml_torque, -self.steer_max, self.steer_max)) - - # Optional blend (default 1.0 = full ML) - a = float(np.clip(self._ml_blend, 0.0, 1.0)) - steer_cmd = (1.0 - a) * steer_cmd + a * ml_torque - - self._prev_des_lat_accel = desired_lat_accel - self._prev_meas_lat_accel = meas_lat_accel - self._prev_ml_torque = steer_cmd - - # TODO left is positive in this convention - return steer_cmd, 0.0, pid_log - + return -output_torque, 0.0, pid_log diff --git a/selfdrive/controls/lib/torque_ml_artifacts/norm.pkl b/selfdrive/controls/lib/torque_ml_artifacts/norm.pkl deleted file mode 100644 index 23a457c91acda7b270927947d8d9877f51ee6bf8..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 526 zcmZ8dJ4h=*6x}4o2>#Hg78as~U?Hglg9L{>3&B7N6~sb@arfqvIbsJP1*l z&a0&$3YH3%f>_ww3Swm=h+tu1rB1Rc>V=u%oclRvZnPX#EJ*hELsSvX^on|wO3R<+$)y+I5iys z@uFwT0uzGyTAvCq9qvlunG(0Pa=j8&j9Wp;Hl~dVPJ|Y*iqklh4yBtbnD10kOZp|K zwS==HxVaHzBPx`{OeQn+rlWqP4T+1J6T`8aa#Fs`CA0IhW3@H!Uq=!TiLF%2>)l!S zevbsZH;nK=HHo@K#nz~6GX3d%N4~4(A>Z@eL7$#^yj&&fv_dZb&y4qvK9k*xD{^qQ vo;plClBe^*lwl}+4ALQQ$Llkj{Lht4l9QvqWt~(p|3hbz-1YJr<;C9tW4X=C diff --git a/selfdrive/controls/lib/torque_ml_artifacts/weights.npz b/selfdrive/controls/lib/torque_ml_artifacts/weights.npz deleted file mode 100644 index 922213cc3b4a0aeba27ef7432bcb7fbd41edd140..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6529 zcmbVxc{G>Z*SDEcDO02jnJYrNiRTkG)6>bzXOHu5_R5{^zRr_;-+ep1ohQFF^cE>D(Ojf>O!5DprUnU|_xra= zQPQ6N9^QwwopyWqIBR?OyZgEB^zqqwI3a49laKRmFHc`TAOGEc3GoS0g8$M?h!XbP z^N&h=f+bIAf+_E=gzyC01Px1GOFm2fu!Md5QG$E?4tqQQ{)x}VFd;JHA31*%Z(xG8 zwe_0coAj^AlH-@KE7BoBV;|o>!QWu}g#QC2YNhflSTQl=y7KuY#CC6rNt@Oa#RzxW|g9u@N?8X zEW+I_ISb|MYVb%^F|4?Mi}+mGh3mRaQMuU;v-ghDHRF+ZAee$!peb}!nsag-%foy`i6G=@EqRp<9 zRP7Yw=ou{q!CzT4Okj+8zRC*B#~dhk>2l~BXXf*K6X$GP$%DAw@9$LX@)NwJ7lj>c8>q}v0M6~_bWBSDR&V)}4(pv_whSDn zimOMAdAru417R?1Nhf1;w;&IJ9)ty90K&W@eao;=hnsxy$z zmV|oHy1#2l+`v}$@3(eLPlPTB42@>L{t{p`8ms8_C6&a;r;0$?aP8xKQ7BwX=#=(2 zqq&VYS&sBt=A|2hB=`brq>xHx8*~xz!bQ+_`z+k&5r&{9E)yJc1pS`sU~5J!O|r9R zBxYQydoa6#s#w*qqkVBq@M1wY-Ng?@;hV{hk!93qT_#&HrbX+g&V_>)Mo1^W0xR); z37d80F*%=9OXv9;f@`J&Dt4Q&t_yeJ-GXbRJ!k?BTB>mpDj#FOY-6|_t%Kze0!)#r z47x};;6aOEtZTguBjN>cCE@~6Zj#`t6swXwdDm&4i!BD`j)QUebuzK93B7)d;lpL6 zpeOkgrF^T|I{b!qV{Rf(W-F<1R|b&ZFYDETECkm09N7P1ZR1a3#VmI8_e^NcjB* z>RiTfj2?x+Dur-JTkwE7Dp%ki#1Y)agE3Fw89d@9;EJ~=Y3h)zB(9+P{eNsl}b# zXLaCR;(h#bq69`fFQ9>UD%#x_cyZxN-R^J0g z^M?_>HB7<{*EEqDaM^~3<>0udla_=uQn?rkh7BUP=|>T)OU;9zWBW13^*j5_mLH*C zfyg)(;rK35?5CUYkxUs$ohZZ{mn$@P_Ec`x!xqdeUjdSrVyRo2F5J5J3#L!c#N_4~ zD0S5uByuBxM=cKrcJ76|u4i!1^9Kg{9>Fyov9zx47d)EvofvPc1;ylBG{cOC`$lFV z{PlAkr0w2_S$3DO(y@ya#G2#t;HB8*AjWO7I)zWAT2Vk`F>UZ}!C!Ba(fzCwCQL@+ z|1*p${ws{jMw(1sgFG1t*MRmI0?&+g42{jEwJBS1_lnKEL16A znk(uU{^dW3S#1<4vaW|4+k4r_E?poy6XC1YHkxXo1TIUjBX86!s3ZmGmJ&`J&%UEK z24$fC;~%u5#sIfOYJ+6-QwsVAP`_CSbeoo;um2MoBh!m&#x>w8xQ9lbC?O7?b4jyX z5nj8P&kD!?NvdS3==(e7SXA5s&)ueA$j2(wpR$dW6>TG@C(Pk!)h@ahHOPqVZU|WK z17DA8qVB*tcvBur^x{%szjO{dpWx*_H=T-Wcj$o8a2X!3Jci#G6KFaa51g1lTDPeP zr`J$sdu#@EaqB|i+gZ?WXI*zfD-?b%1p3Fr)3EYuGM+IH!NhltB*5z~O3CR^??<~a z>v1R>dZLPP&A&@ZtD&t;HH0@c0gWwZqRZ%X?))WOv^BQG z&rbfRVwK8D{sb6JtA{DaW5^EWzhFSali4ui7cE!GgYw=)s91EF%$wm0wIS}bbAcmX znzs&%b}5oxgH`m1l_B0}I7b@GoT-%p zms&KA(iy^+u*=efeESgxX`B@#=;B$(*>C~f#GJ9Z(**@izoXI5m*SAB658vw)Ae$) zkY^H$Eqf~vefb%+=Su9i`X!iZrwgBT8rf3cZsQL%7Qoqdp0w%JfKT0O;&(d_|H@0k zxz6Lb>&hb%Seb<0XFfohQ4CSqpABX=mO%UVncUR^{p^9@URG5vk_xO5%mBM{~e5?*RL51Yo7$s)_&A3GQdj--gw+!0Ha%nh>}n{ zjq5v#gPVoXC~t_Go|hqBLZNicN<(6_qPLaMUiS)7&xs4I!%s-Pup5+H{{c^I zr7@!7BVje<$@RDqh&R4RV#)*XkB>vhH>VvR{JD`iYbL@8oA^ce76qct$BRVUzlit> zKBfx?OVG}CBh-JDVz$+9gJi{e9Q!dwR(B<1_sUSLc;Aa2Hc2omd=8$7nS!iS7YU!S zii%|<5$Q!&&|23U!=f)hW2YveL*1li)+B@L_&N%V3zDTcl6tIw$ zhsy2iv71dm+leN&P4+SF=zU>yeL(_z<8J`t<&QCF{ZTTv<1uj0%;9FKHBgDVKR{uO zAy@V0!v~=UWHiK)rZjb7ziclQv{#{KdNy6GSpzva4rI6{k5;YrqQ_zt@$C3rX1_rz zta*T z1WNTPI5%+$=7wgVPk0v<_7(zj-4|fPs{$fK0v*JL@K*I1(qUCV)tY;VgJ?c}maf8P z9W%(vEQ8Dgee|(e5_BH!rp<<*>5Nx9pm<+9s-{-b$G(@r`07=*R!*4f@GTK)=IsHK zyGA&zSC!kpY&RW}76FTIcF=f0k?W-}2mPM+L1+0*+U{PD)ff56r@(SZdwCuXMNOmG z8DaE%%}#6@GlPi@qqvI9gti$c>+0udLeeJ-RDEwucZ8M_=@)D1s@4eDzJ2meN-M-* z(`Y#0ya!T@qR`^99WcvfFxfs26u%0Q7^Ajt!Kx&B9G9JL#83KIDDu26kB^^w#s6bjWrV^oXA! zTJt&3JtEBAJayFQi@|wg$9>Y=GfwX0>=w4pea!`YVrz<*HrN6cT?2plyECDI$8n*a z59q$xfM%UtuxZ0S*i`I%B+BsaFIVT(9E$v3F#~toP)M=n(MR9w35x0l43#k<1P{15eDCAbrqBMr4BM z88REHJ4)cPz7m#dY{AFhn`rWuJ(ydmfybhH$?}mp5Z2VkaGe^kTD$@e7Q|sCk%0KD zjkFMD@z07)IIV*NH%_c2hgOBdqpLwMZmL!{R=T4I`9Kcr_+&dlV%``vg z_})e=9Ybh{aXJ><>xZmP09RK>JU@{N-xMX`auPoZxK5Chlkq=eQ$2I`D#0J-6&QJL z5Pik-=@w-JsMb+No17@Heu=m@qzx8`AA*PySx8;K7?d~ML*s7&Fl_z|FWYQGQRjro zCpRzmu#Y@EIDG`F1`UC8>^@k>2a~Xj2OXSn_0GH4 zr+N(?mJl%cBm^Eys*PF_lI!HE2S~rpJ}S_mOS8ZFqC-mpQE!vyzBXgw?)O3Zy{QDo ztsL;JTs8aJ=reQc^gJ|?_lF*jE7&zA$$gt&z_bKf!AzrDQBm$V#YO>wR+z)A z$Ub&CtA_JtykP6aS7OS>Fsw?I0gcexcs@22a`b1>`!jb^5xJ|Z^Nmni)EGczVv12u zv5xAk@TYsBnsmo>fmd!4nm-t1eck2~9py#1z3V0u#-9z(bhp9Px2~Y88OL5o=TM#7 z*D+=w5I5enhb4#RkT(b9U{p$nzPo8eV_t8?Hl86WA@hut>l`9)ef!Z(`8#G$vxm+6 zO2j|Wn#?e3LwW!;-1h2lfD}f=Yw7L`tW9PHAbhH zpxAj188RJa7IAp6SS<#oucy$ST!JR$aTs4$jDJ@<)7jp4VdMcHKD1s* z>v+J^JDtQ=WhDttH-QcX8&G*FK&FotklUH5$jQ#3!inP;|6ZG2vcHB5G6#&lZs^4% zmtmT-q!uZF2%C{{8&JqR;M-Q+CuXBxCQ@>(teG9wKUWw%TFF@1n ztI*z}1vyfDP|AB8Y9-Ht(q0!lp|A$m9MvZa%uR5ln1h`%6?9dAIw{pAoTj!A}G11sptj}E1b%NvaA&@z<9&EqAqmE~a$bgz0)Pyl;G%CdQ>`9@8 zk?llV&jK%gc#f*il<}vbhco3+_8^gUl+1+s9q}moMiW=+ z?<9QAVMN@NkNfF(4zQ-%fcK9uJYLvFlb!saTk|ZHyJZ9RS|^#FZ|2a=u7ed#zo>7| z2V;MJL-Lht3P+ZDLday_X~iPiH>!=|E$TI?m~_ zf-8l6^xlgjREftAE>D(N%9bC)@K-m{@NFhsJ{Sw{-3q|VuAjPVq{H`j*2vORWJSkM zYTuOwJx1Xq+tnELQn%tm(I#*@EC4bO+lYDMAp66^l;$am!LIBO`1K9-iB9!rU$L`jxs4?v*ne)CHS9MFHcwjEHWk=DFXVF-E-Uo#n z>Y)YNAR>(=dh^2IYVb)9HCfVk$dLH5&SjJf}^jy z94!o#I2(^Wg!GOpQw=pEVn_gA7a)sZ=XC5ZR;{7)NYou%NvM7%t_Jo=Md1KHoQ F{{n}FA%g$_ diff --git a/selfdrive/controls/lib/torque_ml_model.py b/selfdrive/controls/lib/torque_ml_model.py deleted file mode 100644 index d945c863d..000000000 --- a/selfdrive/controls/lib/torque_ml_model.py +++ /dev/null @@ -1,45 +0,0 @@ -import pickle -import numpy as np - - -def _leaky_relu(x: np.ndarray, negative_slope: float = 0.3) -> np.ndarray: - return np.where(x > 0, x, x * negative_slope) - - -class TorqueMLModel: - """ - Minimal Konverter-style Dense+LeakyReLU(0.3) inference. - - Expects: - - weights.npz containing wb = [w_list, b_list] - - norm.pkl containing feature_names/x_center/x_scale/y_center/y_scale - """ - - def __init__(self, weights_npz: str, norm_pkl: str): - wb = np.load(weights_npz, allow_pickle=True) - self.w, self.b = wb["wb"] - - with open(norm_pkl, "rb") as f: - payload = pickle.load(f) - - self.feature_names = list(payload["feature_names"]) - self.x_center = np.asarray(payload["x_center"], dtype=np.float32) - self.x_scale = np.asarray(payload["x_scale"], dtype=np.float32) - self.y_center = float(payload["y_center"]) - self.y_scale = float(payload["y_scale"]) - - # Sanity - if len(self.feature_names) != len(self.x_center) or len(self.x_center) != len(self.x_scale): - raise ValueError("Normalization shape mismatch") - - def predict(self, features: list[float]) -> float: - x = np.asarray(features, dtype=np.float32).reshape((1, -1)) - x = (x - self.x_center) / self.x_scale - - l = x - # Dense, LeakyReLU, Dense, LeakyReLU, Dense(1) - l = _leaky_relu(l @ self.w[0] + self.b[0]) - l = _leaky_relu(l @ self.w[1] + self.b[1]) - y = l @ self.w[2] + self.b[2] - return float(y.reshape(-1)[0] * self.y_scale + self.y_center) -