def _create_episode(
episode_id,
scene_id,
start_position,
start_rotation,
target_position,
shortest_paths=None,
radius=None,
info=None,
multi_target=None,
multi_taget_radius=0.2,
) -> Optional[NavigationEpisode]:
ep = NavigationEpisode(
episode_id=str(episode_id),
goals=[],
scene_id=scene_id,
start_position=start_position,
start_rotation=start_rotation,
shortest_paths=shortest_paths,
info=info,
)
if multi_target is None:
goals = [NavigationGoal(position=target_position, radius=radius)]
else:
ep.tested_coord = target_position
goals = []
for t in multi_target:
goals.append(NavigationGoal(position=t, radius=multi_taget_radius))
ep.goals = goals
return ep
def from_json(
self, json_str: str, scenes_dir: Optional[str] = None
) -> None:
deserialized = json.loads(json_str)
if CONTENT_SCENES_PATH_FIELD in deserialized:
self.content_scenes_path = deserialized[CONTENT_SCENES_PATH_FIELD]
for episode in deserialized["episodes"]:
episode = NavigationEpisode(**episode)
if scenes_dir is not None:
if episode.scene_id.startswith(DEFAULT_SCENE_PATH_PREFIX):
episode.scene_id = episode.scene_id[
len(DEFAULT_SCENE_PATH_PREFIX) :
]
episode.scene_id = os.path.join(scenes_dir, episode.scene_id)
for g_index, goal in enumerate(episode.goals):
episode.goals[g_index] = NavigationGoal(**goal)
if episode.shortest_paths is not None:
for path in episode.shortest_paths:
for p_index, point in enumerate(path):
path[p_index] = ShortestPathPoint(**point)
self.episodes.append(episode)
def from_json(self, json_str: str) -> None:
deserialized = json.loads(json_str)
if CONTENT_SCENES_PATH_FIELD in deserialized:
self.content_scenes_path = deserialized[CONTENT_SCENES_PATH_FIELD]
for episode in deserialized["episodes"]:
episode = NavigationEpisode(**episode)
for g_index, goal in enumerate(episode.goals):
episode.goals[g_index] = NavigationGoal(**goal)
if episode.shortest_paths is not None:
for path in episode.shortest_paths:
for p_index, point in enumerate(path):
path[p_index] = ShortestPathPoint(**point)
self.episodes.append(episode)
def example_pointnav_draw_target_birdseye_view_agent_on_border():
goal_radius = 0.5
goal = NavigationGoal([0, 0.25, 0], goal_radius)
ii = 0
for x_edge in [-1, 0, 1]:
for y_edge in [-1, 0, 1]:
if not np.bitwise_xor(x_edge == 0, y_edge == 0):
continue
ii += 1
agent_position = np.array([7.8 * x_edge, 0.25, 7.8 * y_edge])
agent_rotation = np.pi / 2
dummy_episode = NavigationEpisode(
[goal],
episode_id="dummy_id",
scene_id="dummy_scene",
start_position=agent_position,
start_rotation=agent_rotation,
)
target_image = maps.pointnav_draw_target_birdseye_view(
agent_position,
agent_rotation,
np.asarray(dummy_episode.goals[0].position),
goal_radius=dummy_episode.goals[0].radius,
agent_radius_px=25,
)
imageio.imsave(
os.path.join(
IMAGE_DIR, "pointnav_target_image_edge_%d.png" % ii
),
target_image,
)
def test_heading_sensor():
config = get_config()
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
config.defrost()
config.TASK.SENSORS = ["HEADING_SENSOR"]
config.freeze()
env = habitat.Env(config=config, dataset=None)
env.reset()
random.seed(123)
np.random.seed(123)
for _ in range(100):
random_heading = np.random.uniform(-np.pi, np.pi)
random_rotation = [
0,
np.sin(random_heading / 2),
0,
np.cos(random_heading / 2),
]
env.episode_iterator = iter([
NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=[03.00611, 0.072447, -2.67867],
start_rotation=random_rotation,
goals=[],
)
])
obs = env.reset()
heading = obs["heading"]
assert np.allclose(heading, random_heading)
env.close()
def test_env():
config = get_config(CFG_TEST)
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
env = habitat.Env(config=config, dataset=None)
env.episodes = [
NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=[03.00611, 0.072447, -2.67867],
start_rotation=[0, 0.163276, 0, 0.98658],
goals=[],
)
]
env.reset()
non_stop_actions = [
k for k, v in SIM_ACTION_TO_NAME.items()
if v != SimulatorActions.STOP.value
]
for _ in range(config.ENVIRONMENT.MAX_EPISODE_STEPS):
env.step(np.random.choice(non_stop_actions))
# check for steps limit on environment
assert env.episode_over is True, ("episode should be over after "
"max_episode_steps")
env.reset()
env.step(SIM_NAME_TO_ACTION[SimulatorActions.STOP.value])
# check for STOP action
assert env.episode_over is True, ("episode should be over after STOP "
"action")
env.close()
def test_pointgoal_with_gps_compass_sensor():
config = get_config()
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
config.defrost()
config.TASK.SENSORS = [
"POINTGOAL_WITH_GPS_COMPASS_SENSOR",
"COMPASS_SENSOR",
"GPS_SENSOR",
"POINTGOAL_SENSOR",
]
config.TASK.POINTGOAL_WITH_GPS_COMPASS_SENSOR.DIMENSIONALITY = 3
config.TASK.POINTGOAL_WITH_GPS_COMPASS_SENSOR.GOAL_FORMAT = "CARTESIAN"
config.TASK.POINTGOAL_SENSOR.DIMENSIONALITY = 3
config.TASK.POINTGOAL_SENSOR.GOAL_FORMAT = "CARTESIAN"
config.TASK.GPS_SENSOR.DIMENSIONALITY = 3
config.freeze()
env = habitat.Env(config=config, dataset=None)
# start position is checked for validity for the specific test scene
valid_start_position = [-1.3731, 0.08431, 8.60692]
expected_pointgoal = [0.1, 0.2, 0.3]
goal_position = np.add(valid_start_position, expected_pointgoal)
# starting quaternion is rotated 180 degree along z-axis, which
# corresponds to simulator using z-negative as forward action
start_rotation = [0, 0, 0, 1]
env.episode_iterator = iter([
NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=valid_start_position,
start_rotation=start_rotation,
goals=[NavigationGoal(position=goal_position)],
)
])
env.reset()
for _ in range(100):
obs = env.step(sample_non_stop_action(env.action_space))
pointgoal = obs["pointgoal"]
pointgoal_with_gps_compass = obs["pointgoal_with_gps_compass"]
comapss = obs["compass"]
gps = obs["gps"]
# check to see if taking non-stop actions will affect static point_goal
assert np.allclose(
pointgoal_with_gps_compass,
quaternion_rotate_vector(
quaternion.from_rotation_vector(comapss *
np.array([0, 1, 0])).inverse(),
pointgoal - gps,
),
)
env.close()
def test_get_observations_at():
config = get_config()
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
config.defrost()
config.TASK.SENSORS = []
config.SIMULATOR.AGENT_0.SENSORS = ["RGB_SENSOR", "DEPTH_SENSOR"]
config.freeze()
env = habitat.Env(config=config, dataset=None)
# start position is checked for validity for the specific test scene
valid_start_position = [-1.3731, 0.08431, 8.60692]
expected_static_pointgoal = [0.1, 0.2, 0.3]
goal_position = np.add(valid_start_position, expected_static_pointgoal)
# starting quaternion is rotated 180 degree along z-axis, which
# corresponds to simulator using z-negative as forward action
start_rotation = [0, 0, 0, 1]
env.episodes = [
NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=valid_start_position,
start_rotation=start_rotation,
goals=[NavigationGoal(position=goal_position)],
)
]
obs = env.reset()
start_state = env.sim.get_agent_state()
for _ in range(100):
# Note, this test will not currently work for camera change actions
# (look up/down), only for movement actions.
new_obs = env.step(np.random.choice(MOVEMENT_ACTIONS))
for key, val in new_obs.items():
agent_state = env.sim.get_agent_state()
if not (np.allclose(agent_state.position, start_state.position) and
np.allclose(agent_state.rotation, start_state.rotation)):
assert not np.allclose(val, obs[key])
obs_at_point = env.sim.get_observations_at(
start_state.position,
start_state.rotation,
keep_agent_at_new_pose=False,
)
for key, val in obs_at_point.items():
assert np.allclose(val, obs[key])
obs_at_point = env.sim.get_observations_at(start_state.position,
start_state.rotation,
keep_agent_at_new_pose=True)
for key, val in obs_at_point.items():
assert np.allclose(val, obs[key])
agent_state = env.sim.get_agent_state()
assert np.allclose(agent_state.position, start_state.position)
assert np.allclose(agent_state.rotation, start_state.rotation)
env.close()
def test_env(gpu2gpu):
import habitat_sim
if gpu2gpu and not habitat_sim.cuda_enabled:
pytest.skip("GPU-GPU requires CUDA")
config = get_config(CFG_TEST)
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
config.defrost()
config.SIMULATOR.HABITAT_SIM_V0.GPU_GPU = gpu2gpu
config.freeze()
env = habitat.Env(config=config, dataset=None)
env.episodes = [
NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=[-3.0133917, 0.04623024, 7.3064547],
start_rotation=[0, 0.163276, 0, 0.98658],
goals=[
NavigationGoal(position=[-3.0133917, 0.04623024, 7.3064547])
],
info={"geodesic_distance": 0.001},
)
]
env.reset()
non_stop_actions = [
act for act in range(env.action_space.n)
if act != SimulatorActions.STOP
]
for _ in range(config.ENVIRONMENT.MAX_EPISODE_STEPS):
act = np.random.choice(non_stop_actions)
env.step(act)
# check for steps limit on environment
assert env.episode_over is True, ("episode should be over after "
"max_episode_steps")
env.reset()
env.step(SimulatorActions.STOP)
# check for STOP action
assert env.episode_over is True, ("episode should be over after STOP "
"action")
env.close()
def test_pointgoal_sensor():
config = get_config()
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
config.defrost()
config.TASK.SENSORS = ["POINTGOAL_SENSOR"]
config.TASK.POINTGOAL_SENSOR.DIMENSIONALITY = 3
config.TASK.POINTGOAL_SENSOR.GOAL_FORMAT = "CARTESIAN"
config.freeze()
env = habitat.Env(config=config, dataset=None)
# start position is checked for validity for the specific test scene
valid_start_position = [-1.3731, 0.08431, 8.60692]
expected_pointgoal = [0.1, 0.2, 0.3]
goal_position = np.add(valid_start_position, expected_pointgoal)
# starting quaternion is rotated 180 degree along z-axis, which
# corresponds to simulator using z-negative as forward action
start_rotation = [0, 0, 0, 1]
env.episode_iterator = iter(
[
NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=valid_start_position,
start_rotation=start_rotation,
goals=[NavigationGoal(position=goal_position)],
)
]
)
non_stop_actions = [
act
for act in range(env.action_space.n)
if act != SimulatorActions.STOP
]
env.reset()
for _ in range(100):
obs = env.step(np.random.choice(non_stop_actions))
pointgoal = obs["pointgoal"]
# check to see if taking non-stop actions will affect static point_goal
assert np.allclose(pointgoal, expected_pointgoal)
env.close()
def _random_episode(env, config):
random_location = env._sim.sample_navigable_point()
random_heading = np.random.uniform(-np.pi, np.pi)
random_rotation = [
0,
np.sin(random_heading / 2),
0,
np.cos(random_heading / 2),
]
env.episode_iterator = iter([
NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=random_location,
start_rotation=random_rotation,
goals=[],
)
])
def _create_episode(
episode_id,
scene_id,
start_position,
start_rotation,
target_position,
shortest_paths=None,
radius=None,
info=None,
) -> Optional[NavigationEpisode]:
goals = [NavigationGoal(position=target_position, radius=radius)]
return NavigationEpisode(
episode_id=str(episode_id),
goals=goals,
scene_id=scene_id,
start_position=start_position,
start_rotation=start_rotation,
shortest_paths=shortest_paths,
info=info,
)
def test_env():
config = get_config(CFG_TEST)
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
env = habitat.Env(config=config, dataset=None)
env.episodes = [
NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=[-3.0133917, 0.04623024, 7.3064547],
start_rotation=[0, 0.163276, 0, 0.98658],
goals=[
NavigationGoal(position=[-3.0133917, 0.04623024, 7.3064547])
],
info={"geodesic_distance": 0.001},
)
]
env.reset()
non_stop_actions = [
v for v in range(len(SimulatorActions))
if v != SimulatorActions.STOP.value
]
for _ in range(config.ENVIRONMENT.MAX_EPISODE_STEPS):
act = np.random.choice(non_stop_actions)
env.step(act)
# check for steps limit on environment
assert env.episode_over is True, ("episode should be over after "
"max_episode_steps")
env.reset()
env.step(SimulatorActions.STOP.value)
# check for STOP action
assert env.episode_over is True, ("episode should be over after STOP "
"action")
env.close()
def example_pointnav_draw_target_birdseye_view():
goal_radius = 0.5
goal = NavigationGoal([10, 0.25, 10], goal_radius)
agent_position = np.array([0, 0.25, 0])
agent_rotation = -np.pi / 4
dummy_episode = NavigationEpisode(
[goal],
episode_id="dummy_id",
scene_id="dummy_scene",
start_position=agent_position,
start_rotation=agent_rotation,
)
target_image = maps.pointnav_draw_target_birdseye_view(
agent_position,
agent_rotation,
np.asarray(dummy_episode.goals[0].position),
goal_radius=dummy_episode.goals[0].radius,
agent_radius_px=25,
)
imageio.imsave("pointnav_target_image.png", target_image)
def test_rl_env():
config = get_config(CFG_TEST)
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
env = DummyRLEnv(config=config, dataset=None)
env.episodes = [
NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=[-3.0133917, 0.04623024, 7.3064547],
start_rotation=[0, 0.163276, 0, 0.98658],
goals=[NavigationGoal([-3.0133917, 0.04623024, 7.3064547])],
info={"geodesic_distance": 0.001},
)
]
done = False
observation = env.reset()
non_stop_actions = [
k for k, v in SIM_ACTION_TO_NAME.items()
if v != SimulatorActions.STOP.value
]
for _ in range(config.ENVIRONMENT.MAX_EPISODE_STEPS):
observation, reward, done, info = env.step(
np.random.choice(non_stop_actions))
# check for steps limit on environment
assert done is True, "episodes should be over after max_episode_steps"
env.reset()
observation, reward, done, info = env.step(
SIM_NAME_TO_ACTION[SimulatorActions.STOP.value])
assert done is True, "done should be true after STOP action"
env.close()
def test_static_pointgoal_sensor():
config = get_config()
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
config.defrost()
config.TASK.SENSORS = ["STATIC_POINTGOAL_SENSOR"]
config.freeze()
env = habitat.Env(config=config, dataset=None)
# start position is checked for validity for the specific test scene
valid_start_position = [-1.3731, 0.08431, 8.60692]
expected_static_pointgoal = [0.1, 0.2, 0.3]
goal_position = np.add(valid_start_position, expected_static_pointgoal)
# starting quaternion is rotated 180 degree along z-axis, which
# corresponds to simulator using z-negative as forward action
start_rotation = [0, 0, 0, 1]
env.episodes = [
NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=valid_start_position,
start_rotation=start_rotation,
goals=[NavigationGoal(goal_position)],
)
]
obs = env.reset()
for _ in range(5):
env.step(np.random.choice(NON_STOP_ACTIONS))
static_pointgoal = obs["static_pointgoal"]
# check to see if taking non-stop actions will affect static point_goal
assert np.allclose(static_pointgoal, expected_static_pointgoal)
env.close()
distance = self._env.sim.geodesic_distance(current_position,
target_position)
return distance
def transform_rgb_bgr(image):
return image[:, :, [2, 1, 0]]
apartment = "office_3"
ep = NavigationEpisode(
episode_id="0",
scene_id=f"/raid/workspace/alexandrug/Replica-Dataset/dataset"
f"/{apartment}/habitat/mesh_semantic.ply",
start_position=[-0.6644544, -1.020689, -0.4631982],
start_rotation=[0, 0.163276, 0, 0.98658],
goals=[NavigationGoal(position=[-2.452475, -1.02069, 1.156327])],
info={"geodesic_distance": 0.001},
)
dataset = Dataset()
dataset.episodes = [ep]
def draw_top_down_map(info, heading, output_size):
top_down_map = maps.colorize_topdown_map(
info["top_down_map"]["map"], info["top_down_map"]["fog_of_war_mask"])
original_map_size = top_down_map.shape[:2]
map_scale = np.array(
(1, original_map_size[1] * 1.0 / original_map_size[0]))
new_map_size = np.round(output_size * map_scale).astype(np.int32)
print("Total num scenes", len(scenes) * NUM_EPS_PER_LOCATION)
episodes = []
for ii, house in enumerate(scenes):
if DATASET == "mp3d":
scene_id = house + os.sep + house + ".glb"
elif DATASET == "gibson":
scene_id = house + ".glb"
else:
raise NotImplementedError("Not implemented for this dataset")
dummy_episode = NavigationEpisode(
episode_id=str(ii),
goals=[NavigationGoal([0, 0, 0])],
scene_id=scene_id,
start_position=[0, 0, 0],
start_rotation=[0, 0, 0, 1],
)
episodes.append(dummy_episode)
dataset = Dataset()
dataset.episodes = episodes
json = dataset.to_json().encode("utf-8")
with gzip.GzipFile(
os.path.join(OUTPUT_PATH, "dataset_one_ep_per_scene.json.gz"),
"w") as fout:
fout.write(json)
env = habitat.Env(config=config, dataset=dataset)
episodes = []
