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_efficiency_cost(a, max_v, T, trajectory):
"""
Rewards high average speeds.
"""
t = T
dist = trajectory.end[0] - trajectory.start[0]
t = t + (trajectory.full_distance - dist) / max_v
avg_v = trajectory.full_distance / t
return logistic(2 * float(max_v - avg_v) / avg_v)
def buffer_cost(traj, target_vehicle, delta, T, predictions):
"""
Penalizes getting close to other vehicles.
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 }
"""
nearest = nearest_approach_to_any_vehicle(traj, predictions)
return logistic(2 * VEHICLE_RADIUS / nearest)
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_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 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 time_diff_cost(traj, target_vehicle, delta, T, predictions):
"""
Penalizes trajectories that span a duration which is longer or
shorter than the duration requested.
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 }
"""
_, _, t = traj
return logistic(float(abs(t - T)) / T)
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 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 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_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 buffer_cost(traj, other_vehicles, cnt_points=100):
"""
Penalizes getting close to other vehicles.
"""
nearest = 9999
T = traj[-1]
q = traj[:2]
for i in range(cnt_points):
t = T * float(i) / cnt_points
s = sum([q[0][j] * t**j for j in range(6)])
d = sum([q[1][j] * t**j for j in range(6)])
other_sd = [np.array(v.state_in(t))[[0, 3]] for v in other_vehicles]
tmp_nearst = min([(((s1 - s) / 2)**2 + (d1 - d)**2)**(0.5)
for s1, d1 in other_sd])
nearest = min([tmp_nearst, nearest])
#print (nearest)
return logistic(2 * VEHICLE_RADIUS / nearest)
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 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 s_diff_cost(traj, target_vehicle, delta, T, predictions):
"""
Penalizes trajectories whose s 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 }
"""
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 s_diff_cost(traj, target_vehicle, delta, T, predictions):
# print("s_diff_cost: ", T)
"""
Penalizes trajectories whose s coordinate (and derivatives)
differ from the goal.
"""
s, _, T = traj
target = predictions[target_vehicle].state_in(
T) # target vehicle's state at time T
target = list(np.array(target) + np.array(delta)) # my desired position
s_targ = target[:3] # target's s vals
S = [f(T) for f in get_f_and_N_derivatives(s, 2)
] # calculate the s vals at time T
# print("S: ", S)
cost = 0
for actual, expected, sigma in zip(S, s_targ, SIGMA_S):
diff = float(
abs(actual - expected)
) # difference between the my desired s vals and target vehicle's s vals
cost += logistic(
diff / sigma
) # how off were they? if the vehicle the desired distance away from me
return cost # cost of how off the calculated values are from the expected value
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 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)