def _step_along_grad(
self,
grad_dir: np.quaternion) -> Union[SimulatorActions, np.array]:
current_state = self._sim.get_agent_state()
alpha = angle_between_quaternions(grad_dir, current_state.rotation)
if alpha <= np.deg2rad(self._sim.config.TURN_ANGLE) + EPSILON:
return self._get_return_value(SimulatorActions.FORWARD)
else:
sim_action = SimulatorActions.LEFT.value
self._sim.step(sim_action)
best_turn = (SimulatorActions.LEFT if (angle_between_quaternions(
grad_dir,
self._sim.get_agent_state().rotation) < alpha) else
SimulatorActions.RIGHT)
self._reset_agent_state(current_state)
return self._get_return_value(best_turn)
def get_optimal_action(self, start, pose_estimate):
EPSILON = 1e-6
config_env = super().habitat_env._config
goal_pos = super().habitat_env.current_episode.goals[0].position
goal_radius = config_env.TASK.SUCCESS_DISTANCE
current_state = super().habitat_env.sim.get_agent_state()
position_estimate, rotation_estimate = self.ans_frame_to_hab(
start, pose_estimate)
if (super().habitat_env.sim.geodesic_distance(
position_estimate, goal_pos) <= goal_radius):
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: STOP".format(goal_pos, current_state.position))
return HabitatSimActions.STOP
points = super().habitat_env.sim.get_straight_shortest_path_points(
position_estimate, goal_pos)
# Add a little offset as things get weird if
# points[1] - points[0] is anti-parallel with forward
if len(points) < 2:
max_grad_dir = None
else:
max_grad_dir = quaternion_from_two_vectors(
super().habitat_env.sim.forward_vector,
points[1] - points[0] +
EPSILON * np.cross(super().habitat_env.sim.up_vector,
super().habitat_env.sim.forward_vector),
)
max_grad_dir.x = 0
max_grad_dir = np.normalized(max_grad_dir)
if max_grad_dir is None:
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: MOVE FORWARD".format(goal_pos, current_state.position))
return HabitatSimActions.MOVE_FORWARD
else:
alpha = angle_between_quaternions(max_grad_dir, rotation_estimate)
if alpha <= np.deg2rad(config_env.SIMULATOR.TURN_ANGLE) + EPSILON:
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: MOVE FORWARD".format(goal_pos, current_state.position))
return HabitatSimActions.MOVE_FORWARD
else:
if (angle_between_quaternions(max_grad_dir, rotation_estimate)
< alpha):
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: TURN LEFT".format(goal_pos, current_state.position))
return HabitatSimActions.TURN_LEFT
else:
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: TURN LEFT".format(goal_pos, current_state.position))
return HabitatSimActions.TURN_RIGHT
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: STOP".format(goal_pos, current_state.position))
return HabitatSimActions.STOP
def check_state(agent_state, position, rotation):
assert (angle_between_quaternions(agent_state.rotation,
quaternion_from_coeff(rotation)) <
1e-5), "Agent's rotation diverges from the shortest path."
assert np.allclose(
agent_state.position, position
), "Agent's position position diverges from the shortest path's one."
def _step_along_grad(self, grad_dir: np.quaternion, goal_pos: np.array,
previous_action: int) -> np.array:
current_state = self._sim.get_agent_state()
alpha = angle_between_quaternions(grad_dir, current_state.rotation)
if alpha <= np.deg2rad(self._sim.config.TURN_ANGLE) + EPSILON:
return action_to_one_hot(SimulatorActions.MOVE_FORWARD)
else:
if previous_action == SimulatorActions.TURN_LEFT or previous_action == SimulatorActions.TURN_RIGHT:
# Previous action was a turn, make current suggestion match previous action.
best_turn = previous_action
else:
sim_action = SimulatorActions.TURN_LEFT
self._sim.step(sim_action)
best_turn = (SimulatorActions.TURN_LEFT if
(angle_between_quaternions(
grad_dir,
self._sim.get_agent_state().rotation) < alpha)
else SimulatorActions.TURN_RIGHT)
self._reset_agent_state(current_state)
# Check if forward reduces geodesic distance
curr_dist = self._geo_dist(goal_pos)
self._sim.step(SimulatorActions.MOVE_FORWARD)
new_dist = self._geo_dist(goal_pos)
new_pos = self._sim.get_agent_state().position
movement_size = np.linalg.norm(new_pos - current_state.position)
self._reset_agent_state(current_state)
if new_dist < curr_dist and movement_size / self._step_size > 0.95:
# Make probability proportional to benefit of doing forward action
forward_ind = SimulatorActions.MOVE_FORWARD
one_hot = np.zeros(len(SimulatorActions), dtype=np.float32)
one_hot[forward_ind] = (curr_dist - new_dist) / self._step_size
if one_hot[forward_ind] > 0.8:
one_hot[forward_ind] = 1
elif one_hot[forward_ind] < 0.2:
one_hot[forward_ind] = 0
one_hot[best_turn] = 1 - one_hot[forward_ind]
return one_hot
else:
return action_to_one_hot(best_turn)
def _step_along_grad(
self, grad_dir: np.quaternion, position_estimate: np.array, orientation_estimate: np.array
) -> Union[int, np.array]:
# current_state = self._sim.get_agent_state()
# alpha = angle_between_quaternions(grad_dir, current_state.rotation)
alpha = angle_between_quaternions(grad_dir, orientation_estimate)
if alpha <= np.deg2rad(self._sim.config.TURN_ANGLE) + EPSILON:
return self._get_return_value(HabitatSimActions.MOVE_FORWARD)
else:
sim_action = HabitatSimActions.TURN_LEFT
self._sim.step(sim_action)
best_turn = (
HabitatSimActions.TURN_LEFT
if (
angle_between_quaternions(
grad_dir, orientation_estimate
)
< alpha
)
else HabitatSimActions.TURN_RIGHT
)
# self._reset_agent_state(current_state)
return self._get_return_value(best_turn)
def test_noise_models_rgbd():
DEMO_MODE = False
N_STEPS = 100
config = get_config()
config.defrost()
config.SIMULATOR.SCENE = (
"data/scene_datasets/habitat-test-scenes/skokloster-castle.glb")
config.SIMULATOR.AGENT_0.SENSORS = ["RGB_SENSOR", "DEPTH_SENSOR"]
config.freeze()
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
valid_start_position = [-1.3731, 0.08431, 8.60692]
expected_pointgoal = [0.1, 0.2, 0.3]
goal_position = np.add(valid_start_position, expected_pointgoal)
# starting quaternion is rotated 180 degree along z-axis, which
# corresponds to simulator using z-negative as forward action
start_rotation = [0, 0, 0, 1]
test_episode = NavigationEpisode(
episode_id="0",
scene_id=config.SIMULATOR.SCENE,
start_position=valid_start_position,
start_rotation=start_rotation,
goals=[NavigationGoal(position=goal_position)],
)
print(f"{test_episode}")
with habitat.Env(config=config, dataset=None) as env:
env.episode_iterator = iter([test_episode])
no_noise_obs = [env.reset()]
no_noise_states = [env.sim.get_agent_state()]
actions = [
sample_non_stop_action(env.action_space) for _ in range(N_STEPS)
]
for action in actions:
no_noise_obs.append(env.step(action))
no_noise_states.append(env.sim.get_agent_state())
env.close()
config.defrost()
config.SIMULATOR.RGB_SENSOR.NOISE_MODEL = "GaussianNoiseModel"
config.SIMULATOR.RGB_SENSOR.NOISE_MODEL_KWARGS = habitat.Config()
config.SIMULATOR.RGB_SENSOR.NOISE_MODEL_KWARGS.INTENSITY_CONSTANT = 0.5
config.SIMULATOR.DEPTH_SENSOR.NOISE_MODEL = "RedwoodDepthNoiseModel"
config.SIMULATOR.ACTION_SPACE_CONFIG = "pyrobotnoisy"
config.SIMULATOR.NOISE_MODEL = habitat.Config()
config.SIMULATOR.NOISE_MODEL.ROBOT = "LoCoBot"
config.SIMULATOR.NOISE_MODEL.CONTROLLER = "Proportional"
config.SIMULATOR.NOISE_MODEL.NOISE_MULTIPLIER = 0.5
config.freeze()
env = habitat.Env(config=config, dataset=None)
env.episode_iterator = iter([test_episode])
obs = env.reset()
assert np.linalg.norm(
obs["rgb"].astype(np.float) -
no_noise_obs[0]["rgb"].astype(np.float)) > 1.5e-2 * np.linalg.norm(
no_noise_obs[0]["rgb"].astype(
np.float)), "No RGB noise detected."
assert np.linalg.norm(obs["depth"].astype(np.float) -
no_noise_obs[0]["depth"].astype(np.float)
) > 1.5e-2 * np.linalg.norm(
no_noise_obs[0]["depth"].astype(
np.float)), "No Depth noise detected."
images = []
state = env.sim.get_agent_state()
angle_diffs = []
pos_diffs = []
for action in actions:
prev_state = state
obs = env.step(action)
state = env.sim.get_agent_state()
position_change = np.linalg.norm(np.array(state.position) -
np.array(prev_state.position),
ord=2)
if action["action"][:5] == "TURN_":
angle_diff = abs(
angle_between_quaternions(state.rotation,
prev_state.rotation) -
np.deg2rad(config.SIMULATOR.TURN_ANGLE))
angle_diffs.append(angle_diff)
else:
pos_diffs.append(
abs(position_change - config.SIMULATOR.FORWARD_STEP_SIZE))
if DEMO_MODE:
images.append(observations_to_image(obs, {}))
if DEMO_MODE:
images_to_video(images, "data/video/test_noise", "test_noise")
assert (np.mean(angle_diffs) >
0.025), "No turn action actuation noise detected."
assert (np.mean(pos_diffs) >
0.025), "No forward action actuation noise detected."
def 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)
with habitat.Env(config=eqa_config, dataset=dataset) as env:
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,
angle_between_quaternions(
cur_state.rotation,
quaternion_from_coeff(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."
if point.action != OLD_STOP_ACTION_ID:
obs = env.step(action=point.action)
if not env.episode_over:
rgb_mean += obs["rgb"][:, :, :3].mean()
if ep_i < len(RGB_EPISODE_MEANS):
# Slightly bigger atol for basis meshes
rgb_mean = rgb_mean / len(episode.shortest_paths[0])
assert np.isclose(
RGB_EPISODE_MEANS[int(episode.episode_id)],
rgb_mean,
atol=0.5,
), "RGB output doesn't match the ground truth."
ep_i = (ep_i + 1) % EPISODES_LIMIT
if ep_i == 0:
cycles_n -= 1
def get_optimal_gt_action(self, debug=False):
EPSILON = 1e-6
config_env = super().habitat_env._config
goal_pos = super().habitat_env.current_episode.goals[0].position
goal_radius = config_env.TASK.SUCCESS_DISTANCE
current_state = super().habitat_env.sim.get_agent_state()
# return HabitatSimActions.STOP
log_bool = self.total_num_steps % self.args.log_interval == 0
self.total_num_steps += 1
# add date
getdate = lambda: str(datetime.now()).split('.')[0]
# set true for debuging
if debug:
log = lambda w, x, y, z: print(
"\n{}\nGoal Position:{}\nCurrent Position:{}\nOptimal Action: {}\nGeodistic distance to goal: {}"
.format(getdate(), w, x, y, z))
else:
log = lambda w, x, y, z: print("\n{}\nGeodistic Dist: {}".format(
getdate(), z))
geoDist = super().habitat_env.sim.geodesic_distance(
current_state.position, goal_pos)
if (geoDist <= goal_radius):
if log_bool: log(goal_pos, current_state.position, 'STOP', geoDist)
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: STOP\nGeodistic Dist: {2}".format(goal_pos, current_state.position, geoDist))
return HabitatSimActions.STOP
points = super().habitat_env.sim.get_straight_shortest_path_points(
current_state.position, goal_pos)
# Add a little offset as things get weird if
# points[1] - points[0] is anti-parallel with forward
if len(points) < 2:
max_grad_dir = None
else:
max_grad_dir = quaternion_from_two_vectors(
super().habitat_env.sim.forward_vector,
points[1] - points[0] +
EPSILON * np.cross(super().habitat_env.sim.up_vector,
super().habitat_env.sim.forward_vector),
)
max_grad_dir.x = 0
max_grad_dir = np.normalized(max_grad_dir)
if max_grad_dir is None:
if log_bool:
log(goal_pos, current_state.position, 'FORWARD', geoDist)
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: MOVE FORWARD".format(goal_pos, current_state.position))
return HabitatSimActions.MOVE_FORWARD
else:
alpha = angle_between_quaternions(max_grad_dir,
current_state.rotation)
if alpha <= np.deg2rad(config_env.SIMULATOR.TURN_ANGLE) + EPSILON:
if log_bool:
log(goal_pos, current_state.position, 'FORWARD', geoDist)
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: MOVE FORWARD\nGeodistic Dist: {2}".format(goal_pos, current_state.position, geoDist))
return HabitatSimActions.MOVE_FORWARD
else:
if (angle_between_quaternions(max_grad_dir,
current_state.rotation) < alpha):
if log_bool:
log(goal_pos, current_state.position, 'LEFT', geoDist)
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: TURN LEFT\nGeodistic Dist: {2}".format(goal_pos, current_state.position, geoDist))
return HabitatSimActions.TURN_LEFT
else:
if log_bool:
log(goal_pos, current_state.position, 'RIGHT', geoDist)
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: TURN LEFT\nGeodistic Dist: {2}".format(goal_pos, current_state.position, geoDist))
return HabitatSimActions.TURN_RIGHT
if log_bool: log(goal_pos, current_state.position, 'STOP', geoDist)
# print("\nGoal Position:{0}\nCurrent Position:{1}\nOptimal Action: STOP\nGeodistic Dist: {2}".format(goal_pos, current_state.position, geoDist))
return HabitatSimActions.STOP
