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 get_topdown_map_from_sim(
sim: Simulator,
map_resolution: int = 1024,
draw_border: bool = True,
meters_per_pixel: Optional[float] = None,
agent_id: int = 0,
) -> np.ndarray:
r"""Wrapper around :py:`get_topdown_map` that retrieves that pathfinder and heigh from the current simulator
:param sim: Simulator instance.
:param agent_id: The agent ID
"""
return get_topdown_map(
sim.pathfinder,
sim.get_agent(agent_id).state.position[1],
map_resolution,
draw_border,
meters_per_pixel,
)
def generate_objectnav_task_category_id(sim: Simulator, task_category):
scene = sim.semantic_annotations()
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]
task_category_id = task_category
for i in target:
task_category_id[target[i]
[0].category.name()] = target[i][0].category.index()
# print(task_category_id)
return task_category_id
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_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 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_objectnav_goals_by_category(sim: Simulator,
task_category) -> ObjectGoal:
scene = sim.semantic_annotations()
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]
for i in target:
# print("target episode len:", len(target[i]))
object_category = target[i][0].category.name()
# print("object_category :", object_category)
str_goal = f"{os.path.basename(sim.config.SCENE)}_{object_category}"
# print(str_goal)
goals_by = []
for j in range(len(target[i])):
object_view_list = []
for x in np.arange(
target[i][j].aabb.center[0] -
target[i][j].aabb.sizes[0] / 2.0,
target[i][j].aabb.center[0] +
target[i][j].aabb.sizes[0] / 2.0, 0.1):
for y in np.arange(
target[i][j].aabb.center[2] -
target[i][j].aabb.sizes[2] / 2.0,
target[i][j].aabb.center[2] +
target[i][j].aabb.sizes[2] / 2.0, 0.1):
# print("x, y: ", type(x)) # np.float6
# print("x, y: ", type(target[i][j].aabb.center[1])) # np.floa32 报错
object_view_list.append(
ObjectViewLocation(
agent_state=AgentState(position=[
x, target[i][j].aabb.center[1].astype(
np.float64), y
]),
iou=0.0,
))
# print(type(object_view_list))
goal_by_object = ObjectGoal(
position=target[i][j].aabb.center,
radius=0.5,
object_id=int(re.findall(r"\d+", target[i][j].id)[0]),
object_name=target[i][j].id,
object_category=object_category,
view_points=object_view_list,
)
goals_by.append(goal_by_object)
yield str_goal, goals_by
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