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, )