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 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
