def d_diff_cost(traj, target_vehicle, delta, T, predictions):
# print("d_diff_cost: ", T)
"""
Penalizes trajectories whose d coordinate (and derivatives)
differ from the goal.
"""
_, d_coeffs, T = traj
d_dot_coeffs = differentiate(d_coeffs) # calculates velocity
d_ddot_coeffs = differentiate(d_dot_coeffs) # calculates acceleration
d = to_equation(d_coeffs) # position
d_dot = to_equation(d_dot_coeffs) # velocity
d_ddot = to_equation(d_ddot_coeffs) # acceleration
D = [d(T), d_dot(T),
d_ddot(T)] # functions to calculate the d vals at time 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 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 d_diff_cost(traj, target_vehicle, delta, T, predictions):
"""
Penalizes trajectories whose d coordinate (and derivatives)
differ from the goal.
Args:
trajectory (list): list of tuple([s_coefficients, d_coefficients, t])
target_vehicle (int): the label of the target vehicle
delta (list): [s, s_dot, s_double_dot, d, d_dot, d_double_dot]
goal_t (float): the require time to get to the goal
predictions (dict): dictionary of {v_id : vehicle }
"""
_, 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 nearest_approach(traj, vehicle):
closest = 999999
s_, d_, T = traj
s = to_equation(s_)
d = to_equation(d_)
for i in range(100):
t = float(i) / 100 * T
cur_s = s(t)
cur_d = d(t)
targ_s, _, _, targ_d, _, _ = vehicle.state_in(t)
dist = sqrt((cur_s - targ_s)**2 + (cur_d - targ_d)**2)
if dist < closest:
closest = dist
return closest
def max_accel_cost(traj):
s, d, T = traj
s_dot = differentiate(s)
s_d_dot = differentiate(s_dot)
a_s = to_equation(s_d_dot)
d_dot = differentiate(d)
d_d_dot = differentiate(d_dot)
a_d = to_equation(d_d_dot)
all_accs = [
a_s(float(T) / 100 * i) + a_d(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_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_accel_cost(a, max_v, T, trajectory):
s = a
s_dot = differentiate(s)
s_d_dot = differentiate(s_dot)
a = to_equation(s_d_dot)
all_accs = [a(float(T) / 500 * i) for i in range(500)]
max_acc = max(all_accs, key=abs)
if abs(max_acc) > MAX_ACCEL: return 1
else: return 0
def efficiency_cost(a, max_v, T, trajectory):
"""
Rewards high average speeds.
"""
t = T
s = a
s = to_equation(s)
avg_v = float(s(t)) / t
return logistic(2 * float(max_v - avg_v) / avg_v)
def min_speed_cost(traj, target_vehicle, delta, T, predictions):
s, _, t = traj
speed = differentiate(s)
speed = to_equation(speed)
all_speeds = [speed(float(t)/100 * i) for i in range(100)]
min_speed = min(all_speeds)
if min_speed < MIN_SPEED: return 1
else: return 0
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 max_jerk_cost(a, max_v, T, trajectory):
s = a
s_dot = differentiate(s)
s_d_dot = differentiate(s_dot)
jerk = differentiate(s_d_dot)
jerk = to_equation(jerk)
all_jerks = [jerk(float(T) / 500 * i) for i in range(500)]
max_jerk = max(all_jerks, key=abs)
if abs(max_jerk) > MAX_JERK: return 1
else: return 0
def min_velocity_cost(a, max_v, T, trajectory):
s = a
s_dot = differentiate(s)
v = to_equation(s_dot)
all_vels = [v(float(T) / 500 * i) for i in range(500)]
min_vel = min(all_vels)
if min_vel < -0.001:
return 1
else:
return 0
def total_accel_cost(traj):
s, d, T = traj
s_dot = differentiate(s)
s_d_dot = differentiate(s_dot)
a_s = to_equation(s_d_dot)
d_dot = differentiate(d)
d_d_dot = differentiate(d_dot)
a_d = to_equation(d_d_dot)
total_acc = 0
dt = float(T) / 100.0
for i in range(100):
t = dt * i
acc = a_s(t) + a_d(t)
total_acc += abs(acc * dt)
acc_per_second = total_acc / T
return logistic(acc_per_second / EXPECTED_ACC_IN_ONE_SEC)
def max_velocity_cost(a, max_v, T, trajectory):
s = a
s_dot = differentiate(s)
v = to_equation(s_dot)
all_vels = [v(float(T) / 500 * i) for i in range(500)]
max_vel = max(all_vels, key=abs)
if abs(max_vel) > max_v + 0.1:
return 1
else:
return 0
def stays_on_road_cost(traj, target_vehicle, delta, T, predictions):
_, d_coeffs, t = traj
d = to_equation(d_coeffs)
all_ds = [d(float(t)/100 * i) for i in range(100)]
if max(all_ds) <= MAX_D and min(all_ds) >=MIN_D:
return 0
else:
return 1
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 max_jerk_cost(traj):
s, d, T = traj
s_dot = differentiate(s)
s_d_dot = differentiate(s_dot)
jerk = differentiate(s_d_dot)
jerk_s = to_equation(jerk)
d_dot = differentiate(d)
d_d_dot = differentiate(d_dot)
jerk = differentiate(d_d_dot)
jerk_d = to_equation(jerk)
all_jerks = [
jerk_s(float(T) / 100 * i) + jerk_d(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 show_trajectory(s_coeffs, d_coeffs, T, vehicle=None):
s = to_equation(s_coeffs)
d = to_equation(d_coeffs)
X = []
Y = []
if vehicle:
X2 = []
Y2 = []
t = 0
while t <= T + 0.01:
X.append(s(t))
Y.append(d(t))
if vehicle:
s_, _, _, d_, _, _ = vehicle.state_in(t)
X2.append(s_)
Y2.append(d_)
t += 0.25
plt.scatter(X, Y, color="blue")
if vehicle:
plt.scatter(X2, Y2, color="red")
plt.show()
def exceeds_speed_limit_cost(traj, target_vehicle, delta, T, predictions):
s, _, t = traj
speed = differentiate(s)
speed = to_equation(speed)
all_speeds = [speed(float(t)/100 * i) for i in range(100)]
max_speed = max(all_speeds)
if max_speed > SPEED_LIMIT:
return 1
else:
return 0
def total_jerk_cost(traj):
s, d, t = traj
T = t
s_dot = differentiate(s)
s_d_dot = differentiate(s_dot)
jerk = differentiate(s_d_dot)
jerk_s = to_equation(jerk)
d_dot = differentiate(d)
d_d_dot = differentiate(d_dot)
jerk = differentiate(d_d_dot)
jerk_d = to_equation(jerk)
total_jerk = 0
dt = float(T) / 100.0
for i in range(100):
t = dt * i
j = abs(jerk_s(t)) + abs(jerk_d(t))
total_jerk += abs(j * dt)
jerk_per_second = total_jerk / T
return logistic(jerk_per_second / EXPECTED_JERK_IN_ONE_SEC)
def max_accel_cost_d(traj, target_vehicle, delta, T, predictions):
"""
Penalizes the maximum accleration in d-direction
"""
s, d, t = traj
d_dot = differentiate(d)
d_d_dot = differentiate(d_dot)
a_d = to_equation(d_d_dot)
all_accs = [a_d(float(T) / 100 * i) for i in range(100)]
max_acc = max(all_accs, key=abs)
if abs(max_acc) > MAX_ACCEL_d: return 1
else: return 0
def total_jerk_cost(a, max_v, T, trajectory):
s = a
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)
def exceeds_speed_limit_cost(traj, target_vehicle, delta, T, predictions):
"""
Penalizes exceeding speed limit
"""
s_coeffs = traj[0]
s_dot_coeffs = differentiate(s_coeffs)
velocity = to_equation(s_dot_coeffs)
nb_steps = 101
for time in np.linspace(0, T, nb_steps):
if velocity(time) > SPEED_LIMIT:
return 1.0
return 0.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)
def total_accel_cost(a, max_v, T, trajectory):
s = a
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 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 exceeds_speed_limit_cost(traj, target_vehicle, delta, T, predictions):
s, _, t = traj
s = to_equation(s)
v = float(s(t)) / t
target_speed = 0.95 * SPEED_LIMIT
stop_cost = 0.8
if v < target_speed:
cost = stop_cost * ((target_speed- v)/target_speed)
else : cost = 1
return cost
def total_accel_cost_d(traj, target_vehicle, delta, T, predictions):
"""
Penalizes total amount of acceleration in 1 sec in d-direction
"""
s, d, t = traj
d_dot = differentiate(d)
d_d_dot = differentiate(d_dot)
a_d = to_equation(d_d_dot)
total_acc = 0
dt = float(T) / 100.0
for i in range(100):
t = dt * i
acc_d = a_d(t)
total_acc += abs(acc_d * dt)
acc_per_second = total_acc / T
return logistic(acc_per_second / EXPECTED_ACC_IN_ONE_SEC)
def total_jerk_cost(traj, target_vehicle, delta, T, predictions):
# print("total_jerk_cost: ", T)
s, d, t = traj
s_dot = differentiate(s) # velocity
s_d_dot = differentiate(s_dot) # acceleration
jerk = to_equation(differentiate(s_d_dot)) # jerk
total_jerk = 0
dt = float(T) / 100.0 # .05
for i in range(100): # 0 to 99
t = dt * i
# print("t:", t)
j = jerk(t) # jerk at time t
# print("jerk: ", j)
# print("abs: ", abs(j*dt))
# total_jerk += abs(j*dt)
# print("abs: ", abs(j))
total_jerk += abs(j)
# jerk_per_second = total_jerk / T
jerk_per_second = total_jerk / 100 # average jerk
# print("jerk per second: ", jerk_per_second)
return logistic(jerk_per_second / EXPECTED_JERK_IN_ONE_SEC)
def exceeds_speed_limit_cost(traj, target_vehicle, delta, T, predictions):
s, _, _ = traj
#calculate coefficients for speed
s_dot = differentiate(s)
#make a polynomial function of time for speed
s_dot_equation = to_equation(s_dot)
all_speeds = []
for i in range(100):
#take i% of total time T
t = float(T) / 100 * i
#calculate speed at given timestep
v = s_dot_equation(t)
#add this speed to list of all speeds
all_speeds.append(v)
max_v = max(all_speeds, key=abs)
#check if it exceeds SPEED_LIMIT
if (abs(max_v) > SPEED_LIMIT):
return 1
return 0
