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