def test_change_state():
random_state = np.random.get_state()
np.random.seed(233)
scene_graph = habitat_sim.SceneGraph()
agent = habitat_sim.Agent(scene_graph.get_root_node().create_child())
for _ in range(100):
state = agent.state
state.position += np.random.uniform(-1, 1, size=3)
state.rotation *= quat_from_angle_axis(np.random.uniform(0, 2 * np.pi),
np.array([0.0, 1.0, 0.0]))
for v in state.sensor_states.values():
v.position += np.random.uniform(-1, 1, size=3)
v.rotation *= quat_from_angle_axis(np.random.uniform(0, 2 * np.pi),
np.array([1.0, 1.0, 1.0]))
agent.set_state(state, infer_sensor_states=False)
new_state = agent.state
_check_state_same(state, new_state)
for k, v in state.sensor_states.items():
assert k in new_state.sensor_states
_check_state_same(v, new_state.sensor_states[k])
np.random.set_state(random_state)
def test_default_body_contorls(action, expected):
scene_graph = habitat_sim.SceneGraph()
agent_config = habitat_sim.AgentConfiguration()
agent_config.action_space = dict(
move_backward=habitat_sim.ActionSpec(
"move_backward", habitat_sim.ActuationSpec(amount=0.25)),
move_forward=habitat_sim.ActionSpec(
"move_forward", habitat_sim.ActuationSpec(amount=0.25)),
move_left=habitat_sim.ActionSpec(
"move_left", habitat_sim.ActuationSpec(amount=0.25)),
move_right=habitat_sim.ActionSpec(
"move_right", habitat_sim.ActuationSpec(amount=0.25)),
turn_left=habitat_sim.ActionSpec(
"turn_left", habitat_sim.ActuationSpec(amount=10.0)),
turn_right=habitat_sim.ActionSpec(
"turn_right", habitat_sim.ActuationSpec(amount=10.0)),
)
agent = habitat_sim.Agent(scene_graph.get_root_node().create_child(),
agent_config)
state = agent.state
agent.act(action)
new_state = agent.state
_check_state_expected(state, new_state, expected)
for k, v in state.sensor_states.items():
assert k in new_state.sensor_states
_check_state_expected(v, new_state.sensor_states[k], expected)
def test_default_sensor_contorls(action, expected):
scene_graph = habitat_sim.SceneGraph()
agent_config = habitat_sim.AgentConfiguration()
agent_config.action_space = dict(
move_up=habitat_sim.ActionSpec("move_up",
habitat_sim.ActuationSpec(amount=0.25)),
move_down=habitat_sim.ActionSpec(
"move_down", habitat_sim.ActuationSpec(amount=0.25)),
look_left=habitat_sim.ActionSpec(
"look_left", habitat_sim.ActuationSpec(amount=10.0)),
look_right=habitat_sim.ActionSpec(
"look_right", habitat_sim.ActuationSpec(amount=10.0)),
look_up=habitat_sim.ActionSpec("look_up",
habitat_sim.ActuationSpec(amount=10.0)),
look_down=habitat_sim.ActionSpec(
"look_down", habitat_sim.ActuationSpec(amount=10.0)),
)
agent = habitat_sim.Agent(scene_graph.get_root_node().create_child(),
agent_config)
state = agent.state
agent.act(action)
new_state = agent.state
_check_state_same(state, new_state)
for k, v in state.sensor_states.items():
assert k in new_state.sensor_states
_check_state_expected(v, new_state.sensor_states[k], expected)
def test_set_state_error():
scene_graph = habitat_sim.SceneGraph()
agent = habitat_sim.Agent(scene_graph.get_root_node().create_child())
state = agent.state
state.position = np.array([float("NaN")] * 3)
with pytest.raises(ValueError):
agent.set_state(state)
def test_no_move_fun():
scene_graph = habitat_sim.SceneGraph()
agent_config = habitat_sim.AgentConfiguration()
agent_config.action_space = dict(move_forward=habitat_sim.ActionSpec(
"DNF", habitat_sim.ActuationSpec(amount=0.25)))
agent = habitat_sim.Agent(scene_graph.get_root_node().create_child(),
agent_config)
with pytest.raises(AssertionError):
agent.act("move_forward")
def test_set_state():
scene_graph = habitat_sim.SceneGraph()
agent = habitat_sim.Agent(scene_graph.get_root_node().create_child())
state = agent.state
agent.set_state(state, infer_sensor_states=True)
new_state = agent.state
_check_state_same(state, new_state)
for k, v in state.sensor_states.items():
assert k in new_state.sensor_states
_check_state_same(v, new_state.sensor_states[k])
def test_attach_detach():
scene_graph = hsim.SceneGraph()
agent = habitat_sim.Agent()
agent.attach(scene_graph.get_root_node().create_child())
habitat_sim.errors.assert_obj_valid(agent.body)
for _, v in agent.sensors.items():
habitat_sim.errors.assert_obj_valid(v)
with pytest.raises(habitat_sim.errors.InvalidAttachedObject):
habitat_sim.errors.assert_obj_valid(agent.body)
for _, v in agent.sensors.items():
with pytest.raises(habitat_sim.errors.InvalidAttachedObject):
habitat_sim.errors.assert_obj_valid(v)
def test_reconfigure():
scene_graph = habitat_sim.SceneGraph()
agent = habitat_sim.Agent(scene_graph.get_root_node().create_child())
habitat_sim.errors.assert_obj_valid(agent.body)
for _, v in agent._sensors.items():
habitat_sim.errors.assert_obj_valid(v)
agent.reconfigure(agent.agent_config)
for _, v in agent._sensors.items():
habitat_sim.errors.assert_obj_valid(v)
agent.reconfigure(agent.agent_config, True)
for _, v in agent._sensors.items():
habitat_sim.errors.assert_obj_valid(v)
def test_change_state():
scene_graph = hsim.SceneGraph()
agent = habitat_sim.Agent()
agent.attach(scene_graph.get_root_node().create_child())
for _ in range(100):
state = agent.state
state.position += np.random.uniform(-1, 1, size=3)
state.rotation *= habitat_sim.utils.quat_from_angle_axis(
np.random.uniform(0, 2 * np.pi), np.array([0.0, 1.0, 0.0]))
for k, v in state.sensor_states.items():
v.position += np.random.uniform(-1, 1, size=3)
v.rotation *= habitat_sim.utils.quat_from_angle_axis(
np.random.uniform(0, 2 * np.pi), np.array([1.0, 1.0, 1.0]))
agent.state = state
new_state = agent.state
_check_state_same(state, new_state)
for k, v in state.sensor_states.items():
assert k in new_state.sensor_states
_check_state_same(v, new_state.sensor_states[k])
def test_bad_state():
agent = habitat_sim.Agent()
with pytest.raises(habitat_sim.errors.InvalidAttachedObject):
_ = agent.state
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)]
def test_greedy_follower(test_navmesh, scene_graph, pbar):
global num_fails
if not osp.exists(test_navmesh):
pytest.skip(f"{test_navmesh} not found")
pathfinder = hsim.PathFinder()
pathfinder.load_nav_mesh(test_navmesh)
assert pathfinder.is_loaded
agent = habitat_sim.Agent(scene_graph.get_root_node().create_child())
agent.controls.move_filter_fn = pathfinder.try_step
follower = habitat_sim.GreedyGeodesicFollower(pathfinder, agent)
num_tests = 50
for _ in range(num_tests):
state = agent.state
while True:
state.position = pathfinder.get_random_navigable_point()
goal_pos = pathfinder.get_random_navigable_point()
path = hsim.ShortestPath()
path.requested_start = state.position
path.requested_end = goal_pos
if pathfinder.find_path(path) and path.geodesic_distance > 2.0:
break
try:
agent.state = state
path = follower.find_path(goal_pos)
for i, action in enumerate(path):
if action is not None:
agent.act(action)
else:
assert i == len(path) - 1
end_state = agent.state
assert (np.linalg.norm(end_state.position - goal_pos) <=
follower.forward_spec.amount), "Didn't make it"
except Exception as e:
if test_all:
num_fails += 1
pbar.set_postfix(num_fails=num_fails)
else:
raise e
try:
agent.state = state
for _ in range(int(1e4)):
action = follower.next_action_along(goal_pos)
if action is None:
break
agent.act(action)
state = agent.state
assert (np.linalg.norm(state.position - goal_pos) <=
follower.forward_spec.amount), "Didn't make it"
except Exception as e:
if test_all:
num_fails += 1
pbar.set_postfix(num_fails=num_fails)
else:
raise e
if test_all:
pbar.update()
