def generate_graph(points, pathfinder):
navigable_idx = [
i for i, p in enumerate(points) if pathfinder.is_navigable(p)
]
graph = nx.Graph()
for idx in navigable_idx:
graph.add_node(idx, point=points[idx])
for a_idx, a_loc in enumerate(points):
if a_idx not in navigable_idx:
continue
for b_idx, b_loc in enumerate(points):
if b_idx not in navigable_idx:
continue
if a_idx == b_idx:
continue
euclidean_distance = np.linalg.norm(
np.array(a_loc) - np.array(b_loc))
if 0.1 < euclidean_distance < 1.01:
path = habitat_sim.ShortestPath()
path.requested_start = np.array(a_loc, dtype=np.float32)
path.requested_end = np.array(b_loc, dtype=np.float32)
pathfinder.find_path(path)
# relax the constraint a bit
if path.geodesic_distance < 1.3:
graph.add_edge(a_idx, b_idx)
return graph
def get_shortest_path(sim, samples):
path_results = []
for sample in samples:
path = habitat_sim.ShortestPath()
path.requested_start = sample[0]
path.requested_end = sample[1]
found_path = sim.pathfinder.find_path(path)
path_results.append((found_path, path.geodesic_distance, path.points))
return path_results
# @markdown With a valid PathFinder instance:
if not sim.pathfinder.is_loaded:
print("Pathfinder not initialized, aborting.")
else:
seed = 4 # @param {type:"integer"}
sim.pathfinder.seed(seed)
# fmt off
# @markdown 1. Sample valid points on the NavMesh for agent spawn location and pathfinding goal.
# fmt on
sample1 = sim.pathfinder.get_random_navigable_point()
sample2 = sim.pathfinder.get_random_navigable_point()
# @markdown 2. Use ShortestPath module to compute path between samples.
path = habitat_sim.ShortestPath()
path.requested_start = sample1
path.requested_end = sample2
found_path = sim.pathfinder.find_path(path)
geodesic_distance = path.geodesic_distance
path_points = path.points
# @markdown - Success, geodesic path length, and 3D points can be queried.
print("found_path : " + str(found_path))
print("geodesic_distance : " + str(geodesic_distance))
print("path_points : " + str(path_points))
# @markdown 3. Display trajectory (if found) on a topdown map of ground floor
if found_path:
meters_per_pixel = 0.025
scene_bb = sim.get_active_scene_graph().get_root_node().cumulative_bb
height = scene_bb.y().min
# create and configure a new VelocityControl structure
# Note: this is NOT the object's VelocityControl, so it will not be consumed automatically in sim.step_physics
vel_control = habitat_sim.physics.VelocityControl()
vel_control.controlling_lin_vel = True
vel_control.lin_vel_is_local = True
vel_control.controlling_ang_vel = True
vel_control.ang_vel_is_local = True
# reset observations and robot state
sim.set_translation(sim.pathfinder.get_random_navigable_point(), locobot_id)
observations = []
# get shortest path to the object from the agent position
found_path = False
path1 = habitat_sim.ShortestPath()
path2 = habitat_sim.ShortestPath()
while not found_path:
if not sample_object_state(
sim, obj_id_1, from_navmesh=True, maintain_object_up=True, max_tries=1000
):
print("Couldn't find an initial object placement. Aborting.")
break
path1.requested_start = sim.get_translation(locobot_id)
path1.requested_end = sim.get_translation(obj_id_1)
path2.requested_start = path1.requested_end
path2.requested_end = sim.pathfinder.get_random_navigable_point()
found_path = sim.pathfinder.find_path(path1) and sim.pathfinder.find_path(path2)
if not found_path:
def get_straight_shortest_path_points(self, position_a, position_b):
path = habitat_sim.ShortestPath()
path.requested_start = position_a
path.requested_end = position_b
self._sim.pathfinder.find_path(path)
return path.points
def geodesic_distance(self, position_a, position_b):
path = habitat_sim.ShortestPath()
path.requested_start = np.array(position_a, dtype=np.float32)
path.requested_end = np.array(position_b, dtype=np.float32)
self._sim.pathfinder.find_path(path)
return path.geodesic_distance
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 = habitat_sim.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 = 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
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()
