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