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()