diff --git a/common/op_params.py b/common/op_params.py index f8096a299ff864..a3995aa6de59da 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -99,6 +99,19 @@ def __init__(self): self.fork_params = {'camera_offset': Param(0.06, NUMBER, 'Your camera offset to use in lane_planner.py', live=True), 'dynamic_follow': Param('auto', str, static=True, hidden=True), + 'eager_accel': Param(False, [bool, int], 'Experimentalâť— Combats hysteresis in the cruise control system, braking sooner to eliminate jerking\n' + 'Set the param to `1` to use the first method: uses the smoothened derivative of desired accel\n' + 'Setting the param to `2` uses the smoothened jerk of acceleration (tighter control/modification)\n' + 'Try out both and see which is smoother. False disables this feature (so does True)', live=True), + 'accel_eagerness': Param(1.0, NUMBER, 'Multiplier for the acceleration modifier. 1 is full eagerness (default), 0.8 is 80%, etc.', live=True), + + 'accel_time_constant_0_mph': Param(0.01, NUMBER, 'Time constant for eager accel at 0 mph in seconds\n' # todo: tune using these then remove + 'Approaching 0 is no change, the higher it is the more the response\n' + 'The defaults are generally okay', live=True), + 'accel_time_constant_10_mph': Param(0.1, NUMBER, 'Time constant for eager accel at 10 mph in seconds', live=True), + 'accel_time_constant_80_mph': Param(1.0, NUMBER, 'Time constant for eager accel at 80 mph in seconds', live=True), + + # 'replay_accel': Param(False, bool, 'Set param to True while engaged at 20 mph', live=True), 'global_df_mod': Param(1.0, NUMBER, 'The multiplier for the current distance used by dynamic follow. The range is limited from 0.85 to 2.5\n' 'Smaller values will get you closer, larger will get you farther\n' 'This is multiplied by any profile that\'s active. Set to 1. to disable', live=True), diff --git a/misc_scripts/debug/explore_eager.py b/misc_scripts/debug/explore_eager.py new file mode 100644 index 00000000000000..c46486c89334a2 --- /dev/null +++ b/misc_scripts/debug/explore_eager.py @@ -0,0 +1,159 @@ +import matplotlib.pyplot as plt +import os +import numpy as np +import json +import ast +from selfdrive.config import Conversions as CV + +from common.numpy_fast import clip + +DT_CTRL = 0.01 + +# eager_file = 'C:/Users/Shane Smiskol/eager-new.txt' # to compare with and without eagerness +eager_file = 'C:/Users/Shane Smiskol/eager.txt' +with open(eager_file, 'r') as f: + data = f.read() + +data = [ast.literal_eval(l) for l in data.split('\n') if len(l) > 1] + +sequences = [[]] +for idx, line in enumerate(data): + if line['enabled']: # and line['v_ego'] > .25 and (line['apply_accel'] < 0.5 and line['v_ego'] < 8.9 or line['v_ego'] > 8.9): + line['apply_accel'] *= 3 # this is what we want a_ego to match + line['eager_accel'] *= 3 # this is the actual accel sent to the car + sequences[-1].append(line) + elif len(sequences[-1]) != 0 and len(data) - 1 != idx: + sequences.append([]) +del data + +# todo: sequences = [seq for seq in sequences if len(seq) > 5 * 100] + +print('Samples: {}'.format(sum([len(s) for s in sequences]))) +print('Sequences: {}'.format(len(sequences))) + + +# 34, 35, 36 these sequences have eager accel disabled + +def get_alpha_jerk(speed): + RC = np.interp(speed * CV.MS_TO_MPH, [0, 10, 80], [.01, .1, 1]) + return 1. - DT_CTRL / (RC + DT_CTRL) + +def get_alpha_eager(speed): + RC = np.interp(speed * CV.MS_TO_MPH, [0, 10, 80], [.01, .1, 1]) + return 1. - DT_CTRL / (RC + DT_CTRL) + + +def plot_seq(idx=33, title=''): + seq = sequences[idx] + apply_accel, eager_accel, a_ego, v_ego = zip(*[(l['apply_accel'], l['eager_accel'], l['a_ego'], l['v_ego']) for l in seq]) + + # RC_jerk = 0.5 + # alpha_jerk = 1. - DT_CTRL / (RC_jerk + DT_CTRL) + # RC_eager = 0.5 + # alpha_eager = 1. - DT_CTRL / (RC_eager + DT_CTRL) + + # Variables for new eager accel (using jerk) + _delayed_output_jerk = 0 + _delayed_derivative_jerk = 0 + + # For original eager accel + _delayed_output_eager = 0 + + # Variables to visualize (not required in practice) + # _delayed_outputs_jerk = [] + _new_accels_jerk = [] + + # For original eager accel + # _delayed_outputs_eager = [] + _eager_accels = [] + + + # eags = [0] + # accel_with_deriv = [] + # accel_with_sorta_smooth_jerk = [] + # derivatives = [] + # jerks = [] + # sorta_smooth_jerks = [] + # less_smooth_derivative_2 = [] + # original_eager_accel = [] + # jerk_TC = round(0.25 * 100) + for idx, line in enumerate(seq): # todo: left off at trying to plot derivative of accel (jerk) + alpha_jerk = get_alpha_jerk(line['v_ego']) + _delayed_output_jerk = _delayed_output_jerk * alpha_jerk + line['apply_accel'] * (1. - alpha_jerk) + alpha_eager = get_alpha_eager(line['v_ego']) + _delayed_output_eager = _delayed_output_eager * alpha_eager + line['apply_accel'] * (1. - alpha_eager) + + _derivative_jerk = line['apply_accel'] - _delayed_output_jerk + _delayed_derivative_jerk = _delayed_derivative_jerk * alpha_jerk + _derivative_jerk * (1. - alpha_jerk) + + # Visualize + _new_accels_jerk.append(line['apply_accel'] - (_delayed_derivative_jerk - _derivative_jerk)) + _eager_accels.append(line['apply_accel'] + (line['apply_accel'] - _delayed_output_eager)) + + # # _delayed_outputs_jerk.append(_delayed_output_jerk) + # + # original_eager_accel.append(line['apply_accel'] - (_delayed_output - line['apply_accel'])) + # + # + # # todo: edit: accel_with_sorta_smooth_jerk seems promising + # eags.append(eags[-1] * alpha_1 + line['apply_accel'] * (1. - alpha_1)) + # + # less_smooth_derivative_2.append((line['apply_accel'] - eags[-1])) # todo: ideally use two delayed output variables + # if idx > jerk_TC: + # derivatives.append((line['apply_accel'] - seq[idx - jerk_TC]['apply_accel'])) + # jerks.append(derivatives[-1] - derivatives[idx - jerk_TC]) + # sorta_smooth_jerks.append(less_smooth_derivative_2[-1] - less_smooth_derivative_2[idx - jerk_TC]) + # else: + # jerks.append(0) + # derivatives.append(0) + # sorta_smooth_jerks.append(0) + # accel_with_deriv.append(line['apply_accel'] + derivatives[-1] / 10) + # accel_with_sorta_smooth_jerk.append(line['apply_accel'] + sorta_smooth_jerks[-1] / 2) + # # calc_eager_accels.append(line['apply_accel'] - (eag - line['apply_accel']) * 0.5) + + plt.figure() + plt.plot(apply_accel, label='original desired accel') + # plt.plot(a_ego, label='a_ego') + plt.plot(_new_accels_jerk, label='eager accel using jerk') + # plt.plot(eager_accel, label='current eager accel') + # plt.plot(eags, label='exp. average') + # plt.plot(derivatives, label='reg derivative') + # plt.plot(jerks, label='jerk of reg deriv') + # plt.plot(accel_with_sorta_smooth_jerk, label='acc with sorta smooth jerk') + # plt.plot(_eager_accels, label='original eager accel') + # plt.plot(accel_with_deriv, label='acc with true derivative') + + plt.legend() + plt.title(title) + + # plt.figure() + # plt.plot(v_ego, label='v_ego') + # plt.legend() + # plt.title(title) + + + + # calc_eager_accels = [] + # eag = 0 + # for line in seq: + # eag = eag * alpha + line['apply_accel'] * (1. - alpha) + # calc_eager_accels.append(line['apply_accel'] - (eag - line['apply_accel']) * 0.5) + + # plt.plot(apply_accel, label='original desired accel') + # plt.plot(calc_eager_accels, label='calc. accel sent to car') + # plt.plot(a_ego, label='a_ego') + plt.show() + + +plot_seq(14) + +# # to compare with and without eagerness +# # 0 to 6 are good seqs with new data +# plot_seq(2, title='eager 1') # eager +# plot_seq(3, title='eager 2') # eager +# # plot_seq(4) # not eager (todo: think this is bad) +# plot_seq(5, title='not eager 1') # not eager +# plot_seq(6, title='not eager 2') # not eager +# +# # with open('C:/Users/Shane Smiskol/apply_accel_test', 'w') as f: +# # f.write(json.dumps(apply_accel)) diff --git a/helper_scripts/df-test-tuner-mods.py b/misc_scripts/df-test-tuner-mods.py similarity index 100% rename from helper_scripts/df-test-tuner-mods.py rename to misc_scripts/df-test-tuner-mods.py diff --git a/helper_scripts/df-test-tuner-profiles.py b/misc_scripts/df-test-tuner-profiles.py similarity index 100% rename from helper_scripts/df-test-tuner-profiles.py rename to misc_scripts/df-test-tuner-profiles.py diff --git a/helper_scripts/df_profile_mod_tuner.py b/misc_scripts/df_profile_mod_tuner.py similarity index 100% rename from helper_scripts/df_profile_mod_tuner.py rename to misc_scripts/df_profile_mod_tuner.py diff --git a/helper_scripts/dg-test-tuner.py b/misc_scripts/dg-test-tuner.py similarity index 100% rename from helper_scripts/dg-test-tuner.py rename to misc_scripts/dg-test-tuner.py diff --git a/helper_scripts/dg_test.py b/misc_scripts/dg_test.py similarity index 100% rename from helper_scripts/dg_test.py rename to misc_scripts/dg_test.py diff --git a/helper_scripts/dyn_fol_times.py b/misc_scripts/dyn_fol_times.py similarity index 100% rename from helper_scripts/dyn_fol_times.py rename to misc_scripts/dyn_fol_times.py diff --git a/helper_scripts/gas_tuner.py b/misc_scripts/gas_tuner.py similarity index 100% rename from helper_scripts/gas_tuner.py rename to misc_scripts/gas_tuner.py diff --git a/helper_scripts/mpc_tr.py b/misc_scripts/mpc_tr.py similarity index 100% rename from helper_scripts/mpc_tr.py rename to misc_scripts/mpc_tr.py diff --git a/helper_scripts/temp/convert_old_df_mods.py b/misc_scripts/temp/convert_old_df_mods.py similarity index 100% rename from helper_scripts/temp/convert_old_df_mods.py rename to misc_scripts/temp/convert_old_df_mods.py diff --git a/helper_scripts/temp/steer_unavailable_testing.py b/misc_scripts/temp/steer_unavailable_testing.py similarity index 100% rename from helper_scripts/temp/steer_unavailable_testing.py rename to misc_scripts/temp/steer_unavailable_testing.py diff --git a/helper_scripts/train_derivative.py b/misc_scripts/train_derivative.py similarity index 100% rename from helper_scripts/train_derivative.py rename to misc_scripts/train_derivative.py diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index a9f62cb6fd9eca..fafb613d47304e 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,5 +1,6 @@ from cereal import car from common.numpy_fast import clip, interp +from common.realtime import DT_CTRL from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, make_can_msg from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ create_accel_command, create_acc_cancel_command, \ @@ -47,7 +48,10 @@ def __init__(self, dbc_name, CP, VM): self.alert_active = False self.last_standstill = False self.standstill_req = False - self.standstill_hack = opParams().get('standstill_hack') + + self.op_params = opParams() + self.standstill_hack = self.op_params.get('standstill_hack') + self.reset_eager() self.steer_rate_limited = False @@ -59,6 +63,10 @@ def __init__(self, dbc_name, CP, VM): self.packer = CANPacker(dbc_name) + def reset_eager(self): + self.delayed_accel = 0. + self.delayed_derivative = 0. + def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart): @@ -73,7 +81,25 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, # +0.06 offset to reduce ABS pump usage when applying very small gas if apply_accel * CarControllerParams.ACCEL_SCALE > coast_accel(CS.out.vEgo): apply_gas = clip(compute_gb_pedal(apply_accel * CarControllerParams.ACCEL_SCALE, CS.out.vEgo), 0., 1.) - apply_accel = 0.06 - actuators.brake + + + eager_accel_method = self.op_params.get('eager_accel') + if eager_accel_method in [1, 2]: + if not enabled: # reset states + self.reset_eager() + + y = [self.op_params.get(p) for p in ['accel_time_constant_0_mph', 'accel_time_constant_10_mph', 'accel_time_constant_80_mph']] + RC = interp(CS.out.vEgo, [0, 5, 35], y) + alpha = 1. - DT_CTRL / (RC + DT_CTRL) + self.delayed_accel = self.delayed_accel * alpha + apply_accel * (1. - alpha) + + eagerness = self.op_params.get('accel_eagerness') + if eager_accel_method == 1: # new accel is simply accel - change in accel over exponential time (time constant varies with speed) + apply_accel = apply_accel - (self.delayed_accel - apply_accel) * eagerness + else: # subtracting difference in smoothened accel derivative and current derivative (jerk, takes one more variable to keep track of derivative over time but control is more tight) + derivative = apply_accel - self.delayed_accel # store change in accel over some time constant (using exponential moving avg.) + self.delayed_derivative = self.delayed_derivative * alpha + derivative * (1. - alpha) # calc exp. moving average for derivative + apply_accel = apply_accel - (self.delayed_derivative - derivative) * eagerness # then modify accel using jerk of accel apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled) apply_accel = clip(apply_accel * CarControllerParams.ACCEL_SCALE, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 92cf6b0db46ffd..329830c936a78d 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -109,7 +109,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, 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.04, 0.1]] # 45 to 70 mph + ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kpV = [[20, 31], [0.05, 0.12]] # 45 to 70 mph ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[20, 31], [0.001, 0.01]] ret.lateralTuning.pid.kdBP, ret.lateralTuning.pid.kdV = [[20, 31], [0.0, 0.0]] ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594