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)
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 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]
x_y_flip = -np.pi / 2
return np.array(phi) + x_y_flip
def get_observation(self, *args: Any, observations, episode,
**kwargs: Any):
episode_id = (episode.episode_id, episode.scene_id)
agent_state = self._sim.get_agent_state()
# At the start of a new episode, reset pose estimate
if self.current_episode_id != episode_id:
self.current_episode_id = episode_id
# Initialize with the ground-truth position, rotation
self._estimated_position = agent_state.position
self._estimated_rotation = agent_state.rotation
self._past_gt_position = agent_state.position
self._past_gt_rotation = agent_state.rotation
# Compute noisy delta
delta_xz_noisy, delta_y_noisy = self._get_noisy_delta(agent_state)
# Update past gt states
self._past_gt_position = agent_state.position
self._past_gt_rotation = agent_state.rotation
# Update noisy pose estimates
(
self._estimated_position,
self._estimated_rotation,
) = compute_updated_pose(
self._estimated_position,
self._estimated_rotation,
delta_xz_noisy,
delta_y_noisy,
)
# Compute sensor readings with noisy estimates
origin = np.array(episode.start_position, dtype=np.float32)
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
agent_position = self._estimated_position
agent_position = quaternion_rotate_vector(
rotation_world_start.inverse(), agent_position - origin)
rotation_world_agent = self._estimated_rotation
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
agent_heading = self._quat_to_xy_heading(
rotation_world_agent.inverse() * rotation_world_start)
# This is rotation from -Z to -X. We want -Z to X for this particular sensor.
agent_heading = -agent_heading
return np.array(
[-agent_position[2], agent_position[0], agent_heading],
dtype=np.float32,
)
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_position_vector(env):
origin = np.array(env.current_episode.start_position, dtype=np.float32)
rotation_world_start = quaternion_from_coeff(
env.current_episode.start_rotation)
agent_position = quaternion_rotate_vector(rotation_world_start, origin)
rotation_world_start = quaternion_from_coeff(
env.current_episode.start_rotation)
agent_heading = quat_to_xy_heading(rotation_world_start)
# This is rotation from -Z to -X. We want -Z to X for this particular sensor.
agent_heading = -agent_heading
return np.array(
[-agent_position[2], agent_position[0], agent_heading],
dtype=np.float32,
)
def get_position_vector_from_obs(observations):
origin = np.array(observations[0]['start_coords'][:3], dtype=np.float32)
rotation_world_start = quaternion_from_coeff(
observations[0]['start_coords'][3:])
agent_position = quaternion_rotate_vector(rotation_world_start, origin)
rotation_world_start = quaternion_from_coeff(
observations[0]['start_coords'][3:])
agent_heading = quat_to_xy_heading(rotation_world_start)
# This is rotation from -Z to -X. We want -Z to X for this particular sensor.
agent_heading = -agent_heading
return np.array(
[-agent_position[2], agent_position[0], agent_heading],
dtype=np.float32,
)
def get_observation(self, *args: Any, observations, episode, **kwargs: Any):
agent_state = self._sim.get_agent_state()
origin = np.array(episode.start_position, dtype=np.float32)
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
agent_position = agent_state.position
agent_position = quaternion_rotate_vector(
rotation_world_start.inverse(), agent_position - origin
)
rotation_world_agent = agent_state.rotation
rotation_world_start = quaternion_from_coeff(episode.start_rotation)
agent_heading = self._quat_to_xy_heading(
rotation_world_agent.inverse() * rotation_world_start
)
# This is rotation from -Z to -X. We want -Z to X for this particular sensor.
agent_heading = -agent_heading
return np.array(
[-agent_position[2], agent_position[0], agent_heading], dtype=np.float32,
)
