Skip to content

use sqrt(v_ego) for jerk costs #551

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
sshane wants to merge 2 commits into
base: SA-master
Choose a base branch
from
Open

use sqrt(v_ego) for jerk costs #551

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6ecd2cc
use sqrt(v_ego) for jerk costs
sshane Jan 7, 2022
482b687
if we reduce jerk constraints can we use fully feedforward?
sshane Jan 7, 2022
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
12 changes: 6 additions & 6 deletions selfdrive/car/toyota/tunes.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,12 +33,12 @@ class LatTunes(Enum):
def set_long_tune(tune, name):
# Improved longitudinal tune
if name == LongTunes.TSS2 or name == LongTunes.PEDAL:
tune.deadzoneBP = [0., 8.05]
tune.deadzoneV = [.0, .14]
tune.kpBP = [0., 5., 20.]
tune.kpV = [1.3, 1.0, 0.7]
tune.kiBP = [0., 5., 12., 20., 27.]
tune.kiV = [.35, .23, .20, .17, .1]
tune.deadzoneBP = [0.]
tune.deadzoneV = [0.]
tune.kpBP = [0.]
tune.kpV = [0.1]
tune.kiBP = [0.]
tune.kiV = [0.05]
# Default longitudinal tune
elif name == LongTunes.TSS:
tune.deadzoneBP = [0., 9.]
Expand Down
9 changes: 6 additions & 3 deletions selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#!/usr/bin/env python3
import os
import math
import numpy as np

from common.realtime import sec_since_boot
Expand Down Expand Up @@ -244,10 +245,12 @@ def set_weights_for_lead_policy(self):
x_ego_obstacle_cost_multiplier = 1 # interp(self.desired_TR, TRs, [3., 1.0, 0.1])
j_ego_cost_multiplier = 1 # interp(self.desired_TR, TRs, [0.5, 1.0, 1.0])
d_zone_cost_multiplier = 1 # interp(self.desired_TR, TRs, [4., 1.0, 1.0])
_J_EGO_COST = math.sqrt(max(self.v_ego - 2., 0.)) # reaches 5 at ~60 mph
_A_CHANGE_COST = math.sqrt(max(self.v_ego - 2., 0.)) * 0.2 # reaches 0.5 at ~60 mph

W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST * x_ego_obstacle_cost_multiplier, X_EGO_COST, V_EGO_COST, A_EGO_COST, A_CHANGE_COST, J_EGO_COST * j_ego_cost_multiplier]))
W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST * x_ego_obstacle_cost_multiplier, X_EGO_COST, V_EGO_COST, A_EGO_COST, _A_CHANGE_COST, _J_EGO_COST]))
for i in range(N):
W[4,4] = A_CHANGE_COST * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
W[4,4] = _A_CHANGE_COST * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
self.solver.cost_set(i, 'W', W)
# Setting the slice without the copy make the array not contiguous,
# causing issues with the C interface.
Expand Down Expand Up @@ -322,7 +325,6 @@ def set_accel_limits(self, min_a, max_a):

def set_desired_TR(self, desired_TR):
self.desired_TR = desired_TR
self.set_weights()

def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False):
self.v_ego = carstate.vEgo
Expand All @@ -335,6 +337,7 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False):

if not gh_actions:
self.set_desired_TR(self.dynamic_follow.update(carstate)) # update dynamic follow and get desired TR
self.set_weights()

# set accel limits in params
self.params[:,0] = interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL])
Expand Down