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 Simulator(SimulatorBackend):
r"""The core class of habitat-sim
:property config: configuration for the simulator
The simulator ties together the backend, the agent, controls functions,
NavMesh collision checking/pathfinding, attribute template management,
object manipulation, and physics simulation.
"""
config: Configuration
agents: List[Agent] = attr.ib(factory=list, init=False)
_num_total_frames: int = attr.ib(default=0, init=False)
_default_agent_id: int = attr.ib(default=0, init=False)
__sensors: List[Dict[str, "Sensor"]] = attr.ib(factory=list, init=False)
_initialized: bool = attr.ib(default=False, init=False)
_previous_step_time: float = attr.ib(
default=0.0, init=False
) # track the compute time of each step
__last_state: Dict[int, AgentState] = attr.ib(factory=dict, init=False)
@staticmethod
def _sanitize_config(config: Configuration) -> None:
if len(config.agents) == 0:
raise RuntimeError(
"Config has not agents specified. Must specify at least 1 agent"
)
config.sim_cfg.create_renderer = any(
map(lambda cfg: len(cfg.sensor_specifications) > 0, config.agents)
)
config.sim_cfg.load_semantic_mesh = any(
map(
lambda cfg: any(
map(
lambda sens_spec: sens_spec.sensor_type == SensorType.SEMANTIC,
cfg.sensor_specifications,
)
),
config.agents,
)
)
config.sim_cfg.requires_textures = any(
map(
lambda cfg: any(
map(
lambda sens_spec: sens_spec.sensor_type == SensorType.COLOR,
cfg.sensor_specifications,
)
),
config.agents,
)
)
def __attrs_post_init__(self) -> None:
self._sanitize_config(self.config)
self.__set_from_config(self.config)
def close(self) -> None:
for agent_sensorsuite in self.__sensors:
for sensor in agent_sensorsuite.values():
sensor.close()
del sensor
self.__sensors = []
for agent in self.agents:
agent.close()
del agent
self.agents = []
self.__last_state.clear()
super().close()
def __enter__(self) -> "Simulator":
return self
def __exit__(self, exc_type, exc_val, exc_tb):
self.close()
def seed(self, new_seed: int) -> None:
super().seed(new_seed)
self.pathfinder.seed(new_seed)
@overload
def reset(self, agent_ids: List[int]) -> Dict[int, ObservationDict]:
...
@overload
def reset(self, agent_ids: Optional[int] = None) -> ObservationDict:
...
def reset(
self, agent_ids: Union[Optional[int], List[int]] = None
) -> Union[ObservationDict, Dict[int, ObservationDict],]:
super().reset()
for i in range(len(self.agents)):
self.reset_agent(i)
if agent_ids is None:
agent_ids = [self._default_agent_id]
return_single = True
else:
agent_ids = cast(List[int], agent_ids)
return_single = False
obs = self.get_sensor_observations(agent_ids=agent_ids)
if return_single:
return obs[agent_ids[0]]
return obs
def reset_agent(self, agent_id: int) -> None:
agent = self.get_agent(agent_id)
initial_agent_state = agent.initial_state
if initial_agent_state is None:
raise RuntimeError("reset called before agent was given an initial state")
self.initialize_agent(agent_id, initial_agent_state)
def _config_backend(self, config: Configuration) -> None:
if not self._initialized:
super().__init__(config.sim_cfg, config.metadata_mediator)
self._initialized = True
else:
super().reconfigure(config.sim_cfg)
def _config_agents(self, config: Configuration) -> None:
self.agents = [
Agent(self.get_active_scene_graph().get_root_node().create_child(), cfg)
for cfg in config.agents
]
def _config_pathfinder(self, config: Configuration) -> None:
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)
def reconfigure(self, config: Configuration) -> None:
self._sanitize_config(config)
if self.config != config:
self.__set_from_config(config)
self.config = config
def __set_from_config(self, config: Configuration) -> None:
self._config_backend(config)
self._config_agents(config)
self._config_pathfinder(config)
self.frustum_culling = config.sim_cfg.frustum_culling
for i in range(len(self.agents)):
self.agents[i].controls.move_filter_fn = self.step_filter
self._default_agent_id = config.sim_cfg.default_agent_id
self.__sensors: List[Dict[str, Sensor]] = [
dict() for i in range(len(config.agents))
]
self.__last_state = dict()
for agent_id, agent_cfg in enumerate(config.agents):
for spec in agent_cfg.sensor_specifications:
self._update_simulator_sensors(spec.uuid, agent_id=agent_id)
self.initialize_agent(agent_id)
def _update_simulator_sensors(self, uuid: str, agent_id: int) -> None:
self.__sensors[agent_id][uuid] = Sensor(
sim=self, agent=self.get_agent(agent_id), sensor_id=uuid
)
def add_sensor(
self, sensor_spec: SensorSpec, agent_id: Optional[int] = None
) -> None:
if (
(
not self.config.sim_cfg.load_semantic_mesh
and sensor_spec.sensor_type == SensorType.SEMANTIC
)
or (
not self.config.sim_cfg.requires_textures
and sensor_spec.sensor_type == SensorType.COLOR
)
or (
not self.config.sim_cfg.create_renderer
and sensor_spec.sensor_type == SensorType.DEPTH
)
):
sensor_type = sensor_spec.sensor_type
raise ValueError(
f"""Data for {sensor_type} sensor was not loaded during Simulator init.
Cannot dynamically add a {sensor_type} sensor unless one already exists.
"""
)
if agent_id is None:
agent_id = self._default_agent_id
agent = self.get_agent(agent_id=agent_id)
agent._add_sensor(sensor_spec)
self._update_simulator_sensors(sensor_spec.uuid, agent_id=agent_id)
def get_agent(self, agent_id: int) -> Agent:
return self.agents[agent_id]
def initialize_agent(
self, agent_id: int, initial_state: Optional[AgentState] = None
) -> Agent:
agent = self.get_agent(agent_id=agent_id)
if initial_state is None:
initial_state = AgentState()
if self.pathfinder.is_loaded:
initial_state.position = self.pathfinder.get_random_navigable_point()
initial_state.rotation = quat_from_angle_axis(
self.random.uniform_float(0, 2.0 * np.pi), np.array([0, 1, 0])
)
agent.set_state(initial_state, is_initial=True)
self.__last_state[agent_id] = agent.state
return agent
@overload
def get_sensor_observations(self, agent_ids: int = 0) -> ObservationDict:
...
@overload
def get_sensor_observations(
self, agent_ids: List[int]
) -> Dict[int, ObservationDict]:
...
def get_sensor_observations(
self, agent_ids: Union[int, List[int]] = 0
) -> Union[ObservationDict, Dict[int, ObservationDict],]:
if isinstance(agent_ids, int):
agent_ids = [agent_ids]
return_single = True
else:
return_single = False
for agent_id in agent_ids:
agent_sensorsuite = self.__sensors[agent_id]
for _sensor_uuid, sensor in agent_sensorsuite.items():
sensor.draw_observation()
# As backport. All Dicts are ordered in Python >= 3.7
observations: Dict[int, ObservationDict] = OrderedDict()
for agent_id in agent_ids:
agent_observations: ObservationDict = {}
for sensor_uuid, sensor in self.__sensors[agent_id].items():
agent_observations[sensor_uuid] = sensor.get_observation()
observations[agent_id] = agent_observations
if return_single:
return next(iter(observations.values()))
return observations
@property
def _default_agent(self) -> Agent:
# TODO Deprecate and remove
return self.get_agent(agent_id=self._default_agent_id)
@property
def _last_state(self) -> AgentState:
# TODO Deprecate and remove
return self.__last_state[self._default_agent_id]
@_last_state.setter
def _last_state(self, state: AgentState) -> None:
# TODO Deprecate and remove
self.__last_state[self._default_agent_id] = state
@property
def _sensors(self) -> Dict[str, "Sensor"]:
# TODO Deprecate and remove
return self.__sensors[self._default_agent_id]
def last_state(self, agent_id: Optional[int] = None) -> AgentState:
if agent_id is None:
agent_id = self._default_agent_id
return self.__last_state[agent_id]
@overload
def step(self, action: Union[str, int], dt: float = 1.0 / 60.0) -> ObservationDict:
...
@overload
def step(
self, action: MutableMapping_T[int, Union[str, int]], dt: float = 1.0 / 60.0
) -> Dict[int, ObservationDict]:
...
def step(
self,
action: Union[str, int, MutableMapping_T[int, Union[str, int]]],
dt: float = 1.0 / 60.0,
) -> Union[ObservationDict, Dict[int, ObservationDict],]:
self._num_total_frames += 1
if isinstance(action, MutableMapping):
return_single = False
else:
action = cast(Dict[int, Union[str, int]], {self._default_agent_id: action})
return_single = True
collided_dict: Dict[int, bool] = {}
for agent_id, agent_act in action.items():
agent = self.get_agent(agent_id)
collided_dict[agent_id] = agent.act(agent_act)
self.__last_state[agent_id] = agent.get_state()
# step physics by dt
step_start_Time = time.time()
super().step_world(dt)
self._previous_step_time = time.time() - step_start_Time
multi_observations = self.get_sensor_observations(agent_ids=list(action.keys()))
for agent_id, agent_observation in multi_observations.items():
agent_observation["collided"] = collided_dict[agent_id]
if return_single:
return multi_observations[self._default_agent_id]
return multi_observations
def make_greedy_follower(
self,
agent_id: Optional[int] = None,
goal_radius: float = None,
*,
stop_key: Optional[Any] = None,
forward_key: Optional[Any] = None,
left_key: Optional[Any] = None,
right_key: Optional[Any] = None,
fix_thrashing: bool = True,
thrashing_threshold: int = 16,
):
if agent_id is None:
agent_id = self._default_agent_id
return GreedyGeodesicFollower(
self.pathfinder,
self.get_agent(agent_id),
goal_radius,
stop_key=stop_key,
forward_key=forward_key,
left_key=left_key,
right_key=right_key,
fix_thrashing=fix_thrashing,
thrashing_threshold=thrashing_threshold,
)
def step_filter(self, start_pos: Vector3, end_pos: Vector3) -> Vector3:
r"""Computes a valid navigable end point given a target translation on the NavMesh.
Uses the configured sliding flag.
:param start_pos: The valid initial position of a translation.
:param end_pos: The target end position of a translation.
"""
if self.pathfinder.is_loaded:
if self.config.sim_cfg.allow_sliding:
end_pos = self.pathfinder.try_step(start_pos, end_pos)
else:
end_pos = self.pathfinder.try_step_no_sliding(start_pos, end_pos)
return end_pos
def __del__(self) -> None:
self.close()
def step_physics(self, dt: float, scene_id: int = 0) -> None:
self.step_world(dt)
class Simulator(SimulatorBackend):
r"""The core class of habitat-sim
:property config: configuration for the simulator
The simulator ties together the backend, the agent, controls functions,
NavMesh collision checking/pathfinding, attribute template management,
object manipulation, and physics simulation.
"""
config: Configuration
agents: List[Agent] = attr.ib(factory=list, init=False)
_num_total_frames: int = attr.ib(default=0, init=False)
_default_agent: Agent = attr.ib(init=False, default=None)
_sensors: Dict = attr.ib(factory=dict, init=False)
_initialized: bool = attr.ib(default=False, init=False)
_previous_step_time: float = attr.ib(
default=0.0, init=False) # track the compute time of each step
@staticmethod
def _sanitize_config(config: Configuration) -> None:
if not len(config.agents) > 0:
raise RuntimeError(
"Config has not agents specified. Must specify at least 1 agent"
)
config.sim_cfg.create_renderer = any(
map(lambda cfg: len(cfg.sensor_specifications) > 0, config.agents))
config.sim_cfg.load_semantic_mesh = any(
map(
lambda cfg: any(
map(
lambda sens_spec: sens_spec.sensor_type == SensorType.
SEMANTIC,
cfg.sensor_specifications,
)),
config.agents,
))
config.sim_cfg.requires_textures = any(
map(
lambda cfg: any(
map(
lambda sens_spec: sens_spec.sensor_type == SensorType.
COLOR,
cfg.sensor_specifications,
)),
config.agents,
))
def __attrs_post_init__(self) -> None:
self._sanitize_config(self.config)
self.__set_from_config(self.config)
def close(self) -> None:
for sensor in self._sensors.values():
sensor.close()
del sensor
self._sensors = {}
for agent in self.agents:
agent.close()
del agent
self.agents = []
del self._default_agent
self._default_agent = None
self.config = None
super().close()
def __enter__(self) -> "Simulator":
return self
def __exit__(self, exc_type, exc_val, exc_tb):
self.close()
def seed(self, new_seed: int) -> None:
super().seed(new_seed)
self.pathfinder.seed(new_seed)
def reset(self) -> Dict[str, ndarray]:
super().reset()
for i in range(len(self.agents)):
self.reset_agent(i)
return self.get_sensor_observations()
def reset_agent(self, agent_id: int) -> None:
agent = self.get_agent(agent_id)
initial_agent_state = agent.initial_state
if initial_agent_state is None:
raise RuntimeError(
"reset called before agent was given an initial state")
self.initialize_agent(agent_id, initial_agent_state)
def _config_backend(self, config: Configuration) -> None:
if not self._initialized:
super().__init__(config.sim_cfg)
self._initialized = True
else:
super().reconfigure(config.sim_cfg)
def _config_agents(self, config: Configuration) -> None:
self.agents = [
Agent(self.get_active_scene_graph().get_root_node().create_child(),
cfg) for cfg in config.agents
]
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)
def reconfigure(self, config: Configuration) -> None:
self._sanitize_config(config)
if self.config != config:
self.__set_from_config(config)
self.config = config
def __set_from_config(self, config: Configuration):
self._config_backend(config)
self._config_agents(config)
self._config_pathfinder(config)
self.frustum_culling = config.sim_cfg.frustum_culling
for i in range(len(self.agents)):
self.agents[i].controls.move_filter_fn = self.step_filter
self._default_agent = self.get_agent(config.sim_cfg.default_agent_id)
agent_cfg = config.agents[config.sim_cfg.default_agent_id]
self._sensors = {}
for spec in agent_cfg.sensor_specifications:
self._sensors[spec.uuid] = Sensor(sim=self,
agent=self._default_agent,
sensor_id=spec.uuid)
for i in range(len(self.agents)):
self.initialize_agent(i)
def get_agent(self, agent_id: int) -> Agent:
return self.agents[agent_id]
def initialize_agent(self,
agent_id: int,
initial_state: Optional[AgentState] = None) -> Agent:
agent = self.get_agent(agent_id=agent_id)
if initial_state is None:
initial_state = AgentState()
if self.pathfinder.is_loaded:
initial_state.position = self.pathfinder.get_random_navigable_point(
)
initial_state.rotation = quat_from_angle_axis(
self.random.uniform_float(0, 2.0 * np.pi),
np.array([0, 1, 0]))
agent.set_state(initial_state, is_initial=True)
self._last_state = agent.state
return agent
def get_sensor_observations(self) -> Dict[str, Union[ndarray, "Tensor"]]:
for _, sensor in self._sensors.items():
sensor.draw_observation()
observations = {}
for sensor_uuid, sensor in self._sensors.items():
observations[sensor_uuid] = sensor.get_observation()
return observations
def last_state(self):
return self._last_state
def step(
self,
action: str,
dt: float = 1.0 / 60.0
) -> Dict[str, Union[bool, ndarray, "Tensor"]]:
self._num_total_frames += 1
collided = self._default_agent.act(action)
self._last_state = self._default_agent.get_state()
# step physics by dt
step_start_Time = time.time()
super().step_world(dt)
self._previous_step_time = time.time() - step_start_Time
observations = self.get_sensor_observations()
# Whether or not the action taken resulted in a collision
observations["collided"] = collided
return observations
def make_greedy_follower(
self,
agent_id: int = 0,
goal_radius: float = None,
*,
stop_key: Optional[Any] = None,
forward_key: Optional[Any] = None,
left_key: Optional[Any] = None,
right_key: Optional[Any] = None,
fix_thrashing: bool = True,
thrashing_threshold: int = 16,
):
return GreedyGeodesicFollower(
self.pathfinder,
self.get_agent(agent_id),
goal_radius,
stop_key=stop_key,
forward_key=forward_key,
left_key=left_key,
right_key=right_key,
fix_thrashing=fix_thrashing,
thrashing_threshold=thrashing_threshold,
)
def step_filter(self, start_pos: Vector3, end_pos: Vector3) -> Vector3:
r"""Computes a valid navigable end point given a target translation on the NavMesh.
Uses the configured sliding flag.
:param start_pos: The valid initial position of a translation.
:param end_pos: The target end position of a translation.
"""
if self.pathfinder.is_loaded:
if self.config.sim_cfg.allow_sliding:
end_pos = self.pathfinder.try_step(start_pos, end_pos)
else:
end_pos = self.pathfinder.try_step_no_sliding(
start_pos, end_pos)
return end_pos
def __del__(self) -> None:
self.close()
def step_physics(self, dt: float, scene_id: int = 0) -> None:
self.step_world(dt)
class Simulator:
r"""The core class of habitat-sim
:property config: configuration for the simulator
The simulator ties together the backend, the agent, controls functions,
and collision checking/pathfinding.
"""
config: Configuration
agents: List[Agent] = attr.ib(factory=list, init=False)
pathfinder: PathFinder = attr.ib(default=None, init=False)
_sim: SimulatorBackend = attr.ib(default=None, init=False)
_num_total_frames: int = attr.ib(default=0, init=False)
_default_agent: Agent = attr.ib(init=False, default=None)
_sensors: Dict = attr.ib(factory=dict, init=False)
_previous_step_time = 0.0 # track the compute time of each step
def __attrs_post_init__(self):
config = self.config
self.config = None
self.reconfigure(config)
def close(self):
for sensor in self._sensors.values():
sensor.close()
del sensor
self._sensors = {}
for agent in self.agents:
agent.close()
del agent
self.agents = []
del self._default_agent
self._default_agent = None
del self._sim
self._sim = None
self.config = None
def seed(self, new_seed):
self._sim.seed(new_seed)
self.pathfinder.seed(new_seed)
def reset(self):
self._sim.reset()
for i in range(len(self.agents)):
self.reset_agent(i)
return self.get_sensor_observations()
def reset_agent(self, agent_id):
agent = self.get_agent(agent_id)
initial_agent_state = agent.initial_state
if initial_agent_state is None:
raise RuntimeError(
"reset called before agent was given an initial state")
self.initialize_agent(agent_id, initial_agent_state)
def _config_backend(self, config: Configuration):
if self._sim is None:
self._sim = SimulatorBackend(config.sim_cfg)
else:
self._sim.reconfigure(config.sim_cfg)
def _config_agents(self, config: Configuration):
if self.config is not None and self.config.agents == config.agents:
return
self.agents = [
Agent(
self._sim.get_active_scene_graph().get_root_node().
create_child(), cfg) for cfg in config.agents
]
def _config_pathfinder(self, config: Configuration):
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)
def reconfigure(self, config: Configuration):
assert len(config.agents) > 0
config.sim_cfg.create_renderer = any(
map(lambda cfg: len(cfg.sensor_specifications) > 0, config.agents))
config.sim_cfg.load_semantic_mesh = any(
map(
lambda cfg: any(
map(
lambda sens_spec: sens_spec.sensor_type == SensorType.
SEMANTIC,
cfg.sensor_specifications,
)),
config.agents,
))
if self.config == config:
return
# NB: Configure backend last as this gives more time for python's GC
# to delete any previous instances of the simulator
# TODO: can't do the above, sorry -- the Agent constructor needs access
# to self._sim.get_active_scene_graph()
self._config_backend(config)
self._config_agents(config)
self._config_pathfinder(config)
self._sim.frustum_culling = config.sim_cfg.frustum_culling
for i in range(len(self.agents)):
self.agents[i].controls.move_filter_fn = self._step_filter
self._default_agent = self.get_agent(config.sim_cfg.default_agent_id)
agent_cfg = config.agents[config.sim_cfg.default_agent_id]
self._sensors = {}
for spec in agent_cfg.sensor_specifications:
self._sensors[spec.uuid] = Sensor(sim=self._sim,
agent=self._default_agent,
sensor_id=spec.uuid)
for i in range(len(self.agents)):
self.initialize_agent(i)
self.config = config
def get_agent(self, agent_id):
return self.agents[agent_id]
def initialize_agent(self, agent_id, initial_state=None):
agent = self.get_agent(agent_id=agent_id)
if initial_state is None:
initial_state = AgentState()
if self.pathfinder.is_loaded:
initial_state.position = self.pathfinder.get_random_navigable_point(
)
initial_state.rotation = quat_from_angle_axis(
np.random.uniform(0, 2.0 * np.pi), np.array([0, 1, 0]))
agent.set_state(initial_state, is_initial=True)
self._last_state = agent.state
return agent
def sample_random_agent_state(self, state_to_return):
return self._sim.sample_random_agent_state(state_to_return)
@property
def semantic_scene(self):
r"""The semantic scene graph
.. note-warning::
Not avaliable for all datasets
"""
return self._sim.semantic_scene
def get_sensor_observations(self):
for _, sensor in self._sensors.items():
sensor.draw_observation()
observations = {}
for sensor_uuid, sensor in self._sensors.items():
observations[sensor_uuid] = sensor.get_observation()
return observations
def last_state(self):
return self._last_state
def step(self, action, dt=1.0 / 60.0):
self._num_total_frames += 1
collided = self._default_agent.act(action)
self._last_state = self._default_agent.get_state()
# step physics by dt
step_start_Time = time.time()
self._sim.step_world(dt)
_previous_step_time = time.time() - step_start_Time
observations = self.get_sensor_observations()
# Whether or not the action taken resulted in a collision
observations["collided"] = collided
return observations
def make_greedy_follower(self,
agent_id: int = 0,
goal_radius: float = None):
return GreedyGeodesicFollower(self.pathfinder,
self.get_agent(agent_id), goal_radius)
def _step_filter(self, start_pos, end_pos):
if self.pathfinder.is_loaded:
if self.config.sim_cfg.allow_sliding:
end_pos = self.pathfinder.try_step(start_pos, end_pos)
else:
end_pos = self.pathfinder.try_step_no_sliding(
start_pos, end_pos)
return end_pos
def __del__(self):
self.close()
# --- object template functions ---
def get_physics_object_library_size(self):
return self._sim.get_physics_object_library_size()
def get_object_template(self, template_id):
return self._sim.get_object_template(template_id)
def load_object_configs(self, path):
return self._sim.load_object_configs(path)
def load_object_template(self, object_template, object_template_handle):
return self._sim.load_object_template(object_template,
object_template_handle)
def get_object_initialization_template(self, object_id, scene_id=0):
return self._sim.get_object_initialization_template(
object_id, scene_id)
# --- physics functions ---
def add_object(
self,
object_lib_index,
attachment_node=None,
light_setup_key=DEFAULT_LIGHTING_KEY,
):
return self._sim.add_object(object_lib_index, attachment_node,
light_setup_key)
def remove_object(self,
object_id,
delete_object_node=True,
delete_visual_node=True):
self._sim.remove_object(object_id, delete_object_node,
delete_visual_node)
def get_existing_object_ids(self, scene_id=0):
return self._sim.get_existing_object_ids(scene_id)
def get_object_motion_type(self, object_id, scene_id=0):
return self._sim.get_object_motion_type(object_id, scene_id)
def set_object_motion_type(self, motion_type, object_id, scene_id=0):
return self._sim.set_object_motion_type(motion_type, object_id,
scene_id)
def set_transformation(self, transform, object_id, scene_id=0):
self._sim.set_transformation(transform, object_id, scene_id)
def get_transformation(self, object_id, scene_id=0):
return self._sim.get_transformation(object_id, scene_id)
def set_translation(self, translation, object_id, scene_id=0):
self._sim.set_translation(translation, object_id, scene_id)
def get_translation(self, object_id, scene_id=0):
return self._sim.get_translation(object_id, scene_id)
def set_rotation(self, rotation, object_id, scene_id=0):
self._sim.set_rotation(rotation, object_id, scene_id)
def get_rotation(self, object_id, scene_id=0):
return self._sim.get_rotation(object_id, scene_id)
def get_object_velocity_control(self, object_id, scene_id=0):
return self._sim.get_object_velocity_control(object_id, scene_id)
def set_linear_velocity(self, lin_vel, object_id, scene_id=0):
self._sim.set_linear_velocity(lin_vel, object_id, scene_id)
def get_linear_velocity(self, object_id, scene_id=0):
return self._sim.get_linear_velocity(object_id, scene_id)
def set_angular_velocity(self, ang_vel, object_id, scene_id=0):
self._sim.set_angular_velocity(ang_vel, object_id, scene_id)
def get_angular_velocity(self, object_id, scene_id=0):
return self._sim.get_angular_velocity(object_id, scene_id)
def apply_force(self, force, relative_position, object_id, scene_id=0):
self._sim.apply_force(force, relative_position, object_id, scene_id)
def apply_torque(self, torque, object_id, scene_id=0):
self._sim.apply_torque(torque, object_id, scene_id)
def contact_test(self, object_id, scene_id=0):
return self._sim.contact_test(object_id, scene_id)
def step_physics(self, dt, scene_id=0):
self._sim.step_world(dt)
def get_world_time(self, scene_id=0):
return self._sim.get_world_time()
def get_gravity(self, scene_id=0):
return self._sim.get_gravity(scene_id)
def set_gravity(self, gravity, scene_id=0):
return self._sim.set_gravity(gravity, scene_id)
def recompute_navmesh(self,
pathfinder,
navmesh_settings,
include_static_objects=False):
return self._sim.recompute_navmesh(pathfinder, navmesh_settings,
include_static_objects)
# --- lighting functions ---
def get_light_setup(self, key=DEFAULT_LIGHTING_KEY):
return self._sim.get_light_setup(key)
def set_light_setup(self, light_setup, key=DEFAULT_LIGHTING_KEY):
self._sim.set_light_setup(light_setup, key)
def set_object_light_setup(self, object_id, light_setup_key, scene_id=0):
self._sim.set_object_light_setup(object_id, light_setup_key, scene_id)