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_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_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_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_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 scene_graph():
return habitat_sim.SceneGraph()
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)]
