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 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] if "category_to_task_category_id" in deserialized: self.category_to_task_category_id = deserialized[ "category_to_task_category_id"] if "category_to_scene_annotation_category_id" in deserialized: self.category_to_scene_annotation_category_id = deserialized[ "category_to_scene_annotation_category_id"] if "category_to_mp3d_category_id" in deserialized: self.category_to_scene_annotation_category_id = deserialized[ "category_to_mp3d_category_id"] assert len(self.category_to_task_category_id) == len( self.category_to_scene_annotation_category_id) assert set(self.category_to_task_category_id.keys()) == set( self.category_to_scene_annotation_category_id.keys( )), "category_to_task and category_to_mp3d must have the same keys" 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 i in range(len(episode.goals)): episode.goals[i] = ObjectGoal(**episode.goals[i]) for vidx, view in enumerate(episode.goals[i].view_points): view_location = ObjectViewLocation(**view) view_location.agent_state = AgentState( **view_location.agent_state) episode.goals[i].view_points[vidx] = view_location if episode.shortest_paths is not None: for path in episode.shortest_paths: for p_index, point in enumerate(path): point = { "action": point, "rotation": None, "position": None, } path[p_index] = ShortestPathPoint(**point) self.episodes.append(episode) for i, ep in enumerate(self.episodes): ep.episode_id = str(i)
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 test_state_sensors(): 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", "COMPASS_SENSOR", "GPS_SENSOR"] config.freeze() with habitat.Env(config=config, dataset=None) as env: 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.072_447, -2.67867], start_rotation=random_rotation, goals=[], ) ]) obs = env.reset() heading = obs["heading"] assert np.allclose(heading, [random_heading]) assert np.allclose(obs["compass"], [0.0], atol=1e-5) assert np.allclose(obs["gps"], [0.0, 0.0], atol=1e-5)
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 _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 _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 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