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 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 test_derivative(self): Tvals = np.linspace(np.min(self.T), np.max(self.T))[1:-1] for T in Tvals: self.assertTrue(np.isclose(self.obj.dconductivity(T), differentiate(lambda x: self.obj.conductivity(x), T))) self.assertTrue(np.isclose(self.obj.ddiffusivity(T), differentiate(lambda x: self.obj.diffusivity(x), T)))
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 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 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_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 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(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 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 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 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 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 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 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 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, 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): """ 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 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
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 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 get_f_and_N_derivatives(coeffs, N=3): functions = [to_equation(coeffs)] for i in range(N): coeffs = differentiate(coeffs) functions.append(to_equation(coeffs)) return functions
def test_derivtive(self): for T in self.Trange: self.assertAlmostEqual(self.obj.dcoefficient(self.mat, T), differentiate(lambda x: self.obj.coefficient(self.mat, x), T))