def test_action_space_shortest_path():
config = get_config()
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
env = habitat.Env(config=config, dataset=None)
# action space shortest path
source_position = env.sim.sample_navigable_point()
angles = [x for x in range(-180, 180, config.SIMULATOR.TURN_ANGLE)]
angle = np.radians(np.random.choice(angles))
source_rotation = [0, np.sin(angle / 2), 0, np.cos(angle / 2)]
source = AgentState(source_position, source_rotation)
reachable_targets = []
unreachable_targets = []
while len(reachable_targets) < 5:
position = env.sim.sample_navigable_point()
angles = [x for x in range(-180, 180, config.SIMULATOR.TURN_ANGLE)]
angle = np.radians(np.random.choice(angles))
rotation = [0, np.sin(angle / 2), 0, np.cos(angle / 2)]
if env.sim.geodesic_distance(source_position, position) != np.inf:
reachable_targets.append(AgentState(position, rotation))
while len(unreachable_targets) < 3:
position = env.sim.sample_navigable_point()
angles = [x for x in range(-180, 180, config.SIMULATOR.TURN_ANGLE)]
angle = np.radians(np.random.choice(angles))
rotation = [0, np.sin(angle / 2), 0, np.cos(angle / 2)]
if env.sim.geodesic_distance(source_position, position) == np.inf:
unreachable_targets.append(AgentState(position, rotation))
targets = reachable_targets
shortest_path1 = env.sim.action_space_shortest_path(source, targets)
assert shortest_path1 != []
targets = unreachable_targets
shortest_path2 = env.sim.action_space_shortest_path(source, targets)
assert shortest_path2 == []
targets = reachable_targets + unreachable_targets
shortest_path3 = env.sim.action_space_shortest_path(source, targets)
# shortest_path1 should be identical to shortest_path3
assert len(shortest_path1) == len(shortest_path3)
for i in range(len(shortest_path1)):
assert np.allclose(shortest_path1[i].position,
shortest_path3[i].position)
assert np.allclose(shortest_path1[i].rotation,
shortest_path3[i].rotation)
assert shortest_path1[i].action == shortest_path3[i].action
targets = unreachable_targets + [source]
shortest_path4 = env.sim.action_space_shortest_path(source, targets)
assert len(shortest_path4) == 1
assert np.allclose(shortest_path4[0].position, source.position)
assert np.allclose(shortest_path4[0].rotation, source.rotation)
assert shortest_path4[0].action is None
env.close()
def from_json(self,
json_str: str,
scenes_dir: Optional[str] = None) -> None:
deserialized = json.loads(json_str)
default_rotation = [0, 0, 0, 1]
self.train_vocab = VocabDict(
word_list=deserialized["train_vocab"]["word_list"])
self.trainval_vocab = VocabDict(
word_list=deserialized["trainval_vocab"]["word_list"])
self.action_tokens = deserialized["BERT_vocab"]["action_tokens"]
self.mini_alignments = deserialized["mini_alignments"]
self.scenes = deserialized["scenes"]
self.connectivity = load_connectivity(self.config.CONNECTIVITY_PATH,
self.scenes)
for ep_index, r2r_episode in enumerate(deserialized["episodes"]):
r2r_episode["curr_viewpoint"] = ViewpointData(
image_id=r2r_episode["goals"][0],
view_point=AgentState(position=r2r_episode["start_position"],
rotation=r2r_episode["start_rotation"]))
instruction_encoding = r2r_episode["instruction_encoding"]
mask = r2r_episode["mask"]
del r2r_episode["instruction_encoding"]
del r2r_episode["mask"]
episode = VLNEpisode(**r2r_episode)
if scenes_dir is not None:
if episode.scene_id.startswith(DEFAULT_SCENE_PATH_PREFIX):
episode.scene_id = episode.scene_id[
len(DEFAULT_SCENE_PATH_PREFIX):]
episode.scene_id = os.path.join(scenes_dir, episode.scene_id)
episode.instruction = InstructionData(
instruction=r2r_episode["instruction"],
tokens=instruction_encoding,
tokens_length=sum(mask),
mask=mask)
scan = episode.scan
for v_index, viewpoint in enumerate(episode.goals):
viewpoint_id = self.connectivity[scan]["idxtoid"][viewpoint]
pos = self.connectivity[scan]["viewpoints"][viewpoint_id]
rot = default_rotation
episode.goals[v_index] = ViewpointData(image_id=viewpoint,
view_point=AgentState(
position=pos,
rotation=rot))
episode.distance = self.get_distance_to_target(
scan, episode.goals[0].image_id, episode.goals[-1].image_id)
self.episodes.append(episode)
def from_json(self,
json_str: str,
scenes_dir: Optional[str] = None) -> None:
deserialized = json.loads(json_str)
self.__dict__.update(deserialized)
self.answer_vocab = VocabDict(word_list=self.answer_vocab["word_list"])
self.question_vocab = VocabDict(
word_list=self.question_vocab["word_list"])
for ep_index, episode in enumerate(deserialized["episodes"]):
episode = EQAEpisode(**episode)
if scenes_dir is not None:
if episode.scene_id.startswith(DEFAULT_SCENE_PATH_PREFIX):
episode.scene_id = episode.scene_id[
len(DEFAULT_SCENE_PATH_PREFIX):]
episode.scene_id = os.path.join(scenes_dir, episode.scene_id)
episode.question = QuestionData(**episode.question)
for g_index, goal in enumerate(episode.goals):
episode.goals[g_index] = ObjectGoal(**goal)
new_goal = episode.goals[g_index]
if new_goal.view_points is not None:
for p_index, agent_state in enumerate(
new_goal.view_points):
new_goal.view_points[p_index] = AgentState(
**agent_state)
if episode.shortest_paths is not None:
for path in episode.shortest_paths:
for p_index, point in enumerate(path):
path[p_index] = ShortestPathPoint(**point)
self.episodes[ep_index] = episode
def step(self, *args: Any, target: ViewpointData, **kwargs: Any):
r"""Update ``_metric``, this method is called from ``Env`` on each
``step``.
"""
position = target.view_point.position
rotation = target.view_point.rotation
#print("Teleport Action to pos ", target.view_point.position)
if not isinstance(rotation, list):
rotation = list(rotation)
if not self._sim.is_navigable(position):
#print("The destination is not navigable, running snap point")
# is not navigable then we search for a location close to the target
new_position = np.array(position, dtype='f')
new_position = self._sim._sim.pathfinder.snap_point(new_position)
if np.isnan(new_position[0]):
#print("Snap point couldn't find a place to land, error.")
return self._sim.get_observations_at()
else:
position = new_position #.tolist()
#print("New position found", position)
if kwargs and "episode" in kwargs:
last_viewpoint = kwargs["episode"].curr_viewpoint
kwargs["episode"].curr_viewpoint = ViewpointData(
image_id=target.image_id,
view_point=AgentState(position=position, rotation=rotation))
return self._sim.get_observations_at(position=position,
rotation=rotation,
keep_agent_at_new_pose=True)
def from_json(self,
json_str: str,
scenes_dir: Optional[str] = None) -> None:
deserialized = json.loads(json_str)
if CONTENT_SCENES_PATH_FIELD in deserialized:
self.content_scenes_path = deserialized[CONTENT_SCENES_PATH_FIELD]
if "category_to_task_category_id" in deserialized:
self.category_to_task_category_id = deserialized[
"category_to_task_category_id"]
if "category_to_scene_annotation_category_id" in deserialized:
self.category_to_scene_annotation_category_id = deserialized[
"category_to_scene_annotation_category_id"]
if "category_to_mp3d_category_id" in deserialized:
self.category_to_scene_annotation_category_id = deserialized[
"category_to_mp3d_category_id"]
assert len(self.category_to_task_category_id) == len(
self.category_to_scene_annotation_category_id)
assert set(self.category_to_task_category_id.keys()) == set(
self.category_to_scene_annotation_category_id.keys(
)), "category_to_task and category_to_mp3d must have the same keys"
for episode in deserialized["episodes"]:
episode = NavigationEpisode(**episode)
if scenes_dir is not None:
if episode.scene_id.startswith(DEFAULT_SCENE_PATH_PREFIX):
episode.scene_id = episode.scene_id[
len(DEFAULT_SCENE_PATH_PREFIX):]
episode.scene_id = os.path.join(scenes_dir, episode.scene_id)
for i in range(len(episode.goals)):
episode.goals[i] = ObjectGoal(**episode.goals[i])
for vidx, view in enumerate(episode.goals[i].view_points):
view_location = ObjectViewLocation(**view)
view_location.agent_state = AgentState(
**view_location.agent_state)
episode.goals[i].view_points[vidx] = view_location
if episode.shortest_paths is not None:
for path in episode.shortest_paths:
for p_index, point in enumerate(path):
point = {
"action": point,
"rotation": None,
"position": None,
}
path[p_index] = ShortestPathPoint(**point)
self.episodes.append(episode)
for i, ep in enumerate(self.episodes):
ep.episode_id = str(i)
def __deserialize_goal(serialized_goal: Dict[str, Any]) -> ObjectGoal:
g = ObjectGoal(**serialized_goal)
for vidx, view in enumerate(g.view_points):
view_location = ObjectViewLocation(**view)
view_location.agent_state = AgentState(**view_location.agent_state)
g.view_points[vidx] = view_location
return g
def __deserialize_goal(
serialized_goal: Dict[str, Any]) -> SemanticAudioGoal:
g = SemanticAudioGoal(**serialized_goal)
for vidx, view in enumerate(g.view_points):
view_location = ObjectViewLocation(view, iou=0)
view_location.agent_state = AgentState(view_location.agent_state)
g.view_points[vidx] = view_location
return g
def _teacher_actions(self, observations, goal):
action = ""
action_args = {}
navigable_locations = observations["adjacentViewpoints"]
if goal == navigable_locations[0][1]: # image_id
action = "STOP"
else:
step_size = np.pi/6.0 # default step in R2R
goal_location = None
for location in navigable_locations:
if location[1] == goal: # image_id
goal_location = location
break
# Check if the goal is visible
if goal_location:
rel_heading = goal_location[2] # rel_heading
rel_elevation = goal_location[3] #rel_elevation
if rel_heading > step_size:
action = "TURN_RIGHT"
elif rel_heading < -step_size:
action = "TURN_LEFT"
elif rel_elevation > step_size:
action = "LOOK_UP"
elif rel_elevation < -step_size:
action = "LOOK_DOWN"
else:
if goal_location[0] == 1: # restricted
print("WARNING: The target was not in the" +
" Field of view, but the step action " +
"is going to be performed")
action = "TELEPORT" # Move forward
image_id = goal
posB = goal_location[4:7] # start_position
rotA = navigable_locations[0][14:18] # camera_rotation
viewpoint = ViewpointData(
image_id=image_id,
view_point=AgentState(position=posB, rotation=rotA)
)
action_args.update({"target": viewpoint})
else:
# Episode Failure
action = 'STOP'
print("Target position %s not visible, " % goal +
"This is an error in the system")
'''
for ob in observations["images"]:
image = ob
image = image[:,:, [2,1,0]]
cv2.imshow("RGB", image)
cv2.waitKey(0)
'''
return action, action_args
def test_action_space_shortest_path():
config = get_config()
if not os.path.exists(config.SIMULATOR.SCENE):
pytest.skip("Please download Habitat test data to data folder.")
env = habitat.Env(config=config, dataset=None)
# action space shortest path
source_position = env.sim.sample_navigable_point()
angles = [x for x in range(-180, 180, config.SIMULATOR.TURN_ANGLE)]
angle = np.radians(np.random.choice(angles))
source_rotation = [0, np.sin(angle / 2), 0, np.cos(angle / 2)]
source = AgentState(source_position, source_rotation)
reachable_targets = []
unreachable_targets = []
while len(reachable_targets) < 5:
position = env.sim.sample_navigable_point()
angles = [x for x in range(-180, 180, config.SIMULATOR.TURN_ANGLE)]
angle = np.radians(np.random.choice(angles))
rotation = [0, np.sin(angle / 2), 0, np.cos(angle / 2)]
if env.sim.geodesic_distance(source_position, position) != np.inf:
reachable_targets.append(AgentState(position, rotation))
while len(unreachable_targets) < 3:
position = env.sim.sample_navigable_point()
# Change height of the point to make it unreachable
position[1] = 100
angles = [x for x in range(-180, 180, config.SIMULATOR.TURN_ANGLE)]
angle = np.radians(np.random.choice(angles))
rotation = [0, np.sin(angle / 2), 0, np.cos(angle / 2)]
if env.sim.geodesic_distance(source_position, position) == np.inf:
unreachable_targets.append(AgentState(position, rotation))
targets = reachable_targets
shortest_path1 = env.action_space_shortest_path(source, targets)
assert shortest_path1 != []
targets = unreachable_targets
shortest_path2 = env.action_space_shortest_path(source, targets)
assert shortest_path2 == []
env.close()
def xy_yaw_to_agent_state(self, x, y, yaw):
# xy: spot has x point forward, y point left; but habitat agent state has x point right, z point back
position = np.array([-y, 0., -x], dtype=np.float32)
# yaw: magic conversion... debugged by taking habitat agent_state().rotation and reproduce it from yaw
# it is roughly the inverse of this function
# https://github.com/facebookresearch/habitat-lab/blob/b7a93bc493f7fb89e5bf30b40be204ff7b5570d7/habitat/tasks/nav/nav.py#L864
# quaternion.from_euler_angles(np.pi, -true_yaw - np.pi/2, np.pi)
rotation = quaternion.from_euler_angles(np.pi, -yaw,
np.pi) # - np.pi/2
return AgentState(position=position, rotation=rotation)
def _teleport_target(self, observations):
action = ""
action_args = {}
navigable_locations = observations["adjacentViewpoints"]
for location in navigable_locations[1:]:
if location[0] == 1: # location is restricted
continue
elif location[0] == 0: # Non restricted location
action = "TELEPORT"
image_id = location[1]
posB = location[4:7] # start_position
rotA = navigable_locations[0][14:18] # camera_rotation
viewpoint = ViewpointData(image_id=image_id,
view_point=AgentState(position=posB,
rotation=rotA))
action_args = {"target": viewpoint}
#print("the target is ", location)
return {"action": action, "action_args": action_args}
return {"action": action, "action_args": action_args}
def act(self, observations, elapsed_steps, previous_step_collided):
action = ""
action_args = {}
visible_points = sum([
1 - ob[0] for ob in observations["adjacentViewpoints"]
if ob[0] != -1
])
if elapsed_steps == 0:
# Turn right (direction choosing)
action = "TURN_RIGHT"
num_steps = random.randint(0, 11)
if num_steps > 0:
action_args = {"num_steps": num_steps}
# Stop after teleporting 6 times.
elif elapsed_steps >= 5:
action = "STOP"
# Turn right until we can go forward
elif visible_points > 0:
for ob in observations["adjacentViewpoints"]:
if not ob[0]:
goal = ob
action = "TELEPORT"
image_id = goal[1]
pos = goal[4:7] # agent_position
# Keeping the same rotation as the previous step
# camera rotation
rot = observations["adjacentViewpoints"][0][14:18]
viewpoint = ViewpointData(image_id=image_id,
view_point=AgentState(
position=pos, rotation=rot))
action_args.update({"target": viewpoint})
break
else:
action = "TURN_RIGHT"
return {"action": action, "action_args": action_args}
def act(self, observations, episode, goal):
# Initialization when the action is start
batch_size = 1
if self.previous_action == "<start>":
# should be a tensor of logits
if episode.instruction.tokens_length < self.max_tokens:
pad = self.max_tokens - episode.instruction.tokens_length
tokens = episode.instruction.tokens
tokens.extend([0] * pad)
tokens = np.array([tokens])
else:
tokens = episode.instruction.tokens[:self.max_tokens]
tokens = np.array([tokens])
seq_lengths = np.argmax(tokens == 0, axis=1)
seq_lengths[seq_lengths == 0] = self.max_tokens
seq_tensor = torch.from_numpy(tokens).to('cuda')
seq_lengths = torch.from_numpy(seq_lengths).to('cuda')
seq_mask = (seq_tensor == 0)[:, :seq_lengths[0]]
self.seq_mask = seq_mask.to('cuda')
tokens = None
# Forward through encoder, giving initial hidden state and memory cell for decoder
self.ctx, self.h_t, self.c_t = self.encoder(
seq_tensor, seq_lengths)
self.a_t = torch.ones(batch_size, requires_grad=False).long() * \
self.model_actions.index(self.previous_action)
self.a_t = self.a_t.unsqueeze(0).to('cuda')
im = observations["rgb"][:, :, [2, 1, 0]]
f_t = self._get_image_features(im) #.to('cuda')
im = None
ended = np.array(
[False] *
batch_size) # Indices match permuation of the model, not env
# Do a sequence rollout and calculate the loss
self.h_t, self.c_t, alpha, logit = self.decoder(
self.a_t.view(-1, 1), f_t, self.h_t, self.c_t, self.ctx,
self.seq_mask)
# Mask outputs where agent can't move forward
# Release memory?
f_t = None
visible_points = sum([
1 - ob[0] for ob in observations["adjacentViewpoints"]
if ob[0] != -1
])
if visible_points == 0:
logit[0, self.model_actions.index('TELEPORT')] = -float('inf')
# Supervised training
target_action, action_args = self._teacher_actions(observations, goal)
target = torch.LongTensor(1)
target[0] = self.model_actions.index(target_action)
target = target.to('cuda')
self.loss += self.criterion(logit, target)
#print(logit)
# Determine next model inputs
if self.feedback == 'teacher':
self.a_t = target # teacher forcing
action = target_action
elif self.feedback == 'argmax':
_, self.a_t = logit.max(1) # student forcing - argmax
self.a_t = self.a_t.detach()
action = self.model_actions[self.a_t.item()]
action_args = {
} # What happens if you need to teleport? How to choose?
elif self.feedback == 'sample':
probs = F.softmax(logit, dim=1)
m = D.Categorical(probs)
self.a_t = m.sample() # sampling an action from model
action = self.model_actions[self.a_t.item()]
action_args = {}
else:
sys.exit('Invalid feedback option')
# Teleport to the next location, the one with lower rel heading
if action == "TELEPORT" and self.feedback != 'teacher':
sorted_obs = sorted(
observations["adjacentViewpoints"][1:],
key=lambda x: abs(x[2])) # sort by relative heading
for ob in sorted_obs:
if not ob[0]: # restricted
next_location = ob
action = "TELEPORT"
image_id = next_location[1]
pos = next_location[4:7] # agent_position
# Keeping the same rotation as the previous step
# camera rotation
rot = observations["adjacentViewpoints"][0][14:18]
#print("Teleporting to ",image_id, goal, pos, rot, ob,
#[ ob[:3] for ob in sorted_obs if ob[0] != -1])
viewpoint = ViewpointData(image_id=image_id,
view_point=AgentState(
position=pos, rotation=rot))
action_args = {"target": viewpoint}
break
sorted_obs = None
#print(action, target_action, self.loss.item())
#self.predicted_actions.append(action)
self.previous_action = action
return {"action": action, "action_args": action_args}
def generate_objectnav_goals_by_category(sim: Simulator,
task_category) -> ObjectGoal:
scene = sim.semantic_annotations()
target = dict()
for obj in scene.objects:
if obj is not None:
# print(
# f"Object id:{obj.id}, category:{obj.category.name()}, Index:{obj.category.index()}"
# f" center:{obj.aabb.center}, dims:{obj.aabb.sizes}"
# )
if obj.category.name() in task_category.keys():
if obj.category.index() in target:
target[obj.category.index()].append(obj)
else:
target[obj.category.index()] = [obj]
for i in target:
# print("target episode len:", len(target[i]))
object_category = target[i][0].category.name()
# print("object_category :", object_category)
str_goal = f"{os.path.basename(sim.config.SCENE)}_{object_category}"
# print(str_goal)
goals_by = []
for j in range(len(target[i])):
object_view_list = []
for x in np.arange(
target[i][j].aabb.center[0] -
target[i][j].aabb.sizes[0] / 2.0,
target[i][j].aabb.center[0] +
target[i][j].aabb.sizes[0] / 2.0, 0.1):
for y in np.arange(
target[i][j].aabb.center[2] -
target[i][j].aabb.sizes[2] / 2.0,
target[i][j].aabb.center[2] +
target[i][j].aabb.sizes[2] / 2.0, 0.1):
# print("x, y: ", type(x)) # np.float6
# print("x, y: ", type(target[i][j].aabb.center[1])) # np.floa32 报错
object_view_list.append(
ObjectViewLocation(
agent_state=AgentState(position=[
x, target[i][j].aabb.center[1].astype(
np.float64), y
]),
iou=0.0,
))
# print(type(object_view_list))
goal_by_object = ObjectGoal(
position=target[i][j].aabb.center,
radius=0.5,
object_id=int(re.findall(r"\d+", target[i][j].id)[0]),
object_name=target[i][j].id,
object_category=object_category,
view_points=object_view_list,
)
goals_by.append(goal_by_object)
yield str_goal, goals_by