def get(self):
return {
HabitatSimActions.STOP: habitat_sim.ActionSpec("stop"),
HabitatSimActions.MOVE_FORWARD: habitat_sim.ActionSpec(
"pyrobot_noisy_move_forward",
habitat_sim.PyRobotNoisyActuationSpec(
amount=self.config.FORWARD_STEP_SIZE,
robot=self.config.NOISE_MODEL.ROBOT,
controller=self.config.NOISE_MODEL.CONTROLLER,
noise_multiplier=self.config.NOISE_MODEL.NOISE_MULTIPLIER,
),
),
HabitatSimActions.TURN_LEFT: habitat_sim.ActionSpec(
"pyrobot_noisy_turn_left",
habitat_sim.PyRobotNoisyActuationSpec(
amount=self.config.TURN_ANGLE,
robot=self.config.NOISE_MODEL.ROBOT,
controller=self.config.NOISE_MODEL.CONTROLLER,
noise_multiplier=self.config.NOISE_MODEL.NOISE_MULTIPLIER,
),
),
HabitatSimActions.TURN_RIGHT: habitat_sim.ActionSpec(
"pyrobot_noisy_turn_right",
habitat_sim.PyRobotNoisyActuationSpec(
amount=self.config.TURN_ANGLE,
robot=self.config.NOISE_MODEL.ROBOT,
controller=self.config.NOISE_MODEL.CONTROLLER,
noise_multiplier=self.config.NOISE_MODEL.NOISE_MULTIPLIER,
),
),
HabitatSimActions.LOOK_UP: habitat_sim.ActionSpec(
"look_up",
habitat_sim.ActuationSpec(amount=self.config.TILT_ANGLE),
),
HabitatSimActions.LOOK_DOWN: habitat_sim.ActionSpec(
"look_down",
habitat_sim.ActuationSpec(amount=self.config.TILT_ANGLE),
),
# The perfect actions are needed for the oracle planner
"_forward": habitat_sim.ActionSpec(
"move_forward",
habitat_sim.ActuationSpec(
amount=self.config.FORWARD_STEP_SIZE
),
),
"_left": habitat_sim.ActionSpec(
"turn_left",
habitat_sim.ActuationSpec(amount=self.config.TURN_ANGLE),
),
"_right": habitat_sim.ActionSpec(
"turn_right",
habitat_sim.ActuationSpec(amount=self.config.TURN_ANGLE),
),
}
def get(self):
config = super().get()
new_config = {
SimulatorActions.NOISY_MOVE_FORWARD:
habitat_sim.ActionSpec(
"pyrobot_noisy_move_forward",
habitat_sim.PyRobotNoisyActuationSpec(
amount=self.config.FORWARD_STEP_SIZE,
robot=self.config.ROBOT,
controller=self.config.CONTROLLER,
noise_multiplier=self.config.NOISE_MULTIPLIER,
),
),
SimulatorActions.NOISY_TURN_LEFT:
habitat_sim.ActionSpec(
"pyrobot_noisy_turn_left",
habitat_sim.PyRobotNoisyActuationSpec(
amount=self.config.TURN_ANGLE,
robot=self.config.ROBOT,
controller=self.config.CONTROLLER,
noise_multiplier=self.config.NOISE_MULTIPLIER,
),
),
SimulatorActions.NOISY_TURN_RIGHT:
habitat_sim.ActionSpec(
"pyrobot_noisy_turn_right",
habitat_sim.PyRobotNoisyActuationSpec(
amount=self.config.TURN_ANGLE,
robot=self.config.ROBOT,
controller=self.config.CONTROLLER,
noise_multiplier=self.config.NOISE_MULTIPLIER,
),
),
}
config.update(new_config)
return config
def get(self):
return {
HabitatSimActions.STOP: habitat_sim.ActionSpec("stop"),
HabitatSimActions.MOVE_FORWARD: habitat_sim.ActionSpec(
"pyrobot_noisy_move_forward",
habitat_sim.PyRobotNoisyActuationSpec(
amount=self.config.FORWARD_STEP_SIZE,
# robot=self.config.NOISE_MODEL.ROBOT,
# controller=self.config.NOISE_MODEL.CONTROLLER,
# noise_multiplier=self.config.NOISE_MODEL.NOISE_MULTIPLIER,
),
),
HabitatSimActions.TURN_LEFT: habitat_sim.ActionSpec(
"pyrobot_noisy_turn_left",
habitat_sim.PyRobotNoisyActuationSpec(
amount=self.config.TURN_ANGLE,
# robot=self.config.NOISE_MODEL.ROBOT,
# controller=self.config.NOISE_MODEL.CONTROLLER,
# noise_multiplier=self.config.NOISE_MODEL.NOISE_MULTIPLIER,
),
),
HabitatSimActions.TURN_RIGHT: habitat_sim.ActionSpec(
"pyrobot_noisy_turn_right",
habitat_sim.PyRobotNoisyActuationSpec(
amount=self.config.TURN_ANGLE,
# robot=self.config.NOISE_MODEL.ROBOT,
# controller=self.config.NOISE_MODEL.CONTROLLER,
# noise_multiplier=self.config.NOISE_MODEL.NOISE_MULTIPLIER,
),
),
# HabitatSimActions.LOOK_UP: habitat_sim.ActionSpec(
# "look_up",
# habitat_sim.ActuationSpec(amount=self.config.TILT_ANGLE),
# ),
# HabitatSimActions.LOOK_DOWN: habitat_sim.ActionSpec(
# "look_down",
# habitat_sim.ActuationSpec(amount=self.config.TILT_ANGLE),
# ),
}
def test_pyrobot_noisy_actions(noise_multiplier, robot, controller):
np.random.seed(0)
scene_graph = SceneGraph()
agent_config = habitat_sim.AgentConfiguration()
agent_config.action_space = dict(
noisy_move_backward=habitat_sim.ActionSpec(
"pyrobot_noisy_move_backward",
habitat_sim.PyRobotNoisyActuationSpec(
amount=1.0,
robot=robot,
controller=controller,
noise_multiplier=noise_multiplier,
),
),
noisy_move_forward=habitat_sim.ActionSpec(
"pyrobot_noisy_move_forward",
habitat_sim.PyRobotNoisyActuationSpec(
amount=1.0,
robot=robot,
controller=controller,
noise_multiplier=noise_multiplier,
),
),
noisy_turn_left=habitat_sim.ActionSpec(
"pyrobot_noisy_turn_left",
habitat_sim.PyRobotNoisyActuationSpec(
amount=90.0,
robot=robot,
controller=controller,
noise_multiplier=noise_multiplier,
),
),
noisy_turn_right=habitat_sim.ActionSpec(
"pyrobot_noisy_turn_right",
habitat_sim.PyRobotNoisyActuationSpec(
amount=90.0,
robot=robot,
controller=controller,
noise_multiplier=noise_multiplier,
),
),
move_backward=habitat_sim.ActionSpec(
"move_backward", habitat_sim.ActuationSpec(amount=1.0)
),
move_forward=habitat_sim.ActionSpec(
"move_forward", habitat_sim.ActuationSpec(amount=1.0)
),
turn_left=habitat_sim.ActionSpec(
"turn_left", habitat_sim.ActuationSpec(amount=90.0)
),
turn_right=habitat_sim.ActionSpec(
"turn_right", habitat_sim.ActuationSpec(amount=90.0)
),
)
agent = habitat_sim.Agent(scene_graph.get_root_node().create_child(), agent_config)
for base_action in {act.replace("noisy_", "") for act in agent_config.action_space}:
state = agent.state
state.rotation = np.quaternion(1, 0, 0, 0)
agent.state = state
agent.act(base_action)
base_state = agent.state
delta_translations = []
delta_rotations = []
for _ in range(300):
agent.state = state
agent.act(f"noisy_{base_action}")
noisy_state = agent.state
delta_translations.append(_delta_translation(base_state, noisy_state))
delta_rotations.append(_delta_rotation(base_state, noisy_state))
delta_translations_arr = np.stack(delta_translations)
delta_rotations_arr = np.stack(delta_rotations)
if "move" in base_action:
noise_model = pyrobot_noise_models[robot][controller].linear_motion
else:
noise_model = pyrobot_noise_models[robot][controller].rotational_motion
EPS = 5e-2
assert (
np.linalg.norm(
noise_model.linear.mean * noise_multiplier
- np.abs(delta_translations_arr.mean(0))
)
< EPS
)
assert (
np.linalg.norm(
noise_model.rotation.mean * noise_multiplier
- np.abs(delta_rotations_arr.mean(0))
)
< EPS
)
assert (
np.linalg.norm(
noise_model.linear.cov * noise_multiplier
- np.diag(delta_translations_arr.std(0) ** 2)
)
< EPS
)
assert (
np.linalg.norm(
noise_model.rotation.cov * noise_multiplier
- (delta_rotations_arr.std(0) ** 2)
)
< EPS
)
def test_greedy_follower(test_navmesh, move_filter_fn, action_noise, pbar):
global num_fails
global num_tested
global total_spl
if not osp.exists(test_navmesh):
pytest.skip(f"{test_navmesh} not found")
pathfinder = habitat_sim.PathFinder()
pathfinder.load_nav_mesh(test_navmesh)
assert pathfinder.is_loaded
pathfinder.seed(0)
np.random.seed(seed=0)
scene_graph = habitat_sim.SceneGraph()
agent = habitat_sim.Agent(scene_graph.get_root_node().create_child())
agent.controls.move_filter_fn = getattr(pathfinder, move_filter_fn)
agent.agent_config.action_space["turn_left"].actuation.amount = TURN_DEGREE
agent.agent_config.action_space[
"turn_right"].actuation.amount = TURN_DEGREE
if action_noise:
# "_" prefix the perfect actions so that we can use noisy actions instead
agent.agent_config.action_space = {
"_" + k: v
for k, v in agent.agent_config.action_space.items()
}
agent.agent_config.action_space.update(**dict(
move_forward=habitat_sim.ActionSpec(
"pyrobot_noisy_move_forward",
habitat_sim.PyRobotNoisyActuationSpec(amount=0.25),
),
turn_left=habitat_sim.ActionSpec(
"pyrobot_noisy_turn_left",
habitat_sim.PyRobotNoisyActuationSpec(amount=TURN_DEGREE),
),
turn_right=habitat_sim.ActionSpec(
"pyrobot_noisy_turn_right",
habitat_sim.PyRobotNoisyActuationSpec(amount=TURN_DEGREE),
),
))
follower = habitat_sim.GreedyGeodesicFollower(
pathfinder,
agent,
forward_key="move_forward",
left_key="turn_left",
right_key="turn_right",
)
test_spl = 0.0
for _ in range(NUM_TESTS):
follower.reset()
state = habitat_sim.AgentState()
while True:
state.position = pathfinder.get_random_navigable_point()
goal_pos = pathfinder.get_random_navigable_point()
path = habitat_sim.ShortestPath()
path.requested_start = state.position
path.requested_end = goal_pos
if pathfinder.find_path(path) and path.geodesic_distance > 2.0:
break
agent.state = state
failed = False
gt_geo = path.geodesic_distance
agent_distance = 0.0
last_xyz = state.position
num_acts = 0
# If there is not action noise, then we can use find_path to get all the actions
if not action_noise:
try:
action_list = follower.find_path(goal_pos)
except habitat_sim.errors.GreedyFollowerError:
action_list = [None]
while True:
# If there is action noise, we need to plan a single action, actually take it, and repeat
if action_noise:
try:
next_action = follower.next_action_along(goal_pos)
except habitat_sim.errors.GreedyFollowerError:
break
else:
next_action = action_list[0]
action_list = action_list[1:]
if next_action is None:
break
agent.act(next_action)
agent_distance += np.linalg.norm(last_xyz - agent.state.position)
last_xyz = agent.state.position
num_acts += 1
if num_acts > 1e4:
break
end_state = agent.state
path.requested_start = end_state.position
pathfinder.find_path(path)
failed = path.geodesic_distance > follower.forward_spec.amount
spl = float(not failed) * gt_geo / max(gt_geo, agent_distance)
test_spl += spl
if test_all:
num_fails += float(failed)
num_tested += 1
total_spl += spl
pbar.set_postfix(
num_fails=num_fails,
failure_rate=num_fails / num_tested,
spl=total_spl / num_tested,
)
pbar.update()
if not test_all:
assert test_spl / NUM_TESTS >= ACCEPTABLE_SPLS[(move_filter_fn,
action_noise)]