def predict_step_with_ego(self, selected_speed, delta_t, min_crash_distance=5): current_x, current_y = self.ego_position if current_x < control.merge_point2[0]: # Approximate the chane in position as traveling straight towards the merge point direction = np.array([control.merge_point2[0] - current_x, control.merge_point2[1] - current_y]) direction /= np.linalg.norm(direction) direction *= selected_speed * delta_t predicted_x = current_x + direction[0] predicted_y = current_y + direction[1] if predicted_y < -1.6: predicted_y = -1.6 # The lane to merge into is at y=-1.6 else: predicted_y = current_y predicted_x = current_x + selected_speed * delta_t next_acceleration = (selected_speed - self.ego_speed) / delta_t # The ego can't crash into other cars until this point. Not 100% sure if accurate ego_can_crash = control.get_ego_s((predicted_x, predicted_y)) > self.ego_crash_threshold # Also don't let the other cars really react to the ego car until a threshold ego_has_merged = control.get_ego_s((predicted_x, predicted_y)) > self.ego_reaction_threshold # The other cars are in the order from front to back new_other_xs = [] new_other_speeds = [] new_other_accelerations = [] last_x = np.inf last_speed = 0 ego_encountered = False for other_car_index in range(len(self.other_xs)): other_speed = self.other_speeds[other_car_index] other_x = self.other_xs[other_car_index] if other_x < predicted_x and not ego_encountered: ego_encountered = True if ego_has_merged: last_x = predicted_x last_speed = selected_speed speed_diff = last_speed - other_speed x_diff = last_x - other_x if speed_diff < 0 and x_diff < 30: new_other_acceleration = max(speed_diff, Settings.MAX_PREDICTED_DECELERATION) new_other_speed = other_speed + new_other_acceleration * delta_t else: new_other_acceleration = 0 new_other_speed = other_speed predicted_next_position = other_x + new_other_speed * delta_t last_x = predicted_next_position last_speed = new_other_speed new_other_xs.append(predicted_next_position) new_other_speeds.append(new_other_speed) new_other_accelerations.append(new_other_acceleration) crashed = False crash_detection_distance = max(Settings.CAR_LENGTH, min_crash_distance) for x in new_other_xs: if abs(x - predicted_x) < crash_detection_distance and ego_can_crash: crashed = True return HighwayState((predicted_x, predicted_y), selected_speed, next_acceleration, new_other_xs, new_other_speeds, new_other_accelerations), crashed
def st_reward(state, jerk, crashed, arrived): absolute_metric = 0 speed_metric = 0 acceleration_metric = 0 jerk_metric = 0 distance_metric = 0 if crashed: absolute_metric = -10 elif arrived: absolute_metric = 10 else: jerk_metric = -jerk**2 * Settings.TICK_LENGTH ego_speed = state.ego_speed car_ahead, car_behind = state.get_closest_cars() ego_acceleration = state.ego_acceleration ego_position = state.ego_position ego_x, ego_y = ego_position ego_s = control.get_ego_s(ego_position) speed_metric = -Settings.TICK_LENGTH * (ego_speed - Settings.DESIRED_SPEED)**2 acceleration_metric = -Settings.TICK_LENGTH * ego_acceleration**2 if ego_s > 0: if car_ahead is not None: ahead_x, ahead_speed, ahead_acceleration = car_ahead front_distance = ahead_x - ego_x - Settings.CAR_LENGTH else: front_distance = np.inf if car_behind is not None: behind_x, behind_speed, behind_acceleration = car_behind back_distance = ego_x - behind_x - Settings.CAR_LENGTH else: back_distance = np.inf min_distance = min(front_distance, back_distance) if min_distance < Settings.MIN_FOLLOW_DISTANCE: distance_metric = -2 / max(min_distance, 1) elif min_distance == np.inf: distance_metric = 0 elif np.isnan(min_distance): distance_metric = 0 else: distance_metric = -1 / min_distance distance_metric *= Settings.TICK_LENGTH return Settings.ALT_A_WEIGHT * acceleration_metric + Settings.ALT_D_WEIGHT * distance_metric + \ Settings.ALT_J_WEIGHT * jerk_metric + Settings.ALT_V_WEIGHT * speed_metric + absolute_metric
def continuous_reward(state: prediction.HighwayState, jerk, crashed, arrived): absolute_metric = 0 safety_metric = 0 efficiency_metric = 0 smoothness_metric = 0 if crashed: absolute_metric = -10 elif arrived: absolute_metric = 10 else: smoothness_metric = -abs(jerk) * Settings.TICK_LENGTH ego_speed = state.ego_speed car_ahead, car_behind = state.get_closest_cars() ego_position = state.ego_position ego_x, ego_y = ego_position ego_s = control.get_ego_s(ego_position) if ego_s > 0: if car_ahead is not None: ahead_x, ahead_speed, ahead_acceleration = car_ahead front_distance = ahead_x - ego_x - Settings.CAR_LENGTH else: front_distance = np.inf if car_behind is not None: behind_x, behind_speed, behind_acceleration = car_behind back_distance = ego_x - behind_x - Settings.CAR_LENGTH else: back_distance = np.inf min_distance = min(front_distance, back_distance) if min_distance < Settings.MIN_FOLLOW_DISTANCE: safety_metric = -1 elif min_distance == np.inf: safety_metric = 0 elif np.isnan(min_distance): safety_metric = 0 else: safety_metric = -1 / min_distance safety_metric *= Settings.TICK_LENGTH efficiency_metric = -Settings.TICK_LENGTH * np.abs( ego_speed - Settings.DESIRED_SPEED) return Settings.WT_SMOOTH * smoothness_metric + Settings.WT_SAFE * safety_metric + Settings.WT_EFFICIENT * efficiency_metric + absolute_metric
def predict_step_without_ego(self, delta_t, min_crash_distance=5): # Assume ego will just follow the car in front as closely as possible if ego is merge, or ego just stays put otherwise ego_s = control.get_ego_s(self.ego_position) ego_x = self.ego_position[0] if ego_s < self.ego_reaction_threshold or len(self.other_xs) == 0: return self.predict_step_with_ego(0, delta_t, min_crash_distance) elif self.other_xs[0] < ego_x: # If ego car is in front and on highway, pretend it's not there modified_state = HighwayState((-20, -10), 0, 0, self.other_xs, self.other_speeds, self.other_accelerations) return modified_state.predict_step_with_ego(0, delta_t, min_crash_distance) else: last_speed = 0 last_x = 0 for i, car_x in enumerate(self.other_xs): if car_x < ego_x: # Assume the ego car directly follows the car in front of it for the purposes of allocating # space to other vehicles when predicting motion modified_state = HighwayState((last_x - Settings.CAR_LENGTH - 5, self.ego_position[1]), last_speed, 0, self.other_xs, self.other_speeds, self.other_accelerations) return modified_state.predict_step_with_ego(last_speed, delta_t, min_crash_distance) else: last_speed = self.other_speeds[i] last_x = car_x return self.predict_step_with_ego(last_speed, delta_t, min_crash_distance)
def get_s_state(): return control.get_ego_s(control.get_ego_position())
def find_s_t_obstacles_from_state(current_state, future_s=150, delta_s=0.5, delta_t=0.2, time_limit=5, start_uncertainty=0.0, uncertainty_per_second=0.1): ego_position = current_state.ego_position ego_speed = current_state.ego_speed start_s = control.get_ego_s(ego_position) # We discretize the s and t space, and store the lookup for s and t values in these arrays s_values = np.arange(start_s, start_s + future_s + delta_s, delta_s) t_values = np.arange(0, time_limit + delta_t, delta_t) obstacles = np.zeros((t_values.size, s_values.size), dtype=np.bool) distances = np.zeros(obstacles.shape, dtype=np.float) distances += 1E10 # Big number but not NaN discrete_length = int(Settings.CAR_LENGTH / delta_s) predicted_state = current_state for (t_index, t) in enumerate(t_values): uncertainty = start_uncertainty + uncertainty_per_second * t discrete_uncertainty = int(uncertainty / delta_s) if t_index != 0: predicted_state, _ = predicted_state.predict_step_without_ego( delta_t) for vehicle_index, vehicle_x in enumerate(predicted_state.other_xs): current_obstacle_s = control.get_obstacle_s_from_x(vehicle_x) if current_obstacle_s < Settings.CRASH_MIN_S - Settings.MIN_ALLOWED_DISTANCE: break # Cars do not obstruct path until the merge point elif current_obstacle_s > s_values[-1] + Settings.CAR_LENGTH: continue # calculate the distance from each point to this obstacle vehicle at time t obstacle_distances_front = np.abs(s_values - ( current_obstacle_s - Settings.CAR_LENGTH - uncertainty)) obstacle_distances_back = np.abs(s_values - ( current_obstacle_s + Settings.CAR_LENGTH + uncertainty)) # the distance is the minimum of the distance from the front of the ego vehicle to the back of the # obstacle vehicle, and from the front of the obstacle to the back of the ego vehicle distances[t_index] = np.minimum(obstacle_distances_front, distances[t_index, :]) distances[t_index] = np.minimum(obstacle_distances_back, distances[t_index, :]) # Within a vehicle length of the obstacle's s position, register the presence of an obstacle start_s_index = get_range_index(start_s, delta_s, current_obstacle_s) index_min = max( start_s_index - discrete_length - discrete_uncertainty, 0) index_max = min( start_s_index + discrete_length + discrete_uncertainty, s_values.size) if index_min < s_values.size and index_max > 0: obstacles[t_index, index_min:index_max] = True distances[t_index, index_min:index_max] = 0 # plt.close() # plot_s_t_obstacles(obstacles, s_values, t_values) # plt.show() return obstacles, s_values, t_values, ego_speed, distances
def do_combined_control(self, state: prediction.HighwayState) -> float: start_state = state first_action = self.get_control(state) future_action = first_action rollout_s_history = [control.get_ego_s(start_state.ego_position)] crash_predicted = False test_state = None selected_speed = 0 last_choice_rl = True if len(self.takeover_history) > 0 and self.takeover_history[-1]: last_choice_rl = False i = 0 while not (crash_predicted or i >= max(Settings.ROLLOUT_LENGTH, 1)): i += 1 if i != 1: future_action = self.get_control(state) current_speed = state.ego_speed current_acceleration = state.ego_acceleration selected_speed = control.get_ego_speed_from_jerk( current_speed, current_acceleration, future_action) state, crash_predicted = state.predict_step_with_ego( selected_speed, Settings.TICK_LENGTH, Settings.COMBINATION_MIN_DISTANCE) if i == Settings.ST_TEST_ROLLOUTS: test_state = state rollout_s_history.append(control.get_ego_s(state.ego_position)) if state.ego_position[0] > Settings.STOP_X: break if test_state is None: test_state = state if Settings.CHECK_ROLLOUT_CRASH and crash_predicted: print("Crash predicted") self.takeover_history.append(True) return st.do_st_control(start_state) elif Settings.LIMIT_DQN_SPEED and selected_speed > Settings.DESIRED_SPEED: print("DDPG going too fast") self.takeover_history.append(True) return st.do_st_control(start_state) elif Settings.TEST_ROLLOUT_STATE and st.test_guaranteed_crash_from_state( test_state): print("ST solver not happy with rollout state") self.takeover_history.append(True) return st.do_st_control(start_state) elif Settings.TEST_ST_STRICTLY_BETTER: s_sequence, obstacles, s_values, t_values, distances = st.get_appropriate_base_st_path_and_obstacles( start_state) end_point = len(s_sequence) while s_sequence[end_point - 1] == 0: end_point -= 1 s_sequence = s_sequence[:end_point] if Settings.TICK_LENGTH < Settings.T_DISCRETIZATION: s_sequence = st.finer_fit(s_sequence, Settings.TICK_LENGTH, Settings.T_DISCRETIZATION, start_state.ego_speed, start_state.ego_acceleration) # We are essentially on top of another car, so there's not much we can do anyway if len(s_sequence) <= 1: self.takeover_history.append(False) return control.set_ego_jerk(first_action) min_planning_length = min(len(s_sequence), len(rollout_s_history)) st_jerk = st.get_path_mean_abs_jerk( s_sequence[:min_planning_length], start_state.ego_speed, start_state.ego_acceleration, Settings.TICK_LENGTH) rl_jerk = st.get_path_mean_abs_jerk( rollout_s_history[:min_planning_length], start_state.ego_speed, start_state.ego_acceleration, Settings.TICK_LENGTH) st_distance = s_sequence[min_planning_length - 1] - s_sequence[0] rl_distance = rollout_s_history[min_planning_length - 1] - rollout_s_history[0] if last_choice_rl or not Settings.REMEMBER_LAST_CHOICE_FOR_SWITCHING_COMBINED: if (st_jerk < rl_jerk and st_distance > rl_distance) or rl_distance == 0: print("ST Path deemed better") self.takeover_history.append(True) planned_distance_first_step = s_sequence[1] - s_sequence[0] end_speed_first_step = planned_distance_first_step / ( Settings.TICK_LENGTH) control.set_ego_speed(end_speed_first_step) return end_speed_first_step else: self.takeover_history.append(False) return control.set_ego_jerk(first_action) else: if rl_jerk < st_jerk and rl_distance > st_distance: print("RL path deemed better") self.takeover_history.append(False) return control.set_ego_jerk(first_action) else: self.takeover_history.append(True) planned_distance_first_step = s_sequence[1] - s_sequence[0] end_speed_first_step = planned_distance_first_step / ( Settings.TICK_LENGTH) control.set_ego_speed(end_speed_first_step) return end_speed_first_step else: self.takeover_history.append(False) return control.set_ego_jerk(first_action)