Skip to content

Revert "Add dynamic mods back to dynamic follow (#483)" #486

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: SA-master-0810
Choose a base branch
from
Open

Revert "Add dynamic mods back to dynamic follow (#483)" #486

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
84 changes: 44 additions & 40 deletions selfdrive/controls/lib/dynamic_follow/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
import math
import numpy as np
import cereal.messaging as messaging
from common.realtime import sec_since_boot, DT_MDL
from common.realtime import sec_since_boot
from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
from common.op_params import opParams
from common.numpy_fast import interp, clip, mean
Expand All @@ -17,6 +17,8 @@

class DistanceModController:
def __init__(self, k_i, k_d, x_clip, mods):
self._rate = 1 / 20.

self._k_i = k_i
self._k_d = k_d
self._to_clip = x_clip # reaches this with v_rel=3.5 mph for 4 seconds
Expand All @@ -34,7 +36,7 @@ def update(self, error):
if (d := self._k_d * (error - self.last_error)) < 0: # only add if it will add distance
self.i += d

self.i += error * DT_MDL * self._k_i
self.i += error * self._rate * self._k_i
self.i = clip(self.i, self._to_clip[0], self._to_clip[-1]) # clip to reasonable range
self._slow_reset() # slowly reset from max to 0

Expand All @@ -48,7 +50,7 @@ def _slow_reset(self):
if abs(self.i) > 0.01: # oscillation starts around 0.006
reset_time = 15 # in x seconds i goes from max to 0
sign = 1 if self.i > 0 else -1
self.i -= sign * max(self._to_clip) / (reset_time / DT_MDL)
self.i -= sign * max(self._to_clip) / (reset_time / self._rate)


class DynamicFollow:
Expand All @@ -66,10 +68,11 @@ def __init__(self, mpc_id):
self.pm = None

# Model variables
mpc_rate = 1 / 20.
self.model_scales = {'v_ego': [-0.06112159043550491, 37.96522521972656], 'a_lead': [-3.109330892562866, 3.3612186908721924], 'v_lead': [0.0, 35.27671432495117], 'x_lead': [2.4600000381469727, 141.44000244140625]}
self.predict_rate = 1 / 4.
self.skip_every = round(0.25 / DT_MDL)
self.model_input_len = round(45 / DT_MDL)
self.skip_every = round(0.25 / mpc_rate)
self.model_input_len = round(45 / mpc_rate)

# Dynamic follow variables
self.default_TR = 1.8
Expand Down Expand Up @@ -287,6 +290,10 @@ def _get_TR(self):
v_rel_dist_factor = self.dmc_v_rel.update(self.lead_data.v_lead - self.car_data.v_ego)
a_lead_dist_factor = self.dmc_a_rel.update(self.lead_data.a_lead - self.car_data.a_ego)

TR = interp(self.car_data.v_ego, x_vel, y_dist)
TR *= v_rel_dist_factor
TR *= a_lead_dist_factor

if self.car_data.v_ego > self.sng_speed: # keep sng distance until we're above sng speed again
self.sng = False

Expand All @@ -299,43 +306,40 @@ def _get_TR(self):
y = [self.sng_TR, interp(self.sng_speed, x_vel, y_dist)]
TR = interp(self.car_data.v_ego, x, y)

TR *= v_rel_dist_factor
TR *= a_lead_dist_factor

return float(clip(TR, self.min_TR, 2.7))

