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