-
Notifications
You must be signed in to change notification settings - Fork 0
/
cost_functions.py
154 lines (124 loc) · 4.26 KB
/
cost_functions.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
import numpy as np
from constants import *
from helpers import logistic, to_equation, differentiate, nearest_approach_to_any_vehicle, get_f_and_N_derivatives
# COST FUNCTIONS
def time_diff_cost(traj, target_vehicle, delta, T, predictions):
"""
Penalizes trajectories that span a duration which is longer or
shorter than the duration requested.
"""
_, _, t = traj
return logistic(float(abs(t - T)) / T)
def s_diff_cost(traj, target_vehicle, delta, T, predictions):
"""
Penalizes trajectories whose s coordinate (and derivatives)
differ from the goal.
"""
s, _, T = traj
target = predictions[target_vehicle].state_in(T)
target = list(np.array(target) + np.array(delta))
s_targ = target[:3]
S = [f(T) for f in get_f_and_N_derivatives(s, 2)]
cost = 0
for actual, expected, sigma in zip(S, s_targ, SIGMA_S):
diff = float(abs(actual - expected))
cost += logistic(diff / sigma)
return cost
def d_diff_cost(traj, target_vehicle, delta, T, predictions):
"""
Penalizes trajectories whose d coordinate (and derivatives)
differ from the goal.
"""
_, d_coeffs, T = traj
d_dot_coeffs = differentiate(d_coeffs)
d_ddot_coeffs = differentiate(d_dot_coeffs)
d = to_equation(d_coeffs)
d_dot = to_equation(d_dot_coeffs)
d_ddot = to_equation(d_ddot_coeffs)
D = [d(T), d_dot(T), d_ddot(T)]
target = predictions[target_vehicle].state_in(T)
target = list(np.array(target) + np.array(delta))
d_targ = target[3:]
cost = 0
for actual, expected, sigma in zip(D, d_targ, SIGMA_D):
diff = float(abs(actual - expected))
cost += logistic(diff / sigma)
return cost
def collision_cost(traj, target_vehicle, delta, T, predictions):
"""
Binary cost function which penalizes collisions.
"""
nearest = nearest_approach_to_any_vehicle(traj, predictions)
if nearest < 2 * VEHICLE_RADIUS:
return 1.0
else:
return 0.0
def buffer_cost(traj, target_vehicle, delta, T, predictions):
"""
Penalizes getting close to other vehicles.
"""
nearest = nearest_approach_to_any_vehicle(traj, predictions)
return logistic(2 * VEHICLE_RADIUS / nearest)
def stays_on_road_cost(traj, target_vehicle, delta, T, predictions):
pass
def exceeds_speed_limit_cost(traj, target_vehicle, delta, T, predictions):
pass
def efficiency_cost(traj, target_vehicle, delta, T, predictions):
"""
Rewards high average speeds.
"""
s, _, t = traj
s = to_equation(s)
avg_v = float(s(t)) / t
targ_s, _, _, _, _, _ = predictions[target_vehicle].state_in(t)
targ_v = float(targ_s) / t
return logistic(2 * float(targ_v - avg_v) / avg_v)
def total_accel_cost(traj, target_vehicle, delta, T, predictions):
s, d, t = traj
s_dot = differentiate(s)
s_d_dot = differentiate(s_dot)
a = to_equation(s_d_dot)
total_acc = 0
dt = float(T) / 100.0
for i in range(100):
t = dt * i
acc = a(t)
total_acc += abs(acc * dt)
acc_per_second = total_acc / T
return logistic(acc_per_second / EXPECTED_ACC_IN_ONE_SEC)
def max_accel_cost(traj, target_vehicle, delta, T, predictions):
s, d, t = traj
s_dot = differentiate(s)
s_d_dot = differentiate(s_dot)
a = to_equation(s_d_dot)
all_accs = [a(float(T) / 100 * i) for i in range(100)]
max_acc = max(all_accs, key=abs)
if abs(max_acc) > MAX_ACCEL:
return 1
else:
return 0
def max_jerk_cost(traj, target_vehicle, delta, T, predictions):
s, d, t = traj
s_dot = differentiate(s)
s_d_dot = differentiate(s_dot)
jerk = differentiate(s_d_dot)
jerk = to_equation(jerk)
all_jerks = [jerk(float(T) / 100 * i) for i in range(100)]
max_jerk = max(all_jerks, key=abs)
if abs(max_jerk) > MAX_JERK:
return 1
else:
return 0
def total_jerk_cost(traj, target_vehicle, delta, T, predictions):
s, d, t = traj
s_dot = differentiate(s)
s_d_dot = differentiate(s_dot)
jerk = to_equation(differentiate(s_d_dot))
total_jerk = 0
dt = float(T) / 100.0
for i in range(100):
t = dt * i
j = jerk(t)
total_jerk += abs(j * dt)
jerk_per_second = total_jerk / T
return logistic(jerk_per_second / EXPECTED_JERK_IN_ONE_SEC)