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 = []