def main():
# Get the default config node
config = habitat.get_config(config_paths="configs/tasks/pointnav.yaml")
config.defrost()
# Add things to the config to for the measure
config.TASK.EPISODE_INFO_EXAMPLE = habitat.Config()
# The type field is used to look-up the measure in the registry.
# By default, the things are registered with the class name
config.TASK.EPISODE_INFO_EXAMPLE.TYPE = "EpisodeInfoExample"
config.TASK.EPISODE_INFO_EXAMPLE.VALUE = 5
# Add the measure to the list of measures in use
config.TASK.MEASUREMENTS.append("EPISODE_INFO_EXAMPLE")
# Now define the config for the sensor
config.TASK.AGENT_POSITION_SENSOR = habitat.Config()
# Use the custom name
config.TASK.AGENT_POSITION_SENSOR.TYPE = "my_supercool_sensor"
config.TASK.AGENT_POSITION_SENSOR.ANSWER_TO_LIFE = 42
# Add the sensor to the list of sensors in use
config.TASK.SENSORS.append("AGENT_POSITION_SENSOR")
config.freeze()
with habitat.Env(config=config) as env:
print(env.reset()["agent_position"])
print(env.get_metrics()["episode_info"])
print(env.step("MOVE_FORWARD")["agent_position"])
print(env.get_metrics()["episode_info"])
def get_config(training: bool = False,
top_down_map: bool = False,
max_steps: Optional[Union[int, float]] = None,
task: str = 'pointnav',
train_dataset: str = 'habitat_test',
train_split: str = 'train',
eval_dataset: str = 'habitat_test',
eval_split: str = 'val',
gpu_id: int = 0,
image_key: str = 'rgb',
goal_key: str = 'pointgoal_with_gps_compass',
depth_key: Optional[str] = None,
reward_function: RewardFunction = gin.REQUIRED,
eval_episodes_per_scene: int = 3) -> Dict[str, Any]:
mode = 'train' if training else 'eval'
dataset = train_dataset if training else eval_dataset
split = train_split if training else eval_split
opts = ['SIMULATOR.HABITAT_SIM_V0.GPU_DEVICE_ID', gpu_id]
if max_steps:
opts += ['ENVIRONMENT.MAX_EPISODE_STEPS', int(max_steps)]
opts += ['DATASET.SPLIT', split]
if not training:
opts += ['ENVIRONMENT.ITERATOR_OPTIONS.SHUFFLE', False]
opts += [
'ENVIRONMENT.ITERATOR_OPTIONS.MAX_SCENE_REPEAT_EPISODES',
eval_episodes_per_scene
]
if not task.endswith('.yaml'):
task = f'{get_config_dir()}/habitat/tasks/{task}.yaml'
if not dataset.endswith('.yaml'):
dataset = f'{get_config_dir()}/habitat/datasets/{dataset}.yaml'
task_file = Path(wandb.run.dir) / 'task.yaml'
dataset_file = Path(wandb.run.dir) / f'{mode}_dataset.yaml'
copyfile(task, task_file)
copyfile(dataset, dataset_file)
wandb.save(str(task_file))
wandb.save(str(dataset_file))
if not HabitatSimActions.has_action('TURN_ANGLE'):
HabitatSimActions.extend_action_space('TURN_ANGLE')
config = habitat.get_config([task, dataset], opts)
config.defrost()
config.TASK.SUCCESS = habitat.Config()
config.TASK.SUCCESS.TYPE = 'Success'
config.TASK.SUCCESS.SUCCESS_DISTANCE = config.TASK.SUCCESS_DISTANCE
config.TASK.MEASUREMENTS.append('SUCCESS')
config.TASK.ACTIONS.TURN_ANGLE = habitat.Config()
config.TASK.ACTIONS.TURN_ANGLE.TYPE = 'TurnAngleAction'
config.TASK.POSSIBLE_ACTIONS = ['STOP', 'MOVE_FORWARD', 'TURN_ANGLE']
if top_down_map and 'TOP_DOWN_MAP' not in config.TASK.MEASUREMENTS:
# Top-down map is expensive to compute, so we only enable it when needed.
config.TASK.MEASUREMENTS.append('TOP_DOWN_MAP')
config.freeze()
return {
'config': config,
'image_key': image_key,
'goal_key': goal_key,
'depth_key': depth_key,
'reward_function': reward_function
}
def add_panoramic_camera(task_config):
task_config.SIMULATOR.RGB_SENSOR_LEFT = task_config.SIMULATOR.RGB_SENSOR.clone(
)
task_config.SIMULATOR.RGB_SENSOR_LEFT.TYPE = "PanoramicPartRGBSensor"
task_config.SIMULATOR.RGB_SENSOR_LEFT.ORIENTATION = [0, 2 / 3 * np.pi, 0]
task_config.SIMULATOR.RGB_SENSOR_LEFT.ANGLE = "left"
task_config.SIMULATOR.RGB_SENSOR_RIGHT = task_config.SIMULATOR.RGB_SENSOR.clone(
)
task_config.SIMULATOR.RGB_SENSOR_RIGHT.TYPE = "PanoramicPartRGBSensor"
task_config.SIMULATOR.RGB_SENSOR_RIGHT.ORIENTATION = [0, -2 / 3 * np.pi, 0]
task_config.SIMULATOR.RGB_SENSOR_RIGHT.ANGLE = "right"
task_config.SIMULATOR.AGENT_0.SENSORS = [
'RGB_SENSOR', 'RGB_SENSOR_LEFT', 'RGB_SENSOR_RIGHT'
]
task_config.SIMULATOR.DEPTH_SENSOR_LEFT = task_config.SIMULATOR.DEPTH_SENSOR.clone(
)
task_config.SIMULATOR.DEPTH_SENSOR_LEFT.TYPE = "PanoramicPartDepthSensor"
task_config.SIMULATOR.DEPTH_SENSOR_LEFT.ORIENTATION = [0, 2 / 3 * np.pi, 0]
task_config.SIMULATOR.DEPTH_SENSOR_LEFT.ANGLE = "left"
task_config.SIMULATOR.DEPTH_SENSOR_RIGHT = task_config.SIMULATOR.DEPTH_SENSOR.clone(
)
task_config.SIMULATOR.DEPTH_SENSOR_RIGHT.TYPE = "PanoramicPartDepthSensor"
task_config.SIMULATOR.DEPTH_SENSOR_RIGHT.ORIENTATION = [
0, -2 / 3 * np.pi, 0
]
task_config.SIMULATOR.DEPTH_SENSOR_RIGHT.ANGLE = "right"
task_config.SIMULATOR.AGENT_0.SENSORS += [
'DEPTH_SENSOR', 'DEPTH_SENSOR_LEFT', 'DEPTH_SENSOR_RIGHT'
]
task_config.TASK.CUSTOM_VISTARGET_SENSOR = habitat.Config()
task_config.TASK.CUSTOM_VISTARGET_SENSOR.TYPE = 'CustomVisTargetSensor'
task_config.TASK.PANORAMIC_SENSOR = habitat.Config()
task_config.TASK.PANORAMIC_SENSOR.TYPE = 'PanoramicRGBSensor'
task_config.TASK.PANORAMIC_SENSOR.WIDTH = task_config.SIMULATOR.RGB_SENSOR.WIDTH
task_config.TASK.PANORAMIC_SENSOR.HEIGHT = task_config.SIMULATOR.RGB_SENSOR.HEIGHT
task_config.TASK.PANORAMIC_DEPTH_SENSOR = task_config.SIMULATOR.DEPTH_SENSOR.clone(
)
task_config.TASK.PANORAMIC_DEPTH_SENSOR.TYPE = 'PanoramicDepthSensor'
task_config.TASK.PANORAMIC_DEPTH_SENSOR.WIDTH = task_config.SIMULATOR.DEPTH_SENSOR.WIDTH
task_config.TASK.PANORAMIC_DEPTH_SENSOR.HEIGHT = task_config.SIMULATOR.DEPTH_SENSOR.HEIGHT
if "STOP" not in task_config.TASK.POSSIBLE_ACTIONS:
task_config.TASK.SUCCESS.TYPE = "Success_woSTOP"
task_config.TASK.SUCCESS.SUCCESS_DISTANCE = task_config.TASK.SUCCESS_DISTANCE
return task_config
def main():
config = habitat.get_config(config_paths="/home/rene/catkin_ws/src/habitat-api/configs/tasks/pointnav_rgbd.yaml")
config.defrost()
# Add things to the config to for the measure
config.TASK.EPISODE_INFO = habitat.Config()
# The type field is used to look-up the measure in the registry.
# By default, the things are registered with the class name
config.TASK.EPISODE_INFO.TYPE = "EpisodeInfo"
config.TASK.EPISODE_INFO.VALUE = 5
# Now define the config for the sensor
config.TASK.AGENT_POSITION_SENSOR = habitat.Config()
# Use the custom name
config.TASK.AGENT_POSITION_SENSOR.TYPE = "my_supercool_sensor"
# Add the sensor to the list of sensors in use
config.TASK.SENSORS.append("AGENT_POSITION_SENSOR")
# Now define the config for the sensor
config.TASK.SEMANTIC_SEGMENTATION_MASK_SENSOR = habitat.Config()
# Use the custom name
config.TASK.SEMANTIC_SEGMENTATION_MASK_SENSOR.TYPE = "semantic_segmentation_mask"
# Add the sensor to the list of sensors in use
config.TASK.SENSORS.append("SEMANTIC_SEGMENTATION_MASK_SENSOR")
config.freeze()
my_env = sim_env(config)
# start the thread that publishes sensor readings
my_env.start()
rospy.Subscriber("/cmd_vel", Twist, callback, (my_env), queue_size=1)
# define a list capturing how long it took
# to update agent orientation for past 3 instances
# TODO modify dt_list to depend on r1
dt_list = [0.009, 0.009, 0.009]
while not rospy.is_shutdown():
start_time = time.time()
# cv2.imshow("bc_sensor", my_env.observations['bc_sensor'])
# cv2.waitKey(100)
# time.sleep(0.1)
my_env.update_orientation()
dt_list.insert(0, time.time() - start_time)
dt_list.pop()
my_env.set_dt(sum(dt_list) / len(dt_list))
def config_custom(path="configs/tasks/pointnav_my.yaml"):
config = habitat.get_config(path)
config.defrost()
# Add things to the config to for the measure
config.TASK.EPISODE_INFO_EXAMPLE = habitat.Config()
# The type field is used to look-up the measure in the registry.
# By default, the things are registered with the class name
config.TASK.EPISODE_INFO_EXAMPLE.TYPE = "EpisodeInfoExample"
config.TASK.EPISODE_INFO_EXAMPLE.VALUE = 5
# Add the measure to the list of measures in use
config.TASK.MEASUREMENTS.append("EPISODE_INFO_EXAMPLE")
# Now define the config for the sensor
config.TASK.AGENT_POSITION_SENSOR = habitat.Config()
# Use the custom name
config.TASK.AGENT_POSITION_SENSOR.TYPE = "my_supercool_sensor"
config.TASK.AGENT_POSITION_SENSOR.ANSWER_TO_LIFE = 42
# Add the sensor to the list of sensors in use
config.TASK.SENSORS.append("AGENT_POSITION_SENSOR")
config.freeze()
return config
def run_exp(exp_config: str,
run_type: str,
ckpt_path="",
run_id=None,
run_suffix="",
opts=None) -> None:
r"""Runs experiment given mode and config
Args:
exp_config: path to config file.
run_type: "train" or "eval.
ckpt_path: If training, ckpt to resume. If evaluating, ckpt to evaluate.
run_id: If using slurm batch, run id to prefix.s
opts: list of strings of additional config options.
Returns:
None.
"""
config = get_config(exp_config, opts)
# Add tracking of the number of episodes
config.defrost()
config.TASK_CONFIG.TASK.EPISODE_INFO_EXAMPLE = habitat.Config()
# The type field is used to look-up the measure in the registry.
# By default, the things are registered with the class name
config.TASK_CONFIG.TASK.EPISODE_INFO_EXAMPLE.TYPE = "EpisodeInfoExample"
config.TASK_CONFIG.TASK.EPISODE_INFO_EXAMPLE.VALUE = 5
# Add the measure to the list of measures in use
config.TASK_CONFIG.TASK.MEASUREMENTS.append("EPISODE_INFO_EXAMPLE")
config.freeze()
variant_name = os.path.split(exp_config)[1].split('.')[0]
config.defrost()
if run_suffix != "" and run_suffix is not None:
variant_name = f"{variant_name}-{run_suffix}"
if not osp.exists(config.LOG_FILE):
os.makedirs(config.LOG_FILE)
config.TENSORBOARD_DIR = os.path.join(config.TENSORBOARD_DIR, variant_name)
config.CHECKPOINT_FOLDER = os.path.join(config.CHECKPOINT_FOLDER,
variant_name)
config.VIDEO_DIR = os.path.join(config.VIDEO_DIR, variant_name)
config.LOG_FILE = os.path.join(config.LOG_FILE,
f"{variant_name}.log") # actually a logdir
if run_type == "eval":
# config.TRAINER_NAME = "ppo"
config.NUM_PROCESSES = 6 # nice
else:
# Add necessary supervisory signals
train_sensors = config.RL.AUX_TASKS.required_sensors
config.SENSORS.extend(
train_sensors) # the task cfg sensors are overwritten by this one
if run_id is None:
random.seed(config.TASK_CONFIG.SEED)
np.random.seed(config.TASK_CONFIG.SEED)
trainer_init = baseline_registry.get_trainer(config.TRAINER_NAME)
assert trainer_init is not None, f"{config.TRAINER_NAME} is not supported"
trainer = trainer_init(config)
# If not doing multiple runs (with run_id), default behavior is to overwrite
if run_type == "train":
if ckpt_path is not None:
ckpt_dir, ckpt_file = os.path.split(ckpt_path)
ckpt_index = ckpt_file.split('.')[1]
ckpt = int(ckpt_index)
start_updates = ckpt * config.CHECKPOINT_INTERVAL + 1
trainer.train(ckpt_path=ckpt_path,
ckpt=ckpt,
start_updates=start_updates)
elif not DO_PRESERVE_RUNS:
# if os.path.exists(config.TENSORBOARD_DIR):
# print("Removing tensorboard directory...")
# shutil.rmtree(config.TENSORBOARD_DIR, ignore_errors=True)
# if os.path.exists(config.CHECKPOINT_FOLDER):
# print("Removing checkpoint folder...")
# shutil.rmtree(config.CHECKPOINT_FOLDER, ignore_errors=True)
# if os.path.exists(config.LOG_FILE):
# print("Removing log file...")
# shutil.rmtree(config.LOG_FILE, ignore_errors=True)
trainer.train()
else:
# if os.path.exists(config.TENSORBOARD_DIR) or os.path.exists(config.CHECKPOINT_FOLDER) \
# or os.path.exists(config.LOG_FILE):
# print(f"TB dir exists: {os.path.exists(config.TENSORBOARD_DIR)}")
# print(f"Ckpt dir exists: {os.path.exists(config.CHECKPOINT_FOLDER)}")
# print(f"Log file exists: {os.path.exists(config.LOG_FILE)}")
# print("Run artifact exists, please clear manually")
# exit(1)
trainer.train()
elif run_type == "eval":
trainer.eval(ckpt_path)
return
run_prefix = f'run_{run_id}'
seed = run_id
random.seed(seed)
np.random.seed(seed)
torch.manual_seed(config.TASK_CONFIG.SEED)
# Exetnds off old modifications
tb_dir = os.path.join(config.TENSORBOARD_DIR, run_prefix)
ckpt_dir = os.path.join(config.CHECKPOINT_FOLDER, run_prefix)
log_dir, log_file = os.path.split(config.LOG_FILE)
log_file_extended = f"{run_prefix}--{log_file}"
log_file_path = os.path.join(log_dir, log_file_extended)
config.TASK_CONFIG.SEED = seed
config.TENSORBOARD_DIR = tb_dir
config.CHECKPOINT_FOLDER = ckpt_dir
config.LOG_FILE = log_file_path
trainer_init = baseline_registry.get_trainer(config.TRAINER_NAME)
assert trainer_init is not None, f"{config.TRAINER_NAME} is not supported"
trainer = trainer_init(config)
if run_type == "train":
if ckpt_path is None:
# if DO_PRESERVE_RUNS and (os.path.exists(tb_dir) or os.path.exists(ckpt_dir) or os.path.exists(log_file_extended)):
# print(f"TB dir exists: {os.path.exists(tb_dir)}")
# print(f"Ckpt dir exists: {os.path.exists(ckpt_dir)}")
# print(f"Log file exists: {os.path.exists(log_file_extended)}")
# print("Run artifact exists, please clear manually")
# exit(1)
# else:
# shutil.rmtree(tb_dir, ignore_errors=True)
# shutil.rmtree(ckpt_dir, ignore_errors=True)
# if os.path.exists(log_file_extended):
# os.remove(log_file_extended)
trainer.train()
else: # Resume training from checkpoint
# Parse the checkpoint #, calculate num updates, update the config
ckpt_dir, ckpt_file = os.path.split(ckpt_path)
ckpt_index = ckpt_file.split('.')[1]
true_path = os.path.join(ckpt_dir, run_prefix,
f"{run_prefix}.{ckpt_index}.pth")
ckpt = int(ckpt_index)
start_updates = ckpt * config.CHECKPOINT_INTERVAL + 1
trainer.train(ckpt_path=true_path,
ckpt=ckpt,
start_updates=start_updates)
else:
ckpt_dir, ckpt_file = os.path.split(ckpt_path)
ckpt_index = ckpt_file.split('.')[1]
true_path = os.path.join(ckpt_dir, run_prefix,
f"{run_prefix}.{ckpt_index}.pth")
trainer.eval(true_path)
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."
)
# This is called whenver reset is called or an action is taken
def get_observation(self, observations, *args, episode, **kwargs):
return self._sim.get_agent_state().position
# %%
if __name__ == "__main__":
config = habitat.get_config(
config_paths="./configs/test/habitat_all_sensors_test.yaml"
)
config.defrost()
# Now define the config for the sensor
config.TASK.AGENT_POSITION_SENSOR = habitat.Config()
# Use the custom name
config.TASK.AGENT_POSITION_SENSOR.TYPE = "agent_position_sensor"
# Add the sensor to the list of sensors in use
config.TASK.SENSORS.append("AGENT_POSITION_SENSOR")
config.freeze()
try:
env.close()
except NameError:
pass
env = habitat.Env(config=config)
# %%
obs = env.reset()
def example():
config = habitat.get_config("configs/tasks/pointnav_my.yaml")
config.defrost()
# Add things to the config to for the measure
config.TASK.EPISODE_INFO_EXAMPLE = habitat.Config()
# The type field is used to look-up the measure in the registry.
# By default, the things are registered with the class name
config.TASK.EPISODE_INFO_EXAMPLE.TYPE = "EpisodeInfoExample"
config.TASK.EPISODE_INFO_EXAMPLE.VALUE = 5
# Add the measure to the list of measures in use
config.TASK.MEASUREMENTS.append("EPISODE_INFO_EXAMPLE")
# Now define the config for the sensor
config.TASK.AGENT_POSITION_SENSOR = habitat.Config()
# Use the custom name
config.TASK.AGENT_POSITION_SENSOR.TYPE = "my_supercool_sensor"
config.TASK.AGENT_POSITION_SENSOR.ANSWER_TO_LIFE = 42
# Add the sensor to the list of sensors in use
config.TASK.SENSORS.append("AGENT_POSITION_SENSOR")
config.freeze()
if len(sys.argv) > 1:
max_depth = int(sys.argv[1])
else:
max_depth = 3
env = habitat.Env(config=config)
with torch.no_grad():
agent = MBTSAgent(max_depth=max_depth)
print("Environment creation successful")
observations = env.reset()
cv2.imshow("",
observations["depth"]) #transform_rgb_bgr(observations["rgb"]))
print("Agent stepping around inside environment.")
print("==========================================")
print("Search tree depth : {}".format(max_depth))
# print(env.observation_space)
count_steps = 0
view_flag = False
action = None
while not env.episode_over:
keystroke = cv2.waitKey(0)
if keystroke == ord(RESET):
observations = env.reset()
count_steps = 0
view_flag = False
action = None
cv2.imshow("", observations["depth"])
if keystroke == ord(FORWARD_KEY):
action = 1 #habitat.SimulatorActions.MOVE_FORWARD
print("action: FORWARD")
view_flag = False
elif keystroke == ord(LEFT_KEY):
action = 2 #habitat.SimulatorActions.TURN_LEFT
print("action: LEFT")
view_flag = False
elif keystroke == ord(RIGHT_KEY):
action = 3 #habitat.SimulatorActions.TURN_RIGHT
print("action: RIGHT")
view_flag = False
elif keystroke == ord(FINISH):
action = 0 #habitat.SimulatorActions.STOP
print("action: FINISH")
view_flag = False
elif keystroke == ord(AGENT):
state_depth = observations['depth'].swapaxes(0, 2).swapaxes(1, 2)
# action = agent.greedy_policy(state_depth)
action, q = agent.predict(state_depth)
view_flag = False
print("Agent action: {}".format(ACTION_DICT_REVERSE[action]))
# print(q)
elif keystroke == ord(VIEW_FORWARD_KEY):
agent.show_future(0)
continue
elif keystroke == ord(VIEW_LEFT_KEY):
agent.show_future(1)
continue
elif keystroke == ord(VIEW_RIGHT_KEY):
agent.show_future(2)
continue
elif keystroke == ord(VIEW_FINISH):
continue
elif keystroke == ord(VIEW_AGENT):
agent.show_future()
continue
else:
print("INVALID KEY")
continue
observations = env.step(action)
count_steps += 1
print("x: {}, y: {}".format(observations["agent_position"][0],
observations["agent_position"][2]))
cv2.imshow(
"",
observations["depth"]) #transform_rgb_bgr(observations["rgb"]))
print("Episode finished after {} steps.".format(count_steps))
if action == 0:
pos_x, pos_y = observations['agent_position'][0], observations[
'agent_position'][2]
goal_x, goal_y = 2, 12
dist = math.sqrt((pos_x - goal_x)**2 + (pos_y - goal_y)**2)
if dist < 2:
print("you successfully navigated to destination point")
else:
print(dist)
print("your navigation was unsuccessful")
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
from envs.habitat_objectnav_env import ObjectNavENV
from habitat.config.default import get_config as get_habitat_config
from configs.default_cfg import get_config
import habitat
from habitat import make_dataset
import numpy as np
import envs.habitat_utils
import time
import psutil
pid = os.getpid()
current_process = psutil.Process(pid)
habitat_cfg = get_habitat_config('configs/objectnav_mp3d.yaml')
habitat_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.freeze()
cfg = get_config()
cfg.defrost()
cfg.task.habitat.TASK_CONFIG = habitat_cfg
cfg.freeze()
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',
def main():
parser = argparse.ArgumentParser()
parser.add_argument("--task-config", type=str, default="configs/tasks/pointnav.yaml")
parser.add_argument("--publish-odom", type=bool, default=True)
parser.add_argument("--create-map", type=bool, default=False)
parser.add_argument("--save-observations", type=bool, default=False)
parser.add_argument("--preset-trajectory", type=bool, default=False)
args = parser.parse_args()
# Now define the config for the sensor
config = habitat.get_config(args.task_config)
config.defrost()
config.TASK.AGENT_POSITION_SENSOR = habitat.Config()
# Use the custom name
config.TASK.AGENT_POSITION_SENSOR.TYPE = "position_sensor"
config.TASK.AGENT_POSITION_SENSOR.ANSWER_TO_LIFE = 42
# Add the sensor to the list of sensors in use
config.TASK.SENSORS.append("AGENT_POSITION_SENSOR")
config.freeze()
max_depth = config.SIMULATOR.DEPTH_SENSOR.MAX_DEPTH
print(args.create_map)
agent = KeyboardAgent(args.save_observations)
env = habitat.Env(config=config)
if args.create_map:
print('create map')
top_down_map = maps.get_topdown_map(env.sim, map_resolution=(5000, 5000))
print(top_down_map.min(), top_down_map.mean(), top_down_map.max())
recolor_map = np.array([[0, 0, 0], [128, 128, 128], [255, 255, 255]], dtype=np.uint8)
range_x = np.where(np.any(top_down_map, axis=1))[0]
range_y = np.where(np.any(top_down_map, axis=0))[0]
padding = int(np.ceil(top_down_map.shape[0] / 125))
range_x = (
max(range_x[0] - padding, 0),
min(range_x[-1] + padding + 1, top_down_map.shape[0]),
)
range_y = (
max(range_y[0] - padding, 0),
min(range_y[-1] + padding + 1, top_down_map.shape[1]),
)
top_down_map = top_down_map[
range_x[0] : range_x[1], range_y[0] : range_y[1]
]
top_down_map = recolor_map[top_down_map]
imsave('top_down_map.png', top_down_map)
observations = env.reset()
if not args.preset_trajectory:
while not keyboard.is_pressed('q'):
action = agent.act(observations)
observations = env.step(action)
else:
fin = open('actions.txt', 'r')
actions = [int(x) for x in fin.readlines()]
for action in actions:
agent.act(observations)
observations = env.step(action)
fin.close()
if args.save_observations:
with h5py.File('observations.hdf5', 'w') as f:
f.create_dataset("points", data=np.array(agent.points))
f.create_dataset("true_positions", data=np.array(agent.true_positions))
f.create_dataset("true_rotations", data=np.array(agent.true_rotations))
f.create_dataset("predicted_positions", data=np.array(agent.predicted_positions))
f.create_dataset("predicted_rotations", data=np.array(agent.predicted_rotations))
f.create_dataset("rgb", data=np.array(agent.rgbs))
f.create_dataset("depth", data=np.array(agent.depths))
f.create_dataset("true_timestamps", data=np.array(agent.true_timestamps))
f.create_dataset("predicted_timestamps", data=np.array(agent.predicted_timestamps))
print(np.array(agent.points).shape, np.array(agent.rgbs).shape)
fout = open('actions.txt', 'w')
for action in agent.actions:
print(action, file=fout)
fout.close()
def example():
config = habitat.get_config("configs/tasks/pointnav_my.yaml")
config.defrost()
# Add things to the config to for the measure
config.TASK.EPISODE_INFO_EXAMPLE = habitat.Config()
# The type field is used to look-up the measure in the registry.
# By default, the things are registered with the class name
config.TASK.EPISODE_INFO_EXAMPLE.TYPE = "EpisodeInfoExample"
config.TASK.EPISODE_INFO_EXAMPLE.VALUE = 5
# Add the measure to the list of measures in use
config.TASK.MEASUREMENTS.append("EPISODE_INFO_EXAMPLE")
# Now define the config for the sensor
config.TASK.AGENT_POSITION_SENSOR = habitat.Config()
# Use the custom name
config.TASK.AGENT_POSITION_SENSOR.TYPE = "my_supercool_sensor"
config.TASK.AGENT_POSITION_SENSOR.ANSWER_TO_LIFE = 42
# Add the sensor to the list of sensors in use
config.TASK.SENSORS.append("AGENT_POSITION_SENSOR")
config.freeze()
env = habitat.Env(config=config)
agent = DQN_agent(env, hyperparams)
agent.load_model_best()
# agent.load_model()
print("Environment creation successful")
observations = env.reset()
# print(observations)
print("Destination, distance: {:3f}, theta(radians): {:.2f}".format(
observations["pointgoal_with_gps_compass"][0],
observations["pointgoal_with_gps_compass"][1]))
cv2.imshow("",
observations["depth"]) #transform_rgb_bgr(observations["rgb"]))
print("Agent stepping around inside environment.")
print(env.observation_space)
count_steps = 0
while not env.episode_over:
keystroke = cv2.waitKey(0)
if keystroke == ord(FORWARD_KEY):
action = 1 #habitat.SimulatorActions.MOVE_FORWARD
print("action: FORWARD")
elif keystroke == ord(LEFT_KEY):
action = 2 #habitat.SimulatorActions.TURN_LEFT
print("action: LEFT")
elif keystroke == ord(RIGHT_KEY):
action = 3 #habitat.SimulatorActions.TURN_RIGHT
print("action: RIGHT")
elif keystroke == ord(FINISH):
action = 0 #habitat.SimulatorActions.STOP
print("action: FINISH")
elif keystroke == ord(AGENT):
state_depth = observations['depth'].swapaxes(0, 2).swapaxes(1, 2)
# action = agent.greedy_policy(state_depth)
action, q = agent.eval_model.predict(state_depth)
print(q)
else:
print("INVALID KEY")
continue
observations = env.step(action)
reward, done = agent.reward_func(observations, action)
print(reward, done)
count_steps += 1
# env = habitat.Env(
# config=config
# )
# observations = env.reset()
# x:1.9, y: 0.17
# print("Destination, distance: {:3f}, theta(radians): {:.2f}".format(
# observations["pointgoal_with_gps_compass"][0], observations["pointgoal_with_gps_compass"][1]))
print("x: {}, y: {}".format(observations["agent_position"][0],
observations["agent_position"][2]))
cv2.imshow(
"",
observations["depth"]) #transform_rgb_bgr(observations["rgb"]))
print("Episode finished after {} steps.".format(count_steps))
if action == 0 and observations["pointgoal_with_gps_compass"][0] < 0.2:
print("you successfully navigated to destination point")
else:
print("your navigation was unsuccessful")
def example():
config = habitat.get_config("configs/tasks/pointnav_my.yaml")
config.defrost()
# Add things to the config to for the measure
config.TASK.EPISODE_INFO_EXAMPLE = habitat.Config()
# The type field is used to look-up the measure in the registry.
# By default, the things are registered with the class name
config.TASK.EPISODE_INFO_EXAMPLE.TYPE = "EpisodeInfoExample"
config.TASK.EPISODE_INFO_EXAMPLE.VALUE = 5
# Add the measure to the list of measures in use
config.TASK.MEASUREMENTS.append("EPISODE_INFO_EXAMPLE")
# Now define the config for the sensor
config.TASK.AGENT_POSITION_SENSOR = habitat.Config()
# Use the custom name
config.TASK.AGENT_POSITION_SENSOR.TYPE = "my_supercool_sensor"
config.TASK.AGENT_POSITION_SENSOR.ANSWER_TO_LIFE = 42
# Add the sensor to the list of sensors in use
config.TASK.SENSORS.append("AGENT_POSITION_SENSOR")
config.freeze()
env = habitat.Env(config=config)
with torch.no_grad():
agent = AE_DQN_agent(env, hyperparams)
# agent.load_model_best_ae()
# agent.load_model_best_dqn()
agent.load_model_best_ae()
agent.load_model_best_dqn()
agent.eval_mode()
# agent.load_model()
print("Environment creation successful")
observations = env.reset()
# print(observations)
print("Destination, distance: {:3f}, theta(radians): {:.2f}".format(
observations["pointgoal_with_gps_compass"][0],
observations["pointgoal_with_gps_compass"][1]))
cv2.imshow("",
observations["depth"]) #transform_rgb_bgr(observations["rgb"]))
print("Agent stepping around inside environment.")
print(env.observation_space)
count_steps = 0
view_flag = False
while not env.episode_over:
keystroke = cv2.waitKey(0)
if keystroke == ord(FORWARD_KEY):
action = 1 #habitat.SimulatorActions.MOVE_FORWARD
print("action: FORWARD")
view_flag = False
elif keystroke == ord(LEFT_KEY):
action = 2 #habitat.SimulatorActions.TURN_LEFT
print("action: LEFT")
view_flag = False
elif keystroke == ord(RIGHT_KEY):
action = 3 #habitat.SimulatorActions.TURN_RIGHT
print("action: RIGHT")
view_flag = False
elif keystroke == ord(FINISH):
action = 0 #habitat.SimulatorActions.STOP
print("action: FINISH")
view_flag = False
elif keystroke == ord(AGENT):
state_depth = observations['depth'].swapaxes(0, 2).swapaxes(1, 2)
# action = agent.greedy_policy(state_depth)
action, q = agent.eval_model.predict(state_depth)
view_flag = False
print(q)
elif keystroke == ord(VIEW_FORWARD_KEY):
action = np.array([0, 1, 0, 0])
if view_flag:
state_depth = view_obs.swapaxes(0, 2).swapaxes(1, 2)
else:
state_depth = observations['depth'].swapaxes(0,
2).swapaxes(1, 2)
view_flag = True
elif keystroke == ord(VIEW_LEFT_KEY):
action = np.array([0, 0, 1, 0])
if view_flag:
state_depth = view_obs.swapaxes(0, 2).swapaxes(1, 2)
else:
state_depth = observations['depth'].swapaxes(0,
2).swapaxes(1, 2)
view_flag = True
elif keystroke == ord(VIEW_RIGHT_KEY):
action = np.array([0, 0, 0, 1])
if view_flag:
state_depth = view_obs.swapaxes(0, 2).swapaxes(1, 2)
else:
state_depth = observations['depth'].swapaxes(0,
2).swapaxes(1, 2)
view_flag = True
elif keystroke == ord(VIEW_FINISH):
action = np.array([1, 0, 0, 0])
if view_flag:
state_depth = view_obs.swapaxes(0, 2).swapaxes(1, 2)
else:
state_depth = observations['depth'].swapaxes(0,
2).swapaxes(1, 2)
view_flag = True
elif keystroke == ord(VIEW_AGENT):
if view_flag:
state_depth = view_obs.swapaxes(0, 2).swapaxes(1, 2)
else:
state_depth = observations['depth'].swapaxes(0,
2).swapaxes(1, 2)
view_flag = True
action, q = agent.eval_model.predict(state_depth)
print(action)
if action == 0:
print("predict finish")
continue
action_vector = np.zeros(4)
action_vector[action] = 1
action = action_vector
else:
print("INVALID KEY")
continue
if view_flag:
with torch.no_grad():
# print(state_depth)
# print(state_depth.shape)
# state_depth = observations['depth'].swapaxes(0, 2).swapaxes(1, 2)
action = FloatTensor(action)
view_obs = agent.ae_model.predict(state_depth, action)[0]
view_obs = view_obs.detach().cpu().numpy().swapaxes(
0, 2).swapaxes(0, 1)
# print(view_obs.shape)
view_obs = np.clip(view_obs, 0, 1)
# print(view_obs)
cv2.imshow("view", view_obs)
continue
observations = env.step(action)
# reward, done = agent.reward_func(observations, action)
# print(reward, done)
count_steps += 1
# env = habitat.Env(
# config=config
# )
# observations = env.reset()
# x:1.9, y: 0.17
# print("Destination, distance: {:3f}, theta(radians): {:.2f}".format(
# observations["pointgoal_with_gps_compass"][0], observations["pointgoal_with_gps_compass"][1]))
print("x: {}, y: {}".format(observations["agent_position"][0],
observations["agent_position"][2]))
cv2.imshow(
"",
observations["depth"]) #transform_rgb_bgr(observations["rgb"]))
print("Episode finished after {} steps.".format(count_steps))
if action == 0:
pos_x, pos_y = observations['agent_position'][0], observations[
'agent_position'][2]
goal_x, goal_y = 2, 12
dist = math.sqrt((pos_x - goal_x)**2 + (pos_y - goal_y)**2)
if dist < 2:
print("you successfully navigated to destination point")
else:
print(dist)
print("your navigation was unsuccessful")
def construct_envs(config: Config, env_class: Type[Union[Env,
RLEnv]]) -> VectorEnv:
r"""Create VectorEnv object with specified config and env class type.
To allow better performance, dataset are split into small ones for
each individual env, grouped by scenes.
Args:
config: configs that contain num_processes as well as information
necessary to create individual environments.
env_class: class type of the envs to be created.
Returns:
VectorEnv object created according to specification.
"""
num_processes = config.NUM_PROCESSES
configs = []
env_classes = [env_class for _ in range(num_processes)]
dataset = make_dataset(config.TASK_CONFIG.DATASET.TYPE,
**{'filter_fn': filter_fn})
scenes = config.TASK_CONFIG.DATASET.CONTENT_SCENES
if "*" in config.TASK_CONFIG.DATASET.CONTENT_SCENES:
scenes = dataset.get_scenes_to_load(config.TASK_CONFIG.DATASET)
if num_processes > 1:
if len(scenes) == 0:
raise RuntimeError(
"No scenes to load, multiple process logic relies on being able to split scenes uniquely between processes"
)
if len(scenes) < num_processes:
raise RuntimeError("reduce the number of processes as there "
"aren't enough number of scenes")
random.shuffle(scenes)
scene_splits = [[] for _ in range(num_processes)]
for idx, scene in enumerate(scenes):
scene_splits[idx % len(scene_splits)].append(scene)
print('Total Process : %d ' % num_processes)
for i, s in enumerate(scene_splits):
print('proc %d :' % i, s)
assert sum(map(len, scene_splits)) == len(scenes)
for i in range(num_processes):
proc_config = config.clone()
proc_config.defrost()
task_config = proc_config.TASK_CONFIG
if len(scenes) > 0:
task_config.DATASET.CONTENT_SCENES = scene_splits[i]
task_config.TASK.CUSTOM_OBJECT_GOAL_SENSOR = habitat.Config()
task_config.TASK.CUSTOM_OBJECT_GOAL_SENSOR.TYPE = 'CustomObjectSensor'
task_config.TASK.CUSTOM_OBJECT_GOAL_SENSOR.GOAL_SPEC = "OBJECT_IMG"
task_config.SIMULATOR.HABITAT_SIM_V0.GPU_DEVICE_ID = (
config.SIMULATOR_GPU_ID)
task_config.SIMULATOR.AGENT_0.SENSORS = config.SENSORS
proc_config.freeze()
configs.append(proc_config)
envs = habitat.VectorEnv(make_env_fn=make_env_fn,
env_fn_args=tuple(
tuple(
zip(configs, env_classes,
range(num_processes),
[{
'filter_fn': filter_fn
}] * num_processes))))
return envs
