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