diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index e4fd4c95d5b..5c34474e39d 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -216,7 +216,6 @@ def benchmark_xform_prim_view( # close simulation sim.clear() - sim.clear_all_callbacks() sim.clear_instance() return timing_results, computed_results diff --git a/source/isaaclab/isaaclab/assets/asset_base.py b/source/isaaclab/isaaclab/assets/asset_base.py index 3c78c079377..6dc450baca1 100644 --- a/source/isaaclab/isaaclab/assets/asset_base.py +++ b/source/isaaclab/isaaclab/assets/asset_base.py @@ -308,15 +308,18 @@ def _initialize_callback(self, event): called whenever the simulator "plays" from a "stop" state. """ if not self._is_initialized: - # obtain simulation related information - self._backend = SimulationManager.get_backend() - self._device = SimulationManager.get_physics_sim_device() - # initialize the asset try: + # Obtain Simulation Context + sim = sim_utils.SimulationContext.instance() + if sim is None: + raise RuntimeError("Simulation Context is not initialized!") + # Obtain device and backend + self._device = sim.device + # initialize the asset self._initialize_impl() except Exception as e: - if builtins.ISAACLAB_CALLBACK_EXCEPTION is None: - builtins.ISAACLAB_CALLBACK_EXCEPTION = e + if builtins.ISAACLAB_CALLBACK_EXCEPTION is None: # type: ignore + builtins.ISAACLAB_CALLBACK_EXCEPTION = e # type: ignore # set flag self._is_initialized = True diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env.py b/source/isaaclab/isaaclab/envs/direct_marl_env.py index 97fb3ae3da5..dbc6bb4c966 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env.py @@ -5,7 +5,6 @@ from __future__ import annotations -import builtins import gymnasium as gym import inspect import logging @@ -150,17 +149,16 @@ def __init__(self, cfg: DirectMARLEnvCfg, render_mode: str | None = None, **kwar # play the simulator to activate physics handles # note: this activates the physics simulation view that exposes TensorAPIs # note: when started in extension mode, first call sim.reset_async() and then initialize the managers - if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: - print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") - with Timer("[INFO]: Time taken for simulation start", "simulation_start"): - # since the reset can trigger callbacks which use the stage, - # we need to set the stage context here - with use_stage(self.sim.get_initial_stage()): - self.sim.reset() - # update scene to pre populate data buffers for assets and sensors. - # this is needed for the observation manager to get valid tensors for initialization. - # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. - self.scene.update(dt=self.physics_dt) + print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") + with Timer("[INFO]: Time taken for simulation start", "simulation_start"): + # since the reset can trigger callbacks which use the stage, + # we need to set the stage context here + with use_stage(self.sim.get_initial_stage()): + self.sim.reset() + # update scene to pre populate data buffers for assets and sensors. + # this is needed for the observation manager to get valid tensors for initialization. + # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. + self.scene.update(dt=self.physics_dt) # check if debug visualization is has been implemented by the environment source_code = inspect.getsource(self._set_debug_vis_impl) @@ -549,7 +547,6 @@ def close(self): self.sim.stop() self.sim.clear() - self.sim.clear_all_callbacks() self.sim.clear_instance() # destroy the window diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index 22b8f3217c5..8922934cc15 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -5,7 +5,6 @@ from __future__ import annotations -import builtins import gymnasium as gym import inspect import logging @@ -157,17 +156,16 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs # play the simulator to activate physics handles # note: this activates the physics simulation view that exposes TensorAPIs # note: when started in extension mode, first call sim.reset_async() and then initialize the managers - if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: - print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") - with Timer("[INFO]: Time taken for simulation start", "simulation_start"): - # since the reset can trigger callbacks which use the stage, - # we need to set the stage context here - with use_stage(self.sim.get_initial_stage()): - self.sim.reset() - # update scene to pre populate data buffers for assets and sensors. - # this is needed for the observation manager to get valid tensors for initialization. - # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. - self.scene.update(dt=self.physics_dt) + print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") + with Timer("[INFO]: Time taken for simulation start", "simulation_start"): + # since the reset can trigger callbacks which use the stage, + # we need to set the stage context here + with use_stage(self.sim.get_initial_stage()): + self.sim.reset() + # update scene to pre populate data buffers for assets and sensors. + # this is needed for the observation manager to get valid tensors for initialization. + # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. + self.scene.update(dt=self.physics_dt) # check if debug visualization is has been implemented by the environment source_code = inspect.getsource(self._set_debug_vis_impl) @@ -518,7 +516,6 @@ def close(self): self.sim.stop() self.sim.clear() - self.sim.clear_all_callbacks() self.sim.clear_instance() # destroy the window diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 564668d55f3..9f03968b0a7 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -3,7 +3,6 @@ # # SPDX-License-Identifier: BSD-3-Clause -import builtins import logging import torch import warnings @@ -101,11 +100,7 @@ def __init__(self, cfg: ManagerBasedEnvCfg): # since it gets confused with Isaac Sim's SimulationContext class self.sim: SimulationContext = SimulationContext(self.cfg.sim) else: - # simulation context should only be created before the environment - # when in extension mode - if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: - raise RuntimeError("Simulation context already exists. Cannot create a new one.") - self.sim: SimulationContext = SimulationContext.instance() + raise RuntimeError("Simulation context already exists. Cannot create a new one.") # make sure torch is running on the correct device if "cuda" in self.device: @@ -162,19 +157,18 @@ def __init__(self, cfg: ManagerBasedEnvCfg): # play the simulator to activate physics handles # note: this activates the physics simulation view that exposes TensorAPIs # note: when started in extension mode, first call sim.reset_async() and then initialize the managers - if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: - print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") - with Timer("[INFO]: Time taken for simulation start", "simulation_start"): - # since the reset can trigger callbacks which use the stage, - # we need to set the stage context here - with use_stage(self.sim.get_initial_stage()): - self.sim.reset() - # update scene to pre populate data buffers for assets and sensors. - # this is needed for the observation manager to get valid tensors for initialization. - # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. - self.scene.update(dt=self.physics_dt) - # add timeline event to load managers - self.load_managers() + print("[INFO]: Starting the simulation. This may take a few seconds. Please wait...") + with Timer("[INFO]: Time taken for simulation start", "simulation_start"): + # since the reset can trigger callbacks which use the stage, + # we need to set the stage context here + with use_stage(self.sim.get_initial_stage()): + self.sim.reset() + # update scene to pre populate data buffers for assets and sensors. + # this is needed for the observation manager to get valid tensors for initialization. + # this shouldn't cause an issue since later on, users do a reset over all the environments so the lazy buffers would be reset. + self.scene.update(dt=self.physics_dt) + # add timeline event to load managers + self.load_managers() # extend UI elements # we need to do this here after all the managers are initialized @@ -536,7 +530,6 @@ def close(self): self.sim.stop() self.sim.clear() - self.sim.clear_all_callbacks() self.sim.clear_instance() # destroy the window diff --git a/source/isaaclab/isaaclab/sensors/sensor_base.py b/source/isaaclab/isaaclab/sensors/sensor_base.py index 7de0c82541a..7d6bdf88183 100644 --- a/source/isaaclab/isaaclab/sensors/sensor_base.py +++ b/source/isaaclab/isaaclab/sensors/sensor_base.py @@ -203,7 +203,6 @@ def _initialize_impl(self): raise RuntimeError("Simulation Context is not initialized!") # Obtain device and backend self._device = sim.device - self._backend = sim.backend self._sim_physics_dt = sim.get_physics_dt() # Count number of environments env_prim_path_expr = self.cfg.prim_path.rsplit("/", 1)[0] @@ -301,8 +300,8 @@ def _initialize_callback(self, event): try: self._initialize_impl() except Exception as e: - if builtins.ISAACLAB_CALLBACK_EXCEPTION is None: - builtins.ISAACLAB_CALLBACK_EXCEPTION = e + if builtins.ISAACLAB_CALLBACK_EXCEPTION is None: # type: ignore + builtins.ISAACLAB_CALLBACK_EXCEPTION = e # type: ignore self._is_initialized = True def _invalidate_initialize_callback(self, event): diff --git a/source/isaaclab/isaaclab/sim/simulation_cfg.py b/source/isaaclab/isaaclab/sim/simulation_cfg.py index 9f74b5ba016..ac19ca22b32 100644 --- a/source/isaaclab/isaaclab/sim/simulation_cfg.py +++ b/source/isaaclab/isaaclab/sim/simulation_cfg.py @@ -43,6 +43,26 @@ class PhysxCfg: * :obj:`1`: TGS (Temporal Gauss-Seidel) """ + solve_articulation_contact_last: bool = False + """Changes the ordering inside the articulation solver. Default is False. + + PhysX employs a strict ordering for handling constraints in an articulation. The outcome of + each constraint resolution modifies the joint and associated link speeds. However, the default + ordering may not be ideal for gripping scenarios because the solver favours the constraint + types that are resolved last. This is particularly true of stiff constraint systems that are hard + to resolve without resorting to vanishingly small simulation timesteps. + + With dynamic contact resolution being such an important part of gripping, it may make + more sense to solve dynamic contact towards the end of the solver rather than at the + beginning. This parameter modifies the default ordering to enable this change. + + For more information, please check `here `__. + + .. versionadded:: v2.3 + This parameter is only available with Isaac Sim 5.1. + + """ + min_position_iteration_count: int = 1 """Minimum number of solver position iterations (rigid bodies, cloth, particles etc.). Default is 1. @@ -173,26 +193,6 @@ class PhysxCfg: gpu_max_particle_contacts: int = 2**20 """Size of particle contacts stream buffer allocated in pinned host memory. Default is 2 ** 20.""" - solve_articulation_contact_last: bool = False - """Changes the ordering inside the articulation solver. Default is False. - - PhysX employs a strict ordering for handling constraints in an articulation. The outcome of - each constraint resolution modifies the joint and associated link speeds. However, the default - ordering may not be ideal for gripping scenarios because the solver favours the constraint - types that are resolved last. This is particularly true of stiff constraint systems that are hard - to resolve without resorting to vanishingly small simulation timesteps. - - With dynamic contact resolution being such an important part of gripping, it may make - more sense to solve dynamic contact towards the end of the solver rather than at the - beginning. This parameter modifies the default ordering to enable this change. - - For more information, please check `here `__. - - .. versionadded:: v2.3 - This parameter is only available with Isaac Sim 5.1. - - """ - @configclass class RenderCfg: diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index e27053f1362..8f7a675e6be 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -3,12 +3,13 @@ # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + import builtins import enum import flatdict import glob import logging -import numpy as np import os import re import time @@ -19,15 +20,17 @@ from collections.abc import Iterator from contextlib import contextmanager from datetime import datetime -from typing import Any +from typing import Any, ClassVar import carb +import omni.kit.app +import omni.physics.tensors import omni.physx +import omni.timeline import omni.usd -from isaacsim.core.api.simulation_context import SimulationContext as _SimulationContext from isaacsim.core.simulation_manager import SimulationManager from isaacsim.core.utils.viewports import set_camera_view -from pxr import Gf, PhysxSchema, Sdf, Usd, UsdPhysics +from pxr import Gf, PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdUtils import isaaclab.sim as sim_utils from isaaclab.utils.logger import configure_logging @@ -35,13 +38,12 @@ from .simulation_cfg import SimulationCfg from .spawners import DomeLightCfg, GroundPlaneCfg -from .utils import bind_physics_material # import logger logger = logging.getLogger(__name__) -class SimulationContext(_SimulationContext): +class SimulationContext: """A class to control simulation-related events such as physics stepping and rendering. The simulation context helps control various simulation aspects. This includes: @@ -82,6 +84,12 @@ class SimulationContext(_SimulationContext): the non-``_async`` functions are used in the standalone python script mode. """ + _instance: SimulationContext | None = None + """The singleton instance of the simulation context.""" + + _is_initialized: ClassVar[bool] = False + """Whether the simulation context is initialized.""" + class RenderMode(enum.IntEnum): """Different rendering modes for the simulation. @@ -124,15 +132,18 @@ def __init__(self, cfg: SimulationCfg | None = None): cfg: The configuration of the simulation. Defaults to None, in which case the default configuration is used. """ + # skip if already initialized + if SimulationContext._is_initialized: + return + SimulationContext._is_initialized = True + # store input if cfg is None: cfg = SimulationCfg() # check that the config is valid - cfg.validate() + cfg.validate() # type: ignore + # store configuration self.cfg = cfg - # check that simulation is running - if sim_utils.get_current_stage() is None: - raise RuntimeError("The stage has not been created. Did you run the simulator?") # setup logger self.logger = configure_logging( @@ -146,13 +157,21 @@ def __init__(self, cfg: SimulationCfg | None = None): self._initial_stage = sim_utils.create_new_stage_in_memory() else: self._initial_stage = omni.usd.get_context().get_stage() + if not self._initial_stage: + self._initial_stage = sim_utils.create_new_stage() + # check that stage is created + if self._initial_stage is None: + raise RuntimeError("Failed to create a new stage. Please check if the USD context is valid.") + # add stage to USD cache + stage_cache = UsdUtils.StageCache.Get() + stage_id = stage_cache.GetId(self._initial_stage).ToLongInt() + if stage_id < 0: + stage_id = stage_cache.Insert(self._initial_stage).ToLongInt() + self._initial_stage_id = stage_id # acquire settings interface self.carb_settings = carb.settings.get_settings() - # apply carb physics settings - self._apply_physics_settings() - # note: we read this once since it is not expected to change during runtime # read flag for whether a local GUI is enabled self._local_gui = self.carb_settings.get("/app/window/enabled") @@ -222,11 +241,6 @@ def __init__(self, cfg: SimulationCfg | None = None): # this is needed for some GUI features if self._has_gui: self.cfg.enable_scene_query_support = True - # set up flatcache/fabric interface (default is None) - # this is needed to flush the flatcache data into Hydra manually when calling `render()` - # ref: https://docs.omniverse.nvidia.com/prod_extensions/prod_extensions/ext_physics.html - # note: need to do this here because super().__init__ calls render and this variable is needed - self._fabric_iface = None # create a tensor for gravity # note: this line is needed to create a "tensor" in the device to avoid issues with torch 2.1 onwards. @@ -239,73 +253,118 @@ def __init__(self, cfg: SimulationCfg | None = None): # add callback to deal the simulation app when simulation is stopped. # this is needed because physics views go invalid once we stop the simulation - if not builtins.ISAAC_LAUNCHED_FROM_TERMINAL: - timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() - self._app_control_on_stop_handle = timeline_event_stream.create_subscription_to_pop_by_type( - int(omni.timeline.TimelineEventType.STOP), - lambda *args, obj=weakref.proxy(self): obj._app_control_on_stop_handle_fn(*args), - order=15, - ) - else: - self._app_control_on_stop_handle = None + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + self._app_control_on_stop_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), + lambda *args, obj=weakref.proxy(self): obj._app_control_on_stop_handle_fn(*args), + order=15, + ) self._disable_app_control_on_stop_handle = False - # flatten out the simulation dictionary - sim_params = self.cfg.to_dict() - if sim_params is not None: - if "physx" in sim_params: - physx_params = sim_params.pop("physx") - sim_params.update(physx_params) + # obtain interfaces for simulation + self._app_iface = omni.kit.app.get_app_interface() + self._framework = carb.get_framework() + self._physx_iface = omni.physx.get_physx_interface() + self._physx_sim_iface = omni.physx.get_physx_simulation_interface() + self._timeline_iface = omni.timeline.get_timeline_interface() + + # set timeline auto update to True + self._timeline_iface.set_auto_update(True) + + # set stage properties + with sim_utils.use_stage(self._initial_stage): + # correct conventions for metric units + UsdGeom.SetStageUpAxis(self._initial_stage, "Z") + UsdGeom.SetStageMetersPerUnit(self._initial_stage, 1.0) + UsdPhysics.SetStageKilogramsPerUnit(self._initial_stage, 1.0) + + # find if any physics prim already exists and delete it + for prim in self._initial_stage.Traverse(): + if prim.HasAPI(PhysxSchema.PhysxSceneAPI) or prim.GetTypeName() == "PhysicsScene": + sim_utils.delete_prim(prim.GetPath().pathString, stage=self._initial_stage) + # create a new physics scene + if self._initial_stage.GetPrimAtPath(self.cfg.physics_prim_path).IsValid(): + raise RuntimeError(f"A prim already exists at path '{self.cfg.physics_prim_path}'.") + self._physics_scene = UsdPhysics.Scene.Define(self._initial_stage, self.cfg.physics_prim_path) + self._physx_scene_api = PhysxSchema.PhysxSceneAPI.Apply(self._physics_scene.GetPrim()) + + # set time codes per second + self._configure_simulation_dt() + # apply physics settings (carb and physx) + self._apply_physics_settings() - # add warning about enabling stabilization for large step sizes - if not self.cfg.physx.enable_stabilization and (self.cfg.dt > 0.0333): - self.logger.warning( - "Large simulation step size (> 0.0333 seconds) is not recommended without enabling stabilization." - " Consider setting the `enable_stabilization` flag to True in the PhysxCfg, or reducing the" - " simulation step size if you run into physics issues." - ) + # a stage update here is needed for the case when physics_dt != rendering_dt, otherwise the app crashes + # when in headless mode + self.set_setting("/app/player/playSimulations", False) + self._app_iface.update() + self.set_setting("/app/player/playSimulations", True) + # load flatcache/fabric interface + self._load_fabric_interface() - # set simulation device - # note: Although Isaac Sim sets the physics device in the init function, - # it does a render call which gets the wrong device. - SimulationManager.set_physics_sim_device(self.cfg.device) - - # obtain the parsed device - # This device should be the same as "self.cfg.device". However, for cases, where users specify the device - # as "cuda" and not "cuda:X", then it fetches the current device from SimulationManager. - # Note: Since we fix the device from the configuration and don't expect users to change it at runtime, - # we can obtain the device once from the SimulationManager.get_physics_sim_device() function. - # This reduces the overhead of calling the function. - self._physics_device = SimulationManager.get_physics_sim_device() - - # create a simulation context to control the simulator - if get_isaac_sim_version().major < 5: - # stage arg is not supported before isaac sim 5.0 - super().__init__( - stage_units_in_meters=1.0, - physics_dt=self.cfg.dt, - rendering_dt=self.cfg.dt * self.cfg.render_interval, - backend="torch", - sim_params=sim_params, - physics_prim_path=self.cfg.physics_prim_path, - device=self.cfg.device, - ) + def __new__(cls, *args, **kwargs) -> SimulationContext: + """Returns the instance of the simulation context. + + This function is used to create a singleton instance of the simulation context. + If the instance already exists, it returns the previously defined one. Otherwise, + it creates a new instance and returns it + """ + if cls._instance is None: + cls._instance = super().__new__(cls) else: - super().__init__( - stage_units_in_meters=1.0, - physics_dt=self.cfg.dt, - rendering_dt=self.cfg.dt * self.cfg.render_interval, - backend="torch", - sim_params=sim_params, - physics_prim_path=self.cfg.physics_prim_path, - device=self.cfg.device, - stage=self._initial_stage, - ) + logger.debug("Returning the previously defined instance of Simulation Context.") + return cls._instance # type: ignore + + """ + Operations - Singleton. + """ + + @classmethod + def instance(cls) -> SimulationContext | None: + """Returns the instance of the simulation context. + + Returns: + The instance of the simulation context. None if the instance is not initialized. + """ + return cls._instance + + @classmethod + def clear_instance(cls) -> None: + """Delete the simulation context singleton instance.""" + if cls._instance is not None: + # check if the instance is initialized + if not cls._is_initialized: + logger.warning("Simulation context is not initialized. Unable to clear the instance.") + return + # clear the callback + if cls._instance._app_control_on_stop_handle is not None: + cls._instance._app_control_on_stop_handle.unsubscribe() + cls._instance._app_control_on_stop_handle = None + # detach the stage from physx + if cls._instance._physx_sim_iface is not None: + cls._instance._physx_sim_iface.detach_stage() + # detach the stage from the USD stage cache + stage_cache = UsdUtils.StageCache.Get() + stage_id = stage_cache.GetId(cls._instance._initial_stage).ToLongInt() + if stage_id > 0: + stage_cache.Erase(cls._instance._initial_stage) + # clear the instance and the flag + cls._instance = None + cls._is_initialized = False """ - Properties - Override. + Properties. """ + @property + def app(self) -> omni.kit.app.IApp: + """Omniverse Kit Application interface.""" + return self._app_iface + + @property + def stage(self) -> Usd.Stage: + """USD Stage.""" + return self._initial_stage + @property def device(self) -> str: """Device used by the simulation. @@ -316,8 +375,13 @@ def device(self) -> str: """ return self._physics_device + @property + def physics_sim_view(self) -> omni.physics.tensors.SimulationView: + """Physics simulation view with torch backend.""" + return SimulationManager.get_physics_sim_view() + """ - Operations - New. + Operations - Simulation Information. """ def has_gui(self) -> bool: @@ -340,7 +404,7 @@ def has_rtx_sensors(self) -> bool: .. _NVIDIA RTX documentation: https://developer.nvidia.com/rendering-technologies """ - return self._settings.get_as_bool("/isaaclab/render/rtx_sensors") + return self.carb_settings.get_as_bool("/isaaclab/render/rtx_sensors") def is_fabric_enabled(self) -> bool: """Returns whether the fabric interface is enabled. @@ -379,8 +443,32 @@ def get_version(self) -> tuple[int, int, int]: """ return get_isaac_sim_version().major, get_isaac_sim_version().minor, get_isaac_sim_version().micro + def get_initial_stage(self) -> Usd.Stage: + """Returns stage handle used during scene creation. + + Returns: + The stage used during scene creation. + """ + return self._initial_stage + + def get_physics_dt(self) -> float: + """Returns the physics time step of the simulation. + + Returns: + The physics time step of the simulation. + """ + return self.cfg.dt + + def get_rendering_dt(self) -> float: + """Returns the rendering time step of the simulation. + + Returns: + The rendering time step of the simulation. + """ + return self.cfg.dt * self.cfg.render_interval + """ - Operations - New utilities. + Operations - Utilities. """ def set_camera_view( @@ -416,8 +504,8 @@ def set_render_mode(self, mode: RenderMode): change the render mode. Args: - mode (RenderMode): The rendering mode. If different than SimulationContext's rendering mode, - SimulationContext's mode is changed to the new mode. + mode: The rendering mode. If different than SimulationContext's rendering mode, + SimulationContext's mode is changed to the new mode. Raises: ValueError: If the input mode is not supported. @@ -491,38 +579,106 @@ def get_setting(self, name: str) -> Any: """ return self.carb_settings.get(name) - def get_initial_stage(self) -> Usd.Stage: - """Returns stage handle used during scene creation. + """ + Operations- Timeline. + """ + + def is_playing(self) -> bool: + """Check whether the simulation is playing. Returns: - The stage used during scene creation. + True if the simulator is playing. """ - return self._initial_stage + return self._timeline_iface.is_playing() + + def is_stopped(self) -> bool: + """Check whether the simulation is stopped. + + Returns: + True if the simulator is stopped. + """ + return self._timeline_iface.is_stopped() + + def play(self) -> None: + """Start playing the simulation.""" + # play the simulation + self._timeline_iface.play() + self._timeline_iface.commit() + # perform one step to propagate all physics handles properly + self.set_setting("/app/player/playSimulations", False) + self._app_iface.update() + self.set_setting("/app/player/playSimulations", True) + # check for callback exceptions + self._check_for_callback_exceptions() + + def pause(self) -> None: + """Pause the simulation.""" + # pause the simulation + self._timeline_iface.pause() + self._timeline_iface.commit() + # we don't need to propagate physics handles since no callbacks are triggered during pause + # check for callback exceptions + self._check_for_callback_exceptions() + + def stop(self) -> None: + """Stop the simulation. + + Note: + Stopping the simulation will lead to the simulation state being lost. + """ + # stop the simulation + self._timeline_iface.stop() + self._timeline_iface.commit() + # perform one step to propagate all physics handles properly + self.set_setting("/app/player/playSimulations", False) + self._app_iface.update() + self.set_setting("/app/player/playSimulations", True) + # check for callback exceptions + self._check_for_callback_exceptions() """ Operations - Override (standalone) """ def reset(self, soft: bool = False): - self._disable_app_control_on_stop_handle = True - # check if we need to raise an exception that was raised in a callback - if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: - exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION - builtins.ISAACLAB_CALLBACK_EXCEPTION = None - raise exception_to_raise - super().reset(soft=soft) + """Reset the simulation. + + .. warning:: + + This method is not intended to be used in the Isaac Sim's Extensions workflow since the Kit application + has the control over the rendering steps. For the Extensions workflow use the ``reset_async`` method instead + + Args: + soft (bool, optional): if set to True simulation won't be stopped and start again. It only calls the reset on the scene objects. + + """ + # reset the simulation + if not soft: + # disable app control on stop handle + self._disable_app_control_on_stop_handle = True + if not self.is_stopped(): + self.stop() + self._disable_app_control_on_stop_handle = False + # play the simulation + self.play() + # initialize the physics simulation + if SimulationManager.get_physics_sim_view() is None: + SimulationManager.initialize_physics() + # check for callback exceptions + self._check_for_callback_exceptions() + # app.update() may be changing the cuda device in reset, so we force it back to our desired device here if "cuda" in self.device: torch.cuda.set_device(self.device) + # enable kinematic rendering with fabric - if self.physics_sim_view: + if self.physics_sim_view is not None: self.physics_sim_view._backend.initialize_kinematic_bodies() # perform additional rendering steps to warm up replicator buffers # this is only needed for the first time we set the simulation if not soft: for _ in range(2): self.render() - self._disable_app_control_on_stop_handle = False def forward(self) -> None: """Updates articulation kinematics and fabric for rendering.""" @@ -542,18 +698,12 @@ def step(self, render: bool = True): render: Whether to render the scene after stepping the physics simulation. If set to False, the scene is not rendered and only the physics simulation is stepped. """ - # check if we need to raise an exception that was raised in a callback - if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: - exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION - builtins.ISAACLAB_CALLBACK_EXCEPTION = None - raise exception_to_raise - # update anim recording if needed if self._anim_recording_enabled: is_anim_recording_finished = self._update_anim_recording() if is_anim_recording_finished: - logger.warning("[INFO][SimulationContext]: Animation recording finished. Closing app.") - self._app.shutdown() + logger.warning("Animation recording finished. Closing app.") + self._app_iface.shutdown() # check if the simulation timeline is paused. in that case keep stepping until it is playing if not self.is_playing(): @@ -567,10 +717,14 @@ def step(self, render: bool = True): # reason: physics has to parse the scene again and inform other extensions like hydra-delegate. # without this the app becomes unresponsive. # FIXME: This steps physics as well, which we is not good in general. - self.app.update() + self._app_iface.update() # step the simulation - super().step(render=render) + if render: + self._app_iface.update() + else: + self._physx_sim_iface.simulate(self.cfg.dt, 0.0) + self._physx_sim_iface.fetch_results() # app.update() may be changing the cuda device in step, so we force it back to our desired device here if "cuda" in self.device: @@ -588,11 +742,6 @@ def render(self, mode: RenderMode | None = None): Args: mode: The rendering mode. Defaults to None, in which case the current rendering mode is used. """ - # check if we need to raise an exception that was raised in a callback - if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: - exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION - builtins.ISAACLAB_CALLBACK_EXCEPTION = None - raise exception_to_raise # check if we need to change the render mode if mode is not None: self.set_render_mode(mode) @@ -606,9 +755,8 @@ def render(self, mode: RenderMode | None = None): if self._render_throttle_counter % self._render_throttle_period == 0: self._render_throttle_counter = 0 # here we don't render viewport so don't need to flush fabric data - # note: we don't call super().render() anymore because they do flush the fabric data self.set_setting("/app/player/playSimulations", False) - self._app.update() + self._app_iface.update() self.set_setting("/app/player/playSimulations", True) else: # manually flush the fabric data to update Hydra textures @@ -617,68 +765,87 @@ def render(self, mode: RenderMode | None = None): # note: we don't call super().render() anymore because they do above operation inside # and we don't want to do it twice. We may remove it once we drop support for Isaac Sim 2022.2. self.set_setting("/app/player/playSimulations", False) - self._app.update() + self._app_iface.update() self.set_setting("/app/player/playSimulations", True) # app.update() may be changing the cuda device, so we force it back to our desired device here if "cuda" in self.device: torch.cuda.set_device(self.device) - """ - Operations - Override (extension) - """ + @classmethod + def clear(cls): + """Clear the current USD stage.""" - async def reset_async(self, soft: bool = False): - # need to load all "physics" information from the USD file - if not soft: - omni.physx.acquire_physx_interface().force_load_physics_from_usd() - # play the simulation - await super().reset_async(soft=soft) + def _predicate(prim: Usd.Prim) -> bool: + """Check if the prim should be deleted. - """ - Initialization/Destruction - Override. - """ - - def _init_stage(self, *args, **kwargs) -> Usd.Stage: - _ = super()._init_stage(*args, **kwargs) - with sim_utils.use_stage(self.get_initial_stage()): - # a stage update here is needed for the case when physics_dt != rendering_dt, otherwise the app crashes - # when in headless mode - self.set_setting("/app/player/playSimulations", False) - self._app.update() - self.set_setting("/app/player/playSimulations", True) - # set additional physx parameters and bind material - self._set_additional_physx_params() - # load flatcache/fabric interface - self._load_fabric_interface() - # return the stage - return self.stage - - async def _initialize_stage_async(self, *args, **kwargs) -> Usd.Stage: - await super()._initialize_stage_async(*args, **kwargs) - # set additional physx parameters and bind material - self._set_additional_physx_params() - # load flatcache/fabric interface - self._load_fabric_interface() - # return the stage - return self.stage + It adds a check for '/World' and 'PhysicsScene' prims. + """ + if prim.GetPath().pathString == "/World": + return False + if prim.GetTypeName() == "PhysicsScene": + return False + return True - @classmethod - def clear_instance(cls): - # clear the callback + # clear the stage if cls._instance is not None: - if cls._instance._app_control_on_stop_handle is not None: - cls._instance._app_control_on_stop_handle.unsubscribe() - cls._instance._app_control_on_stop_handle = None - # call parent to clear the instance - super().clear_instance() + stage = cls._instance._initial_stage + sim_utils.clear_stage(stage=stage, predicate=_predicate) + else: + logger.error("Simulation context is not initialized. Unable to clear the stage.") """ Helper Functions """ + def _configure_simulation_dt(self): + """Configures the simulation step size based on the physics and rendering step sizes.""" + # if rendering is called the substeps term is used to determine + # how many physics steps to perform per rendering step. + # it is not used if step(render=False). + render_interval = max(self.cfg.render_interval, 1) + + # set simulation step per second + steps_per_second = int(1.0 / self.cfg.dt) + self._physx_scene_api.CreateTimeStepsPerSecondAttr(steps_per_second) + # set minimum number of steps per frame + min_steps = int(steps_per_second / render_interval) + self.carb_settings.set_int("/persistent/simulation/minFrameRate", min_steps) + + # compute rendering frequency + rendering_hz = int(1.0 / (self.cfg.dt * render_interval)) + + # If rate limiting is enabled, set the rendering rate to the specified value + # Otherwise run the app as fast as possible and do not specify the target rate + if self.carb_settings.get("/app/runLoops/main/rateLimitEnabled"): + self.carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) + self._timeline_iface.set_target_framerate(rendering_hz) + with Usd.EditContext(self._initial_stage, self._initial_stage.GetRootLayer()): + self._initial_stage.SetTimeCodesPerSecond(rendering_hz) + self._timeline_iface.set_time_codes_per_second(rendering_hz) + # The isaac sim loop runner is enabled by default in isaac sim apps, + # but in case we are in an app with the kit loop runner, protect against this + try: + import omni.kit.loop._loop as omni_loop + + _loop_runner = omni_loop.acquire_loop_interface() + _loop_runner.set_manual_step_size(self.cfg.dt * render_interval) + _loop_runner.set_manual_mode(True) + except Exception: + self.logger.warning( + "Isaac Sim loop runner not found, enabling rate limiting to support rendering at specified rate" + ) + self.carb_settings.set_bool("/app/runLoops/main/rateLimitEnabled", True) + self.carb_settings.set_int("/app/runLoops/main/rateLimitFrequency", rendering_hz) + self._timeline_iface.set_target_framerate(rendering_hz) + def _apply_physics_settings(self): """Sets various carb physics settings.""" + + # -------------------------- + # Carb Physics API settings + # -------------------------- + # enable hydra scene-graph instancing # note: this allows rendering of instanceable assets on the GUI self.carb_settings.set_bool("/persistent/omnihydra/useSceneGraphInstancing", True) @@ -705,6 +872,108 @@ def _apply_physics_settings(self): # hide the Simulation Settings window self.carb_settings.set_bool("/physics/autoPopupSimulationOutputWindow", False) + # handle device settings + if "cuda" in self.cfg.device: + parsed_device = self.cfg.device.split(":") + if len(parsed_device) == 1: + # if users only specified "cuda", we check if carb settings provide a valid device id + # otherwise, we default to device id 0 + device_id = self.carb_settings.get_as_int("/physics/cudaDevice") + if device_id < 0: + self.carb_settings.set_int("/physics/cudaDevice", 0) + device_id = 0 + else: + # if users specified "cuda:N", we use the provided device id + device_id = int(parsed_device[1]) + self.carb_settings.set_int("/physics/cudaDevice", device_id) + # suppress readback for GPU physics + self.carb_settings.set_bool("/physics/suppressReadback", True) + # save the device + self._physics_device = f"cuda:{device_id}" + else: + # enable USD read/write operations for CPU physics + self.carb_settings.set_int("/physics/cudaDevice", -1) + self.carb_settings.set_bool("/physics/suppressReadback", False) + # save the device + self._physics_device = "cpu" + + # -------------------------- + # USDPhysics API settings + # -------------------------- + + # set gravity + gravity = self._gravity_tensor + gravity_magnitude = torch.norm(gravity).item() + # avoid division by zero + if gravity_magnitude == 0.0: + gravity_magnitude = 1.0 + gravity_direction = gravity / gravity_magnitude + gravity_direction = gravity_direction.tolist() + + self._physics_scene.CreateGravityDirectionAttr(Gf.Vec3f(*gravity_direction)) + self._physics_scene.CreateGravityMagnitudeAttr(gravity_magnitude) + + # create the default physics material + # this material is used when no material is specified for a primitive + material_path = f"{self.cfg.physics_prim_path}/defaultMaterial" + self.cfg.physics_material.func(material_path, self.cfg.physics_material) + # bind the physics material to the scene + sim_utils.bind_physics_material(self.cfg.physics_prim_path, material_path) + + # -------------------------- + # PhysX API settings + # -------------------------- + + # set broadphase type + broadphase_type = "GPU" if "cuda" in self.cfg.device else "MBP" + self._physx_scene_api.CreateBroadphaseTypeAttr(broadphase_type) + # set gpu dynamics + enable_gpu_dynamics = "cuda" in self.cfg.device + self._physx_scene_api.CreateEnableGPUDynamicsAttr(enable_gpu_dynamics) + + # GPU-dynamics does not support CCD, so we disable it if it is enabled. + if enable_gpu_dynamics and self.cfg.physx.enable_ccd: + self.cfg.physx.enable_ccd = False + self.logger.warning( + "CCD is disabled when GPU dynamics is enabled. Please disable CCD in the PhysxCfg config to avoid this" + " warning." + ) + self._physx_scene_api.CreateEnableCCDAttr(self.cfg.physx.enable_ccd) + + # set solver type + solver_type = "PGS" if self.cfg.physx.solver_type == 0 else "TGS" + self._physx_scene_api.CreateSolverTypeAttr(solver_type) + + # set solve articulation contact last + attr = self._physx_scene_api.GetPrim().CreateAttribute( + "physxScene:solveArticulationContactLast", Sdf.ValueTypeNames.Bool + ) + attr.Set(self.cfg.physx.solve_articulation_contact_last) + + # iterate over all the settings and set them + for key, value in self.cfg.physx.to_dict().items(): # type: ignore + if key in ["solver_type", "enable_ccd", "solve_articulation_contact_last"]: + continue + if key == "bounce_threshold_velocity": + key = "bounce_threshold" + sim_utils.safe_set_attribute_on_usd_schema(self._physx_scene_api, key, value, camel_case=True) + + # throw warnings for helpful guidance + if self.cfg.physx.solver_type == 1 and not self.cfg.physx.enable_external_forces_every_iteration: + logger.warning( + "The `enable_external_forces_every_iteration` parameter in the PhysxCfg is set to False. If you are" + " experiencing noisy velocities, consider enabling this flag. You may need to slightly increase the" + " number of velocity iterations (setting it to 1 or 2 rather than 0), together with this flag, to" + " improve the accuracy of velocity updates." + ) + + if not self.cfg.physx.enable_stabilization and self.cfg.dt > 0.0333: + self.logger.warning( + "Large simulation step size (> 0.0333 seconds) is not recommended without enabling stabilization." + " Consider setting the `enable_stabilization` flag to True in the PhysxCfg, or reducing the" + " simulation step size if you run into physics issues." + ) + def _apply_render_settings_from_cfg(self): # noqa: C901 """Sets rtx settings specified in the RenderCfg.""" @@ -799,74 +1068,16 @@ def _apply_render_settings_from_cfg(self): # noqa: C901 if self.carb_settings.get("/rtx/rendermode").lower() == "raytracedlighting": self.carb_settings.set_string("/rtx/rendermode", "RaytracedLighting") - def _set_additional_physx_params(self): - """Sets additional PhysX parameters that are not directly supported by the parent class.""" - # obtain the physics scene api - physics_scene: UsdPhysics.Scene = self._physics_context._physics_scene - physx_scene_api: PhysxSchema.PhysxSceneAPI = self._physics_context._physx_scene_api - # assert that scene api is not None - if physx_scene_api is None: - raise RuntimeError("Physics scene API is None! Please create the scene first.") - # set parameters not directly supported by the constructor - # -- Continuous Collision Detection (CCD) - # ref: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/AdvancedCollisionDetection.html?highlight=ccd#continuous-collision-detection - self._physics_context.enable_ccd(self.cfg.physx.enable_ccd) - # -- GPU collision stack size - physx_scene_api.CreateGpuCollisionStackSizeAttr(self.cfg.physx.gpu_collision_stack_size) - # -- Improved determinism by PhysX - physx_scene_api.CreateEnableEnhancedDeterminismAttr(self.cfg.physx.enable_enhanced_determinism) - # -- Set solve_articulation_contact_last by add attribute to the PhysxScene prim, and add attribute there. - physx_prim = physx_scene_api.GetPrim() - physx_prim.CreateAttribute("physxScene:solveArticulationContactLast", Sdf.ValueTypeNames.Bool).Set( - self.cfg.physx.solve_articulation_contact_last - ) - # -- Enable external forces every iteration, helps improve the accuracy of velocity updates. - - if self.cfg.physx.solver_type == 1: - if not self.cfg.physx.enable_external_forces_every_iteration: - logger.warning( - "The `enable_external_forces_every_iteration` parameter in the PhysxCfg is set to False. If you are" - " experiencing noisy velocities, consider enabling this flag. You may need to slightly increase the" - " number of velocity iterations (setting it to 1 or 2 rather than 0), together with this flag, to" - " improve the accuracy of velocity updates." - ) - physx_scene_api.CreateEnableExternalForcesEveryIterationAttr( - self.cfg.physx.enable_external_forces_every_iteration - ) - - # -- Gravity - # note: Isaac sim only takes the "up-axis" as the gravity direction. But physics allows any direction so we - # need to convert the gravity vector to a direction and magnitude pair explicitly. - gravity = np.asarray(self.cfg.gravity) - gravity_magnitude = np.linalg.norm(gravity) - - # Avoid division by zero - if gravity_magnitude != 0.0: - gravity_direction = gravity / gravity_magnitude - else: - gravity_direction = gravity - - physics_scene.CreateGravityDirectionAttr(Gf.Vec3f(*gravity_direction)) - physics_scene.CreateGravityMagnitudeAttr(gravity_magnitude) - - # position iteration count - physx_scene_api.CreateMinPositionIterationCountAttr(self.cfg.physx.min_position_iteration_count) - physx_scene_api.CreateMaxPositionIterationCountAttr(self.cfg.physx.max_position_iteration_count) - # velocity iteration count - physx_scene_api.CreateMinVelocityIterationCountAttr(self.cfg.physx.min_velocity_iteration_count) - physx_scene_api.CreateMaxVelocityIterationCountAttr(self.cfg.physx.max_velocity_iteration_count) - - # create the default physics material - # this material is used when no material is specified for a primitive - # check: https://isaac-sim.github.io/IsaacLab/main/source/api/lab/isaaclab.sim.html#isaaclab.sim.SimulationCfg.physics_material - material_path = f"{self.cfg.physics_prim_path}/defaultMaterial" - self.cfg.physics_material.func(material_path, self.cfg.physics_material) - # bind the physics material to the scene - bind_physics_material(self.cfg.physics_prim_path, material_path) - def _load_fabric_interface(self): """Loads the fabric interface if enabled.""" + extension_manager = omni.kit.app.get_app().get_extension_manager() + fabric_enabled = extension_manager.is_extension_enabled("omni.physx.fabric") + if self.cfg.use_fabric: + if not fabric_enabled: + extension_manager.set_extension_enabled_immediate("omni.physx.fabric", True) + + # load fabric interface from omni.physxfabric import get_physx_fabric_interface # acquire fabric interface @@ -880,6 +1091,38 @@ def _load_fabric_interface(self): else: # Needed for backward compatibility with older Isaac Sim versions self._update_fabric = self._fabric_iface.update + else: + if fabric_enabled: + extension_manager.set_extension_enabled_immediate("omni.physx.fabric", False) + # set fabric interface to None + self._fabric_iface = None + + # set carb settings for fabric + self.carb_settings.set_bool("/physics/fabricEnabled", self.cfg.use_fabric) + self.carb_settings.set_bool("/physics/updateToUsd", not self.cfg.use_fabric) + self.carb_settings.set_bool("/physics/updateParticlesToUsd", not self.cfg.use_fabric) + self.carb_settings.set_bool("/physics/updateVelocitiesToUsd", not self.cfg.use_fabric) + self.carb_settings.set_bool("/physics/updateForceSensorsToUsd", not self.cfg.use_fabric) + self.carb_settings.set_bool("/physics/updateResidualsToUsd", not self.cfg.use_fabric) + # disable simulation output window visibility + self.carb_settings.set_bool("/physics/visualizationDisplaySimulationOutput", False) + + def _check_for_callback_exceptions(self): + """Checks for callback exceptions and raises them if found.""" + # disable simulation stopping control so that we can crash the program + # if an exception is raised in a callback. + self._disable_app_control_on_stop_handle = True + # check if we need to raise an exception that was raised in a callback + if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: # type: ignore + exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION + builtins.ISAACLAB_CALLBACK_EXCEPTION = None # type: ignore + raise exception_to_raise + # re-enable simulation stopping control + self._disable_app_control_on_stop_handle = False + + """ + Helper functions - Animation Recording. + """ def _update_anim_recording(self): """Tracks anim recording timestamps and triggers finish animation recording if the total time has elapsed.""" @@ -998,7 +1241,7 @@ def _finish_anim_recording(self): Callbacks. """ - def _app_control_on_stop_handle_fn(self, event: carb.events.IEvent): + def _app_control_on_stop_handle_fn(self, _): """Callback to deal with the app when the simulation is stopped. Once the simulation is stopped, the physics handles go invalid. After that, it is not possible to @@ -1015,10 +1258,10 @@ def _app_control_on_stop_handle_fn(self, event: carb.events.IEvent): This callback is used only when running the simulation in a standalone python script. In an extension, it is expected that the user handles the extension shutdown. """ - if not self._disable_app_control_on_stop_handle: - while not omni.timeline.get_timeline_interface().is_playing(): - self.render() - return + pass + # if not self._disable_app_control_on_stop_handle: + # while not omni.timeline.get_timeline_interface().is_playing(): + # self.render() @contextmanager @@ -1115,10 +1358,7 @@ def build_simulation_context( sim.stop() # Clear the stage - sim.clear_all_callbacks() sim.clear_instance() - # check if we need to raise an exception that was raised in a callback - if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: - exception_to_raise = builtins.ISAACLAB_CALLBACK_EXCEPTION - builtins.ISAACLAB_CALLBACK_EXCEPTION = None - raise exception_to_raise + # Check if we need to raise an exception that was raised in a callback + if builtins.ISAACLAB_CALLBACK_EXCEPTION is not None: # type: ignore + raise builtins.ISAACLAB_CALLBACK_EXCEPTION # type: ignore diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py index 25658786467..e619de1a142 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py @@ -59,14 +59,18 @@ def spawn_preview_surface(prim_path: str, cfg: visual_materials_cfg.PreviewSurfa # since stage in memory is not supported by the "CreatePreviewSurfaceMaterialPrim" kit command attach_stage_to_usd_context(attaching_early=True) + # note: this command does not support passing 'stage' as an argument omni.kit.commands.execute("CreatePreviewSurfaceMaterialPrim", mtl_path=prim_path, select_new_prim=False) else: raise ValueError(f"A prim already exists at path: '{prim_path}'.") # obtain prim prim = stage.GetPrimAtPath(f"{prim_path}/Shader") + # check prim is valid + if not prim.IsValid(): + raise ValueError(f"Failed to create preview surface material at path: '{prim_path}'.") # apply properties - cfg = cfg.to_dict() + cfg = cfg.to_dict() # type: ignore del cfg["func"] for attr_name, attr_value in cfg.items(): safe_set_attribute_on_usd_prim(prim, f"inputs:{attr_name}", attr_value, camel_case=True) @@ -108,10 +112,6 @@ def spawn_from_mdl_file(prim_path: str, cfg: visual_materials_cfg.MdlMaterialCfg # spawn material if it doesn't exist. if not stage.GetPrimAtPath(prim_path).IsValid(): - # early attach stage to usd context if stage is in memory - # since stage in memory is not supported by the "CreateMdlMaterialPrim" kit command - attach_stage_to_usd_context(attaching_early=True) - # extract material name from path material_name = cfg.mdl_path.split("/")[-1].split(".")[0] omni.kit.commands.execute( @@ -119,12 +119,16 @@ def spawn_from_mdl_file(prim_path: str, cfg: visual_materials_cfg.MdlMaterialCfg mtl_url=cfg.mdl_path.format(NVIDIA_NUCLEUS_DIR=NVIDIA_NUCLEUS_DIR), mtl_name=material_name, mtl_path=prim_path, + stage=stage, select_new_prim=False, ) else: raise ValueError(f"A prim already exists at path: '{prim_path}'.") # obtain prim prim = stage.GetPrimAtPath(f"{prim_path}/Shader") + # check prim is valid + if not prim.IsValid(): + raise ValueError(f"Failed to create MDL material at path: '{prim_path}'.") # apply properties cfg = cfg.to_dict() del cfg["func"] diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index a81ccd2232e..3dca1deac96 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -211,9 +211,15 @@ def delete_prim(prim_path: str | Sequence[str], stage: Usd.Stage | None = None) stage_cache = UsdUtils.StageCache.Get() stage_id = stage_cache.GetId(stage).ToLongInt() if stage_id < 0: + is_stage_inserted = True stage_id = stage_cache.Insert(stage).ToLongInt() + else: + is_stage_inserted = False # delete prims DeletePrimsCommand(prim_path, stage=stage).do() + # erase from cache to prevent memory leaks + if is_stage_inserted: + stage_cache.Erase(stage) def move_prim(path_from: str, path_to: str, keep_world_transform: bool = True, stage: Usd.Stage | None = None) -> None: diff --git a/source/isaaclab/isaaclab/sim/utils/stage.py b/source/isaaclab/isaaclab/sim/utils/stage.py index b4a580fffe4..ecdfca45526 100644 --- a/source/isaaclab/isaaclab/sim/utils/stage.py +++ b/source/isaaclab/isaaclab/sim/utils/stage.py @@ -5,7 +5,8 @@ """Utilities for operating on the USD stage.""" -import builtins +from __future__ import annotations + import contextlib import logging import threading @@ -289,66 +290,67 @@ def close_stage(callback_fn: Callable[[bool, str], None] | None = None) -> bool: return result -def clear_stage(predicate: Callable[[Usd.Prim], bool] | None = None) -> None: +def clear_stage(stage: Usd.Stage | None = None, predicate: Callable[[Usd.Prim], bool] | None = None) -> None: """Deletes all prims in the stage without populating the undo command buffer. The function will delete all prims in the stage that satisfy the predicate. If the predicate - is None, a default predicate will be used that deletes all prims. The default predicate deletes - all prims that are not the root prim, are not under the /Render namespace, have the ``no_delete`` - metadata, are not ancestral to any other prim, and are not hidden in the stage window. + is None, a default predicate will be used that deletes all prims except those that meet any + of the following criteria: + + * The root prim (``"/"``) or prims under the ``/Render`` namespace + * Prims with the ``no_delete`` metadata set + * Prims with the ``hide_in_stage_window`` metadata set + * Ancestral prims (prims that are ancestors of other prims in a reference/payload chain) Args: - predicate: A user defined function that takes the USD prim as an argument and - returns a boolean indicating if the prim should be deleted. If the predicate is None, - a default predicate will be used that deletes all prims. + stage: The stage to clear. Defaults to None, in which case the current stage is used. + predicate: A user-defined function that takes a USD prim as an argument and + returns a boolean indicating if the prim should be deleted. If None, all + prims will be considered for deletion (subject to the default exclusions above). + If provided, both the default exclusions and the predicate must be satisfied + for a prim to be deleted. Example: >>> import isaaclab.sim as sim_utils >>> - >>> # clear the whole stage + >>> # Clear the whole stage (except protected prims) >>> sim_utils.clear_stage() >>> - >>> # given the stage: /World/Cube, /World/Cube_01, /World/Cube_02. + >>> # Given the stage: /World/Cube, /World/Cube_01, /World/Cube_02 >>> # Delete only the prims of type Cube >>> predicate = lambda _prim: _prim.GetTypeName() == "Cube" - >>> sim_utils.clear_stage(predicate) # after the execution the stage will be /World + >>> sim_utils.clear_stage(predicate) # After execution, the stage will only have /World """ # Note: Need to import this here to prevent circular dependencies. from .prims import delete_prim from .queries import get_all_matching_child_prims - def _default_predicate(prim: Usd.Prim) -> bool: + def _predicate(prim: Usd.Prim) -> bool: """Check if the prim should be deleted.""" prim_path = prim.GetPath().pathString - if prim_path == "/": - return False - if prim_path.startswith("/Render"): - return False - if prim.GetMetadata("no_delete"): + # Skip the root prim + if prim_path == "/" or prim_path == "/Render": return False - if prim.GetMetadata("hide_in_stage_window"): + # Skip prims with the "no_delete" metadata + if prim.GetMetadata("no_delete") or prim.GetMetadata("hide_in_stage_window"): return False + # Skip ancestral prims if omni.usd.check_ancestral(prim): return False - return True + # Additional predicate check + return predicate is None or predicate(prim) - def _predicate_from_path(prim: Usd.Prim) -> bool: - if predicate is None: - return _default_predicate(prim) - return predicate(prim) + # get stage handle + stage = get_current_stage() if stage is None else stage # get all prims to delete - if predicate is None: - prims = get_all_matching_child_prims("/", _default_predicate) - else: - prims = get_all_matching_child_prims("/", _predicate_from_path) + prims = get_all_matching_child_prims("/", _predicate, stage=stage, traverse_instance_prims=False) # convert prims to prim paths prim_paths_to_delete = [prim.GetPath().pathString for prim in prims] # delete prims - delete_prim(prim_paths_to_delete) - - if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: # type: ignore - omni.kit.app.get_app_interface().update() + delete_prim(prim_paths_to_delete, stage) + # update stage + update_stage() def is_stage_loading() -> bool: @@ -465,14 +467,11 @@ def attach_stage_to_usd_context(attaching_early: bool = False): " does not support stage in memory." ) - # skip this callback to avoid wiping the stage after attachment - SimulationContext.instance().skip_next_stage_open_callback() - # disable stage open callback to avoid clearing callbacks SimulationManager.enable_stage_open_callback(False) # enable physics fabric - SimulationContext.instance()._physics_context.enable_fabric(True) # type: ignore + SimulationContext.instance().carb_settings.set_bool("/physics/fabricEnabled", True) # attach stage to usd context omni.usd.get_context().attach_stage_with_callback(stage_id) diff --git a/source/isaaclab/test/controllers/test_differential_ik.py b/source/isaaclab/test/controllers/test_differential_ik.py index 65ce828129f..4c4cc60a055 100644 --- a/source/isaaclab/test/controllers/test_differential_ik.py +++ b/source/isaaclab/test/controllers/test_differential_ik.py @@ -78,7 +78,6 @@ def sim(): # Cleanup sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/controllers/test_operational_space.py b/source/isaaclab/test/controllers/test_operational_space.py index 2533eaeeb59..3c309c14f20 100644 --- a/source/isaaclab/test/controllers/test_operational_space.py +++ b/source/isaaclab/test/controllers/test_operational_space.py @@ -203,7 +203,6 @@ def sim(): # Cleanup sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/scene/test_interactive_scene.py b/source/isaaclab/test/scene/test_interactive_scene.py index 1a42a340baa..cee8cc8b694 100644 --- a/source/isaaclab/test/scene/test_interactive_scene.py +++ b/source/isaaclab/test/scene/test_interactive_scene.py @@ -68,7 +68,6 @@ def make_scene(num_envs: int, env_spacing: float = 1.0): yield make_scene, sim sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_camera.py b/source/isaaclab/test/sensors/test_camera.py index eb3bafa91cf..cecbadd0936 100644 --- a/source/isaaclab/test/sensors/test_camera.py +++ b/source/isaaclab/test/sensors/test_camera.py @@ -77,9 +77,8 @@ def teardown(sim: sim_utils.SimulationContext): rep.vp_manager.destroy_hydra_textures("Replicator") # stop simulation # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - sim._timeline.stop() + sim.stop() # clear the stage - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_frame_transformer.py b/source/isaaclab/test/sensors/test_frame_transformer.py index 725aa7d29fa..6c479a87949 100644 --- a/source/isaaclab/test/sensors/test_frame_transformer.py +++ b/source/isaaclab/test/sensors/test_frame_transformer.py @@ -81,7 +81,6 @@ def sim(): sim.set_camera_view(eye=(5.0, 5.0, 5.0), target=(0.0, 0.0, 0.0)) yield sim # Cleanup - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_imu.py b/source/isaaclab/test/sensors/test_imu.py index d221a182568..38736eef0bd 100644 --- a/source/isaaclab/test/sensors/test_imu.py +++ b/source/isaaclab/test/sensors/test_imu.py @@ -213,7 +213,6 @@ def setup_sim(): sim.reset() yield sim, scene # Cleanup - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py index bfe303ea751..1e46dc741d2 100644 --- a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py +++ b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py @@ -81,9 +81,8 @@ def setup_simulation(): rep.vp_manager.destroy_hydra_textures("Replicator") # stop simulation # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - sim._timeline.stop() + sim.stop() # clear the stage - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_multi_tiled_camera.py b/source/isaaclab/test/sensors/test_multi_tiled_camera.py index 583791de79b..59a1b673759 100644 --- a/source/isaaclab/test/sensors/test_multi_tiled_camera.py +++ b/source/isaaclab/test/sensors/test_multi_tiled_camera.py @@ -60,9 +60,8 @@ def setup_camera(): rep.vp_manager.destroy_hydra_textures("Replicator") # stop simulation # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - sim._timeline.stop() + sim.stop() # clear the stage - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_ray_caster_camera.py b/source/isaaclab/test/sensors/test_ray_caster_camera.py index 319d3cbeed1..4967df35d27 100644 --- a/source/isaaclab/test/sensors/test_ray_caster_camera.py +++ b/source/isaaclab/test/sensors/test_ray_caster_camera.py @@ -84,9 +84,8 @@ def teardown(sim: sim_utils.SimulationContext): rep.vp_manager.destroy_hydra_textures("Replicator") # stop simulation # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - sim._timeline.stop() + sim.stop() # clear the stage - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_sensor_base.py b/source/isaaclab/test/sensors/test_sensor_base.py index 2f21c05970c..dcd16939e44 100644 --- a/source/isaaclab/test/sensors/test_sensor_base.py +++ b/source/isaaclab/test/sensors/test_sensor_base.py @@ -108,9 +108,8 @@ def create_dummy_sensor(request, device): # stop simulation # note: cannot use self.sim.stop() since it does one render step after stopping!! This doesn't make sense :( - sim._timeline.stop() + sim.stop() # clear the stage - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sensors/test_tiled_camera.py b/source/isaaclab/test/sensors/test_tiled_camera.py index 5d37143db22..9dc059a68ab 100644 --- a/source/isaaclab/test/sensors/test_tiled_camera.py +++ b/source/isaaclab/test/sensors/test_tiled_camera.py @@ -59,8 +59,7 @@ def setup_camera(device) -> tuple[sim_utils.SimulationContext, TiledCameraCfg, f yield sim, camera_cfg, dt # Teardown rep.vp_manager.destroy_hydra_textures("Replicator") - sim._timeline.stop() - sim.clear_all_callbacks() + sim.stop() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_mesh_converter.py b/source/isaaclab/test/sim/test_mesh_converter.py index f3d73f92e59..a094f57b002 100644 --- a/source/isaaclab/test/sim/test_mesh_converter.py +++ b/source/isaaclab/test/sim/test_mesh_converter.py @@ -71,7 +71,6 @@ def sim(): sim.stop() # cleanup stage and context sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_mjcf_converter.py b/source/isaaclab/test/sim/test_mjcf_converter.py index 211cf4e6b92..2dd0c3ed156 100644 --- a/source/isaaclab/test/sim/test_mjcf_converter.py +++ b/source/isaaclab/test/sim/test_mjcf_converter.py @@ -48,7 +48,6 @@ def test_setup_teardown(): # Teardown: Cleanup simulation sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_schemas.py b/source/isaaclab/test/sim/test_schemas.py index a89d4dcb534..53437c3a77c 100644 --- a/source/isaaclab/test/sim/test_schemas.py +++ b/source/isaaclab/test/sim/test_schemas.py @@ -75,7 +75,6 @@ def setup_simulation(): # Teardown sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index a9072839e87..02a56af1fe4 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -12,12 +12,13 @@ """Rest everything follows.""" +import builtins import numpy as np import pytest -from collections.abc import Generator +import weakref -import omni.physx -from isaacsim.core.api.simulation_context import SimulationContext as IsaacSimulationContext +import omni.timeline +from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager import isaaclab.sim as sim_utils from isaaclab.sim import SimulationCfg, SimulationContext @@ -33,26 +34,63 @@ def test_setup_teardown(): yield # Teardown: Clear the simulation context after each test + SimulationContext.clear() SimulationContext.clear_instance() -@pytest.fixture -def sim_with_stage_in_memory() -> Generator[SimulationContext, None, None]: - """Create a simulation context with stage in memory.""" - # create stage in memory - cfg = SimulationCfg(create_stage_in_memory=True) +""" +Basic Configuration Tests +""" + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_init(device): + """Test the simulation context initialization.""" + cfg = SimulationCfg(physics_prim_path="/Physics/PhysX", render_interval=5, gravity=(0.0, -0.5, -0.5), device=device) + # sim = SimulationContext(cfg) + # TODO: Figure out why keyword argument doesn't work. + # note: added a fix in Isaac Sim 2023.1 for this. sim = SimulationContext(cfg=cfg) - # update stage - sim_utils.update_stage() - # yield simulation context - yield sim - # stop simulation - omni.physx.get_physx_simulation_interface().detach_stage() - sim.stop() - # clear simulation context - sim.clear() - sim.clear_all_callbacks() - sim.clear_instance() + + # verify app interface is valid + assert sim.app is not None + # verify stage is valid + assert sim.stage is not None + # verify device property + assert sim.device == device + # verify no RTX sensors are available + assert not sim.has_rtx_sensors() + + # obtain physics scene api + physx_scene_api = sim._physx_scene_api # type: ignore + physics_scene = sim._physics_scene # type: ignore + + # check valid settings + physics_hz = physx_scene_api.GetTimeStepsPerSecondAttr().Get() + physics_dt = 1.0 / physics_hz + assert physics_dt == cfg.dt + + # check valid paths + assert sim.stage.GetPrimAtPath("/Physics/PhysX").IsValid() + assert sim.stage.GetPrimAtPath("/Physics/PhysX/defaultMaterial").IsValid() + # check valid gravity + gravity_dir, gravity_mag = ( + physics_scene.GetGravityDirectionAttr().Get(), + physics_scene.GetGravityMagnitudeAttr().Get(), + ) + gravity = np.array(gravity_dir) * gravity_mag + np.testing.assert_almost_equal(gravity, cfg.gravity) + + +@pytest.mark.isaacsim_ci +def test_instance_before_creation(): + """Test accessing instance before creating returns None.""" + # clear any existing instance + SimulationContext.clear_instance() + + # accessing instance before creation should return None + assert SimulationContext.instance() is None @pytest.mark.isaacsim_ci @@ -60,43 +98,22 @@ def test_singleton(): """Tests that the singleton is working.""" sim1 = SimulationContext() sim2 = SimulationContext() - sim3 = IsaacSimulationContext() assert sim1 is sim2 - assert sim1 is sim3 # try to delete the singleton sim2.clear_instance() assert sim1.instance() is None # create new instance - sim4 = SimulationContext() - assert sim1 is not sim4 - assert sim3 is not sim4 - assert sim1.instance() is sim4.instance() - assert sim3.instance() is sim4.instance() + sim3 = SimulationContext() + assert sim1 is not sim3 + assert sim1.instance() is sim3.instance() # clear instance sim3.clear_instance() -@pytest.mark.isaacsim_ci -def test_initialization(): - """Test the simulation config.""" - cfg = SimulationCfg(physics_prim_path="/Physics/PhysX", render_interval=5, gravity=(0.0, -0.5, -0.5)) - sim = SimulationContext(cfg) - # TODO: Figure out why keyword argument doesn't work. - # note: added a fix in Isaac Sim 2023.1 for this. - # sim = SimulationContext(cfg=cfg) - - # check valid settings - assert sim.get_physics_dt() == cfg.dt - assert sim.get_rendering_dt() == cfg.dt * cfg.render_interval - assert not sim.has_rtx_sensors() - # check valid paths - assert sim.stage.GetPrimAtPath("/Physics/PhysX").IsValid() - assert sim.stage.GetPrimAtPath("/Physics/PhysX/defaultMaterial").IsValid() - # check valid gravity - gravity_dir, gravity_mag = sim.get_physics_context().get_gravity() - gravity = np.array(gravity_dir) * gravity_mag - np.testing.assert_almost_equal(gravity, cfg.gravity) +""" +Property Tests. +""" @pytest.mark.isaacsim_ci @@ -126,44 +143,975 @@ def test_headless_mode(): sim = SimulationContext() # check default render mode assert sim.render_mode == sim.RenderMode.NO_GUI_OR_RENDERING + assert not sim.has_gui() -# def test_boundedness(): -# """Test that the boundedness of the simulation context remains constant. -# -# Note: This test fails right now because Isaac Sim does not handle boundedness correctly. On creation, -# it is registering itself to various callbacks and hence the boundedness is more than 1. This may not be -# critical for the simulation context since we usually call various clear functions before deleting the -# simulation context. -# """ -# sim = SimulationContext() -# # manually set the boundedness to 1? -- this is not possible because of Isaac Sim. -# sim.clear_all_callbacks() -# sim._stage_open_callback = None -# sim._physics_timer_callback = None -# sim._event_timer_callback = None -# -# # check that boundedness of simulation context is correct -# sim_ref_count = ctypes.c_long.from_address(id(sim)).value -# # reset the simulation -# sim.reset() -# assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count -# # step the simulation -# for _ in range(10): -# sim.step() -# assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count -# # clear the simulation -# sim.clear_instance() -# assert ctypes.c_long.from_address(id(sim)).value == sim_ref_count - 1 +""" +Timeline Operations Tests. +""" @pytest.mark.isaacsim_ci -def test_zero_gravity(): - """Test that gravity can be properly disabled.""" - cfg = SimulationCfg(gravity=(0.0, 0.0, 0.0)) +def test_timeline_play_stop(): + """Test timeline play and stop operations.""" + sim = SimulationContext() + + # initially simulation should be stopped + assert sim.is_stopped() + assert not sim.is_playing() + + # start the simulation + sim.play() + assert sim.is_playing() + assert not sim.is_stopped() + # disable callback to prevent app from continuing + sim._disable_app_control_on_stop_handle = True # type: ignore + # stop the simulation + sim.stop() + assert sim.is_stopped() + assert not sim.is_playing() + + +@pytest.mark.isaacsim_ci +def test_timeline_pause(): + """Test timeline pause operation.""" + sim = SimulationContext() + + # start the simulation + sim.play() + assert sim.is_playing() + + # pause the simulation + sim.pause() + assert not sim.is_playing() + assert not sim.is_stopped() # paused is different from stopped + + +""" +Reset and Step Tests +""" + + +@pytest.mark.isaacsim_ci +def test_reset(): + """Test simulation reset.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create a simple cube to test with + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # reset the simulation + sim.reset() + + # check that simulation is playing after reset + assert sim.is_playing() + + # check that physics sim view is created + assert sim.physics_sim_view is not None + + +@pytest.mark.isaacsim_ci +def test_reset_soft(): + """Test soft reset (without stopping simulation).""" + cfg = SimulationCfg(dt=0.01) sim = SimulationContext(cfg) - gravity_dir, gravity_mag = sim.get_physics_context().get_gravity() + # create a simple cube + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # perform initial reset + sim.reset() + assert sim.is_playing() + + # perform soft reset + sim.reset(soft=True) + + # simulation should still be playing + assert sim.is_playing() + + +@pytest.mark.isaacsim_ci +def test_forward(): + """Test forward propagation for fabric updates.""" + cfg = SimulationCfg(dt=0.01, use_fabric=True) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + sim.reset() + + # call forward + sim.forward() + + # should not raise any errors + assert sim.is_playing() + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("render", [True, False]) +def test_step(render): + """Test stepping simulation with and without rendering.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + sim.reset() + + # step with rendering + for _ in range(10): + sim.step(render=render) + + # simulation should still be playing + assert sim.is_playing() + + +@pytest.mark.isaacsim_ci +def test_render(): + """Test rendering simulation.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + sim.reset() + + # render + for _ in range(10): + sim.render() + + # simulation should still be playing + assert sim.is_playing() + + +""" +Stage Operations Tests +""" + + +@pytest.mark.isaacsim_ci +def test_get_initial_stage(): + """Test getting the initial stage.""" + sim = SimulationContext() + + # get initial stage + stage = sim.get_initial_stage() + + # verify stage is valid + assert stage is not None + assert stage == sim.stage + + +@pytest.mark.isaacsim_ci +def test_clear_stage(): + """Test clearing the stage.""" + sim = SimulationContext() + + # create some objects + cube_cfg1 = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg1.func("/World/Cube1", cube_cfg1) + cube_cfg2 = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg2.func("/World/Cube2", cube_cfg2) + + # verify objects exist + assert sim.stage.GetPrimAtPath("/World/Cube1").IsValid() + assert sim.stage.GetPrimAtPath("/World/Cube2").IsValid() + + # clear the stage + sim.clear() + + # verify objects are removed but World and Physics remain + assert not sim.stage.GetPrimAtPath("/World/Cube1").IsValid() + assert not sim.stage.GetPrimAtPath("/World/Cube2").IsValid() + assert sim.stage.GetPrimAtPath("/World").IsValid() + assert sim.stage.GetPrimAtPath(sim.cfg.physics_prim_path).IsValid() + + +""" +Physics Configuration Tests +""" + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("solver_type", [0, 1]) # 0=PGS, 1=TGS +def test_solver_type(solver_type): + """Test different solver types.""" + from isaaclab.sim.simulation_cfg import PhysxCfg + + cfg = SimulationCfg(physx=PhysxCfg(solver_type=solver_type)) + sim = SimulationContext(cfg) + + # obtain physics scene api + physx_scene_api = sim._physx_scene_api # type: ignore + # check solver type is set + solver_type_str = "PGS" if solver_type == 0 else "TGS" + assert physx_scene_api.GetSolverTypeAttr().Get() == solver_type_str + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("use_fabric", [True, False]) +def test_fabric_setting(use_fabric): + """Test that fabric setting is properly set.""" + cfg = SimulationCfg(use_fabric=use_fabric) + sim = SimulationContext(cfg) + + # check fabric is enabled + assert sim.is_fabric_enabled() == use_fabric + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("dt", [0.01, 0.02, 0.005]) +def test_physics_dt(dt): + """Test that physics time step is properly configured.""" + cfg = SimulationCfg(dt=dt) + sim = SimulationContext(cfg) + + # obtain physics scene api + physx_scene_api = sim._physx_scene_api # type: ignore + # check physics dt + physics_hz = physx_scene_api.GetTimeStepsPerSecondAttr().Get() + physics_dt = 1.0 / physics_hz + assert abs(physics_dt - dt) < 1e-6 + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("gravity", [(0.0, 0.0, 0.0), (0.0, 0.0, -9.81), (0.5, 0.5, 0.5)]) +def test_custom_gravity(gravity): + """Test that gravity can be properly set.""" + cfg = SimulationCfg(gravity=gravity) + sim = SimulationContext(cfg) + + # obtain physics scene api + physics_scene = sim._physics_scene # type: ignore + + gravity_dir, gravity_mag = ( + physics_scene.GetGravityDirectionAttr().Get(), + physics_scene.GetGravityMagnitudeAttr().Get(), + ) gravity = np.array(gravity_dir) * gravity_mag - np.testing.assert_almost_equal(gravity, cfg.gravity) + np.testing.assert_almost_equal(gravity, cfg.gravity, decimal=6) + + +""" +Callback Tests. +""" + + +@pytest.mark.isaacsim_ci +def test_timeline_callbacks_on_play(): + """Test that timeline callbacks are triggered on play event.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create a simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create a flag to track callback execution + callback_state = {"play_called": False, "stop_called": False} + + # define callback functions + def on_play_callback(event): + callback_state["play_called"] = True + + def on_stop_callback(event): + callback_state["stop_called"] = True + + # register callbacks + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + play_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), + lambda event: on_play_callback(event), + order=20, + ) + stop_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), + lambda event: on_stop_callback(event), + order=20, + ) + + try: + # ensure callbacks haven't been called yet + assert not callback_state["play_called"] + assert not callback_state["stop_called"] + + # play the simulation - this should trigger play callback + sim.play() + assert callback_state["play_called"] + assert not callback_state["stop_called"] + + # reset flags + callback_state["play_called"] = False + + # disable app control to prevent hanging + sim._disable_app_control_on_stop_handle = True # type: ignore + + # stop the simulation - this should trigger stop callback + sim.stop() + assert callback_state["stop_called"] + + finally: + # cleanup callbacks + if play_handle is not None: + play_handle.unsubscribe() + if stop_handle is not None: + stop_handle.unsubscribe() + + +@pytest.mark.isaacsim_ci +def test_timeline_callbacks_with_weakref(): + """Test that timeline callbacks work correctly with weak references (similar to asset_base.py).""" + + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create a simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create a test object that will be weakly referenced + class CallbackTracker: + def __init__(self): + self.play_count = 0 + self.stop_count = 0 + + def on_play(self, event): + self.play_count += 1 + + def on_stop(self, event): + self.stop_count += 1 + + # create an instance of the callback tracker + tracker = CallbackTracker() + + # define safe callback wrapper (similar to asset_base.py pattern) + def safe_callback(callback_name, event, obj_ref): + """Safely invoke a callback on a weakly-referenced object.""" + try: + obj = obj_ref() # Dereference the weakref + if obj is not None: + getattr(obj, callback_name)(event) + except ReferenceError: + # Object has been deleted; ignore + pass + + # register callbacks with weakref + obj_ref = weakref.ref(tracker) + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + + play_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), + lambda event, obj_ref=obj_ref: safe_callback("on_play", event, obj_ref), + order=20, + ) + stop_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), + lambda event, obj_ref=obj_ref: safe_callback("on_stop", event, obj_ref), + order=20, + ) + + try: + # verify callbacks haven't been called + assert tracker.play_count == 0 + assert tracker.stop_count == 0 + + # trigger play event + sim.play() + assert tracker.play_count == 1 + assert tracker.stop_count == 0 + + # disable app control to prevent hanging + sim._disable_app_control_on_stop_handle = True # type: ignore + + # trigger stop event + sim.stop() + assert tracker.play_count == 1 + assert tracker.stop_count == 1 + + # delete the tracker object + del tracker + + # trigger events again - callbacks should handle the deleted object gracefully + sim.play() + # disable app control again + sim._disable_app_control_on_stop_handle = True # type: ignore + sim.stop() + # should not raise any errors + + finally: + # cleanup callbacks + if play_handle is not None: + play_handle.unsubscribe() + if stop_handle is not None: + stop_handle.unsubscribe() + + +@pytest.mark.isaacsim_ci +def test_multiple_callbacks_on_same_event(): + """Test that multiple callbacks can be registered for the same event.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create tracking for multiple callbacks + callback_counts = {"callback1": 0, "callback2": 0, "callback3": 0} + + def callback1(event): + callback_counts["callback1"] += 1 + + def callback2(event): + callback_counts["callback2"] += 1 + + def callback3(event): + callback_counts["callback3"] += 1 + + # register multiple callbacks for play event + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + handle1 = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: callback1(event), order=20 + ) + handle2 = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: callback2(event), order=21 + ) + handle3 = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: callback3(event), order=22 + ) + + try: + # verify none have been called + assert all(count == 0 for count in callback_counts.values()) + + # trigger play event + sim.play() + + # all callbacks should have been called + assert callback_counts["callback1"] == 1 + assert callback_counts["callback2"] == 1 + assert callback_counts["callback3"] == 1 + + finally: + # cleanup all callbacks + if handle1 is not None: + handle1.unsubscribe() + if handle2 is not None: + handle2.unsubscribe() + if handle3 is not None: + handle3.unsubscribe() + + +@pytest.mark.isaacsim_ci +def test_callback_execution_order(): + """Test that callbacks are executed in the correct order based on priority.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # track execution order + execution_order = [] + + def callback_low_priority(event): + execution_order.append("low") + + def callback_medium_priority(event): + execution_order.append("medium") + + def callback_high_priority(event): + execution_order.append("high") + + # register callbacks with different priorities (lower order = higher priority) + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + handle_high = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: callback_high_priority(event), order=5 + ) + handle_medium = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: callback_medium_priority(event), order=10 + ) + handle_low = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: callback_low_priority(event), order=15 + ) + + try: + # trigger play event + sim.play() + + # verify callbacks were executed in correct order + assert len(execution_order) == 3 + assert execution_order[0] == "high" + assert execution_order[1] == "medium" + assert execution_order[2] == "low" + + finally: + # cleanup callbacks + if handle_high is not None: + handle_high.unsubscribe() + if handle_medium is not None: + handle_medium.unsubscribe() + if handle_low is not None: + handle_low.unsubscribe() + + +@pytest.mark.isaacsim_ci +def test_callback_unsubscribe(): + """Test that unsubscribing callbacks works correctly.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create callback counter + callback_count = {"count": 0} + + def on_play_callback(event): + callback_count["count"] += 1 + + # register callback + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + play_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: on_play_callback(event), order=20 + ) + + try: + # trigger play event + sim.play() + assert callback_count["count"] == 1 + + # stop simulation + sim._disable_app_control_on_stop_handle = True # type: ignore + sim.stop() + + # unsubscribe the callback + play_handle.unsubscribe() + play_handle = None + + # trigger play event again + sim.play() + + # callback should not have been called again (still 1) + assert callback_count["count"] == 1 + + finally: + # cleanup if needed + if play_handle is not None: + play_handle.unsubscribe() + + +@pytest.mark.isaacsim_ci +def test_pause_event_callback(): + """Test that pause event callbacks are triggered correctly.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create callback tracker + callback_state = {"pause_called": False} + + def on_pause_callback(event): + callback_state["pause_called"] = True + + # register pause callback + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + pause_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PAUSE), lambda event: on_pause_callback(event), order=20 + ) + + try: + # play the simulation first + sim.play() + assert not callback_state["pause_called"] + + # pause the simulation + sim.pause() + + # callback should have been triggered + assert callback_state["pause_called"] + + finally: + # cleanup + if pause_handle is not None: + pause_handle.unsubscribe() + + +""" +Isaac Events Callback Tests. +""" + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize( + "event_type", + [IsaacEvents.PHYSICS_WARMUP, IsaacEvents.SIMULATION_VIEW_CREATED, IsaacEvents.PHYSICS_READY], +) +def test_isaac_event_triggered_on_reset(event_type): + """Test that Isaac events are triggered during reset.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create callback tracker + callback_state = {"called": False} + + def on_event(event): + callback_state["called"] = True + + # register callback for the event + callback_id = SimulationManager.register_callback(lambda event: on_event(event), event=event_type) + + try: + # verify callback hasn't been called yet + assert not callback_state["called"] + + # reset the simulation - should trigger the event + sim.reset() + + # verify callback was triggered + assert callback_state["called"] + + finally: + # cleanup callback + if callback_id is not None: + SimulationManager.deregister_callback(callback_id) + + +@pytest.mark.isaacsim_ci +def test_isaac_event_prim_deletion(): + """Test that PRIM_DELETION Isaac event is triggered when a prim is deleted.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + sim.reset() + + # create callback tracker + callback_state = {"prim_deleted": False, "deleted_path": None} + + def on_prim_deletion(event): + callback_state["prim_deleted"] = True + # event payload should contain the deleted prim path + if hasattr(event, "payload") and event.payload: + callback_state["deleted_path"] = event.payload.get("prim_path") + + # register callback for PRIM_DELETION event + callback_id = SimulationManager.register_callback( + lambda event: on_prim_deletion(event), event=IsaacEvents.PRIM_DELETION + ) + + try: + # verify callback hasn't been called yet + assert not callback_state["prim_deleted"] + + # delete the cube prim + sim_utils.delete_prim("/World/Cube") + + # trigger the event by dispatching it manually (since deletion might be handled differently) + SimulationManager._message_bus.dispatch_event(IsaacEvents.PRIM_DELETION.value, payload={"prim_path": "/World/Cube"}) # type: ignore + + # verify callback was triggered + assert callback_state["prim_deleted"] + + finally: + # cleanup callback + if callback_id is not None: + SimulationManager.deregister_callback(callback_id) + + +@pytest.mark.isaacsim_ci +def test_isaac_event_timeline_stop(): + """Test that TIMELINE_STOP Isaac event can be registered and triggered.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create callback tracker + callback_state = {"timeline_stop_called": False} + + def on_timeline_stop(event): + callback_state["timeline_stop_called"] = True + + # register callback for TIMELINE_STOP event + callback_id = SimulationManager.register_callback( + lambda event: on_timeline_stop(event), event=IsaacEvents.TIMELINE_STOP + ) + + try: + # verify callback hasn't been called yet + assert not callback_state["timeline_stop_called"] + + # play and stop the simulation + sim.play() + + # disable app control to prevent hanging + sim._disable_app_control_on_stop_handle = True # type: ignore + + # stop the simulation + sim.stop() + + # dispatch the event manually + SimulationManager._message_bus.dispatch_event(IsaacEvents.TIMELINE_STOP.value, payload={}) # type: ignore + + # verify callback was triggered + assert callback_state["timeline_stop_called"] + + finally: + # cleanup callback + if callback_id is not None: + SimulationManager.deregister_callback(callback_id) + + +@pytest.mark.isaacsim_ci +def test_isaac_event_callbacks_with_weakref(): + """Test Isaac event callbacks with weak references (similar to asset_base.py pattern).""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create a test object that will be weakly referenced + class PhysicsTracker: + def __init__(self): + self.warmup_count = 0 + self.ready_count = 0 + + def on_warmup(self, event): + self.warmup_count += 1 + + def on_ready(self, event): + self.ready_count += 1 + + tracker = PhysicsTracker() + + # define safe callback wrapper (same pattern as asset_base.py) + def safe_callback(callback_name, event, obj_ref): + """Safely invoke a callback on a weakly-referenced object.""" + try: + obj = obj_ref() + if obj is not None: + getattr(obj, callback_name)(event) + except ReferenceError: + # Object has been deleted; ignore + pass + + # register callbacks with weakref + obj_ref = weakref.ref(tracker) + + warmup_id = SimulationManager.register_callback( + lambda event, obj_ref=obj_ref: safe_callback("on_warmup", event, obj_ref), + event=IsaacEvents.PHYSICS_WARMUP, + ) + ready_id = SimulationManager.register_callback( + lambda event, obj_ref=obj_ref: safe_callback("on_ready", event, obj_ref), event=IsaacEvents.PHYSICS_READY + ) + + try: + # verify callbacks haven't been called + assert tracker.warmup_count == 0 + assert tracker.ready_count == 0 + + # reset simulation - triggers WARMUP and READY events + sim.reset() + + # verify callbacks were triggered + assert tracker.warmup_count == 1 + assert tracker.ready_count == 1 + + # delete the tracker object + del tracker + + # reset again - callbacks should handle the deleted object gracefully + sim.reset(soft=True) + + # should not raise any errors even though tracker is deleted + + finally: + # cleanup callbacks + if warmup_id is not None: + SimulationManager.deregister_callback(warmup_id) + if ready_id is not None: + SimulationManager.deregister_callback(ready_id) + + +@pytest.mark.isaacsim_ci +def test_multiple_isaac_event_callbacks(): + """Test that multiple callbacks can be registered for the same Isaac event.""" + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create tracking for multiple callbacks + callback_counts = {"callback1": 0, "callback2": 0, "callback3": 0} + + def callback1(event): + callback_counts["callback1"] += 1 + + def callback2(event): + callback_counts["callback2"] += 1 + + def callback3(event): + callback_counts["callback3"] += 1 + + # register multiple callbacks for PHYSICS_READY event + id1 = SimulationManager.register_callback(lambda event: callback1(event), event=IsaacEvents.PHYSICS_READY) + id2 = SimulationManager.register_callback(lambda event: callback2(event), event=IsaacEvents.PHYSICS_READY) + id3 = SimulationManager.register_callback(lambda event: callback3(event), event=IsaacEvents.PHYSICS_READY) + + try: + # verify none have been called + assert all(count == 0 for count in callback_counts.values()) + + # reset simulation - triggers PHYSICS_READY event + sim.reset() + + # all callbacks should have been called + assert callback_counts["callback1"] == 1 + assert callback_counts["callback2"] == 1 + assert callback_counts["callback3"] == 1 + + finally: + # cleanup all callbacks + if id1 is not None: + SimulationManager.deregister_callback(id1) + if id2 is not None: + SimulationManager.deregister_callback(id2) + if id3 is not None: + SimulationManager.deregister_callback(id3) + + +""" +Exception Handling in Callbacks Tests. +""" + + +@pytest.mark.isaacsim_ci +def test_exception_in_callback_on_reset(): + """Test that exceptions in callbacks during reset are properly captured and raised.""" + + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create simple scene + cube_cfg = sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)) + cube_cfg.func("/World/Cube", cube_cfg) + + # create a callback that raises an exception + def on_physics_ready_with_exception(event): + try: + raise RuntimeError("Test exception in callback during reset!") + except Exception as e: + builtins.ISAACLAB_CALLBACK_EXCEPTION = e # type: ignore + + # register callback that will raise an exception + callback_id = SimulationManager.register_callback( + lambda event: on_physics_ready_with_exception(event), event=IsaacEvents.PHYSICS_READY + ) + + try: + # reset should capture and re-raise the exception + with pytest.raises(RuntimeError, match="Test exception in callback during reset!"): + sim.reset() + + # verify the exception was cleared after being raised + assert builtins.ISAACLAB_CALLBACK_EXCEPTION is None # type: ignore + + finally: + # cleanup callback + if callback_id is not None: + SimulationManager.deregister_callback(callback_id) + # ensure exception is cleared + builtins.ISAACLAB_CALLBACK_EXCEPTION = None # type: ignore + + +@pytest.mark.isaacsim_ci +def test_exception_in_callback_on_play(): + """Test that exceptions in callbacks during play are properly captured and raised.""" + + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # create callback tracker + callback_state = {"called": False} + + def on_play_with_exception(event): + try: + callback_state["called"] = True + raise ValueError("Test exception in play callback!") + except Exception as e: + builtins.ISAACLAB_CALLBACK_EXCEPTION = e # type: ignore + + # register callback via timeline + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + play_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.PLAY), lambda event: on_play_with_exception(event), order=20 + ) + + try: + # play should capture and re-raise the exception + with pytest.raises(ValueError, match="Test exception in play callback!"): + sim.play() + + # verify callback was called + assert callback_state["called"] + + # verify the exception was cleared + assert builtins.ISAACLAB_CALLBACK_EXCEPTION is None # type: ignore + + finally: + # cleanup + if play_handle is not None: + play_handle.unsubscribe() + # ensure exception is cleared + builtins.ISAACLAB_CALLBACK_EXCEPTION = None # type: ignore + + +@pytest.mark.isaacsim_ci +def test_exception_in_callback_on_stop(): + """Test that exceptions in callbacks during stop are properly captured and raised.""" + + cfg = SimulationCfg(dt=0.01) + sim = SimulationContext(cfg) + + # start the simulation first + sim.play() + + def on_stop_with_exception(event): + try: + raise OSError("Test exception in stop callback!") + except Exception as e: + builtins.ISAACLAB_CALLBACK_EXCEPTION = e # type: ignore + + # register callback via timeline + timeline_event_stream = omni.timeline.get_timeline_interface().get_timeline_event_stream() + stop_handle = timeline_event_stream.create_subscription_to_pop_by_type( + int(omni.timeline.TimelineEventType.STOP), lambda event: on_stop_with_exception(event), order=20 + ) + + try: + # disable app control to prevent hanging + sim._disable_app_control_on_stop_handle = True # type: ignore + + # stop should capture and re-raise the exception + with pytest.raises(OSError, match="Test exception in stop callback!"): + sim.stop() + + # verify the exception was cleared + assert builtins.ISAACLAB_CALLBACK_EXCEPTION is None # type: ignore + + finally: + # cleanup + if stop_handle is not None: + stop_handle.unsubscribe() + # ensure exception is cleared + builtins.ISAACLAB_CALLBACK_EXCEPTION = None # type: ignore diff --git a/source/isaaclab/test/sim/test_simulation_stage_in_memory.py b/source/isaaclab/test/sim/test_simulation_stage_in_memory.py index 3cf15126621..5f1dd306a3d 100644 --- a/source/isaaclab/test/sim/test_simulation_stage_in_memory.py +++ b/source/isaaclab/test/sim/test_simulation_stage_in_memory.py @@ -40,8 +40,9 @@ def sim(): omni.physx.get_physx_simulation_interface().detach_stage() sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() + # Create a new stage in the USD context to ensure subsequent tests have a valid context stage + sim_utils.create_new_stage() """ @@ -68,16 +69,9 @@ def test_stage_in_memory_with_shapes(sim): cfg = sim_utils.MultiAssetSpawnerCfg( assets_cfg=[ - sim_utils.ConeCfg( - radius=0.3, - height=0.6, - ), - sim_utils.CuboidCfg( - size=(0.3, 0.3, 0.3), - ), - sim_utils.SphereCfg( - radius=0.3, - ), + sim_utils.ConeCfg(radius=0.3, height=0.6), + sim_utils.CuboidCfg(size=(0.3, 0.3, 0.3)), + sim_utils.SphereCfg(radius=0.3), ], random_choice=True, rigid_props=sim_utils.RigidBodyPropertiesCfg( diff --git a/source/isaaclab/test/sim/test_spawn_from_files.py b/source/isaaclab/test/sim/test_spawn_from_files.py index 293642a15d2..a8c84cb8d94 100644 --- a/source/isaaclab/test/sim/test_spawn_from_files.py +++ b/source/isaaclab/test/sim/test_spawn_from_files.py @@ -40,7 +40,6 @@ def sim(): # cleanup after test sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_lights.py b/source/isaaclab/test/sim/test_spawn_lights.py index 325cd06866e..f39a795bf8c 100644 --- a/source/isaaclab/test/sim/test_spawn_lights.py +++ b/source/isaaclab/test/sim/test_spawn_lights.py @@ -40,7 +40,6 @@ def sim(): # Teardown: Stop simulation sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_materials.py b/source/isaaclab/test/sim/test_spawn_materials.py index ee8cb38f90a..fe104165fca 100644 --- a/source/isaaclab/test/sim/test_spawn_materials.py +++ b/source/isaaclab/test/sim/test_spawn_materials.py @@ -32,7 +32,6 @@ def sim(): yield sim sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_meshes.py b/source/isaaclab/test/sim/test_spawn_meshes.py index 66a48422c0c..2884707a633 100644 --- a/source/isaaclab/test/sim/test_spawn_meshes.py +++ b/source/isaaclab/test/sim/test_spawn_meshes.py @@ -35,7 +35,6 @@ def sim(): # Cleanup sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_sensors.py b/source/isaaclab/test/sim/test_spawn_sensors.py index 320a47b3336..d32bbde24be 100644 --- a/source/isaaclab/test/sim/test_spawn_sensors.py +++ b/source/isaaclab/test/sim/test_spawn_sensors.py @@ -33,7 +33,6 @@ def sim(): yield sim sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_shapes.py b/source/isaaclab/test/sim/test_spawn_shapes.py index ed7ba68f89b..eb4b8fa5aa7 100644 --- a/source/isaaclab/test/sim/test_spawn_shapes.py +++ b/source/isaaclab/test/sim/test_spawn_shapes.py @@ -29,7 +29,6 @@ def sim(): yield sim sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_spawn_wrappers.py b/source/isaaclab/test/sim/test_spawn_wrappers.py index 3dd07a54e6f..35b3a875839 100644 --- a/source/isaaclab/test/sim/test_spawn_wrappers.py +++ b/source/isaaclab/test/sim/test_spawn_wrappers.py @@ -31,7 +31,6 @@ def sim(): yield sim sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab/test/sim/test_urdf_converter.py b/source/isaaclab/test/sim/test_urdf_converter.py index f12a7515fb2..6f4a2120444 100644 --- a/source/isaaclab/test/sim/test_urdf_converter.py +++ b/source/isaaclab/test/sim/test_urdf_converter.py @@ -57,7 +57,6 @@ def sim_config(): # Teardown sim.stop() sim.clear() - sim.clear_all_callbacks() sim.clear_instance() diff --git a/source/isaaclab_tasks/test/env_test_utils.py b/source/isaaclab_tasks/test/env_test_utils.py index 43973e8994a..93d926218dc 100644 --- a/source/isaaclab_tasks/test/env_test_utils.py +++ b/source/isaaclab_tasks/test/env_test_utils.py @@ -12,8 +12,10 @@ import torch import carb +import omni.physx import omni.usd +import isaaclab.sim as sim_utils from isaaclab.envs.utils.spaces import sample_space from isaaclab.utils.version import get_isaac_sim_version @@ -188,6 +190,7 @@ def _check_random_actions( # reset the rtx sensors carb setting to False carb.settings.get_settings().set_bool("/isaaclab/render/rtx_sensors", False) + try: # parse config env_cfg = parse_env_cfg(task_name, device=device, num_envs=num_envs) @@ -262,6 +265,11 @@ def _check_random_actions( # close environment env.close() + # Create a new stage in the USD context to ensure subsequent tests have a valid context stage + # This is necessary when using stage in memory, as the in-memory stage is destroyed on close + if create_stage_in_memory: + sim_utils.create_new_stage() + def _check_valid_tensor(data: torch.Tensor | dict) -> bool: """Checks if given data does not have corrupted values.