def get_observation(self, observations, episode, task, *args: Any,
**kwargs: Any):
scan = episode.scan
curr_viewpoint = episode.curr_viewpoint.image_id
near_viewpoints = \
task.get_navigable_locations(scan, curr_viewpoint)
agent_state = self._sim.get_agent_state()
agent_pos = agent_state.position
camera_rot = quaternion_to_list(
agent_state.sensor_states["rgb"].rotation)
camera_pos = agent_state.sensor_states["rgb"].position
agent_rot = quaternion_to_list(agent_state.rotation)
angle = self._sim.config.RGB_SENSOR.HFOV * 2 * np.pi / 360 / 2
navigable_viewpoints = [
self.format_location(
1,
curr_viewpoint,
0,
0,
agent_pos,
agent_rot,
camera_pos,
camera_rot,
)
]
for viewpoint in near_viewpoints:
image_id = viewpoint["image_id"]
target_pos = viewpoint["start_position"]
rel_heading = self.get_rel_heading(agent_pos, agent_rot,
target_pos)
rel_elevation = self.get_rel_elevation(agent_pos, agent_rot,
camera_rot, target_pos)
restricted = 1
if -angle <= rel_heading <= angle:
restricted = 0
navigable_viewpoints.append(
self.format_location(
restricted,
image_id,
rel_heading,
rel_elevation,
target_pos,
agent_rot,
camera_pos,
camera_rot,
))
return self.pad_locations(navigable_viewpoints)
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 default(self, object):
# JSON doesn't support numpy ndarray and quaternion
if isinstance(object, np.ndarray):
return object.tolist()
if isinstance(object, np.quaternion):
return quaternion_to_list(object)
quaternion
return object.__dict__
def default(self, object):
# JSON doesn't support numpy ndarray and quaternion
if isinstance(object, np.ndarray):
return object.tolist()
if isinstance(object, np.quaternion):
return quaternion_to_list(object)
return (object.__getstate__()
if hasattr(object, "__getstate__") else object.__dict__)
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