def get_action_shortest_path( sim, source_position, source_rotation, goal_position, success_distance=0.05, max_episode_steps=500, ) -> List[ShortestPathPoint]: sim.reset() sim.set_agent_state(source_position, source_rotation) follower = ShortestPathFollower(sim, success_distance, False) shortest_path = [] step_count = 0 action = follower.get_next_action(goal_position) while (action is not HabitatSimActions.STOP and step_count < max_episode_steps): state = sim.get_agent_state() shortest_path.append( ShortestPathPoint( state.position.tolist(), quaternion_to_list(state.rotation), action, )) sim.step(action) step_count += 1 action = follower.get_next_action(goal_position) if step_count == max_episode_steps: logger.warning("Shortest path wasn't found.") return shortest_path
def get_action_shortest_path( sim, source_position, source_rotation, goal_position, success_distance=0.05, max_episode_steps=500, shortest_path_mode="greedy", ) -> List[ShortestPathPoint]: sim.reset() sim.set_agent_state(source_position, source_rotation) follower = ShortestPathFollower(sim, success_distance, False) follower.mode = shortest_path_mode shortest_path = [] action = None step_count = 0 while action != sim.index_stop_action and step_count < max_episode_steps: action = follower.get_next_action(goal_position) state = sim.get_agent_state() shortest_path.append( ShortestPathPoint( state.position.tolist(), quaternion_to_list(state.rotation), action, )) sim.step(action) step_count += 1 if step_count == max_episode_steps: logger.warning("Shortest path wasn't found.") return shortest_path
def get_action_shortest_path( sim, source_position, source_rotation, goal_position, success_distance=0.05, max_episode_steps=500, shortest_path_mode="geodesic_path", max_still_moves=5, ) -> List[ShortestPathPoint]: sim.reset() sim.set_agent_state(source_position, source_rotation) follower = ShortestPathFollower(sim, success_distance, False) follower.mode = shortest_path_mode shortest_path = [] step_count = 0 action = follower.get_next_action(goal_position) state = sim.get_agent_state() prev_pos, prev_rot = state.position, state.rotation no_move = 0 while action is not None and step_count < max_episode_steps: state = sim.get_agent_state() shortest_path.append( ShortestPathPoint( state.position.tolist(), quaternion_to_list(state.rotation), action, )) sim.step(action) step_count += 1 action = follower.get_next_action(goal_position) if (prev_pos == state.position).all() and \ prev_rot == state.rotation: no_move += 1 if no_move > max_still_moves: logger.warning(f"Reached {no_move} steps X no movement.") break else: no_move = 0 prev_pos, prev_rot = state.position, state.rotation if step_count == max_episode_steps: logger.warning("Shortest path wasn't found.") return shortest_path