From 5b6bbeacd7e1f16f83274491271dfab712576069 Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Thu, 22 Jan 2026 19:51:58 +0000 Subject: [PATCH 01/14] initial work on omni physx compat newton-based viewers --- source/isaaclab/isaaclab/app/app_launcher.py | 59 +++++++++ .../isaaclab/isaaclab/sim/simulation_cfg.py | 16 +++ .../isaaclab/sim/simulation_context.py | 121 ++++++++++++++++++ source/isaaclab/isaaclab/utils/__init__.py | 1 + source/isaaclab/setup.py | 12 +- 5 files changed, 207 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/isaaclab/app/app_launcher.py b/source/isaaclab/isaaclab/app/app_launcher.py index e986d4b664a..1a65c6c81f1 100644 --- a/source/isaaclab/isaaclab/app/app_launcher.py +++ b/source/isaaclab/isaaclab/app/app_launcher.py @@ -115,6 +115,7 @@ def __init__(self, launcher_args: argparse.Namespace | dict | None = None, **kwa self._livestream: Literal[0, 1, 2] # 0: Disabled, 1: WebRTC public, 2: WebRTC private self._offscreen_render: bool # 0: Disabled, 1: Enabled self._sim_experience_file: str # Experience file to load + self._visualizer: list[str] | None # Visualizer backends to use # Exposed to train scripts self.device_id: int # device ID for GPU simulation (defaults to 0) @@ -304,6 +305,16 @@ def add_app_launcher_args(parser: argparse.ArgumentParser) -> None: default=AppLauncher._APPLAUNCHER_CFG_INFO["device"][1], help='The device to run the simulation on. Can be "cpu", "cuda", "cuda:N", where N is the device ID', ) + arg_group.add_argument( + "--visualizer", + type=str, + nargs="+", + default=None, + help=( + "Visualizer backend(s) to use. Valid values: newton, rerun, omniverse." + " Multiple visualizers can be specified: --visualizer rerun newton" + ), + ) # Add the deprecated cpu flag to raise an error if it is used arg_group.add_argument("--cpu", action="store_true", help=argparse.SUPPRESS) arg_group.add_argument( @@ -389,6 +400,7 @@ def add_app_launcher_args(parser: argparse.ArgumentParser) -> None: "device": ([str], "cuda:0"), "experience": ([str], ""), "rendering_mode": ([str], "balanced"), + "visualizer": ([list, type(None)], None), } """A dictionary of arguments added manually by the :meth:`AppLauncher.add_app_launcher_args` method. @@ -488,6 +500,7 @@ def _config_resolution(self, launcher_args: dict): self._resolve_headless_settings(launcher_args, livestream_arg, livestream_env) self._resolve_camera_settings(launcher_args) self._resolve_xr_settings(launcher_args) + self._resolve_visualizer_settings(launcher_args) self._resolve_viewport_settings(launcher_args) # Handle device and distributed settings @@ -777,6 +790,46 @@ def _resolve_anim_recording_settings(self, launcher_args: dict): ) sys.argv += ["--enable", "omni.physx.pvd"] + def _resolve_visualizer_settings(self, launcher_args: dict) -> None: + """Resolve visualizer related settings.""" + visualizers = launcher_args.pop("visualizer", AppLauncher._APPLAUNCHER_CFG_INFO["visualizer"][1]) + valid_visualizers = {"newton", "rerun", "omniverse"} + if visualizers is not None and len(visualizers) > 0: + invalid = [v for v in visualizers if v not in valid_visualizers] + if invalid: + raise ValueError( + f"Invalid visualizer(s) specified: {invalid}. Valid options are: {sorted(valid_visualizers)}" + ) + self._visualizer = visualizers if visualizers and len(visualizers) > 0 else None + + # Auto-adjust headless based on requested visualizers (parity with feature/newton behavior). + if self._visualizer is None: + if not self._headless and self._livestream not in {1, 2}: + self._headless = True + launcher_args["headless"] = True + print( + "[INFO][AppLauncher]: No visualizers specified. " + "Automatically enabling headless mode. Use --visualizer to enable GUI." + ) + return + + if "omniverse" in self._visualizer: + if self._headless: + self._headless = False + launcher_args["headless"] = False + print( + "[INFO][AppLauncher]: Omniverse visualizer requested. " + "Forcing headless=False for GUI." + ) + else: + if not self._headless and self._livestream not in {1, 2}: + self._headless = True + launcher_args["headless"] = True + print( + f"[INFO][AppLauncher]: Visualizer(s) {self._visualizer} requested. " + "Enabling headless mode for SimulationApp (visualizers run independently)." + ) + def _resolve_kit_args(self, launcher_args: dict): """Resolve additional arguments passed to Kit.""" # Resolve additional arguments passed to Kit @@ -867,6 +920,12 @@ def _load_extensions(self): # for example: the `Camera` sensor class carb_settings_iface.set_bool("/isaaclab/render/rtx_sensors", False) + # store visualizer selection for SimulationContext + if self._visualizer is not None: + carb_settings_iface.set_string("/isaaclab/visualizer", ",".join(self._visualizer)) + else: + carb_settings_iface.set_string("/isaaclab/visualizer", "") + # set fabric update flag to disable updating transforms when rendering is disabled carb_settings_iface.set_bool("/physics/fabricUpdateTransformations", self._rendering_enabled()) diff --git a/source/isaaclab/isaaclab/sim/simulation_cfg.py b/source/isaaclab/isaaclab/sim/simulation_cfg.py index f3bf40b83d0..e85317cca8b 100644 --- a/source/isaaclab/isaaclab/sim/simulation_cfg.py +++ b/source/isaaclab/isaaclab/sim/simulation_cfg.py @@ -12,6 +12,7 @@ from typing import Any, Literal from isaaclab.utils import configclass +from isaaclab.visualizers import VisualizerCfg from .spawners.materials import RigidBodyMaterialCfg @@ -412,6 +413,9 @@ class SimulationCfg: physx: PhysxCfg = PhysxCfg() """PhysX solver settings. Default is PhysxCfg().""" + physics_backend: Literal["omni", "newton"] = "omni" + """Physics backend to use for scene data providers and visualizers.""" + physics_material: RigidBodyMaterialCfg = RigidBodyMaterialCfg() """Default physics material settings for rigid bodies. Default is RigidBodyMaterialCfg(). @@ -424,6 +428,18 @@ class SimulationCfg: render: RenderCfg = RenderCfg() """Render settings. Default is RenderCfg().""" + visualizer_cfgs: list[VisualizerCfg] | VisualizerCfg | None = None + """Visualizer settings. Default is no visualizer. + + Visualizers are separate from Renderers and intended for light-weight monitoring and debugging. + + This field can support multiple visualizer backends. It accepts: + + * A single VisualizerCfg: One visualizer will be created + * A list of VisualizerCfg: Multiple visualizers will be created + * None or empty list: No visualizers will be created + """ + create_stage_in_memory: bool = False """If stage is first created in memory. Default is False. diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index a3d3a3d2d68..fb9a14fb1a0 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -34,9 +34,11 @@ from isaaclab.utils.logger import configure_logging from isaaclab.utils.version import get_isaac_sim_version +from .scene_data_providers import SceneDataProvider from .simulation_cfg import SimulationCfg from .spawners import DomeLightCfg, GroundPlaneCfg from .utils import bind_physics_material +from isaaclab.visualizers import NewtonVisualizerCfg, OVVisualizerCfg, RerunVisualizerCfg, Visualizer # import logger logger = logging.getLogger(__name__) @@ -256,6 +258,11 @@ def __init__(self, cfg: SimulationCfg | None = None): self._app_control_on_stop_handle = None self._disable_app_control_on_stop_handle = False + # initialize visualizers and scene data provider + self._visualizers: list[Visualizer] = [] + self._visualizer_step_counter = 0 + self._scene_data_provider: SceneDataProvider | None = None + # flatten out the simulation dictionary sim_params = self.cfg.to_dict() if sim_params is not None: @@ -505,6 +512,113 @@ def get_initial_stage(self) -> Usd.Stage: """ return self._initial_stage + """ + Visualizers. + """ + + def _create_default_visualizer_configs(self, requested_visualizers: list[str]) -> list: + """Create default visualizer configs for requested types.""" + default_configs = [] + for viz_type in requested_visualizers: + try: + if viz_type == "newton": + default_configs.append(NewtonVisualizerCfg()) + elif viz_type == "rerun": + default_configs.append(RerunVisualizerCfg()) + elif viz_type == "omniverse": + default_configs.append(OVVisualizerCfg()) + else: + logger.warning( + f"[SimulationContext] Unknown visualizer type '{viz_type}' requested. " + "Valid types: 'newton', 'rerun', 'omniverse'. Skipping." + ) + except Exception as exc: + logger.error(f"[SimulationContext] Failed to create default config for visualizer '{viz_type}': {exc}") + return default_configs + + def initialize_visualizers(self) -> None: + """Initialize visualizers from SimulationCfg.visualizer_cfgs.""" + visualizer_cfgs: list = [] + if self.cfg.visualizer_cfgs is not None: + if isinstance(self.cfg.visualizer_cfgs, list): + visualizer_cfgs = self.cfg.visualizer_cfgs + else: + visualizer_cfgs = [self.cfg.visualizer_cfgs] + + if len(visualizer_cfgs) == 0: + requested_visualizers_str = self.get_setting("/isaaclab/visualizer") + if requested_visualizers_str: + requested_visualizers = [v.strip() for v in requested_visualizers_str.split(",") if v.strip()] + visualizer_cfgs = self._create_default_visualizer_configs(requested_visualizers) + else: + return + + self._scene_data_provider = SceneDataProvider( + backend=self.cfg.physics_backend, + visualizer_cfgs=visualizer_cfgs, + stage=self.stage, + simulation_context=self, + ) + + for viz_cfg in visualizer_cfgs: + try: + visualizer = viz_cfg.create_visualizer() + scene_data: dict[str, Any] = {"scene_data_provider": self._scene_data_provider} + if viz_cfg.visualizer_type == "omniverse": + scene_data["usd_stage"] = self.stage + scene_data["simulation_context"] = self + + visualizer.initialize(scene_data) + self._visualizers.append(visualizer) + logger.info(f"Initialized visualizer: {type(visualizer).__name__} (type: {viz_cfg.visualizer_type})") + except Exception as exc: + logger.error( + f"Failed to initialize visualizer '{viz_cfg.visualizer_type}' ({type(viz_cfg).__name__}): {exc}" + ) + + def step_visualizers(self, dt: float) -> None: + """Update all active visualizers.""" + if not self._visualizers: + return + + self._visualizer_step_counter += 1 + if self._scene_data_provider: + self._scene_data_provider.update() + + visualizers_to_remove = [] + for visualizer in self._visualizers: + try: + if not visualizer.is_running(): + visualizers_to_remove.append(visualizer) + continue + + while visualizer.is_training_paused() and visualizer.is_running(): + visualizer.step(0.0, state=None) + + visualizer.step(dt, state=None) + except Exception as exc: + logger.error(f"Error stepping visualizer '{type(visualizer).__name__}': {exc}") + visualizers_to_remove.append(visualizer) + + for visualizer in visualizers_to_remove: + try: + visualizer.close() + self._visualizers.remove(visualizer) + logger.info(f"Removed visualizer: {type(visualizer).__name__}") + except Exception as exc: + logger.error(f"Error closing visualizer: {exc}") + + def close_visualizers(self) -> None: + """Close all active visualizers and clean up resources.""" + for visualizer in self._visualizers: + try: + visualizer.close() + except Exception as exc: + logger.error(f"Error closing visualizer '{type(visualizer).__name__}': {exc}") + + self._visualizers.clear() + logger.info("All visualizers closed") + """ Operations - Override (standalone) """ @@ -528,6 +642,8 @@ def reset(self, soft: bool = False): if not soft: for _ in range(2): self.render() + if not soft and not self._visualizers: + self.initialize_visualizers() self._disable_app_control_on_stop_handle = False def forward(self) -> None: @@ -578,6 +694,9 @@ def step(self, render: bool = True): # step the simulation super().step(render=render) + # Update visualizers after stepping + self.step_visualizers(self.cfg.dt) + # 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: torch.cuda.set_device(self.device) @@ -676,6 +795,8 @@ def clear_instance(cls): 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 + if hasattr(cls._instance, "_visualizers"): + cls._instance.close_visualizers() # call parent to clear the instance super().clear_instance() diff --git a/source/isaaclab/isaaclab/utils/__init__.py b/source/isaaclab/isaaclab/utils/__init__.py index 1295715857f..5dc6e92da58 100644 --- a/source/isaaclab/isaaclab/utils/__init__.py +++ b/source/isaaclab/isaaclab/utils/__init__.py @@ -13,6 +13,7 @@ from .logger import * from .mesh import * from .modifiers import * +from .simulation_runner import close_simulation, is_simulation_running from .string import * from .timer import Timer from .types import * diff --git a/source/isaaclab/setup.py b/source/isaaclab/setup.py index 19c297df715..19712d01dcf 100644 --- a/source/isaaclab/setup.py +++ b/source/isaaclab/setup.py @@ -29,11 +29,17 @@ "gymnasium==1.2.1", # procedural-generation "trimesh", - "pyglet<2", + "pyglet>=2.1.6", # image processing "transformers", "einops", # needed for transformers, doesn't always auto-install - "warp-lang", + "warp-lang==1.11.0.dev20251205", + # newton visualizers / backend dependencies + "mujoco>=3.4.0.dev839962392", + "mujoco-warp@ git+https://github.com/google-deepmind/mujoco_warp.git@e9a67538f2c14486121635074c5a5fd6ca55fa83", + "newton@ git+https://github.com/newton-physics/newton.git@beta-0.2.1", + "imgui-bundle==1.92.0", + "PyOpenGL-accelerate==3.1.10", # make sure this is consistent with isaac sim version "pillow==11.3.0", # livestream @@ -56,6 +62,8 @@ f"daqp==0.7.2 ; platform_system == 'Linux' and ({SUPPORTED_ARCHS_ARM})", # required by isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1_t2_dex_retargeting_utils f"dex-retargeting==0.4.6 ; platform_system == 'Linux' and ({SUPPORTED_ARCHS})", + f"usd-core==25.05.0 ; platform_system == 'Linux' and ({SUPPORTED_ARCHS})", + f"usd-exchange>=2.1 ; platform_system == 'Linux' and ({SUPPORTED_ARCHS_ARM})", ] PYTORCH_INDEX_URL = ["https://download.pytorch.org/whl/cu128"] From 3c53d7a181cfcf8ed84d892ae057d1ab65bd430a Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Sat, 24 Jan 2026 03:22:48 +0000 Subject: [PATCH 02/14] testing --- .../isaaclab/envs/manager_based_env.py | 8 + .../isaaclab/scene/interactive_scene.py | 21 + .../sim/scene_data_providers/__init__.py | 18 + .../newton_scene_data_provider.py | 99 +++ .../ov_scene_data_provider.py | 571 ++++++++++++++++++ .../scene_data_provider.py | 124 ++++ .../spawners/materials/visual_materials.py | 30 +- .../isaaclab/utils/simulation_runner.py | 37 ++ .../isaaclab/isaaclab/visualizers/__init__.py | 63 ++ .../isaaclab/visualizers/newton_visualizer.py | 333 ++++++++++ .../visualizers/newton_visualizer_cfg.py | 57 ++ .../isaaclab/visualizers/ov_visualizer.py | 276 +++++++++ .../isaaclab/visualizers/ov_visualizer_cfg.py | 33 + .../isaaclab/visualizers/rerun_visualizer.py | 225 +++++++ .../visualizers/rerun_visualizer_cfg.py | 35 ++ .../isaaclab/visualizers/visualizer.py | 78 +++ .../isaaclab/visualizers/visualizer_cfg.py | 63 ++ 17 files changed, 2064 insertions(+), 7 deletions(-) create mode 100644 source/isaaclab/isaaclab/sim/scene_data_providers/__init__.py create mode 100644 source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py create mode 100644 source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py create mode 100644 source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py create mode 100644 source/isaaclab/isaaclab/utils/simulation_runner.py create mode 100644 source/isaaclab/isaaclab/visualizers/__init__.py create mode 100644 source/isaaclab/isaaclab/visualizers/newton_visualizer.py create mode 100644 source/isaaclab/isaaclab/visualizers/newton_visualizer_cfg.py create mode 100644 source/isaaclab/isaaclab/visualizers/ov_visualizer.py create mode 100644 source/isaaclab/isaaclab/visualizers/ov_visualizer_cfg.py create mode 100644 source/isaaclab/isaaclab/visualizers/rerun_visualizer.py create mode 100644 source/isaaclab/isaaclab/visualizers/rerun_visualizer_cfg.py create mode 100644 source/isaaclab/isaaclab/visualizers/visualizer.py create mode 100644 source/isaaclab/isaaclab/visualizers/visualizer_cfg.py diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 3ff6d291c0a..88bf0d85271 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -142,6 +142,14 @@ def __init__(self, cfg: ManagerBasedEnvCfg): self.scene = InteractiveScene(self.cfg.scene) attach_stage_to_usd_context() print("[INFO]: Scene manager: ", self.scene) + from isaaclab.sim.utils import find_matching_prim_paths + + env_prim_paths = find_matching_prim_paths("/World/envs/env_.*", stage=self.scene.stage) + print( + "[SceneDebug] env prims after InteractiveScene: " + f"num_envs_setting={self.cfg.scene.num_envs}, env_prims={len(env_prim_paths)}" + ) + import ipdb; ipdb.set_trace() # set up camera viewport controller # viewport is not available in other rendering modes so the function will throw a warning diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 291e371ca39..89c3fed939b 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -120,6 +120,10 @@ def __init__(self, cfg: InteractiveSceneCfg): cfg.validate() # store inputs self.cfg = cfg + + # TODO(mtrepte): remove + # self.cfg.clone_in_fabric = False + # initialize scene elements self._terrain = None self._articulations = dict() @@ -133,6 +137,11 @@ def __init__(self, cfg: InteractiveSceneCfg): self.sim = SimulationContext.instance() self.stage = get_current_stage() self.stage_id = get_current_stage_id() + # publish num_envs for consumers outside the scene + try: + self.sim.set_setting("/isaaclab/scene/num_envs", int(self.cfg.num_envs)) + except Exception: + pass # physics scene path self._physics_scene_path = None # prepare cloner for environment replication @@ -270,6 +279,18 @@ def clone_environments(self, copy_from_source: bool = False): if self._default_env_origins is None: self._default_env_origins = torch.tensor(env_origins, device=self.device, dtype=torch.float32) + # publish env origins for consumers that cannot read USD (e.g., Fabric clones) + try: + if hasattr(env_origins, "flatten"): + origins_list = env_origins.flatten().tolist() + else: + origins_list = [] + for origin in env_origins: + origins_list.extend(list(origin)) + self.sim.set_setting("/isaaclab/scene/env_origins", origins_list) + except Exception: + pass + def filter_collisions(self, global_prim_paths: list[str] | None = None): """Filter environments collisions. diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/__init__.py b/source/isaaclab/isaaclab/sim/scene_data_providers/__init__.py new file mode 100644 index 00000000000..55ccb14863c --- /dev/null +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/__init__.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python3 +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Scene data providers for visualizers and renderers.""" + +from .scene_data_provider import SceneDataProvider, SceneDataProviderBase +from .newton_scene_data_provider import NewtonSceneDataProvider +from .ov_scene_data_provider import OmniSceneDataProvider + +__all__ = [ + "SceneDataProvider", + "SceneDataProviderBase", + "NewtonSceneDataProvider", + "OmniSceneDataProvider", +] diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py new file mode 100644 index 00000000000..d664abbe343 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Newton-backed scene data provider.""" + +from __future__ import annotations + +import logging +from typing import Any + +from .scene_data_provider import SceneDataProviderBase + +logger = logging.getLogger(__name__) + + +class NewtonSceneDataProvider(SceneDataProviderBase): + """Newton-backed scene data provider (when Newton physics is the active backend).""" + + def __init__(self, visualizer_cfgs: list[Any] | None) -> None: + self._has_newton_visualizer = False + self._has_rerun_visualizer = False + self._has_ov_visualizer = False + self._metadata: dict[str, Any] = {} + + if visualizer_cfgs: + for cfg in visualizer_cfgs: + viz_type = getattr(cfg, "visualizer_type", None) + if viz_type == "newton": + self._has_newton_visualizer = True + elif viz_type == "rerun": + self._has_rerun_visualizer = True + elif viz_type == "omniverse": + self._has_ov_visualizer = True + + # Lazy import to keep develop usable without Newton installed. + try: + from isaaclab.sim._impl.newton_manager import NewtonManager + + self._metadata = { + "physics_backend": "newton", + "num_envs": NewtonManager._num_envs if NewtonManager._num_envs is not None else 0, + "gravity_vector": NewtonManager._gravity_vector, + "clone_physics_only": NewtonManager._clone_physics_only, + } + except Exception: + self._metadata = {"physics_backend": "newton"} + + def update(self) -> None: + return None + + def get_model(self): + if not (self._has_newton_visualizer or self._has_rerun_visualizer): + return None + try: + from isaaclab.sim._impl.newton_manager import NewtonManager + + return NewtonManager._model + except Exception: + return None + + def get_state(self): + if not (self._has_newton_visualizer or self._has_rerun_visualizer): + return None + try: + from isaaclab.sim._impl.newton_manager import NewtonManager + + return NewtonManager._state_0 + except Exception: + return None + + def get_metadata(self) -> dict[str, Any]: + return dict(self._metadata) + + def get_transforms(self) -> dict[str, Any] | None: + return None + + def get_velocities(self) -> dict[str, Any] | None: + try: + from isaaclab.sim._impl.newton_manager import NewtonManager + + state = NewtonManager._state_0 + if state is None: + return None + return {"body_qd": state.body_qd} + except Exception: + return None + + def get_contacts(self) -> dict[str, Any] | None: + try: + from isaaclab.sim._impl.newton_manager import NewtonManager + + if NewtonManager._contacts is None: + return None + return {"contacts": NewtonManager._contacts} + except Exception: + return None diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py new file mode 100644 index 00000000000..52e5afd0fd9 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py @@ -0,0 +1,571 @@ +#!/usr/bin/env python3 +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Omni PhysX-backed scene data provider.""" + +from __future__ import annotations + +import logging +import re +from typing import Any + +from .scene_data_provider import SceneDataProviderBase + +logger = logging.getLogger(__name__) + + +class OmniSceneDataProvider(SceneDataProviderBase): + """Omni PhysX-backed scene data provider. + + This provider can generate a Newton-compatible Model/State for use by Newton and Rerun + visualizers while the active physics backend is Omni PhysX. + """ + + def __init__(self, visualizer_cfgs: list[Any] | None, stage, simulation_context) -> None: + from isaacsim.core.simulation_manager import SimulationManager + from pxr import UsdGeom + + self._stage = stage + self._simulation_context = simulation_context + self._physics_sim_view = SimulationManager.get_physics_sim_view() + self._rigid_body_view = None + self._articulation_view = None + self._contact_view = None + self._xform_views: dict[str, Any] = {} + self._body_key_index_map: dict[str, int] = {} + self._view_body_index_map: dict[str, list[int]] = {} + + self._has_newton_visualizer = False + self._has_rerun_visualizer = False + self._has_ov_visualizer = False + if visualizer_cfgs: + for cfg in visualizer_cfgs: + viz_type = getattr(cfg, "visualizer_type", None) + if viz_type == "newton": + self._has_newton_visualizer = True + elif viz_type == "rerun": + self._has_rerun_visualizer = True + elif viz_type == "omniverse": + self._has_ov_visualizer = True + + self._metadata = { + "physics_backend": "omni", + "num_envs": self._get_num_envs(), + "gravity_vector": tuple(self._simulation_context.cfg.gravity), + "clone_physics_only": False, + } + + self._device = getattr(self._simulation_context, "device", "cuda:0") + self._newton_model = None + self._newton_state = None + self._rigid_body_paths: list[str] = [] + self._set_body_q_kernel = None + self._up_axis = UsdGeom.GetStageUpAxis(self._stage) + + if self._has_newton_visualizer or self._has_rerun_visualizer: + self._build_newton_model_from_usd() + self._setup_rigid_body_view() + self._setup_articulation_view() + + def _get_num_envs(self) -> int: + try: + import carb + + carb_settings_iface = carb.settings.get_settings() + num_envs = carb_settings_iface.get("/isaaclab/scene/num_envs") + if num_envs: + return int(num_envs) + except Exception: + return 0 + return 0 + + @staticmethod + def _wildcard_env_paths(paths: list[str]) -> list[str]: + wildcard_paths = [] + for path in paths: + if "/World/envs/env_0" in path: + wildcard_paths.append(path.replace("/World/envs/env_0", "/World/envs/env_*")) + return list(dict.fromkeys(wildcard_paths)) if wildcard_paths else paths + + def _refresh_newton_model_if_needed(self) -> None: + num_envs = self._get_num_envs() + if num_envs <= 0: + return + + if self._newton_model is None or self._newton_state is None: + self._build_newton_model_from_usd() + self._setup_rigid_body_view() + self._setup_articulation_view() + return + + if self._metadata.get("num_envs", 0) != num_envs: + self._build_newton_model_from_usd() + self._setup_rigid_body_view() + self._setup_articulation_view() + return + + def _build_newton_model_from_usd(self) -> None: + # TODO(mtrepte): add support for fabric cloning + try: + from newton import ModelBuilder + from isaaclab.sim.utils import find_matching_prim_paths + + num_envs = self._get_num_envs() + + import ipdb; ipdb.set_trace() + env_prim_paths = find_matching_prim_paths("/World/envs/env_.*", stage=self._stage) + print( + "[SceneDataProvider] Stage env prims before add_usd: "+ + f"num_envs_setting={num_envs}, env_prims={len(env_prim_paths)}" + ) + + builder = ModelBuilder(up_axis=self._up_axis) + builder.add_usd(self._stage) + + self._newton_model = builder.finalize(device=self._device) + self._metadata["num_envs"] = num_envs + self._newton_model.num_envs = self._metadata.get("num_envs", 0) + self._newton_state = self._newton_model.state() + self._rigid_body_paths = list(self._newton_model.body_key) + self._xform_views.clear() + self._contact_view = None + self._body_key_index_map = {path: i for i, path in enumerate(self._rigid_body_paths)} + self._view_body_index_map = {} + except ModuleNotFoundError as exc: + logger.error( + "[SceneDataProvider] Newton module not available. " + "Install the Newton backend to use newton/rerun visualizers." + ) + logger.debug(f"[SceneDataProvider] Newton import error: {exc}") + except Exception as exc: + logger.error(f"[SceneDataProvider] Failed to build Newton model from USD: {exc}") + self._newton_model = None + self._newton_state = None + self._rigid_body_paths = [] + + def _setup_rigid_body_view(self) -> None: + if not self._rigid_body_paths: + return + try: + paths_to_use = self._wildcard_env_paths(list(self._rigid_body_paths)) + self._rigid_body_view = self._physics_sim_view.create_rigid_body_view(paths_to_use) + self._cache_view_index_map(self._rigid_body_view, "rigid_body_view") + except Exception as exc: + logger.warning(f"[SceneDataProvider] Failed to create RigidBodyView: {exc}") + self._rigid_body_view = None + + def _setup_articulation_view(self) -> None: + try: + from pxr import UsdPhysics + + from isaaclab.sim.utils import get_all_matching_child_prims + + root_prims = get_all_matching_child_prims( + "/World", + predicate=lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI), + stage=self._stage, + traverse_instance_prims=True, + ) + if not root_prims: + return + + paths_to_use = self._wildcard_env_paths([prim.GetPath().pathString for prim in root_prims]) + exprs = [path.replace(".*", "*") for path in paths_to_use] + self._articulation_view = self._physics_sim_view.create_articulation_view( + exprs if len(exprs) > 1 else exprs[0] + ) + self._cache_view_index_map(self._articulation_view, "articulation_view") + except Exception as exc: + logger.warning(f"[SceneDataProvider] Failed to create ArticulationView: {exc}") + self._articulation_view = None + + def _get_view_world_poses(self, view): + if view is None: + return None, None + + method_names = ( + "get_world_poses", + "get_world_transforms", + "get_transforms", + "get_poses", + ) + + for name in method_names: + method = getattr(view, name, None) + if method is None: + continue + try: + result = method() + except Exception: + continue + + if isinstance(result, tuple) and len(result) == 2: + return result + + try: + if hasattr(result, "shape") and result.shape[-1] == 7: + positions = result[..., :3] + orientations = result[..., 3:7] + return positions, orientations + except Exception: + continue + + return None, None + + def _cache_view_index_map(self, view, key: str) -> None: + prim_paths = getattr(view, "prim_paths", None) + if not prim_paths or not self._rigid_body_paths: + return + + def split_env(path: str) -> tuple[int | None, str]: + match = re.search(r"/World/envs/env_(\d+)(/.*)", path) + return (int(match.group(1)), match.group(2)) if match else (None, path) + + view_map: dict[tuple[int | None, str], int] = {} + for view_idx, path in enumerate(prim_paths): + env_id, rel = split_env(path) + view_map[(env_id, rel)] = view_idx + + order: list[int | None] = [None] * len(self._rigid_body_paths) + for body_idx, path in enumerate(self._rigid_body_paths): + env_id, rel = split_env(path) + view_idx = view_map.get((env_id, rel)) + if view_idx is None: + view_idx = view_map.get((None, rel)) + order[body_idx] = view_idx + + if all(idx is not None for idx in order): + self._view_body_index_map[key] = order # type: ignore[arg-type] + + def _get_view_velocities(self, view): + if view is None: + return None, None + + # Preferred: combined velocities + method = getattr(view, "get_velocities", None) + if method is not None: + try: + result = method() + if isinstance(result, tuple) and len(result) == 2: + return result + if hasattr(result, "shape") and result.shape[-1] == 6: + return result[..., :3], result[..., 3:6] + except Exception: + pass + + # Fallback: split linear/angular + get_linear = getattr(view, "get_linear_velocities", None) + get_angular = getattr(view, "get_angular_velocities", None) + if get_linear is not None and get_angular is not None: + try: + return get_linear(), get_angular() + except Exception: + return None, None + + return None, None + + def _setup_contact_view(self) -> None: + if not self._rigid_body_paths or self._contact_view is not None: + return + try: + from pxr import PhysxSchema + + contact_paths = [ + path + for path in self._rigid_body_paths + if self._stage.GetPrimAtPath(path).HasAPI(PhysxSchema.PhysxContactReportAPI) + ] + if contact_paths: + contact_paths = self._wildcard_env_paths(contact_paths) + self._contact_view = self._physics_sim_view.create_rigid_contact_view(contact_paths) + except Exception as exc: + logger.debug(f"[SceneDataProvider] Failed to create ContactView: {exc}") + self._contact_view = None + + def _get_view_contacts(self, view): + if view is None: + return None + dt = getattr(self._simulation_context, "cfg", None) + dt = getattr(dt, "dt", 1.0 / 60.0) if dt is not None else 1.0 / 60.0 + + try: + num_envs = int(self._metadata.get("num_envs") or 1) + view_count = getattr(view, "count", 0) + num_bodies = view_count // num_envs if num_envs > 0 and view_count else 0 + num_filters = getattr(view, "filter_count", 0) + + output: dict[str, Any] = {} + + method = getattr(view, "get_net_contact_forces", None) + if method is not None: + output["net_forces_w"] = method(dt=dt) + + method = getattr(view, "get_contact_data", None) + if method is not None: + data = method(dt=dt) + if isinstance(data, tuple) and len(data) >= 6: + _, contact_points, contact_normals, contact_distances, buffer_count, buffer_start_indices = data[:6] + if num_envs > 0 and num_bodies > 0 and num_filters > 0: + output["contact_points_w"] = self._unpack_contact_buffer_data( + contact_points, + buffer_count, + buffer_start_indices, + num_envs, + num_bodies, + num_filters, + avg=True, + default=float("nan"), + ) + output["contact_normals_w"] = self._unpack_contact_buffer_data( + contact_normals, + buffer_count, + buffer_start_indices, + num_envs, + num_bodies, + num_filters, + avg=True, + default=float("nan"), + ) + output["contact_distances"] = self._unpack_contact_buffer_data( + contact_distances.view(-1, 1), + buffer_count, + buffer_start_indices, + num_envs, + num_bodies, + num_filters, + avg=True, + default=float("nan"), + ).squeeze(-1) + + method = getattr(view, "get_friction_data", None) + if method is not None: + data = method(dt=dt) + if isinstance(data, tuple) and len(data) >= 4: + friction_forces, _, buffer_count, buffer_start_indices = data[:4] + if num_envs > 0 and num_bodies > 0 and num_filters > 0: + output["friction_forces_w"] = self._unpack_contact_buffer_data( + friction_forces, + buffer_count, + buffer_start_indices, + num_envs, + num_bodies, + num_filters, + avg=False, + default=0.0, + ) + + if not output: + return None + return output + except Exception as exc: + logger.debug(f"[SceneDataProvider] Failed to read ContactView data: {exc}") + return None + + def _unpack_contact_buffer_data( + self, + contact_data, + buffer_count, + buffer_start_indices, + num_envs: int, + num_bodies: int, + num_filters: int, + avg: bool = True, + default: float = float("nan"), + ): + try: + import torch + + if num_envs <= 0 or num_bodies <= 0 or num_filters <= 0: + return None + + counts = buffer_count.view(-1) + starts = buffer_start_indices.view(-1) + n_rows = counts.numel() + if contact_data is None or n_rows == 0: + return None + + data = contact_data + if data.ndim == 1: + data = data.view(-1, 1) + dim = data.shape[-1] + + agg = torch.full((n_rows, dim), default, device=data.device, dtype=data.dtype) + total = int(counts.sum().item()) + if total > 0: + row_ids = torch.repeat_interleave(torch.arange(n_rows, device=data.device), counts) + block_starts = counts.cumsum(0) - counts + deltas = torch.arange(row_ids.numel(), device=data.device) - block_starts.repeat_interleave(counts) + flat_idx = starts[row_ids] + deltas + pts = data.index_select(0, flat_idx) + agg = agg.zero_().index_add_(0, row_ids, pts) + agg = agg / counts.clamp_min(1).unsqueeze(-1) if avg else agg + agg[counts == 0] = default + + return agg.view(num_envs, num_bodies, num_filters, dim) + except Exception: + return None + + def _get_xform_world_poses(self): + if not self._rigid_body_paths: + return None, None + try: + import torch + + from isaaclab.sim.views import XformPrimView + + positions = [] + orientations = [] + for path in self._rigid_body_paths: + view = self._xform_views.get(path) + if view is None: + view = XformPrimView(path, device=self._device, stage=self._stage, validate_xform_ops=False) + self._xform_views[path] = view + pos, quat = view.get_world_poses() + positions.append(pos) + orientations.append(quat) + return (torch.cat(positions, dim=0), torch.cat(orientations, dim=0)) if positions else (None, None) + except Exception as exc: + logger.debug(f"[SceneDataProvider] Failed to read XformPrimView poses: {exc}") + return None, None + + def _get_set_body_q_kernel(self): + if self._set_body_q_kernel is not None: + return self._set_body_q_kernel + try: + import warp as wp + + @wp.kernel(enable_backward=False) + def _set_body_q( + positions: wp.array(dtype=wp.vec3), + orientations: wp.array(dtype=wp.quatf), + body_q: wp.array(dtype=wp.transformf), + ): + i = wp.tid() + body_q[i] = wp.transformf(positions[i], orientations[i]) + + self._set_body_q_kernel = _set_body_q + return self._set_body_q_kernel + except Exception as exc: + logger.warning(f"[SceneDataProvider] Warp unavailable for Newton state sync: {exc}") + return None + + def update(self) -> None: + if not (self._has_newton_visualizer or self._has_rerun_visualizer): + return + self._refresh_newton_model_if_needed() + if self._newton_state is None: + return + if self._rigid_body_view is None and self._articulation_view is None and not self._rigid_body_paths: + return + + try: + import torch + import warp as wp + + from isaaclab.utils.math import convert_quat + + expected_count = self._newton_state.body_q.shape[0] + pose_sources = ( + ("articulation_view", lambda: self._get_view_world_poses(self._articulation_view)), + ("rigid_body_view", lambda: self._get_view_world_poses(self._rigid_body_view)), + ("xform_view", self._get_xform_world_poses), + ) + positions = orientations = None + source_view = "none" + for name, getter in pose_sources: + positions, orientations = getter() + if positions is not None and orientations is not None: + if positions.reshape(-1, 3).shape[0] == expected_count: + source_view = name + break + if positions is None or orientations is None: + return + order = self._view_body_index_map.get(source_view) + if order: + positions = positions[order] + orientations = orientations[order] + + positions = positions.reshape(-1, 3).to(dtype=torch.float32, device=self._device) + orientations = orientations.reshape(-1, 4).to(dtype=torch.float32, device=self._device) + # TODO(mtrepte) currently converting quaternion convention from wxyz to xyzw + # in the future, we can avoid this step + orientations_xyzw = convert_quat(orientations, to="xyzw") + + positions_wp = wp.from_torch(positions, dtype=wp.vec3) + orientations_wp = wp.from_torch(orientations_xyzw, dtype=wp.quatf) + + set_body_q = self._get_set_body_q_kernel() + if set_body_q is None: + return + + if positions_wp.shape[0] != expected_count: + logger.debug( + "[SceneDataProvider] Body count mismatch for Newton sync " + f"(poses={positions_wp.shape[0]}, state={expected_count}, source={source_view})." + ) + return + + wp.launch( + set_body_q, + dim=positions_wp.shape[0], + inputs=[positions_wp, orientations_wp, self._newton_state.body_q], + device=self._device, + ) + + # Future extensions: + # - Populate velocities into self._newton_state.body_qd + # - Sync contacts into Newton Contacts for richer debug visualizations + # - Cache mesh/material data for Rerun/renderer integrations + except Exception as exc: + logger.debug(f"[SceneDataProvider] Failed to sync Omni transforms to Newton state: {exc}") + + def get_model(self): + if not (self._has_newton_visualizer or self._has_rerun_visualizer): + return None + return self._newton_model + + def get_state(self): + if not (self._has_newton_visualizer or self._has_rerun_visualizer): + return None + return self._newton_state + + def get_metadata(self) -> dict[str, Any]: + return dict(self._metadata) + + def get_transforms(self) -> dict[str, Any] | None: + if self._rigid_body_view is None and self._articulation_view is None: + return None + try: + for getter in ( + lambda: self._get_view_world_poses(self._articulation_view), + lambda: self._get_view_world_poses(self._rigid_body_view), + self._get_xform_world_poses, + ): + positions, orientations = getter() + if positions is not None and orientations is not None: + return {"positions": positions, "orientations": orientations} + return None + except Exception: + return None + + def get_velocities(self) -> dict[str, Any] | None: + for source, view in ( + ("articulation_view", self._articulation_view), + ("rigid_body_view", self._rigid_body_view), + ): + linear, angular = self._get_view_velocities(view) + if linear is not None and angular is not None: + return {"linear": linear, "angular": angular, "source": source} + return None + + def get_contacts(self) -> dict[str, Any] | None: + if self._contact_view is None: + self._setup_contact_view() + data = self._get_view_contacts(self._contact_view) + if data is None: + return None + data["source"] = "contact_view" + return data diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py new file mode 100644 index 00000000000..a999315d4af --- /dev/null +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Scene data provider abstraction for visualizers and renderers.""" + +from __future__ import annotations + +import logging +from abc import ABC, abstractmethod +from typing import Any + +logger = logging.getLogger(__name__) + + +class SceneDataProviderBase(ABC): + """Base interface for scene data providers.""" + + @abstractmethod + def update(self) -> None: + """Sync any per-step data needed by visualizers.""" + + @abstractmethod + def get_model(self): + """Return a physics-backend model object, if applicable.""" + + @abstractmethod + def get_state(self): + """Return a physics-backend state object, if applicable.""" + + @abstractmethod + def get_metadata(self) -> dict[str, Any]: + """Return basic metadata about the scene/backend.""" + + @abstractmethod + def get_transforms(self) -> dict[str, Any] | None: + """Return transform data keyed by semantic names.""" + + def get_velocities(self) -> dict[str, Any] | None: + """Return velocity data keyed by semantic names.""" + # TODO: Populate linear/angular velocities once available per backend. + return None + + def get_contacts(self) -> dict[str, Any] | None: + """Return contact data keyed by semantic names.""" + # TODO: Populate contact data once available per backend. + return None + + def get_meshes(self) -> dict[str, Any] | None: + """Return mesh/material data keyed by semantic names.""" + # TODO: Populate mesh/material data once available per backend. + return None + + +class SceneDataProvider: + """Facade that selects the correct scene data provider for the active backend.""" + + def __init__( + self, + backend: str, + visualizer_cfgs: list[Any] | None, + stage=None, + simulation_context=None, + ) -> None: + self._backend = backend + self._provider: SceneDataProviderBase | None = None + + if backend == "newton": + from .newton_scene_data_provider import NewtonSceneDataProvider + + self._provider = NewtonSceneDataProvider(visualizer_cfgs) + elif backend == "omni": + if stage is None or simulation_context is None: + logger.warning( + "Omni scene data provider requires stage and simulation context; skipping initialization." + ) + self._provider = None + else: + from .ov_scene_data_provider import OmniSceneDataProvider + + self._provider = OmniSceneDataProvider(visualizer_cfgs, stage, simulation_context) + else: + logger.warning(f"Unknown physics backend '{backend}'. No scene data provider created.") + + def update(self) -> None: + if self._provider is not None: + self._provider.update() + + def get_model(self): + if self._provider is None: + return None + return self._provider.get_model() + + def get_state(self): + if self._provider is None: + return None + return self._provider.get_state() + + def get_metadata(self) -> dict[str, Any]: + if self._provider is None: + return {} + return self._provider.get_metadata() + + def get_transforms(self) -> dict[str, Any] | None: + if self._provider is None: + return None + return self._provider.get_transforms() + + def get_velocities(self) -> dict[str, Any] | None: + if self._provider is None: + return None + return self._provider.get_velocities() + + def get_contacts(self) -> dict[str, Any] | None: + if self._provider is None: + return None + return self._provider.get_contacts() + + def get_meshes(self) -> dict[str, Any] | None: + if self._provider is None: + return None + return self._provider.get_meshes() diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py index 074d6ac0e43..713ce34749b 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py @@ -61,12 +61,19 @@ def spawn_preview_surface(prim_path: str, cfg: visual_materials_cfg.PreviewSurfa # handle scene creation on a custom stage. material_prim = UsdShade.Material.Define(stage, prim_path) if material_prim: - shader_prim = CreateShaderPrimFromSdrCommand( - parent_path=prim_path, - identifier="UsdPreviewSurface", - stage_or_context=stage, - name="Shader", - ).do() + try: + shader_prim = CreateShaderPrimFromSdrCommand( + parent_path=prim_path, + identifier="UsdPreviewSurface", + stage_or_context=stage, + name="Shader", + ).do() + except TypeError: + shader_prim = CreateShaderPrimFromSdrCommand( + parent_path=prim_path, + identifier="UsdPreviewSurface", + stage_or_context=stage, + ).do() # bind the shader graph to the material if shader_prim: surface_out = shader_prim.GetOutput("surface") @@ -82,7 +89,16 @@ def spawn_preview_surface(prim_path: str, cfg: visual_materials_cfg.PreviewSurfa raise ValueError(f"A prim already exists at path: '{prim_path}'.") # obtain prim - prim = stage.GetPrimAtPath(f"{prim_path}/Shader") + prim = None + if shader_prim: + if hasattr(shader_prim, "GetPrim"): + prim = shader_prim.GetPrim() + elif hasattr(shader_prim, "IsValid"): + prim = shader_prim + elif hasattr(shader_prim, "GetPath"): + prim = stage.GetPrimAtPath(str(shader_prim.GetPath())) + if prim is None or not prim.IsValid(): + 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}'.") diff --git a/source/isaaclab/isaaclab/utils/simulation_runner.py b/source/isaaclab/isaaclab/utils/simulation_runner.py new file mode 100644 index 00000000000..1b0b89a031e --- /dev/null +++ b/source/isaaclab/isaaclab/utils/simulation_runner.py @@ -0,0 +1,37 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utility functions for running simulation loops with visualizer-agnostic is_running checks.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any + +if TYPE_CHECKING: + from isaaclab.sim import SimulationContext + + +def is_simulation_running(simulation_app: Any | None, sim_context: SimulationContext | None = None) -> bool: + """Check if the simulation should continue running. + + - Omniverse mode: delegates to SimulationApp.is_running() + - Standalone mode with visualizers: checks if any visualizer window is still open + - Headless mode (no visualizers): always returns True (use Ctrl+C or break to exit) + """ + if simulation_app is not None: + return simulation_app.is_running() + + if sim_context is not None and hasattr(sim_context, "_visualizers"): + visualizers = sim_context._visualizers + if visualizers: + return any(v.is_running() for v in visualizers) + + return True + + +def close_simulation(simulation_app: Any | None) -> None: + """Close the simulation app if running in Omniverse mode.""" + if simulation_app is not None: + simulation_app.close() diff --git a/source/isaaclab/isaaclab/visualizers/__init__.py b/source/isaaclab/isaaclab/visualizers/__init__.py new file mode 100644 index 00000000000..df9f3b60ab8 --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/__init__.py @@ -0,0 +1,63 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package for visualizer configurations and implementations.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any + +from .newton_visualizer_cfg import NewtonVisualizerCfg +from .ov_visualizer_cfg import OVVisualizerCfg +from .rerun_visualizer_cfg import RerunVisualizerCfg +from .visualizer import Visualizer +from .visualizer_cfg import VisualizerCfg + +if TYPE_CHECKING: + from typing import Type + + from .newton_visualizer import NewtonVisualizer + from .ov_visualizer import OVVisualizer + from .rerun_visualizer import RerunVisualizer + +_VISUALIZER_REGISTRY: dict[str, Any] = {} + +__all__ = [ + "Visualizer", + "VisualizerCfg", + "NewtonVisualizerCfg", + "OVVisualizerCfg", + "RerunVisualizerCfg", + "get_visualizer_class", +] + + +def get_visualizer_class(name: str) -> type[Visualizer] | None: + """Get a visualizer class by name (lazy-loaded).""" + if name in _VISUALIZER_REGISTRY: + return _VISUALIZER_REGISTRY[name] + + try: + if name == "newton": + from .newton_visualizer import NewtonVisualizer + + _VISUALIZER_REGISTRY["newton"] = NewtonVisualizer + return NewtonVisualizer + if name == "omniverse": + from .ov_visualizer import OVVisualizer + + _VISUALIZER_REGISTRY["omniverse"] = OVVisualizer + return OVVisualizer + if name == "rerun": + from .rerun_visualizer import RerunVisualizer + + _VISUALIZER_REGISTRY["rerun"] = RerunVisualizer + return RerunVisualizer + return None + except ImportError as exc: + import warnings + + warnings.warn(f"Failed to load visualizer '{name}': {exc}", ImportWarning) + return None diff --git a/source/isaaclab/isaaclab/visualizers/newton_visualizer.py b/source/isaaclab/isaaclab/visualizers/newton_visualizer.py new file mode 100644 index 00000000000..b3c01fedb8a --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/newton_visualizer.py @@ -0,0 +1,333 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Newton OpenGL Visualizer implementation.""" + +from __future__ import annotations + +import contextlib +import numpy as np +from typing import Any + +import warp as wp +from newton.viewer import ViewerGL + +from .newton_visualizer_cfg import NewtonVisualizerCfg +from .visualizer import Visualizer + + +class NewtonViewerGL(ViewerGL): + """Wrapper around Newton's ViewerGL with training/rendering pause controls.""" + + def __init__(self, *args, metadata: dict | None = None, update_frequency: int = 1, **kwargs): + super().__init__(*args, **kwargs) + self._paused_training = False + self._paused_rendering = False + self._metadata = metadata or {} + self._fallback_draw_controls = False + self._update_frequency = update_frequency + + try: + self.register_ui_callback(self._render_training_controls, position="side") + except AttributeError: + self._fallback_draw_controls = True + + def is_training_paused(self) -> bool: + return self._paused_training + + def is_rendering_paused(self) -> bool: + return self._paused_rendering + + def _render_training_controls(self, imgui): + imgui.separator() + imgui.text("IsaacLab Controls") + + pause_label = "Resume Training" if self._paused_training else "Pause Training" + if imgui.button(pause_label): + self._paused_training = not self._paused_training + + rendering_label = "Resume Rendering" if self._paused_rendering else "Pause Rendering" + if imgui.button(rendering_label): + self._paused_rendering = not self._paused_rendering + self._paused = self._paused_rendering + + imgui.text("Visualizer Update Frequency") + current_frequency = self._update_frequency + changed, new_frequency = imgui.slider_int( + "##VisualizerUpdateFreq", current_frequency, 1, 20, f"Every {current_frequency} frames" + ) + if changed: + self._update_frequency = new_frequency + + if imgui.is_item_hovered(): + imgui.set_tooltip( + "Controls visualizer update frequency\nlower values -> more responsive visualizer but slower" + " training\nhigher values -> less responsive visualizer but faster training" + ) + + def on_key_press(self, symbol, modifiers): + if self.ui.is_capturing(): + return + + try: + import pyglet # noqa: PLC0415 + except Exception: + return + + if symbol == pyglet.window.key.SPACE: + self._paused_rendering = not self._paused_rendering + self._paused = self._paused_rendering + return + + super().on_key_press(symbol, modifiers) + + def _render_ui(self): + if not self._fallback_draw_controls: + return super()._render_ui() + + super()._render_ui() + imgui = self.ui.imgui + from contextlib import suppress + + with suppress(Exception): + imgui.set_next_window_pos(imgui.ImVec2(320, 10)) + + flags = 0 + if imgui.begin("Training Controls", flags=flags): + self._render_training_controls(imgui) + imgui.end() + return None + + def _render_left_panel(self): + """Override the left panel to remove the base pause checkbox.""" + import newton as nt + + imgui = self.ui.imgui + nav_highlight_color = self.ui.get_theme_color(imgui.Col_.nav_cursor, (1.0, 1.0, 1.0, 1.0)) + + io = self.ui.io + imgui.set_next_window_pos(imgui.ImVec2(10, 10)) + imgui.set_next_window_size(imgui.ImVec2(300, io.display_size[1] - 20)) + + flags = imgui.WindowFlags_.no_resize.value + + if imgui.begin(f"Newton Viewer v{nt.__version__}", flags=flags): + imgui.separator() + + header_flags = 0 + + imgui.set_next_item_open(True, imgui.Cond_.appearing) + if imgui.collapsing_header("IsaacLab Options"): + for callback in self._ui_callbacks["side"]: + callback(self.ui.imgui) + + if self.model is not None: + imgui.set_next_item_open(True, imgui.Cond_.appearing) + if imgui.collapsing_header("Model Information", flags=header_flags): + imgui.separator() + num_envs = self._metadata.get("num_envs", 0) + imgui.text(f"Environments: {num_envs}") + axis_names = ["X", "Y", "Z"] + imgui.text(f"Up Axis: {axis_names[self.model.up_axis]}") + gravity = wp.to_torch(self.model.gravity)[0] + gravity_text = f"Gravity: ({gravity[0]:.2f}, {gravity[1]:.2f}, {gravity[2]:.2f})" + imgui.text(gravity_text) + + imgui.set_next_item_open(True, imgui.Cond_.appearing) + if imgui.collapsing_header("Visualization", flags=header_flags): + imgui.separator() + + show_joints = self.show_joints + changed, self.show_joints = imgui.checkbox("Show Joints", show_joints) + + show_contacts = self.show_contacts + changed, self.show_contacts = imgui.checkbox("Show Contacts", show_contacts) + + show_springs = self.show_springs + changed, self.show_springs = imgui.checkbox("Show Springs", show_springs) + + show_com = self.show_com + changed, self.show_com = imgui.checkbox("Show Center of Mass", show_com) + + imgui.set_next_item_open(True, imgui.Cond_.appearing) + if imgui.collapsing_header("Rendering Options"): + imgui.separator() + + changed, self.renderer.draw_sky = imgui.checkbox("Sky", self.renderer.draw_sky) + changed, self.renderer.draw_shadows = imgui.checkbox("Shadows", self.renderer.draw_shadows) + changed, self.renderer.draw_wireframe = imgui.checkbox("Wireframe", self.renderer.draw_wireframe) + + changed, self.renderer._light_color = imgui.color_edit3("Light Color", self.renderer._light_color) + changed, self.renderer.sky_upper = imgui.color_edit3("Sky Color", self.renderer.sky_upper) + changed, self.renderer.sky_lower = imgui.color_edit3("Ground Color", self.renderer.sky_lower) + + imgui.set_next_item_open(True, imgui.Cond_.appearing) + if imgui.collapsing_header("Camera"): + imgui.separator() + + pos = self.camera.pos + pos_text = f"Position: ({pos[0]:.2f}, {pos[1]:.2f}, {pos[2]:.2f})" + imgui.text(pos_text) + imgui.text(f"FOV: {self.camera.fov:.1f}°") + imgui.text(f"Yaw: {self.camera.yaw:.1f}°") + imgui.text(f"Pitch: {self.camera.pitch:.1f}°") + + imgui.separator() + imgui.push_style_color(imgui.Col_.text, imgui.ImVec4(*nav_highlight_color)) + imgui.text("Controls:") + imgui.pop_style_color() + imgui.text("WASD - Forward/Left/Back/Right") + imgui.text("QE - Down/Up") + imgui.text("Left Click - Look around") + imgui.text("Scroll - Zoom") + imgui.text("Space - Pause/Resume Rendering") + imgui.text("H - Toggle UI") + imgui.text("ESC - Exit") + + imgui.end() + return + + +class NewtonVisualizer(Visualizer): + """Newton OpenGL visualizer for Isaac Lab.""" + + def __init__(self, cfg: NewtonVisualizerCfg): + super().__init__(cfg) + self.cfg: NewtonVisualizerCfg = cfg + self._viewer: NewtonViewerGL | None = None + self._sim_time = 0.0 + self._step_counter = 0 + self._model = None + self._state = None + self._update_frequency = cfg.update_frequency + self._scene_data_provider = None + + def initialize(self, scene_data: dict[str, Any] | None = None) -> None: + if self._is_initialized: + return + + if scene_data and "scene_data_provider" in scene_data: + self._scene_data_provider = scene_data["scene_data_provider"] + + metadata = {} + if self._scene_data_provider: + self._model = self._scene_data_provider.get_model() + self._state = self._scene_data_provider.get_state() + metadata = self._scene_data_provider.get_metadata() + else: + try: + from isaaclab.sim._impl.newton_manager import NewtonManager + + self._model = NewtonManager._model + self._state = NewtonManager._state_0 + metadata = { + "physics_backend": "newton", + "num_envs": NewtonManager._num_envs if NewtonManager._num_envs is not None else 0, + "gravity_vector": NewtonManager._gravity_vector, + "clone_physics_only": NewtonManager._clone_physics_only, + } + except Exception: + pass + + if self._model is None: + raise RuntimeError("Newton visualizer requires a Newton Model. Ensure a scene data provider is available.") + + self._viewer = NewtonViewerGL( + width=self.cfg.window_width, + height=self.cfg.window_height, + metadata=metadata, + update_frequency=self.cfg.update_frequency, + ) + + self._viewer.set_model(self._model) + self._viewer.set_world_offsets((0.0, 0.0, 0.0)) + self._viewer.camera.pos = wp.vec3(*self.cfg.camera_position) + self._viewer.up_axis = 2 # Z-up + + cam_pos = np.array(self.cfg.camera_position, dtype=np.float32) + cam_target = np.array(self.cfg.camera_target, dtype=np.float32) + direction = cam_target - cam_pos + yaw = np.degrees(np.arctan2(direction[1], direction[0])) + horizontal_dist = np.sqrt(direction[0] ** 2 + direction[1] ** 2) + pitch = np.degrees(np.arctan2(direction[2], horizontal_dist)) + + self._viewer.camera.yaw = float(yaw) + self._viewer.camera.pitch = float(pitch) + + self._viewer.scaling = 1.0 + self._viewer._paused = False + + self._viewer.show_joints = self.cfg.show_joints + self._viewer.show_contacts = self.cfg.show_contacts + self._viewer.show_springs = self.cfg.show_springs + self._viewer.show_com = self.cfg.show_com + + self._viewer.renderer.draw_shadows = self.cfg.enable_shadows + self._viewer.renderer.draw_sky = self.cfg.enable_sky + self._viewer.renderer.draw_wireframe = self.cfg.enable_wireframe + + self._viewer.renderer.sky_upper = self.cfg.background_color + self._viewer.renderer.sky_lower = self.cfg.ground_color + self._viewer.renderer._light_color = self.cfg.light_color + + self._is_initialized = True + + def step(self, dt: float, state: Any | None = None) -> None: + if not self._is_initialized or self._is_closed or self._viewer is None: + return + + self._sim_time += dt + self._step_counter += 1 + + if self._scene_data_provider: + self._state = self._scene_data_provider.get_state() + else: + try: + from isaaclab.sim._impl.newton_manager import NewtonManager + + self._state = NewtonManager._state_0 + except Exception: + self._state = None + + update_frequency = self._viewer._update_frequency if self._viewer else self._update_frequency + if self._step_counter % update_frequency != 0: + return + + with contextlib.suppress(Exception): + if not self._viewer.is_paused(): + self._viewer.begin_frame(self._sim_time) + if self._state is not None: + self._viewer.log_state(self._state) + self._viewer.end_frame() + else: + self._viewer._update() + + def close(self) -> None: + if self._is_closed: + return + if self._viewer is not None: + self._viewer = None + self._is_closed = True + + def is_running(self) -> bool: + if not self._is_initialized or self._is_closed or self._viewer is None: + return False + return self._viewer.is_running() + + def supports_markers(self) -> bool: + return False + + def supports_live_plots(self) -> bool: + return False + + def is_training_paused(self) -> bool: + if not self._is_initialized or self._viewer is None: + return False + return self._viewer.is_training_paused() + + def is_rendering_paused(self) -> bool: + if not self._is_initialized or self._viewer is None: + return False + return self._viewer.is_rendering_paused() diff --git a/source/isaaclab/isaaclab/visualizers/newton_visualizer_cfg.py b/source/isaaclab/isaaclab/visualizers/newton_visualizer_cfg.py new file mode 100644 index 00000000000..0465c43ba6b --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/newton_visualizer_cfg.py @@ -0,0 +1,57 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for Newton OpenGL Visualizer.""" + +from isaaclab.utils import configclass + +from .visualizer_cfg import VisualizerCfg + + +@configclass +class NewtonVisualizerCfg(VisualizerCfg): + """Configuration for Newton OpenGL visualizer.""" + + visualizer_type: str = "newton" + """Type identifier for Newton visualizer.""" + + window_width: int = 1920 + """Window width in pixels.""" + + window_height: int = 1080 + """Window height in pixels.""" + + update_frequency: int = 1 + """Visualizer update frequency (updates every N frames).""" + + show_joints: bool = False + """Show joint visualization.""" + + show_contacts: bool = False + """Show contact visualization.""" + + show_springs: bool = False + """Show spring visualization.""" + + show_com: bool = False + """Show center of mass visualization.""" + + enable_shadows: bool = True + """Enable shadow rendering.""" + + enable_sky: bool = True + """Enable sky rendering.""" + + enable_wireframe: bool = False + """Enable wireframe rendering.""" + + background_color: tuple[float, float, float] = (0.53, 0.81, 0.92) + """Background/sky color RGB [0,1].""" + + ground_color: tuple[float, float, float] = (0.18, 0.20, 0.25) + """Ground color RGB [0,1].""" + + light_color: tuple[float, float, float] = (1.0, 1.0, 1.0) + """Light color RGB [0,1].""" diff --git a/source/isaaclab/isaaclab/visualizers/ov_visualizer.py b/source/isaaclab/isaaclab/visualizers/ov_visualizer.py new file mode 100644 index 00000000000..cd16c203ea9 --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/ov_visualizer.py @@ -0,0 +1,276 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Omniverse-based visualizer using Isaac Sim viewport.""" + +from __future__ import annotations + +import asyncio +import logging +from typing import Any + +from pxr import UsdGeom + +from .ov_visualizer_cfg import OVVisualizerCfg +from .visualizer import Visualizer + +logger = logging.getLogger(__name__) + + +class OVVisualizer(Visualizer): + """Omniverse visualizer using Isaac Sim viewport.""" + + def __init__(self, cfg: OVVisualizerCfg): + super().__init__(cfg) + self.cfg: OVVisualizerCfg = cfg + + self._simulation_app = None + self._viewport_window = None + self._viewport_api = None + self._is_initialized = False + self._sim_time = 0.0 + self._step_counter = 0 + + def initialize(self, scene_data: dict[str, Any] | None = None) -> None: + if self._is_initialized: + logger.warning("[OVVisualizer] Already initialized.") + return + + usd_stage = None + simulation_context = None + if scene_data is not None: + usd_stage = scene_data.get("usd_stage") + simulation_context = scene_data.get("simulation_context") + + if usd_stage is None: + raise RuntimeError("OV visualizer requires a USD stage.") + + metadata = {} + if simulation_context is not None: + num_envs = 0 + if hasattr(simulation_context, "scene") and simulation_context.scene is not None: + if hasattr(simulation_context.scene, "num_envs"): + num_envs = simulation_context.scene.num_envs + + metadata = { + "num_envs": num_envs, + "physics_backend": "omni", + "env_prim_pattern": "/World/envs/env_{}", + } + + self._ensure_simulation_app() + self._setup_viewport(usd_stage, metadata) + + num_envs = metadata.get("num_envs", 0) + physics_backend = metadata.get("physics_backend", "unknown") + logger.info(f"[OVVisualizer] Initialized ({num_envs} envs, {physics_backend} physics)") + + self._is_initialized = True + + def step(self, dt: float, state: Any | None = None) -> None: + if not self._is_initialized: + return + self._sim_time += dt + self._step_counter += 1 + + def close(self) -> None: + if not self._is_initialized: + return + self._simulation_app = None + self._viewport_window = None + self._viewport_api = None + self._is_initialized = False + + def is_running(self) -> bool: + if self._simulation_app is None: + return False + return self._simulation_app.is_running() + + def is_training_paused(self) -> bool: + return False + + def supports_markers(self) -> bool: + return True + + def supports_live_plots(self) -> bool: + return True + + def set_camera_view( + self, eye: tuple[float, float, float] | list[float], target: tuple[float, float, float] | list[float] + ) -> None: + if not self._is_initialized: + logger.warning("[OVVisualizer] Cannot set camera view - visualizer not initialized.") + return + self._set_viewport_camera(tuple(eye), tuple(target)) + + def _ensure_simulation_app(self) -> None: + try: + import omni.kit.app + + app = omni.kit.app.get_app() + if app is None or not app.is_running(): + raise RuntimeError( + "[OVVisualizer] No Isaac Sim app is running. " + "OV visualizer requires Isaac Sim to be launched before initialization." + ) + + try: + from isaacsim import SimulationApp + + sim_app = None + if hasattr(SimulationApp, "_instance") and SimulationApp._instance is not None: + sim_app = SimulationApp._instance + elif hasattr(SimulationApp, "instance") and callable(SimulationApp.instance): + sim_app = SimulationApp.instance() + + if sim_app is not None: + self._simulation_app = sim_app + if self._simulation_app.config.get("headless", False): + logger.warning( + "[OVVisualizer] Running in headless mode. " + "OV visualizer requires GUI mode (launch with --headless=False)." + ) + else: + logger.info("[OVVisualizer] Using existing Isaac Sim app instance.") + else: + logger.info("[OVVisualizer] Isaac Sim app is running (via omni.kit.app).") + except ImportError: + logger.info("[OVVisualizer] Using running Isaac Sim app (SimulationApp module not available).") + except ImportError as exc: + raise ImportError( + f"[OVVisualizer] Could not import omni.kit.app: {exc}. Isaac Sim may not be installed or not running." + ) + + def _setup_viewport(self, usd_stage, metadata: dict) -> None: + try: + import omni.kit.viewport.utility as vp_utils + from omni.ui import DockPosition + + if self.cfg.create_viewport and self.cfg.viewport_name: + dock_position_map = { + "LEFT": DockPosition.LEFT, + "RIGHT": DockPosition.RIGHT, + "BOTTOM": DockPosition.BOTTOM, + "SAME": DockPosition.SAME, + } + dock_pos = dock_position_map.get(self.cfg.dock_position.upper(), DockPosition.SAME) + + self._viewport_window = vp_utils.create_viewport_window( + name=self.cfg.viewport_name, + width=self.cfg.window_width, + height=self.cfg.window_height, + position_x=50, + position_y=50, + docked=True, + ) + + logger.info(f"[OVVisualizer] Created viewport '{self.cfg.viewport_name}'") + asyncio.ensure_future(self._dock_viewport_async(self.cfg.viewport_name, dock_pos)) + + if self._viewport_window: + self._create_and_assign_camera(usd_stage) + else: + if self.cfg.viewport_name: + self._viewport_window = vp_utils.get_viewport_window_by_name(self.cfg.viewport_name) + if self._viewport_window is None: + logger.warning( + f"[OVVisualizer] Viewport '{self.cfg.viewport_name}' not found. Using active viewport." + ) + self._viewport_window = vp_utils.get_active_viewport_window() + else: + logger.info(f"[OVVisualizer] Using existing viewport '{self.cfg.viewport_name}'") + else: + self._viewport_window = vp_utils.get_active_viewport_window() + logger.info("[OVVisualizer] Using existing active viewport") + + if self._viewport_window is None: + logger.warning("[OVVisualizer] Could not get/create viewport.") + return + + self._viewport_api = self._viewport_window.viewport_api + self._set_viewport_camera(self.cfg.camera_position, self.cfg.camera_target) + logger.info(f"[OVVisualizer] Viewport configured (size: {self.cfg.window_width}x{self.cfg.window_height})") + except ImportError as exc: + logger.warning(f"[OVVisualizer] Viewport utilities unavailable: {exc}") + except Exception as exc: + logger.error(f"[OVVisualizer] Error setting up viewport: {exc}") + + async def _dock_viewport_async(self, viewport_name: str, dock_position) -> None: + try: + import omni.kit.app + import omni.ui + + viewport_window = None + for i in range(10): + viewport_window = omni.ui.Workspace.get_window(viewport_name) + if viewport_window: + logger.info(f"[OVVisualizer] Found viewport window '{viewport_name}' after {i} frames") + break + await omni.kit.app.get_app().next_update_async() + + if not viewport_window: + logger.warning(f"[OVVisualizer] Could not find viewport window '{viewport_name}' in workspace.") + return + + main_viewport = omni.ui.Workspace.get_window("Viewport") + if not main_viewport: + for alt_name in ["/OmniverseKit/Viewport", "Viewport Next"]: + main_viewport = omni.ui.Workspace.get_window(alt_name) + if main_viewport: + break + + if main_viewport and main_viewport != viewport_window: + viewport_window.dock_in(main_viewport, dock_position, 0.5) + await omni.kit.app.get_app().next_update_async() + viewport_window.focus() + viewport_window.visible = True + await omni.kit.app.get_app().next_update_async() + viewport_window.focus() + + logger.info( + f"[OVVisualizer] Docked viewport '{viewport_name}' at position {self.cfg.dock_position} and set as" + " active" + ) + else: + logger.info( + f"[OVVisualizer] Could not find main viewport for docking. Viewport '{viewport_name}' will remain" + " floating." + ) + except Exception as exc: + logger.warning(f"[OVVisualizer] Error docking viewport: {exc}") + + def _create_and_assign_camera(self, usd_stage) -> None: + try: + camera_path = f"/World/Cameras/{self.cfg.viewport_name}_Camera" + camera_prim = usd_stage.GetPrimAtPath(camera_path) + if not camera_prim.IsValid(): + UsdGeom.Camera.Define(usd_stage, camera_path) + logger.info(f"[OVVisualizer] Created camera: {camera_path}") + else: + logger.info(f"[OVVisualizer] Using existing camera: {camera_path}") + + if self._viewport_api: + self._viewport_api.set_active_camera(camera_path) + logger.info(f"[OVVisualizer] Assigned camera '{camera_path}' to viewport '{self.cfg.viewport_name}'") + except Exception as exc: + logger.warning(f"[OVVisualizer] Could not create/assign camera: {exc}. Using default camera.") + + def _set_viewport_camera(self, position: tuple[float, float, float], target: tuple[float, float, float]) -> None: + if self._viewport_api is None: + return + + try: + import isaacsim.core.utils.viewports as vp_utils + + camera_path = self._viewport_api.get_active_camera() + if not camera_path: + camera_path = "/OmniverseKit_Persp" + + vp_utils.set_camera_view( + eye=list(position), target=list(target), camera_prim_path=camera_path, viewport_api=self._viewport_api + ) + logger.info(f"[OVVisualizer] Camera set: pos={position}, target={target}, camera={camera_path}") + except Exception as exc: + logger.warning(f"[OVVisualizer] Could not set camera: {exc}") diff --git a/source/isaaclab/isaaclab/visualizers/ov_visualizer_cfg.py b/source/isaaclab/isaaclab/visualizers/ov_visualizer_cfg.py new file mode 100644 index 00000000000..33099ef34f8 --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/ov_visualizer_cfg.py @@ -0,0 +1,33 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for Omniverse-based visualizer.""" + +from isaaclab.utils import configclass + +from .visualizer_cfg import VisualizerCfg + + +@configclass +class OVVisualizerCfg(VisualizerCfg): + """Configuration for Omniverse visualizer using Isaac Sim viewport.""" + + visualizer_type: str = "omniverse" + """Type identifier for Omniverse visualizer.""" + + viewport_name: str | None = "Visualizer Viewport" + """Viewport name to use. If None, uses active viewport.""" + + create_viewport: bool = True + """Create new viewport with specified name and camera pose.""" + + dock_position: str = "SAME" + """Dock position for new viewport. Options: 'LEFT', 'RIGHT', 'BOTTOM', 'SAME'.""" + + window_width: int = 1280 + """Viewport width in pixels.""" + + window_height: int = 720 + """Viewport height in pixels.""" diff --git a/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py b/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py new file mode 100644 index 00000000000..f75555096cc --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py @@ -0,0 +1,225 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Rerun-based visualizer using rerun-sdk.""" + +from __future__ import annotations + +import logging +from typing import Any + +from .rerun_visualizer_cfg import RerunVisualizerCfg +from .visualizer import Visualizer + +logger = logging.getLogger(__name__) + +try: + import rerun as rr + import rerun.blueprint as rrb + from newton.viewer import ViewerRerun + + _RERUN_AVAILABLE = True +except ImportError: + rr = None + rrb = None + ViewerRerun = None + _RERUN_AVAILABLE = False + + +class NewtonViewerRerun(ViewerRerun if _RERUN_AVAILABLE else object): + """Isaac Lab wrapper for Newton's ViewerRerun.""" + + def __init__( + self, + app_id: str | None = None, + web_port: int | None = None, + keep_historical_data: bool = False, + keep_scalar_history: bool = True, + record_to_rrd: str | None = None, + metadata: dict | None = None, + ): + super().__init__( + app_id=app_id, + web_port=web_port, + serve_web_viewer=True, + keep_historical_data=keep_historical_data, + keep_scalar_history=keep_scalar_history, + record_to_rrd=record_to_rrd, + ) + + self._metadata = metadata or {} + self._log_metadata() + + def _log_metadata(self) -> None: + metadata_text = "# Isaac Lab Scene Metadata\n\n" + physics_backend = self._metadata.get("physics_backend", "unknown") + metadata_text += f"**Physics Backend:** {physics_backend}\n" + num_envs = self._metadata.get("num_envs", 0) + metadata_text += f"**Total Environments:** {num_envs}\n" + + for key, value in self._metadata.items(): + if key not in ["physics_backend", "num_envs"]: + metadata_text += f"**{key}:** {value}\n" + + rr.log("metadata", rr.TextDocument(metadata_text, media_type=rr.MediaType.MARKDOWN)) + + +class RerunVisualizer(Visualizer): + """Rerun web-based visualizer with time scrubbing, recording, and data inspection.""" + + def __init__(self, cfg: RerunVisualizerCfg): + super().__init__(cfg) + self.cfg: RerunVisualizerCfg = cfg + + if not _RERUN_AVAILABLE: + raise ImportError("Rerun visualizer requires rerun-sdk and Newton. Install: pip install rerun-sdk") + + self._viewer: NewtonViewerRerun | None = None + self._model = None + self._state = None + self._is_initialized = False + self._sim_time = 0.0 + self._scene_data_provider = None + + def initialize(self, scene_data: dict[str, Any] | None = None) -> None: + if self._is_initialized: + logger.warning("[RerunVisualizer] Already initialized. Skipping re-initialization.") + return + + if scene_data and "scene_data_provider" in scene_data: + self._scene_data_provider = scene_data["scene_data_provider"] + + metadata = {} + if self._scene_data_provider: + self._model = self._scene_data_provider.get_model() + self._state = self._scene_data_provider.get_state() + metadata = self._scene_data_provider.get_metadata() + else: + try: + from isaaclab.sim._impl.newton_manager import NewtonManager + + self._model = NewtonManager._model + self._state = NewtonManager._state_0 + metadata = { + "physics_backend": "newton", + "num_envs": NewtonManager._num_envs if NewtonManager._num_envs is not None else 0, + "gravity_vector": NewtonManager._gravity_vector, + "clone_physics_only": NewtonManager._clone_physics_only, + } + except Exception: + pass + + if self._model is None: + raise RuntimeError( + "Rerun visualizer requires a Newton Model. Ensure a scene data provider is available." + ) + + if self._state is None: + logger.warning("[RerunVisualizer] No Newton State available. Visualization may not work correctly.") + + try: + if self.cfg.record_to_rrd: + logger.info(f"[RerunVisualizer] Recording enabled to: {self.cfg.record_to_rrd}") + + self._viewer = NewtonViewerRerun( + app_id=self.cfg.app_id, + web_port=self.cfg.web_port, + keep_historical_data=self.cfg.keep_historical_data, + keep_scalar_history=self.cfg.keep_scalar_history, + record_to_rrd=self.cfg.record_to_rrd, + metadata=metadata, + ) + + self._viewer.set_model(self._model) + self._viewer.set_world_offsets((0.0, 0.0, 0.0)) + + try: + cam_pos = self.cfg.camera_position + cam_target = self.cfg.camera_target + + blueprint = rrb.Blueprint( + rrb.Spatial3DView( + name="3D View", + origin="/", + eye_controls=rrb.EyeControls3D( + position=cam_pos, + look_target=cam_target, + ), + ), + collapse_panels=True, + ) + rr.send_blueprint(blueprint) + + logger.info(f"[RerunVisualizer] Set initial camera view: position={cam_pos}, target={cam_target}") + except Exception as exc: + logger.warning(f"[RerunVisualizer] Could not set initial camera view: {exc}") + + num_envs = metadata.get("num_envs", 0) + physics_backend = metadata.get("physics_backend", "unknown") + logger.info(f"[RerunVisualizer] Initialized with {num_envs} environments (physics: {physics_backend})") + + self._is_initialized = True + except Exception as exc: + logger.error(f"[RerunVisualizer] Failed to initialize viewer: {exc}") + raise + + def step(self, dt: float, state: Any | None = None) -> None: + if not self._is_initialized or self._viewer is None: + logger.warning("[RerunVisualizer] Not initialized. Call initialize() first.") + return + + if self._scene_data_provider: + self._state = self._scene_data_provider.get_state() + else: + try: + from isaaclab.sim._impl.newton_manager import NewtonManager + + self._state = NewtonManager._state_0 + except Exception: + self._state = None + + self._sim_time += dt + + self._viewer.begin_frame(self._sim_time) + if self._state is not None: + self._viewer.log_state(self._state) + self._viewer.end_frame() + + def close(self) -> None: + if not self._is_initialized or self._viewer is None: + return + + try: + if self.cfg.record_to_rrd: + logger.info(f"[RerunVisualizer] Finalizing recording to: {self.cfg.record_to_rrd}") + self._viewer.close() + logger.info("[RerunVisualizer] Closed successfully.") + if self.cfg.record_to_rrd: + import os + + if os.path.exists(self.cfg.record_to_rrd): + size = os.path.getsize(self.cfg.record_to_rrd) + logger.info(f"[RerunVisualizer] Recording saved: {self.cfg.record_to_rrd} ({size} bytes)") + else: + logger.warning(f"[RerunVisualizer] Recording file not found: {self.cfg.record_to_rrd}") + except Exception as exc: + logger.warning(f"[RerunVisualizer] Error during close: {exc}") + + self._viewer = None + self._is_initialized = False + + def is_running(self) -> bool: + if self._viewer is None: + return False + return self._viewer.is_running() + + def is_training_paused(self) -> bool: + return False + + def supports_markers(self) -> bool: + return False + + def supports_live_plots(self) -> bool: + return False diff --git a/source/isaaclab/isaaclab/visualizers/rerun_visualizer_cfg.py b/source/isaaclab/isaaclab/visualizers/rerun_visualizer_cfg.py new file mode 100644 index 00000000000..8b1e32d7ded --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/rerun_visualizer_cfg.py @@ -0,0 +1,35 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Rerun visualizer.""" + +from __future__ import annotations + +from isaaclab.utils import configclass + +from .visualizer_cfg import VisualizerCfg + + +@configclass +class RerunVisualizerCfg(VisualizerCfg): + """Configuration for Rerun visualizer (web-based visualization).""" + + visualizer_type: str = "rerun" + """Type identifier for Rerun visualizer.""" + + app_id: str = "isaaclab-simulation" + """Application identifier shown in viewer title.""" + + web_port: int = 9090 + """Port of the local rerun web viewer which is launched in the browser.""" + + keep_historical_data: bool = False + """Keep transform history for time scrubbing (False = constant memory for training).""" + + keep_scalar_history: bool = False + """Keep scalar/plot history in timeline.""" + + record_to_rrd: str | None = None + """Path to save .rrd recording file. None = no recording.""" diff --git a/source/isaaclab/isaaclab/visualizers/visualizer.py b/source/isaaclab/isaaclab/visualizers/visualizer.py new file mode 100644 index 00000000000..f3991113d3b --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/visualizer.py @@ -0,0 +1,78 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Base class for visualizers.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from typing import TYPE_CHECKING, Any + +if TYPE_CHECKING: + from .visualizer_cfg import VisualizerCfg + + +class Visualizer(ABC): + """Base class for all visualizer backends. + + Lifecycle: __init__() -> initialize() -> step() (repeated) -> close() + """ + + def __init__(self, cfg: VisualizerCfg): + """Initialize visualizer with config.""" + self.cfg = cfg + self._is_initialized = False + self._is_closed = False + + @abstractmethod + def initialize(self, scene_data: dict[str, Any] | None = None) -> None: + """Initialize visualizer with scene data (model, state, usd_stage, etc.).""" + raise NotImplementedError + + @abstractmethod + def step(self, dt: float, state: Any | None = None) -> None: + """Update visualization for one step. + + Args: + dt: Time step in seconds. + state: Updated physics state (e.g., newton.State). + """ + raise NotImplementedError + + @abstractmethod + def close(self) -> None: + """Clean up resources.""" + raise NotImplementedError + + @abstractmethod + def is_running(self) -> bool: + """Check if visualizer is still running (e.g., window not closed).""" + raise NotImplementedError + + def is_training_paused(self) -> bool: + """Check if training is paused by visualizer controls.""" + return False + + def is_rendering_paused(self) -> bool: + """Check if rendering is paused by visualizer controls.""" + return False + + @property + def is_initialized(self) -> bool: + """Check if initialize() has been called.""" + return self._is_initialized + + @property + def is_closed(self) -> bool: + """Check if close() has been called.""" + return self._is_closed + + def supports_markers(self) -> bool: + """Check if visualizer supports VisualizationMarkers.""" + return False + + def supports_live_plots(self) -> bool: + """Check if visualizer supports LivePlots.""" + return False diff --git a/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py b/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py new file mode 100644 index 00000000000..c361d3dfe90 --- /dev/null +++ b/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py @@ -0,0 +1,63 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Base configuration for visualizers.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.utils import configclass + +if TYPE_CHECKING: + from .visualizer import Visualizer + + +@configclass +class VisualizerCfg: + """Base configuration for all visualizer backends.""" + + visualizer_type: str | None = None + """Type identifier (e.g., 'newton', 'rerun', 'omniverse').""" + + enable_markers: bool = True + """Enable visualization markers (debug drawing).""" + + enable_live_plots: bool = True + """Enable live plotting of data.""" + + camera_position: tuple[float, float, float] = (8.0, 8.0, 3.0) + """Initial camera position (x, y, z) in world coordinates.""" + + camera_target: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Initial camera target/look-at point (x, y, z) in world coordinates.""" + + def get_visualizer_type(self) -> str | None: + """Get the visualizer type identifier.""" + return self.visualizer_type + + def create_visualizer(self) -> "Visualizer": + """Create visualizer instance from this config using factory pattern.""" + from . import get_visualizer_class + + if self.visualizer_type is None: + raise ValueError( + "Cannot create visualizer from base VisualizerCfg class. " + "Use a specific visualizer config: NewtonVisualizerCfg, RerunVisualizerCfg, or OVVisualizerCfg." + ) + + visualizer_class = get_visualizer_class(self.visualizer_type) + if visualizer_class is None: + if self.visualizer_type in ("newton", "rerun"): + raise ImportError( + f"Visualizer '{self.visualizer_type}' requires the Newton Python module and its dependencies. " + "Install the Newton backend (e.g., newton package/isaaclab_newton) and retry." + ) + raise ValueError( + f"Visualizer type '{self.visualizer_type}' is not registered. " + "Valid types: 'newton', 'rerun', 'omniverse'." + ) + + return visualizer_class(self) From 434908b32a65ac156a4d2e78a57d7a340163265f Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Tue, 27 Jan 2026 18:09:30 +0000 Subject: [PATCH 03/14] wip: initial design for adding viz --- .../visualization.rst | 4 +- .../isaaclab/envs/manager_based_env.py | 2 +- .../isaaclab/scene/interactive_scene.py | 2 +- .../newton_scene_data_provider.py | 47 +++++++------- .../ov_scene_data_provider.py | 46 +++++++++---- .../scene_data_provider.py | 65 +++++++++++-------- .../isaaclab/sim/simulation_context.py | 8 ++- .../isaaclab/visualizers/newton_visualizer.py | 29 +++++++-- .../visualizers/newton_visualizer_cfg.py | 8 +-- .../isaaclab/visualizers/ov_visualizer.py | 15 ++--- .../isaaclab/visualizers/rerun_visualizer.py | 6 +- source/isaaclab/setup.py | 10 +-- 12 files changed, 144 insertions(+), 98 deletions(-) diff --git a/docs/source/experimental-features/newton-physics-integration/visualization.rst b/docs/source/experimental-features/newton-physics-integration/visualization.rst index f5443573393..661a98cf9fe 100644 --- a/docs/source/experimental-features/newton-physics-integration/visualization.rst +++ b/docs/source/experimental-features/newton-physics-integration/visualization.rst @@ -213,8 +213,8 @@ Newton Visualizer enable_wireframe=False, # Enable wireframe mode # Color customization - background_color=(0.53, 0.81, 0.92), # Sky/background color (RGB [0,1]) - ground_color=(0.18, 0.20, 0.25), # Ground plane color (RGB [0,1]) + sky_upper_color=(0.53, 0.81, 0.92), # Sky upper color (RGB [0,1]) + sky_lower_color=(0.18, 0.20, 0.25), # Sky lower color (RGB [0,1]) light_color=(1.0, 1.0, 1.0), # Directional light color (RGB [0,1]) ) diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 88bf0d85271..e4c8d5cb14d 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -149,7 +149,7 @@ def __init__(self, cfg: ManagerBasedEnvCfg): "[SceneDebug] env prims after InteractiveScene: " f"num_envs_setting={self.cfg.scene.num_envs}, env_prims={len(env_prim_paths)}" ) - import ipdb; ipdb.set_trace() + # import ipdb; ipdb.set_trace() # set up camera viewport controller # viewport is not available in other rendering modes so the function will throw a warning diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 89c3fed939b..86abca135a4 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -122,7 +122,7 @@ def __init__(self, cfg: InteractiveSceneCfg): self.cfg = cfg # TODO(mtrepte): remove - # self.cfg.clone_in_fabric = False + self.cfg.clone_in_fabric = False # initialize scene elements self._terrain = None diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py index d664abbe343..7144b599b3a 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py @@ -17,25 +17,21 @@ class NewtonSceneDataProvider(SceneDataProviderBase): - """Newton-backed scene data provider (when Newton physics is the active backend).""" + """Scene data provider for Newton Warp physics backend. + + Native (cheap): Newton Model/State from NewtonManager + Adapted (future): USD stage (would need Newton→USD sync for OV visualizer) + """ def __init__(self, visualizer_cfgs: list[Any] | None) -> None: - self._has_newton_visualizer = False - self._has_rerun_visualizer = False self._has_ov_visualizer = False self._metadata: dict[str, Any] = {} if visualizer_cfgs: for cfg in visualizer_cfgs: - viz_type = getattr(cfg, "visualizer_type", None) - if viz_type == "newton": - self._has_newton_visualizer = True - elif viz_type == "rerun": - self._has_rerun_visualizer = True - elif viz_type == "omniverse": + if getattr(cfg, "visualizer_type", None) == "omniverse": self._has_ov_visualizer = True - # Lazy import to keep develop usable without Newton installed. try: from isaaclab.sim._impl.newton_manager import NewtonManager @@ -49,51 +45,54 @@ def __init__(self, visualizer_cfgs: list[Any] | None) -> None: self._metadata = {"physics_backend": "newton"} def update(self) -> None: - return None + """No-op for Newton backend (state updated by Newton solver).""" + pass - def get_model(self): - if not (self._has_newton_visualizer or self._has_rerun_visualizer): - return None + def get_newton_model(self) -> Any | None: + """NATIVE: Newton Model from NewtonManager.""" try: from isaaclab.sim._impl.newton_manager import NewtonManager - return NewtonManager._model except Exception: return None - def get_state(self): - if not (self._has_newton_visualizer or self._has_rerun_visualizer): - return None + def get_newton_state(self) -> Any | None: + """NATIVE: Newton State from NewtonManager.""" try: from isaaclab.sim._impl.newton_manager import NewtonManager - return NewtonManager._state_0 except Exception: return None + def get_usd_stage(self) -> None: + """UNAVAILABLE: Newton backend doesn't provide USD (future: Newton→USD sync).""" + return None + def get_metadata(self) -> dict[str, Any]: return dict(self._metadata) def get_transforms(self) -> dict[str, Any] | None: + """Extract transforms from Newton state (future work).""" return None def get_velocities(self) -> dict[str, Any] | None: try: from isaaclab.sim._impl.newton_manager import NewtonManager - - state = NewtonManager._state_0 - if state is None: + if NewtonManager._state_0 is None: return None - return {"body_qd": state.body_qd} + return {"body_qd": NewtonManager._state_0.body_qd} except Exception: return None def get_contacts(self) -> dict[str, Any] | None: try: from isaaclab.sim._impl.newton_manager import NewtonManager - if NewtonManager._contacts is None: return None return {"contacts": NewtonManager._contacts} except Exception: return None + + def get_mesh_data(self) -> dict[str, Any] | None: + """ADAPTED: Extract mesh data from Newton shapes (future work).""" + return None \ No newline at end of file diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py index 52e5afd0fd9..462421d4073 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py @@ -4,11 +4,12 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Omni PhysX-backed scene data provider.""" +"""OV (Omniverse) scene data provider for Omni PhysX backend.""" from __future__ import annotations import logging +import os import re from typing import Any @@ -17,11 +18,13 @@ logger = logging.getLogger(__name__) -class OmniSceneDataProvider(SceneDataProviderBase): - """Omni PhysX-backed scene data provider. - - This provider can generate a Newton-compatible Model/State for use by Newton and Rerun - visualizers while the active physics backend is Omni PhysX. +class OVSceneDataProvider(SceneDataProviderBase): + """Scene data provider for Omni PhysX physics backend. + + Native (cheap): USD stage, PhysX transforms/velocities/contacts + Adapted (expensive): Newton Model/State (built from USD, synced each step) + + Performance: Only builds Newton data if Newton/Rerun visualizers are active. """ def __init__(self, visualizer_cfgs: list[Any] | None, stage, simulation_context) -> None: @@ -65,10 +68,13 @@ def __init__(self, visualizer_cfgs: list[Any] | None, stage, simulation_context) self._set_body_q_kernel = None self._up_axis = UsdGeom.GetStageUpAxis(self._stage) + # Only build Newton model if Newton/Rerun visualizers need it if self._has_newton_visualizer or self._has_rerun_visualizer: self._build_newton_model_from_usd() self._setup_rigid_body_view() self._setup_articulation_view() + elif self._has_ov_visualizer: + logger.info("[OVSceneDataProvider] OV visualizer only - skipping Newton model build") def _get_num_envs(self) -> int: try: @@ -115,7 +121,7 @@ def _build_newton_model_from_usd(self) -> None: num_envs = self._get_num_envs() - import ipdb; ipdb.set_trace() + # import ipdb; ipdb.set_trace() env_prim_paths = find_matching_prim_paths("/World/envs/env_.*", stage=self._stage) print( "[SceneDataProvider] Stage env prims before add_usd: "+ @@ -453,8 +459,9 @@ def _set_body_q( return None def update(self) -> None: + """Sync PhysX transforms to Newton state if Newton/Rerun visualizers are active.""" if not (self._has_newton_visualizer or self._has_rerun_visualizer): - return + return # OV visualizer only - USD auto-synced by omni.physics self._refresh_newton_model_if_needed() if self._newton_state is None: return @@ -490,9 +497,12 @@ def update(self) -> None: positions = positions.reshape(-1, 3).to(dtype=torch.float32, device=self._device) orientations = orientations.reshape(-1, 4).to(dtype=torch.float32, device=self._device) - # TODO(mtrepte) currently converting quaternion convention from wxyz to xyzw - # in the future, we can avoid this step - orientations_xyzw = convert_quat(orientations, to="xyzw") + # NOTE: PhysX tensor views return quats in xyzw, while XformPrimView returns wxyz. + # Convert only when needed to avoid scrambling orientations. + if source_view == "xform_view": + orientations_xyzw = convert_quat(orientations, to="xyzw") + else: + orientations_xyzw = orientations positions_wp = wp.from_torch(positions, dtype=wp.vec3) orientations_wp = wp.from_torch(orientations_xyzw, dtype=wp.quatf) @@ -522,16 +532,26 @@ def update(self) -> None: except Exception as exc: logger.debug(f"[SceneDataProvider] Failed to sync Omni transforms to Newton state: {exc}") - def get_model(self): + def get_newton_model(self) -> Any | None: + """ADAPTED: Newton Model built from USD (only if Newton/Rerun visualizers active).""" if not (self._has_newton_visualizer or self._has_rerun_visualizer): return None return self._newton_model - def get_state(self): + def get_newton_state(self) -> Any | None: + """ADAPTED: Newton State synced from PhysX (only if Newton/Rerun visualizers active).""" if not (self._has_newton_visualizer or self._has_rerun_visualizer): return None return self._newton_state + def get_usd_stage(self) -> Any: + """NATIVE: USD stage.""" + return self._stage + + def get_mesh_data(self) -> dict[str, Any] | None: + """NATIVE: Extract mesh data from USD stage (future work).""" + return None + def get_metadata(self) -> dict[str, Any]: return dict(self._metadata) diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py index a999315d4af..dae8ee5c468 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py @@ -16,46 +16,52 @@ class SceneDataProviderBase(ABC): - """Base interface for scene data providers.""" + """Base interface for scene data providers. + + Provides simulation data in multiple formats for visualizers, renderers, + and other downstream consumers. Each backend provider implements this interface, + exposing native data cheaply and adapted data when needed. + """ @abstractmethod def update(self) -> None: - """Sync any per-step data needed by visualizers.""" + """Update adapted data for current simulation step.""" @abstractmethod - def get_model(self): - """Return a physics-backend model object, if applicable.""" + def get_newton_model(self) -> Any | None: + """Get Newton Model.""" @abstractmethod - def get_state(self): - """Return a physics-backend state object, if applicable.""" + def get_newton_state(self) -> Any | None: + """Get Newton State.""" + + @abstractmethod + def get_usd_stage(self) -> Any | None: + """Get USD stage.""" @abstractmethod def get_metadata(self) -> dict[str, Any]: - """Return basic metadata about the scene/backend.""" + """Get provider metadata and performance hints.""" @abstractmethod def get_transforms(self) -> dict[str, Any] | None: - """Return transform data keyed by semantic names.""" + """Get world-space transforms in backend-agnostic format.""" def get_velocities(self) -> dict[str, Any] | None: - """Return velocity data keyed by semantic names.""" - # TODO: Populate linear/angular velocities once available per backend. + """Get velocities in backend-agnostic format.""" return None def get_contacts(self) -> dict[str, Any] | None: - """Return contact data keyed by semantic names.""" - # TODO: Populate contact data once available per backend. + """Get contact data in backend-agnostic format.""" return None - def get_meshes(self) -> dict[str, Any] | None: - """Return mesh/material data keyed by semantic names.""" - # TODO: Populate mesh/material data once available per backend. + def get_mesh_data(self) -> dict[str, Any] | None: + """Get mesh geometry and materials.""" return None class SceneDataProvider: - """Facade that selects the correct scene data provider for the active backend.""" + """Facade that creates appropriate provider based on physics backend.""" def __init__( self, @@ -73,30 +79,33 @@ def __init__( self._provider = NewtonSceneDataProvider(visualizer_cfgs) elif backend == "omni": if stage is None or simulation_context is None: - logger.warning( - "Omni scene data provider requires stage and simulation context; skipping initialization." - ) + logger.warning("OV scene data provider requires stage and simulation context.") self._provider = None else: - from .ov_scene_data_provider import OmniSceneDataProvider + from .ov_scene_data_provider import OVSceneDataProvider - self._provider = OmniSceneDataProvider(visualizer_cfgs, stage, simulation_context) + self._provider = OVSceneDataProvider(visualizer_cfgs, stage, simulation_context) else: - logger.warning(f"Unknown physics backend '{backend}'. No scene data provider created.") + logger.warning(f"Unknown physics backend '{backend}'.") def update(self) -> None: if self._provider is not None: self._provider.update() - def get_model(self): + def get_newton_model(self) -> Any | None: + if self._provider is None: + return None + return self._provider.get_newton_model() + + def get_newton_state(self) -> Any | None: if self._provider is None: return None - return self._provider.get_model() + return self._provider.get_newton_state() - def get_state(self): + def get_usd_stage(self) -> Any | None: if self._provider is None: return None - return self._provider.get_state() + return self._provider.get_usd_stage() def get_metadata(self) -> dict[str, Any]: if self._provider is None: @@ -118,7 +127,7 @@ def get_contacts(self) -> dict[str, Any] | None: return None return self._provider.get_contacts() - def get_meshes(self) -> dict[str, Any] | None: + def get_mesh_data(self) -> dict[str, Any] | None: if self._provider is None: return None - return self._provider.get_meshes() + return self._provider.get_mesh_data() \ No newline at end of file diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index fb9a14fb1a0..95ba4eff5a7 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -564,9 +564,13 @@ def initialize_visualizers(self) -> None: try: visualizer = viz_cfg.create_visualizer() scene_data: dict[str, Any] = {"scene_data_provider": self._scene_data_provider} + + # OV visualizer gets USD stage if viz_cfg.visualizer_type == "omniverse": - scene_data["usd_stage"] = self.stage - scene_data["simulation_context"] = self + if self._scene_data_provider: + scene_data["usd_stage"] = self._scene_data_provider.get_usd_stage() + else: + scene_data["usd_stage"] = self.stage visualizer.initialize(scene_data) self._visualizers.append(visualizer) diff --git a/source/isaaclab/isaaclab/visualizers/newton_visualizer.py b/source/isaaclab/isaaclab/visualizers/newton_visualizer.py index b3c01fedb8a..719331a31dc 100644 --- a/source/isaaclab/isaaclab/visualizers/newton_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/newton_visualizer.py @@ -160,8 +160,8 @@ def _render_left_panel(self): changed, self.renderer.draw_wireframe = imgui.checkbox("Wireframe", self.renderer.draw_wireframe) changed, self.renderer._light_color = imgui.color_edit3("Light Color", self.renderer._light_color) - changed, self.renderer.sky_upper = imgui.color_edit3("Sky Color", self.renderer.sky_upper) - changed, self.renderer.sky_lower = imgui.color_edit3("Ground Color", self.renderer.sky_lower) + changed, self.renderer.sky_upper = imgui.color_edit3("Upper Sky Color", self.renderer.sky_upper) + changed, self.renderer.sky_lower = imgui.color_edit3("Lower Sky Color", self.renderer.sky_lower) imgui.set_next_item_open(True, imgui.Cond_.appearing) if imgui.collapsing_header("Camera"): @@ -213,8 +213,8 @@ def initialize(self, scene_data: dict[str, Any] | None = None) -> None: metadata = {} if self._scene_data_provider: - self._model = self._scene_data_provider.get_model() - self._state = self._scene_data_provider.get_state() + self._model = self._scene_data_provider.get_newton_model() + self._state = self._scene_data_provider.get_newton_state() metadata = self._scene_data_provider.get_metadata() else: try: @@ -268,8 +268,8 @@ def initialize(self, scene_data: dict[str, Any] | None = None) -> None: self._viewer.renderer.draw_sky = self.cfg.enable_sky self._viewer.renderer.draw_wireframe = self.cfg.enable_wireframe - self._viewer.renderer.sky_upper = self.cfg.background_color - self._viewer.renderer.sky_lower = self.cfg.ground_color + self._viewer.renderer.sky_upper = self.cfg.sky_upper_color + self._viewer.renderer.sky_lower = self.cfg.sky_lower_color self._viewer.renderer._light_color = self.cfg.light_color self._is_initialized = True @@ -281,13 +281,23 @@ def step(self, dt: float, state: Any | None = None) -> None: self._sim_time += dt self._step_counter += 1 + contacts = None + contacts_data = None if self._scene_data_provider: - self._state = self._scene_data_provider.get_state() + self._state = self._scene_data_provider.get_newton_state() + if self._viewer.show_contacts: + contacts_data = self._scene_data_provider.get_contacts() + if isinstance(contacts_data, dict): + contacts = contacts_data.get("contacts", contacts_data) + else: + contacts = contacts_data else: try: from isaaclab.sim._impl.newton_manager import NewtonManager self._state = NewtonManager._state_0 + if self._viewer.show_contacts: + contacts = NewtonManager._contacts except Exception: self._state = None @@ -300,6 +310,11 @@ def step(self, dt: float, state: Any | None = None) -> None: self._viewer.begin_frame(self._sim_time) if self._state is not None: self._viewer.log_state(self._state) + if contacts is not None and hasattr(self._viewer, "log_contacts"): + try: + self._viewer.log_contacts(contacts, self._state) + except Exception as exc: + logger.debug(f"[NewtonVisualizer] Failed to log contacts: {exc}") self._viewer.end_frame() else: self._viewer._update() diff --git a/source/isaaclab/isaaclab/visualizers/newton_visualizer_cfg.py b/source/isaaclab/isaaclab/visualizers/newton_visualizer_cfg.py index 0465c43ba6b..0d0439843b7 100644 --- a/source/isaaclab/isaaclab/visualizers/newton_visualizer_cfg.py +++ b/source/isaaclab/isaaclab/visualizers/newton_visualizer_cfg.py @@ -47,11 +47,11 @@ class NewtonVisualizerCfg(VisualizerCfg): enable_wireframe: bool = False """Enable wireframe rendering.""" - background_color: tuple[float, float, float] = (0.53, 0.81, 0.92) - """Background/sky color RGB [0,1].""" + sky_upper_color: tuple[float, float, float] = (0.2, 0.4, 0.6) + """Sky upper color RGB [0,1].""" - ground_color: tuple[float, float, float] = (0.18, 0.20, 0.25) - """Ground color RGB [0,1].""" + sky_lower_color: tuple[float, float, float] = (0.5, 0.6, 0.7) + """Sky lower color RGB [0,1].""" light_color: tuple[float, float, float] = (1.0, 1.0, 1.0) """Light color RGB [0,1].""" diff --git a/source/isaaclab/isaaclab/visualizers/ov_visualizer.py b/source/isaaclab/isaaclab/visualizers/ov_visualizer.py index cd16c203ea9..97b64adc9b8 100644 --- a/source/isaaclab/isaaclab/visualizers/ov_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/ov_visualizer.py @@ -39,23 +39,20 @@ def initialize(self, scene_data: dict[str, Any] | None = None) -> None: return usd_stage = None - simulation_context = None + scene_data_provider = None if scene_data is not None: usd_stage = scene_data.get("usd_stage") - simulation_context = scene_data.get("simulation_context") + scene_data_provider = scene_data.get("scene_data_provider") if usd_stage is None: raise RuntimeError("OV visualizer requires a USD stage.") metadata = {} - if simulation_context is not None: - num_envs = 0 - if hasattr(simulation_context, "scene") and simulation_context.scene is not None: - if hasattr(simulation_context.scene, "num_envs"): - num_envs = simulation_context.scene.num_envs - + if scene_data_provider is not None: + metadata = scene_data_provider.get_metadata() + else: metadata = { - "num_envs": num_envs, + "num_envs": 0, "physics_backend": "omni", "env_prim_pattern": "/World/envs/env_{}", } diff --git a/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py b/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py index f75555096cc..a56f4581654 100644 --- a/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py @@ -93,8 +93,8 @@ def initialize(self, scene_data: dict[str, Any] | None = None) -> None: metadata = {} if self._scene_data_provider: - self._model = self._scene_data_provider.get_model() - self._state = self._scene_data_provider.get_state() + self._model = self._scene_data_provider.get_newton_model() + self._state = self._scene_data_provider.get_newton_state() metadata = self._scene_data_provider.get_metadata() else: try: @@ -171,7 +171,7 @@ def step(self, dt: float, state: Any | None = None) -> None: return if self._scene_data_provider: - self._state = self._scene_data_provider.get_state() + self._state = self._scene_data_provider.get_newton_state() else: try: from isaaclab.sim._impl.newton_manager import NewtonManager diff --git a/source/isaaclab/setup.py b/source/isaaclab/setup.py index 19712d01dcf..04f883ce73e 100644 --- a/source/isaaclab/setup.py +++ b/source/isaaclab/setup.py @@ -33,13 +33,15 @@ # image processing "transformers", "einops", # needed for transformers, doesn't always auto-install - "warp-lang==1.11.0.dev20251205", + "warp-lang>=1.11.0.dev20251205", # newton visualizers / backend dependencies "mujoco>=3.4.0.dev839962392", - "mujoco-warp@ git+https://github.com/google-deepmind/mujoco_warp.git@e9a67538f2c14486121635074c5a5fd6ca55fa83", - "newton@ git+https://github.com/newton-physics/newton.git@beta-0.2.1", - "imgui-bundle==1.92.0", + "mujoco-warp>=0.0.1", + "cbor2>=5.7.0", + "newton>=0.2.1", + "imgui_bundle>=1.92.0", "PyOpenGL-accelerate==3.1.10", + "rerun-sdk>=0.27.1", # make sure this is consistent with isaac sim version "pillow==11.3.0", # livestream From 3e6fbef4ae11681e23a8a4d272b3ed87d81c102b Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Tue, 27 Jan 2026 18:16:30 +0000 Subject: [PATCH 04/14] simplify --- .../newton_scene_data_provider.py | 4 +- .../ov_scene_data_provider.py | 4 +- .../scene_data_provider.py | 52 ++----------------- 3 files changed, 5 insertions(+), 55 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py index 7144b599b3a..bbeb4794978 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py @@ -11,12 +11,10 @@ import logging from typing import Any -from .scene_data_provider import SceneDataProviderBase - logger = logging.getLogger(__name__) -class NewtonSceneDataProvider(SceneDataProviderBase): +class NewtonSceneDataProvider: """Scene data provider for Newton Warp physics backend. Native (cheap): Newton Model/State from NewtonManager diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py index 462421d4073..a6e6e7c4050 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py @@ -13,12 +13,10 @@ import re from typing import Any -from .scene_data_provider import SceneDataProviderBase - logger = logging.getLogger(__name__) -class OVSceneDataProvider(SceneDataProviderBase): +class OVSceneDataProvider: """Scene data provider for Omni PhysX physics backend. Native (cheap): USD stage, PhysX transforms/velocities/contacts diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py index dae8ee5c468..a5cebf10e48 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py @@ -4,64 +4,18 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Scene data provider abstraction for visualizers and renderers.""" +"""Scene data provider for visualizers and renderers.""" from __future__ import annotations import logging -from abc import ABC, abstractmethod from typing import Any logger = logging.getLogger(__name__) -class SceneDataProviderBase(ABC): - """Base interface for scene data providers. - - Provides simulation data in multiple formats for visualizers, renderers, - and other downstream consumers. Each backend provider implements this interface, - exposing native data cheaply and adapted data when needed. - """ - - @abstractmethod - def update(self) -> None: - """Update adapted data for current simulation step.""" - - @abstractmethod - def get_newton_model(self) -> Any | None: - """Get Newton Model.""" - - @abstractmethod - def get_newton_state(self) -> Any | None: - """Get Newton State.""" - - @abstractmethod - def get_usd_stage(self) -> Any | None: - """Get USD stage.""" - - @abstractmethod - def get_metadata(self) -> dict[str, Any]: - """Get provider metadata and performance hints.""" - - @abstractmethod - def get_transforms(self) -> dict[str, Any] | None: - """Get world-space transforms in backend-agnostic format.""" - - def get_velocities(self) -> dict[str, Any] | None: - """Get velocities in backend-agnostic format.""" - return None - - def get_contacts(self) -> dict[str, Any] | None: - """Get contact data in backend-agnostic format.""" - return None - - def get_mesh_data(self) -> dict[str, Any] | None: - """Get mesh geometry and materials.""" - return None - - class SceneDataProvider: - """Facade that creates appropriate provider based on physics backend.""" + """Creates appropriate data provider based on physics backend.""" def __init__( self, @@ -71,7 +25,7 @@ def __init__( simulation_context=None, ) -> None: self._backend = backend - self._provider: SceneDataProviderBase | None = None + self._provider = None if backend == "newton": from .newton_scene_data_provider import NewtonSceneDataProvider From 323f638126a979c3ef02ee972eba192afbf6251e Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Tue, 27 Jan 2026 18:18:20 +0000 Subject: [PATCH 05/14] correction --- .../isaaclab/isaaclab/sim/scene_data_providers/__init__.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/__init__.py b/source/isaaclab/isaaclab/sim/scene_data_providers/__init__.py index 55ccb14863c..79d7bff100c 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/__init__.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/__init__.py @@ -6,13 +6,12 @@ """Scene data providers for visualizers and renderers.""" -from .scene_data_provider import SceneDataProvider, SceneDataProviderBase from .newton_scene_data_provider import NewtonSceneDataProvider -from .ov_scene_data_provider import OmniSceneDataProvider +from .ov_scene_data_provider import OVSceneDataProvider +from .scene_data_provider import SceneDataProvider __all__ = [ "SceneDataProvider", - "SceneDataProviderBase", "NewtonSceneDataProvider", - "OmniSceneDataProvider", + "OVSceneDataProvider", ] From 24a515e3cd6eb43c7c2470110aa5d4e654e57e4c Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Tue, 27 Jan 2026 19:04:21 +0000 Subject: [PATCH 06/14] clean --- source/isaaclab/isaaclab/utils/__init__.py | 1 - .../isaaclab/utils/simulation_runner.py | 37 ------------------- 2 files changed, 38 deletions(-) delete mode 100644 source/isaaclab/isaaclab/utils/simulation_runner.py diff --git a/source/isaaclab/isaaclab/utils/__init__.py b/source/isaaclab/isaaclab/utils/__init__.py index 5dc6e92da58..1295715857f 100644 --- a/source/isaaclab/isaaclab/utils/__init__.py +++ b/source/isaaclab/isaaclab/utils/__init__.py @@ -13,7 +13,6 @@ from .logger import * from .mesh import * from .modifiers import * -from .simulation_runner import close_simulation, is_simulation_running from .string import * from .timer import Timer from .types import * diff --git a/source/isaaclab/isaaclab/utils/simulation_runner.py b/source/isaaclab/isaaclab/utils/simulation_runner.py deleted file mode 100644 index 1b0b89a031e..00000000000 --- a/source/isaaclab/isaaclab/utils/simulation_runner.py +++ /dev/null @@ -1,37 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Utility functions for running simulation loops with visualizer-agnostic is_running checks.""" - -from __future__ import annotations - -from typing import TYPE_CHECKING, Any - -if TYPE_CHECKING: - from isaaclab.sim import SimulationContext - - -def is_simulation_running(simulation_app: Any | None, sim_context: SimulationContext | None = None) -> bool: - """Check if the simulation should continue running. - - - Omniverse mode: delegates to SimulationApp.is_running() - - Standalone mode with visualizers: checks if any visualizer window is still open - - Headless mode (no visualizers): always returns True (use Ctrl+C or break to exit) - """ - if simulation_app is not None: - return simulation_app.is_running() - - if sim_context is not None and hasattr(sim_context, "_visualizers"): - visualizers = sim_context._visualizers - if visualizers: - return any(v.is_running() for v in visualizers) - - return True - - -def close_simulation(simulation_app: Any | None) -> None: - """Close the simulation app if running in Omniverse mode.""" - if simulation_app is not None: - simulation_app.close() From 7bb61d1cd183dcbe0866ab6344237d4bd8dff236 Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Tue, 27 Jan 2026 19:05:27 +0000 Subject: [PATCH 07/14] rm files --- .../isaaclab/envs/manager_based_env.py | 593 ------------- .../isaaclab/scene/interactive_scene.py | 821 ------------------ .../spawners/materials/visual_materials.py | 173 ---- 3 files changed, 1587 deletions(-) delete mode 100644 source/isaaclab/isaaclab/envs/manager_based_env.py delete mode 100644 source/isaaclab/isaaclab/scene/interactive_scene.py delete mode 100644 source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py deleted file mode 100644 index e4c8d5cb14d..00000000000 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ /dev/null @@ -1,593 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -import builtins -import logging -import warnings -from collections.abc import Sequence -from typing import Any - -import torch - -import omni.physx -from isaacsim.core.simulation_manager import SimulationManager - -from isaaclab.managers import ActionManager, EventManager, ObservationManager, RecorderManager -from isaaclab.scene import InteractiveScene -from isaaclab.sim import SimulationContext -from isaaclab.sim.utils.stage import attach_stage_to_usd_context, use_stage -from isaaclab.ui.widgets import ManagerLiveVisualizer -from isaaclab.utils.seed import configure_seed -from isaaclab.utils.timer import Timer -from isaaclab.utils.version import get_isaac_sim_version - -from .common import VecEnvObs -from .manager_based_env_cfg import ManagerBasedEnvCfg -from .ui import ViewportCameraController -from .utils.io_descriptors import export_articulations_data, export_scene_data - -# import logger -logger = logging.getLogger(__name__) - - -class ManagerBasedEnv: - """The base environment encapsulates the simulation scene and the environment managers for - the manager-based workflow. - - While a simulation scene or world comprises of different components such as the robots, objects, - and sensors (cameras, lidars, etc.), the environment is a higher level abstraction - that provides an interface for interacting with the simulation. The environment is comprised of - the following components: - - * **Scene**: The scene manager that creates and manages the virtual world in which the robot operates. - This includes defining the robot, static and dynamic objects, sensors, etc. - * **Observation Manager**: The observation manager that generates observations from the current simulation - state and the data gathered from the sensors. These observations may include privileged information - that is not available to the robot in the real world. Additionally, user-defined terms can be added - to process the observations and generate custom observations. For example, using a network to embed - high-dimensional observations into a lower-dimensional space. - * **Action Manager**: The action manager that processes the raw actions sent to the environment and - converts them to low-level commands that are sent to the simulation. It can be configured to accept - raw actions at different levels of abstraction. For example, in case of a robotic arm, the raw actions - can be joint torques, joint positions, or end-effector poses. Similarly for a mobile base, it can be - the joint torques, or the desired velocity of the floating base. - * **Event Manager**: The event manager orchestrates operations triggered based on simulation events. - This includes resetting the scene to a default state, applying random pushes to the robot at different intervals - of time, or randomizing properties such as mass and friction coefficients. This is useful for training - and evaluating the robot in a variety of scenarios. - * **Recorder Manager**: The recorder manager that handles recording data produced during different steps - in the simulation. This includes recording in the beginning and end of a reset and a step. The recorded data - is distinguished per episode, per environment and can be exported through a dataset file handler to a file. - - The environment provides a unified interface for interacting with the simulation. However, it does not - include task-specific quantities such as the reward function, or the termination conditions. These - quantities are often specific to defining Markov Decision Processes (MDPs) while the base environment - is agnostic to the MDP definition. - - The environment steps forward in time at a fixed time-step. The physics simulation is decimated at a - lower time-step. This is to ensure that the simulation is stable. These two time-steps can be configured - independently using the :attr:`ManagerBasedEnvCfg.decimation` (number of simulation steps per environment step) - and the :attr:`ManagerBasedEnvCfg.sim.dt` (physics time-step) parameters. Based on these parameters, the - environment time-step is computed as the product of the two. The two time-steps can be obtained by - querying the :attr:`physics_dt` and the :attr:`step_dt` properties respectively. - """ - - def __init__(self, cfg: ManagerBasedEnvCfg): - """Initialize the environment. - - Args: - cfg: The configuration object for the environment. - - Raises: - RuntimeError: If a simulation context already exists. The environment must always create one - since it configures the simulation context and controls the simulation. - """ - # check that the config is valid - cfg.validate() - # store inputs to class - self.cfg = cfg - # initialize internal variables - self._is_closed = False - - # set the seed for the environment - if self.cfg.seed is not None: - self.cfg.seed = self.seed(self.cfg.seed) - else: - logger.warning("Seed not set for the environment. The environment creation may not be deterministic.") - - # create a simulation context to control the simulator - if SimulationContext.instance() is None: - # the type-annotation is required to avoid a type-checking error - # 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() - - # make sure torch is running on the correct device - if "cuda" in self.device: - torch.cuda.set_device(self.device) - - # print useful information - print("[INFO]: Base environment:") - print(f"\tEnvironment device : {self.device}") - print(f"\tEnvironment seed : {self.cfg.seed}") - print(f"\tPhysics step-size : {self.physics_dt}") - print(f"\tRendering step-size : {self.physics_dt * self.cfg.sim.render_interval}") - print(f"\tEnvironment step-size : {self.step_dt}") - - if self.cfg.sim.render_interval < self.cfg.decimation: - msg = ( - f"The render interval ({self.cfg.sim.render_interval}) is smaller than the decimation " - f"({self.cfg.decimation}). Multiple render calls will happen for each environment step. " - "If this is not intended, set the render interval to be equal to the decimation." - ) - logger.warning(msg) - - # counter for simulation steps - self._sim_step_counter = 0 - - # allocate dictionary to store metrics - self.extras = {} - - # generate scene - with Timer("[INFO]: Time taken for scene creation", "scene_creation"): - # set the stage context for scene creation steps which use the stage - with use_stage(self.sim.get_initial_stage()): - self.scene = InteractiveScene(self.cfg.scene) - attach_stage_to_usd_context() - print("[INFO]: Scene manager: ", self.scene) - from isaaclab.sim.utils import find_matching_prim_paths - - env_prim_paths = find_matching_prim_paths("/World/envs/env_.*", stage=self.scene.stage) - print( - "[SceneDebug] env prims after InteractiveScene: " - f"num_envs_setting={self.cfg.scene.num_envs}, env_prims={len(env_prim_paths)}" - ) - # import ipdb; ipdb.set_trace() - - # set up camera viewport controller - # viewport is not available in other rendering modes so the function will throw a warning - # FIXME: This needs to be fixed in the future when we unify the UI functionalities even for - # non-rendering modes. - if self.sim.render_mode >= self.sim.RenderMode.PARTIAL_RENDERING: - self.viewport_camera_controller = ViewportCameraController(self, self.cfg.viewer) - else: - self.viewport_camera_controller = None - - # create event manager - # note: this is needed here (rather than after simulation play) to allow USD-related randomization events - # that must happen before the simulation starts. Example: randomizing mesh scale - self.event_manager = EventManager(self.cfg.events, self) - - # apply USD-related randomization events - if "prestartup" in self.event_manager.available_modes: - self.event_manager.apply(mode="prestartup") - - # 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() - - # extend UI elements - # we need to do this here after all the managers are initialized - # this is because they dictate the sensors and commands right now - if self.sim.has_gui() and self.cfg.ui_window_class_type is not None: - # setup live visualizers - self.setup_manager_visualizers() - self._window = self.cfg.ui_window_class_type(self, window_name="IsaacLab") - else: - # if no window, then we don't need to store the window - self._window = None - - # initialize observation buffers - self.obs_buf = {} - - # export IO descriptors if requested - if self.cfg.export_io_descriptors: - self.export_IO_descriptors() - - # show deprecation message for rerender_on_reset - if self.cfg.rerender_on_reset: - msg = ( - "\033[93m\033[1m[DEPRECATION WARNING] ManagerBasedEnvCfg.rerender_on_reset is deprecated. Use" - " ManagerBasedEnvCfg.num_rerenders_on_reset instead.\033[0m" - ) - warnings.warn( - msg, - FutureWarning, - stacklevel=2, - ) - if self.cfg.num_rerenders_on_reset == 0: - self.cfg.num_rerenders_on_reset = 1 - - def __del__(self): - """Cleanup for the environment.""" - self.close() - - """ - Properties. - """ - - @property - def num_envs(self) -> int: - """The number of instances of the environment that are running.""" - return self.scene.num_envs - - @property - def physics_dt(self) -> float: - """The physics time-step (in s). - - This is the lowest time-decimation at which the simulation is happening. - """ - return self.cfg.sim.dt - - @property - def step_dt(self) -> float: - """The environment stepping time-step (in s). - - This is the time-step at which the environment steps forward. - """ - return self.cfg.sim.dt * self.cfg.decimation - - @property - def device(self): - """The device on which the environment is running.""" - return self.sim.device - - @property - def get_IO_descriptors(self): - """Get the IO descriptors for the environment. - - Returns: - A dictionary with keys as the group names and values as the IO descriptors. - """ - return { - "observations": self.observation_manager.get_IO_descriptors, - "actions": self.action_manager.get_IO_descriptors, - "articulations": export_articulations_data(self), - "scene": export_scene_data(self), - } - - def export_IO_descriptors(self, output_dir: str | None = None): - """Export the IO descriptors for the environment. - - Args: - output_dir: The directory to export the IO descriptors to. - """ - import os - - import yaml - - IO_descriptors = self.get_IO_descriptors - - if output_dir is None: - if self.cfg.log_dir is not None: - output_dir = os.path.join(self.cfg.log_dir, "io_descriptors") - else: - raise ValueError( - "Output directory is not set. Please set the log directory using the `log_dir`" - " configuration or provide an explicit output_dir parameter." - ) - - if not os.path.exists(output_dir): - os.makedirs(output_dir, exist_ok=True) - - with open(os.path.join(output_dir, "IO_descriptors.yaml"), "w") as f: - print(f"[INFO]: Exporting IO descriptors to {os.path.join(output_dir, 'IO_descriptors.yaml')}") - yaml.safe_dump(IO_descriptors, f) - - """ - Operations - Setup. - """ - - def load_managers(self): - """Load the managers for the environment. - - This function is responsible for creating the various managers (action, observation, - events, etc.) for the environment. Since the managers require access to physics handles, - they can only be created after the simulator is reset (i.e. played for the first time). - - .. note:: - In case of standalone application (when running simulator from Python), the function is called - automatically when the class is initialized. - - However, in case of extension mode, the user must call this function manually after the simulator - is reset. This is because the simulator is only reset when the user calls - :meth:`SimulationContext.reset_async` and it isn't possible to call async functions in the constructor. - - """ - # prepare the managers - # -- event manager (we print it here to make the logging consistent) - print("[INFO] Event Manager: ", self.event_manager) - # -- recorder manager - self.recorder_manager = RecorderManager(self.cfg.recorders, self) - print("[INFO] Recorder Manager: ", self.recorder_manager) - # -- action manager - self.action_manager = ActionManager(self.cfg.actions, self) - print("[INFO] Action Manager: ", self.action_manager) - # -- observation manager - self.observation_manager = ObservationManager(self.cfg.observations, self) - print("[INFO] Observation Manager:", self.observation_manager) - - # perform events at the start of the simulation - # in-case a child implementation creates other managers, the randomization should happen - # when all the other managers are created - if self.__class__ == ManagerBasedEnv and "startup" in self.event_manager.available_modes: - self.event_manager.apply(mode="startup") - - def setup_manager_visualizers(self): - """Creates live visualizers for manager terms.""" - - self.manager_visualizers = { - "action_manager": ManagerLiveVisualizer(manager=self.action_manager), - "observation_manager": ManagerLiveVisualizer(manager=self.observation_manager), - } - - """ - Operations - MDP. - """ - - def reset( - self, seed: int | None = None, env_ids: Sequence[int] | None = None, options: dict[str, Any] | None = None - ) -> tuple[VecEnvObs, dict]: - """Resets the specified environments and returns observations. - - This function calls the :meth:`_reset_idx` function to reset the specified environments. - However, certain operations, such as procedural terrain generation, that happened during initialization - are not repeated. - - Args: - seed: The seed to use for randomization. Defaults to None, in which case the seed is not set. - env_ids: The environment ids to reset. Defaults to None, in which case all environments are reset. - options: Additional information to specify how the environment is reset. Defaults to None. - - Note: - This argument is used for compatibility with Gymnasium environment definition. - - Returns: - A tuple containing the observations and extras. - """ - if env_ids is None: - env_ids = torch.arange(self.num_envs, dtype=torch.int64, device=self.device) - - # trigger recorder terms for pre-reset calls - self.recorder_manager.record_pre_reset(env_ids) - - # set the seed - if seed is not None: - self.seed(seed) - - # reset state of scene - self._reset_idx(env_ids) - - # update articulation kinematics - self.scene.write_data_to_sim() - self.sim.forward() - # if sensors are added to the scene, make sure we render to reflect changes in reset - if self.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: - for _ in range(self.cfg.num_rerenders_on_reset): - self.sim.render() - - # trigger recorder terms for post-reset calls - self.recorder_manager.record_post_reset(env_ids) - - # compute observations - self.obs_buf = self.observation_manager.compute(update_history=True) - - if self.cfg.wait_for_textures and self.sim.has_rtx_sensors(): - while SimulationManager.assets_loading(): - self.sim.render() - - # return observations - return self.obs_buf, self.extras - - def reset_to( - self, - state: dict[str, dict[str, dict[str, torch.Tensor]]], - env_ids: Sequence[int] | None, - seed: int | None = None, - is_relative: bool = False, - ): - """Resets specified environments to provided states. - - This function resets the environments to the provided states. The state is a dictionary - containing the state of the scene entities. Please refer to :meth:`InteractiveScene.get_state` - for the format. - - The function is different from the :meth:`reset` function as it resets the environments to specific states, - instead of using the randomization events for resetting the environments. - - Args: - state: The state to reset the specified environments to. Please refer to - :meth:`InteractiveScene.get_state` for the format. - env_ids: The environment ids to reset. Defaults to None, in which case all environments are reset. - seed: The seed to use for randomization. Defaults to None, in which case the seed is not set. - is_relative: If set to True, the state is considered relative to the environment origins. - Defaults to False. - """ - # reset all envs in the scene if env_ids is None - if env_ids is None: - env_ids = torch.arange(self.num_envs, dtype=torch.int64, device=self.device) - - # trigger recorder terms for pre-reset calls - self.recorder_manager.record_pre_reset(env_ids) - - # set the seed - if seed is not None: - self.seed(seed) - - self._reset_idx(env_ids) - - # set the state - self.scene.reset_to(state, env_ids, is_relative=is_relative) - - # update articulation kinematics - self.sim.forward() - - # if sensors are added to the scene, make sure we render to reflect changes in reset - if self.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: - for _ in range(self.cfg.num_rerenders_on_reset): - self.sim.render() - - # trigger recorder terms for post-reset calls - self.recorder_manager.record_post_reset(env_ids) - - # compute observations - self.obs_buf = self.observation_manager.compute(update_history=True) - - # return observations - return self.obs_buf, self.extras - - def step(self, action: torch.Tensor) -> tuple[VecEnvObs, dict]: - """Execute one time-step of the environment's dynamics. - - The environment steps forward at a fixed time-step, while the physics simulation is - decimated at a lower time-step. This is to ensure that the simulation is stable. These two - time-steps can be configured independently using the :attr:`ManagerBasedEnvCfg.decimation` (number of - simulation steps per environment step) and the :attr:`ManagerBasedEnvCfg.sim.dt` (physics time-step). - Based on these parameters, the environment time-step is computed as the product of the two. - - Args: - action: The actions to apply on the environment. Shape is (num_envs, action_dim). - - Returns: - A tuple containing the observations and extras. - """ - # process actions - self.action_manager.process_action(action.to(self.device)) - - self.recorder_manager.record_pre_step() - - # check if we need to do rendering within the physics loop - # note: checked here once to avoid multiple checks within the loop - is_rendering = self.sim.has_gui() or self.sim.has_rtx_sensors() - - # perform physics stepping - for _ in range(self.cfg.decimation): - self._sim_step_counter += 1 - # set actions into buffers - self.action_manager.apply_action() - # set actions into simulator - self.scene.write_data_to_sim() - # simulate - self.sim.step(render=False) - # render between steps only if the GUI or an RTX sensor needs it - # note: we assume the render interval to be the shortest accepted rendering interval. - # If a camera needs rendering at a faster frequency, this will lead to unexpected behavior. - if self._sim_step_counter % self.cfg.sim.render_interval == 0 and is_rendering: - self.sim.render() - # update buffers at sim dt - self.scene.update(dt=self.physics_dt) - - # post-step: step interval event - if "interval" in self.event_manager.available_modes: - self.event_manager.apply(mode="interval", dt=self.step_dt) - - # -- compute observations - self.obs_buf = self.observation_manager.compute(update_history=True) - self.recorder_manager.record_post_step() - - # return observations and extras - return self.obs_buf, self.extras - - @staticmethod - def seed(seed: int = -1) -> int: - """Set the seed for the environment. - - Args: - seed: The seed for random generator. Defaults to -1. - - Returns: - The seed used for random generator. - """ - # set seed for replicator - try: - import omni.replicator.core as rep - - rep.set_global_seed(seed) - except ModuleNotFoundError: - pass - # set seed for torch and other libraries - return configure_seed(seed) - - def close(self): - """Cleanup for the environment.""" - if not self._is_closed: - # destructor is order-sensitive - del self.viewport_camera_controller - del self.action_manager - del self.observation_manager - del self.event_manager - del self.recorder_manager - del self.scene - - # clear callbacks and instance - if get_isaac_sim_version().major >= 5: - if self.cfg.sim.create_stage_in_memory: - # detach physx stage - omni.physx.get_physx_simulation_interface().detach_stage() - self.sim.stop() - self.sim.clear() - - self.sim.clear_all_callbacks() - self.sim.clear_instance() - - # destroy the window - if self._window is not None: - self._window = None - # update closing status - self._is_closed = True - - """ - Helper functions. - """ - - def _reset_idx(self, env_ids: Sequence[int]): - """Reset environments based on specified indices. - - Args: - env_ids: List of environment ids which must be reset - """ - # reset the internal buffers of the scene elements - self.scene.reset(env_ids) - - # apply events such as randomization for environments that need a reset - if "reset" in self.event_manager.available_modes: - env_step_count = self._sim_step_counter // self.cfg.decimation - self.event_manager.apply(mode="reset", env_ids=env_ids, global_env_step_count=env_step_count) - - # iterate over all managers and reset them - # this returns a dictionary of information which is stored in the extras - # note: This is order-sensitive! Certain things need be reset before others. - self.extras["log"] = dict() - # -- observation manager - info = self.observation_manager.reset(env_ids) - self.extras["log"].update(info) - # -- action manager - info = self.action_manager.reset(env_ids) - self.extras["log"].update(info) - # -- event manager - info = self.event_manager.reset(env_ids) - self.extras["log"].update(info) - # -- recorder manager - info = self.recorder_manager.reset(env_ids) - self.extras["log"].update(info) diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py deleted file mode 100644 index 86abca135a4..00000000000 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ /dev/null @@ -1,821 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -import logging -from collections.abc import Sequence -from typing import Any - -import torch - -import carb -from isaacsim.core.cloner import GridCloner -from pxr import PhysxSchema - -import isaaclab.sim as sim_utils -from isaaclab.assets import ( - Articulation, - ArticulationCfg, - AssetBaseCfg, - DeformableObject, - DeformableObjectCfg, - RigidObject, - RigidObjectCfg, - RigidObjectCollection, - RigidObjectCollectionCfg, - SurfaceGripper, - SurfaceGripperCfg, -) -from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg, SensorBase, SensorBaseCfg, VisuoTactileSensorCfg -from isaaclab.sim import SimulationContext -from isaaclab.sim.utils.stage import get_current_stage, get_current_stage_id -from isaaclab.sim.views import XformPrimView -from isaaclab.terrains import TerrainImporter, TerrainImporterCfg -from isaaclab.utils.version import get_isaac_sim_version - -from .interactive_scene_cfg import InteractiveSceneCfg - -# import logger -logger = logging.getLogger(__name__) - - -class InteractiveScene: - """A scene that contains entities added to the simulation. - - The interactive scene parses the :class:`InteractiveSceneCfg` class to create the scene. - Based on the specified number of environments, it clones the entities and groups them into different - categories (e.g., articulations, sensors, etc.). - - Cloning can be performed in two ways: - - * For tasks where all environments contain the same assets, a more performant cloning paradigm - can be used to allow for faster environment creation. This is specified by the ``replicate_physics`` flag. - - .. code-block:: python - - scene = InteractiveScene(cfg=InteractiveSceneCfg(replicate_physics=True)) - - * For tasks that require having separate assets in the environments, ``replicate_physics`` would have to - be set to False, which will add some costs to the overall startup time. - - .. code-block:: python - - scene = InteractiveScene(cfg=InteractiveSceneCfg(replicate_physics=False)) - - Each entity is registered to scene based on its name in the configuration class. For example, if the user - specifies a robot in the configuration class as follows: - - .. code-block:: python - - from isaaclab.scene import InteractiveSceneCfg - from isaaclab.utils import configclass - - from isaaclab_assets.robots.anymal import ANYMAL_C_CFG - - - @configclass - class MySceneCfg(InteractiveSceneCfg): - # ANYmal-C robot spawned in each environment - robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - - Then the robot can be accessed from the scene as follows: - - .. code-block:: python - - from isaaclab.scene import InteractiveScene - - # create 128 environments - scene = InteractiveScene(cfg=MySceneCfg(num_envs=128)) - - # access the robot from the scene - robot = scene["robot"] - # access the robot based on its type - robot = scene.articulations["robot"] - - If the :class:`InteractiveSceneCfg` class does not include asset entities, the cloning process - can still be triggered if assets were added to the stage outside of the :class:`InteractiveScene` class: - - .. code-block:: python - - scene = InteractiveScene(cfg=InteractiveSceneCfg(num_envs=128, replicate_physics=True)) - scene.clone_environments() - - .. note:: - It is important to note that the scene only performs common operations on the entities. For example, - resetting the internal buffers, writing the buffers to the simulation and updating the buffers from the - simulation. The scene does not perform any task specific to the entity. For example, it does not apply - actions to the robot or compute observations from the robot. These tasks are handled by different - modules called "managers" in the framework. Please refer to the :mod:`isaaclab.managers` sub-package - for more details. - """ - - def __init__(self, cfg: InteractiveSceneCfg): - """Initializes the scene. - - Args: - cfg: The configuration class for the scene. - """ - # check that the config is valid - cfg.validate() - # store inputs - self.cfg = cfg - - # TODO(mtrepte): remove - self.cfg.clone_in_fabric = False - - # initialize scene elements - self._terrain = None - self._articulations = dict() - self._deformable_objects = dict() - self._rigid_objects = dict() - self._rigid_object_collections = dict() - self._sensors = dict() - self._surface_grippers = dict() - self._extras = dict() - # get stage handle - self.sim = SimulationContext.instance() - self.stage = get_current_stage() - self.stage_id = get_current_stage_id() - # publish num_envs for consumers outside the scene - try: - self.sim.set_setting("/isaaclab/scene/num_envs", int(self.cfg.num_envs)) - except Exception: - pass - # physics scene path - self._physics_scene_path = None - # prepare cloner for environment replication - self.cloner = GridCloner(spacing=self.cfg.env_spacing, stage=self.stage) - self.cloner.define_base_env(self.env_ns) - self.env_prim_paths = self.cloner.generate_paths(f"{self.env_ns}/env", self.cfg.num_envs) - # create source prim - self.stage.DefinePrim(self.env_prim_paths[0], "Xform") - # allocate env indices - self._ALL_INDICES = torch.arange(self.cfg.num_envs, dtype=torch.long, device=self.device) - # when replicate_physics=False, we assume heterogeneous environments and clone the xforms first. - # this triggers per-object level cloning in the spawner. - if not self.cfg.replicate_physics: - # check version of Isaac Sim to determine whether clone_in_fabric is valid - if get_isaac_sim_version().major < 5: - # clone the env xform - env_origins = self.cloner.clone( - source_prim_path=self.env_prim_paths[0], - prim_paths=self.env_prim_paths, - replicate_physics=False, - copy_from_source=True, - enable_env_ids=( - self.cfg.filter_collisions if self.device != "cpu" else False - ), # this won't do anything because we are not replicating physics - ) - else: - # clone the env xform - env_origins = self.cloner.clone( - source_prim_path=self.env_prim_paths[0], - prim_paths=self.env_prim_paths, - replicate_physics=False, - copy_from_source=True, - enable_env_ids=( - self.cfg.filter_collisions if self.device != "cpu" else False - ), # this won't do anything because we are not replicating physics - clone_in_fabric=self.cfg.clone_in_fabric, - ) - self._default_env_origins = torch.tensor(env_origins, device=self.device, dtype=torch.float32) - else: - # otherwise, environment origins will be initialized during cloning at the end of environment creation - self._default_env_origins = None - - self._global_prim_paths = list() - if self._is_scene_setup_from_cfg(): - # add entities from config - self._add_entities_from_cfg() - # clone environments on a global scope if environment is homogeneous - if self.cfg.replicate_physics: - self.clone_environments(copy_from_source=False) - # replicate physics if we have more than one environment - # this is done to make scene initialization faster at play time - if self.cfg.replicate_physics and self.cfg.num_envs > 1: - # check version of Isaac Sim to determine whether clone_in_fabric is valid - if get_isaac_sim_version().major < 5: - self.cloner.replicate_physics( - source_prim_path=self.env_prim_paths[0], - prim_paths=self.env_prim_paths, - base_env_path=self.env_ns, - root_path=self.env_regex_ns.replace(".*", ""), - enable_env_ids=self.cfg.filter_collisions if self.device != "cpu" else False, - ) - else: - self.cloner.replicate_physics( - source_prim_path=self.env_prim_paths[0], - prim_paths=self.env_prim_paths, - base_env_path=self.env_ns, - root_path=self.env_regex_ns.replace(".*", ""), - enable_env_ids=self.cfg.filter_collisions if self.device != "cpu" else False, - clone_in_fabric=self.cfg.clone_in_fabric, - ) - - # since env_ids is only applicable when replicating physics, we have to fallback to the previous method - # to filter collisions if replicate_physics is not enabled - # additionally, env_ids is only supported in GPU simulation - if (not self.cfg.replicate_physics and self.cfg.filter_collisions) or self.device == "cpu": - self.filter_collisions(self._global_prim_paths) - - def clone_environments(self, copy_from_source: bool = False): - """Creates clones of the environment ``/World/envs/env_0``. - - Args: - copy_from_source: (bool): If set to False, clones inherit from /World/envs/env_0 and mirror its changes. - If True, clones are independent copies of the source prim and won't reflect its changes (start-up time - may increase). Defaults to False. - """ - # check if user spawned different assets in individual environments - # this flag will be None if no multi asset is spawned - carb_settings_iface = carb.settings.get_settings() - has_multi_assets = carb_settings_iface.get("/isaaclab/spawn/multi_assets") - if has_multi_assets and self.cfg.replicate_physics: - logger.warning( - "Varying assets might have been spawned under different environments." - " However, the replicate physics flag is enabled in the 'InteractiveScene' configuration." - " This may adversely affect PhysX parsing. We recommend disabling this property." - ) - - # check version of Isaac Sim to determine whether clone_in_fabric is valid - if get_isaac_sim_version().major < 5: - # clone the environment - env_origins = self.cloner.clone( - source_prim_path=self.env_prim_paths[0], - prim_paths=self.env_prim_paths, - replicate_physics=self.cfg.replicate_physics, - copy_from_source=copy_from_source, - enable_env_ids=( - self.cfg.filter_collisions if self.device != "cpu" else False - ), # this automatically filters collisions between environments - ) - else: - # clone the environment - env_origins = self.cloner.clone( - source_prim_path=self.env_prim_paths[0], - prim_paths=self.env_prim_paths, - replicate_physics=self.cfg.replicate_physics, - copy_from_source=copy_from_source, - enable_env_ids=( - self.cfg.filter_collisions if self.device != "cpu" else False - ), # this automatically filters collisions between environments - clone_in_fabric=self.cfg.clone_in_fabric, - ) - - # since env_ids is only applicable when replicating physics, we have to fallback to the previous method - # to filter collisions if replicate_physics is not enabled - # additionally, env_ids is only supported in GPU simulation - if (not self.cfg.replicate_physics and self.cfg.filter_collisions) or self.device == "cpu": - # if scene is specified through cfg, this is already taken care of - if not self._is_scene_setup_from_cfg(): - logger.warning( - "Collision filtering can only be automatically enabled when replicate_physics=True and using GPU" - " simulation. Please call scene.filter_collisions(global_prim_paths) to filter collisions across" - " environments." - ) - - # in case of heterogeneous cloning, the env origins is specified at init - if self._default_env_origins is None: - self._default_env_origins = torch.tensor(env_origins, device=self.device, dtype=torch.float32) - - # publish env origins for consumers that cannot read USD (e.g., Fabric clones) - try: - if hasattr(env_origins, "flatten"): - origins_list = env_origins.flatten().tolist() - else: - origins_list = [] - for origin in env_origins: - origins_list.extend(list(origin)) - self.sim.set_setting("/isaaclab/scene/env_origins", origins_list) - except Exception: - pass - - def filter_collisions(self, global_prim_paths: list[str] | None = None): - """Filter environments collisions. - - Disables collisions between the environments in ``/World/envs/env_.*`` and enables collisions with the prims - in global prim paths (e.g. ground plane). - - Args: - global_prim_paths: A list of global prim paths to enable collisions with. - Defaults to None, in which case no global prim paths are considered. - """ - # validate paths in global prim paths - if global_prim_paths is None: - global_prim_paths = [] - else: - # remove duplicates in paths - global_prim_paths = list(set(global_prim_paths)) - - # if "/World/collisions" already exists in the stage, we don't filter again - if self.stage.GetPrimAtPath("/World/collisions"): - return - - # set global prim paths list if not previously defined - if len(self._global_prim_paths) < 1: - self._global_prim_paths += global_prim_paths - - # filter collisions within each environment instance - self.cloner.filter_collisions( - self.physics_scene_path, - "/World/collisions", - self.env_prim_paths, - global_paths=self._global_prim_paths, - ) - - def __str__(self) -> str: - """Returns a string representation of the scene.""" - msg = f"\n" - msg += f"\tNumber of environments: {self.cfg.num_envs}\n" - msg += f"\tEnvironment spacing : {self.cfg.env_spacing}\n" - msg += f"\tSource prim name : {self.env_prim_paths[0]}\n" - msg += f"\tGlobal prim paths : {self._global_prim_paths}\n" - msg += f"\tReplicate physics : {self.cfg.replicate_physics}" - return msg - - """ - Properties. - """ - - @property - def physics_scene_path(self) -> str: - """The path to the USD Physics Scene.""" - if self._physics_scene_path is None: - for prim in self.stage.Traverse(): - if prim.HasAPI(PhysxSchema.PhysxSceneAPI): - self._physics_scene_path = prim.GetPrimPath().pathString - logger.info(f"Physics scene prim path: {self._physics_scene_path}") - break - if self._physics_scene_path is None: - raise RuntimeError("No physics scene found! Please make sure one exists.") - return self._physics_scene_path - - @property - def physics_dt(self) -> float: - """The physics timestep of the scene.""" - return sim_utils.SimulationContext.instance().get_physics_dt() # pyright: ignore [reportOptionalMemberAccess] - - @property - def device(self) -> str: - """The device on which the scene is created.""" - return sim_utils.SimulationContext.instance().device # pyright: ignore [reportOptionalMemberAccess] - - @property - def env_ns(self) -> str: - """The namespace ``/World/envs`` in which all environments created. - - The environments are present w.r.t. this namespace under "env_{N}" prim, - where N is a natural number. - """ - return "/World/envs" - - @property - def env_regex_ns(self) -> str: - """The namespace ``/World/envs/env_.*`` in which all environments created.""" - return f"{self.env_ns}/env_.*" - - @property - def num_envs(self) -> int: - """The number of environments handled by the scene.""" - return self.cfg.num_envs - - @property - def env_origins(self) -> torch.Tensor: - """The origins of the environments in the scene. Shape is (num_envs, 3).""" - if self._terrain is not None: - return self._terrain.env_origins - else: - return self._default_env_origins - - @property - def terrain(self) -> TerrainImporter | None: - """The terrain in the scene. If None, then the scene has no terrain. - - Note: - We treat terrain separate from :attr:`extras` since terrains define environment origins and are - handled differently from other miscellaneous entities. - """ - return self._terrain - - @property - def articulations(self) -> dict[str, Articulation]: - """A dictionary of articulations in the scene.""" - return self._articulations - - @property - def deformable_objects(self) -> dict[str, DeformableObject]: - """A dictionary of deformable objects in the scene.""" - return self._deformable_objects - - @property - def rigid_objects(self) -> dict[str, RigidObject]: - """A dictionary of rigid objects in the scene.""" - return self._rigid_objects - - @property - def rigid_object_collections(self) -> dict[str, RigidObjectCollection]: - """A dictionary of rigid object collections in the scene.""" - return self._rigid_object_collections - - @property - def sensors(self) -> dict[str, SensorBase]: - """A dictionary of the sensors in the scene, such as cameras and contact reporters.""" - return self._sensors - - @property - def surface_grippers(self) -> dict[str, SurfaceGripper]: - """A dictionary of the surface grippers in the scene.""" - return self._surface_grippers - - @property - def extras(self) -> dict[str, XformPrimView]: - """A dictionary of miscellaneous simulation objects that neither inherit from assets nor sensors. - - The keys are the names of the miscellaneous objects, and the values are the - :class:`~isaaclab.sim.views.XformPrimView` instances of the corresponding prims. - - As an example, lights or other props in the scene that do not have any attributes or properties that you - want to alter at runtime can be added to this dictionary. - - Note: - These are not reset or updated by the scene. They are mainly other prims that are not necessarily - handled by the interactive scene, but are useful to be accessed by the user. - - """ - return self._extras - - @property - def state(self) -> dict[str, dict[str, dict[str, torch.Tensor]]]: - """A dictionary of the state of the scene entities in the simulation world frame. - - Please refer to :meth:`get_state` for the format. - """ - return self.get_state(is_relative=False) - - """ - Operations. - """ - - def reset(self, env_ids: Sequence[int] | None = None): - """Resets the scene entities. - - Args: - env_ids: The indices of the environments to reset. - Defaults to None (all instances). - """ - # -- assets - for articulation in self._articulations.values(): - articulation.reset(env_ids) - for deformable_object in self._deformable_objects.values(): - deformable_object.reset(env_ids) - for rigid_object in self._rigid_objects.values(): - rigid_object.reset(env_ids) - for surface_gripper in self._surface_grippers.values(): - surface_gripper.reset(env_ids) - for rigid_object_collection in self._rigid_object_collections.values(): - rigid_object_collection.reset(env_ids) - # -- sensors - for sensor in self._sensors.values(): - sensor.reset(env_ids) - - def write_data_to_sim(self): - """Writes the data of the scene entities to the simulation.""" - # -- assets - for articulation in self._articulations.values(): - articulation.write_data_to_sim() - for deformable_object in self._deformable_objects.values(): - deformable_object.write_data_to_sim() - for rigid_object in self._rigid_objects.values(): - rigid_object.write_data_to_sim() - for surface_gripper in self._surface_grippers.values(): - surface_gripper.write_data_to_sim() - for rigid_object_collection in self._rigid_object_collections.values(): - rigid_object_collection.write_data_to_sim() - - def update(self, dt: float) -> None: - """Update the scene entities. - - Args: - dt: The amount of time passed from last :meth:`update` call. - """ - # -- assets - for articulation in self._articulations.values(): - articulation.update(dt) - for deformable_object in self._deformable_objects.values(): - deformable_object.update(dt) - for rigid_object in self._rigid_objects.values(): - rigid_object.update(dt) - for rigid_object_collection in self._rigid_object_collections.values(): - rigid_object_collection.update(dt) - for surface_gripper in self._surface_grippers.values(): - surface_gripper.update(dt) - # -- sensors - for sensor in self._sensors.values(): - sensor.update(dt, force_recompute=not self.cfg.lazy_sensor_update) - - """ - Operations: Scene State. - """ - - def reset_to( - self, - state: dict[str, dict[str, dict[str, torch.Tensor]]], - env_ids: Sequence[int] | None = None, - is_relative: bool = False, - ): - """Resets the entities in the scene to the provided state. - - Args: - state: The state to reset the scene entities to. Please refer to :meth:`get_state` for the format. - env_ids: The indices of the environments to reset. Defaults to None, in which case - all environment instances are reset. - is_relative: If set to True, the state is considered relative to the environment origins. - Defaults to False. - """ - # resolve env_ids - if env_ids is None: - env_ids = self._ALL_INDICES - # articulations - for asset_name, articulation in self._articulations.items(): - asset_state = state["articulation"][asset_name] - # root state - root_pose = asset_state["root_pose"].clone() - if is_relative: - root_pose[:, :3] += self.env_origins[env_ids] - root_velocity = asset_state["root_velocity"].clone() - articulation.write_root_pose_to_sim(root_pose, env_ids=env_ids) - articulation.write_root_velocity_to_sim(root_velocity, env_ids=env_ids) - # joint state - joint_position = asset_state["joint_position"].clone() - joint_velocity = asset_state["joint_velocity"].clone() - articulation.write_joint_state_to_sim(joint_position, joint_velocity, env_ids=env_ids) - # FIXME: This is not generic as it assumes PD control over the joints. - # This assumption does not hold for effort controlled joints. - articulation.set_joint_position_target(joint_position, env_ids=env_ids) - articulation.set_joint_velocity_target(joint_velocity, env_ids=env_ids) - # deformable objects - for asset_name, deformable_object in self._deformable_objects.items(): - asset_state = state["deformable_object"][asset_name] - nodal_position = asset_state["nodal_position"].clone() - if is_relative: - nodal_position[:, :3] += self.env_origins[env_ids] - nodal_velocity = asset_state["nodal_velocity"].clone() - deformable_object.write_nodal_pos_to_sim(nodal_position, env_ids=env_ids) - deformable_object.write_nodal_velocity_to_sim(nodal_velocity, env_ids=env_ids) - # rigid objects - for asset_name, rigid_object in self._rigid_objects.items(): - asset_state = state["rigid_object"][asset_name] - root_pose = asset_state["root_pose"].clone() - if is_relative: - root_pose[:, :3] += self.env_origins[env_ids] - root_velocity = asset_state["root_velocity"].clone() - rigid_object.write_root_pose_to_sim(root_pose, env_ids=env_ids) - rigid_object.write_root_velocity_to_sim(root_velocity, env_ids=env_ids) - # surface grippers - for asset_name, surface_gripper in self._surface_grippers.items(): - asset_state = state["gripper"][asset_name] - surface_gripper.set_grippers_command(asset_state) - - # write data to simulation to make sure initial state is set - # this propagates the joint targets to the simulation - self.write_data_to_sim() - - def get_state(self, is_relative: bool = False) -> dict[str, dict[str, dict[str, torch.Tensor]]]: - """Returns the state of the scene entities. - - Based on the type of the entity, the state comprises of different components. - - * For an articulation, the state comprises of the root pose, root velocity, and joint position and velocity. - * For a deformable object, the state comprises of the nodal position and velocity. - * For a rigid object, the state comprises of the root pose and root velocity. - - The returned state is a dictionary with the following format: - - .. code-block:: python - - { - "articulation": { - "entity_1_name": { - "root_pose": torch.Tensor, - "root_velocity": torch.Tensor, - "joint_position": torch.Tensor, - "joint_velocity": torch.Tensor, - }, - "entity_2_name": { - "root_pose": torch.Tensor, - "root_velocity": torch.Tensor, - "joint_position": torch.Tensor, - "joint_velocity": torch.Tensor, - }, - }, - "deformable_object": { - "entity_3_name": { - "nodal_position": torch.Tensor, - "nodal_velocity": torch.Tensor, - } - }, - "rigid_object": { - "entity_4_name": { - "root_pose": torch.Tensor, - "root_velocity": torch.Tensor, - } - }, - } - - where ``entity_N_name`` is the name of the entity registered in the scene. - - Args: - is_relative: If set to True, the state is considered relative to the environment origins. - Defaults to False. - - Returns: - A dictionary of the state of the scene entities. - """ - state = dict() - # articulations - state["articulation"] = dict() - for asset_name, articulation in self._articulations.items(): - asset_state = dict() - asset_state["root_pose"] = articulation.data.root_pose_w.clone() - if is_relative: - asset_state["root_pose"][:, :3] -= self.env_origins - asset_state["root_velocity"] = articulation.data.root_vel_w.clone() - asset_state["joint_position"] = articulation.data.joint_pos.clone() - asset_state["joint_velocity"] = articulation.data.joint_vel.clone() - state["articulation"][asset_name] = asset_state - # deformable objects - state["deformable_object"] = dict() - for asset_name, deformable_object in self._deformable_objects.items(): - asset_state = dict() - asset_state["nodal_position"] = deformable_object.data.nodal_pos_w.clone() - if is_relative: - asset_state["nodal_position"][:, :3] -= self.env_origins - asset_state["nodal_velocity"] = deformable_object.data.nodal_vel_w.clone() - state["deformable_object"][asset_name] = asset_state - # rigid objects - state["rigid_object"] = dict() - for asset_name, rigid_object in self._rigid_objects.items(): - asset_state = dict() - asset_state["root_pose"] = rigid_object.data.root_pose_w.clone() - if is_relative: - asset_state["root_pose"][:, :3] -= self.env_origins - asset_state["root_velocity"] = rigid_object.data.root_vel_w.clone() - state["rigid_object"][asset_name] = asset_state - # surface grippers - state["gripper"] = dict() - for asset_name, gripper in self._surface_grippers.items(): - state["gripper"][asset_name] = gripper.state.clone() - return state - - """ - Operations: Iteration. - """ - - def keys(self) -> list[str]: - """Returns the keys of the scene entities. - - Returns: - The keys of the scene entities. - """ - all_keys = ["terrain"] - for asset_family in [ - self._articulations, - self._deformable_objects, - self._rigid_objects, - self._rigid_object_collections, - self._sensors, - self._surface_grippers, - self._extras, - ]: - all_keys += list(asset_family.keys()) - return all_keys - - def __getitem__(self, key: str) -> Any: - """Returns the scene entity with the given key. - - Args: - key: The key of the scene entity. - - Returns: - The scene entity. - """ - # check if it is a terrain - if key == "terrain": - return self._terrain - - all_keys = ["terrain"] - # check if it is in other dictionaries - for asset_family in [ - self._articulations, - self._deformable_objects, - self._rigid_objects, - self._rigid_object_collections, - self._sensors, - self._surface_grippers, - self._extras, - ]: - out = asset_family.get(key) - # if found, return - if out is not None: - return out - all_keys += list(asset_family.keys()) - # if not found, raise error - raise KeyError(f"Scene entity with key '{key}' not found. Available Entities: '{all_keys}'") - - """ - Internal methods. - """ - - def _is_scene_setup_from_cfg(self) -> bool: - """Check if scene entities are setup from the config or not. - - Returns: - True if scene entities are setup from the config, False otherwise. - """ - return any( - not (asset_name in InteractiveSceneCfg.__dataclass_fields__ or asset_cfg is None) - for asset_name, asset_cfg in self.cfg.__dict__.items() - ) - - def _add_entities_from_cfg(self): - """Add scene entities from the config.""" - # store paths that are in global collision filter - self._global_prim_paths = list() - # parse the entire scene config and resolve regex - for asset_name, asset_cfg in self.cfg.__dict__.items(): - # skip keywords - # note: easier than writing a list of keywords: [num_envs, env_spacing, lazy_sensor_update] - if asset_name in InteractiveSceneCfg.__dataclass_fields__ or asset_cfg is None: - continue - # resolve regex - if hasattr(asset_cfg, "prim_path"): - asset_cfg.prim_path = asset_cfg.prim_path.format(ENV_REGEX_NS=self.env_regex_ns) - # create asset - if isinstance(asset_cfg, TerrainImporterCfg): - # terrains are special entities since they define environment origins - asset_cfg.num_envs = self.cfg.num_envs - asset_cfg.env_spacing = self.cfg.env_spacing - self._terrain = asset_cfg.class_type(asset_cfg) - elif isinstance(asset_cfg, ArticulationCfg): - self._articulations[asset_name] = asset_cfg.class_type(asset_cfg) - elif isinstance(asset_cfg, DeformableObjectCfg): - self._deformable_objects[asset_name] = asset_cfg.class_type(asset_cfg) - elif isinstance(asset_cfg, RigidObjectCfg): - self._rigid_objects[asset_name] = asset_cfg.class_type(asset_cfg) - elif isinstance(asset_cfg, RigidObjectCollectionCfg): - for rigid_object_cfg in asset_cfg.rigid_objects.values(): - rigid_object_cfg.prim_path = rigid_object_cfg.prim_path.format(ENV_REGEX_NS=self.env_regex_ns) - self._rigid_object_collections[asset_name] = asset_cfg.class_type(asset_cfg) - for rigid_object_cfg in asset_cfg.rigid_objects.values(): - if hasattr(rigid_object_cfg, "collision_group") and rigid_object_cfg.collision_group == -1: - asset_paths = sim_utils.find_matching_prim_paths(rigid_object_cfg.prim_path) - self._global_prim_paths += asset_paths - elif isinstance(asset_cfg, SurfaceGripperCfg): - # add surface grippers to scene - self._surface_grippers[asset_name] = asset_cfg.class_type(asset_cfg) - elif isinstance(asset_cfg, SensorBaseCfg): - # Update target frame path(s)' regex name space for FrameTransformer - if isinstance(asset_cfg, FrameTransformerCfg): - updated_target_frames = [] - for target_frame in asset_cfg.target_frames: - target_frame.prim_path = target_frame.prim_path.format(ENV_REGEX_NS=self.env_regex_ns) - updated_target_frames.append(target_frame) - asset_cfg.target_frames = updated_target_frames - elif isinstance(asset_cfg, ContactSensorCfg): - updated_filter_prim_paths_expr = [] - for filter_prim_path in asset_cfg.filter_prim_paths_expr: - updated_filter_prim_paths_expr.append(filter_prim_path.format(ENV_REGEX_NS=self.env_regex_ns)) - asset_cfg.filter_prim_paths_expr = updated_filter_prim_paths_expr - elif isinstance(asset_cfg, VisuoTactileSensorCfg): - if hasattr(asset_cfg, "camera_cfg") and asset_cfg.camera_cfg is not None: - asset_cfg.camera_cfg.prim_path = asset_cfg.camera_cfg.prim_path.format( - ENV_REGEX_NS=self.env_regex_ns - ) - if ( - hasattr(asset_cfg, "contact_object_prim_path_expr") - and asset_cfg.contact_object_prim_path_expr is not None - ): - asset_cfg.contact_object_prim_path_expr = asset_cfg.contact_object_prim_path_expr.format( - ENV_REGEX_NS=self.env_regex_ns - ) - - self._sensors[asset_name] = asset_cfg.class_type(asset_cfg) - elif isinstance(asset_cfg, AssetBaseCfg): - # manually spawn asset - if asset_cfg.spawn is not None: - asset_cfg.spawn.func( - asset_cfg.prim_path, - asset_cfg.spawn, - translation=asset_cfg.init_state.pos, - orientation=asset_cfg.init_state.rot, - ) - # store xform prim view corresponding to this asset - # all prims in the scene are Xform prims (i.e. have a transform component) - self._extras[asset_name] = XformPrimView(asset_cfg.prim_path, device=self.device, stage=self.stage) - else: - raise ValueError(f"Unknown asset config type for {asset_name}: {asset_cfg}") - # store global collision paths - if hasattr(asset_cfg, "collision_group") and asset_cfg.collision_group == -1: - asset_paths = sim_utils.find_matching_prim_paths(asset_cfg.prim_path) - self._global_prim_paths += asset_paths diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py deleted file mode 100644 index 713ce34749b..00000000000 --- a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py +++ /dev/null @@ -1,173 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -from __future__ import annotations - -import logging -from typing import TYPE_CHECKING - -from omni.usd.commands import CreateMdlMaterialPrimCommand, CreateShaderPrimFromSdrCommand -from pxr import Usd, UsdShade - -from isaaclab.sim.utils import clone, safe_set_attribute_on_usd_prim -from isaaclab.sim.utils.stage import get_current_stage -from isaaclab.utils.assets import NVIDIA_NUCLEUS_DIR - -if TYPE_CHECKING: - from . import visual_materials_cfg - -# import logger -logger = logging.getLogger(__name__) - - -@clone -def spawn_preview_surface(prim_path: str, cfg: visual_materials_cfg.PreviewSurfaceCfg) -> Usd.Prim: - """Create a preview surface prim and override the settings with the given config. - - A preview surface is a physically-based surface that handles simple shaders while supporting - both *specular* and *metallic* workflows. All color inputs are in linear color space (RGB). - For more information, see the `documentation `__. - - The function calls the USD command `CreateShaderPrimFromSdrCommand`_ to create the prim. - - .. _CreateShaderPrimFromSdrCommand: https://docs.omniverse.nvidia.com/kit/docs/omni.usd/latest/omni.usd.commands/omni.usd.commands.CreateShaderPrimFromSdrCommand.html - - .. note:: - This function is decorated with :func:`clone` that resolves prim path into list of paths - if the input prim path is a regex pattern. This is done to support spawning multiple assets - from a single and cloning the USD prim at the given path expression. - - Args: - prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, - then the asset is spawned at all the matching prim paths. - cfg: The configuration instance. - - Returns: - The created prim. - - Raises: - ValueError: If a prim already exists at the given path. - """ - # get stage handle - stage = get_current_stage() - - # spawn material if it doesn't exist. - if not stage.GetPrimAtPath(prim_path).IsValid(): - # note: we don't use Omniverse's CreatePreviewSurfaceMaterialPrimCommand - # since it does not support USD stage as an argument. The created material - # in that case is always the one from USD Context which makes it difficult to - # handle scene creation on a custom stage. - material_prim = UsdShade.Material.Define(stage, prim_path) - if material_prim: - try: - shader_prim = CreateShaderPrimFromSdrCommand( - parent_path=prim_path, - identifier="UsdPreviewSurface", - stage_or_context=stage, - name="Shader", - ).do() - except TypeError: - shader_prim = CreateShaderPrimFromSdrCommand( - parent_path=prim_path, - identifier="UsdPreviewSurface", - stage_or_context=stage, - ).do() - # bind the shader graph to the material - if shader_prim: - surface_out = shader_prim.GetOutput("surface") - if surface_out: - material_prim.CreateSurfaceOutput().ConnectToSource(surface_out) - - displacement_out = shader_prim.GetOutput("displacement") - if displacement_out: - material_prim.CreateDisplacementOutput().ConnectToSource(displacement_out) - else: - raise ValueError(f"Failed to create preview surface shader at path: '{prim_path}'.") - else: - raise ValueError(f"A prim already exists at path: '{prim_path}'.") - - # obtain prim - prim = None - if shader_prim: - if hasattr(shader_prim, "GetPrim"): - prim = shader_prim.GetPrim() - elif hasattr(shader_prim, "IsValid"): - prim = shader_prim - elif hasattr(shader_prim, "GetPath"): - prim = stage.GetPrimAtPath(str(shader_prim.GetPath())) - if prim is None or not prim.IsValid(): - 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() # 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) - - return prim - - -@clone -def spawn_from_mdl_file( - prim_path: str, cfg: visual_materials_cfg.MdlFileCfg | visual_materials_cfg.GlassMdlCfg -) -> Usd.Prim: - """Load a material from its MDL file and override the settings with the given config. - - NVIDIA's `Material Definition Language (MDL) `__ - is a language for defining physically-based materials. The MDL file format is a binary format - that can be loaded by Omniverse and other applications such as Adobe Substance Designer. - To learn more about MDL, see the `documentation `_. - - The function calls the USD command `CreateMdlMaterialPrim`_ to create the prim. - - .. _CreateMdlMaterialPrim: https://docs.omniverse.nvidia.com/kit/docs/omni.usd/latest/omni.usd.commands/omni.usd.commands.CreateMdlMaterialPrimCommand.html - - .. note:: - This function is decorated with :func:`clone` that resolves prim path into list of paths - if the input prim path is a regex pattern. This is done to support spawning multiple assets - from a single and cloning the USD prim at the given path expression. - - Args: - prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, - then the asset is spawned at all the matching prim paths. - cfg: The configuration instance. - - Returns: - The created prim. - - Raises: - ValueError: If a prim already exists at the given path. - """ - # get stage handle - stage = get_current_stage() - - # spawn material if it doesn't exist. - if not stage.GetPrimAtPath(prim_path).IsValid(): - # extract material name from path - material_name = cfg.mdl_path.split("/")[-1].split(".")[0] - CreateMdlMaterialPrimCommand( - 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, - ).do() - 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() # type: ignore - del cfg["func"] - del cfg["mdl_path"] - for attr_name, attr_value in cfg.items(): - safe_set_attribute_on_usd_prim(prim, f"inputs:{attr_name}", attr_value, camel_case=False) - # return prim - return prim From 700af2f206f29f49c1199870091bf95e560152fb Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Tue, 27 Jan 2026 19:08:12 +0000 Subject: [PATCH 08/14] restore --- .../isaaclab/envs/manager_based_env.py | 593 +++++++++++++ .../isaaclab/scene/interactive_scene.py | 800 ++++++++++++++++++ .../spawners/materials/visual_materials.py | 157 ++++ 3 files changed, 1550 insertions(+) create mode 100644 source/isaaclab/isaaclab/envs/manager_based_env.py create mode 100644 source/isaaclab/isaaclab/scene/interactive_scene.py create mode 100644 source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py new file mode 100644 index 00000000000..e4c8d5cb14d --- /dev/null +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -0,0 +1,593 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import builtins +import logging +import warnings +from collections.abc import Sequence +from typing import Any + +import torch + +import omni.physx +from isaacsim.core.simulation_manager import SimulationManager + +from isaaclab.managers import ActionManager, EventManager, ObservationManager, RecorderManager +from isaaclab.scene import InteractiveScene +from isaaclab.sim import SimulationContext +from isaaclab.sim.utils.stage import attach_stage_to_usd_context, use_stage +from isaaclab.ui.widgets import ManagerLiveVisualizer +from isaaclab.utils.seed import configure_seed +from isaaclab.utils.timer import Timer +from isaaclab.utils.version import get_isaac_sim_version + +from .common import VecEnvObs +from .manager_based_env_cfg import ManagerBasedEnvCfg +from .ui import ViewportCameraController +from .utils.io_descriptors import export_articulations_data, export_scene_data + +# import logger +logger = logging.getLogger(__name__) + + +class ManagerBasedEnv: + """The base environment encapsulates the simulation scene and the environment managers for + the manager-based workflow. + + While a simulation scene or world comprises of different components such as the robots, objects, + and sensors (cameras, lidars, etc.), the environment is a higher level abstraction + that provides an interface for interacting with the simulation. The environment is comprised of + the following components: + + * **Scene**: The scene manager that creates and manages the virtual world in which the robot operates. + This includes defining the robot, static and dynamic objects, sensors, etc. + * **Observation Manager**: The observation manager that generates observations from the current simulation + state and the data gathered from the sensors. These observations may include privileged information + that is not available to the robot in the real world. Additionally, user-defined terms can be added + to process the observations and generate custom observations. For example, using a network to embed + high-dimensional observations into a lower-dimensional space. + * **Action Manager**: The action manager that processes the raw actions sent to the environment and + converts them to low-level commands that are sent to the simulation. It can be configured to accept + raw actions at different levels of abstraction. For example, in case of a robotic arm, the raw actions + can be joint torques, joint positions, or end-effector poses. Similarly for a mobile base, it can be + the joint torques, or the desired velocity of the floating base. + * **Event Manager**: The event manager orchestrates operations triggered based on simulation events. + This includes resetting the scene to a default state, applying random pushes to the robot at different intervals + of time, or randomizing properties such as mass and friction coefficients. This is useful for training + and evaluating the robot in a variety of scenarios. + * **Recorder Manager**: The recorder manager that handles recording data produced during different steps + in the simulation. This includes recording in the beginning and end of a reset and a step. The recorded data + is distinguished per episode, per environment and can be exported through a dataset file handler to a file. + + The environment provides a unified interface for interacting with the simulation. However, it does not + include task-specific quantities such as the reward function, or the termination conditions. These + quantities are often specific to defining Markov Decision Processes (MDPs) while the base environment + is agnostic to the MDP definition. + + The environment steps forward in time at a fixed time-step. The physics simulation is decimated at a + lower time-step. This is to ensure that the simulation is stable. These two time-steps can be configured + independently using the :attr:`ManagerBasedEnvCfg.decimation` (number of simulation steps per environment step) + and the :attr:`ManagerBasedEnvCfg.sim.dt` (physics time-step) parameters. Based on these parameters, the + environment time-step is computed as the product of the two. The two time-steps can be obtained by + querying the :attr:`physics_dt` and the :attr:`step_dt` properties respectively. + """ + + def __init__(self, cfg: ManagerBasedEnvCfg): + """Initialize the environment. + + Args: + cfg: The configuration object for the environment. + + Raises: + RuntimeError: If a simulation context already exists. The environment must always create one + since it configures the simulation context and controls the simulation. + """ + # check that the config is valid + cfg.validate() + # store inputs to class + self.cfg = cfg + # initialize internal variables + self._is_closed = False + + # set the seed for the environment + if self.cfg.seed is not None: + self.cfg.seed = self.seed(self.cfg.seed) + else: + logger.warning("Seed not set for the environment. The environment creation may not be deterministic.") + + # create a simulation context to control the simulator + if SimulationContext.instance() is None: + # the type-annotation is required to avoid a type-checking error + # 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() + + # make sure torch is running on the correct device + if "cuda" in self.device: + torch.cuda.set_device(self.device) + + # print useful information + print("[INFO]: Base environment:") + print(f"\tEnvironment device : {self.device}") + print(f"\tEnvironment seed : {self.cfg.seed}") + print(f"\tPhysics step-size : {self.physics_dt}") + print(f"\tRendering step-size : {self.physics_dt * self.cfg.sim.render_interval}") + print(f"\tEnvironment step-size : {self.step_dt}") + + if self.cfg.sim.render_interval < self.cfg.decimation: + msg = ( + f"The render interval ({self.cfg.sim.render_interval}) is smaller than the decimation " + f"({self.cfg.decimation}). Multiple render calls will happen for each environment step. " + "If this is not intended, set the render interval to be equal to the decimation." + ) + logger.warning(msg) + + # counter for simulation steps + self._sim_step_counter = 0 + + # allocate dictionary to store metrics + self.extras = {} + + # generate scene + with Timer("[INFO]: Time taken for scene creation", "scene_creation"): + # set the stage context for scene creation steps which use the stage + with use_stage(self.sim.get_initial_stage()): + self.scene = InteractiveScene(self.cfg.scene) + attach_stage_to_usd_context() + print("[INFO]: Scene manager: ", self.scene) + from isaaclab.sim.utils import find_matching_prim_paths + + env_prim_paths = find_matching_prim_paths("/World/envs/env_.*", stage=self.scene.stage) + print( + "[SceneDebug] env prims after InteractiveScene: " + f"num_envs_setting={self.cfg.scene.num_envs}, env_prims={len(env_prim_paths)}" + ) + # import ipdb; ipdb.set_trace() + + # set up camera viewport controller + # viewport is not available in other rendering modes so the function will throw a warning + # FIXME: This needs to be fixed in the future when we unify the UI functionalities even for + # non-rendering modes. + if self.sim.render_mode >= self.sim.RenderMode.PARTIAL_RENDERING: + self.viewport_camera_controller = ViewportCameraController(self, self.cfg.viewer) + else: + self.viewport_camera_controller = None + + # create event manager + # note: this is needed here (rather than after simulation play) to allow USD-related randomization events + # that must happen before the simulation starts. Example: randomizing mesh scale + self.event_manager = EventManager(self.cfg.events, self) + + # apply USD-related randomization events + if "prestartup" in self.event_manager.available_modes: + self.event_manager.apply(mode="prestartup") + + # 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() + + # extend UI elements + # we need to do this here after all the managers are initialized + # this is because they dictate the sensors and commands right now + if self.sim.has_gui() and self.cfg.ui_window_class_type is not None: + # setup live visualizers + self.setup_manager_visualizers() + self._window = self.cfg.ui_window_class_type(self, window_name="IsaacLab") + else: + # if no window, then we don't need to store the window + self._window = None + + # initialize observation buffers + self.obs_buf = {} + + # export IO descriptors if requested + if self.cfg.export_io_descriptors: + self.export_IO_descriptors() + + # show deprecation message for rerender_on_reset + if self.cfg.rerender_on_reset: + msg = ( + "\033[93m\033[1m[DEPRECATION WARNING] ManagerBasedEnvCfg.rerender_on_reset is deprecated. Use" + " ManagerBasedEnvCfg.num_rerenders_on_reset instead.\033[0m" + ) + warnings.warn( + msg, + FutureWarning, + stacklevel=2, + ) + if self.cfg.num_rerenders_on_reset == 0: + self.cfg.num_rerenders_on_reset = 1 + + def __del__(self): + """Cleanup for the environment.""" + self.close() + + """ + Properties. + """ + + @property + def num_envs(self) -> int: + """The number of instances of the environment that are running.""" + return self.scene.num_envs + + @property + def physics_dt(self) -> float: + """The physics time-step (in s). + + This is the lowest time-decimation at which the simulation is happening. + """ + return self.cfg.sim.dt + + @property + def step_dt(self) -> float: + """The environment stepping time-step (in s). + + This is the time-step at which the environment steps forward. + """ + return self.cfg.sim.dt * self.cfg.decimation + + @property + def device(self): + """The device on which the environment is running.""" + return self.sim.device + + @property + def get_IO_descriptors(self): + """Get the IO descriptors for the environment. + + Returns: + A dictionary with keys as the group names and values as the IO descriptors. + """ + return { + "observations": self.observation_manager.get_IO_descriptors, + "actions": self.action_manager.get_IO_descriptors, + "articulations": export_articulations_data(self), + "scene": export_scene_data(self), + } + + def export_IO_descriptors(self, output_dir: str | None = None): + """Export the IO descriptors for the environment. + + Args: + output_dir: The directory to export the IO descriptors to. + """ + import os + + import yaml + + IO_descriptors = self.get_IO_descriptors + + if output_dir is None: + if self.cfg.log_dir is not None: + output_dir = os.path.join(self.cfg.log_dir, "io_descriptors") + else: + raise ValueError( + "Output directory is not set. Please set the log directory using the `log_dir`" + " configuration or provide an explicit output_dir parameter." + ) + + if not os.path.exists(output_dir): + os.makedirs(output_dir, exist_ok=True) + + with open(os.path.join(output_dir, "IO_descriptors.yaml"), "w") as f: + print(f"[INFO]: Exporting IO descriptors to {os.path.join(output_dir, 'IO_descriptors.yaml')}") + yaml.safe_dump(IO_descriptors, f) + + """ + Operations - Setup. + """ + + def load_managers(self): + """Load the managers for the environment. + + This function is responsible for creating the various managers (action, observation, + events, etc.) for the environment. Since the managers require access to physics handles, + they can only be created after the simulator is reset (i.e. played for the first time). + + .. note:: + In case of standalone application (when running simulator from Python), the function is called + automatically when the class is initialized. + + However, in case of extension mode, the user must call this function manually after the simulator + is reset. This is because the simulator is only reset when the user calls + :meth:`SimulationContext.reset_async` and it isn't possible to call async functions in the constructor. + + """ + # prepare the managers + # -- event manager (we print it here to make the logging consistent) + print("[INFO] Event Manager: ", self.event_manager) + # -- recorder manager + self.recorder_manager = RecorderManager(self.cfg.recorders, self) + print("[INFO] Recorder Manager: ", self.recorder_manager) + # -- action manager + self.action_manager = ActionManager(self.cfg.actions, self) + print("[INFO] Action Manager: ", self.action_manager) + # -- observation manager + self.observation_manager = ObservationManager(self.cfg.observations, self) + print("[INFO] Observation Manager:", self.observation_manager) + + # perform events at the start of the simulation + # in-case a child implementation creates other managers, the randomization should happen + # when all the other managers are created + if self.__class__ == ManagerBasedEnv and "startup" in self.event_manager.available_modes: + self.event_manager.apply(mode="startup") + + def setup_manager_visualizers(self): + """Creates live visualizers for manager terms.""" + + self.manager_visualizers = { + "action_manager": ManagerLiveVisualizer(manager=self.action_manager), + "observation_manager": ManagerLiveVisualizer(manager=self.observation_manager), + } + + """ + Operations - MDP. + """ + + def reset( + self, seed: int | None = None, env_ids: Sequence[int] | None = None, options: dict[str, Any] | None = None + ) -> tuple[VecEnvObs, dict]: + """Resets the specified environments and returns observations. + + This function calls the :meth:`_reset_idx` function to reset the specified environments. + However, certain operations, such as procedural terrain generation, that happened during initialization + are not repeated. + + Args: + seed: The seed to use for randomization. Defaults to None, in which case the seed is not set. + env_ids: The environment ids to reset. Defaults to None, in which case all environments are reset. + options: Additional information to specify how the environment is reset. Defaults to None. + + Note: + This argument is used for compatibility with Gymnasium environment definition. + + Returns: + A tuple containing the observations and extras. + """ + if env_ids is None: + env_ids = torch.arange(self.num_envs, dtype=torch.int64, device=self.device) + + # trigger recorder terms for pre-reset calls + self.recorder_manager.record_pre_reset(env_ids) + + # set the seed + if seed is not None: + self.seed(seed) + + # reset state of scene + self._reset_idx(env_ids) + + # update articulation kinematics + self.scene.write_data_to_sim() + self.sim.forward() + # if sensors are added to the scene, make sure we render to reflect changes in reset + if self.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: + for _ in range(self.cfg.num_rerenders_on_reset): + self.sim.render() + + # trigger recorder terms for post-reset calls + self.recorder_manager.record_post_reset(env_ids) + + # compute observations + self.obs_buf = self.observation_manager.compute(update_history=True) + + if self.cfg.wait_for_textures and self.sim.has_rtx_sensors(): + while SimulationManager.assets_loading(): + self.sim.render() + + # return observations + return self.obs_buf, self.extras + + def reset_to( + self, + state: dict[str, dict[str, dict[str, torch.Tensor]]], + env_ids: Sequence[int] | None, + seed: int | None = None, + is_relative: bool = False, + ): + """Resets specified environments to provided states. + + This function resets the environments to the provided states. The state is a dictionary + containing the state of the scene entities. Please refer to :meth:`InteractiveScene.get_state` + for the format. + + The function is different from the :meth:`reset` function as it resets the environments to specific states, + instead of using the randomization events for resetting the environments. + + Args: + state: The state to reset the specified environments to. Please refer to + :meth:`InteractiveScene.get_state` for the format. + env_ids: The environment ids to reset. Defaults to None, in which case all environments are reset. + seed: The seed to use for randomization. Defaults to None, in which case the seed is not set. + is_relative: If set to True, the state is considered relative to the environment origins. + Defaults to False. + """ + # reset all envs in the scene if env_ids is None + if env_ids is None: + env_ids = torch.arange(self.num_envs, dtype=torch.int64, device=self.device) + + # trigger recorder terms for pre-reset calls + self.recorder_manager.record_pre_reset(env_ids) + + # set the seed + if seed is not None: + self.seed(seed) + + self._reset_idx(env_ids) + + # set the state + self.scene.reset_to(state, env_ids, is_relative=is_relative) + + # update articulation kinematics + self.sim.forward() + + # if sensors are added to the scene, make sure we render to reflect changes in reset + if self.sim.has_rtx_sensors() and self.cfg.num_rerenders_on_reset > 0: + for _ in range(self.cfg.num_rerenders_on_reset): + self.sim.render() + + # trigger recorder terms for post-reset calls + self.recorder_manager.record_post_reset(env_ids) + + # compute observations + self.obs_buf = self.observation_manager.compute(update_history=True) + + # return observations + return self.obs_buf, self.extras + + def step(self, action: torch.Tensor) -> tuple[VecEnvObs, dict]: + """Execute one time-step of the environment's dynamics. + + The environment steps forward at a fixed time-step, while the physics simulation is + decimated at a lower time-step. This is to ensure that the simulation is stable. These two + time-steps can be configured independently using the :attr:`ManagerBasedEnvCfg.decimation` (number of + simulation steps per environment step) and the :attr:`ManagerBasedEnvCfg.sim.dt` (physics time-step). + Based on these parameters, the environment time-step is computed as the product of the two. + + Args: + action: The actions to apply on the environment. Shape is (num_envs, action_dim). + + Returns: + A tuple containing the observations and extras. + """ + # process actions + self.action_manager.process_action(action.to(self.device)) + + self.recorder_manager.record_pre_step() + + # check if we need to do rendering within the physics loop + # note: checked here once to avoid multiple checks within the loop + is_rendering = self.sim.has_gui() or self.sim.has_rtx_sensors() + + # perform physics stepping + for _ in range(self.cfg.decimation): + self._sim_step_counter += 1 + # set actions into buffers + self.action_manager.apply_action() + # set actions into simulator + self.scene.write_data_to_sim() + # simulate + self.sim.step(render=False) + # render between steps only if the GUI or an RTX sensor needs it + # note: we assume the render interval to be the shortest accepted rendering interval. + # If a camera needs rendering at a faster frequency, this will lead to unexpected behavior. + if self._sim_step_counter % self.cfg.sim.render_interval == 0 and is_rendering: + self.sim.render() + # update buffers at sim dt + self.scene.update(dt=self.physics_dt) + + # post-step: step interval event + if "interval" in self.event_manager.available_modes: + self.event_manager.apply(mode="interval", dt=self.step_dt) + + # -- compute observations + self.obs_buf = self.observation_manager.compute(update_history=True) + self.recorder_manager.record_post_step() + + # return observations and extras + return self.obs_buf, self.extras + + @staticmethod + def seed(seed: int = -1) -> int: + """Set the seed for the environment. + + Args: + seed: The seed for random generator. Defaults to -1. + + Returns: + The seed used for random generator. + """ + # set seed for replicator + try: + import omni.replicator.core as rep + + rep.set_global_seed(seed) + except ModuleNotFoundError: + pass + # set seed for torch and other libraries + return configure_seed(seed) + + def close(self): + """Cleanup for the environment.""" + if not self._is_closed: + # destructor is order-sensitive + del self.viewport_camera_controller + del self.action_manager + del self.observation_manager + del self.event_manager + del self.recorder_manager + del self.scene + + # clear callbacks and instance + if get_isaac_sim_version().major >= 5: + if self.cfg.sim.create_stage_in_memory: + # detach physx stage + omni.physx.get_physx_simulation_interface().detach_stage() + self.sim.stop() + self.sim.clear() + + self.sim.clear_all_callbacks() + self.sim.clear_instance() + + # destroy the window + if self._window is not None: + self._window = None + # update closing status + self._is_closed = True + + """ + Helper functions. + """ + + def _reset_idx(self, env_ids: Sequence[int]): + """Reset environments based on specified indices. + + Args: + env_ids: List of environment ids which must be reset + """ + # reset the internal buffers of the scene elements + self.scene.reset(env_ids) + + # apply events such as randomization for environments that need a reset + if "reset" in self.event_manager.available_modes: + env_step_count = self._sim_step_counter // self.cfg.decimation + self.event_manager.apply(mode="reset", env_ids=env_ids, global_env_step_count=env_step_count) + + # iterate over all managers and reset them + # this returns a dictionary of information which is stored in the extras + # note: This is order-sensitive! Certain things need be reset before others. + self.extras["log"] = dict() + # -- observation manager + info = self.observation_manager.reset(env_ids) + self.extras["log"].update(info) + # -- action manager + info = self.action_manager.reset(env_ids) + self.extras["log"].update(info) + # -- event manager + info = self.event_manager.reset(env_ids) + self.extras["log"].update(info) + # -- recorder manager + info = self.recorder_manager.reset(env_ids) + self.extras["log"].update(info) diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py new file mode 100644 index 00000000000..291e371ca39 --- /dev/null +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -0,0 +1,800 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import logging +from collections.abc import Sequence +from typing import Any + +import torch + +import carb +from isaacsim.core.cloner import GridCloner +from pxr import PhysxSchema + +import isaaclab.sim as sim_utils +from isaaclab.assets import ( + Articulation, + ArticulationCfg, + AssetBaseCfg, + DeformableObject, + DeformableObjectCfg, + RigidObject, + RigidObjectCfg, + RigidObjectCollection, + RigidObjectCollectionCfg, + SurfaceGripper, + SurfaceGripperCfg, +) +from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg, SensorBase, SensorBaseCfg, VisuoTactileSensorCfg +from isaaclab.sim import SimulationContext +from isaaclab.sim.utils.stage import get_current_stage, get_current_stage_id +from isaaclab.sim.views import XformPrimView +from isaaclab.terrains import TerrainImporter, TerrainImporterCfg +from isaaclab.utils.version import get_isaac_sim_version + +from .interactive_scene_cfg import InteractiveSceneCfg + +# import logger +logger = logging.getLogger(__name__) + + +class InteractiveScene: + """A scene that contains entities added to the simulation. + + The interactive scene parses the :class:`InteractiveSceneCfg` class to create the scene. + Based on the specified number of environments, it clones the entities and groups them into different + categories (e.g., articulations, sensors, etc.). + + Cloning can be performed in two ways: + + * For tasks where all environments contain the same assets, a more performant cloning paradigm + can be used to allow for faster environment creation. This is specified by the ``replicate_physics`` flag. + + .. code-block:: python + + scene = InteractiveScene(cfg=InteractiveSceneCfg(replicate_physics=True)) + + * For tasks that require having separate assets in the environments, ``replicate_physics`` would have to + be set to False, which will add some costs to the overall startup time. + + .. code-block:: python + + scene = InteractiveScene(cfg=InteractiveSceneCfg(replicate_physics=False)) + + Each entity is registered to scene based on its name in the configuration class. For example, if the user + specifies a robot in the configuration class as follows: + + .. code-block:: python + + from isaaclab.scene import InteractiveSceneCfg + from isaaclab.utils import configclass + + from isaaclab_assets.robots.anymal import ANYMAL_C_CFG + + + @configclass + class MySceneCfg(InteractiveSceneCfg): + # ANYmal-C robot spawned in each environment + robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + + Then the robot can be accessed from the scene as follows: + + .. code-block:: python + + from isaaclab.scene import InteractiveScene + + # create 128 environments + scene = InteractiveScene(cfg=MySceneCfg(num_envs=128)) + + # access the robot from the scene + robot = scene["robot"] + # access the robot based on its type + robot = scene.articulations["robot"] + + If the :class:`InteractiveSceneCfg` class does not include asset entities, the cloning process + can still be triggered if assets were added to the stage outside of the :class:`InteractiveScene` class: + + .. code-block:: python + + scene = InteractiveScene(cfg=InteractiveSceneCfg(num_envs=128, replicate_physics=True)) + scene.clone_environments() + + .. note:: + It is important to note that the scene only performs common operations on the entities. For example, + resetting the internal buffers, writing the buffers to the simulation and updating the buffers from the + simulation. The scene does not perform any task specific to the entity. For example, it does not apply + actions to the robot or compute observations from the robot. These tasks are handled by different + modules called "managers" in the framework. Please refer to the :mod:`isaaclab.managers` sub-package + for more details. + """ + + def __init__(self, cfg: InteractiveSceneCfg): + """Initializes the scene. + + Args: + cfg: The configuration class for the scene. + """ + # check that the config is valid + cfg.validate() + # store inputs + self.cfg = cfg + # initialize scene elements + self._terrain = None + self._articulations = dict() + self._deformable_objects = dict() + self._rigid_objects = dict() + self._rigid_object_collections = dict() + self._sensors = dict() + self._surface_grippers = dict() + self._extras = dict() + # get stage handle + self.sim = SimulationContext.instance() + self.stage = get_current_stage() + self.stage_id = get_current_stage_id() + # physics scene path + self._physics_scene_path = None + # prepare cloner for environment replication + self.cloner = GridCloner(spacing=self.cfg.env_spacing, stage=self.stage) + self.cloner.define_base_env(self.env_ns) + self.env_prim_paths = self.cloner.generate_paths(f"{self.env_ns}/env", self.cfg.num_envs) + # create source prim + self.stage.DefinePrim(self.env_prim_paths[0], "Xform") + # allocate env indices + self._ALL_INDICES = torch.arange(self.cfg.num_envs, dtype=torch.long, device=self.device) + # when replicate_physics=False, we assume heterogeneous environments and clone the xforms first. + # this triggers per-object level cloning in the spawner. + if not self.cfg.replicate_physics: + # check version of Isaac Sim to determine whether clone_in_fabric is valid + if get_isaac_sim_version().major < 5: + # clone the env xform + env_origins = self.cloner.clone( + source_prim_path=self.env_prim_paths[0], + prim_paths=self.env_prim_paths, + replicate_physics=False, + copy_from_source=True, + enable_env_ids=( + self.cfg.filter_collisions if self.device != "cpu" else False + ), # this won't do anything because we are not replicating physics + ) + else: + # clone the env xform + env_origins = self.cloner.clone( + source_prim_path=self.env_prim_paths[0], + prim_paths=self.env_prim_paths, + replicate_physics=False, + copy_from_source=True, + enable_env_ids=( + self.cfg.filter_collisions if self.device != "cpu" else False + ), # this won't do anything because we are not replicating physics + clone_in_fabric=self.cfg.clone_in_fabric, + ) + self._default_env_origins = torch.tensor(env_origins, device=self.device, dtype=torch.float32) + else: + # otherwise, environment origins will be initialized during cloning at the end of environment creation + self._default_env_origins = None + + self._global_prim_paths = list() + if self._is_scene_setup_from_cfg(): + # add entities from config + self._add_entities_from_cfg() + # clone environments on a global scope if environment is homogeneous + if self.cfg.replicate_physics: + self.clone_environments(copy_from_source=False) + # replicate physics if we have more than one environment + # this is done to make scene initialization faster at play time + if self.cfg.replicate_physics and self.cfg.num_envs > 1: + # check version of Isaac Sim to determine whether clone_in_fabric is valid + if get_isaac_sim_version().major < 5: + self.cloner.replicate_physics( + source_prim_path=self.env_prim_paths[0], + prim_paths=self.env_prim_paths, + base_env_path=self.env_ns, + root_path=self.env_regex_ns.replace(".*", ""), + enable_env_ids=self.cfg.filter_collisions if self.device != "cpu" else False, + ) + else: + self.cloner.replicate_physics( + source_prim_path=self.env_prim_paths[0], + prim_paths=self.env_prim_paths, + base_env_path=self.env_ns, + root_path=self.env_regex_ns.replace(".*", ""), + enable_env_ids=self.cfg.filter_collisions if self.device != "cpu" else False, + clone_in_fabric=self.cfg.clone_in_fabric, + ) + + # since env_ids is only applicable when replicating physics, we have to fallback to the previous method + # to filter collisions if replicate_physics is not enabled + # additionally, env_ids is only supported in GPU simulation + if (not self.cfg.replicate_physics and self.cfg.filter_collisions) or self.device == "cpu": + self.filter_collisions(self._global_prim_paths) + + def clone_environments(self, copy_from_source: bool = False): + """Creates clones of the environment ``/World/envs/env_0``. + + Args: + copy_from_source: (bool): If set to False, clones inherit from /World/envs/env_0 and mirror its changes. + If True, clones are independent copies of the source prim and won't reflect its changes (start-up time + may increase). Defaults to False. + """ + # check if user spawned different assets in individual environments + # this flag will be None if no multi asset is spawned + carb_settings_iface = carb.settings.get_settings() + has_multi_assets = carb_settings_iface.get("/isaaclab/spawn/multi_assets") + if has_multi_assets and self.cfg.replicate_physics: + logger.warning( + "Varying assets might have been spawned under different environments." + " However, the replicate physics flag is enabled in the 'InteractiveScene' configuration." + " This may adversely affect PhysX parsing. We recommend disabling this property." + ) + + # check version of Isaac Sim to determine whether clone_in_fabric is valid + if get_isaac_sim_version().major < 5: + # clone the environment + env_origins = self.cloner.clone( + source_prim_path=self.env_prim_paths[0], + prim_paths=self.env_prim_paths, + replicate_physics=self.cfg.replicate_physics, + copy_from_source=copy_from_source, + enable_env_ids=( + self.cfg.filter_collisions if self.device != "cpu" else False + ), # this automatically filters collisions between environments + ) + else: + # clone the environment + env_origins = self.cloner.clone( + source_prim_path=self.env_prim_paths[0], + prim_paths=self.env_prim_paths, + replicate_physics=self.cfg.replicate_physics, + copy_from_source=copy_from_source, + enable_env_ids=( + self.cfg.filter_collisions if self.device != "cpu" else False + ), # this automatically filters collisions between environments + clone_in_fabric=self.cfg.clone_in_fabric, + ) + + # since env_ids is only applicable when replicating physics, we have to fallback to the previous method + # to filter collisions if replicate_physics is not enabled + # additionally, env_ids is only supported in GPU simulation + if (not self.cfg.replicate_physics and self.cfg.filter_collisions) or self.device == "cpu": + # if scene is specified through cfg, this is already taken care of + if not self._is_scene_setup_from_cfg(): + logger.warning( + "Collision filtering can only be automatically enabled when replicate_physics=True and using GPU" + " simulation. Please call scene.filter_collisions(global_prim_paths) to filter collisions across" + " environments." + ) + + # in case of heterogeneous cloning, the env origins is specified at init + if self._default_env_origins is None: + self._default_env_origins = torch.tensor(env_origins, device=self.device, dtype=torch.float32) + + def filter_collisions(self, global_prim_paths: list[str] | None = None): + """Filter environments collisions. + + Disables collisions between the environments in ``/World/envs/env_.*`` and enables collisions with the prims + in global prim paths (e.g. ground plane). + + Args: + global_prim_paths: A list of global prim paths to enable collisions with. + Defaults to None, in which case no global prim paths are considered. + """ + # validate paths in global prim paths + if global_prim_paths is None: + global_prim_paths = [] + else: + # remove duplicates in paths + global_prim_paths = list(set(global_prim_paths)) + + # if "/World/collisions" already exists in the stage, we don't filter again + if self.stage.GetPrimAtPath("/World/collisions"): + return + + # set global prim paths list if not previously defined + if len(self._global_prim_paths) < 1: + self._global_prim_paths += global_prim_paths + + # filter collisions within each environment instance + self.cloner.filter_collisions( + self.physics_scene_path, + "/World/collisions", + self.env_prim_paths, + global_paths=self._global_prim_paths, + ) + + def __str__(self) -> str: + """Returns a string representation of the scene.""" + msg = f"\n" + msg += f"\tNumber of environments: {self.cfg.num_envs}\n" + msg += f"\tEnvironment spacing : {self.cfg.env_spacing}\n" + msg += f"\tSource prim name : {self.env_prim_paths[0]}\n" + msg += f"\tGlobal prim paths : {self._global_prim_paths}\n" + msg += f"\tReplicate physics : {self.cfg.replicate_physics}" + return msg + + """ + Properties. + """ + + @property + def physics_scene_path(self) -> str: + """The path to the USD Physics Scene.""" + if self._physics_scene_path is None: + for prim in self.stage.Traverse(): + if prim.HasAPI(PhysxSchema.PhysxSceneAPI): + self._physics_scene_path = prim.GetPrimPath().pathString + logger.info(f"Physics scene prim path: {self._physics_scene_path}") + break + if self._physics_scene_path is None: + raise RuntimeError("No physics scene found! Please make sure one exists.") + return self._physics_scene_path + + @property + def physics_dt(self) -> float: + """The physics timestep of the scene.""" + return sim_utils.SimulationContext.instance().get_physics_dt() # pyright: ignore [reportOptionalMemberAccess] + + @property + def device(self) -> str: + """The device on which the scene is created.""" + return sim_utils.SimulationContext.instance().device # pyright: ignore [reportOptionalMemberAccess] + + @property + def env_ns(self) -> str: + """The namespace ``/World/envs`` in which all environments created. + + The environments are present w.r.t. this namespace under "env_{N}" prim, + where N is a natural number. + """ + return "/World/envs" + + @property + def env_regex_ns(self) -> str: + """The namespace ``/World/envs/env_.*`` in which all environments created.""" + return f"{self.env_ns}/env_.*" + + @property + def num_envs(self) -> int: + """The number of environments handled by the scene.""" + return self.cfg.num_envs + + @property + def env_origins(self) -> torch.Tensor: + """The origins of the environments in the scene. Shape is (num_envs, 3).""" + if self._terrain is not None: + return self._terrain.env_origins + else: + return self._default_env_origins + + @property + def terrain(self) -> TerrainImporter | None: + """The terrain in the scene. If None, then the scene has no terrain. + + Note: + We treat terrain separate from :attr:`extras` since terrains define environment origins and are + handled differently from other miscellaneous entities. + """ + return self._terrain + + @property + def articulations(self) -> dict[str, Articulation]: + """A dictionary of articulations in the scene.""" + return self._articulations + + @property + def deformable_objects(self) -> dict[str, DeformableObject]: + """A dictionary of deformable objects in the scene.""" + return self._deformable_objects + + @property + def rigid_objects(self) -> dict[str, RigidObject]: + """A dictionary of rigid objects in the scene.""" + return self._rigid_objects + + @property + def rigid_object_collections(self) -> dict[str, RigidObjectCollection]: + """A dictionary of rigid object collections in the scene.""" + return self._rigid_object_collections + + @property + def sensors(self) -> dict[str, SensorBase]: + """A dictionary of the sensors in the scene, such as cameras and contact reporters.""" + return self._sensors + + @property + def surface_grippers(self) -> dict[str, SurfaceGripper]: + """A dictionary of the surface grippers in the scene.""" + return self._surface_grippers + + @property + def extras(self) -> dict[str, XformPrimView]: + """A dictionary of miscellaneous simulation objects that neither inherit from assets nor sensors. + + The keys are the names of the miscellaneous objects, and the values are the + :class:`~isaaclab.sim.views.XformPrimView` instances of the corresponding prims. + + As an example, lights or other props in the scene that do not have any attributes or properties that you + want to alter at runtime can be added to this dictionary. + + Note: + These are not reset or updated by the scene. They are mainly other prims that are not necessarily + handled by the interactive scene, but are useful to be accessed by the user. + + """ + return self._extras + + @property + def state(self) -> dict[str, dict[str, dict[str, torch.Tensor]]]: + """A dictionary of the state of the scene entities in the simulation world frame. + + Please refer to :meth:`get_state` for the format. + """ + return self.get_state(is_relative=False) + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None): + """Resets the scene entities. + + Args: + env_ids: The indices of the environments to reset. + Defaults to None (all instances). + """ + # -- assets + for articulation in self._articulations.values(): + articulation.reset(env_ids) + for deformable_object in self._deformable_objects.values(): + deformable_object.reset(env_ids) + for rigid_object in self._rigid_objects.values(): + rigid_object.reset(env_ids) + for surface_gripper in self._surface_grippers.values(): + surface_gripper.reset(env_ids) + for rigid_object_collection in self._rigid_object_collections.values(): + rigid_object_collection.reset(env_ids) + # -- sensors + for sensor in self._sensors.values(): + sensor.reset(env_ids) + + def write_data_to_sim(self): + """Writes the data of the scene entities to the simulation.""" + # -- assets + for articulation in self._articulations.values(): + articulation.write_data_to_sim() + for deformable_object in self._deformable_objects.values(): + deformable_object.write_data_to_sim() + for rigid_object in self._rigid_objects.values(): + rigid_object.write_data_to_sim() + for surface_gripper in self._surface_grippers.values(): + surface_gripper.write_data_to_sim() + for rigid_object_collection in self._rigid_object_collections.values(): + rigid_object_collection.write_data_to_sim() + + def update(self, dt: float) -> None: + """Update the scene entities. + + Args: + dt: The amount of time passed from last :meth:`update` call. + """ + # -- assets + for articulation in self._articulations.values(): + articulation.update(dt) + for deformable_object in self._deformable_objects.values(): + deformable_object.update(dt) + for rigid_object in self._rigid_objects.values(): + rigid_object.update(dt) + for rigid_object_collection in self._rigid_object_collections.values(): + rigid_object_collection.update(dt) + for surface_gripper in self._surface_grippers.values(): + surface_gripper.update(dt) + # -- sensors + for sensor in self._sensors.values(): + sensor.update(dt, force_recompute=not self.cfg.lazy_sensor_update) + + """ + Operations: Scene State. + """ + + def reset_to( + self, + state: dict[str, dict[str, dict[str, torch.Tensor]]], + env_ids: Sequence[int] | None = None, + is_relative: bool = False, + ): + """Resets the entities in the scene to the provided state. + + Args: + state: The state to reset the scene entities to. Please refer to :meth:`get_state` for the format. + env_ids: The indices of the environments to reset. Defaults to None, in which case + all environment instances are reset. + is_relative: If set to True, the state is considered relative to the environment origins. + Defaults to False. + """ + # resolve env_ids + if env_ids is None: + env_ids = self._ALL_INDICES + # articulations + for asset_name, articulation in self._articulations.items(): + asset_state = state["articulation"][asset_name] + # root state + root_pose = asset_state["root_pose"].clone() + if is_relative: + root_pose[:, :3] += self.env_origins[env_ids] + root_velocity = asset_state["root_velocity"].clone() + articulation.write_root_pose_to_sim(root_pose, env_ids=env_ids) + articulation.write_root_velocity_to_sim(root_velocity, env_ids=env_ids) + # joint state + joint_position = asset_state["joint_position"].clone() + joint_velocity = asset_state["joint_velocity"].clone() + articulation.write_joint_state_to_sim(joint_position, joint_velocity, env_ids=env_ids) + # FIXME: This is not generic as it assumes PD control over the joints. + # This assumption does not hold for effort controlled joints. + articulation.set_joint_position_target(joint_position, env_ids=env_ids) + articulation.set_joint_velocity_target(joint_velocity, env_ids=env_ids) + # deformable objects + for asset_name, deformable_object in self._deformable_objects.items(): + asset_state = state["deformable_object"][asset_name] + nodal_position = asset_state["nodal_position"].clone() + if is_relative: + nodal_position[:, :3] += self.env_origins[env_ids] + nodal_velocity = asset_state["nodal_velocity"].clone() + deformable_object.write_nodal_pos_to_sim(nodal_position, env_ids=env_ids) + deformable_object.write_nodal_velocity_to_sim(nodal_velocity, env_ids=env_ids) + # rigid objects + for asset_name, rigid_object in self._rigid_objects.items(): + asset_state = state["rigid_object"][asset_name] + root_pose = asset_state["root_pose"].clone() + if is_relative: + root_pose[:, :3] += self.env_origins[env_ids] + root_velocity = asset_state["root_velocity"].clone() + rigid_object.write_root_pose_to_sim(root_pose, env_ids=env_ids) + rigid_object.write_root_velocity_to_sim(root_velocity, env_ids=env_ids) + # surface grippers + for asset_name, surface_gripper in self._surface_grippers.items(): + asset_state = state["gripper"][asset_name] + surface_gripper.set_grippers_command(asset_state) + + # write data to simulation to make sure initial state is set + # this propagates the joint targets to the simulation + self.write_data_to_sim() + + def get_state(self, is_relative: bool = False) -> dict[str, dict[str, dict[str, torch.Tensor]]]: + """Returns the state of the scene entities. + + Based on the type of the entity, the state comprises of different components. + + * For an articulation, the state comprises of the root pose, root velocity, and joint position and velocity. + * For a deformable object, the state comprises of the nodal position and velocity. + * For a rigid object, the state comprises of the root pose and root velocity. + + The returned state is a dictionary with the following format: + + .. code-block:: python + + { + "articulation": { + "entity_1_name": { + "root_pose": torch.Tensor, + "root_velocity": torch.Tensor, + "joint_position": torch.Tensor, + "joint_velocity": torch.Tensor, + }, + "entity_2_name": { + "root_pose": torch.Tensor, + "root_velocity": torch.Tensor, + "joint_position": torch.Tensor, + "joint_velocity": torch.Tensor, + }, + }, + "deformable_object": { + "entity_3_name": { + "nodal_position": torch.Tensor, + "nodal_velocity": torch.Tensor, + } + }, + "rigid_object": { + "entity_4_name": { + "root_pose": torch.Tensor, + "root_velocity": torch.Tensor, + } + }, + } + + where ``entity_N_name`` is the name of the entity registered in the scene. + + Args: + is_relative: If set to True, the state is considered relative to the environment origins. + Defaults to False. + + Returns: + A dictionary of the state of the scene entities. + """ + state = dict() + # articulations + state["articulation"] = dict() + for asset_name, articulation in self._articulations.items(): + asset_state = dict() + asset_state["root_pose"] = articulation.data.root_pose_w.clone() + if is_relative: + asset_state["root_pose"][:, :3] -= self.env_origins + asset_state["root_velocity"] = articulation.data.root_vel_w.clone() + asset_state["joint_position"] = articulation.data.joint_pos.clone() + asset_state["joint_velocity"] = articulation.data.joint_vel.clone() + state["articulation"][asset_name] = asset_state + # deformable objects + state["deformable_object"] = dict() + for asset_name, deformable_object in self._deformable_objects.items(): + asset_state = dict() + asset_state["nodal_position"] = deformable_object.data.nodal_pos_w.clone() + if is_relative: + asset_state["nodal_position"][:, :3] -= self.env_origins + asset_state["nodal_velocity"] = deformable_object.data.nodal_vel_w.clone() + state["deformable_object"][asset_name] = asset_state + # rigid objects + state["rigid_object"] = dict() + for asset_name, rigid_object in self._rigid_objects.items(): + asset_state = dict() + asset_state["root_pose"] = rigid_object.data.root_pose_w.clone() + if is_relative: + asset_state["root_pose"][:, :3] -= self.env_origins + asset_state["root_velocity"] = rigid_object.data.root_vel_w.clone() + state["rigid_object"][asset_name] = asset_state + # surface grippers + state["gripper"] = dict() + for asset_name, gripper in self._surface_grippers.items(): + state["gripper"][asset_name] = gripper.state.clone() + return state + + """ + Operations: Iteration. + """ + + def keys(self) -> list[str]: + """Returns the keys of the scene entities. + + Returns: + The keys of the scene entities. + """ + all_keys = ["terrain"] + for asset_family in [ + self._articulations, + self._deformable_objects, + self._rigid_objects, + self._rigid_object_collections, + self._sensors, + self._surface_grippers, + self._extras, + ]: + all_keys += list(asset_family.keys()) + return all_keys + + def __getitem__(self, key: str) -> Any: + """Returns the scene entity with the given key. + + Args: + key: The key of the scene entity. + + Returns: + The scene entity. + """ + # check if it is a terrain + if key == "terrain": + return self._terrain + + all_keys = ["terrain"] + # check if it is in other dictionaries + for asset_family in [ + self._articulations, + self._deformable_objects, + self._rigid_objects, + self._rigid_object_collections, + self._sensors, + self._surface_grippers, + self._extras, + ]: + out = asset_family.get(key) + # if found, return + if out is not None: + return out + all_keys += list(asset_family.keys()) + # if not found, raise error + raise KeyError(f"Scene entity with key '{key}' not found. Available Entities: '{all_keys}'") + + """ + Internal methods. + """ + + def _is_scene_setup_from_cfg(self) -> bool: + """Check if scene entities are setup from the config or not. + + Returns: + True if scene entities are setup from the config, False otherwise. + """ + return any( + not (asset_name in InteractiveSceneCfg.__dataclass_fields__ or asset_cfg is None) + for asset_name, asset_cfg in self.cfg.__dict__.items() + ) + + def _add_entities_from_cfg(self): + """Add scene entities from the config.""" + # store paths that are in global collision filter + self._global_prim_paths = list() + # parse the entire scene config and resolve regex + for asset_name, asset_cfg in self.cfg.__dict__.items(): + # skip keywords + # note: easier than writing a list of keywords: [num_envs, env_spacing, lazy_sensor_update] + if asset_name in InteractiveSceneCfg.__dataclass_fields__ or asset_cfg is None: + continue + # resolve regex + if hasattr(asset_cfg, "prim_path"): + asset_cfg.prim_path = asset_cfg.prim_path.format(ENV_REGEX_NS=self.env_regex_ns) + # create asset + if isinstance(asset_cfg, TerrainImporterCfg): + # terrains are special entities since they define environment origins + asset_cfg.num_envs = self.cfg.num_envs + asset_cfg.env_spacing = self.cfg.env_spacing + self._terrain = asset_cfg.class_type(asset_cfg) + elif isinstance(asset_cfg, ArticulationCfg): + self._articulations[asset_name] = asset_cfg.class_type(asset_cfg) + elif isinstance(asset_cfg, DeformableObjectCfg): + self._deformable_objects[asset_name] = asset_cfg.class_type(asset_cfg) + elif isinstance(asset_cfg, RigidObjectCfg): + self._rigid_objects[asset_name] = asset_cfg.class_type(asset_cfg) + elif isinstance(asset_cfg, RigidObjectCollectionCfg): + for rigid_object_cfg in asset_cfg.rigid_objects.values(): + rigid_object_cfg.prim_path = rigid_object_cfg.prim_path.format(ENV_REGEX_NS=self.env_regex_ns) + self._rigid_object_collections[asset_name] = asset_cfg.class_type(asset_cfg) + for rigid_object_cfg in asset_cfg.rigid_objects.values(): + if hasattr(rigid_object_cfg, "collision_group") and rigid_object_cfg.collision_group == -1: + asset_paths = sim_utils.find_matching_prim_paths(rigid_object_cfg.prim_path) + self._global_prim_paths += asset_paths + elif isinstance(asset_cfg, SurfaceGripperCfg): + # add surface grippers to scene + self._surface_grippers[asset_name] = asset_cfg.class_type(asset_cfg) + elif isinstance(asset_cfg, SensorBaseCfg): + # Update target frame path(s)' regex name space for FrameTransformer + if isinstance(asset_cfg, FrameTransformerCfg): + updated_target_frames = [] + for target_frame in asset_cfg.target_frames: + target_frame.prim_path = target_frame.prim_path.format(ENV_REGEX_NS=self.env_regex_ns) + updated_target_frames.append(target_frame) + asset_cfg.target_frames = updated_target_frames + elif isinstance(asset_cfg, ContactSensorCfg): + updated_filter_prim_paths_expr = [] + for filter_prim_path in asset_cfg.filter_prim_paths_expr: + updated_filter_prim_paths_expr.append(filter_prim_path.format(ENV_REGEX_NS=self.env_regex_ns)) + asset_cfg.filter_prim_paths_expr = updated_filter_prim_paths_expr + elif isinstance(asset_cfg, VisuoTactileSensorCfg): + if hasattr(asset_cfg, "camera_cfg") and asset_cfg.camera_cfg is not None: + asset_cfg.camera_cfg.prim_path = asset_cfg.camera_cfg.prim_path.format( + ENV_REGEX_NS=self.env_regex_ns + ) + if ( + hasattr(asset_cfg, "contact_object_prim_path_expr") + and asset_cfg.contact_object_prim_path_expr is not None + ): + asset_cfg.contact_object_prim_path_expr = asset_cfg.contact_object_prim_path_expr.format( + ENV_REGEX_NS=self.env_regex_ns + ) + + self._sensors[asset_name] = asset_cfg.class_type(asset_cfg) + elif isinstance(asset_cfg, AssetBaseCfg): + # manually spawn asset + if asset_cfg.spawn is not None: + asset_cfg.spawn.func( + asset_cfg.prim_path, + asset_cfg.spawn, + translation=asset_cfg.init_state.pos, + orientation=asset_cfg.init_state.rot, + ) + # store xform prim view corresponding to this asset + # all prims in the scene are Xform prims (i.e. have a transform component) + self._extras[asset_name] = XformPrimView(asset_cfg.prim_path, device=self.device, stage=self.stage) + else: + raise ValueError(f"Unknown asset config type for {asset_name}: {asset_cfg}") + # store global collision paths + if hasattr(asset_cfg, "collision_group") and asset_cfg.collision_group == -1: + asset_paths = sim_utils.find_matching_prim_paths(asset_cfg.prim_path) + self._global_prim_paths += asset_paths diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py new file mode 100644 index 00000000000..074d6ac0e43 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py @@ -0,0 +1,157 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +from typing import TYPE_CHECKING + +from omni.usd.commands import CreateMdlMaterialPrimCommand, CreateShaderPrimFromSdrCommand +from pxr import Usd, UsdShade + +from isaaclab.sim.utils import clone, safe_set_attribute_on_usd_prim +from isaaclab.sim.utils.stage import get_current_stage +from isaaclab.utils.assets import NVIDIA_NUCLEUS_DIR + +if TYPE_CHECKING: + from . import visual_materials_cfg + +# import logger +logger = logging.getLogger(__name__) + + +@clone +def spawn_preview_surface(prim_path: str, cfg: visual_materials_cfg.PreviewSurfaceCfg) -> Usd.Prim: + """Create a preview surface prim and override the settings with the given config. + + A preview surface is a physically-based surface that handles simple shaders while supporting + both *specular* and *metallic* workflows. All color inputs are in linear color space (RGB). + For more information, see the `documentation `__. + + The function calls the USD command `CreateShaderPrimFromSdrCommand`_ to create the prim. + + .. _CreateShaderPrimFromSdrCommand: https://docs.omniverse.nvidia.com/kit/docs/omni.usd/latest/omni.usd.commands/omni.usd.commands.CreateShaderPrimFromSdrCommand.html + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # get stage handle + stage = get_current_stage() + + # spawn material if it doesn't exist. + if not stage.GetPrimAtPath(prim_path).IsValid(): + # note: we don't use Omniverse's CreatePreviewSurfaceMaterialPrimCommand + # since it does not support USD stage as an argument. The created material + # in that case is always the one from USD Context which makes it difficult to + # handle scene creation on a custom stage. + material_prim = UsdShade.Material.Define(stage, prim_path) + if material_prim: + shader_prim = CreateShaderPrimFromSdrCommand( + parent_path=prim_path, + identifier="UsdPreviewSurface", + stage_or_context=stage, + name="Shader", + ).do() + # bind the shader graph to the material + if shader_prim: + surface_out = shader_prim.GetOutput("surface") + if surface_out: + material_prim.CreateSurfaceOutput().ConnectToSource(surface_out) + + displacement_out = shader_prim.GetOutput("displacement") + if displacement_out: + material_prim.CreateDisplacementOutput().ConnectToSource(displacement_out) + else: + raise ValueError(f"Failed to create preview surface shader at path: '{prim_path}'.") + 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() # 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) + + return prim + + +@clone +def spawn_from_mdl_file( + prim_path: str, cfg: visual_materials_cfg.MdlFileCfg | visual_materials_cfg.GlassMdlCfg +) -> Usd.Prim: + """Load a material from its MDL file and override the settings with the given config. + + NVIDIA's `Material Definition Language (MDL) `__ + is a language for defining physically-based materials. The MDL file format is a binary format + that can be loaded by Omniverse and other applications such as Adobe Substance Designer. + To learn more about MDL, see the `documentation `_. + + The function calls the USD command `CreateMdlMaterialPrim`_ to create the prim. + + .. _CreateMdlMaterialPrim: https://docs.omniverse.nvidia.com/kit/docs/omni.usd/latest/omni.usd.commands/omni.usd.commands.CreateMdlMaterialPrimCommand.html + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration instance. + + Returns: + The created prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + # get stage handle + stage = get_current_stage() + + # spawn material if it doesn't exist. + if not stage.GetPrimAtPath(prim_path).IsValid(): + # extract material name from path + material_name = cfg.mdl_path.split("/")[-1].split(".")[0] + CreateMdlMaterialPrimCommand( + 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, + ).do() + 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() # type: ignore + del cfg["func"] + del cfg["mdl_path"] + for attr_name, attr_value in cfg.items(): + safe_set_attribute_on_usd_prim(prim, f"inputs:{attr_name}", attr_value, camel_case=False) + # return prim + return prim From 2da5d712bfd20b5007d219bb01de3ea824163031 Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Tue, 27 Jan 2026 19:09:43 +0000 Subject: [PATCH 09/14] revert file --- source/isaaclab/isaaclab/envs/manager_based_env.py | 8 -------- 1 file changed, 8 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index e4c8d5cb14d..3ff6d291c0a 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -142,14 +142,6 @@ def __init__(self, cfg: ManagerBasedEnvCfg): self.scene = InteractiveScene(self.cfg.scene) attach_stage_to_usd_context() print("[INFO]: Scene manager: ", self.scene) - from isaaclab.sim.utils import find_matching_prim_paths - - env_prim_paths = find_matching_prim_paths("/World/envs/env_.*", stage=self.scene.stage) - print( - "[SceneDebug] env prims after InteractiveScene: " - f"num_envs_setting={self.cfg.scene.num_envs}, env_prims={len(env_prim_paths)}" - ) - # import ipdb; ipdb.set_trace() # set up camera viewport controller # viewport is not available in other rendering modes so the function will throw a warning From 35e5adb1eeff887b54dd8e59eaf30a9a9ebed7e5 Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Tue, 27 Jan 2026 21:39:54 +0000 Subject: [PATCH 10/14] fixes + rm contact debug --- .../isaaclab/scene/interactive_scene.py | 4 + .../ov_scene_data_provider.py | 156 +--------- .../spawners/materials/visual_materials.py | 42 +-- .../isaaclab/visualizers/newton_visualizer.py | 58 +--- .../isaaclab/visualizers/ov_visualizer.py | 267 +++++++----------- .../isaaclab/visualizers/rerun_visualizer.py | 77 +---- .../isaaclab/visualizers/visualizer_cfg.py | 10 +- 7 files changed, 157 insertions(+), 457 deletions(-) diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 291e371ca39..7d2c9be845c 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -120,6 +120,10 @@ def __init__(self, cfg: InteractiveSceneCfg): cfg.validate() # store inputs self.cfg = cfg + + # TODO(mtrepte): rm after fix + self.cfg.clone_in_fabric = False + # initialize scene elements self._terrain = None self._articulations = dict() diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py index a6e6e7c4050..3a6f1b57ca9 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py @@ -19,7 +19,7 @@ class OVSceneDataProvider: """Scene data provider for Omni PhysX physics backend. - Native (cheap): USD stage, PhysX transforms/velocities/contacts + Native (cheap): USD stage, PhysX transforms/velocities Adapted (expensive): Newton Model/State (built from USD, synced each step) Performance: Only builds Newton data if Newton/Rerun visualizers are active. @@ -34,7 +34,6 @@ def __init__(self, visualizer_cfgs: list[Any] | None, stage, simulation_context) self._physics_sim_view = SimulationManager.get_physics_sim_view() self._rigid_body_view = None self._articulation_view = None - self._contact_view = None self._xform_views: dict[str, Any] = {} self._body_key_index_map: dict[str, int] = {} self._view_body_index_map: dict[str, list[int]] = {} @@ -75,6 +74,7 @@ def __init__(self, visualizer_cfgs: list[Any] | None, stage, simulation_context) logger.info("[OVSceneDataProvider] OV visualizer only - skipping Newton model build") def _get_num_envs(self) -> int: + # TODO(mtrepte): is there a better way to get num_envs? try: import carb @@ -135,7 +135,6 @@ def _build_newton_model_from_usd(self) -> None: self._newton_state = self._newton_model.state() self._rigid_body_paths = list(self._newton_model.body_key) self._xform_views.clear() - self._contact_view = None self._body_key_index_map = {path: i for i, path in enumerate(self._rigid_body_paths)} self._view_body_index_map = {} except ModuleNotFoundError as exc: @@ -187,6 +186,7 @@ def _setup_articulation_view(self) -> None: self._articulation_view = None def _get_view_world_poses(self, view): + # TODO(mtrepte): this can be revisited & simplifiedafter the function naming gets unified if view is None: return None, None @@ -271,146 +271,6 @@ def _get_view_velocities(self, view): return None, None - def _setup_contact_view(self) -> None: - if not self._rigid_body_paths or self._contact_view is not None: - return - try: - from pxr import PhysxSchema - - contact_paths = [ - path - for path in self._rigid_body_paths - if self._stage.GetPrimAtPath(path).HasAPI(PhysxSchema.PhysxContactReportAPI) - ] - if contact_paths: - contact_paths = self._wildcard_env_paths(contact_paths) - self._contact_view = self._physics_sim_view.create_rigid_contact_view(contact_paths) - except Exception as exc: - logger.debug(f"[SceneDataProvider] Failed to create ContactView: {exc}") - self._contact_view = None - - def _get_view_contacts(self, view): - if view is None: - return None - dt = getattr(self._simulation_context, "cfg", None) - dt = getattr(dt, "dt", 1.0 / 60.0) if dt is not None else 1.0 / 60.0 - - try: - num_envs = int(self._metadata.get("num_envs") or 1) - view_count = getattr(view, "count", 0) - num_bodies = view_count // num_envs if num_envs > 0 and view_count else 0 - num_filters = getattr(view, "filter_count", 0) - - output: dict[str, Any] = {} - - method = getattr(view, "get_net_contact_forces", None) - if method is not None: - output["net_forces_w"] = method(dt=dt) - - method = getattr(view, "get_contact_data", None) - if method is not None: - data = method(dt=dt) - if isinstance(data, tuple) and len(data) >= 6: - _, contact_points, contact_normals, contact_distances, buffer_count, buffer_start_indices = data[:6] - if num_envs > 0 and num_bodies > 0 and num_filters > 0: - output["contact_points_w"] = self._unpack_contact_buffer_data( - contact_points, - buffer_count, - buffer_start_indices, - num_envs, - num_bodies, - num_filters, - avg=True, - default=float("nan"), - ) - output["contact_normals_w"] = self._unpack_contact_buffer_data( - contact_normals, - buffer_count, - buffer_start_indices, - num_envs, - num_bodies, - num_filters, - avg=True, - default=float("nan"), - ) - output["contact_distances"] = self._unpack_contact_buffer_data( - contact_distances.view(-1, 1), - buffer_count, - buffer_start_indices, - num_envs, - num_bodies, - num_filters, - avg=True, - default=float("nan"), - ).squeeze(-1) - - method = getattr(view, "get_friction_data", None) - if method is not None: - data = method(dt=dt) - if isinstance(data, tuple) and len(data) >= 4: - friction_forces, _, buffer_count, buffer_start_indices = data[:4] - if num_envs > 0 and num_bodies > 0 and num_filters > 0: - output["friction_forces_w"] = self._unpack_contact_buffer_data( - friction_forces, - buffer_count, - buffer_start_indices, - num_envs, - num_bodies, - num_filters, - avg=False, - default=0.0, - ) - - if not output: - return None - return output - except Exception as exc: - logger.debug(f"[SceneDataProvider] Failed to read ContactView data: {exc}") - return None - - def _unpack_contact_buffer_data( - self, - contact_data, - buffer_count, - buffer_start_indices, - num_envs: int, - num_bodies: int, - num_filters: int, - avg: bool = True, - default: float = float("nan"), - ): - try: - import torch - - if num_envs <= 0 or num_bodies <= 0 or num_filters <= 0: - return None - - counts = buffer_count.view(-1) - starts = buffer_start_indices.view(-1) - n_rows = counts.numel() - if contact_data is None or n_rows == 0: - return None - - data = contact_data - if data.ndim == 1: - data = data.view(-1, 1) - dim = data.shape[-1] - - agg = torch.full((n_rows, dim), default, device=data.device, dtype=data.dtype) - total = int(counts.sum().item()) - if total > 0: - row_ids = torch.repeat_interleave(torch.arange(n_rows, device=data.device), counts) - block_starts = counts.cumsum(0) - counts - deltas = torch.arange(row_ids.numel(), device=data.device) - block_starts.repeat_interleave(counts) - flat_idx = starts[row_ids] + deltas - pts = data.index_select(0, flat_idx) - agg = agg.zero_().index_add_(0, row_ids, pts) - agg = agg / counts.clamp_min(1).unsqueeze(-1) if avg else agg - agg[counts == 0] = default - - return agg.view(num_envs, num_bodies, num_filters, dim) - except Exception: - return None def _get_xform_world_poses(self): if not self._rigid_body_paths: @@ -525,7 +385,6 @@ def update(self) -> None: # Future extensions: # - Populate velocities into self._newton_state.body_qd - # - Sync contacts into Newton Contacts for richer debug visualizations # - Cache mesh/material data for Rerun/renderer integrations except Exception as exc: logger.debug(f"[SceneDataProvider] Failed to sync Omni transforms to Newton state: {exc}") @@ -580,10 +439,5 @@ def get_velocities(self) -> dict[str, Any] | None: return None def get_contacts(self) -> dict[str, Any] | None: - if self._contact_view is None: - self._setup_contact_view() - data = self._get_view_contacts(self._contact_view) - if data is None: - return None - data["source"] = "contact_view" - return data + """Contacts not yet supported for OV backend.""" + return None diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py index 074d6ac0e43..6b87c99ea65 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py @@ -60,30 +60,32 @@ def spawn_preview_surface(prim_path: str, cfg: visual_materials_cfg.PreviewSurfa # in that case is always the one from USD Context which makes it difficult to # handle scene creation on a custom stage. material_prim = UsdShade.Material.Define(stage, prim_path) - if material_prim: - shader_prim = CreateShaderPrimFromSdrCommand( - parent_path=prim_path, - identifier="UsdPreviewSurface", - stage_or_context=stage, - name="Shader", - ).do() - # bind the shader graph to the material - if shader_prim: - surface_out = shader_prim.GetOutput("surface") - if surface_out: - material_prim.CreateSurfaceOutput().ConnectToSource(surface_out) - - displacement_out = shader_prim.GetOutput("displacement") - if displacement_out: - material_prim.CreateDisplacementOutput().ConnectToSource(displacement_out) - else: + if not material_prim: raise ValueError(f"Failed to create preview surface shader at path: '{prim_path}'.") + + shader_prim = CreateShaderPrimFromSdrCommand( + parent_path=prim_path, + identifier="UsdPreviewSurface", + stage_or_context=stage, + ).do() + + if not shader_prim: + raise ValueError(f"Failed to create shader prim at path: '{prim_path}'.") + + # The command returns a Shader object directly, not a path + if shader_prim: + surface_out = shader_prim.GetOutput("surface") + if surface_out: + material_prim.CreateSurfaceOutput().ConnectToSource(surface_out) + + displacement_out = shader_prim.GetOutput("displacement") + if displacement_out: + material_prim.CreateDisplacementOutput().ConnectToSource(displacement_out) 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 + # Get the underlying prim from the shader + prim = shader_prim.GetPrim() if not prim.IsValid(): raise ValueError(f"Failed to create preview surface material at path: '{prim_path}'.") # apply properties diff --git a/source/isaaclab/isaaclab/visualizers/newton_visualizer.py b/source/isaaclab/isaaclab/visualizers/newton_visualizer.py index 719331a31dc..39c9eddf82d 100644 --- a/source/isaaclab/isaaclab/visualizers/newton_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/newton_visualizer.py @@ -208,31 +208,13 @@ def initialize(self, scene_data: dict[str, Any] | None = None) -> None: if self._is_initialized: return - if scene_data and "scene_data_provider" in scene_data: - self._scene_data_provider = scene_data["scene_data_provider"] - - metadata = {} - if self._scene_data_provider: - self._model = self._scene_data_provider.get_newton_model() - self._state = self._scene_data_provider.get_newton_state() - metadata = self._scene_data_provider.get_metadata() - else: - try: - from isaaclab.sim._impl.newton_manager import NewtonManager - - self._model = NewtonManager._model - self._state = NewtonManager._state_0 - metadata = { - "physics_backend": "newton", - "num_envs": NewtonManager._num_envs if NewtonManager._num_envs is not None else 0, - "gravity_vector": NewtonManager._gravity_vector, - "clone_physics_only": NewtonManager._clone_physics_only, - } - except Exception: - pass - - if self._model is None: - raise RuntimeError("Newton visualizer requires a Newton Model. Ensure a scene data provider is available.") + if not scene_data or "scene_data_provider" not in scene_data: + raise RuntimeError("Newton visualizer requires scene_data_provider.") + + self._scene_data_provider = scene_data["scene_data_provider"] + self._model = self._scene_data_provider.get_newton_model() + self._state = self._scene_data_provider.get_newton_state() + metadata = self._scene_data_provider.get_metadata() self._viewer = NewtonViewerGL( width=self.cfg.window_width, @@ -281,25 +263,15 @@ def step(self, dt: float, state: Any | None = None) -> None: self._sim_time += dt self._step_counter += 1 + self._state = self._scene_data_provider.get_newton_state() + contacts = None - contacts_data = None - if self._scene_data_provider: - self._state = self._scene_data_provider.get_newton_state() - if self._viewer.show_contacts: - contacts_data = self._scene_data_provider.get_contacts() - if isinstance(contacts_data, dict): - contacts = contacts_data.get("contacts", contacts_data) - else: - contacts = contacts_data - else: - try: - from isaaclab.sim._impl.newton_manager import NewtonManager - - self._state = NewtonManager._state_0 - if self._viewer.show_contacts: - contacts = NewtonManager._contacts - except Exception: - self._state = None + if self._viewer.show_contacts: + contacts_data = self._scene_data_provider.get_contacts() + if isinstance(contacts_data, dict): + contacts = contacts_data.get("contacts", contacts_data) + else: + contacts = contacts_data update_frequency = self._viewer._update_frequency if self._viewer else self._update_frequency if self._step_counter % update_frequency != 0: diff --git a/source/isaaclab/isaaclab/visualizers/ov_visualizer.py b/source/isaaclab/isaaclab/visualizers/ov_visualizer.py index 97b64adc9b8..33596629561 100644 --- a/source/isaaclab/isaaclab/visualizers/ov_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/ov_visualizer.py @@ -38,24 +38,9 @@ def initialize(self, scene_data: dict[str, Any] | None = None) -> None: logger.warning("[OVVisualizer] Already initialized.") return - usd_stage = None - scene_data_provider = None - if scene_data is not None: - usd_stage = scene_data.get("usd_stage") - scene_data_provider = scene_data.get("scene_data_provider") - - if usd_stage is None: - raise RuntimeError("OV visualizer requires a USD stage.") - - metadata = {} - if scene_data_provider is not None: - metadata = scene_data_provider.get_metadata() - else: - metadata = { - "num_envs": 0, - "physics_backend": "omni", - "env_prim_pattern": "/World/envs/env_{}", - } + usd_stage = scene_data["usd_stage"] + scene_data_provider = scene_data["scene_data_provider"] + metadata = scene_data_provider.get_metadata() self._ensure_simulation_app() self._setup_viewport(usd_stage, metadata) @@ -103,171 +88,111 @@ def set_camera_view( self._set_viewport_camera(tuple(eye), tuple(target)) def _ensure_simulation_app(self) -> None: + import omni.kit.app + + app = omni.kit.app.get_app() + if app is None or not app.is_running(): + raise RuntimeError("[OVVisualizer] Isaac Sim app is not running.") + try: - import omni.kit.app - - app = omni.kit.app.get_app() - if app is None or not app.is_running(): - raise RuntimeError( - "[OVVisualizer] No Isaac Sim app is running. " - "OV visualizer requires Isaac Sim to be launched before initialization." - ) - - try: - from isaacsim import SimulationApp - - sim_app = None - if hasattr(SimulationApp, "_instance") and SimulationApp._instance is not None: - sim_app = SimulationApp._instance - elif hasattr(SimulationApp, "instance") and callable(SimulationApp.instance): - sim_app = SimulationApp.instance() - - if sim_app is not None: - self._simulation_app = sim_app - if self._simulation_app.config.get("headless", False): - logger.warning( - "[OVVisualizer] Running in headless mode. " - "OV visualizer requires GUI mode (launch with --headless=False)." - ) - else: - logger.info("[OVVisualizer] Using existing Isaac Sim app instance.") - else: - logger.info("[OVVisualizer] Isaac Sim app is running (via omni.kit.app).") - except ImportError: - logger.info("[OVVisualizer] Using running Isaac Sim app (SimulationApp module not available).") - except ImportError as exc: - raise ImportError( - f"[OVVisualizer] Could not import omni.kit.app: {exc}. Isaac Sim may not be installed or not running." - ) + from isaacsim import SimulationApp + + sim_app = None + if hasattr(SimulationApp, "_instance") and SimulationApp._instance is not None: + sim_app = SimulationApp._instance + elif hasattr(SimulationApp, "instance") and callable(SimulationApp.instance): + sim_app = SimulationApp.instance() + + if sim_app is not None: + self._simulation_app = sim_app + if self._simulation_app.config.get("headless", False): + logger.warning("[OVVisualizer] Running in headless mode. Viewport may not display.") + except ImportError: + pass def _setup_viewport(self, usd_stage, metadata: dict) -> None: - try: - import omni.kit.viewport.utility as vp_utils - from omni.ui import DockPosition - - if self.cfg.create_viewport and self.cfg.viewport_name: - dock_position_map = { - "LEFT": DockPosition.LEFT, - "RIGHT": DockPosition.RIGHT, - "BOTTOM": DockPosition.BOTTOM, - "SAME": DockPosition.SAME, - } - dock_pos = dock_position_map.get(self.cfg.dock_position.upper(), DockPosition.SAME) - - self._viewport_window = vp_utils.create_viewport_window( - name=self.cfg.viewport_name, - width=self.cfg.window_width, - height=self.cfg.window_height, - position_x=50, - position_y=50, - docked=True, - ) - - logger.info(f"[OVVisualizer] Created viewport '{self.cfg.viewport_name}'") - asyncio.ensure_future(self._dock_viewport_async(self.cfg.viewport_name, dock_pos)) - - if self._viewport_window: - self._create_and_assign_camera(usd_stage) - else: - if self.cfg.viewport_name: - self._viewport_window = vp_utils.get_viewport_window_by_name(self.cfg.viewport_name) - if self._viewport_window is None: - logger.warning( - f"[OVVisualizer] Viewport '{self.cfg.viewport_name}' not found. Using active viewport." - ) - self._viewport_window = vp_utils.get_active_viewport_window() - else: - logger.info(f"[OVVisualizer] Using existing viewport '{self.cfg.viewport_name}'") - else: - self._viewport_window = vp_utils.get_active_viewport_window() - logger.info("[OVVisualizer] Using existing active viewport") + import omni.kit.viewport.utility as vp_utils + from omni.ui import DockPosition + + if self.cfg.create_viewport and self.cfg.viewport_name: + dock_position_map = { + "LEFT": DockPosition.LEFT, + "RIGHT": DockPosition.RIGHT, + "BOTTOM": DockPosition.BOTTOM, + "SAME": DockPosition.SAME, + } + dock_pos = dock_position_map.get(self.cfg.dock_position.upper(), DockPosition.SAME) + + self._viewport_window = vp_utils.create_viewport_window( + name=self.cfg.viewport_name, + width=self.cfg.window_width, + height=self.cfg.window_height, + position_x=50, + position_y=50, + docked=True, + ) - if self._viewport_window is None: - logger.warning("[OVVisualizer] Could not get/create viewport.") - return + logger.info(f"[OVVisualizer] Created viewport '{self.cfg.viewport_name}'") + asyncio.ensure_future(self._dock_viewport_async(self.cfg.viewport_name, dock_pos)) + self._create_and_assign_camera(usd_stage) + else: + if self.cfg.viewport_name: + self._viewport_window = vp_utils.get_viewport_window_by_name(self.cfg.viewport_name) + if self._viewport_window is None: + logger.warning(f"[OVVisualizer] Viewport '{self.cfg.viewport_name}' not found. Using active.") + self._viewport_window = vp_utils.get_active_viewport_window() + else: + self._viewport_window = vp_utils.get_active_viewport_window() - self._viewport_api = self._viewport_window.viewport_api - self._set_viewport_camera(self.cfg.camera_position, self.cfg.camera_target) - logger.info(f"[OVVisualizer] Viewport configured (size: {self.cfg.window_width}x{self.cfg.window_height})") - except ImportError as exc: - logger.warning(f"[OVVisualizer] Viewport utilities unavailable: {exc}") - except Exception as exc: - logger.error(f"[OVVisualizer] Error setting up viewport: {exc}") + self._viewport_api = self._viewport_window.viewport_api + self._set_viewport_camera(self.cfg.camera_position, self.cfg.camera_target) async def _dock_viewport_async(self, viewport_name: str, dock_position) -> None: - try: - import omni.kit.app - import omni.ui - - viewport_window = None - for i in range(10): - viewport_window = omni.ui.Workspace.get_window(viewport_name) - if viewport_window: - logger.info(f"[OVVisualizer] Found viewport window '{viewport_name}' after {i} frames") + import omni.kit.app + import omni.ui + + viewport_window = None + for _ in range(10): + viewport_window = omni.ui.Workspace.get_window(viewport_name) + if viewport_window: + break + await omni.kit.app.get_app().next_update_async() + + if not viewport_window: + logger.warning(f"[OVVisualizer] Could not find viewport window '{viewport_name}'.") + return + + main_viewport = omni.ui.Workspace.get_window("Viewport") + if not main_viewport: + for alt_name in ["/OmniverseKit/Viewport", "Viewport Next"]: + main_viewport = omni.ui.Workspace.get_window(alt_name) + if main_viewport: break - await omni.kit.app.get_app().next_update_async() - - if not viewport_window: - logger.warning(f"[OVVisualizer] Could not find viewport window '{viewport_name}' in workspace.") - return - - main_viewport = omni.ui.Workspace.get_window("Viewport") - if not main_viewport: - for alt_name in ["/OmniverseKit/Viewport", "Viewport Next"]: - main_viewport = omni.ui.Workspace.get_window(alt_name) - if main_viewport: - break - - if main_viewport and main_viewport != viewport_window: - viewport_window.dock_in(main_viewport, dock_position, 0.5) - await omni.kit.app.get_app().next_update_async() - viewport_window.focus() - viewport_window.visible = True - await omni.kit.app.get_app().next_update_async() - viewport_window.focus() - - logger.info( - f"[OVVisualizer] Docked viewport '{viewport_name}' at position {self.cfg.dock_position} and set as" - " active" - ) - else: - logger.info( - f"[OVVisualizer] Could not find main viewport for docking. Viewport '{viewport_name}' will remain" - " floating." - ) - except Exception as exc: - logger.warning(f"[OVVisualizer] Error docking viewport: {exc}") + + if main_viewport and main_viewport != viewport_window: + viewport_window.dock_in(main_viewport, dock_position, 0.5) + await omni.kit.app.get_app().next_update_async() + viewport_window.focus() + viewport_window.visible = True + await omni.kit.app.get_app().next_update_async() + viewport_window.focus() def _create_and_assign_camera(self, usd_stage) -> None: - try: - camera_path = f"/World/Cameras/{self.cfg.viewport_name}_Camera" - camera_prim = usd_stage.GetPrimAtPath(camera_path) - if not camera_prim.IsValid(): - UsdGeom.Camera.Define(usd_stage, camera_path) - logger.info(f"[OVVisualizer] Created camera: {camera_path}") - else: - logger.info(f"[OVVisualizer] Using existing camera: {camera_path}") + camera_path = f"/World/Cameras/{self.cfg.viewport_name}_Camera" + camera_prim = usd_stage.GetPrimAtPath(camera_path) + if not camera_prim.IsValid(): + UsdGeom.Camera.Define(usd_stage, camera_path) - if self._viewport_api: - self._viewport_api.set_active_camera(camera_path) - logger.info(f"[OVVisualizer] Assigned camera '{camera_path}' to viewport '{self.cfg.viewport_name}'") - except Exception as exc: - logger.warning(f"[OVVisualizer] Could not create/assign camera: {exc}. Using default camera.") + if self._viewport_api: + self._viewport_api.set_active_camera(camera_path) def _set_viewport_camera(self, position: tuple[float, float, float], target: tuple[float, float, float]) -> None: - if self._viewport_api is None: - return + import isaacsim.core.utils.viewports as vp_utils - try: - import isaacsim.core.utils.viewports as vp_utils - - camera_path = self._viewport_api.get_active_camera() - if not camera_path: - camera_path = "/OmniverseKit_Persp" + camera_path = self._viewport_api.get_active_camera() + if not camera_path: + camera_path = "/OmniverseKit_Persp" - vp_utils.set_camera_view( - eye=list(position), target=list(target), camera_prim_path=camera_path, viewport_api=self._viewport_api - ) - logger.info(f"[OVVisualizer] Camera set: pos={position}, target={target}, camera={camera_path}") - except Exception as exc: - logger.warning(f"[OVVisualizer] Could not set camera: {exc}") + vp_utils.set_camera_view( + eye=list(position), target=list(target), camera_prim_path=camera_path, viewport_api=self._viewport_api + ) diff --git a/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py b/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py index a56f4581654..00d2623001e 100644 --- a/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py @@ -13,22 +13,14 @@ from .rerun_visualizer_cfg import RerunVisualizerCfg from .visualizer import Visualizer -logger = logging.getLogger(__name__) - -try: - import rerun as rr - import rerun.blueprint as rrb - from newton.viewer import ViewerRerun +import rerun as rr +import rerun.blueprint as rrb +from newton.viewer import ViewerRerun - _RERUN_AVAILABLE = True -except ImportError: - rr = None - rrb = None - ViewerRerun = None - _RERUN_AVAILABLE = False +logger = logging.getLogger(__name__) -class NewtonViewerRerun(ViewerRerun if _RERUN_AVAILABLE else object): +class NewtonViewerRerun(ViewerRerun): """Isaac Lab wrapper for Newton's ViewerRerun.""" def __init__( @@ -72,10 +64,6 @@ class RerunVisualizer(Visualizer): def __init__(self, cfg: RerunVisualizerCfg): super().__init__(cfg) self.cfg: RerunVisualizerCfg = cfg - - if not _RERUN_AVAILABLE: - raise ImportError("Rerun visualizer requires rerun-sdk and Newton. Install: pip install rerun-sdk") - self._viewer: NewtonViewerRerun | None = None self._model = None self._state = None @@ -85,39 +73,16 @@ def __init__(self, cfg: RerunVisualizerCfg): def initialize(self, scene_data: dict[str, Any] | None = None) -> None: if self._is_initialized: - logger.warning("[RerunVisualizer] Already initialized. Skipping re-initialization.") + logger.warning("[RerunVisualizer] Already initialized.") return - if scene_data and "scene_data_provider" in scene_data: - self._scene_data_provider = scene_data["scene_data_provider"] - - metadata = {} - if self._scene_data_provider: - self._model = self._scene_data_provider.get_newton_model() - self._state = self._scene_data_provider.get_newton_state() - metadata = self._scene_data_provider.get_metadata() - else: - try: - from isaaclab.sim._impl.newton_manager import NewtonManager - - self._model = NewtonManager._model - self._state = NewtonManager._state_0 - metadata = { - "physics_backend": "newton", - "num_envs": NewtonManager._num_envs if NewtonManager._num_envs is not None else 0, - "gravity_vector": NewtonManager._gravity_vector, - "clone_physics_only": NewtonManager._clone_physics_only, - } - except Exception: - pass - - if self._model is None: - raise RuntimeError( - "Rerun visualizer requires a Newton Model. Ensure a scene data provider is available." - ) + if not scene_data or "scene_data_provider" not in scene_data: + raise RuntimeError("Rerun visualizer requires scene_data_provider.") - if self._state is None: - logger.warning("[RerunVisualizer] No Newton State available. Visualization may not work correctly.") + self._scene_data_provider = scene_data["scene_data_provider"] + self._model = self._scene_data_provider.get_newton_model() + self._state = self._scene_data_provider.get_newton_state() + metadata = self._scene_data_provider.get_metadata() try: if self.cfg.record_to_rrd: @@ -166,25 +131,11 @@ def initialize(self, scene_data: dict[str, Any] | None = None) -> None: raise def step(self, dt: float, state: Any | None = None) -> None: - if not self._is_initialized or self._viewer is None: - logger.warning("[RerunVisualizer] Not initialized. Call initialize() first.") - return - - if self._scene_data_provider: - self._state = self._scene_data_provider.get_newton_state() - else: - try: - from isaaclab.sim._impl.newton_manager import NewtonManager - - self._state = NewtonManager._state_0 - except Exception: - self._state = None - + self._state = self._scene_data_provider.get_newton_state() self._sim_time += dt self._viewer.begin_frame(self._sim_time) - if self._state is not None: - self._viewer.log_state(self._state) + self._viewer.log_state(self._state) self._viewer.end_frame() def close(self) -> None: diff --git a/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py b/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py index c361d3dfe90..4c8f4529162 100644 --- a/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py +++ b/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py @@ -50,14 +50,6 @@ def create_visualizer(self) -> "Visualizer": visualizer_class = get_visualizer_class(self.visualizer_type) if visualizer_class is None: - if self.visualizer_type in ("newton", "rerun"): - raise ImportError( - f"Visualizer '{self.visualizer_type}' requires the Newton Python module and its dependencies. " - "Install the Newton backend (e.g., newton package/isaaclab_newton) and retry." - ) - raise ValueError( - f"Visualizer type '{self.visualizer_type}' is not registered. " - "Valid types: 'newton', 'rerun', 'omniverse'." - ) + raise ValueError(f"Visualizer type '{self.visualizer_type}' is not available.") return visualizer_class(self) From 8370a815d00df748cd772e8249d05fe87ee1c965 Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Wed, 28 Jan 2026 00:16:37 +0000 Subject: [PATCH 11/14] fix ov visualizer, edit docs --- apps/isaaclab.python.kit | 2 +- .../visualization.rst | 24 +++++++++---------- .../ov_scene_data_provider.py | 24 +++++-------------- 3 files changed, 19 insertions(+), 31 deletions(-) diff --git a/apps/isaaclab.python.kit b/apps/isaaclab.python.kit index de4252f2995..04c996aa98f 100644 --- a/apps/isaaclab.python.kit +++ b/apps/isaaclab.python.kit @@ -38,7 +38,7 @@ keywords = ["experience", "app", "usd"] # Isaac Sim Extra "isaacsim.asset.importer.mjcf" = {} -"isaacsim.asset.importer.urdf" = {version = "2.4.31", exact = true} +"isaacsim.asset.importer.urdf" = {} "omni.physx.bundle" = {} "omni.physx.tensors" = {} "omni.replicator.core" = {} diff --git a/docs/source/experimental-features/newton-physics-integration/visualization.rst b/docs/source/experimental-features/newton-physics-integration/visualization.rst index 661a98cf9fe..b50d3467f83 100644 --- a/docs/source/experimental-features/newton-physics-integration/visualization.rst +++ b/docs/source/experimental-features/newton-physics-integration/visualization.rst @@ -5,7 +5,7 @@ Visualization Isaac Lab offers several lightweight visualizers for real-time simulation inspection and debugging. Unlike renderers that process sensor data, visualizers are meant for fast, interactive feedback. -You can use any visualizer regardless of your chosen physics engine or rendering backend. +You can launch any number of visualizers at once, and they work with any physics engine or rendering backend. Overview @@ -31,7 +31,7 @@ Isaac Lab supports three visualizer backends, each optimized for different use c - Webviewer, time scrubbing, recording export -*The following visualizers are shown training the Isaac-Velocity-Flat-Anymal-D-v0 environment.* +*The following visualizers are shown training Isaac-Velocity-Flat-Anymal-D-v0 with 4096 concurrent environments.* .. figure:: ../../_static/visualizers/ov_viz.jpg :width: 100% @@ -139,8 +139,8 @@ Omniverse Visualizer window_height=720, # Viewport height in pixels # Camera settings - camera_position=(8.0, 8.0, 3.0), # Initial camera position (x, y, z) - camera_target=(0.0, 0.0, 0.0), # Camera look-at target + camera_position=(8.0, 8.0, 3.0), # Initial camera position (x, y, z) + camera_target=(0.0, 0.0, 0.0), # Camera look-at target # Feature toggles enable_markers=True, # Enable visualization markers @@ -195,8 +195,8 @@ Newton Visualizer window_height=1080, # Window height in pixels # Camera settings - camera_position=(8.0, 8.0, 3.0), # Initial camera position (x, y, z) - camera_target=(0.0, 0.0, 0.0), # Camera look-at target + camera_position=(8.0, 8.0, 3.0), # Initial camera position (x, y, z) + camera_target=(0.0, 0.0, 0.0), # Camera look-at target # Performance tuning update_frequency=1, # Update every N frames (1=every frame) @@ -213,9 +213,9 @@ Newton Visualizer enable_wireframe=False, # Enable wireframe mode # Color customization - sky_upper_color=(0.53, 0.81, 0.92), # Sky upper color (RGB [0,1]) - sky_lower_color=(0.18, 0.20, 0.25), # Sky lower color (RGB [0,1]) - light_color=(1.0, 1.0, 1.0), # Directional light color (RGB [0,1]) + sky_upper_color=(0.53, 0.81, 0.92), # Sky upper color (RGB [0,1]) + sky_lower_color=(0.18, 0.20, 0.25), # Sky lower color (RGB [0,1]) + light_color=(1.0, 1.0, 1.0), # Directional light color (RGB [0,1]) ) @@ -241,8 +241,8 @@ Rerun Visualizer web_port=9090, # Port for local web viewer (launched in browser) # Camera settings - camera_position=(8.0, 8.0, 3.0), # Initial camera position (x, y, z) - camera_target=(0.0, 0.0, 0.0), # Camera look-at target + camera_position=(8.0, 8.0, 3.0), # Initial camera position (x, y, z) + camera_target=(0.0, 0.0, 0.0), # Camera look-at target # History settings keep_historical_data=False, # Keep transforms for time scrubbing @@ -260,7 +260,7 @@ To reduce overhead when visualizing large-scale environments, consider: - Using Newton instead of Omniverse or Rerun - Reducing window sizes -- Higher update frequencies +- Lower update frequencies - Pausing visualizers while they are not being used diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py index 3a6f1b57ca9..9bdac3b379a 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py @@ -112,28 +112,15 @@ def _refresh_newton_model_if_needed(self) -> None: return def _build_newton_model_from_usd(self) -> None: - # TODO(mtrepte): add support for fabric cloning try: from newton import ModelBuilder - from isaaclab.sim.utils import find_matching_prim_paths - - num_envs = self._get_num_envs() - - # import ipdb; ipdb.set_trace() - env_prim_paths = find_matching_prim_paths("/World/envs/env_.*", stage=self._stage) - print( - "[SceneDataProvider] Stage env prims before add_usd: "+ - f"num_envs_setting={num_envs}, env_prims={len(env_prim_paths)}" - ) builder = ModelBuilder(up_axis=self._up_axis) builder.add_usd(self._stage) - self._newton_model = builder.finalize(device=self._device) - self._metadata["num_envs"] = num_envs - self._newton_model.num_envs = self._metadata.get("num_envs", 0) self._newton_state = self._newton_model.state() self._rigid_body_paths = list(self._newton_model.body_key) + self._xform_views.clear() self._body_key_index_map = {path: i for i, path in enumerate(self._rigid_body_paths)} self._view_body_index_map = {} @@ -343,7 +330,8 @@ def update(self) -> None: for name, getter in pose_sources: positions, orientations = getter() if positions is not None and orientations is not None: - if positions.reshape(-1, 3).shape[0] == expected_count: + count = positions.reshape(-1, 3).shape[0] + if count == expected_count: source_view = name break if positions is None or orientations is None: @@ -370,9 +358,9 @@ def update(self) -> None: return if positions_wp.shape[0] != expected_count: - logger.debug( - "[SceneDataProvider] Body count mismatch for Newton sync " - f"(poses={positions_wp.shape[0]}, state={expected_count}, source={source_view})." + logger.warning( + f"[SceneDataProvider] Body count mismatch for Newton sync: " + f"poses={positions_wp.shape[0]}, state={expected_count}, source={source_view}" ) return From 0705f33b52f8f08c1dc9e2a5fedd4c06bb6d49df Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Thu, 29 Jan 2026 23:07:51 +0000 Subject: [PATCH 12/14] prepping for review --- source/isaaclab/isaaclab/app/app_launcher.py | 5 +-- .../isaaclab/scene/interactive_scene.py | 35 +++++++++++++++++-- .../sim/scene_data_providers/__init__.py | 1 - .../newton_scene_data_provider.py | 11 +++--- .../ov_scene_data_provider.py | 23 ++---------- .../scene_data_provider.py | 3 +- .../isaaclab/sim/simulation_context.py | 35 +++++++++++-------- .../spawners/materials/visual_materials.py | 6 ++-- .../isaaclab/visualizers/newton_visualizer.py | 7 ++-- .../isaaclab/visualizers/rerun_visualizer.py | 6 ++-- .../isaaclab/visualizers/visualizer_cfg.py | 2 +- 11 files changed, 76 insertions(+), 58 deletions(-) diff --git a/source/isaaclab/isaaclab/app/app_launcher.py b/source/isaaclab/isaaclab/app/app_launcher.py index 1a65c6c81f1..e1cd3e352c6 100644 --- a/source/isaaclab/isaaclab/app/app_launcher.py +++ b/source/isaaclab/isaaclab/app/app_launcher.py @@ -817,10 +817,7 @@ def _resolve_visualizer_settings(self, launcher_args: dict) -> None: if self._headless: self._headless = False launcher_args["headless"] = False - print( - "[INFO][AppLauncher]: Omniverse visualizer requested. " - "Forcing headless=False for GUI." - ) + print("[INFO][AppLauncher]: Omniverse visualizer requested. Forcing headless=False for GUI.") else: if not self._headless and self._livestream not in {1, 2}: self._headless = True diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 7d2c9be845c..ea11e2766e3 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -121,9 +121,6 @@ def __init__(self, cfg: InteractiveSceneCfg): # store inputs self.cfg = cfg - # TODO(mtrepte): rm after fix - self.cfg.clone_in_fabric = False - # initialize scene elements self._terrain = None self._articulations = dict() @@ -174,6 +171,7 @@ def __init__(self, cfg: InteractiveSceneCfg): ), # this won't do anything because we are not replicating physics clone_in_fabric=self.cfg.clone_in_fabric, ) + self._ensure_usd_env_clones(copy_from_source=True) self._default_env_origins = torch.tensor(env_origins, device=self.device, dtype=torch.float32) else: # otherwise, environment origins will be initialized during cloning at the end of environment creation @@ -257,6 +255,7 @@ def clone_environments(self, copy_from_source: bool = False): ), # this automatically filters collisions between environments clone_in_fabric=self.cfg.clone_in_fabric, ) + self._ensure_usd_env_clones(copy_from_source=copy_from_source) # since env_ids is only applicable when replicating physics, we have to fallback to the previous method # to filter collisions if replicate_physics is not enabled @@ -274,6 +273,36 @@ def clone_environments(self, copy_from_source: bool = False): if self._default_env_origins is None: self._default_env_origins = torch.tensor(env_origins, device=self.device, dtype=torch.float32) + def _ensure_usd_env_clones(self, copy_from_source: bool) -> None: + """Ensure USD env prims exist when cloning in fabric.""" + if not self.cfg.clone_in_fabric: + return + if get_isaac_sim_version().major < 5: + return + if not self._should_ensure_usd_env_clones(): + return + + self.cloner.clone( + source_prim_path=self.env_prim_paths[0], + prim_paths=self.env_prim_paths, + replicate_physics=False, + copy_from_source=copy_from_source, + enable_env_ids=False, + clone_in_fabric=False, + ) + + def _should_ensure_usd_env_clones(self) -> bool: + """Check if USD clones are required for current backend/visualizers.""" + sim_cfg = getattr(self.sim, "cfg", None) + if sim_cfg is None: + return True + if sim_cfg.physics_backend != "omni": + return True + + visualizer_types = self.sim.resolve_visualizer_types() + + return bool(visualizer_types) and any(viz_type != "omniverse" for viz_type in visualizer_types) + def filter_collisions(self, global_prim_paths: list[str] | None = None): """Filter environments collisions. diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/__init__.py b/source/isaaclab/isaaclab/sim/scene_data_providers/__init__.py index 79d7bff100c..243ca11b055 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/__init__.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/__init__.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 # Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py index bbeb4794978..43814c48551 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/newton_scene_data_provider.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 # Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # @@ -16,7 +15,7 @@ class NewtonSceneDataProvider: """Scene data provider for Newton Warp physics backend. - + Native (cheap): Newton Model/State from NewtonManager Adapted (future): USD stage (would need Newton→USD sync for OV visualizer) """ @@ -50,6 +49,7 @@ def get_newton_model(self) -> Any | None: """NATIVE: Newton Model from NewtonManager.""" try: from isaaclab.sim._impl.newton_manager import NewtonManager + return NewtonManager._model except Exception: return None @@ -58,13 +58,14 @@ def get_newton_state(self) -> Any | None: """NATIVE: Newton State from NewtonManager.""" try: from isaaclab.sim._impl.newton_manager import NewtonManager + return NewtonManager._state_0 except Exception: return None def get_usd_stage(self) -> None: """UNAVAILABLE: Newton backend doesn't provide USD (future: Newton→USD sync).""" - return None + return def get_metadata(self) -> dict[str, Any]: return dict(self._metadata) @@ -76,6 +77,7 @@ def get_transforms(self) -> dict[str, Any] | None: def get_velocities(self) -> dict[str, Any] | None: try: from isaaclab.sim._impl.newton_manager import NewtonManager + if NewtonManager._state_0 is None: return None return {"body_qd": NewtonManager._state_0.body_qd} @@ -85,6 +87,7 @@ def get_velocities(self) -> dict[str, Any] | None: def get_contacts(self) -> dict[str, Any] | None: try: from isaaclab.sim._impl.newton_manager import NewtonManager + if NewtonManager._contacts is None: return None return {"contacts": NewtonManager._contacts} @@ -93,4 +96,4 @@ def get_contacts(self) -> dict[str, Any] | None: def get_mesh_data(self) -> dict[str, Any] | None: """ADAPTED: Extract mesh data from Newton shapes (future work).""" - return None \ No newline at end of file + return None diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py index 9bdac3b379a..6fc0f80b4cf 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 # Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # @@ -9,7 +8,6 @@ from __future__ import annotations import logging -import os import re from typing import Any @@ -17,13 +15,7 @@ class OVSceneDataProvider: - """Scene data provider for Omni PhysX physics backend. - - Native (cheap): USD stage, PhysX transforms/velocities - Adapted (expensive): Newton Model/State (built from USD, synced each step) - - Performance: Only builds Newton data if Newton/Rerun visualizers are active. - """ + """Scene data provider for Omni PhysX backend.""" def __init__(self, visualizer_cfgs: list[Any] | None, stage, simulation_context) -> None: from isaacsim.core.simulation_manager import SimulationManager @@ -50,7 +42,7 @@ def __init__(self, visualizer_cfgs: list[Any] | None, stage, simulation_context) self._has_rerun_visualizer = True elif viz_type == "omniverse": self._has_ov_visualizer = True - + self._metadata = { "physics_backend": "omni", "num_envs": self._get_num_envs(), @@ -65,7 +57,6 @@ def __init__(self, visualizer_cfgs: list[Any] | None, stage, simulation_context) self._set_body_q_kernel = None self._up_axis = UsdGeom.GetStageUpAxis(self._stage) - # Only build Newton model if Newton/Rerun visualizers need it if self._has_newton_visualizer or self._has_rerun_visualizer: self._build_newton_model_from_usd() self._setup_rigid_body_view() @@ -74,7 +65,6 @@ def __init__(self, visualizer_cfgs: list[Any] | None, stage, simulation_context) logger.info("[OVSceneDataProvider] OV visualizer only - skipping Newton model build") def _get_num_envs(self) -> int: - # TODO(mtrepte): is there a better way to get num_envs? try: import carb @@ -120,7 +110,7 @@ def _build_newton_model_from_usd(self) -> None: self._newton_model = builder.finalize(device=self._device) self._newton_state = self._newton_model.state() self._rigid_body_paths = list(self._newton_model.body_key) - + self._xform_views.clear() self._body_key_index_map = {path: i for i, path in enumerate(self._rigid_body_paths)} self._view_body_index_map = {} @@ -173,7 +163,6 @@ def _setup_articulation_view(self) -> None: self._articulation_view = None def _get_view_world_poses(self, view): - # TODO(mtrepte): this can be revisited & simplifiedafter the function naming gets unified if view is None: return None, None @@ -235,7 +224,6 @@ def _get_view_velocities(self, view): if view is None: return None, None - # Preferred: combined velocities method = getattr(view, "get_velocities", None) if method is not None: try: @@ -247,7 +235,6 @@ def _get_view_velocities(self, view): except Exception: pass - # Fallback: split linear/angular get_linear = getattr(view, "get_linear_velocities", None) get_angular = getattr(view, "get_angular_velocities", None) if get_linear is not None and get_angular is not None: @@ -258,7 +245,6 @@ def _get_view_velocities(self, view): return None, None - def _get_xform_world_poses(self): if not self._rigid_body_paths: return None, None @@ -371,9 +357,6 @@ def update(self) -> None: device=self._device, ) - # Future extensions: - # - Populate velocities into self._newton_state.body_qd - # - Cache mesh/material data for Rerun/renderer integrations except Exception as exc: logger.debug(f"[SceneDataProvider] Failed to sync Omni transforms to Newton state: {exc}") diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py index a5cebf10e48..14708219c2d 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/scene_data_provider.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 # Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). # All rights reserved. # @@ -84,4 +83,4 @@ def get_contacts(self) -> dict[str, Any] | None: def get_mesh_data(self) -> dict[str, Any] | None: if self._provider is None: return None - return self._provider.get_mesh_data() \ No newline at end of file + return self._provider.get_mesh_data() diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 95ba4eff5a7..97fd1d58d1c 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -33,12 +33,12 @@ import isaaclab.sim as sim_utils from isaaclab.utils.logger import configure_logging from isaaclab.utils.version import get_isaac_sim_version +from isaaclab.visualizers import NewtonVisualizerCfg, OVVisualizerCfg, RerunVisualizerCfg, Visualizer from .scene_data_providers import SceneDataProvider from .simulation_cfg import SimulationCfg from .spawners import DomeLightCfg, GroundPlaneCfg from .utils import bind_physics_material -from isaaclab.visualizers import NewtonVisualizerCfg, OVVisualizerCfg, RerunVisualizerCfg, Visualizer # import logger logger = logging.getLogger(__name__) @@ -536,22 +536,27 @@ def _create_default_visualizer_configs(self, requested_visualizers: list[str]) - logger.error(f"[SimulationContext] Failed to create default config for visualizer '{viz_type}': {exc}") return default_configs + def resolve_visualizer_types(self) -> list[str]: + """Resolve visualizer types from config or CLI settings.""" + visualizer_cfgs = self.cfg.visualizer_cfgs + if visualizer_cfgs is None: + requested = self.get_setting("/isaaclab/visualizer") + return [v.strip() for v in requested.split(",") if v.strip()] if requested else [] + + if not isinstance(visualizer_cfgs, list): + visualizer_cfgs = [visualizer_cfgs] + return [cfg.visualizer_type for cfg in visualizer_cfgs if getattr(cfg, "visualizer_type", None)] + def initialize_visualizers(self) -> None: """Initialize visualizers from SimulationCfg.visualizer_cfgs.""" - visualizer_cfgs: list = [] - if self.cfg.visualizer_cfgs is not None: - if isinstance(self.cfg.visualizer_cfgs, list): - visualizer_cfgs = self.cfg.visualizer_cfgs - else: - visualizer_cfgs = [self.cfg.visualizer_cfgs] - - if len(visualizer_cfgs) == 0: - requested_visualizers_str = self.get_setting("/isaaclab/visualizer") - if requested_visualizers_str: - requested_visualizers = [v.strip() for v in requested_visualizers_str.split(",") if v.strip()] - visualizer_cfgs = self._create_default_visualizer_configs(requested_visualizers) - else: + visualizer_cfgs = self.cfg.visualizer_cfgs + if visualizer_cfgs is None: + requested_visualizers = self.resolve_visualizer_types() + if not requested_visualizers: return + visualizer_cfgs = self._create_default_visualizer_configs(requested_visualizers) + elif not isinstance(visualizer_cfgs, list): + visualizer_cfgs = [visualizer_cfgs] self._scene_data_provider = SceneDataProvider( backend=self.cfg.physics_backend, @@ -564,7 +569,7 @@ def initialize_visualizers(self) -> None: try: visualizer = viz_cfg.create_visualizer() scene_data: dict[str, Any] = {"scene_data_provider": self._scene_data_provider} - + # OV visualizer gets USD stage if viz_cfg.visualizer_type == "omniverse": if self._scene_data_provider: diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py index 6b87c99ea65..27cc41c7725 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py @@ -62,16 +62,16 @@ def spawn_preview_surface(prim_path: str, cfg: visual_materials_cfg.PreviewSurfa material_prim = UsdShade.Material.Define(stage, prim_path) if not material_prim: raise ValueError(f"Failed to create preview surface shader at path: '{prim_path}'.") - + shader_prim = CreateShaderPrimFromSdrCommand( parent_path=prim_path, identifier="UsdPreviewSurface", stage_or_context=stage, ).do() - + if not shader_prim: raise ValueError(f"Failed to create shader prim at path: '{prim_path}'.") - + # The command returns a Shader object directly, not a path if shader_prim: surface_out = shader_prim.GetOutput("surface") diff --git a/source/isaaclab/isaaclab/visualizers/newton_visualizer.py b/source/isaaclab/isaaclab/visualizers/newton_visualizer.py index 39c9eddf82d..a2c50b8d640 100644 --- a/source/isaaclab/isaaclab/visualizers/newton_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/newton_visualizer.py @@ -8,15 +8,18 @@ from __future__ import annotations import contextlib -import numpy as np +import logging from typing import Any +import numpy as np import warp as wp from newton.viewer import ViewerGL from .newton_visualizer_cfg import NewtonVisualizerCfg from .visualizer import Visualizer +logger = logging.getLogger(__name__) + class NewtonViewerGL(ViewerGL): """Wrapper around Newton's ViewerGL with training/rendering pause controls.""" @@ -264,7 +267,7 @@ def step(self, dt: float, state: Any | None = None) -> None: self._step_counter += 1 self._state = self._scene_data_provider.get_newton_state() - + contacts = None if self._viewer.show_contacts: contacts_data = self._scene_data_provider.get_contacts() diff --git a/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py b/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py index 00d2623001e..d73e313cfdd 100644 --- a/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/rerun_visualizer.py @@ -10,13 +10,13 @@ import logging from typing import Any -from .rerun_visualizer_cfg import RerunVisualizerCfg -from .visualizer import Visualizer - import rerun as rr import rerun.blueprint as rrb from newton.viewer import ViewerRerun +from .rerun_visualizer_cfg import RerunVisualizerCfg +from .visualizer import Visualizer + logger = logging.getLogger(__name__) diff --git a/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py b/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py index 4c8f4529162..d86c10504f3 100644 --- a/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py +++ b/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py @@ -38,7 +38,7 @@ def get_visualizer_type(self) -> str | None: """Get the visualizer type identifier.""" return self.visualizer_type - def create_visualizer(self) -> "Visualizer": + def create_visualizer(self) -> Visualizer: """Create visualizer instance from this config using factory pattern.""" from . import get_visualizer_class From b6c91a9d7719ca5136512ffd7527cabab61c0cd4 Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Fri, 30 Jan 2026 19:23:50 +0000 Subject: [PATCH 13/14] sanatize ov camera path --- source/isaaclab/isaaclab/visualizers/ov_visualizer.py | 4 +++- .../benchmarking/configs_custom_reward_thresholds.yaml | 9 +++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) create mode 100644 source/isaaclab_tasks/test/benchmarking/configs_custom_reward_thresholds.yaml diff --git a/source/isaaclab/isaaclab/visualizers/ov_visualizer.py b/source/isaaclab/isaaclab/visualizers/ov_visualizer.py index 33596629561..d3e2c8ea822 100644 --- a/source/isaaclab/isaaclab/visualizers/ov_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/ov_visualizer.py @@ -178,7 +178,9 @@ async def _dock_viewport_async(self, viewport_name: str, dock_position) -> None: viewport_window.focus() def _create_and_assign_camera(self, usd_stage) -> None: - camera_path = f"/World/Cameras/{self.cfg.viewport_name}_Camera" + # Create camera prim path based on viewport name (sanitize to enure valid USD path) 1 + camera_path = f"/World/Cameras/{self.cfg.viewport_name}_Camera".replace(" ", "_") + camera_prim = usd_stage.GetPrimAtPath(camera_path) if not camera_prim.IsValid(): UsdGeom.Camera.Define(usd_stage, camera_path) diff --git a/source/isaaclab_tasks/test/benchmarking/configs_custom_reward_thresholds.yaml b/source/isaaclab_tasks/test/benchmarking/configs_custom_reward_thresholds.yaml new file mode 100644 index 00000000000..cb899cd591b --- /dev/null +++ b/source/isaaclab_tasks/test/benchmarking/configs_custom_reward_thresholds.yaml @@ -0,0 +1,9 @@ +custom_rewards: + rl_games:Isaac-Dexsuite-Kuka-Allegro-Lift-v0: + max_iterations: 2000 + upper_thresholds: + duration: 999999 + sb3:Isaac-Lift-Cube-Franka-v0: + max_iterations: 200 + upper_thresholds: + duration: 999999 From 5dadc37da74c03eefe98b244c122be8b6ca55244 Mon Sep 17 00:00:00 2001 From: Matthew Trepte Date: Tue, 3 Feb 2026 01:11:24 +0000 Subject: [PATCH 14/14] refactor scene data provider, unify apis --- .../ov_scene_data_provider.py | 283 ++++++++++-------- 1 file changed, 166 insertions(+), 117 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py index 6fc0f80b4cf..100ff7f0b10 100644 --- a/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py +++ b/source/isaaclab/isaaclab/sim/scene_data_providers/ov_scene_data_provider.py @@ -30,6 +30,7 @@ def __init__(self, visualizer_cfgs: list[Any] | None, stage, simulation_context) self._body_key_index_map: dict[str, int] = {} self._view_body_index_map: dict[str, list[int]] = {} + # Determine which visualizers need Newton state sync self._has_newton_visualizer = False self._has_rerun_visualizer = False self._has_ov_visualizer = False @@ -43,6 +44,9 @@ def __init__(self, visualizer_cfgs: list[Any] | None, stage, simulation_context) elif viz_type == "omniverse": self._has_ov_visualizer = True + # Explicit mode flag for Newton synchronization + self._needs_newton_sync = self._has_newton_visualizer or self._has_rerun_visualizer + self._metadata = { "physics_backend": "omni", "num_envs": self._get_num_envs(), @@ -54,14 +58,16 @@ def __init__(self, visualizer_cfgs: list[Any] | None, stage, simulation_context) self._newton_model = None self._newton_state = None self._rigid_body_paths: list[str] = [] + self._articulation_paths: list[str] = [] self._set_body_q_kernel = None self._up_axis = UsdGeom.GetStageUpAxis(self._stage) - if self._has_newton_visualizer or self._has_rerun_visualizer: + # Initialize Newton pipeline only if needed for visualization + if self._needs_newton_sync: self._build_newton_model_from_usd() self._setup_rigid_body_view() self._setup_articulation_view() - elif self._has_ov_visualizer: + else: logger.info("[OVSceneDataProvider] OV visualizer only - skipping Newton model build") def _get_num_envs(self) -> int: @@ -102,6 +108,7 @@ def _refresh_newton_model_if_needed(self) -> None: return def _build_newton_model_from_usd(self) -> None: + """Build Newton model from USD and extract scene structure.""" try: from newton import ModelBuilder @@ -109,7 +116,10 @@ def _build_newton_model_from_usd(self) -> None: builder.add_usd(self._stage) self._newton_model = builder.finalize(device=self._device) self._newton_state = self._newton_model.state() + + # Extract scene structure from Newton model (single source of truth) self._rigid_body_paths = list(self._newton_model.body_key) + self._articulation_paths = list(self._newton_model.articulation_key) self._xform_views.clear() self._body_key_index_map = {path: i for i, path in enumerate(self._rigid_body_paths)} @@ -125,12 +135,18 @@ def _build_newton_model_from_usd(self) -> None: self._newton_model = None self._newton_state = None self._rigid_body_paths = [] + self._articulation_paths = [] def _setup_rigid_body_view(self) -> None: + """Create PhysX RigidBodyView from Newton's body paths. + + Uses body paths extracted from Newton model to create PhysX tensor API view + for reading rigid body transforms. + """ if not self._rigid_body_paths: return try: - paths_to_use = self._wildcard_env_paths(list(self._rigid_body_paths)) + paths_to_use = self._wildcard_env_paths(self._rigid_body_paths) self._rigid_body_view = self._physics_sim_view.create_rigid_body_view(paths_to_use) self._cache_view_index_map(self._rigid_body_view, "rigid_body_view") except Exception as exc: @@ -138,21 +154,11 @@ def _setup_rigid_body_view(self) -> None: self._rigid_body_view = None def _setup_articulation_view(self) -> None: + """Create PhysX ArticulationView from Newton's articulation paths.""" + if not self._articulation_paths: + return try: - from pxr import UsdPhysics - - from isaaclab.sim.utils import get_all_matching_child_prims - - root_prims = get_all_matching_child_prims( - "/World", - predicate=lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI), - stage=self._stage, - traverse_instance_prims=True, - ) - if not root_prims: - return - - paths_to_use = self._wildcard_env_paths([prim.GetPath().pathString for prim in root_prims]) + paths_to_use = self._wildcard_env_paths(self._articulation_paths) exprs = [path.replace(".*", "*") for path in paths_to_use] self._articulation_view = self._physics_sim_view.create_articulation_view( exprs if len(exprs) > 1 else exprs[0] @@ -163,15 +169,15 @@ def _setup_articulation_view(self) -> None: self._articulation_view = None def _get_view_world_poses(self, view): + """Read world poses from PhysX tensor API view (ArticulationView or RigidBodyView). + + Tries multiple method names for compatibility across PhysX API versions. + Returns (positions, orientations) tuple or (None, None) if unavailable. + """ if view is None: return None, None - method_names = ( - "get_world_poses", - "get_world_transforms", - "get_transforms", - "get_poses", - ) + method_names = ("get_world_poses", "get_world_transforms", "get_transforms", "get_poses") for name in method_names: method = getattr(view, name, None) @@ -182,9 +188,11 @@ def _get_view_world_poses(self, view): except Exception: continue + # Handle tuple return: (positions, orientations) if isinstance(result, tuple) and len(result) == 2: return result + # Handle packed array: [..., 7] -> split into pos and quat try: if hasattr(result, "shape") and result.shape[-1] == 7: positions = result[..., :3] @@ -196,25 +204,29 @@ def _get_view_world_poses(self, view): return None, None def _cache_view_index_map(self, view, key: str) -> None: + """Map PhysX view indices to Newton body_key ordering.""" prim_paths = getattr(view, "prim_paths", None) if not prim_paths or not self._rigid_body_paths: return def split_env(path: str) -> tuple[int | None, str]: + """Extract environment ID and relative path from prim path.""" match = re.search(r"/World/envs/env_(\d+)(/.*)", path) return (int(match.group(1)), match.group(2)) if match else (None, path) + # Build map: (env_id, relative_path) -> view_index view_map: dict[tuple[int | None, str], int] = {} for view_idx, path in enumerate(prim_paths): env_id, rel = split_env(path) view_map[(env_id, rel)] = view_idx + # Build reordering: newton_body_index -> view_index order: list[int | None] = [None] * len(self._rigid_body_paths) for body_idx, path in enumerate(self._rigid_body_paths): env_id, rel = split_env(path) view_idx = view_map.get((env_id, rel)) if view_idx is None: - view_idx = view_map.get((None, rel)) + view_idx = view_map.get((None, rel)) # Try without env_id order[body_idx] = view_idx if all(idx is not None for idx in order): @@ -245,30 +257,116 @@ def _get_view_velocities(self, view): return None, None - def _get_xform_world_poses(self): - if not self._rigid_body_paths: - return None, None - try: - import torch - - from isaaclab.sim.views import XformPrimView - - positions = [] - orientations = [] - for path in self._rigid_body_paths: - view = self._xform_views.get(path) - if view is None: - view = XformPrimView(path, device=self._device, stage=self._stage, validate_xform_ops=False) - self._xform_views[path] = view - pos, quat = view.get_world_poses() - positions.append(pos) - orientations.append(quat) - return (torch.cat(positions, dim=0), torch.cat(orientations, dim=0)) if positions else (None, None) - except Exception as exc: - logger.debug(f"[SceneDataProvider] Failed to read XformPrimView poses: {exc}") - return None, None + def _apply_view_poses(self, view: Any, view_key: str, positions: Any, orientations: Any, covered: Any) -> int: + """Read poses from a PhysX view and write uncovered bodies to output tensors.""" + import torch + + if view is None: + return 0 + + pos, quat = self._get_view_world_poses(view) + if pos is None or quat is None: + return 0 + + order = self._view_body_index_map.get(view_key) + if not order: + return 0 + + pos = pos.to(device=self._device, dtype=torch.float32) + quat = quat.to(device=self._device, dtype=torch.float32) + + count = 0 + for newton_idx, view_idx in enumerate(order): + if view_idx is not None and not covered[newton_idx]: + positions[newton_idx] = pos[view_idx] + orientations[newton_idx] = quat[view_idx] + covered[newton_idx] = True + count += 1 + + return count + + def _apply_xform_poses(self, positions: Any, orientations: Any, covered: Any, xform_mask: Any) -> int: + """Use XformPrimView fallback for remaining uncovered bodies.""" + import torch + + from isaaclab.sim.views import XformPrimView + + uncovered = torch.where(~covered)[0].cpu().tolist() + if not uncovered: + return 0 + + count = 0 + for idx in uncovered: + path = self._rigid_body_paths[idx] + try: + if path not in self._xform_views: + self._xform_views[path] = XformPrimView( + path, device=self._device, stage=self._stage, validate_xform_ops=False + ) + + pos, quat = self._xform_views[path].get_world_poses() + if pos is not None and quat is not None: + positions[idx] = pos.to(device=self._device, dtype=torch.float32).squeeze() + orientations[idx] = quat.to(device=self._device, dtype=torch.float32).squeeze() + covered[idx] = True + xform_mask[idx] = True + count += 1 + except Exception: + continue + + return count + + def _convert_xform_quats(self, orientations: Any, xform_mask: Any) -> Any: + """Convert XformPrimView quaternions from wxyz to xyzw where needed.""" + if not xform_mask.any(): + return orientations + + import torch + + from isaaclab.utils.math import convert_quat + + orientations_xyzw = orientations.clone() + xform_indices = torch.where(xform_mask)[0] + if len(xform_indices) > 0: + orientations_xyzw[xform_indices] = convert_quat(orientations[xform_indices], to="xyzw") + return orientations_xyzw + + def _read_poses_from_best_source(self) -> tuple[Any, Any, str, Any] | None: + """Merge pose data from ArticulationView, RigidBodyView, and XformPrimView.""" + if self._newton_state is None or not self._rigid_body_paths: + return None + + import torch + + num_bodies = len(self._rigid_body_paths) + if num_bodies != self._newton_state.body_q.shape[0]: + logger.warning(f"Body count mismatch: body_key={num_bodies}, state={self._newton_state.body_q.shape[0]}") + return None + + positions = torch.zeros((num_bodies, 3), dtype=torch.float32, device=self._device) + orientations = torch.zeros((num_bodies, 4), dtype=torch.float32, device=self._device) + covered = torch.zeros(num_bodies, dtype=torch.bool, device=self._device) + xform_mask = torch.zeros(num_bodies, dtype=torch.bool, device=self._device) + + artic = self._apply_view_poses(self._articulation_view, "articulation_view", positions, orientations, covered) + rigid = self._apply_view_poses(self._rigid_body_view, "rigid_body_view", positions, orientations, covered) + xform = self._apply_xform_poses(positions, orientations, covered, xform_mask) + + if not covered.all(): + logger.warning(f"Failed to read {(~covered).sum().item()}/{num_bodies} body poses") + return None + + active = sum([artic > 0, rigid > 0, xform > 0]) + source = ( + "merged" + if active > 1 + else ("articulation_view" if artic else "rigid_body_view" if rigid else "xform_view" if xform else "none") + ) + + return positions, orientations, source, xform_mask def _get_set_body_q_kernel(self): + """Get or create the Warp kernel for writing transforms to Newton state.""" if self._set_body_q_kernel is not None: return self._set_body_q_kernel try: @@ -290,64 +388,27 @@ def _set_body_q( return None def update(self) -> None: - """Sync PhysX transforms to Newton state if Newton/Rerun visualizers are active.""" - if not (self._has_newton_visualizer or self._has_rerun_visualizer): - return # OV visualizer only - USD auto-synced by omni.physics - self._refresh_newton_model_if_needed() - if self._newton_state is None: - return - if self._rigid_body_view is None and self._articulation_view is None and not self._rigid_body_paths: + """Sync PhysX transforms to Newton state for visualization.""" + if not self._needs_newton_sync or self._newton_state is None: return + self._refresh_newton_model_if_needed() + try: - import torch import warp as wp - from isaaclab.utils.math import convert_quat - - expected_count = self._newton_state.body_q.shape[0] - pose_sources = ( - ("articulation_view", lambda: self._get_view_world_poses(self._articulation_view)), - ("rigid_body_view", lambda: self._get_view_world_poses(self._rigid_body_view)), - ("xform_view", self._get_xform_world_poses), - ) - positions = orientations = None - source_view = "none" - for name, getter in pose_sources: - positions, orientations = getter() - if positions is not None and orientations is not None: - count = positions.reshape(-1, 3).shape[0] - if count == expected_count: - source_view = name - break - if positions is None or orientations is None: + result = self._read_poses_from_best_source() + if result is None: return - order = self._view_body_index_map.get(source_view) - if order: - positions = positions[order] - orientations = orientations[order] - - positions = positions.reshape(-1, 3).to(dtype=torch.float32, device=self._device) - orientations = orientations.reshape(-1, 4).to(dtype=torch.float32, device=self._device) - # NOTE: PhysX tensor views return quats in xyzw, while XformPrimView returns wxyz. - # Convert only when needed to avoid scrambling orientations. - if source_view == "xform_view": - orientations_xyzw = convert_quat(orientations, to="xyzw") - else: - orientations_xyzw = orientations - - positions_wp = wp.from_torch(positions, dtype=wp.vec3) + + positions, orientations, _, xform_mask = result + orientations_xyzw = self._convert_xform_quats(orientations.reshape(-1, 4), xform_mask) + + positions_wp = wp.from_torch(positions.reshape(-1, 3), dtype=wp.vec3) orientations_wp = wp.from_torch(orientations_xyzw, dtype=wp.quatf) set_body_q = self._get_set_body_q_kernel() - if set_body_q is None: - return - - if positions_wp.shape[0] != expected_count: - logger.warning( - f"[SceneDataProvider] Body count mismatch for Newton sync: " - f"poses={positions_wp.shape[0]}, state={expected_count}, source={source_view}" - ) + if set_body_q is None or positions_wp.shape[0] != self._newton_state.body_q.shape[0]: return wp.launch( @@ -358,44 +419,32 @@ def update(self) -> None: ) except Exception as exc: - logger.debug(f"[SceneDataProvider] Failed to sync Omni transforms to Newton state: {exc}") + logger.debug(f"Failed to sync transforms to Newton: {exc}") def get_newton_model(self) -> Any | None: - """ADAPTED: Newton Model built from USD (only if Newton/Rerun visualizers active).""" - if not (self._has_newton_visualizer or self._has_rerun_visualizer): - return None - return self._newton_model + return self._newton_model if self._needs_newton_sync else None def get_newton_state(self) -> Any | None: - """ADAPTED: Newton State synced from PhysX (only if Newton/Rerun visualizers active).""" - if not (self._has_newton_visualizer or self._has_rerun_visualizer): - return None - return self._newton_state + return self._newton_state if self._needs_newton_sync else None def get_usd_stage(self) -> Any: - """NATIVE: USD stage.""" return self._stage def get_mesh_data(self) -> dict[str, Any] | None: - """NATIVE: Extract mesh data from USD stage (future work).""" return None def get_metadata(self) -> dict[str, Any]: return dict(self._metadata) def get_transforms(self) -> dict[str, Any] | None: - if self._rigid_body_view is None and self._articulation_view is None: - return None try: - for getter in ( - lambda: self._get_view_world_poses(self._articulation_view), - lambda: self._get_view_world_poses(self._rigid_body_view), - self._get_xform_world_poses, - ): - positions, orientations = getter() - if positions is not None and orientations is not None: - return {"positions": positions, "orientations": orientations} - return None + result = self._read_poses_from_best_source() + if result is None: + return None + + positions, orientations, _, xform_mask = result + orientations_xyzw = self._convert_xform_quats(orientations, xform_mask) + return {"positions": positions, "orientations": orientations_xyzw} except Exception: return None