def test_imagegoal_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 = ["IMAGEGOAL_SENSOR"]
config.SIMULATOR.AGENT_0.SENSORS = ["RGB_SENSOR"]
config.freeze()
with habitat.Env(config=config, dataset=None) as env:
# start position is checked for validity for the specific test scene
valid_start_position = [-1.3731, 0.08431, 8.60692]
pointgoal = [0.1, 0.2, 0.3]
goal_position = np.add(valid_start_position, pointgoal)
pointgoal_2 = [0.3, 0.2, 0.1]
goal_position_2 = np.add(valid_start_position, pointgoal_2)
# 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)],
),
NavigationEpisode(
episode_id="1",
scene_id=config.SIMULATOR.SCENE,
start_position=valid_start_position,
start_rotation=start_rotation,
goals=[NavigationGoal(position=goal_position_2)],
),
]
)
obs = env.reset()
for _ in range(100):
new_obs = env.step(sample_non_stop_action(env.action_space))
# check to see if taking non-stop actions will affect static image_goal
assert np.allclose(obs["imagegoal"], new_obs["imagegoal"])
assert np.allclose(obs["rgb"].shape, new_obs["imagegoal"].shape)
previous_episode_obs = obs
_ = env.reset()
for _ in range(10):
new_obs = env.step(sample_non_stop_action(env.action_space))
# check to see if taking non-stop actions will affect static image_goal
assert not np.allclose(
previous_episode_obs["imagegoal"], new_obs["imagegoal"]
)
assert np.allclose(
previous_episode_obs["rgb"].shape, new_obs["imagegoal"].shape
)
def example_pointnav_draw_target_birdseye_view_agent_on_border():
goal_radius = 0.5
goal = NavigationGoal(position=[0, 0.25, 0], radius=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(
goals=[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 from_json(
self, json_str: str, scenes_dir: Optional[str] = None, scene_filename: 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]
episode_cnt = 0
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)
episode_cnt += 1
def get_next_episode(self, episode_id, scene_id):
scene_name = scene_id.split('/')[-1][:-4]
if self._current_scene_episode_idx >= len(
self._episode_datasets[scene_name]):
self._current_scene_episode_idx = 0
episode = self._episode_datasets[scene_name][
self._current_scene_episode_idx]
goals = []
goal = NavigationGoal(**{'position': episode['goal_position']})
goals.append(goal)
self.start_position = episode['start_position']
self.start_rotation = episode['start_rotation']
episode_info = {
'episode_id': self._current_scene_episode_idx,
'scene_id': scene_id,
'start_position': self.start_position,
'start_rotation': self.start_rotation.components,
'goals': goals,
'start_room': None,
'shortest_paths': None
}
episode = NavigationEpisode(**episode_info)
#TODO load demonstration data
self.demonstration = []
#print(episode)
return episode
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()
with habitat.Env(config=config, dataset=None) as env:
# 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"]
compass = float(obs["compass"][0])
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(
compass * np.array([0, 1, 0])
).inverse(),
pointgoal - gps,
),
atol=1e-5,
)
def smoke_test_sensor(config, N_STEPS=100):
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
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]
test_episode = NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=valid_start_position,
start_rotation=start_rotation,
goals=[NavigationGoal(position=goal_position)],
)
with habitat.Env(config=config, dataset=None) as env:
env.episode_iterator = iter([test_episode])
no_noise_obs = env.reset()
assert no_noise_obs is not None
actions = [
sample_non_stop_action(env.action_space) for _ in range(N_STEPS)
]
for action in actions:
assert env.step(action) is not None
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()
with habitat.Env(config=config, dataset=None) as env:
# 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)],
)
])
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(sample_non_stop_action(env.action_space))
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)
def test_rl_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 = 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(position=[-3.0133917, 0.04623024, 7.3064547])
],
info={"geodesic_distance": 0.001},
)
]
done = False
env.reset()
for _ in range(config.ENVIRONMENT.MAX_EPISODE_STEPS):
observation, reward, done, info = env.step(
action=sample_non_stop_action(env.action_space)
)
# 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(
action={"action": StopAction.name}
)
assert done is True, "done should be true after STOP action"
env.close()
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 _create_episode(
episode_id: Union[int, str],
scene_id: str,
start_position: List[float],
start_rotation: List[Union[int, float64]],
target_position: List[float],
shortest_paths: Optional[List[List[ShortestPathPoint]]] = None,
radius: Optional[float] = None,
info: Optional[Dict[str, float]] = 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 from_json(self,
json_str: str,
scenes_dir: Optional[str] = None) -> None:
deserialized = json.loads(json_str)
self.instruction_vocab = VocabDict(
word_list=deserialized["instruction_vocab"]["word_list"])
for episode in deserialized["episodes"]:
episode = VLNEpisode(**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)
episode.instruction = InstructionData(**episode.instruction)
for g_index, goal in enumerate(episode.goals):
episode.goals[g_index] = NavigationGoal(**goal)
self.episodes.append(episode)
def example_pointnav_draw_target_birdseye_view():
goal_radius = 0.5
goal = NavigationGoal(position=[10, 0.25, 10], radius=goal_radius)
agent_position = np.array([0, 0.25, 0])
agent_rotation = -np.pi / 4
dummy_episode = NavigationEpisode(
goals=[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.png"),
target_image)
def test_noise_models_rgbd():
DEMO_MODE = False
N_STEPS = 100
config = get_config()
config.defrost()
config.SIMULATOR.SCENE = (
"data/scene_datasets/habitat-test-scenes/skokloster-castle.glb")
config.SIMULATOR.AGENT_0.SENSORS = ["RGB_SENSOR", "DEPTH_SENSOR"]
config.freeze()
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
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]
test_episode = NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=valid_start_position,
start_rotation=start_rotation,
goals=[NavigationGoal(position=goal_position)],
)
print(f"{test_episode}")
with habitat.Env(config=config, dataset=None) as env:
env.episode_iterator = iter([test_episode])
no_noise_obs = [env.reset()]
no_noise_states = [env.sim.get_agent_state()]
actions = [
sample_non_stop_action(env.action_space) for _ in range(N_STEPS)
]
for action in actions:
no_noise_obs.append(env.step(action))
no_noise_states.append(env.sim.get_agent_state())
env.close()
config.defrost()
config.SIMULATOR.RGB_SENSOR.NOISE_MODEL = "GaussianNoiseModel"
config.SIMULATOR.RGB_SENSOR.NOISE_MODEL_KWARGS = habitat.Config()
config.SIMULATOR.RGB_SENSOR.NOISE_MODEL_KWARGS.INTENSITY_CONSTANT = 0.5
config.SIMULATOR.DEPTH_SENSOR.NOISE_MODEL = "RedwoodDepthNoiseModel"
config.SIMULATOR.ACTION_SPACE_CONFIG = "pyrobotnoisy"
config.SIMULATOR.NOISE_MODEL = habitat.Config()
config.SIMULATOR.NOISE_MODEL.ROBOT = "LoCoBot"
config.SIMULATOR.NOISE_MODEL.CONTROLLER = "Proportional"
config.SIMULATOR.NOISE_MODEL.NOISE_MULTIPLIER = 0.5
config.freeze()
env = habitat.Env(config=config, dataset=None)
env.episode_iterator = iter([test_episode])
obs = env.reset()
assert np.linalg.norm(
obs["rgb"].astype(np.float) -
no_noise_obs[0]["rgb"].astype(np.float)) > 1.5e-2 * np.linalg.norm(
no_noise_obs[0]["rgb"].astype(
np.float)), "No RGB noise detected."
assert np.linalg.norm(obs["depth"].astype(np.float) -
no_noise_obs[0]["depth"].astype(np.float)
) > 1.5e-2 * np.linalg.norm(
no_noise_obs[0]["depth"].astype(
np.float)), "No Depth noise detected."
images = []
state = env.sim.get_agent_state()
angle_diffs = []
pos_diffs = []
for action in actions:
prev_state = state
obs = env.step(action)
state = env.sim.get_agent_state()
position_change = np.linalg.norm(np.array(state.position) -
np.array(prev_state.position),
ord=2)
if action["action"][:5] == "TURN_":
angle_diff = abs(
angle_between_quaternions(state.rotation,
prev_state.rotation) -
np.deg2rad(config.SIMULATOR.TURN_ANGLE))
angle_diffs.append(angle_diff)
else:
pos_diffs.append(
abs(position_change - config.SIMULATOR.FORWARD_STEP_SIZE))
if DEMO_MODE:
images.append(observations_to_image(obs, {}))
if DEMO_MODE:
images_to_video(images, "data/video/test_noise", "test_noise")
assert (np.mean(angle_diffs) >
0.025), "No turn action actuation noise detected."
assert (np.mean(pos_diffs) >
0.025), "No forward action actuation noise detected."
def main(dataset):
parser = argparse.ArgumentParser()
parser.add_argument(
"--config-path",
type=str,
default='baselines/config/{}/train_telephone/pointgoal_rgb.yaml'.format(dataset)
)
parser.add_argument(
"opts",
default=None,
nargs=argparse.REMAINDER,
help="Modify config options from command line",
)
args = parser.parse_args()
config = get_config(args.config_path, opts=args.opts)
config.defrost()
config.TASK_CONFIG.SIMULATOR.AGENT_0.SENSORS = ["RGB_SENSOR", "DEPTH_SENSOR"]
config.freeze()
simulator = None
scene_obs = defaultdict(dict)
num_obs = 0
scene_obs_dir = 'data/scene_observations/' + dataset
os.makedirs(scene_obs_dir, exist_ok=True)
metadata_dir = 'data/metadata/' + dataset
for scene in os.listdir(metadata_dir):
scene_obs = dict()
scene_metadata_dir = os.path.join(metadata_dir, scene)
points, graph = load_metadata(scene_metadata_dir)
if dataset == 'replica':
scene_mesh_dir = os.path.join('data/scene_datasets', dataset, scene, 'habitat/mesh_semantic.ply')
else:
scene_mesh_dir = os.path.join('data/scene_datasets', dataset, scene, scene + '.glb')
for node in graph.nodes():
agent_position = graph.nodes()[node]['point']
for angle in [0, 90, 180, 270]:
agent_rotation = quat_to_coeffs(quat_from_angle_axis(np.deg2rad(angle), np.array([0, 1, 0]))).tolist()
goal_radius = 0.00001
goal = NavigationGoal(
position=agent_position,
radius=goal_radius
)
episode = NavigationEpisode(
goals=[goal],
episode_id=str(0),
scene_id=scene_mesh_dir,
start_position=agent_position,
start_rotation=agent_rotation,
info={'sound': 'telephone'}
)
episode_sim_config = merge_sim_episode_config(config.TASK_CONFIG.SIMULATOR, episode)
if simulator is None:
simulator = SoundSpaces(episode_sim_config)
simulator.reconfigure(episode_sim_config)
obs, rotation_index = simulator.step(None)
scene_obs[(node, rotation_index)] = obs
num_obs += 1
print('Total number of observations: {}'.format(num_obs))
with open(os.path.join(scene_obs_dir, '{}.pkl'.format(scene)), 'wb') as fo:
pickle.dump(scene_obs, fo)
simulator.close()
del simulator