Experimental feature: eager accel (commaai#382)

* add eager accel (adding derivative using accel's exponential moving average)

* make sure to reset both when not engaged

* gather data

* keyframe

* use jerk of accel

* replay acceleration lists

* add torque model

* Fix

* update

* revert unnecessary changes

* snapshot

* prepare for merge

* prepare for merge 2

* prepare for merge 3

* prepare for merge 4

* steering should be in another PR

* move to folder

* comment

* simplify descriptions

* revert

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),

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))

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)

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