# Copyright (c) 2022-2025, The Isaac Lab Project Developers.# All rights reserved.## SPDX-License-Identifier: BSD-3-Clauseimporttorchfromcollections.abcimportSequencefromtypingimportAnyimportcarbimportomni.usdfromisaacsim.core.clonerimportGridClonerfromisaacsim.core.primsimportXFormPrimfrompxrimportPhysxSchemaimportisaaclab.simassim_utilsfromisaaclab.assetsimport(Articulation,ArticulationCfg,AssetBaseCfg,DeformableObject,DeformableObjectCfg,RigidObject,RigidObjectCfg,RigidObjectCollection,RigidObjectCollectionCfg,)fromisaaclab.sensorsimportContactSensorCfg,FrameTransformerCfg,SensorBase,SensorBaseCfgfromisaaclab.terrainsimportTerrainImporter,TerrainImporterCfgfrom.interactive_scene_cfgimportInteractiveSceneCfg
[docs]classInteractiveScene:"""A scene that contains entities added to the simulation. The interactive scene parses the :class:`InteractiveSceneCfg` class to create the scene. Based on the specified number of environments, it clones the entities and groups them into different categories (e.g., articulations, sensors, etc.). Cloning can be performed in two ways: * For tasks where all environments contain the same assets, a more performant cloning paradigm can be used to allow for faster environment creation. This is specified by the ``replicate_physics`` flag. .. code-block:: python scene = InteractiveScene(cfg=InteractiveSceneCfg(replicate_physics=True)) * For tasks that require having separate assets in the environments, ``replicate_physics`` would have to be set to False, which will add some costs to the overall startup time. .. code-block:: python scene = InteractiveScene(cfg=InteractiveSceneCfg(replicate_physics=False)) Each entity is registered to scene based on its name in the configuration class. For example, if the user specifies a robot in the configuration class as follows: .. code-block:: python from isaaclab.scene import InteractiveSceneCfg from isaaclab.utils import configclass from isaaclab_assets.robots.anymal import ANYMAL_C_CFG @configclass class MySceneCfg(InteractiveSceneCfg): robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") Then the robot can be accessed from the scene as follows: .. code-block:: python from isaaclab.scene import InteractiveScene # create 128 environments scene = InteractiveScene(cfg=MySceneCfg(num_envs=128)) # access the robot from the scene robot = scene["robot"] # access the robot based on its type robot = scene.articulations["robot"] If the :class:`InteractiveSceneCfg` class does not include asset entities, the cloning process can still be triggered if assets were added to the stage outside of the :class:`InteractiveScene` class: .. code-block:: python scene = InteractiveScene(cfg=InteractiveSceneCfg(num_envs=128, replicate_physics=True)) scene.clone_environments() .. note:: It is important to note that the scene only performs common operations on the entities. For example, resetting the internal buffers, writing the buffers to the simulation and updating the buffers from the simulation. The scene does not perform any task specific to the entity. For example, it does not apply actions to the robot or compute observations from the robot. These tasks are handled by different modules called "managers" in the framework. Please refer to the :mod:`isaaclab.managers` sub-package for more details. """
[docs]def__init__(self,cfg:InteractiveSceneCfg):"""Initializes the scene. Args: cfg: The configuration class for the scene. """# check that the config is validcfg.validate()# store inputsself.cfg=cfg# initialize scene elementsself._terrain=Noneself._articulations=dict()self._deformable_objects=dict()self._rigid_objects=dict()self._rigid_object_collections=dict()self._sensors=dict()self._extras=dict()# obtain the current stageself.stage=omni.usd.get_context().get_stage()# physics scene pathself._physics_scene_path=None# prepare cloner for environment replicationself.cloner=GridCloner(spacing=self.cfg.env_spacing)self.cloner.define_base_env(self.env_ns)self.env_prim_paths=self.cloner.generate_paths(f"{self.env_ns}/env",self.cfg.num_envs)# create source primself.stage.DefinePrim(self.env_prim_paths[0],"Xform")# when replicate_physics=False, we assume heterogeneous environments and clone the xforms first.# this triggers per-object level cloning in the spawner.ifnotself.cfg.replicate_physics:# clone the env xformenv_origins=self.cloner.clone(source_prim_path=self.env_prim_paths[0],prim_paths=self.env_prim_paths,replicate_physics=False,copy_from_source=True,enable_env_ids=self.cfg.filter_collisions,# this won't do anything because we are not replicating physics)self._default_env_origins=torch.tensor(env_origins,device=self.device,dtype=torch.float32)else:# otherwise, environment origins will be initialized during cloning at the end of environment creationself._default_env_origins=Noneself._global_prim_paths=list()ifself._is_scene_setup_from_cfg():# add entities from configself._add_entities_from_cfg()# clone environments on a global scope if environment is homogeneousifself.cfg.replicate_physics:self.clone_environments(copy_from_source=False)# replicate physics if we have more than one environment# this is done to make scene initialization faster at play timeifself.cfg.replicate_physicsandself.cfg.num_envs>1:self.cloner.replicate_physics(source_prim_path=self.env_prim_paths[0],prim_paths=self.env_prim_paths,base_env_path=self.env_ns,root_path=self.env_regex_ns.replace(".*",""),enable_env_ids=self.cfg.filter_collisions,)# since env_ids is only applicable when replicating physics, we have to fallback to the previous method# to filter collisions if replicate_physics is not enabledifnotself.cfg.replicate_physicsandself.cfg.filter_collisions:self.filter_collisions(self._global_prim_paths)
[docs]defclone_environments(self,copy_from_source:bool=False):"""Creates clones of the environment ``/World/envs/env_0``. Args: copy_from_source: (bool): If set to False, clones inherit from /World/envs/env_0 and mirror its changes. If True, clones are independent copies of the source prim and won't reflect its changes (start-up time may increase). Defaults to False. """# check if user spawned different assets in individual environments# this flag will be None if no multi asset is spawnedcarb_settings_iface=carb.settings.get_settings()has_multi_assets=carb_settings_iface.get("/isaaclab/spawn/multi_assets")ifhas_multi_assetsandself.cfg.replicate_physics:omni.log.warn("Varying assets might have been spawned under different environments."" However, the replicate physics flag is enabled in the 'InteractiveScene' configuration."" This may adversely affect PhysX parsing. We recommend disabling this property.")# clone the environmentenv_origins=self.cloner.clone(source_prim_path=self.env_prim_paths[0],prim_paths=self.env_prim_paths,replicate_physics=self.cfg.replicate_physics,copy_from_source=copy_from_source,enable_env_ids=self.cfg.filter_collisions,# this automatically filters collisions between environments)# since env_ids is only applicable when replicating physics, we have to fallback to the previous method# to filter collisions if replicate_physics is not enabledifnotself.cfg.replicate_physicsandself.cfg.filter_collisions:omni.log.warn("Collision filtering can only be automatically enabled when replicate_physics=True."" Please call scene.filter_collisions(global_prim_paths) to filter collisions across environments.")# in case of heterogeneous cloning, the env origins is specified at initifself._default_env_originsisNone:self._default_env_origins=torch.tensor(env_origins,device=self.device,dtype=torch.float32)
[docs]deffilter_collisions(self,global_prim_paths:list[str]|None=None):"""Filter environments collisions. Disables collisions between the environments in ``/World/envs/env_.*`` and enables collisions with the prims in global prim paths (e.g. ground plane). Args: global_prim_paths: A list of global prim paths to enable collisions with. Defaults to None, in which case no global prim paths are considered. """# validate paths in global prim pathsifglobal_prim_pathsisNone:global_prim_paths=[]else:# remove duplicates in pathsglobal_prim_paths=list(set(global_prim_paths))# set global prim paths list if not previously definediflen(self._global_prim_paths)<1:self._global_prim_paths+=global_prim_paths# filter collisions within each environment instanceself.cloner.filter_collisions(self.physics_scene_path,"/World/collisions",self.env_prim_paths,global_paths=self._global_prim_paths,)
def__str__(self)->str:"""Returns a string representation of the scene."""msg=f"<class {self.__class__.__name__}>\n"msg+=f"\tNumber of environments: {self.cfg.num_envs}\n"msg+=f"\tEnvironment spacing : {self.cfg.env_spacing}\n"msg+=f"\tSource prim name : {self.env_prim_paths[0]}\n"msg+=f"\tGlobal prim paths : {self._global_prim_paths}\n"msg+=f"\tReplicate physics : {self.cfg.replicate_physics}"returnmsg""" Properties. """@propertydefphysics_scene_path(self)->str:"""The path to the USD Physics Scene."""ifself._physics_scene_pathisNone:forpriminself.stage.Traverse():ifprim.HasAPI(PhysxSchema.PhysxSceneAPI):self._physics_scene_path=prim.GetPrimPath().pathStringomni.log.info(f"Physics scene prim path: {self._physics_scene_path}")breakifself._physics_scene_pathisNone:raiseRuntimeError("No physics scene found! Please make sure one exists.")returnself._physics_scene_path@propertydefphysics_dt(self)->float:"""The physics timestep of the scene."""returnsim_utils.SimulationContext.instance().get_physics_dt()# pyright: ignore [reportOptionalMemberAccess]@propertydefdevice(self)->str:"""The device on which the scene is created."""returnsim_utils.SimulationContext.instance().device# pyright: ignore [reportOptionalMemberAccess]@propertydefenv_ns(self)->str:"""The namespace ``/World/envs`` in which all environments created. The environments are present w.r.t. this namespace under "env_{N}" prim, where N is a natural number. """return"/World/envs"@propertydefenv_regex_ns(self)->str:"""The namespace ``/World/envs/env_.*`` in which all environments created."""returnf"{self.env_ns}/env_.*"@propertydefnum_envs(self)->int:"""The number of environments handled by the scene."""returnself.cfg.num_envs@propertydefenv_origins(self)->torch.Tensor:"""The origins of the environments in the scene. Shape is (num_envs, 3)."""ifself._terrainisnotNone:returnself._terrain.env_originselse:returnself._default_env_origins@propertydefterrain(self)->TerrainImporter|None:"""The terrain in the scene. If None, then the scene has no terrain. Note: We treat terrain separate from :attr:`extras` since terrains define environment origins and are handled differently from other miscellaneous entities. """returnself._terrain@propertydefarticulations(self)->dict[str,Articulation]:"""A dictionary of articulations in the scene."""returnself._articulations@propertydefdeformable_objects(self)->dict[str,DeformableObject]:"""A dictionary of deformable objects in the scene."""returnself._deformable_objects@propertydefrigid_objects(self)->dict[str,RigidObject]:"""A dictionary of rigid objects in the scene."""returnself._rigid_objects@propertydefrigid_object_collections(self)->dict[str,RigidObjectCollection]:"""A dictionary of rigid object collections in the scene."""returnself._rigid_object_collections@propertydefsensors(self)->dict[str,SensorBase]:"""A dictionary of the sensors in the scene, such as cameras and contact reporters."""returnself._sensors@propertydefextras(self)->dict[str,XFormPrim]:"""A dictionary of miscellaneous simulation objects that neither inherit from assets nor sensors. The keys are the names of the miscellaneous objects, and the values are the `XFormPrim`_ of the corresponding prims. As an example, lights or other props in the scene that do not have any attributes or properties that you want to alter at runtime can be added to this dictionary. Note: These are not reset or updated by the scene. They are mainly other prims that are not necessarily handled by the interactive scene, but are useful to be accessed by the user. .. _XFormPrim: https://docs.omniverse.nvidia.com/py/isaacsim/source/isaacsim.core/docs/index.html#isaacsim.core.prims.XFormPrim """returnself._extras@propertydefstate(self)->dict[str,dict[str,dict[str,torch.Tensor]]]:"""A dictionary of the state of the scene entities in the simulation world frame. Please refer to :meth:`get_state` for the format. """returnself.get_state(is_relative=False)""" Operations. """
[docs]defreset(self,env_ids:Sequence[int]|None=None):"""Resets the scene entities. Args: env_ids: The indices of the environments to reset. Defaults to None (all instances). """# -- assetsforarticulationinself._articulations.values():articulation.reset(env_ids)fordeformable_objectinself._deformable_objects.values():deformable_object.reset(env_ids)forrigid_objectinself._rigid_objects.values():rigid_object.reset(env_ids)forrigid_object_collectioninself._rigid_object_collections.values():rigid_object_collection.reset(env_ids)# -- sensorsforsensorinself._sensors.values():sensor.reset(env_ids)
[docs]defwrite_data_to_sim(self):"""Writes the data of the scene entities to the simulation."""# -- assetsforarticulationinself._articulations.values():articulation.write_data_to_sim()fordeformable_objectinself._deformable_objects.values():deformable_object.write_data_to_sim()forrigid_objectinself._rigid_objects.values():rigid_object.write_data_to_sim()forrigid_object_collectioninself._rigid_object_collections.values():rigid_object_collection.write_data_to_sim()
[docs]defupdate(self,dt:float)->None:"""Update the scene entities. Args: dt: The amount of time passed from last :meth:`update` call. """# -- assetsforarticulationinself._articulations.values():articulation.update(dt)fordeformable_objectinself._deformable_objects.values():deformable_object.update(dt)forrigid_objectinself._rigid_objects.values():rigid_object.update(dt)forrigid_object_collectioninself._rigid_object_collections.values():rigid_object_collection.update(dt)# -- sensorsforsensorinself._sensors.values():sensor.update(dt,force_recompute=notself.cfg.lazy_sensor_update)
""" Operations: Scene State. """
[docs]defreset_to(self,state:dict[str,dict[str,dict[str,torch.Tensor]]],env_ids:Sequence[int]|None=None,is_relative:bool=False,):"""Resets the entities in the scene to the provided state. Args: state: The state to reset the scene entities to. Please refer to :meth:`get_state` for the format. env_ids: The indices of the environments to reset. Defaults to None, in which case all environment instances are reset. is_relative: If set to True, the state is considered relative to the environment origins. Defaults to False. """# resolve env_idsifenv_idsisNone:env_ids=slice(None)# articulationsforasset_name,articulationinself._articulations.items():asset_state=state["articulation"][asset_name]# root stateroot_pose=asset_state["root_pose"].clone()ifis_relative:root_pose[:,:3]+=self.env_origins[env_ids]root_velocity=asset_state["root_velocity"].clone()articulation.write_root_pose_to_sim(root_pose,env_ids=env_ids)articulation.write_root_velocity_to_sim(root_velocity,env_ids=env_ids)# joint statejoint_position=asset_state["joint_position"].clone()joint_velocity=asset_state["joint_velocity"].clone()articulation.write_joint_state_to_sim(joint_position,joint_velocity,env_ids=env_ids)# FIXME: This is not generic as it assumes PD control over the joints.# This assumption does not hold for effort controlled joints.articulation.set_joint_position_target(joint_position,env_ids=env_ids)articulation.set_joint_velocity_target(joint_velocity,env_ids=env_ids)# deformable objectsforasset_name,deformable_objectinself._deformable_objects.items():asset_state=state["deformable_object"][asset_name]nodal_position=asset_state["nodal_position"].clone()ifis_relative:nodal_position[:,:3]+=self.env_origins[env_ids]nodal_velocity=asset_state["nodal_velocity"].clone()deformable_object.write_nodal_pos_to_sim(nodal_position,env_ids=env_ids)deformable_object.write_nodal_velocity_to_sim(nodal_velocity,env_ids=env_ids)# rigid objectsforasset_name,rigid_objectinself._rigid_objects.items():asset_state=state["rigid_object"][asset_name]root_pose=asset_state["root_pose"].clone()ifis_relative:root_pose[:,:3]+=self.env_origins[env_ids]root_velocity=asset_state["root_velocity"].clone()rigid_object.write_root_pose_to_sim(root_pose,env_ids=env_ids)rigid_object.write_root_velocity_to_sim(root_velocity,env_ids=env_ids)# write data to simulation to make sure initial state is set# this propagates the joint targets to the simulationself.write_data_to_sim()
[docs]defget_state(self,is_relative:bool=False)->dict[str,dict[str,dict[str,torch.Tensor]]]:"""Returns the state of the scene entities. Based on the type of the entity, the state comprises of different components. * For an articulation, the state comprises of the root pose, root velocity, and joint position and velocity. * For a deformable object, the state comprises of the nodal position and velocity. * For a rigid object, the state comprises of the root pose and root velocity. The returned state is a dictionary with the following format: .. code-block:: python { "articulation": { "entity_1_name": { "root_pose": torch.Tensor, "root_velocity": torch.Tensor, "joint_position": torch.Tensor, "joint_velocity": torch.Tensor, }, "entity_2_name": { "root_pose": torch.Tensor, "root_velocity": torch.Tensor, "joint_position": torch.Tensor, "joint_velocity": torch.Tensor, }, }, "deformable_object": { "entity_3_name": { "nodal_position": torch.Tensor, "nodal_velocity": torch.Tensor, } }, "rigid_object": { "entity_4_name": { "root_pose": torch.Tensor, "root_velocity": torch.Tensor, } }, } where ``entity_N_name`` is the name of the entity registered in the scene. Args: is_relative: If set to True, the state is considered relative to the environment origins. Defaults to False. Returns: A dictionary of the state of the scene entities. """state=dict()# articulationsstate["articulation"]=dict()forasset_name,articulationinself._articulations.items():asset_state=dict()asset_state["root_pose"]=articulation.data.root_state_w[:,:7].clone()ifis_relative:asset_state["root_pose"][:,:3]-=self.env_originsasset_state["root_velocity"]=articulation.data.root_vel_w.clone()asset_state["joint_position"]=articulation.data.joint_pos.clone()asset_state["joint_velocity"]=articulation.data.joint_vel.clone()state["articulation"][asset_name]=asset_state# deformable objectsstate["deformable_object"]=dict()forasset_name,deformable_objectinself._deformable_objects.items():asset_state=dict()asset_state["nodal_position"]=deformable_object.data.nodal_pos_w.clone()ifis_relative:asset_state["nodal_position"][:,:3]-=self.env_originsasset_state["nodal_velocity"]=deformable_object.data.nodal_vel_w.clone()state["deformable_object"][asset_name]=asset_state# rigid objectsstate["rigid_object"]=dict()forasset_name,rigid_objectinself._rigid_objects.items():asset_state=dict()asset_state["root_pose"]=rigid_object.data.root_state_w[:,:7].clone()ifis_relative:asset_state["root_pose"][:,:3]-=self.env_originsasset_state["root_velocity"]=rigid_object.data.root_vel_w.clone()state["rigid_object"][asset_name]=asset_statereturnstate
""" Operations: Iteration. """
[docs]defkeys(self)->list[str]:"""Returns the keys of the scene entities. Returns: The keys of the scene entities. """all_keys=["terrain"]forasset_familyin[self._articulations,self._deformable_objects,self._rigid_objects,self._rigid_object_collections,self._sensors,self._extras,]:all_keys+=list(asset_family.keys())returnall_keys
def__getitem__(self,key:str)->Any:"""Returns the scene entity with the given key. Args: key: The key of the scene entity. Returns: The scene entity. """# check if it is a terrainifkey=="terrain":returnself._terrainall_keys=["terrain"]# check if it is in other dictionariesforasset_familyin[self._articulations,self._deformable_objects,self._rigid_objects,self._rigid_object_collections,self._sensors,self._extras,]:out=asset_family.get(key)# if found, returnifoutisnotNone:returnoutall_keys+=list(asset_family.keys())# if not found, raise errorraiseKeyError(f"Scene entity with key '{key}' not found. Available Entities: '{all_keys}'")""" Internal methods. """def_is_scene_setup_from_cfg(self):returnany(not(asset_nameinInteractiveSceneCfg.__dataclass_fields__orasset_cfgisNone)forasset_name,asset_cfginself.cfg.__dict__.items())def_add_entities_from_cfg(self):"""Add scene entities from the config."""# store paths that are in global collision filterself._global_prim_paths=list()# parse the entire scene config and resolve regexforasset_name,asset_cfginself.cfg.__dict__.items():# skip keywords# note: easier than writing a list of keywords: [num_envs, env_spacing, lazy_sensor_update]ifasset_nameinInteractiveSceneCfg.__dataclass_fields__orasset_cfgisNone:continue# resolve regexifhasattr(asset_cfg,"prim_path"):asset_cfg.prim_path=asset_cfg.prim_path.format(ENV_REGEX_NS=self.env_regex_ns)# create assetifisinstance(asset_cfg,TerrainImporterCfg):# terrains are special entities since they define environment originsasset_cfg.num_envs=self.cfg.num_envsasset_cfg.env_spacing=self.cfg.env_spacingself._terrain=asset_cfg.class_type(asset_cfg)elifisinstance(asset_cfg,ArticulationCfg):self._articulations[asset_name]=asset_cfg.class_type(asset_cfg)elifisinstance(asset_cfg,DeformableObjectCfg):self._deformable_objects[asset_name]=asset_cfg.class_type(asset_cfg)elifisinstance(asset_cfg,RigidObjectCfg):self._rigid_objects[asset_name]=asset_cfg.class_type(asset_cfg)elifisinstance(asset_cfg,RigidObjectCollectionCfg):forrigid_object_cfginasset_cfg.rigid_objects.values():rigid_object_cfg.prim_path=rigid_object_cfg.prim_path.format(ENV_REGEX_NS=self.env_regex_ns)self._rigid_object_collections[asset_name]=asset_cfg.class_type(asset_cfg)forrigid_object_cfginasset_cfg.rigid_objects.values():ifhasattr(rigid_object_cfg,"collision_group")andrigid_object_cfg.collision_group==-1:asset_paths=sim_utils.find_matching_prim_paths(rigid_object_cfg.prim_path)self._global_prim_paths+=asset_pathselifisinstance(asset_cfg,SensorBaseCfg):# Update target frame path(s)' regex name space for FrameTransformerifisinstance(asset_cfg,FrameTransformerCfg):updated_target_frames=[]fortarget_frameinasset_cfg.target_frames:target_frame.prim_path=target_frame.prim_path.format(ENV_REGEX_NS=self.env_regex_ns)updated_target_frames.append(target_frame)asset_cfg.target_frames=updated_target_frameselifisinstance(asset_cfg,ContactSensorCfg):updated_filter_prim_paths_expr=[]forfilter_prim_pathinasset_cfg.filter_prim_paths_expr:updated_filter_prim_paths_expr.append(filter_prim_path.format(ENV_REGEX_NS=self.env_regex_ns))asset_cfg.filter_prim_paths_expr=updated_filter_prim_paths_exprself._sensors[asset_name]=asset_cfg.class_type(asset_cfg)elifisinstance(asset_cfg,AssetBaseCfg):# manually spawn assetifasset_cfg.spawnisnotNone:asset_cfg.spawn.func(asset_cfg.prim_path,asset_cfg.spawn,translation=asset_cfg.init_state.pos,orientation=asset_cfg.init_state.rot,)# store xform prim view corresponding to this asset# all prims in the scene are Xform prims (i.e. have a transform component)self._extras[asset_name]=XFormPrim(asset_cfg.prim_path,reset_xform_properties=False)else:raiseValueError(f"Unknown asset config type for {asset_name}: {asset_cfg}")# store global collision pathsifhasattr(asset_cfg,"collision_group")andasset_cfg.collision_group==-1:asset_paths=sim_utils.find_matching_prim_paths(asset_cfg.prim_path)self._global_prim_paths+=asset_paths