def get_shortest_path(self):
'''
Returns observations and actions corresponding to the shortest path to the goal.
If the goal cannot be reached within the episode steps limit, the best
path (closest to the goal) will be returned.
'''
max_steps = self.unwrapped._env._max_episode_steps - 1
try:
shortest_path = [
get_action_shortest_path(
self.unwrapped._env._sim,
source_position=self.unwrapped._env._dataset.episodes[0].
start_position,
source_rotation=self.unwrapped._env._dataset.episodes[0].
start_rotation,
goal_position=self.unwrapped._env._dataset.episodes[0].
goals[0].position,
success_distance=self.unwrapped._core_env_config.TASK.
SUCCESS_DISTANCE,
max_episode_steps=max_steps,
)
][0]
self.reset() # get_action_shortest_path changes the agent state
actions = [p.action for p in shortest_path]
obs = [
self.unwrapped._env.sim.get_observations_at(
p.position, p.rotation)['rgb'] for p in shortest_path
]
shortest_steps = len(actions)
if shortest_steps == max_steps:
print(
'WARNING! Shortest path not found with the given steps limit ({steps}).'
.format(steps=max_steps), 'Returning best path.')
else:
print('Shortest path found: {steps} steps.'.format(
steps=shortest_steps))
return obs, actions
except GreedyFollowerError:
print('WARNING! Cannot find shortest path (GreedyFollowerError).')
return None, None
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