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(position=[-3.0133917, 0.04623024, 7.3064547]) ], info={"geodesic_distance": 0.001}, ) ] done = False observation = 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): 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(SimulatorActions.STOP.value) assert done is True, "done should be true after STOP action" env.close()
def test_collisions(): 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.MEASUREMENTS = ["COLLISIONS"] config.freeze() env = habitat.Env(config=config, dataset=None) env.reset() for _ in range(20): _random_episode(env, config) env.reset() assert env.get_metrics()["collisions"] is None prev_collisions = 0 prev_loc = env.sim.get_agent_state().position for _ in range(50): action = sample_non_stop_action(env.action_space) env.step(action) collisions = env.get_metrics()["collisions"]["count"] loc = env.sim.get_agent_state().position if (np.linalg.norm(loc - prev_loc) < 0.9 * config.SIMULATOR.FORWARD_STEP_SIZE and action["action"] == MoveForwardAction.name): # Check to see if the new method of doing collisions catches # all the same collisions as the old method assert collisions == prev_collisions + 1 # We can _never_ collide with standard turn actions if action["action"] != MoveForwardAction.name: assert collisions == prev_collisions prev_loc = loc prev_collisions = collisions 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)], ) ]) env.reset() for _ in range(100): obs = env.step(sample_non_stop_action(env.action_space)) 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 __init__(self, config_paths: Optional[str] = None, eval_remote=False, enable_physics=False) -> None: r""".. :param config_paths: file to be used for creating the environment :param eval_remote: boolean indicating whether evaluation should be run remotely or locally """ config_env = get_config(config_paths) # embed top-down map and heading sensor in config config_env.defrost() config_env.TASK.MEASUREMENTS.append("TOP_DOWN_MAP") config_env.TASK.SENSORS.append("HEADING_SENSOR") config_env.freeze() self._eval_remote = eval_remote self._enable_physics = enable_physics if self._eval_remote is True: self._env = None else: self._env = BenchmarkingRLEnv(config=config_env)
def test_tactile(): 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 = ["PROXIMITY_SENSOR"] config.freeze() env = habitat.Env(config=config, dataset=None) env.reset() random.seed(1234) for _ in range(20): _random_episode(env, config) env.reset() action = env._sim.index_forward_action for _ in range(10): obs = env.step(action) proximity = obs["proximity"] assert 0.0 <= proximity assert 2.0 >= proximity 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=[-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}, ) ] 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): 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(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_mp3d_eqa_sim(): eqa_config = get_config(CFG_TEST) if not mp3d_dataset.Matterport3dDatasetV1.check_config_paths_exist( eqa_config.DATASET ): pytest.skip( "Please download Matterport3D EQA dataset to " "data folder." ) dataset = make_dataset( id_dataset=eqa_config.DATASET.TYPE, config=eqa_config.DATASET ) env = habitat.Env(config=eqa_config, dataset=dataset) env.episodes = dataset.episodes[:EPISODES_LIMIT] assert env env.reset() while not env.episode_over: action = env.action_space.sample() assert env.action_space.contains(action) obs = env.step(action) if not env.episode_over: assert "rgb" in obs, "RGB image is missing in observation." assert obs["rgb"].shape[:2] == ( eqa_config.SIMULATOR.RGB_SENSOR.HEIGHT, eqa_config.SIMULATOR.RGB_SENSOR.WIDTH, ), ( "Observation resolution {} doesn't correspond to config " "({}, {}).".format( obs["rgb"].shape[:2], eqa_config.SIMULATOR.RGB_SENSOR.HEIGHT, eqa_config.SIMULATOR.RGB_SENSOR.WIDTH, ) ) env.close()
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() 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) assert np.allclose(obs["compass"], [0.0], atol=1e-5) assert np.allclose(obs["gps"], [0.0, 0.0], atol=1e-5) env.close()
def main(): parser = argparse.ArgumentParser() parser.add_argument( "--input-type", default="blind", choices=["blind", "rgb", "depth", "rgbd"], ) parser.add_argument("--model-path", default="", type=str) parser.add_argument( "--task-config", type=str, default="configs/tasks/pointnav.yaml" ) parser.add_argument( "--num-episodes", type=int, default=50 ) # frame rate defines the number of frame per action you want for videos generated parser.add_argument( "--frame-rate", type=int, default=1 ) args = parser.parse_args() config = get_config(args.task_config) agent_config = get_default_config() agent_config.INPUT_TYPE = args.input_type agent_config.MODEL_PATH = args.model_path num_episodes = args.num_episodes frame_rate = args.frame_rate agent = PPOAgent(agent_config) print("Establishing benchmark:") benchmark = habitat.Benchmark(config_paths=args.task_config) print("Evaluating:") metrics = benchmark.evaluate(agent, num_episodes=num_episodes, frame_rate=frame_rate) for k, v in metrics.items(): habitat.logger.info("{}: {:.3f}".format(k, v))
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()
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=[03.00611, 0.072447, -2.67867], start_rotation=[0, 0.163276, 0, 0.98658], goals=[], ) ] 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_pointnav_episode_generator(): config = get_config(CFG_TEST) config.defrost() config.DATASET.SPLIT = "val" config.ENVIRONMENT.MAX_EPISODE_STEPS = 500 config.freeze() env = habitat.Env(config) env.seed(config.SEED) random.seed(config.SEED) generator = pointnav_generator.generate_pointnav_episode( sim=env.sim, shortest_path_success_distance=config.TASK.SUCCESS_DISTANCE, shortest_path_max_steps=config.ENVIRONMENT.MAX_EPISODE_STEPS, ) episodes = [] for i in range(NUM_EPISODES): episode = next(generator) episodes.append(episode) for episode in pointnav_generator.generate_pointnav_episode( sim=env.sim, num_episodes=NUM_EPISODES, shortest_path_success_distance=config.TASK.SUCCESS_DISTANCE, shortest_path_max_steps=config.ENVIRONMENT.MAX_EPISODE_STEPS, geodesic_to_euclid_min_ratio=0, ): episodes.append(episode) assert len(episodes) == 2 * NUM_EPISODES env.episodes = episodes for episode in episodes: check_shortest_path(env, episode) dataset = habitat.Dataset() dataset.episodes = episodes assert dataset.to_json(), "Generated episodes aren't json serializable."
def test_dataset_splitting(split): dataset_config = get_config(CFG_TEST).DATASET dataset_config.defrost() dataset_config.SPLIT = split if not mp3d_dataset.Matterport3dDatasetV1.check_config_paths_exist( dataset_config): pytest.skip("Please download Matterport3D EQA dataset to data folder.") scenes = mp3d_dataset.Matterport3dDatasetV1.get_scenes_to_load( config=dataset_config) assert (len(scenes) > 0), "Expected dataset contains separate episode file per scene." dataset_config.CONTENT_SCENES = scenes full_dataset = make_dataset(id_dataset=dataset_config.TYPE, config=dataset_config) full_episodes = {(ep.scene_id, ep.episode_id) for ep in full_dataset.episodes} dataset_config.CONTENT_SCENES = scenes[0:len(scenes) // 2] split1_dataset = make_dataset(id_dataset=dataset_config.TYPE, config=dataset_config) split1_episodes = {(ep.scene_id, ep.episode_id) for ep in split1_dataset.episodes} dataset_config.CONTENT_SCENES = scenes[len(scenes) // 2:] split2_dataset = make_dataset(id_dataset=dataset_config.TYPE, config=dataset_config) split2_episodes = {(ep.scene_id, ep.episode_id) for ep in split2_dataset.episodes} assert full_episodes == split1_episodes.union( split2_episodes), "Split dataset is not equal to full dataset" assert (len(split1_episodes.intersection(split2_episodes)) == 0 ), "Intersection of split datasets is not the empty set"
def init_sim(): config = get_config() if not os.path.exists(config.SIMULATOR.SCENE): pytest.skip("Please download Habitat test data to data folder.") return make_sim(config.SIMULATOR.TYPE, config=config.SIMULATOR)
def main(): parser = argparse.ArgumentParser() parser.add_argument("--model-path", type=str, required=True) parser.add_argument("--sim-gpu-id", type=int, required=True) parser.add_argument("--pth-gpu-id", type=int, required=True) parser.add_argument("--num-processes", type=int, required=True) parser.add_argument("--hidden-size", type=int, default=512) parser.add_argument("--count-test-episodes", type=int, default=100) parser.add_argument( "--sensors", type=str, default="RGB_SENSOR,DEPTH_SENSOR", help="comma separated string containing different" "sensors to use, currently 'RGB_SENSOR' and" "'DEPTH_SENSOR' are supported", ) parser.add_argument( "--task-config", type=str, default="configs/tasks/pointnav.yaml", help="path to config yaml containing information about task", ) args = parser.parse_args() device = torch.device("cuda:{}".format(args.pth_gpu_id)) env_configs = [] baseline_configs = [] for _ in range(args.num_processes): config_env = get_config(config_paths=args.task_config) config_env.defrost() config_env.DATASET.SPLIT = "val" agent_sensors = args.sensors.strip().split(",") for sensor in agent_sensors: assert sensor in ["RGB_SENSOR", "DEPTH_SENSOR"] config_env.SIMULATOR.AGENT_0.SENSORS = agent_sensors config_env.freeze() env_configs.append(config_env) config_baseline = cfg_baseline() baseline_configs.append(config_baseline) assert len(baseline_configs) > 0, "empty list of datasets" envs = habitat.VectorEnv( make_env_fn=make_env_fn, env_fn_args=tuple( tuple(zip(env_configs, baseline_configs, range(args.num_processes)))), ) ckpt = torch.load(args.model_path, map_location=device) actor_critic = Policy( observation_space=envs.observation_spaces[0], action_space=envs.action_spaces[0], hidden_size=512, goal_sensor_uuid=env_configs[0].TASK.GOAL_SENSOR_UUID, ) actor_critic.to(device) ppo = PPO( actor_critic=actor_critic, clip_param=0.1, ppo_epoch=4, num_mini_batch=32, value_loss_coef=0.5, entropy_coef=0.01, lr=2.5e-4, eps=1e-5, max_grad_norm=0.5, ) ppo.load_state_dict(ckpt["state_dict"]) actor_critic = ppo.actor_critic observations = envs.reset() batch = batch_obs(observations) for sensor in batch: batch[sensor] = batch[sensor].to(device) episode_rewards = torch.zeros(envs.num_envs, 1, device=device) episode_spls = torch.zeros(envs.num_envs, 1, device=device) episode_success = torch.zeros(envs.num_envs, 1, device=device) episode_counts = torch.zeros(envs.num_envs, 1, device=device) current_episode_reward = torch.zeros(envs.num_envs, 1, device=device) test_recurrent_hidden_states = torch.zeros(args.num_processes, args.hidden_size, device=device) not_done_masks = torch.zeros(args.num_processes, 1, device=device) while episode_counts.sum() < args.count_test_episodes: with torch.no_grad(): _, actions, _, test_recurrent_hidden_states = actor_critic.act( batch, test_recurrent_hidden_states, not_done_masks, deterministic=False, ) outputs = envs.step([a[0].item() for a in actions]) observations, rewards, dones, infos = [list(x) for x in zip(*outputs)] batch = batch_obs(observations) for sensor in batch: batch[sensor] = batch[sensor].to(device) not_done_masks = torch.tensor( [[0.0] if done else [1.0] for done in dones], dtype=torch.float, device=device, ) for i in range(not_done_masks.shape[0]): if not_done_masks[i].item() == 0: episode_spls[i] += infos[i]["roomnavmetric"] if infos[i]["roomnavmetric"] > 0: episode_success[i] += 1 rewards = torch.tensor(rewards, dtype=torch.float, device=device).unsqueeze(1) current_episode_reward += rewards episode_rewards += (1 - not_done_masks) * current_episode_reward episode_counts += 1 - not_done_masks current_episode_reward *= not_done_masks episode_reward_mean = (episode_rewards / episode_counts).mean().item() episode_spl_mean = (episode_spls / episode_counts).mean().item() episode_success_mean = (episode_success / episode_counts).mean().item() print("Average episode reward: {:.6f}".format(episode_reward_mean)) print("Average episode success: {:.6f}".format(episode_success_mean)) print("Average episode spl: {:.6f}".format(episode_spl_mean))
def eval_checkpoint(checkpoint_path, args, writer, cur_ckpt_idx=0): env_configs = [] baseline_configs = [] device = torch.device("cuda", args.pth_gpu_id) for _ in range(args.num_processes): config_env = get_config(config_paths=args.task_config) config_env.defrost() config_env.DATASET.SPLIT = "val" agent_sensors = args.sensors.strip().split(",") for sensor in agent_sensors: assert sensor in ["RGB_SENSOR", "DEPTH_SENSOR"] config_env.SIMULATOR.AGENT_0.SENSORS = agent_sensors if args.video_option: config_env.TASK.MEASUREMENTS.append("TOP_DOWN_MAP") config_env.TASK.MEASUREMENTS.append("COLLISIONS") config_env.freeze() env_configs.append(config_env) config_baseline = cfg_baseline() baseline_configs.append(config_baseline) assert len(baseline_configs) > 0, "empty list of datasets" envs = habitat.VectorEnv( make_env_fn=make_env_fn, env_fn_args=tuple( tuple( zip(env_configs, baseline_configs, range(args.num_processes)) ) ), ) ckpt = torch.load(checkpoint_path, map_location=device) actor_critic = Policy( observation_space=envs.observation_spaces[0], action_space=envs.action_spaces[0], hidden_size=512, goal_sensor_uuid=env_configs[0].TASK.GOAL_SENSOR_UUID, ) actor_critic.to(device) ppo = PPO( actor_critic=actor_critic, clip_param=0.1, ppo_epoch=4, num_mini_batch=32, value_loss_coef=0.5, entropy_coef=0.01, lr=2.5e-4, eps=1e-5, max_grad_norm=0.5, ) ppo.load_state_dict(ckpt["state_dict"]) actor_critic = ppo.actor_critic observations = envs.reset() batch = batch_obs(observations) for sensor in batch: batch[sensor] = batch[sensor].to(device) episode_rewards = torch.zeros(envs.num_envs, 1, device=device) episode_spls = torch.zeros(envs.num_envs, 1, device=device) episode_success = torch.zeros(envs.num_envs, 1, device=device) episode_counts = torch.zeros(envs.num_envs, 1, device=device) current_episode_reward = torch.zeros(envs.num_envs, 1, device=device) test_recurrent_hidden_states = torch.zeros( args.num_processes, args.hidden_size, device=device ) not_done_masks = torch.zeros(args.num_processes, 1, device=device) stats_episodes = set() rgb_frames = None if args.video_option: rgb_frames = [[]] * args.num_processes os.makedirs(args.video_dir, exist_ok=True) while episode_counts.sum() < args.count_test_episodes: current_episodes = envs.current_episodes() with torch.no_grad(): _, actions, _, test_recurrent_hidden_states = actor_critic.act( batch, test_recurrent_hidden_states, not_done_masks, deterministic=False, ) outputs = envs.step([a[0].item() for a in actions]) observations, rewards, dones, infos = [list(x) for x in zip(*outputs)] batch = batch_obs(observations) for sensor in batch: batch[sensor] = batch[sensor].to(device) not_done_masks = torch.tensor( [[0.0] if done else [1.0] for done in dones], dtype=torch.float, device=device, ) for i in range(not_done_masks.shape[0]): if not_done_masks[i].item() == 0: episode_spls[i] += infos[i]["spl"] if infos[i]["spl"] > 0: episode_success[i] += 1 rewards = torch.tensor( rewards, dtype=torch.float, device=device ).unsqueeze(1) current_episode_reward += rewards episode_rewards += (1 - not_done_masks) * current_episode_reward episode_counts += 1 - not_done_masks current_episode_reward *= not_done_masks next_episodes = envs.current_episodes() envs_to_pause = [] n_envs = envs.num_envs for i in range(n_envs): if next_episodes[i].episode_id in stats_episodes: envs_to_pause.append(i) # episode ended if not_done_masks[i].item() == 0: stats_episodes.add(current_episodes[i].episode_id) if args.video_option: generate_video( args, rgb_frames[i], current_episodes[i].episode_id, cur_ckpt_idx, infos[i]["spl"], writer, ) rgb_frames[i] = [] # episode continues elif args.video_option: frame = observations_to_image(observations[i], infos[i]) rgb_frames[i].append(frame) # stop tracking ended episodes if they exist if len(envs_to_pause) > 0: state_index = list(range(envs.num_envs)) for idx in reversed(envs_to_pause): state_index.pop(idx) envs.pause_at(idx) # indexing along the batch dimensions test_recurrent_hidden_states = test_recurrent_hidden_states[ :, state_index ] not_done_masks = not_done_masks[state_index] current_episode_reward = current_episode_reward[state_index] for k, v in batch.items(): batch[k] = v[state_index] if args.video_option: rgb_frames = [rgb_frames[i] for i in state_index] episode_reward_mean = (episode_rewards / episode_counts).mean().item() episode_spl_mean = (episode_spls / episode_counts).mean().item() episode_success_mean = (episode_success / episode_counts).mean().item() logger.info("Average episode reward: {:.6f}".format(episode_reward_mean)) logger.info("Average episode success: {:.6f}".format(episode_success_mean)) logger.info("Average episode SPL: {:.6f}".format(episode_spl_mean)) writer.add_scalars( "eval_reward", {"average reward": episode_reward_mean}, cur_ckpt_idx ) writer.add_scalars( "eval_SPL", {"average SPL": episode_spl_mean}, cur_ckpt_idx ) writer.add_scalars( "eval_success", {"average success": episode_success_mean}, cur_ckpt_idx )
def habitat_objectnav_make_env_fn(cfg, rank, log_dir, visdom_name='main', visdom_log_file=None, vis_interval=200, visdom_server='localhost', visdom_port='8097'): habitat_cfg = get_config(cfg.task.habitat.config_file) training_scenes = [ 'PX4nDJXEHrG', '5q7pvUzZiYa', 'S9hNv5qa7GM', 'ac26ZMwG7aT', '29hnd4uzFmX', '82sE5b5pLXE', 'p5wJjkQkbXX', 'B6ByNegPMKs', '17DRP5sb8fy', 'pRbA3pwrgk9', 'gZ6f7yhEvPG', 'HxpKQynjfin', 'ZMojNkEp431', '5LpN3gDmAk7', 'dhjEzFoUFzH', 'vyrNrziPKCB', 'sKLMLpTHeUy', '759xd9YjKW5', 'sT4fr6TAbpF', '1pXnuDYAj8r', 'E9uDoFAP3SH', 'GdvgFV5R1Z5', 'rPc6DW4iMge', 'D7N2EKCX4Sj', 'uNb9QFRL6hY', 'VVfe2KiqLaN', 'Vvot9Ly1tCj', 's8pcmisQ38h', 'EDJbREhghzL', 'YmJkqBEsHnH', 'XcA2TqTSSAj', '7y3sRwLe3Va', 'e9zR4mvMWw7', 'JeFG25nYj2p', 'VLzqgDo317F', 'kEZ7cmS4wCh', 'r1Q1Z4BcV1o', 'qoiz87JEwZ2', '1LXtFkjw3qL', 'VFuaQ6m2Qom', 'b8cTxDM8gDG', 'ur6pFq6Qu1A', 'V2XKFyX4ASd', 'Uxmj2M2itWa', 'Pm6F8kyY3z2', 'PuKPg4mmafe', '8WUmhLawc2A', 'ULsKaCPVFJR', 'r47D5H71a5s', 'jh4fc5c5qoQ', 'JF19kD82Mey', 'D7G3Y4RVNrH', 'cV4RVeZvu5T', 'mJXqzFtmKg4', 'i5noydFURQK', 'aayBHfsNo7d' ] length = int(len(training_scenes) / cfg.training.num_envs) habitat_cfg.defrost() if rank < cfg.training.num_envs: habitat_cfg.DATASET.SPLIT = 'train' else: habitat_cfg.DATASET.SPLIT = 'val' cfg.defrost() habitat_cfg.TASK.CUSTOM_OBJECT_GOAL_SENSOR = habitat.Config() habitat_cfg.TASK.CUSTOM_OBJECT_GOAL_SENSOR.TYPE = 'CustomObjectSensor' habitat_cfg.TASK.CUSTOM_OBJECT_GOAL_SENSOR.GOAL_SPEC = "OBJECT_IMG" habitat_cfg.DATASET.CONTENT_SCENES = training_scenes[rank * length:(rank + 1) * length] habitat_cfg.freeze() cfg.task.habitat.TASK_CONFIG = habitat_cfg cfg.freeze() def filter_fn(episode): if episode.info['geodesic_distance'] > 3.0: return False else: return True dataset = make_dataset(habitat_cfg.DATASET.TYPE, config=habitat_cfg.DATASET, **{'filter_fn': filter_fn}) env = ObjectNavENV(cfg, dataset=dataset) env.seed(rank) env = VisdomMonitor(env, directory=os.path.join(log_dir, visdom_name), video_callable=lambda x: x % vis_interval == 0, uid=str(rank), server=visdom_server, port=visdom_port, visdom_log_file=visdom_log_file, visdom_env=visdom_name) return env
def __init__(self, config_paths: Optional[str] = None) -> None: self.action_history: Dict = defaultdict() self.agg_metrics: Dict = defaultdict(float) config_env = get_config(config_paths) self._env = Env(config=config_env)
def test_mp3d_eqa_sim_correspondence(): eqa_config = get_config(CFG_TEST) if not mp3d_dataset.Matterport3dDatasetV1.check_config_paths_exist( eqa_config.DATASET ): pytest.skip( "Please download Matterport3D EQA dataset to " "data folder." ) dataset = make_dataset( id_dataset=eqa_config.DATASET.TYPE, config=eqa_config.DATASET ) env = habitat.Env(config=eqa_config, dataset=dataset) env.episodes = [ episode for episode in dataset.episodes if int(episode.episode_id) in TEST_EPISODE_SET[:EPISODES_LIMIT] ] ep_i = 0 cycles_n = 2 while cycles_n > 0: env.reset() episode = env.current_episode assert ( len(episode.goals) == 1 ), "Episode has no goals or more than one." assert ( len(episode.shortest_paths) == 1 ), "Episode has no shortest paths or more than one." start_state = env.sim.get_agent_state() assert np.allclose( start_state.position, episode.start_position ), "Agent's start position diverges from the shortest path's one." rgb_mean = 0 logger.info( "{id} {question}\n{answer}".format( id=episode.episode_id, question=episode.question.question_text, answer=episode.question.answer_text, ) ) for step_id, point in enumerate(episode.shortest_paths[0]): cur_state = env.sim.get_agent_state() logger.info( "diff position: {} diff rotation: {} " "cur_state.position: {} shortest_path.position: {} " "cur_state.rotation: {} shortest_path.rotation: {} action: {}" "".format( cur_state.position - point.position, cur_state.rotation - point.rotation, cur_state.position, point.position, cur_state.rotation, point.rotation, point.action, ) ) assert np.allclose( [cur_state.position[0], cur_state.position[2]], [point.position[0], point.position[2]], atol=CLOSE_STEP_THRESHOLD * (step_id + 1), ), "Agent's path diverges from the shortest path." obs = env.step(point.action) if not env.episode_over: rgb_mean += obs["rgb"][:, :, :3].mean() if ep_i < len(RGB_EPISODE_MEANS): rgb_mean = rgb_mean / len(episode.shortest_paths[0]) assert np.isclose( RGB_EPISODE_MEANS[int(episode.episode_id)], rgb_mean ), "RGB output doesn't match the ground truth." ep_i = (ep_i + 1) % EPISODES_LIMIT if ep_i == 0: cycles_n -= 1 env.close()
if self.is_goal_reached(): if self.located_true_goal: action = HabitatSimActions.STOP else: action = HabitatSimActions.TURN_LEFT self.needs_inspection = True return {"action": action} if self.unseen_obstacle: command = HabitatSimActions.TURN_RIGHT return command command = HabitatSimActions.STOP command = self.planner_prediction_to_command(self.waypointPose6D) return command config = get_config("../habitat-api/configs/tasks/objectnav_mp3d_fast.yaml") config.defrost() # ----------------------------------------------------------------------------- # ORBSLAM2 BASELINE # ----------------------------------------------------------------------------- config.ORBSLAM2 = CN() config.ORBSLAM2.SLAM_VOCAB_PATH = "../habitat-api/habitat_baselines/slambased/data/ORBvoc.txt" config.ORBSLAM2.SLAM_SETTINGS_PATH = ( "../habitat-api/habitat_baselines/slambased/data/mp3d3_small1k.yaml") config.ORBSLAM2.MAP_CELL_SIZE = 0.1 config.ORBSLAM2.MAP_SIZE = 40 config.ORBSLAM2.CAMERA_HEIGHT = config.SIMULATOR.DEPTH_SENSOR.POSITION[1] config.ORBSLAM2.BETA = 100 config.ORBSLAM2.H_OBSTACLE_MIN = 0.3 * config.ORBSLAM2.CAMERA_HEIGHT config.ORBSLAM2.H_OBSTACLE_MAX = 1.0 * config.ORBSLAM2.CAMERA_HEIGHT config.ORBSLAM2.D_OBSTACLE_MIN = 0.1
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_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 ] 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(non_stop_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_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."
from typing import List, Optional, Union from habitat.config.default import Config as CN from habitat.config.default import get_config _C = get_config() _C.defrost() # ----------------------------------------------------------------------------- # GPS SENSOR # ----------------------------------------------------------------------------- _C.TASK.GLOBAL_GPS_SENSOR = CN() _C.TASK.GLOBAL_GPS_SENSOR.TYPE = "GlobalGPSSensor" _C.TASK.GLOBAL_GPS_SENSOR.DIMENSIONALITY = 3 # ----------------------------------------------------------------------------- # ORACLE ACTION SENSOR # ----------------------------------------------------------------------------- _C.TASK.ORACLE_ACTION_SENSOR = CN() _C.TASK.ORACLE_ACTION_SENSOR.TYPE = "OracleActionSensor" _C.TASK.ORACLE_ACTION_SENSOR.GOAL_RADIUS = 0.5 # ----------------------------------------------------------------------------- # VLN ORACLE ACTION SENSOR # ----------------------------------------------------------------------------- _C.TASK.VLN_ORACLE_ACTION_SENSOR = CN() _C.TASK.VLN_ORACLE_ACTION_SENSOR.TYPE = "VLNOracleActionSensor" _C.TASK.VLN_ORACLE_ACTION_SENSOR.GOAL_RADIUS = 0.5 # ----------------------------------------------------------------------------- # VLN ORACLE PROGRESS SENSOR # ----------------------------------------------------------------------------- _C.TASK.VLN_ORACLE_PROGRESS_SENSOR = CN() _C.TASK.VLN_ORACLE_PROGRESS_SENSOR.TYPE = "VLNOracleProgressSensor"
def run_episodes(self): self.ep_idx = 0 # if os.path.exists('/home/nel/gsarch/habitat/habitat-lab/HabitatScripts/maps'): # dir_name = "/home/nel/gsarch/habitat/habitat-lab/HabitatScripts/maps/" # test = os.listdir(dir_name) # for item in test: # if item.endswith(".png"): # os.remove(os.path.join(dir_name, item)) for episode in range(self.num_episodes): print("STARTING EPISODE ", episode) print("DELETING PREVIOUS FILES") # remove png files from previous episode filelist = glob.glob( os.path.join(self.orbslam_path, "rgb", "*.png")) for f in filelist: os.remove(f) filelist = glob.glob( os.path.join(self.orbslam_path, "depth", "*.png")) for f in filelist: os.remove(f) # # reset trajectory files # deleted = [] # if os.path.exists(self.ORBSLAM_Cam_Traj): # deleted.append(['cam_traj']) # os.remove(self.ORBSLAM_Cam_Traj) # if os.path.exists(self.ORBSLAM_Cam_Keypoints): # deleted.append(['keypoint_traj']) # os.remove(self.ORBSLAM_Cam_Keypoints) # associations_file = "/home/nel/ORB_SLAM2/Examples/RGB-D/associations/replica.txt" # if os.path.exists(associations_file): # deleted.append(['associations']) # os.remove(associations_file) # if os.path.exists(self.orbslam_path + "/" + 'rgb.txt'): # deleted.append(['rgb.txt']) # os.remove(self.orbslam_path + "/" + 'rgb.txt') # if os.path.exists(self.orbslam_path + "/" + 'depth.txt'): # deleted.append(['depth.txt']) # os.remove(self.orbslam_path + "/" + 'depth.txt') # print('Deleted: ', deleted) # print("DONE. ", "DELETED ", len(filelist), " files.") # mapname = np.random.choice(self.mapnames) mapname = self.mapnames[episode] # KEEP THIS #mapname = 'apartment_0' #mapname = 'frl_apartment_4' self.test_scene = "/home/nel/gsarch/Replica-Dataset/out/{}/habitat/mesh_semantic.ply".format( mapname) self.object_json = "/home/nel/gsarch/Replica-Dataset/out/{}/habitat/info_semantic.json".format( mapname) # self.test_scene = "/hdd/replica/Replica-Dataset/out/{}/habitat/mesh_semantic.ply".format(mapname) # self.object_json = "/hdd/replica/Replica-Dataset/out/{}/habitat/info_semantic.json".format(mapname) self.sim_settings = { "width": 256, # Spatial resolution of the observations "height": 256, "scene": self.test_scene, # Scene path "default_agent": 0, "sensor_height": 1.5, # Height of sensors in meters "color_sensor": True, # RGB sensor "semantic_sensor": True, # Semantic sensor "depth_sensor": True, # Depth sensor "seed": 1, } # self.basepath = f"/home/nel/gsarch/replica_novel_categories/{mapname}_{episode}" self.basepath = '/home/nel/gsarch/replica_depth_temp' # if os.path.exists(self.basepath): # os.remove(self.basepath + "/*") self.basepath = self.basepath + f"/{mapname}_{episode}" print("BASEPATH: ", self.basepath) # self.basepath = f"/hdd/ayushj/habitat_data/{mapname}_{episode}" if not os.path.exists(self.basepath): os.mkdir(self.basepath) self.cfg, self.sim_cfg = self.make_cfg(self.sim_settings) self.sim = habitat_sim.Simulator(self.cfg) random.seed(self.sim_settings["seed"]) self.sim.seed(self.sim_settings["seed"]) self.set_agent(self.sim_settings) self.nav_pts = self.get_navigable_points() config = get_config() config.defrost() config.TASK.SENSORS = [] config.SIMULATOR.AGENT_0.SENSORS = ["RGB_SENSOR", "DEPTH_SENSOR"] config.freeze() self.run() self.sim.close() time.sleep(3) self.ep_idx += 1
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)], ) ] ) 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"] 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 run_episodes(self): for episode in range(self.num_episodes): print("STARTING EPISODE ", episode) # mapname = np.random.choice(self.mapnames) mapname = self.mapnames[episode] # KEEP THIS #mapname = 'apartment_0' #mapname = 'frl_apartment_4' self.test_scene = "/home/nel/gsarch/Replica-Dataset/out/{}/habitat/mesh_semantic.ply".format( mapname) self.object_json = "/home/nel/gsarch/Replica-Dataset/out/{}/habitat/info_semantic.json".format( mapname) # self.test_scene = "/hdd/replica/Replica-Dataset/out/{}/habitat/mesh_semantic.ply".format(mapname) # self.object_json = "/hdd/replica/Replica-Dataset/out/{}/habitat/info_semantic.json".format(mapname) self.sim_settings = { "width": 256, # Spatial resolution of the observations "height": 256, "scene": self.test_scene, # Scene path "default_agent": 0, "sensor_height": 1.5, # Height of sensors in meters "color_sensor": True, # RGB sensor "semantic_sensor": True, # Semantic sensor "depth_sensor": True, # Depth sensor "seed": 1, } self.basepath = f"/home/nel/gsarch/replica_context/{mapname}_{episode}" # self.basepath = f"/hdd/ayushj/habitat_data/{mapname}_{episode}" if not os.path.exists(self.basepath): os.mkdir(self.basepath) self.cfg, self.sim_cfg = self.make_cfg(self.sim_settings) self.sim = habitat_sim.Simulator(self.cfg) random.seed(self.sim_settings["seed"]) self.sim.seed(self.sim_settings["seed"]) self.set_agent(self.sim_settings) self.nav_pts = self.get_navigable_points() config = get_config() config.defrost() config.TASK.SENSORS = [] config.SIMULATOR.AGENT_0.SENSORS = ["RGB_SENSOR", "DEPTH_SENSOR"] config.freeze() self.run() self.sim.close() time.sleep(3) fig, ax = plt.subplots(nrows=5, ncols=7, sharex=True, sharey=True) fig.text(0.5, 0.04, 'Distance between objects', ha='center') fig.text(0.04, 0.5, 'Density', va='center', rotation='vertical') keys = list(self.object_pair_dist.keys()) idx = 0 for row in ax: for col in row: try: key = keys[idx] except: break # plt.hist(self.object_pair_dist['indoor-plant/chair'], density=True, bins=10) vals = np.array(self.object_pair_dist[key]) vals[vals > 10] = 9.99 col.hist(vals, density=True, bins=20, range=[0, 10]) col.set_ylim(top=1) col.set_title(key, fontsize=5, y=0.9) idx += 1 plt.savefig("/home/nel/gsarch/replica_context/figure.png")
def main(): parser = argparse.ArgumentParser() parser.add_argument("--model-path", type=str, required=True) parser.add_argument("--sim-gpu-id", type=int, required=True) parser.add_argument("--pth-gpu-id", type=int, required=True) parser.add_argument("--num-processes", type=int, required=True) parser.add_argument("--hidden-size", type=int, default=512) parser.add_argument("--count-test-episodes", type=int, default=100) parser.add_argument( "--sensors", type=str, default="DEPTH_SENSOR", help="comma separated string containing different" "sensors to use, currently 'RGB_SENSOR' and" "'DEPTH_SENSOR' are supported", ) parser.add_argument( "--task-config", type=str, default="configs/tasks/pointnav.yaml", help="path to config yaml containing information about task", ) cmd_line_inputs = [ "--model-path", "/home/bruce/NSERC_2019/habitat-api/data/checkpoints/depth.pth", "--sim-gpu-id", "0", "--pth-gpu-id", "0", "--num-processes", "1", "--count-test-episodes", "100", "--task-config", "configs/tasks/pointnav.yaml", ] args = parser.parse_args(cmd_line_inputs) device = torch.device("cuda:{}".format(args.pth_gpu_id)) env_configs = [] baseline_configs = [] for _ in range(args.num_processes): config_env = get_config(config_paths=args.task_config) config_env.defrost() config_env.DATASET.SPLIT = "val" agent_sensors = args.sensors.strip().split(",") for sensor in agent_sensors: assert sensor in ["RGB_SENSOR", "DEPTH_SENSOR"] config_env.SIMULATOR.AGENT_0.SENSORS = agent_sensors config_env.freeze() env_configs.append(config_env) config_baseline = cfg_baseline() baseline_configs.append(config_baseline) assert len(baseline_configs) > 0, "empty list of datasets" envs = habitat.VectorEnv( make_env_fn=make_env_fn, env_fn_args=tuple( tuple(zip(env_configs, baseline_configs, range(args.num_processes))) ), ) ckpt = torch.load(args.model_path, map_location=device) actor_critic = Policy( observation_space=envs.observation_spaces[0], action_space=envs.action_spaces[0], hidden_size=512, goal_sensor_uuid="pointgoal", ) actor_critic.to(device) ppo = PPO( actor_critic=actor_critic, clip_param=0.1, ppo_epoch=4, num_mini_batch=32, value_loss_coef=0.5, entropy_coef=0.01, lr=2.5e-4, eps=1e-5, max_grad_norm=0.5, ) ppo.load_state_dict(ckpt["state_dict"]) actor_critic = ppo.actor_critic observations = envs.reset() batch = batch_obs(observations) for sensor in batch: batch[sensor] = batch[sensor].to(device) test_recurrent_hidden_states = torch.zeros( args.num_processes, args.hidden_size, device=device ) not_done_masks = torch.zeros(args.num_processes, 1, device=device) def transform_callback(data): nonlocal actor_critic nonlocal batch nonlocal not_done_masks nonlocal test_recurrent_hidden_states global flag global t_prev_update global observation if flag == 2: observation["depth"] = np.reshape(data.data[0:-2], (256, 256, 1)) observation["pointgoal"] = data.data[-2:] flag = 1 return pointgoal_received = data.data[-2:] translate_amount = 0.25 # meters rotate_amount = 0.174533 # radians isrotated = ( rotate_amount * 0.95 <= abs(pointgoal_received[1] - observation["pointgoal"][1]) <= rotate_amount * 1.05 ) istimeup = (time.time() - t_prev_update) >= 4 # print('istranslated is '+ str(istranslated)) # print('isrotated is '+ str(isrotated)) # print('istimeup is '+ str(istimeup)) if isrotated or istimeup: vel_msg = Twist() vel_msg.linear.x = 0 vel_msg.linear.y = 0 vel_msg.linear.z = 0 vel_msg.angular.x = 0 vel_msg.angular.y = 0 vel_msg.angular.z = 0 pub_vel.publish(vel_msg) time.sleep(0.2) print("entered update step") # cv2.imshow("Depth", observation['depth']) # cv2.waitKey(100) observation["depth"] = np.reshape(data.data[0:-2], (256, 256, 1)) observation["pointgoal"] = data.data[-2:] batch = batch_obs([observation]) for sensor in batch: batch[sensor] = batch[sensor].to(device) if flag == 1: not_done_masks = torch.tensor([0.0], dtype=torch.float, device=device) flag = 0 else: not_done_masks = torch.tensor([1.0], dtype=torch.float, device=device) _, actions, _, test_recurrent_hidden_states = actor_critic.act( batch, test_recurrent_hidden_states, not_done_masks, deterministic=True ) action_id = actions.item() print( "observation received to produce action_id is " + str(observation["pointgoal"]) ) print("action_id from net is " + str(actions.item())) t_prev_update = time.time() vel_msg = Twist() vel_msg.linear.x = 0 vel_msg.linear.y = 0 vel_msg.linear.z = 0 vel_msg.angular.x = 0 vel_msg.angular.y = 0 vel_msg.angular.z = 0 if action_id == 0: vel_msg.linear.x = 0.25 / 4 pub_vel.publish(vel_msg) elif action_id == 1: vel_msg.angular.z = 10 / 180 * 3.1415926 pub_vel.publish(vel_msg) elif action_id == 2: vel_msg.angular.z = -10 / 180 * 3.1415926 pub_vel.publish(vel_msg) else: pub_vel.publish(vel_msg) sub.unregister() print("NN finished navigation task") sub = rospy.Subscriber( "depth_and_pointgoal", numpy_msg(Floats), transform_callback, queue_size=1 ) rospy.spin()
def __init__(self, config_paths: Optional[str] = None) -> None: config_env = get_config(config_paths) self._env = Env(config=config_env)
def generate_episodes(): # -- Load scene files semantic_data = np.load(SCENE_INFO_PATH, allow_pickle=True).item() df_semantic = semantic_data["df_semantic"] df_objects = semantic_data["df_objects"] # filter classes selected_classes = MATCHING_CLASSES.keys() df_objects = df_objects[df_objects.class_name.apply( lambda x: x in selected_classes)] print("_____________CLASSES INFO_____________") print(df_objects.class_name.value_counts()) print(f"Total count: {len(df_objects)}") print("_______________________________________") # -- Load config config = get_config(BASE_CFG_PATH) base_success = config.TASK.SUCCESS_DISTANCE env = habitat.Env(config) env.seed(config.SEED) random.seed(config.SEED) # Generate spiral coords spiral_shift = np.array(spiral(5, 0.001)) dataset_episodes = [] for room in df_objects.room.unique(): print(f"Generating episodes for room {room}") # if room in ['office_1', 'office_2', 'room_2', 'frl_apartment_0', 'office_3', # 'frl_apartment_2', 'hotel_0', 'apartment_0', 'frl_apartment_5', # 'room_1', 'room_0', 'apartment_2', 'apartment_1']: # continue scene_path = SCENE_MOCKUP_PATH.format(room) scene_df = df_objects[df_objects.room == room] config.defrost() config.SIMULATOR.SCENE = scene_path config.freeze() _random_episode(env, config) out = env.reset() # ===================================================================== # Determine floor coord/s pts = [] while len(pts) < 300: pt = env.sim.sample_navigable_point() if env.sim.island_radius(pt) > ISLAND_RADIUS_LIMIT: pts.append(pt[1]) floor_coords = pd.value_counts(pts).index.values floor_coord = [] while len(floor_coords) > 0: floor_coord.append(floor_coords[0]) floor_coords = floor_coords[floor_coords > (floor_coord[-1] + 2.3)] print(f"Room {room} has {len(floor_coord)} floors @ ({floor_coord})") print("objects", scene_df.class_name.unique()) # ===================================================================== for obj_idx, obj_row in scene_df.iterrows(): print(f"-generating {NUM_EPISODES} for {obj_row['class_name']}") t_coord = obj_row["habitat_coord"] t_size = obj_row["habitat_size"] # Determine success distance h_to_obj = determine_height_to_object(t_coord, t_size, floor_coord) if h_to_obj is None: print(f"Coord under floor {obj_idx}, coord: {t_coord}") continue success_distance = \ determine_height_to_object(t_coord, t_size, floor_coord) + \ base_success + max(t_size) / 2. generator = generate_pointnav_episode( sim=env.sim, target=t_coord, shortest_path_success_distance=success_distance, shortest_path_max_steps=config.ENVIRONMENT.MAX_EPISODE_STEPS, geodesic_to_euclid_min_ratio=1.1, number_retries_per_target=50, geodesic_min_ratio_prob=0.5, floor_coord=floor_coord, spiral_coord=spiral_shift, ) episodes = [] for i in range(NUM_EPISODES): episode = next(generator) # Add arguments episode.room = room episode.t_coord = t_coord episode.t_size = t_size episode.class_name = obj_row['class_name'] episodes.append(episode) for episode in episodes: check_shortest_path(env, episode) dataset_episodes += episodes np.save("dataset_all", dataset_episodes) dataset = habitat.Dataset() dataset.episodes = dataset_episodes
NUM_EPS_PER_LOCATION = 10 if DEBUG else 100 elif DATA_SPLIT == "val": NUM_EPS_PER_LOCATION = 10 SPLIT = "val" elif DATA_SPLIT == "test": NUM_EPS_PER_LOCATION = 10 SPLIT = "test" else: raise NotImplementedError("Not implemented for this split type") OUTPUT_PATH = os.path.join("data", "datasets", "pointnav", DATASET, "v1", SPLIT) if not os.path.exists(os.path.join(OUTPUT_PATH)): os.makedirs(os.path.join(OUTPUT_PATH)) config = get_config(CFG) config.TASK.success_distance = 0.2 config.TASK.success_reward = 10.0 config.TASK.slack_reward = -0.01 config.TASK.goal_type = "dense" # get list of all scenes if DATASET == "mp3d": scenes = sorted(glob.glob("data/scene_datasets/mp3d/*")) elif DATASET == "gibson": scenes = sorted(glob.glob("data/scene_datasets/mp3d/*")) else: raise NotImplementedError("Not implemented for this dataset") num_scenes = len(scenes) if DATA_SPLIT == "train":