def get_nav_mesh_settings_from_height(height): navmesh_settings = NavMeshSettings() navmesh_settings.set_defaults() navmesh_settings.agent_radius = 0.4 navmesh_settings.agent_height = height navmesh_settings.agent_max_climb = 0.05 return navmesh_settings
def _config_pathfinder(self, config: Configuration) -> None: if "navmesh" in config.sim_cfg.scene.filepaths: navmesh_filenname = config.sim_cfg.scene.filepaths["navmesh"] else: scene_basename = osp.basename(config.sim_cfg.scene.id) # "mesh.ply" is identified as a replica model, whose navmesh # is named as "mesh_semantic.navmesh" and is placed in the # subfolder called "habitat" (a level deeper than the "mesh.ply") if scene_basename == "mesh.ply": scene_dir = osp.dirname(config.sim_cfg.scene.id) navmesh_filenname = osp.join( scene_dir, "habitat", "mesh_semantic.navmesh" ) else: navmesh_filenname = ( osp.splitext(config.sim_cfg.scene.id)[0] + ".navmesh" ) self.pathfinder = PathFinder() if osp.exists(navmesh_filenname): self.pathfinder.load_nav_mesh(navmesh_filenname) logger.info(f"Loaded navmesh {navmesh_filenname}") else: logger.warning( f"Could not find navmesh {navmesh_filenname}, no collision checking will be done" ) agent_legacy_config = AgentConfiguration() default_agent_config = config.agents[config.sim_cfg.default_agent_id] if not np.isclose( agent_legacy_config.radius, default_agent_config.radius ) or not np.isclose(agent_legacy_config.height, default_agent_config.height): logger.info( f"Recomputing navmesh for agent's height {default_agent_config.height} and radius" f" {default_agent_config.radius}." ) navmesh_settings = NavMeshSettings() navmesh_settings.set_defaults() navmesh_settings.agent_radius = default_agent_config.radius navmesh_settings.agent_height = default_agent_config.height self.recompute_navmesh(self.pathfinder, navmesh_settings) self.pathfinder.seed(config.sim_cfg.random_seed)
class HabitatSim(habitat_sim.Simulator, Simulator): r"""Simulator wrapper over habitat-sim habitat-sim repo: https://github.com/facebookresearch/habitat-sim Args: config: configuration for initializing the simulator. """ def __init__(self, config: Config) -> None: self.habitat_config = config agent_config = self._get_agent_config() sim_sensors = [] for sensor_name in agent_config.SENSORS: #print("AGENT CONFIG SENSORS : {}".format(agent_config.SENSORS), flush=True) sensor_cfg = getattr(self.habitat_config, sensor_name) sensor_type = registry.get_sensor(sensor_cfg.TYPE) assert sensor_type is not None, "invalid sensor type {}".format( sensor_cfg.TYPE) sim_sensors.append(sensor_type(sensor_cfg)) self._sensor_suite = SensorSuite(sim_sensors) self.sim_config = self.create_sim_config(self._sensor_suite) self._current_scene = self.sim_config.sim_cfg.scene_id super().__init__(self.sim_config) self._action_space = spaces.Discrete( len(self.sim_config.agents[0].action_space)) self._prev_sim_obs: Optional[Observations] = None # NavMesh related settings : self.navmesh_settings = NavMeshSettings() self.navmesh_settings.set_defaults() self.navmesh_settings.agent_radius = agent_config.RADIUS self.navmesh_settings.agent_height = agent_config.HEIGHT def create_sim_config( self, _sensor_suite: SensorSuite) -> habitat_sim.Configuration: sim_config = habitat_sim.SimulatorConfiguration() # Check if Habitat-Sim is post Scene Config Update if not hasattr(sim_config, "scene_id"): raise RuntimeError( "Incompatible version of Habitat-Sim detected, please upgrade habitat_sim" ) overwrite_config( config_from=self.habitat_config.HABITAT_SIM_V0, config_to=sim_config, # Ignore key as it gets propogated to sensor below ignore_keys={"gpu_gpu"}, ) sim_config.scene_id = self.habitat_config.SCENE agent_config = habitat_sim.AgentConfiguration() overwrite_config( config_from=self._get_agent_config(), config_to=agent_config, # These keys are only used by Hab-Lab ignore_keys={ "is_set_start_state", # This is the Sensor Config. Unpacked below "sensors", "start_position", "start_rotation", }, ) sensor_specifications = [] VisualSensorTypeSet = { habitat_sim.SensorType.COLOR, habitat_sim.SensorType.DEPTH, habitat_sim.SensorType.SEMANTIC, } CameraSensorSubTypeSet = { habitat_sim.SensorSubType.PINHOLE, habitat_sim.SensorSubType.ORTHOGRAPHIC, } for sensor in _sensor_suite.sensors.values(): # Check if type VisualSensorSpec, we know that Sensor is one of HabitatSimRGBSensor, HabitatSimDepthSensor, HabitatSimSemanticSensor if (getattr(sensor, "sim_sensor_type", []) not in VisualSensorTypeSet): raise ValueError( f"""{getattr(sensor, "sim_sensor_type", [])} is an illegal sensorType that is not implemented yet""" ) # Check if type CameraSensorSpec if (getattr(sensor, "sim_sensor_subtype", []) not in CameraSensorSubTypeSet): raise ValueError( f"""{getattr(sensor, "sim_sensor_subtype", [])} is an illegal sensorSubType for a VisualSensor""" ) # TODO: Implement checks for other types of SensorSpecs sim_sensor_cfg = habitat_sim.CameraSensorSpec() # TODO Handle configs for custom VisualSensors that might need # their own ignore_keys. Maybe with special key / checking # SensorType overwrite_config( config_from=sensor.config, config_to=sim_sensor_cfg, # These keys are only used by Hab-Lab # or translated into the sensor config manually ignore_keys={ "height", "hfov", "max_depth", "min_depth", "normalize_depth", "type", "width", }, ) sim_sensor_cfg.uuid = sensor.uuid sim_sensor_cfg.resolution = list( sensor.observation_space.shape[:2]) # TODO(maksymets): Add configure method to Sensor API to avoid # accessing child attributes through parent interface # We know that the Sensor has to be one of these Sensors sensor = cast(HabitatSimVizSensors, sensor) sim_sensor_cfg.sensor_type = sensor.sim_sensor_type sim_sensor_cfg.sensor_subtype = sensor.sim_sensor_subtype sim_sensor_cfg.gpu2gpu_transfer = ( self.habitat_config.HABITAT_SIM_V0.GPU_GPU) sensor_specifications.append(sim_sensor_cfg) agent_config.sensor_specifications = sensor_specifications agent_config.action_space = registry.get_action_space_configuration( self.habitat_config.ACTION_SPACE_CONFIG)( self.habitat_config).get() return habitat_sim.Configuration(sim_config, [agent_config]) @property def sensor_suite(self) -> SensorSuite: return self._sensor_suite @property def action_space(self) -> Space: return self._action_space def _update_agents_state(self) -> bool: is_updated = False for agent_id, _ in enumerate(self.habitat_config.AGENTS): agent_cfg = self._get_agent_config(agent_id) if agent_cfg.IS_SET_START_STATE: self.set_agent_state( agent_cfg.START_POSITION, agent_cfg.START_ROTATION, agent_id, ) is_updated = True return is_updated def reset(self) -> Observations: sim_obs = super().reset() self.counter = 0 # agent_cfg = self._get_agent_config(0) # agent1 = habitat_sim.Agent(super().get_active_scene_graph().get_root_node().create_child(), agent_cfg) # agent1.controls.move_filter_fn = super().step_filter if self._update_agents_state(): sim_obs = self.get_sensor_observations() self._prev_sim_obs = sim_obs observations = self._sensor_suite.get_observations(sim_obs) return self._sensor_suite.get_observations(sim_obs) def step(self, action: Union[str, int]) -> Observations: sim_obs = super().step(action) self._prev_sim_obs = sim_obs observations = self._sensor_suite.get_observations(sim_obs) # self.counter += 1 # if self.counter % 5 == 0: # self.init_objects(super()) return observations def set_object_in_front_of_agent(self, sim, obj_id, z_offset=-1.5): r""" Adds an object in front of the agent at some distance. """ #print("Agent : ") #print(self.get_agent(0)) #agent_transform = sim.agents[0].scene_node.transformation_matrix() agent_transform = self.get_agent(0).scene_node.transformation_matrix() obj_translation = agent_transform.transform_point( np.array([0, 0, z_offset])) sim.set_translation(obj_translation, obj_id) obj_node = sim.get_object_scene_node(obj_id) xform_bb = habitat_sim.geo.get_transformed_bb(obj_node.cumulative_bb, obj_node.transformation) # also account for collision margin of the scene scene_collision_margin = 0.04 y_translation = mn.Vector3( 0, xform_bb.size_y() / 2.0 + scene_collision_margin, 0) sim.set_translation(y_translation + sim.get_translation(obj_id), obj_id) def init_objects(self, sim): # Manager of Object Attributes Templates obj_attr_mgr = sim.get_object_template_manager() #print("Object Template Manager : {}".format(obj_attr_mgr)) #print("SIM Config : ") #print(self.sim_config) #print("Habitat Config: ") #print(self.habitat_config) obj_attr_mgr.load_configs("data/test_assets/objects") # Add a chair into the scene. obj_path = "data/test_assets/objects/locobot_merged" chair_template_id = obj_attr_mgr.load_object_configs(obj_path)[0] chair_attr = obj_attr_mgr.get_template_by_ID(chair_template_id) obj_attr_mgr.register_template(chair_attr) # Object's initial position 3m away from the agent. object_id = sim.add_object_by_handle(chair_attr.handle) self.set_object_in_front_of_agent(sim, object_id, -3.0) sim.set_object_motion_type(habitat_sim.physics.MotionType.STATIC, object_id) # Object's final position 7m away from the agent # goal_id = sim.add_object_by_handle(chair_attr.handle) # self.set_object_in_front_of_agent(sim, goal_id, -7.0) # sim.set_object_motion_type(habitat_sim.physics.MotionType.STATIC, goal_id) print("Object introduced at frame : {}".format(self.counter), flush=True) self.recompute_navmesh(self.pathfinder, self.navmesh_settings, True) #return object_id def add_robot_embodiment(self, sim): # Manager of Object Attributes Templates obj_attr_mgr = sim.get_object_template_manager() #print("Object Template Manager : {}".format(obj_attr_mgr)) #print("SIM Config : ") #print(self.sim_config) #print("Habitat Config: ") #print(self.habitat_config) #obj_attr_mgr.load_configs("data/test_assets/objects") # Add a chair into the scene. obj_path = "data/test_assets/objects/locobot_merged" locobot_template_id = obj_attr_mgr.load_object_configs(obj_path)[0] #chair_attr = obj_attr_mgr.get_template_by_ID(chair_template_id) #obj_attr_mgr.register_template(chair_attr) # Object's initial position 3m away from the agent. object_id = sim.add_object(locobot_template_id, self.get_agent(0).scene_node) sim.set_object_motion_type(habitat_sim.physics.MotionType.KINEMATIC, object_id) sim.set_translation(np.array([1.75, -1.02, 0.4]), object_id) vel_control = sim.get_object_velocity_control(object_id) vel_control.linear_velocity = np.array([0, 0, -1.0]) vel_control.angular_velocity = np.array([0.0, 2.0, 0]) #self.set_object_in_front_of_agent(sim, object_id, -3.0) #sim.set_object_motion_type( # habitat_sim.physics.MotionType.STATIC, object_id #) # Object's final position 7m away from the agent #goal_id = sim.add_object_by_handle(chair_attr.handle) #self.set_object_in_front_of_agent(sim, goal_id, -7.0) #sim.set_object_motion_type(habitat_sim.physics.MotionType.STATIC, goal_id) #self.recompute_navmesh(self.pathfinder, self.navmesh_settings, True) #return object_id def _initialize_objects(self): objects = self.habitat_config.objects[0] obj_attr_mgr = self.get_object_template_manager() obj_attr_mgr.load_configs( str(os.path.join(data_path, "test_assets/objects"))) # first remove all existing objects existing_object_ids = self.get_existing_object_ids() if len(existing_object_ids) > 0: for obj_id in existing_object_ids: self.remove_object(obj_id) self.sim_object_to_objid_mapping = {} self.objid_to_sim_object_mapping = {} if objects is not None: object_template = objects["object_template"] object_pos = objects["position"] object_rot = objects["rotation"] object_template_id = obj_attr_mgr.load_object_configs( object_template)[0] object_attr = obj_attr_mgr.get_template_by_ID(object_template_id) obj_attr_mgr.register_template(object_attr) object_id = self.add_object_by_handle(object_attr.handle) self.sim_object_to_objid_mapping[object_id] = objects["object_id"] self.objid_to_sim_object_mapping[objects["object_id"]] = object_id self.set_translation(object_pos, object_id) if isinstance(object_rot, list): object_rot = quat_from_coeffs(object_rot) object_rot = quat_to_magnum(object_rot) self.set_rotation(object_rot, object_id) self.set_object_motion_type(MotionType.STATIC, object_id) # Recompute the navmesh after placing all the objects. self.recompute_navmesh(self.pathfinder, self.navmesh_settings, True) def render(self, mode: str = "rgb") -> Any: r""" Args: mode: sensor whose observation is used for returning the frame, eg: "rgb", "depth", "semantic" Returns: rendered frame according to the mode """ sim_obs = self.get_sensor_observations() observations = self._sensor_suite.get_observations(sim_obs) output = observations.get(mode) assert output is not None, "mode {} sensor is not active".format(mode) if not isinstance(output, np.ndarray): # If it is not a numpy array, it is a torch tensor # The function expects the result to be a numpy array output = output.to("cpu").numpy() return output def reconfigure(self, habitat_config: Config) -> None: # TODO(maksymets): Switch to Habitat-Sim more efficient caching is_same_scene = habitat_config.SCENE == self._current_scene self.habitat_config = habitat_config self.sim_config = self.create_sim_config(self._sensor_suite) if not is_same_scene: self._current_scene = habitat_config.SCENE self.close() super().reconfigure(self.sim_config) self._update_agents_state() def geodesic_distance( self, position_a: Union[Sequence[float], ndarray], position_b: Union[Sequence[float], Sequence[Sequence[float]]], episode: Optional[Episode] = None, ) -> float: if episode is None or episode._shortest_path_cache is None: path = habitat_sim.MultiGoalShortestPath() if isinstance(position_b[0], (Sequence, np.ndarray)): path.requested_ends = np.array(position_b, dtype=np.float32) else: path.requested_ends = np.array( [np.array(position_b, dtype=np.float32)]) else: path = episode._shortest_path_cache path.requested_start = np.array(position_a, dtype=np.float32) self.pathfinder.find_path(path) if episode is not None: episode._shortest_path_cache = path return path.geodesic_distance def action_space_shortest_path( self, source: AgentState, targets: Sequence[AgentState], agent_id: int = 0, ) -> List[ShortestPathPoint]: r""" Returns: List of agent states and actions along the shortest path from source to the nearest target (both included). If one of the target(s) is identical to the source, a list containing only one node with the identical agent state is returned. Returns an empty list in case none of the targets are reachable from the source. For the last item in the returned list the action will be None. """ raise NotImplementedError( "This function is no longer implemented. Please use the greedy " "follower instead") @property def up_vector(self) -> np.ndarray: return np.array([0.0, 1.0, 0.0]) @property def forward_vector(self) -> np.ndarray: return -np.array([0.0, 0.0, 1.0]) def get_straight_shortest_path_points(self, position_a, position_b): path = habitat_sim.ShortestPath() path.requested_start = position_a path.requested_end = position_b self.pathfinder.find_path(path) return path.points def sample_navigable_point(self) -> List[float]: return self.pathfinder.get_random_navigable_point().tolist() def is_navigable(self, point: List[float]) -> bool: return self.pathfinder.is_navigable(point) def semantic_annotations(self): r""" Returns: SemanticScene which is a three level hierarchy of semantic annotations for the current scene. Specifically this method returns a SemanticScene which contains a list of SemanticLevel's where each SemanticLevel contains a list of SemanticRegion's where each SemanticRegion contains a list of SemanticObject's. SemanticScene has attributes: aabb(axis-aligned bounding box) which has attributes aabb.center and aabb.sizes which are 3d vectors, categories, levels, objects, regions. SemanticLevel has attributes: id, aabb, objects and regions. SemanticRegion has attributes: id, level, aabb, category (to get name of category use category.name()) and objects. SemanticObject has attributes: id, region, aabb, obb (oriented bounding box) and category. SemanticScene contains List[SemanticLevels] SemanticLevel contains List[SemanticRegion] SemanticRegion contains List[SemanticObject] Example to loop through in a hierarchical fashion: for level in semantic_scene.levels: for region in level.regions: for obj in region.objects: """ return self.semantic_scene def _get_agent_config(self, agent_id: Optional[int] = None) -> Any: if agent_id is None: agent_id = self.habitat_config.DEFAULT_AGENT_ID agent_name = self.habitat_config.AGENTS[agent_id] agent_config = getattr(self.habitat_config, agent_name) #agent_config.SENSORS = ['DEPTH_SENSOR'] #setattr(self.habitat_config, agent_config.SENSORS[0], "DEPTH_SENSOR") return agent_config def get_agent_state(self, agent_id: int = 0) -> habitat_sim.AgentState: assert agent_id == 0, "No support of multi agent in {} yet.".format( self.__class__.__name__) return self.get_agent(agent_id).get_state() def set_agent_state( self, position: List[float], rotation: List[float], agent_id: int = 0, reset_sensors: bool = True, ) -> bool: r"""Sets agent state similar to initialize_agent, but without agents creation. On failure to place the agent in the proper position, it is moved back to its previous pose. Args: position: list containing 3 entries for (x, y, z). rotation: list with 4 entries for (x, y, z, w) elements of unit quaternion (versor) representing agent 3D orientation, (https://en.wikipedia.org/wiki/Versor) agent_id: int identification of agent from multiagent setup. reset_sensors: bool for if sensor changes (e.g. tilt) should be reset). Returns: True if the set was successful else moves the agent back to its original pose and returns false. """ agent = self.get_agent(agent_id) new_state = self.get_agent_state(agent_id) new_state.position = position new_state.rotation = rotation # NB: The agent state also contains the sensor states in _absolute_ # coordinates. In order to set the agent's body to a specific # location and have the sensors follow, we must not provide any # state for the sensors. This will cause them to follow the agent's # body new_state.sensor_states = {} agent.set_state(new_state, reset_sensors) return True def get_observations_at( self, position: Optional[List[float]] = None, rotation: Optional[List[float]] = None, keep_agent_at_new_pose: bool = False, ) -> Optional[Observations]: current_state = self.get_agent_state() if position is None or rotation is None: success = True else: success = self.set_agent_state(position, rotation, reset_sensors=False) if success: sim_obs = self.get_sensor_observations() self._prev_sim_obs = sim_obs observations = self._sensor_suite.get_observations(sim_obs) if not keep_agent_at_new_pose: self.set_agent_state( current_state.position, current_state.rotation, reset_sensors=False, ) return observations else: return None def distance_to_closest_obstacle(self, position: ndarray, max_search_radius: float = 2.0) -> float: return self.pathfinder.distance_to_closest_obstacle( position, max_search_radius) def island_radius(self, position: Sequence[float]) -> float: return self.pathfinder.island_radius(position) @property def previous_step_collided(self): r"""Whether or not the previous step resulted in a collision Returns: bool: True if the previous step resulted in a collision, false otherwise Warning: This feild is only updated when :meth:`step`, :meth:`reset`, or :meth:`get_observations_at` are called. It does not update when the agent is moved to a new loction. Furthermore, it will _always_ be false after :meth:`reset` or :meth:`get_observations_at` as neither of those result in an action (step) being taken. """ return self._prev_sim_obs.get("collided", False)