def _compute_pointgoal(self, source_position, source_rotation,
goal_position):
direction_vector = goal_position - source_position
direction_vector_agent = quaternion_rotate_vector(
source_rotation.inverse(), direction_vector)
if self._goal_format == "POLAR":
if self._dimensionality == 2:
rho, phi = cartesian_to_polar(-direction_vector_agent[2],
direction_vector_agent[0])
return np.array([rho, -phi], dtype=np.float32)
else:
_, phi = cartesian_to_polar(-direction_vector_agent[2],
direction_vector_agent[0])
theta = np.arccos(direction_vector_agent[1] /
np.linalg.norm(direction_vector_agent))
rho = np.linalg.norm(direction_vector_agent)
return np.array([rho, -phi, theta], dtype=np.float32)
else:
if self._dimensionality == 2:
return np.array(
[-direction_vector_agent[2], direction_vector_agent[0]],
dtype=np.float32,
)
else:
return direction_vector_agent
def _compute_pointgoal(self, source_position, source_rotation, goal_position):
# This is updated to invert the sign of phi (changing conventions from
# the original pointgoal definition.)
direction_vector = goal_position - source_position
direction_vector_agent = quaternion_rotate_vector(
source_rotation.inverse(), direction_vector
)
if self._goal_format == "POLAR":
if self._dimensionality == 2:
rho, phi_orig = cartesian_to_polar(
-direction_vector_agent[2], direction_vector_agent[0]
)
phi = -phi_orig
return np.array([rho, -phi], dtype=np.float32)
else:
_, phi_orig = cartesian_to_polar(
-direction_vector_agent[2], direction_vector_agent[0]
)
phi = -phi_orig
theta = np.arccos(
direction_vector_agent[1] / np.linalg.norm(direction_vector_agent)
)
rho = np.linalg.norm(direction_vector_agent)
return np.array([rho, -phi, theta], dtype=np.float32)
else:
if self._dimensionality == 2:
return np.array(
[-direction_vector_agent[2], direction_vector_agent[0]],
dtype=np.float32,
)
else:
return direction_vector_agent
def _quat_to_xy_heading(self, quat):
direction_vector = np.array([0, 0, -1])
heading_vector = quaternion_rotate_vector(quat, direction_vector)
phi = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
return np.array([phi], dtype=np.float32)
def _compute_pointgoal(self, source_position, source_rotation,
goal_position):
fov = self.fov
direction_vector = goal_position - source_position
direction_vector_agent = quaternion_rotate_vector(
source_rotation.inverse(), direction_vector)
_, phi = cartesian_to_polar(-direction_vector_agent[2],
direction_vector_agent[0])
theta = np.arcsin(direction_vector_agent[1] /
np.linalg.norm(direction_vector_agent))
rho = np.linalg.norm(direction_vector_agent)
dx = phi
dy = -1 * theta
fov_min = -1 * fov / 2
fov_max = 1 * fov / 2
xpx = dx / fov
ypx = dy / fov
if fov_max < dx < fov_min:
xpx = -1
if fov_max < dy < fov_min:
ypx = -1
return np.array([rho, -phi, theta, xpx, ypx], dtype=np.float32)
def get_rel_heading(self, posA, rotA, posB):
direction_vector = np.array([0, 0, -1])
quat = quaternion_from_coeff(rotA).inverse()
# The heading vector and heading angle are in arctan2 format
heading_vector = quaternion_rotate_vector(quat, direction_vector)
heading = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
heading = self.normalize_angle(heading)
adjusted_heading = np.pi / 2 - heading
camera_horizon_vec = [
np.cos(adjusted_heading),
np.sin(adjusted_heading), 0
]
# This vectors are in habitat format we need to rotate them.
rotated_posB = [posB[0], -posB[2], posB[1]]
rotated_posA = [posA[0], -posA[2], posA[1]]
target_vector = np.array(rotated_posB) - np.array(rotated_posA)
y = target_vector[0] * camera_horizon_vec[1] - \
target_vector[1] * camera_horizon_vec[0]
x = target_vector[0] * camera_horizon_vec[0] + \
target_vector[1] * camera_horizon_vec[1]
return np.arctan2(y, x)
def get_info(self, observations):
agent_state = self._env.sim.get_agent_state()
heading_vector = quaternion_rotate_vector(
agent_state.rotation.inverse(), np.array([0, 0, -1]))
heading = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
return {
"position": agent_state.position.tolist(),
"heading": heading,
"stop": self._env.task.is_stop_called,
}
def get_polar_angle(self):
agent_state = self._sim.get_agent_state()
# quaternion is in x, y, z, w format
ref_rotation = agent_state.rotation
heading_vector = quaternion_rotate_vector(ref_rotation.inverse(),
np.array([0, 0, -1]))
phi = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
z_neg_z_flip = np.pi
return np.array(phi) + z_neg_z_flip
def get_observation(self, observations, episode):
agent_state = self._sim.get_agent_state()
rotation_world_agent = agent_state.rotation
direction_vector = np.array([0, 0, -1])
heading_vector = quaternion_rotate_vector(
rotation_world_agent.inverse(), direction_vector)
phi = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
return np.array(phi)
def get_polar_angle(self):
# CHANGED BY ANDREW SZOT FOR FETCH ROBOT
agent_state = self._sim.get_agent_state()
# quaternion is in x, y, z, w format
ref_rotation = agent_state.rotation
heading_vector = quaternion_rotate_vector(ref_rotation.inverse(),
np.array([0, 0, 1]))
phi = cartesian_to_polar(heading_vector[2], heading_vector[0])[1]
#z_neg_z_flip = np.pi
z_neg_z_flip = np.pi / 2.0
return np.array(phi) + z_neg_z_flip
def get_observation(self, observations, episode):
agent_state = self._sim.get_agent_state()
# Quaternion is in x, y, z, w format
ref_rotation = agent_state.rotation
direction_vector = np.array([0, 0, -1])
rotation_world_agent = quaternion_to_rotation(ref_rotation[3],
ref_rotation[0],
ref_rotation[1],
ref_rotation[2])
heading_vector = np.dot(rotation_world_agent.T, direction_vector)
phi = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
return np.array(phi)
def get_observation(self, observations, episode):
agent_state = self._sim.get_agent_state()
ref_position = agent_state.position
rotation_world_agent = agent_state.rotation
direction_vector = (
np.array(episode.goals[0].position, dtype=np.float32) -
ref_position)
direction_vector_agent = quaternion_rotate_vector(
rotation_world_agent.inverse(), direction_vector)
if self._goal_format == "POLAR":
rho, phi = cartesian_to_polar(-direction_vector_agent[2],
direction_vector_agent[0])
direction_vector_agent = np.array([rho, -phi], dtype=np.float32)
return direction_vector_agent
def pos_real_to_map(pos, episode):
agent_position = pos
origin = np.array(episode.start_position, dtype=np.float32)
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
agent_position = quaternion_rotate_vector(
rotation_world_start.inverse(), agent_position - origin
)
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
direction_vector = np.array([0, 0, -1])
heading_vector = quaternion_rotate_vector(rotation_world_start.inverse(), direction_vector)
phi = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
return np.array(
[-agent_position[2], agent_position[0], phi], dtype=np.float32,
)
def get_observation(self, observations, episode):
episode_id = (episode.episode_id, episode.scene_id)
if self.current_episode_id != episode_id:
# Only compute the direction vector when a new episode is started.
self.current_episode_id = episode_id
agent_state = self._sim.get_agent_state()
ref_position = agent_state.position
rotation_world_agent = agent_state.rotation
direction_vector = (
np.array(episode.goals[0].position, dtype=np.float32) -
ref_position)
direction_vector_agent = quaternion_rotate_vector(
rotation_world_agent.inverse(), direction_vector)
if self._goal_format == "POLAR":
rho, phi = cartesian_to_polar(-direction_vector_agent[2],
direction_vector_agent[0])
direction_vector_agent = np.array([rho, -phi],
dtype=np.float32)
self._initial_vector = direction_vector_agent
return self._initial_vector
def quat_to_xy_heading(quat):
direction_vector = np.array([0, 0, -1])
heading_vector = quaternion_rotate_vector(quat, direction_vector)
phi = cartesian_to_polar(-heading_vector[2], heading_vector[0])[1]
return np.array(phi)