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 _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_distance_to_pred(pred, sim): from habitat.utils.geometry_utils import quaternion_rotate_vector import networkx as nx current_position = sim.get_agent_state().position agent_state = sim.get_agent_state() source_position = agent_state.position source_rotation = agent_state.rotation rounded_pred = np.round(pred) direction_vector_agent = np.array([rounded_pred[1], 0, -rounded_pred[0]]) direction_vector = quaternion_rotate_vector(source_rotation, direction_vector_agent) pred_goal_location = source_position + direction_vector.astype(np.float32) pred_goal_location[1] = source_position[1] try: if sim.position_encoding( pred_goal_location) not in sim._position_to_index_mapping: pred_goal_location = sim.find_nearest_graph_node( pred_goal_location) distance_to_target = sim.geodesic_distance(current_position, [pred_goal_location]) except nx.exception.NetworkXNoPath: distance_to_target = -1 return distance_to_target
def get_observation(self, observations, episode, *args: Any, **kwargs: Any): episode_uniq_id = f"{episode.scene_id} {episode.episode_id}" if episode_uniq_id != self._current_episode_id: self._episode_time = 0.0 self._current_episode_id = episode_uniq_id 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_xyz = agent_state.position rotation_world_agent = agent_state.rotation agent_position_xyz = quaternion_rotate_vector( rotation_world_start.inverse(), agent_position_xyz - origin) agent_heading = self._quat_to_xy_heading( rotation_world_agent.inverse() * rotation_world_start) ep_time = self._episode_time self._episode_time += 1.0 return np.array([ -agent_position_xyz[2], agent_position_xyz[0], agent_heading, ep_time ], dtype=np.float32)
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_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_vel_world_frame(self, vel_local, rotation, translation) -> np.ndarray: r"""Get velocity in world frame. Args: vel_local: linear/angular velocity in local frame, a numpy array rotation: rotation w.r.t. the world frame, a quaternion vector translation: translation w.r.t the world frame, a numpy array """ # rotate and translate vel_world = quaternion_rotate_vector(rotation, vel_local) + np.array(translation) return vel_world
def compute_heading_from_quaternion(r): """ r - rotation quaternion Computes clockwise rotation about Y. """ # quaternion - np.quaternion unit quaternion # Real world rotation direction_vector = np.array([0, 0, -1]) # Forward vector heading_vector = quaternion_rotate_vector(r.inverse(), direction_vector) phi = -np.arctan2(heading_vector[0], -heading_vector[2]).item() return 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 plot_top_down_map(info, dataset='replica', pred=None): top_down_map = info["top_down_map"]["map"] top_down_map = maps.colorize_topdown_map( top_down_map, info["top_down_map"]["fog_of_war_mask"]) map_agent_pos = info["top_down_map"]["agent_map_coord"] if dataset == 'replica': agent_radius_px = top_down_map.shape[0] // 16 else: agent_radius_px = top_down_map.shape[0] // 50 top_down_map = maps.draw_agent( image=top_down_map, agent_center_coord=map_agent_pos, agent_rotation=info["top_down_map"]["agent_angle"], agent_radius_px=agent_radius_px) if pred is not None: from habitat.utils.geometry_utils import quaternion_rotate_vector source_rotation = info["top_down_map"]["agent_rotation"] rounded_pred = np.round(pred[1]) direction_vector_agent = np.array( [rounded_pred[1], 0, -rounded_pred[0]]) direction_vector = quaternion_rotate_vector(source_rotation, direction_vector_agent) grid_size = ( (maps.COORDINATE_MAX - maps.COORDINATE_MIN) / 10000, (maps.COORDINATE_MAX - maps.COORDINATE_MIN) / 10000, ) delta_x = int(-direction_vector[0] / grid_size[0]) delta_y = int(direction_vector[2] / grid_size[1]) x = np.clip(map_agent_pos[0] + delta_x, a_min=0, a_max=top_down_map.shape[0]) y = np.clip(map_agent_pos[1] + delta_y, a_min=0, a_max=top_down_map.shape[1]) point_padding = 20 for m in range(x - point_padding, x + point_padding + 1): for n in range(y - point_padding, y + point_padding + 1): if np.linalg.norm(np.array([m - x, n - y])) <= point_padding and \ 0 <= m < top_down_map.shape[0] and 0 <= n < top_down_map.shape[1]: top_down_map[m, n] = (0, 255, 255) if np.linalg.norm(rounded_pred) < 1: assert delta_x == 0 and delta_y == 0 if top_down_map.shape[0] > top_down_map.shape[1]: top_down_map = np.rot90(top_down_map, 1) return top_down_map
def get_observation(self, observations, episode, *args: Any, **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 == 2: return np.array([-agent_position[2], agent_position[0]], dtype=np.float32) else: return agent_position.astype(np.float32)
def observations_to_image(observation: Dict, info: Dict, pred=None) -> np.ndarray: r"""Generate image of single frame from observation and info returned from a single environment step(). Args: observation: observation returned from an environment step(). info: info returned from an environment step(). Returns: generated image of a single frame. """ egocentric_view = [] if "rgb" in observation: observation_size = observation["rgb"].shape[0] rgb = observation["rgb"] if not isinstance(rgb, np.ndarray): rgb = rgb.cpu().numpy() egocentric_view.append(rgb) # draw depth map if observation has depth info if "depth" in observation: observation_size = observation["depth"].shape[0] depth_map = observation["depth"].squeeze() * 255.0 if not isinstance(depth_map, np.ndarray): depth_map = depth_map.cpu().numpy() depth_map = depth_map.astype(np.uint8) depth_map = np.stack([depth_map for _ in range(3)], axis=2) egocentric_view.append(depth_map) assert (len(egocentric_view) > 0), "Expected at least one visual sensor enabled." egocentric_view = np.concatenate(egocentric_view, axis=1) # draw collision if "collisions" in info and info["collisions"]["is_collision"]: egocentric_view = draw_collision(egocentric_view) frame = egocentric_view if "top_down_map" in info: top_down_map = info["top_down_map"]["map"] top_down_map = maps.colorize_topdown_map( top_down_map, info["top_down_map"]["fog_of_war_mask"]) map_agent_pos = info["top_down_map"]["agent_map_coord"] top_down_map = maps.draw_agent( image=top_down_map, agent_center_coord=map_agent_pos, agent_rotation=info["top_down_map"]["agent_angle"], agent_radius_px=top_down_map.shape[0] // 16, ) if pred is not None: from habitat.utils.geometry_utils import quaternion_rotate_vector # current_position = sim.get_agent_state().position # agent_state = sim.get_agent_state() source_rotation = info["top_down_map"]["agent_rotation"] rounded_pred = np.round(pred[1]) direction_vector_agent = np.array( [rounded_pred[1], 0, -rounded_pred[0]]) direction_vector = quaternion_rotate_vector( source_rotation, direction_vector_agent) # pred_goal_location = source_position + direction_vector.astype(np.float32) grid_size = ( (maps.COORDINATE_MAX - maps.COORDINATE_MIN) / 10000, (maps.COORDINATE_MAX - maps.COORDINATE_MIN) / 10000, ) delta_x = int(-direction_vector[0] / grid_size[0]) delta_y = int(direction_vector[2] / grid_size[1]) x = np.clip(map_agent_pos[0] + delta_x, a_min=0, a_max=top_down_map.shape[0]) y = np.clip(map_agent_pos[1] + delta_y, a_min=0, a_max=top_down_map.shape[1]) point_padding = 12 for m in range(x - point_padding, x + point_padding + 1): for n in range(y - point_padding, y + point_padding + 1): if np.linalg.norm(np.array([m - x, n - y])) <= point_padding and \ 0 <= m < top_down_map.shape[0] and 0 <= n < top_down_map.shape[1]: top_down_map[m, n] = (0, 255, 255) if np.linalg.norm(rounded_pred) < 1: assert delta_x == 0 and delta_y == 0 if top_down_map.shape[0] > top_down_map.shape[1]: top_down_map = np.rot90(top_down_map, 1) # scale top down map to align with rgb view if pred is None: old_h, old_w, _ = top_down_map.shape top_down_height = observation_size top_down_width = int(float(top_down_height) / old_h * old_w) # cv2 resize (dsize is width first) top_down_map = cv2.resize( top_down_map.astype(np.float32), (top_down_width, top_down_height), interpolation=cv2.INTER_CUBIC, ) else: # draw label CATEGORY_INDEX_MAPPING = { 'chair': 0, 'table': 1, 'picture': 2, 'cabinet': 3, 'cushion': 4, 'sofa': 5, 'bed': 6, 'chest_of_drawers': 7, 'plant': 8, 'sink': 9, 'toilet': 10, 'stool': 11, 'towel': 12, 'tv_monitor': 13, 'shower': 14, 'bathtub': 15, 'counter': 16, 'fireplace': 17, 'gym_equipment': 18, 'seating': 19, 'clothes': 20 } index2label = {v: k for k, v in CATEGORY_INDEX_MAPPING.items()} pred_label = index2label[pred[0]] text_height = int(observation_size * 0.1) old_h, old_w, _ = top_down_map.shape top_down_height = observation_size - text_height top_down_width = int(float(top_down_height) / old_h * old_w) # cv2 resize (dsize is width first) top_down_map = cv2.resize( top_down_map.astype(np.float32), (top_down_width, top_down_height), interpolation=cv2.INTER_CUBIC, ) top_down_map = np.concatenate([ np.ones([text_height, top_down_map.shape[1], 3], dtype=np.int32) * 255, top_down_map ], axis=0) top_down_map = cv2.putText(top_down_map, 'C_t: ' + pred_label.replace('_', ' '), (10, text_height - 10), cv2.FONT_HERSHEY_SIMPLEX, 1.4, (0, 0, 0), 2, cv2.LINE_AA) frame = np.concatenate((egocentric_view, top_down_map), axis=1) return frame