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): 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 _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 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 test_pointgoal_with_gps_compass_sensor(): config = get_config() if not os.path.exists(config.SIMULATOR.SCENE): pytest.skip("Please download Habitat test data to data folder.") config.defrost() config.TASK.SENSORS = [ "POINTGOAL_WITH_GPS_COMPASS_SENSOR", "COMPASS_SENSOR", "GPS_SENSOR", "POINTGOAL_SENSOR", ] config.TASK.POINTGOAL_WITH_GPS_COMPASS_SENSOR.DIMENSIONALITY = 3 config.TASK.POINTGOAL_WITH_GPS_COMPASS_SENSOR.GOAL_FORMAT = "CARTESIAN" config.TASK.POINTGOAL_SENSOR.DIMENSIONALITY = 3 config.TASK.POINTGOAL_SENSOR.GOAL_FORMAT = "CARTESIAN" config.TASK.GPS_SENSOR.DIMENSIONALITY = 3 config.freeze() with habitat.Env(config=config, dataset=None) as env: # start position is checked for validity for the specific test scene valid_start_position = [-1.3731, 0.08431, 8.60692] expected_pointgoal = [0.1, 0.2, 0.3] goal_position = np.add(valid_start_position, expected_pointgoal) # starting quaternion is rotated 180 degree along z-axis, which # corresponds to simulator using z-negative as forward action start_rotation = [0, 0, 0, 1] env.episode_iterator = iter( [ NavigationEpisode( episode_id="0", scene_id=config.SIMULATOR.SCENE, start_position=valid_start_position, start_rotation=start_rotation, goals=[NavigationGoal(position=goal_position)], ) ] ) env.reset() for _ in range(100): obs = env.step(sample_non_stop_action(env.action_space)) pointgoal = obs["pointgoal"] pointgoal_with_gps_compass = obs["pointgoal_with_gps_compass"] compass = float(obs["compass"][0]) gps = obs["gps"] # check to see if taking non-stop actions will affect static point_goal assert np.allclose( pointgoal_with_gps_compass, quaternion_rotate_vector( quaternion.from_rotation_vector( compass * np.array([0, 1, 0]) ).inverse(), pointgoal - gps, ), atol=1e-5, )
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) if self._dimensionality == 3: rel_start_pos = np.array([-agent_position[2], agent_position[0]], dtype=np.float32) else: rel_start_pos = agent_position.astype(np.float32) rotation_world_agent = agent_state.rotation rotation_world_start = quaternion_from_coeff(episode.start_rotation) relative_start_heading = np.array([ self._quat_to_xy_heading(rotation_world_agent.inverse() * rotation_world_start) ]) return np.concatenate([rel_start_pos, relative_start_heading])
def get_rel_elevation(self, posA, rotA, cameraA, posB): direction_vector = np.array([0, 0, -1]) quat = quaternion_from_coeff(rotA) rot_vector = quaternion_rotate_vector(quat.inverse(), direction_vector) camera_quat = quaternion_from_coeff(cameraA) camera_vector = quaternion_rotate_vector(camera_quat.inverse(), direction_vector) elevation_angle = dir_angle_between_quaternions(quat, camera_quat) 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) target_z = target_vector[2] target_length = np.linalg.norm([target_vector[0], target_vector[1]]) rel_elevation = np.arctan2(target_z, target_length) return rel_elevation - elevation_angle
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): 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, observations, episode): 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) if self._dimensionality == 2: return np.array([-agent_position[2], agent_position[0]], dtype=np.float32) else: return agent_position.astype(np.float32)
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 convert_position_to_relative(self, c_data, prev_pos, prev_heading): agent_position = c_data['position'][[0, 2, 1]] agent_rotation = quaternion.from_euler_angles( np.roll(c_data['rotation'], 1)) relative_pos = quaternion_rotate_vector(agent_rotation, agent_position - prev_pos) heading = np.array([_quat_to_xy_heading(agent_rotation)]) relative_heading = heading - prev_heading if np.abs(relative_heading) > np.pi: relative_heading = np.mod(relative_heading, 2 * np.pi * -np.sign(relative_heading)) pos = np.array([relative_pos[0], relative_pos[2]], dtype=np.float32) prev_pos = agent_position prev_heading = heading return np.concatenate([pos, relative_heading]), prev_pos, prev_heading
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 get_observation(self, *args: Any, observations, episode, **kwargs: Any): env_reset = "action" not in kwargs agent_state = self._sim.get_agent_state() if env_reset: self._prev_rotation.clear() self._prev_pos.clear() self._prev_heading.clear() self._prev_rotation.append(agent_state.rotation) self._prev_pos.append(agent_state.position) self._prev_heading.append( np.array([self._quat_to_xy_heading(agent_state.rotation)])) agent_position = agent_state.position relative_pos = quaternion_rotate_vector( self._prev_rotation[0].inverse(), agent_position - self._prev_pos[0]) heading = np.array([self._quat_to_xy_heading(agent_state.rotation)]) relative_heading = heading - self._prev_heading[0] if np.abs(relative_heading) > np.pi: relative_heading = np.mod(relative_heading, 2 * np.pi * -np.sign(relative_heading)) if self._dimensionality == 3: pos = np.array([-relative_pos[2], relative_pos[0]], dtype=np.float32) else: pos = relative_pos.astype(np.float32) self._prev_pos.append(agent_position) self._prev_heading.append(heading) self._prev_rotation.append(agent_state.rotation) return np.concatenate([pos, relative_heading])
locs = [] for ep_num, ep in enumerate(data): info = ep['info'] episode_info, start_rot, actions, weights, gps, heading = info.values() print(episode_info.keys()) _, _, start_pos, start_rot, more_ep_info, goals, start_room, shortest_path = episode_info.values() # if not in_goal_box(goals[0]["position"]): # continue for i, reading in enumerate(gps): reading_pos = [reading[1], start_pos[1], -reading[0]] # reading_pos = [reading[0], start_pos[1], reading[1]] head_phi = heading[i] # this is already global rotation_world_start = quaternion_from_coeff(start_rot) global_offset = quaternion_rotate_vector( rotation_world_start, reading_pos ) global_pos = global_offset + start_pos cur_x, _, cur_y = global_pos loc_info = [cur_x, cur_y, head_phi, actions[i]] loc_info.extend(weights[i]) locs.append(loc_info) df = pd.DataFrame(locs, columns=cols, index=[i for i in range(len(locs))]) for task in aux_tasks: df[task] = pd.to_numeric(df[task]) df['head_cat'] = pd.cut(df['heading'], [-1 * np.pi, -1 * np.pi / 2, 0, np.pi/2, np.pi],# bins=dist_bins, labels=['N', 'W', 'S', 'E']) df['head_cat'].describe() #%%