# TR_mods = []
# # Dynamic follow modifications (the secret sauce)
# x = [-26, -15.6464, -9.8422, -6.0, -4.0, -2.68, -2.3, -1.8, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68, 4.4704] # relative velocity values
# y = [1.76, 1.504, 1.34, 1.29, 1.25, 1.22, 1.19, 1.13, 1.053, 1.017, 1.0, 0.985, 0.958, 0.87, 0.81, 0.685] # multiplier values
# y = np.array(y) - 1 # converts back to original abs mod
# y *= 1.1 # multiplier for how much to mod
# y = y / TR + 1 # converts back to multipliers
# TR_mods.append(interp(self.lead_data.v_lead - self.car_data.v_ego, x, y))
#
# x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466, 0.5144, 0.6903, 0.9302] # lead acceleration values
# y = [1.16, 1.1067, 1.0613, 1.0343, 1.0203, 1.0147, 1.0, 0.9898, 0.972, 0.9647, 0.9607] # multiplier values
# converted_with_TR = 1.5 # todo: do without numpy and simplify by converting with TR of 1, so only subtract
# absolute_y_TR_mod = np.array(y) * converted_with_TR - converted_with_TR # converts back to original abs mod
# absolute_y_TR_mod *= 1.2 # multiplier for how much to mod
# y = absolute_y_TR_mod / TR + 1 # converts back to multipliers with accel mod of 1.4 taking current TR into account
# TR_mods.append(interp(self.get_rel_accel(), x, y)) # todo: make this over more than 1 sec
#
# # deadzone = self.car_data.v_ego / 3 # 10 mph at 30 mph # todo: tune pedal to react similarly to without before adding/testing this
# # if self.lead_data.v_lead - deadzone > self.car_data.v_ego:
# # TR_mods.append(self._relative_accel_mod())
#
# # x = [self.sng_speed, self.sng_speed / 5.0] # as we approach 0, apply x% more distance
# # y = [1.0, 1.05]
#
# TR *= mean(TR_mods) # with mods as multipliers, profile mods shouldn't be needed
#
# # if (self.car_data.left_blinker or self.car_data.right_blinker) and df_profile != self.df_profiles.traffic:
# # x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph
# # y = [1.0, .75, .65]
# # TR *= interp(self.car_data.v_ego, x, y) # reduce TR when changing lanes
#
# return float(clip(TR, self.min_TR, 2.7))
TR_mods = []
# Dynamic follow modifications (the secret sauce)
x = [-26, -15.6464, -9.8422, -6.0, -4.0, -2.68, -2.3, -1.8, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68, 4.4704] # relative velocity values
y = [1.76, 1.504, 1.34, 1.29, 1.25, 1.22, 1.19, 1.13, 1.053, 1.017, 1.0, 0.985, 0.958, 0.87, 0.81, 0.685] # multiplier values
y = np.array(y) - 1 # converts back to original abs mod
y *= 1.1 # multiplier for how much to mod
y = y / TR + 1 # converts back to multipliers
TR_mods.append(interp(self.lead_data.v_lead - self.car_data.v_ego, x, y))

x = [-4.4795, -2.8122, -1.5727, -1.1129, -0.6611, -0.2692, 0.0, 0.1466, 0.5144, 0.6903, 0.9302] # lead acceleration values
y = [1.16, 1.1067, 1.0613, 1.0343, 1.0203, 1.0147, 1.0, 0.9898, 0.972, 0.9647, 0.9607] # multiplier values
converted_with_TR = 1.5 # todo: do without numpy and simplify by converting with TR of 1, so only subtract
absolute_y_TR_mod = np.array(y) * converted_with_TR - converted_with_TR # converts back to original abs mod
absolute_y_TR_mod *= 1.2 # multiplier for how much to mod
y = absolute_y_TR_mod / TR + 1 # converts back to multipliers with accel mod of 1.4 taking current TR into account
TR_mods.append(interp(self.get_rel_accel(), x, y)) # todo: make this over more than 1 sec

# deadzone = self.car_data.v_ego / 3 # 10 mph at 30 mph # todo: tune pedal to react similarly to without before adding/testing this
# if self.lead_data.v_lead - deadzone > self.car_data.v_ego:
# TR_mods.append(self._relative_accel_mod())

# x = [self.sng_speed, self.sng_speed / 5.0] # as we approach 0, apply x% more distance
# y = [1.0, 1.05]

TR *= mean(TR_mods) # with mods as multipliers, profile mods shouldn't be needed

# if (self.car_data.left_blinker or self.car_data.right_blinker) and df_profile != self.df_profiles.traffic:
# x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph
# y = [1.0, .75, .65]
# TR *= interp(self.car_data.v_ego, x, y) # reduce TR when changing lanes

return float(clip(TR, self.min_TR, 2.7))

def update_lead(self, v_lead=None, a_lead=None, x_lead=None, status=False, new_lead=False):
self.lead_data.v_lead = v_lead
Expand Down