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 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