def sample_navigable_point(sim: Simulator, floor_coord: List[float], max_dist: float = 0.3, max_tries: float = 100): """ Get sample of point only on ground """ for _ in range(max_tries): source_position = sim.sample_navigable_point() dist = source_position[1] - floor_coord if np.min(np.abs(dist)) < max_dist: # TODO Fix points on ceileing return source_position return None
def generate_pointnav_episode( sim: Simulator, target: List = None, # Specify target Coord num_episodes: int = -1, is_gen_shortest_path: bool = True, shortest_path_success_distance: float = 0.2, shortest_path_max_steps: int = 500, closest_dist_limit: float = 1, furthest_dist_limit: float = 30, geodesic_to_euclid_min_ratio: float = 1.1, geodesic_min_ratio_prob: float = 0.01, number_retries_per_target: int = 50, floor_coord: List[float] = None, max_samples_multi_target: int = 40, spiral_coord: np.ndarray = None, ) -> NavigationEpisode: r"""Generator function that generates PointGoal navigation episodes. An episode is trivial if there is an obstacle-free, straight line between the start and goal positions. A good measure of the navigation complexity of an episode is the ratio of geodesic shortest path position to Euclidean distance between start and goal positions to the corresponding Euclidean distance. If the ratio is nearly 1, it indicates there are few obstacles, and the episode is easy; if the ratio is larger than 1, the episode is difficult because strategic navigation is required. To keep the navigation complexity of the precomputed episodes reasonably high, we perform aggressive rejection sampling for episodes with the above ratio falling in the range [1, 1.1]. Following this, there is a significant decrease in the number of straight-line episodes. :param sim: simulator with loaded scene for generation. :param num_episodes: number of episodes needed to generate :param is_gen_shortest_path: option to generate shortest paths :param shortest_path_success_distance: success distance when agent should stop during shortest path generation :param shortest_path_max_steps maximum number of steps shortest path expected to be :param closest_dist_limit episode geodesic distance lowest limit :param furthest_dist_limit episode geodesic distance highest limit :param geodesic_to_euclid_min_ratio geodesic shortest path to Euclid distance ratio upper limit till aggressive sampling is applied. :return: navigation episode that satisfy specified distribution for currently loaded into simulator scene. """ episode_count = 0 m_t = None target_position = None floor_coord = np.array(floor_coord) target_idx = 0 error_code = [] while episode_count < num_episodes or num_episodes < 0: if target_position is None: target_position = sim.sample_navigable_point() if target is None \ else target if sim.island_radius(target_position) < ISLAND_RADIUS_LIMIT and \ target is None: continue if m_t is not None: target_idx = np.random.randint(0, len(m_t)) target_position = m_t[target_idx] found_episode = False episode = None for retry in range(number_retries_per_target): source_position = sample_navigable_point(sim, floor_coord) min_r = 0 if np.random.rand() < geodesic_min_ratio_prob else \ geodesic_to_euclid_min_ratio is_compatible, dist = is_compatible_episode( source_position, target_position, sim, near_dist=closest_dist_limit, far_dist=furthest_dist_limit, geodesic_to_euclid_ratio=min_r, ) if is_compatible: angle = np.random.uniform(0, 2 * np.pi) source_rotation = [0, np.sin(angle / 2), 0, np.cos(angle / 2)] shortest_paths = None if is_gen_shortest_path: shortest_paths = [ get_action_shortest_path( sim, source_position=source_position, source_rotation=source_rotation, goal_position=target_position, success_distance=shortest_path_success_distance, max_episode_steps=shortest_path_max_steps, ) ] episode = _create_episode( episode_id=episode_count, scene_id=sim.config.SCENE, start_position=source_position, start_rotation=source_rotation, target_position=target_position, shortest_paths=shortest_paths, radius=shortest_path_success_distance, info={"geodesic_distance": dist}, multi_target=m_t) episode.target_idx = target_idx episode.error_code = error_code episode_count += 1 found_episode = True break else: error_code.append(dist) if not found_episode and m_t is None: print("Can't reach actual object coordinate, try to find " "shortest reachable points") min_dist = 0.2 # Get floor height of object floor_h = sample_navigable_point(sim, floor_coord)[1] # Generate points on a spiral spiral_pts = spiral_coord + np.array([target[0], target[2]]) n_pts = [] for pt in spiral_pts: npt = [pt[0], floor_h, pt[1]] is_nav = sim.is_navigable(npt) if is_nav: n_pts.append(npt) n_pts = np.array(n_pts) eucl_distance = \ np.linalg.norm(n_pts - np.array(target), axis=1) sortidx = np.argsort(eucl_distance) n_pts = n_pts[sortidx] m_t = [] first_dist = None while len(m_t) < NO_MULTI_GOALS and len(n_pts) > 0: # check reachebilty reachable = False while not reachable: for _ in range(10): check_p = sample_navigable_point(sim, floor_coord) d_sep = sim.geodesic_distance(n_pts[0], check_p) if d_sep != np.inf: reachable = True if not reachable: n_pts = n_pts[1:] if first_dist is None: first_dist = np.linalg.norm(target - n_pts[0]) + min_dist else: if np.linalg.norm(target - n_pts[0]) > first_dist: break m_t.append(n_pts[0]) n_pts = n_pts[1:] n_cnt = len(n_pts) m_cnt = len(m_t) mtt = np.array(m_t) d = np.linalg.norm( np.repeat(np.expand_dims(n_pts, 1), m_cnt, 1).reshape( -1, 3) - np.repeat(np.expand_dims(mtt, 0), n_cnt, 0).reshape(-1, 3), axis=1) select = (d.reshape( n_cnt, m_cnt, ) > 1).all(axis=1) n_pts = n_pts[select] m_t = np.array(m_t) print(m_t) assert len(m_t) > 0, "Still no targets" continue yield episode
def get_topdown_map( sim: Simulator, map_resolution: Tuple[int, int] = (1250, 1250), num_samples: int = 20000, draw_border: bool = True, ) -> np.ndarray: r"""Return a top-down occupancy map for a sim. Note, this only returns valid values for whatever floor the agent is currently on. Args: sim: The simulator. map_resolution: The resolution of map which will be computed and returned. num_samples: The number of random navigable points which will be initially sampled. For large environments it may need to be increased. draw_border: Whether to outline the border of the occupied spaces. Returns: Image containing 0 if occupied, 1 if unoccupied, and 2 if border (if the flag is set). """ top_down_map = np.zeros(map_resolution, dtype=np.uint8) border_padding = 3 start_height = sim.get_agent_state().position[1] # Use sampling to find the extrema points that might be navigable. range_x = (map_resolution[0], 0) range_y = (map_resolution[1], 0) for _ in range(num_samples): point = sim.sample_navigable_point() # Check if on same level as original if np.abs(start_height - point[1]) > 0.5: continue g_x, g_y = to_grid(point[0], point[2], COORDINATE_MIN, COORDINATE_MAX, map_resolution) range_x = (min(range_x[0], g_x), max(range_x[1], g_x)) range_y = (min(range_y[0], g_y), max(range_y[1], g_y)) # Pad the range just in case not enough points were sampled to get the true # extrema. padding = int(np.ceil(map_resolution[0] / 125)) range_x = ( max(range_x[0] - padding, 0), min(range_x[-1] + padding + 1, top_down_map.shape[0]), ) range_y = ( max(range_y[0] - padding, 0), min(range_y[-1] + padding + 1, top_down_map.shape[1]), ) # Search over grid for valid points. for ii in range(range_x[0], range_x[1]): for jj in range(range_y[0], range_y[1]): realworld_x, realworld_y = from_grid(ii, jj, COORDINATE_MIN, COORDINATE_MAX, map_resolution) valid_point = sim.is_navigable( [realworld_x, start_height, realworld_y]) top_down_map[ii, jj] = (MAP_VALID_POINT if valid_point else MAP_INVALID_POINT) # Draw border if necessary if draw_border: # Recompute range in case padding added any more values. range_x = np.where(np.any(top_down_map, axis=1))[0] range_y = np.where(np.any(top_down_map, axis=0))[0] range_x = ( max(range_x[0] - border_padding, 0), min(range_x[-1] + border_padding + 1, top_down_map.shape[0]), ) range_y = ( max(range_y[0] - border_padding, 0), min(range_y[-1] + border_padding + 1, top_down_map.shape[1]), ) _outline_border(top_down_map[range_x[0]:range_x[1], range_y[0]:range_y[1]]) return top_down_map
def generate_pointnav_episode( sim: Simulator, num_episodes: int = -1, is_gen_shortest_path: bool = True, shortest_path_success_distance: float = 0.2, shortest_path_max_steps: int = 500, closest_dist_limit: float = 1, furthest_dist_limit: float = 30, geodesic_to_euclid_min_ratio: float = 1.1, number_retries_per_target: int = 10, ) -> NavigationEpisode: r"""Generator function that generates PointGoal navigation episodes. An episode is trivial if there is an obstacle-free, straight line between the start and goal positions. A good measure of the navigation complexity of an episode is the ratio of geodesic shortest path position to Euclidean distance between start and goal positions to the corresponding Euclidean distance. If the ratio is nearly 1, it indicates there are few obstacles, and the episode is easy; if the ratio is larger than 1, the episode is difficult because strategic navigation is required. To keep the navigation complexity of the precomputed episodes reasonably high, we perform aggressive rejection sampling for episodes with the above ratio falling in the range [1, 1.1]. Following this, there is a significant decrease in the number of straight-line episodes. :param sim: simulator with loaded scene for generation. :param num_episodes: number of episodes needed to generate :param is_gen_shortest_path: option to generate shortest paths :param shortest_path_success_distance: success distance when agent should stop during shortest path generation :param shortest_path_max_steps maximum number of steps shortest path expected to be :param closest_dist_limit episode geodesic distance lowest limit :param furthest_dist_limit episode geodesic distance highest limit :param geodesic_to_euclid_min_ratio geodesic shortest path to Euclid distance ratio upper limit till aggressive sampling is applied. :return: navigation episode that satisfy specified distribution for currently loaded into simulator scene. """ episode_count = 0 while episode_count < num_episodes or num_episodes < 0: target_position = sim.sample_navigable_point() if sim.island_radius(target_position) < ISLAND_RADIUS_LIMIT: continue for retry in range(number_retries_per_target): source_position = sim.sample_navigable_point() is_compatible, dist = is_compatible_episode( source_position, target_position, sim, near_dist=closest_dist_limit, far_dist=furthest_dist_limit, geodesic_to_euclid_ratio=geodesic_to_euclid_min_ratio, ) if is_compatible: angle = np.random.uniform(0, 2 * np.pi) source_rotation = [0, np.sin(angle / 2), 0, np.cos(angle / 2)] shortest_paths = None if is_gen_shortest_path: shortest_paths = [ get_action_shortest_path( sim, source_position=source_position, source_rotation=source_rotation, goal_position=target_position, success_distance=shortest_path_success_distance, max_episode_steps=shortest_path_max_steps, ) ] episode = _create_episode( episode_id=episode_count, scene_id=sim.config.SCENE, start_position=source_position, start_rotation=source_rotation, target_position=target_position, shortest_paths=shortest_paths, radius=shortest_path_success_distance, info={"geodesic_distance": dist}, ) episode_count += 1 yield episode
def generate_objectnav_episode( sim: Simulator, task_category, num_episodes: int = -1, is_gen_shortest_path: bool = True, shortest_path_success_distance: float = 0.2, shortest_path_max_steps: int = 500, closest_dist_limit: float = 1, furthest_dist_limit: float = 10, geodesic_to_euclid_min_ratio: float = 1.1, number_retries_per_target: int = 100, ) -> ObjectGoalNavEpisode: r"""Generator function that generates PointGoal navigation episodes. An episode is trivial if there is an obstacle-free, straight line between the start and goal positions. A good measure of the navigation complexity of an episode is the ratio of geodesic shortest path position to Euclidean distance between start and goal positions to the corresponding Euclidean distance. If the ratio is nearly 1, it indicates there are few obstacles, and the episode is easy; if the ratio is larger than 1, the episode is difficult because strategic navigation is required. To keep the navigation complexity of the precomputed episodes reasonably high, we perform aggressive rejection sampling for episodes with the above ratio falling in the range [1, 1.1]. Following this, there is a significant decrease in the number of straight-line episodes. :param sim: simulator with loaded scene for generation. :param num_episodes: number of episodes needed to generate :param is_gen_shortest_path: option to generate shortest paths :param shortest_path_success_distance: success distance when agent should stop during shortest path generation :param shortest_path_max_steps maximum number of steps shortest path expected to be :param closest_dist_limit episode geodesic distance lowest limit :param furthest_dist_limit episode geodesic distance highest limit :param geodesic_to_euclid_min_ratio geodesic shortest path to Euclid distance ratio upper limit till aggressive sampling is applied. :return: navigation episode that satisfy specified distribution for currently loaded into simulator scene. """ scene = sim.semantic_annotations() # print("scene object len: ", len(scene.objects)) target = dict() for obj in scene.objects: if obj is not None: # print( # f"Object id:{obj.id}, category:{obj.category.name()}, Index:{obj.category.index()}" # f" center:{obj.aabb.center}, dims:{obj.aabb.sizes}" # ) if obj.category.name() in task_category.keys(): if obj.category.index() in target: target[obj.category.index()].append(obj) else: target[obj.category.index()] = [obj] # print("target len:", len(target)) for i in target: episode_count = 0 while episode_count < num_episodes or num_episodes < 0: # print("target episode len:", len(target[i])) object_category = target[i][0].category.name() # print("object_category :", object_category) target_position = [] target_id = [] for j in range(len(target[i])): target_position.append(target[i][j].aabb.center.tolist()) target_id.append(target[i][j].id) shortest_paths = None for retry in range(number_retries_per_target): source_position = sim.sample_navigable_point() # source_position[1] = High is_compatible, dist, euclid, closest_goal_object_id, target_position_episode = is_compatible_episode( source_position, target_position, target_id, sim, near_dist=closest_dist_limit, far_dist=furthest_dist_limit, geodesic_to_euclid_ratio=geodesic_to_euclid_min_ratio, ) if is_compatible and sim.geodesic_distance( source_position, target_position) != np.inf: # print("source: ", source_position) # print("target_position: ", target_position) # print("warning: ", sim.geodesic_distance(source_position, target_position)) angle = np.random.uniform(0, 2 * np.pi) source_rotation = [ 0, np.sin(angle / 2), 0, np.cos(angle / 2) ] if is_gen_shortest_path: try: shortest_paths = [ get_action_shortest_path( sim, source_position=source_position, source_rotation=source_rotation, goal_position=target_position_episode, success_distance= shortest_path_success_distance, max_episode_steps=shortest_path_max_steps, ) ] # Throws an error when it can't find a path except GreedyFollowerError: continue if len(shortest_paths) < shortest_path_max_steps - 1: # print("Found! ", object_category) break # print("episode_count: ", episode_count) if is_compatible and sim.geodesic_distance( source_position, target_position) != np.inf and len( shortest_paths) < shortest_path_max_steps - 1: # angle = np.random.uniform(0, 2 * np.pi) # source_rotation = [0, np.sin(angle / 2), 0, np.cos(angle / 2)] # shortest_paths = None # if is_gen_shortest_path: # try: # shortest_paths = [ # get_action_shortest_path( # sim, # source_position=source_position, # source_rotation=source_rotation, # goal_position=target_position_episode, # success_distance=shortest_path_success_distance, # max_episode_steps=shortest_path_max_steps, # ) # ] # # Throws an error when it can't find a path # except GreedyFollowerError: # continue episode = _create_episode( episode_id=episode_count, scene_id=sim.config.SCENE, start_position=source_position, start_rotation=source_rotation, target_position=target_position, object_category=object_category, shortest_paths=shortest_paths, radius=shortest_path_success_distance, info={ "geodesic_distance": dist, "euclidean_distance": euclid, "closest_goal_object_id": closest_goal_object_id }, ) # print("source_position: ", source_position) # print("episode finish!") episode_count += 1 if episode_count > num_episodes: return yield episode else: break