Exemplo n.º 1
0
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
Exemplo n.º 2
0
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