diff --git a/docs/source/features/visualization.rst b/docs/source/features/visualization.rst index 13d573308efb..3f60dfee485a 100644 --- a/docs/source/features/visualization.rst +++ b/docs/source/features/visualization.rst @@ -109,12 +109,12 @@ You can also configure custom visualizers in the code by defining ``VisualizerCf dock_position="SAME", window_width=1280, window_height=720, - camera_position=(0.0, 0.0, 20.0), # high top down view - camera_target=(0.0, 0.0, 0.0), + eye=(0.0, 0.0, 20.0), # high top down view + lookat=(0.0, 0.0, 0.0), ), NewtonVisualizerCfg( - camera_position=(5.0, 5.0, 5.0), # closer quarter view - camera_target=(0.0, 0.0, 0.0), + eye=(5.0, 5.0, 5.0), # closer quarter view + lookat=(0.0, 0.0, 0.0), show_joints=True, ), RerunVisualizerCfg( @@ -201,8 +201,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 + eye=(8.0, 8.0, 3.0), # Initial camera position (x, y, z) + lookat=(0.0, 0.0, 0.0), # Camera look-at target # Feature toggles enable_markers=True, # Enable visualization markers @@ -255,8 +255,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 + eye=(8.0, 8.0, 3.0), # Initial camera position (x, y, z) + lookat=(0.0, 0.0, 0.0), # Camera look-at target # Performance tuning update_frequency=1, # Update every N frames (1=every frame) @@ -303,8 +303,8 @@ Rerun Visualizer bind_address="0.0.0.0", # Endpoint host formatting/reuse checks # 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 + eye=(8.0, 8.0, 3.0), # Initial camera position (x, y, z) + lookat=(0.0, 0.0, 0.0), # Camera look-at target # History settings keep_historical_data=False, # Keep transforms for time scrubbing diff --git a/source/isaaclab/isaaclab/app/app_launcher.py b/source/isaaclab/isaaclab/app/app_launcher.py index 597714267c20..385c87d4a23c 100644 --- a/source/isaaclab/isaaclab/app/app_launcher.py +++ b/source/isaaclab/isaaclab/app/app_launcher.py @@ -188,6 +188,8 @@ def __init__(self, launcher_args: argparse.Namespace | dict | None = None, **kwa self._offscreen_render: bool # 0: Disabled, 1: Enabled self._sim_experience_file: str # Experience file to load self._visualizer_max_worlds: int | None # Optional max worlds override for Newton-based visualizers + self._video_enabled: bool # Whether --video recording is enabled + self._video_auto_start_kit: bool # Whether headless video should auto-inject Kit visualizer # Exposed to train scripts self.device_id: int # device ID for GPU simulation (defaults to 0) @@ -842,14 +844,28 @@ def _resolve_xr_settings(self, launcher_args: dict): def _resolve_viewport_settings(self, launcher_args: dict): """Resolve viewport related settings.""" + self._video_enabled = bool(launcher_args.get("video", False)) # Check if we can disable the viewport to improve performance # This should only happen if we are running headless and do not require livestreaming or video recording # This is different from offscreen_render because this only affects the default viewport and # not other render-products in the scene self._render_viewport = True - if self._headless and not self._livestream and not launcher_args.get("video", False): + if self._headless and not self._livestream and not self._video_enabled: self._render_viewport = False + # Auto-start a headless Kit visualizer when recording video without an explicit + # visualizer selection. This ensures app.update() is pumped and camera updates + # can be forwarded from viewer settings. + has_explicit_kit = self._cli_visualizer_explicit and "kit" in set(self._cli_visualizer_types) + self._video_auto_start_kit = ( + self._video_enabled + and self._headless + and not self._livestream + and not self._xr + and not self._cli_visualizer_disable_all + and not has_explicit_kit + ) + # hide_ui flag launcher_args["hide_ui"] = False if self._headless and not self._livestream: @@ -1069,6 +1085,9 @@ def _load_extensions(self): # (no Kit GUI) the AR profile must be enabled programmatically so that # the OpenXR session starts without user interaction settings.set_bool("/isaaclab/xr/auto_start", self._headless and self._xr) + # set setting to indicate video recording mode and optional Kit auto-start + settings.set_bool("/isaaclab/video/enabled", self._video_enabled) + settings.set_bool("/isaaclab/video/auto_start_kit", self._video_auto_start_kit) # set setting to indicate no RTX sensors are used (set to True when RTX sensor is created) settings.set_bool("/isaaclab/render/rtx_sensors", False) diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env.py b/source/isaaclab/isaaclab/envs/direct_marl_env.py index eb0a359e4f5b..6c237d3bb974 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env.py @@ -151,8 +151,8 @@ def _init_sim(self, render_mode: str | None = None, **kwargs): # FIXME: This needs to be fixed in the future when we unify the UI functionalities even for # non-rendering modes. # Initialize when GUI is available OR when visualizers are active (headless rendering) - # Visualizers support camera updates via sim.set_camera_view() which forwards to all active visualizers - has_visualizers = bool(self.sim.get_setting("/isaaclab/visualizer")) + # ViewerCfg camera updates are applied through renderer camera control. + has_visualizers = self.sim.has_active_visualizers() if self.sim.has_gui or has_visualizers: self.viewport_camera_controller = ViewportCameraController(self, self.cfg.viewer) else: @@ -522,7 +522,7 @@ def render(self, recompute: bool = False) -> np.ndarray | None: return None elif self.render_mode == "rgb_array": # check that if any render could have happened - if not self.sim.has_gui and not self.sim.has_offscreen_render: + if not self.sim.can_render_rgb_array(): raise RuntimeError( f"Cannot render '{self.render_mode}' - no GUI and offscreen rendering not enabled." " If running headless, make sure --enable_cameras is set." @@ -531,10 +531,9 @@ def render(self, recompute: bool = False) -> np.ndarray | None: if not hasattr(self, "_rgb_annotator"): import omni.replicator.core as rep + camera_prim_path = self.cfg.viewer.cam_prim_path # create render product - self._render_product = rep.create.render_product( - self.cfg.viewer.cam_prim_path, self.cfg.viewer.resolution - ) + self._render_product = rep.create.render_product(camera_prim_path, self.cfg.viewer.resolution) # create rgb annotator -- used to read data from the render product self._rgb_annotator = rep.AnnotatorRegistry.get_annotator("rgb", device="cpu") self._rgb_annotator.attach([self._render_product]) diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index b362ac72bc21..f4ac159802bd 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -156,8 +156,8 @@ def _init_sim(self, render_mode: str | None = None, **kwargs): # FIXME: This needs to be fixed in the future when we unify the UI functionalities even for # non-rendering modes. # Initialize when GUI is available OR when visualizers are active (headless rendering) - # Visualizers support camera updates via sim.set_camera_view() which forwards to all active visualizers - has_visualizers = bool(self.sim.get_setting("/isaaclab/visualizer")) + # ViewerCfg camera updates are applied through renderer camera control. + has_visualizers = self.sim.has_active_visualizers() if self.sim.has_gui or has_visualizers: self.viewport_camera_controller = ViewportCameraController(self, self.cfg.viewer) else: @@ -490,7 +490,7 @@ def render(self, recompute: bool = False) -> np.ndarray | None: return None elif self.render_mode == "rgb_array": # check that if any render could have happened - if not self.sim.has_gui and not self.sim.has_offscreen_render: + if not self.sim.can_render_rgb_array(): raise RuntimeError( f"Cannot render '{self.render_mode}' - no GUI and offscreen rendering not enabled." " If running headless, make sure --enable_cameras is set." @@ -499,10 +499,9 @@ def render(self, recompute: bool = False) -> np.ndarray | None: if not hasattr(self, "_rgb_annotator"): import omni.replicator.core as rep + camera_prim_path = self.cfg.viewer.cam_prim_path # create render product - self._render_product = rep.create.render_product( - self.cfg.viewer.cam_prim_path, self.cfg.viewer.resolution - ) + self._render_product = rep.create.render_product(camera_prim_path, self.cfg.viewer.resolution) # create rgb annotator -- used to read data from the render product self._rgb_annotator = rep.AnnotatorRegistry.get_annotator("rgb", device="cpu") self._rgb_annotator.attach([self._render_product]) diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 996e88216e17..51fe7c329c8a 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -166,8 +166,8 @@ def _init_sim(self): # FIXME: This needs to be fixed in the future when we unify the UI functionalities even for # non-rendering modes. # Initialize when GUI is available OR when visualizers are active (headless rendering) - # Visualizers support camera updates via sim.set_camera_view() which forwards to all active visualizers - has_visualizers = bool(self.sim.get_setting("/isaaclab/visualizer")) + # ViewerCfg camera updates are applied through renderer camera control. + has_visualizers = self.sim.has_active_visualizers() if self.sim.has_gui or has_visualizers: self.viewport_camera_controller = ViewportCameraController(self, self.cfg.viewer) else: diff --git a/source/isaaclab/isaaclab/envs/manager_based_rl_env.py b/source/isaaclab/isaaclab/envs/manager_based_rl_env.py index d08b7e3be3a7..37698d0d25eb 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_rl_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_rl_env.py @@ -6,6 +6,7 @@ # needed to import for allowing type-hinting: np.ndarray | None from __future__ import annotations +import logging import math from collections.abc import Sequence from typing import Any, ClassVar @@ -21,6 +22,8 @@ from .manager_based_env import ManagerBasedEnv from .manager_based_rl_env_cfg import ManagerBasedRLEnvCfg +logger = logging.getLogger(__name__) + class ManagerBasedRLEnv(ManagerBasedEnv, gym.Env): """The superclass for the manager-based workflow reinforcement learning-based environments. @@ -272,8 +275,7 @@ def render(self, recompute: bool = False) -> np.ndarray | None: elif self.render_mode == "rgb_array": # check that if any render could have happened # Check for GUI, offscreen rendering, or visualizers - has_visualizers = bool(self.sim.get_setting("/isaaclab/visualizer")) - if not (self.sim.has_gui or self.sim.has_offscreen_render or has_visualizers): + if not self.sim.can_render_rgb_array(): raise RuntimeError( f"Cannot render '{self.render_mode}' - no GUI and offscreen rendering not enabled." " If running headless, make sure --enable_cameras is set." @@ -282,10 +284,9 @@ def render(self, recompute: bool = False) -> np.ndarray | None: if not hasattr(self, "_rgb_annotator"): import omni.replicator.core as rep + camera_prim_path = self.cfg.viewer.cam_prim_path # create render product - self._render_product = rep.create.render_product( - self.cfg.viewer.cam_prim_path, self.cfg.viewer.resolution - ) + self._render_product = rep.create.render_product(camera_prim_path, self.cfg.viewer.resolution) # create rgb annotator -- used to read data from the render product self._rgb_annotator = rep.AnnotatorRegistry.get_annotator("rgb", device="cpu") self._rgb_annotator.attach([self._render_product]) diff --git a/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py b/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py index 4126d7b74735..6a89c482accc 100644 --- a/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py +++ b/source/isaaclab/isaaclab/envs/ui/viewport_camera_controller.py @@ -218,8 +218,8 @@ def update_view_location(self, eye: Sequence[float] | None = None, lookat: Seque cam_eye = viewer_origin + self.default_cam_eye cam_target = viewer_origin + self.default_cam_lookat - # set the camera view - self._env.sim.set_camera_view(eye=cam_eye, target=cam_target) + # set the renderer viewport camera view (does not broadcast to visualizers) + self._env.sim.set_renderer_camera_view(eye=cam_eye, target=cam_target, camera_prim_path=self.cfg.cam_prim_path) """ Private Functions diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 91cee7d0be46..13470e25cc2f 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -180,7 +180,9 @@ def __init__(self, cfg: SimulationCfg | None = None): self._has_gui = bool(self.get_setting("/isaaclab/has_gui")) self._has_offscreen_render = bool(self.get_setting("/isaaclab/render/offscreen")) self._xr_enabled = bool(self.get_setting("/isaaclab/xr/enabled")) + self._video_auto_start_kit = bool(self.get_setting("/isaaclab/video/auto_start_kit")) # Note: has_rtx_sensors is NOT cached because it changes when Camera sensors are created + self._pending_camera_view: tuple[tuple[float, float, float], tuple[float, float, float]] | None = None # Simulation state self._is_playing = False @@ -188,6 +190,10 @@ def __init__(self, cfg: SimulationCfg | None = None): # Monotonic physics-step counter used by camera sensors for self._physics_step_count: int = 0 + # Monotonic render-generation counter. This increments whenever render() + # is executed and lets downstream camera freshness logic distinguish + # render/reset transitions that occur without advancing physics steps. + self._render_generation: int = 0 type(self)._instance = self # Mark as valid singleton only after successful init @@ -337,6 +343,16 @@ def has_offscreen_render(self) -> bool: """Returns whether offscreen rendering is enabled (cached at init).""" return self._has_offscreen_render + def has_active_visualizers(self) -> bool: + """Return whether any visualizer path is active for rendering/camera control.""" + return bool(self.get_setting("/isaaclab/visualizer/types")) or bool( + self.get_setting("/isaaclab/video/auto_start_kit") + ) + + def can_render_rgb_array(self) -> bool: + """Return whether rgb-array rendering is currently available.""" + return self.has_gui or self.has_offscreen_render or self.has_active_visualizers() + @property def is_rendering(self) -> bool: """Returns whether rendering is active (GUI, RTX sensors, visualizers, or XR).""" @@ -352,6 +368,11 @@ def get_physics_dt(self) -> float: """Returns the physics time step.""" return self.physics_manager.get_physics_dt() + @property + def render_generation(self) -> int: + """Returns a monotonic counter for render() executions.""" + return self._render_generation + def _create_default_visualizer_configs(self, requested_visualizers: list[str]) -> list: """Create default visualizer configs for requested types. @@ -534,6 +555,26 @@ def _resolve_visualizer_cfgs(self) -> list[Any]: exc, ) + # Headless video auto-start: inject a KitVisualizer when needed so that + # app.update() is pumped and viewer camera updates can be applied in + # rgb-array recording flows. + if self._video_auto_start_kit and not cli_disable_all: + has_kit = any(getattr(cfg, "visualizer_type", None) == "kit" for cfg in resolved) + if not has_kit: + try: + import importlib + + mod = importlib.import_module("isaaclab_visualizers.kit") + kit_cfg_cls = getattr(mod, "KitVisualizerCfg") + resolved.append(kit_cfg_cls(headless=True)) + logger.info("[SimulationContext] Auto-injecting KitVisualizer for headless video recording.") + except (ImportError, ModuleNotFoundError, AttributeError, TypeError) as exc: + logger.warning( + "[SimulationContext] Headless video could not auto-inject a KitVisualizer: %s. " + "Install isaaclab_visualizers[kit] or pass --visualizer kit.", + exc, + ) + return resolved def initialize_visualizers(self) -> None: @@ -577,6 +618,12 @@ def initialize_visualizers(self) -> None: exc, ) + # Replay any camera pose requested before visualizers were initialized. + if self._pending_camera_view is not None: + eye, target = self._pending_camera_view + for viz in self._visualizers: + viz.set_camera_view(eye, target) + if not self._visualizers and self._scene_data_provider is not None: close_provider = getattr(self._scene_data_provider, "close", None) if callable(close_provider): @@ -627,9 +674,29 @@ def get_rendering_dt(self) -> float: def set_camera_view(self, eye: tuple, target: tuple) -> None: """Set camera view on all visualizers that support it.""" + self._pending_camera_view = (tuple(eye), tuple(target)) for viz in self._visualizers: viz.set_camera_view(eye, target) + def set_renderer_camera_view( + self, + eye: tuple[float, float, float] | list[float], + target: tuple[float, float, float] | list[float], + camera_prim_path: str = "/OmniverseKit_Persp", + ) -> None: + """Set camera view for renderer/viewport camera only. + + This does not broadcast to visualizers. + """ + try: + import isaacsim.core.utils.viewports as isaacsim_viewports + + isaacsim_viewports.set_camera_view( + eye=list(eye), target=list(target), camera_prim_path=str(camera_prim_path) + ) + except Exception as exc: + logger.debug("[SimulationContext] Renderer camera update skipped: %s", exc) + def forward(self) -> None: """Update kinematics without stepping physics.""" self.physics_manager.forward() @@ -671,6 +738,7 @@ def render(self, mode: int | None = None) -> None: """ self.physics_manager.pre_render() self.update_visualizers(self.get_rendering_dt()) + self._render_generation += 1 # Call render callbacks if hasattr(self, "_render_callbacks"): @@ -695,6 +763,9 @@ def update_visualizers(self, dt: float) -> None: visualizers_to_remove.append(viz) continue if viz.is_rendering_paused(): + # Keep visualizer event loops responsive while rendering is paused + # so UI controls (e.g. "Resume Rendering") remain interactive. + viz.step(0.0) continue while viz.is_training_paused() and viz.is_running(): viz.step(0.0) diff --git a/source/isaaclab/isaaclab/visualizers/base_visualizer.py b/source/isaaclab/isaaclab/visualizers/base_visualizer.py index 2480bc89bbaa..159b50eb7813 100644 --- a/source/isaaclab/isaaclab/visualizers/base_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/base_visualizer.py @@ -187,6 +187,15 @@ def set_camera_view(self, eye: tuple, target: tuple) -> None: """ pass + def _resolve_cfg_camera_pose( + self, visualizer_name: str + ) -> tuple[tuple[float, float, float], tuple[float, float, float]]: + """Resolve camera pose from cfg eye/lookat fields.""" + del visualizer_name + eye = tuple(float(v) for v in getattr(self.cfg, "eye")) + lookat = tuple(float(v) for v in getattr(self.cfg, "lookat")) + return eye, lookat + def _resolve_camera_pose_from_usd_path( self, usd_path: str ) -> tuple[tuple[float, float, float], tuple[float, float, float]] | None: diff --git a/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py b/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py index a96e3c04d2b5..3f62c3e5232e 100644 --- a/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py +++ b/source/isaaclab/isaaclab/visualizers/visualizer_cfg.py @@ -34,17 +34,17 @@ class VisualizerCfg: 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.""" + eye: tuple[float, float, float] = (7.5, 7.5, 7.5) + """Initial camera eye 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.""" + lookat: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Initial camera look-at point (x, y, z) in world coordinates.""" - camera_source: Literal["cfg", "usd_path"] = "cfg" - """Camera source mode: 'cfg' uses camera_position/target, 'usd_path' follows a USD camera prim.""" + cam_source: Literal["cfg", "prim_path"] = "cfg" + """Camera source mode: 'cfg' uses eye/lookat, 'prim_path' follows a camera prim.""" - camera_usd_path: str = "/World/envs/env_0/Camera" - """Absolute USD path to a camera prim when camera_source='usd_path'.""" + cam_prim_path: str = "/World/envs/env_0/Camera" + """Absolute USD path to a camera prim when cam_source='prim_path'.""" env_filter_mode: Literal["none", "env_ids", "random_n"] = "none" """Env filter mode: 'none', 'env_ids', or 'random_n'.""" diff --git a/source/isaaclab/setup.py b/source/isaaclab/setup.py index 2bbdcac07f4d..e2d566e80cc8 100644 --- a/source/isaaclab/setup.py +++ b/source/isaaclab/setup.py @@ -22,6 +22,7 @@ "torch>=2.10", "onnx>=1.18.0", # 1.16.2 throws access violation on Windows "prettytable==3.3.0", + "munch", "toml", # devices "hidapi==0.14.0.post2", diff --git a/source/isaaclab/test/sim/test_simulation_context_visualizers.py b/source/isaaclab/test/sim/test_simulation_context_visualizers.py index f0a1294d2b4a..024fba22cc29 100644 --- a/source/isaaclab/test/sim/test_simulation_context_visualizers.py +++ b/source/isaaclab/test/sim/test_simulation_context_visualizers.py @@ -136,10 +136,22 @@ def test_update_visualizers_removes_closed_nonrunning_and_failed(caplog): assert stopped_viz.close_calls == 1 assert failing_viz.close_calls == 1 assert paused_viz.close_calls == 0 + assert paused_viz.step_calls == [0.0] assert healthy_viz.step_calls == [0.1] assert any("Error stepping visualizer" in r.message for r in caplog.records) +def test_update_visualizers_keeps_rendering_paused_visualizer_ui_responsive(): + provider = _FakeProvider() + paused_viz = _FakeVisualizer(rendering_paused=True) + ctx = _make_context([paused_viz], provider=provider) + + ctx.update_visualizers(0.3) + + # Rendering-paused visualizers still receive zero-dt ticks to keep UI callbacks alive. + assert paused_viz.step_calls == [0.0] + + def test_update_visualizers_handles_training_pause_loop(): provider = _FakeProvider() viz = _FakeVisualizer(training_paused_steps=1) diff --git a/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py b/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py index 71bb5e9925cd..bc923c4b9a21 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py +++ b/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py @@ -5,16 +5,19 @@ import asyncio import contextlib +import os import sys import traceback from typing import Any import torch +import warp as wp from isaaclab.envs import ManagerBasedRLMimicEnv from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg from isaaclab.managers import DatasetExportMode, TerminationTermCfg from isaaclab.managers.recorder_manager import RecorderManagerBaseCfg +from isaaclab.utils.math import subtract_frame_transforms from isaaclab_mimic.datagen.data_generator import DataGenerator from isaaclab_mimic.datagen.datagen_info_pool import DataGenInfoPool @@ -27,6 +30,59 @@ num_attempts = 0 +def _debug_wrist_cam_pose(env: ManagerBasedRLMimicEnv, env_id: int, tag: str, hand_body_idx: int | None): + """Print wrist camera and hand-link transforms for debugging camera alignment.""" + try: + if "wrist_cam" not in env.scene.keys() or "robot" not in env.scene.keys(): + print(f"[WRIST_CAM_DEBUG] {tag} env={env_id} missing wrist_cam or robot in scene") + return + + wrist_cam = env.scene["wrist_cam"] + robot = env.scene["robot"] + cam_pos = wrist_cam.data.pos_w[env_id].detach().cpu() + cam_quat = wrist_cam.data.quat_w_world[env_id].detach().cpu() + cam_frame = int(wrist_cam.frame[env_id].item()) if wrist_cam.frame.numel() > env_id else -1 + live_pos_all, live_quat_all = wrist_cam._view.get_world_poses([env_id]) + live_cam_pos = live_pos_all[0].detach().cpu() + live_cam_quat = live_quat_all[0].detach().cpu() + + if hand_body_idx is None: + print( + f"[WRIST_CAM_DEBUG] {tag} env={env_id} cam_frame={cam_frame} " + f"cam_pos={cam_pos.tolist()} cam_quat_xyzw={cam_quat.tolist()} " + f"live_cam_pos={live_cam_pos.tolist()} live_cam_quat_xyzw={live_cam_quat.tolist()} " + f"hand_body_idx=None" + ) + return + + hand_pos = wp.to_torch(robot.data.body_pos_w)[env_id, hand_body_idx].detach().cpu() + hand_quat = wp.to_torch(robot.data.body_quat_w)[env_id, hand_body_idx].detach().cpu() + rel_pos, rel_quat = subtract_frame_transforms( + hand_pos.unsqueeze(0), + hand_quat.unsqueeze(0), + cam_pos.unsqueeze(0), + cam_quat.unsqueeze(0), + ) + live_rel_pos, live_rel_quat = subtract_frame_transforms( + hand_pos.unsqueeze(0), + hand_quat.unsqueeze(0), + live_cam_pos.unsqueeze(0), + live_cam_quat.unsqueeze(0), + ) + print( + f"[WRIST_CAM_DEBUG] {tag} env={env_id} cam_frame={cam_frame} " + f"cam_pos={cam_pos.tolist()} cam_quat_xyzw={cam_quat.tolist()} " + f"live_cam_pos={live_cam_pos.tolist()} live_cam_quat_xyzw={live_cam_quat.tolist()} " + f"hand_pos={hand_pos.tolist()} hand_quat_xyzw={hand_quat.tolist()} " + f"cam_rel_hand_pos={rel_pos[0].detach().cpu().tolist()} " + f"cam_rel_hand_quat_xyzw={rel_quat[0].detach().cpu().tolist()} " + f"live_cam_rel_hand_pos={live_rel_pos[0].detach().cpu().tolist()} " + f"live_cam_rel_hand_quat_xyzw={live_rel_quat[0].detach().cpu().tolist()}" + ) + except Exception as exc: + print(f"[WRIST_CAM_DEBUG] {tag} env={env_id} logging_failed={exc}") + + async def run_data_generator( env: ManagerBasedRLMimicEnv, env_id: int, @@ -91,6 +147,18 @@ def env_loop( global num_success, num_failures, num_attempts env_id_tensor = torch.tensor([0], dtype=torch.int64, device=env.device) prev_num_attempts = 0 + debug_wrist_cam = os.environ.get("ISAACLAB_DEBUG_WRIST_CAM", "0") == "1" + post_reset_step_counters = [0 for _ in range(env.num_envs)] + hand_body_idx = None + if debug_wrist_cam: + try: + if "robot" in env.scene.keys(): + body_ids, _ = env.scene["robot"].find_bodies("panda_hand") + hand_body_idx = int(body_ids[0]) if len(body_ids) > 0 else None + print(f"[WRIST_CAM_DEBUG] enabled=1 hand_body_idx={hand_body_idx}") + except Exception as exc: + print(f"[WRIST_CAM_DEBUG] enabled=1 hand_body_idx_lookup_failed={exc}") + # simulate environment -- run everything in inference mode with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): while True: @@ -100,6 +168,11 @@ def env_loop( while not env_reset_queue.empty(): env_id_tensor[0] = env_reset_queue.get_nowait() env.reset(env_ids=env_id_tensor) + reset_env_id = int(env_id_tensor[0].item()) + if 0 <= reset_env_id < env.num_envs: + post_reset_step_counters[reset_env_id] = 0 + if debug_wrist_cam: + _debug_wrist_cam_pose(env, reset_env_id, tag="after_reset", hand_body_idx=hand_body_idx) env_reset_queue.task_done() actions = torch.zeros(env.action_space.shape) @@ -112,6 +185,16 @@ def env_loop( # perform action on environment env.step(actions) + if debug_wrist_cam: + for env_id in range(env.num_envs): + if post_reset_step_counters[env_id] <= 1: + _debug_wrist_cam_pose( + env, + env_id, + tag=f"after_step_{post_reset_step_counters[env_id]}", + hand_body_idx=hand_body_idx, + ) + post_reset_step_counters[env_id] += 1 # mark done so the data generators can continue with the step results for i in range(env.num_envs): diff --git a/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer_utils.py b/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer_utils.py index 8e5b9abc8549..8c848be42094 100644 --- a/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer_utils.py +++ b/source/isaaclab_physx/isaaclab_physx/renderers/isaac_rtx_renderer_utils.py @@ -14,11 +14,11 @@ logger = logging.getLogger(__name__) -# Module-level dedup stamp: tracks the last (sim instance, physics step) at +# Module-level dedup stamp: tracks the last (sim instance, physics step, render generation) at # which Kit's ``app.update()`` was pumped. Keyed on ``id(sim)`` so that a # new ``SimulationContext`` (e.g. in a new test) automatically invalidates # any stale stamp from a previous instance. -_last_render_update_key: tuple[int, int] = (0, -1) +_last_render_update_key: tuple[int, int, int] = (0, -1, -1) # --------------------------------------------------------------------------- # RTX streaming status tracking @@ -91,7 +91,7 @@ def _wait_for_streaming_complete() -> None: def ensure_isaac_rtx_render_update() -> None: - """Ensure the Isaac RTX renderer has been pumped for the current physics step. + """Ensure the Isaac RTX renderer has been pumped for the current sim step. This keeps the Kit-specific ``app.update()`` logic inside the renderers package rather than in the backend-agnostic ``SimulationContext``. @@ -99,11 +99,11 @@ def ensure_isaac_rtx_render_update() -> None: Safe to call from multiple ``Camera`` / ``TiledCamera`` instances per step — only the first call triggers ``app.update()``. Subsequent calls are no-ops because the module-level ``_last_render_update_key`` already matches the - current ``(id(sim), step_count)`` pair. + current ``(id(sim), step_count, render_generation)`` tuple. - The key is a ``(sim_instance_id, step_count)`` tuple so that creating a new - ``SimulationContext`` (e.g. in a subsequent test) automatically invalidates - any stale stamp left over from a previous instance. + The key is a ``(sim_instance_id, step_count, render_generation)`` tuple so that: + - creating a new ``SimulationContext`` invalidates stale stamps, and + - render/reset transitions that do not advance physics step count still force a fresh update. If RTX texture/geometry streaming is in progress, additional ``app.update()`` calls are pumped until the streaming subsystem reports @@ -120,7 +120,8 @@ def ensure_isaac_rtx_render_update() -> None: if sim is None: return - key = (id(sim), sim._physics_step_count) + render_generation = getattr(sim, "render_generation", getattr(sim, "_render_generation", 0)) + key = (id(sim), sim._physics_step_count, render_generation) if _last_render_update_key == key: return # Already pumped this step (by another camera or a visualizer) diff --git a/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py b/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py index 6b1b5c2077dc..8593857ae054 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py @@ -81,9 +81,11 @@ def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: logger=logger, title="KitVisualizer Configuration", rows=[ - ("camera_position", self.cfg.camera_position), - ("camera_target", self.cfg.camera_target), - ("camera_source", self.cfg.camera_source), + ("eye", self.cfg.eye), + ("lookat", self.cfg.lookat), + ("cam_source", self.cfg.cam_source), + ("visualizer_camera_prim_path", self.cfg.visualizer_camera_prim_path), + ("enable_visualizer_cam", self.cfg.enable_visualizer_cam), ("num_visualized_envs", num_visualized_envs), ("create_viewport", self.cfg.create_viewport), ("headless", self._runtime_headless), @@ -105,16 +107,9 @@ def step(self, dt: float) -> None: try: import omni.kit.app - from isaaclab.app.settings_manager import get_settings_manager - app = omni.kit.app.get_app() if app is not None and app.is_running(): - # Keep app pumping for viewport/UI updates only. - # Simulation stepping is owned by SimulationContext. - settings = get_settings_manager() - settings.set_bool("/app/player/playSimulations", False) app.update() - settings.set_bool("/app/player/playSimulations", True) except (ImportError, AttributeError) as exc: logger.debug("[KitVisualizer] App update skipped: %s", exc) @@ -148,13 +143,15 @@ def is_running(self) -> bool: return False def is_training_paused(self) -> bool: - """Return whether simulation play flag is paused in Kit settings.""" + """Return whether Kit timeline transport is paused.""" try: - from isaaclab.app.settings_manager import get_settings_manager + import omni.timeline - settings = get_settings_manager() - play_flag = settings.get("/app/player/playSimulations") - return play_flag is False + timeline = omni.timeline.get_timeline_interface() + if timeline is None: + return False + # Pause is transport state that is neither playing nor stopped. + return (not timeline.is_playing()) and (not timeline.is_stopped()) except Exception: return False @@ -186,6 +183,9 @@ def set_camera_view( if not self._is_initialized: logger.debug("[KitVisualizer] set_camera_view() ignored because visualizer is not initialized.") return + if not self.cfg.enable_visualizer_cam: + logger.debug("[KitVisualizer] set_camera_view() ignored because enable_visualizer_cam=False.") + return self._set_viewport_camera(tuple(eye), tuple(target)) # ---- Viewport + camera ---------------------------------------------------------------- @@ -228,9 +228,17 @@ def _setup_viewport(self, usd_stage) -> None: # In headless mode we keep the visualizer active but skip viewport/window setup. self._viewport_window = None self._viewport_api = None + self._controlled_camera_path = "/OmniverseKit_Persp" if self.cfg.enable_visualizer_cam else None + if not self.cfg.enable_visualizer_cam: + return + self._apply_cfg_camera_pose_if_configured() return - if self.cfg.create_viewport and self.cfg.viewport_name: + if self.cfg.create_viewport: + if not self.cfg.viewport_name.strip(): + raise RuntimeError( + "[KitVisualizer] viewport_name must be a non-empty string when create_viewport=True." + ) dock_position_name = self.cfg.dock_position.upper() dock_position_map = { "LEFT": DockPosition.LEFT, @@ -250,7 +258,6 @@ def _setup_viewport(self, usd_stage) -> None: ) asyncio.ensure_future(self._dock_viewport_async(self.cfg.viewport_name, dock_pos)) - self._create_and_assign_camera(usd_stage) else: self._viewport_window = vp_utils.get_active_viewport_window() @@ -259,18 +266,20 @@ def _setup_viewport(self, usd_stage) -> None: self._viewport_api = None return self._viewport_api = self._viewport_window.viewport_api - # Pin the camera path we will write to, using the active camera at init time. - # This must happen before any _set_viewport_camera() call so the path is known. - self._controlled_camera_path = self._viewport_api.get_active_camera() or "/OmniverseKit_Persp" - if self.cfg.camera_source == "usd_path": - if not self._set_active_camera_path(self.cfg.camera_usd_path): - logger.warning( - "[KitVisualizer] camera_usd_path '%s' not found; using configured camera.", - self.cfg.camera_usd_path, + if not self.cfg.enable_visualizer_cam: + self._controlled_camera_path = None + return + # Always create/use a dedicated visualizer-controlled camera so we never mutate + # existing viewport cameras (e.g. /OmniverseKit_Persp or sensor cameras). + self._create_and_assign_camera(usd_stage) + if self.cfg.cam_source == "prim_path": + if not self._set_active_camera_path(self.cfg.cam_prim_path): + raise RuntimeError( + "[KitVisualizer] cam_source='prim_path' requires a valid cam_prim_path. " + f"Camera prim not found: '{self.cfg.cam_prim_path}'." ) - self._set_viewport_camera(self.cfg.camera_position, self.cfg.camera_target) else: - self._set_viewport_camera(self.cfg.camera_position, self.cfg.camera_target) + self._apply_cfg_camera_pose_if_configured() async def _dock_viewport_async(self, viewport_name: str, dock_position) -> None: """Dock a created viewport window relative to main viewport.""" @@ -304,8 +313,10 @@ async def _dock_viewport_async(self, viewport_name: str, dock_position) -> None: viewport_window.focus() def _create_and_assign_camera(self, usd_stage) -> None: - """Create viewport camera prim (if needed) and set it active.""" - camera_path = f"/World/Cameras/{self.cfg.viewport_name}_Camera".replace(" ", "_") + """Create dedicated visualizer camera prim (if needed) and set it active.""" + camera_path = self.cfg.visualizer_camera_prim_path + if not camera_path: + raise RuntimeError("[KitVisualizer] visualizer_camera_prim_path must be a non-empty prim path.") camera_prim = usd_stage.GetPrimAtPath(camera_path) if not camera_prim.IsValid(): @@ -319,8 +330,6 @@ def _set_viewport_camera(self, position: tuple[float, float, float], target: tup """Apply eye/target camera view to the active viewport.""" import isaacsim.core.utils.viewports as isaacsim_viewports - if self._viewport_api is None: - return # Use the camera path pinned at initialization. This prevents user-switching the GUI # viewport to a sensor camera from corrupting the sensor's USD prim transform. camera_path = self._controlled_camera_path @@ -328,10 +337,14 @@ def _set_viewport_camera(self, position: tuple[float, float, float], target: tup camera_path = self._viewport_api.get_active_camera() if self._viewport_api else None if not camera_path: camera_path = "/OmniverseKit_Persp" - - isaacsim_viewports.set_camera_view( - eye=list(position), target=list(target), camera_prim_path=camera_path, viewport_api=self._viewport_api - ) + kwargs = {"eye": list(position), "target": list(target), "camera_prim_path": camera_path} + if self._viewport_api is not None: + kwargs["viewport_api"] = self._viewport_api + isaacsim_viewports.set_camera_view(**kwargs) + + def _apply_cfg_camera_pose_if_configured(self) -> None: + """Apply configured camera pose from eye/lookat.""" + self._set_viewport_camera(self.cfg.eye, self.cfg.lookat) def _set_active_camera_path(self, camera_path: str) -> bool: """Set active camera path for viewport if the prim exists. diff --git a/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer_cfg.py b/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer_cfg.py index 88112a6f20b4..d3fea2e0da6b 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer_cfg.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer_cfg.py @@ -16,12 +16,22 @@ class KitVisualizerCfg(VisualizerCfg): visualizer_type: str = "kit" """Type identifier for Kit visualizer.""" - viewport_name: str | None = "Visualizer Viewport" - """Viewport name to use. If None, uses active viewport.""" + viewport_name: str = "Visualizer Viewport" + """Viewport name to use when :attr:`create_viewport` is True.""" - create_viewport: bool = False + create_viewport: bool = True """Create new viewport with specified name and camera pose.""" + visualizer_camera_prim_path: str = "/World/Cameras/KitVisualizerCamera" + """Dedicated camera prim path controlled by the Kit visualizer.""" + + enable_visualizer_cam: bool = True + """Whether the Kit visualizer should control/bind a dedicated viewport camera. + + If False, Kit does not create/switch camera prims and ignores visualizer camera control + updates (including eye/lookat and cam_source handling). + """ + headless: bool = False """Run without creating viewport windows when supported by the app.""" diff --git a/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py b/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py index 70d44d1956f7..ecc6f6d327d8 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py @@ -308,7 +308,8 @@ def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: max_worlds = self.cfg.max_worlds self._viewer.set_model(self._model, max_worlds=max_worlds) self._viewer.set_world_offsets((0.0, 0.0, 0.0)) - self._apply_camera_pose(self._resolve_initial_camera_pose()) + initial_pose = self._resolve_initial_camera_pose() + self._apply_camera_pose(initial_pose) self._viewer.up_axis = 2 # Z-up self._viewer.scaling = 1.0 @@ -334,13 +335,11 @@ def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: title="NewtonVisualizer Configuration", rows=[ ( - "camera_position", - tuple(float(x) for x in self._viewer.camera.pos) - if self._viewer is not None - else self.cfg.camera_position, + "eye", + tuple(float(x) for x in self._viewer.camera.pos) if self._viewer is not None else self.cfg.eye, ), - ("camera_target", self._last_camera_pose[1] if self._last_camera_pose else self.cfg.camera_target), - ("camera_source", self.cfg.camera_source), + ("lookat", self._last_camera_pose[1] if self._last_camera_pose else self.cfg.lookat), + ("cam_source", self.cfg.cam_source), ("num_visualized_envs", num_visualized_envs), ("headless", self.cfg.headless), ], @@ -364,7 +363,7 @@ def step(self, dt: float) -> None: self._state = self._scene_data_provider.get_newton_state(self._env_ids) return - if self.cfg.camera_source == "usd_path": + if self.cfg.cam_source == "prim_path": self._update_camera_from_usd_path() self._state = self._scene_data_provider.get_newton_state(self._env_ids) @@ -429,15 +428,15 @@ def _resolve_initial_camera_pose(self) -> tuple[tuple[float, float, float], tupl Returns: Camera eye and target tuples. """ - if self.cfg.camera_source == "usd_path": - pose = self._resolve_camera_pose_from_usd_path(self.cfg.camera_usd_path) + if self.cfg.cam_source == "prim_path": + pose = self._resolve_camera_pose_from_usd_path(self.cfg.cam_prim_path) if pose is not None: return pose - logger.warning( - "[NewtonVisualizer] camera_usd_path '%s' not found; using configured camera.", - self.cfg.camera_usd_path, + raise RuntimeError( + "[NewtonVisualizer] cam_source='prim_path' requires a resolvable camera prim path, " + f"but no camera pose was found for '{self.cfg.cam_prim_path}'." ) - return self.cfg.camera_position, self.cfg.camera_target + return self._resolve_cfg_camera_pose("NewtonVisualizer") def _apply_camera_pose(self, pose: tuple[tuple[float, float, float], tuple[float, float, float]]) -> None: """Apply camera eye/target pose to the Newton viewer. @@ -461,7 +460,7 @@ def _apply_camera_pose(self, pose: tuple[tuple[float, float, float], tuple[float def _update_camera_from_usd_path(self) -> None: """Refresh camera pose from configured USD camera path when it changes.""" - pose = self._resolve_camera_pose_from_usd_path(self.cfg.camera_usd_path) + pose = self._resolve_camera_pose_from_usd_path(self.cfg.cam_prim_path) if pose is None: return if self._last_camera_pose == pose: diff --git a/source/isaaclab_visualizers/isaaclab_visualizers/rerun/rerun_visualizer.py b/source/isaaclab_visualizers/isaaclab_visualizers/rerun/rerun_visualizer.py index ab2ed723223f..531b067104c1 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/rerun/rerun_visualizer.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/rerun/rerun_visualizer.py @@ -188,7 +188,8 @@ def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: self._viewer.set_model(self._model, max_worlds=self.cfg.max_worlds) # Preserve simulation world positions (env_spacing) rather than adding viewer-side offsets. self._viewer.set_world_offsets((0.0, 0.0, 0.0)) - self._apply_camera_pose(self._resolve_initial_camera_pose()) + initial_pose = self._resolve_initial_camera_pose() + self._apply_camera_pose(initial_pose) self._viewer.up_axis = 2 self._viewer.scaling = 1.0 self._viewer._paused = False @@ -198,9 +199,9 @@ def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: logger=logger, title="RerunVisualizer Configuration", rows=[ - ("camera_position", self.cfg.camera_position), - ("camera_target", self.cfg.camera_target), - ("camera_source", self.cfg.camera_source), + ("eye", self.cfg.eye), + ("lookat", self.cfg.lookat), + ("cam_source", self.cfg.cam_source), ("num_visualized_envs", num_visualized_envs), ("endpoint", f"http://{viewer_host}:{web_port}"), ("viewer_url", viewer_url), @@ -227,7 +228,7 @@ def step(self, dt: float) -> None: self._sim_time += dt self._step_counter += 1 - if self.cfg.camera_source == "usd_path": + if self.cfg.cam_source == "prim_path": self._update_camera_from_usd_path() self._state = self._scene_data_provider.get_newton_state(self._env_ids) @@ -275,11 +276,15 @@ def is_running(self) -> bool: def _resolve_initial_camera_pose(self) -> tuple[tuple[float, float, float], tuple[float, float, float]]: """Resolve initial camera pose from config or USD camera path.""" - if self.cfg.camera_source == "usd_path": - pose = self._resolve_camera_pose_from_usd_path(self.cfg.camera_usd_path) + if self.cfg.cam_source == "prim_path": + pose = self._resolve_camera_pose_from_usd_path(self.cfg.cam_prim_path) if pose is not None: return pose - return self.cfg.camera_position, self.cfg.camera_target + raise RuntimeError( + "[RerunVisualizer] cam_source='prim_path' requires a resolvable camera prim path, " + f"but no camera pose was found for '{self.cfg.cam_prim_path}'." + ) + return self._resolve_cfg_camera_pose("RerunVisualizer") def _apply_camera_pose(self, pose: tuple[tuple[float, float, float], tuple[float, float, float]]) -> None: """Apply camera pose to rerun's 3D view controls. @@ -307,7 +312,7 @@ def _apply_camera_pose(self, pose: tuple[tuple[float, float, float], tuple[float def _update_camera_from_usd_path(self) -> None: """Refresh camera pose from configured USD camera path when it changes.""" - pose = self._resolve_camera_pose_from_usd_path(self.cfg.camera_usd_path) + pose = self._resolve_camera_pose_from_usd_path(self.cfg.cam_prim_path) if pose is None: return if self._last_camera_pose == pose: diff --git a/source/isaaclab_visualizers/isaaclab_visualizers/viser/viser_visualizer.py b/source/isaaclab_visualizers/isaaclab_visualizers/viser/viser_visualizer.py index c20bfcd85a9f..d6606403bba6 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/viser/viser_visualizer.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/viser/viser_visualizer.py @@ -162,9 +162,9 @@ def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: logger=logger, title="ViserVisualizer Configuration", rows=[ - ("camera_position", self.cfg.camera_position), - ("camera_target", self.cfg.camera_target), - ("camera_source", self.cfg.camera_source), + ("eye", self.cfg.eye), + ("lookat", self.cfg.lookat), + ("cam_source", self.cfg.cam_source), ("num_visualized_envs", num_visualized_envs), ("port", self.cfg.port), ("viewer_url", viewer_url), @@ -182,7 +182,7 @@ def step(self, dt: float) -> None: if not self._is_initialized or self._viewer is None or self._scene_data_provider is None: return - if self.cfg.camera_source == "usd_path": + if self.cfg.cam_source == "prim_path": self._update_camera_from_usd_path() self._apply_pending_camera_pose() @@ -259,7 +259,8 @@ def _create_viewer(self, record_to_viser: str | None, metadata: dict | None = No self._viewer.set_world_offsets((0.0, 0.0, 0.0)) if self.cfg.open_browser: _open_viser_web_viewer(self.cfg.port) - self._set_viser_camera_view(self._resolve_initial_camera_pose()) + initial_pose = self._resolve_initial_camera_pose() + self._set_viser_camera_view(initial_pose) self._sim_time = 0.0 def _close_viewer(self, finalize_viser: bool = False) -> None: @@ -277,15 +278,15 @@ def _close_viewer(self, finalize_viser: bool = False) -> None: def _resolve_initial_camera_pose(self) -> tuple[tuple[float, float, float], tuple[float, float, float]]: """Resolve initial camera pose from config or USD camera path.""" - if self.cfg.camera_source == "usd_path": - pose = self._resolve_camera_pose_from_usd_path(self.cfg.camera_usd_path) + if self.cfg.cam_source == "prim_path": + pose = self._resolve_camera_pose_from_usd_path(self.cfg.cam_prim_path) if pose is not None: return pose - logger.warning( - "[ViserVisualizer] camera_usd_path '%s' not found; using configured camera.", - self.cfg.camera_usd_path, + raise RuntimeError( + "[ViserVisualizer] cam_source='prim_path' requires a resolvable camera prim path, " + f"but no camera pose was found for '{self.cfg.cam_prim_path}'." ) - return self.cfg.camera_position, self.cfg.camera_target + return self._resolve_cfg_camera_pose("ViserVisualizer") def _try_apply_viser_camera_view(self, pose: tuple[tuple[float, float, float], tuple[float, float, float]]) -> bool: """Try applying camera pose to active viser clients. @@ -341,7 +342,7 @@ def _apply_pending_camera_pose(self) -> None: def _update_camera_from_usd_path(self) -> None: """Refresh camera pose from configured USD camera path when it changes.""" - pose = self._resolve_camera_pose_from_usd_path(self.cfg.camera_usd_path) + pose = self._resolve_camera_pose_from_usd_path(self.cfg.cam_prim_path) if pose is None: return if self._last_camera_pose == pose or self._pending_camera_pose == pose: diff --git a/source/isaaclab_visualizers/test/test_visualizer_smoke_logs.py b/source/isaaclab_visualizers/test/test_visualizer_smoke_logs.py index 22f620fb02a8..83cfc36b7194 100644 --- a/source/isaaclab_visualizers/test/test_visualizer_smoke_logs.py +++ b/source/isaaclab_visualizers/test/test_visualizer_smoke_logs.py @@ -10,9 +10,12 @@ # launch Kit app simulation_app = AppLauncher(headless=True, enable_cameras=True).app +import contextlib +import copy import logging import socket +import numpy as np import pytest import torch from isaaclab_visualizers.kit import KitVisualizer, KitVisualizerCfg @@ -26,6 +29,8 @@ from isaaclab.sim import SimulationCfg, SimulationContext from isaaclab.utils import configclass +from isaaclab_tasks.direct.cartpole.cartpole_camera_env import CartpoleCameraEnv +from isaaclab_tasks.direct.cartpole.cartpole_camera_presets_env_cfg import CartpoleCameraPresetsEnvCfg from isaaclab_tasks.manager_based.classic.cartpole.cartpole_env_cfg import ( CartpolePhysicsCfg, CartpoleSceneCfg, @@ -35,6 +40,7 @@ ASSERT_VISUALIZER_WARNINGS = True _SMOKE_STEPS = 4 +_MAX_NON_BLACK_STEPS = max(_SMOKE_STEPS, 8) _VIS_LOGGER_PREFIXES = ( "isaaclab.visualizers", "isaaclab_visualizers", @@ -93,7 +99,8 @@ def _get_visualizer_cfg(visualizer_kind: str): if visualizer_kind == "viser": __import__("newton") __import__("viser") - return ViserVisualizerCfg(open_browser=False), ViserVisualizer + port = _find_free_tcp_port(host="127.0.0.1") + return ViserVisualizerCfg(open_browser=False, port=port), ViserVisualizer if visualizer_kind == "rerun": __import__("newton") __import__("rerun") @@ -151,6 +158,120 @@ def _get_physics_cfg(backend_kind: str): raise ValueError(f"Unknown backend: {backend_kind!r}") +def _assert_non_black_tensor(image_tensor: torch.Tensor, *, min_nonzero_pixels: int = 1) -> None: + """Assert camera-like tensor contains non-black pixels.""" + assert isinstance(image_tensor, torch.Tensor), f"Expected torch.Tensor, got {type(image_tensor)!r}" + assert image_tensor.numel() > 0, "Image tensor is empty." + finite_tensor = torch.where(torch.isfinite(image_tensor), image_tensor, torch.zeros_like(image_tensor)) + if finite_tensor.dtype.is_floating_point: + nonzero = torch.count_nonzero(torch.abs(finite_tensor) > 1e-6).item() + else: + nonzero = torch.count_nonzero(finite_tensor > 0).item() + assert nonzero >= min_nonzero_pixels, "Rendered frame appears black (no non-zero pixels)." + + +def _assert_non_black_frame_array(frame) -> None: + """Assert viewer-captured frame has visible, non-black content.""" + frame_arr = np.asarray(frame) + assert frame_arr.size > 0, "Viewer returned an empty frame." + if frame_arr.ndim == 2: + color = frame_arr + else: + assert frame_arr.shape[-1] >= 3, f"Expected at least 3 channels, got shape {frame_arr.shape}." + color = frame_arr[..., :3] + finite = np.where(np.isfinite(color), color, 0) + assert np.count_nonzero(finite) > 0, "Viewer frame appears fully black." + + +def _step_until_non_black_camera(env, actions: torch.Tensor, *, max_steps: int = _MAX_NON_BLACK_STEPS) -> None: + """Step env until camera frame is non-black, bounded by max_steps.""" + last_rgb = None + for _ in range(max_steps): + env.step(action=actions) + rgb = env._tiled_camera.data.output.get("rgb") + if rgb is None: + rgb = env._tiled_camera.data.output[env.cfg.tiled_camera.data_types[0]] + last_rgb = rgb + try: + _assert_non_black_tensor(rgb) + return + except AssertionError: + continue + # Preserve existing assertion semantics with a final explicit check. + _assert_non_black_tensor(last_rgb) + + +def _build_rgb_annotator_for_camera(camera_path: str, *, resolution: tuple[int, int] = (320, 240)): + """Create CPU RGB annotator attached to a camera render product.""" + import omni.replicator.core as rep + + render_product = rep.create.render_product(camera_path, resolution=resolution) + annotator = rep.AnnotatorRegistry.get_annotator("rgb", device="cpu") + annotator.attach([render_product]) + return annotator, render_product + + +def _annotator_rgb_to_numpy(rgb_data) -> np.ndarray: + """Convert replicator annotator output to HxWx3 uint8 numpy array.""" + rgb_array = np.frombuffer(rgb_data, dtype=np.uint8).reshape(*rgb_data.shape) + if rgb_array.size == 0: + return np.zeros((1, 1, 3), dtype=np.uint8) + return rgb_array[:, :, :3] + + +def _step_until_non_black_kit_viewport( + env, kit_visualizer: KitVisualizer, *, max_steps: int = _MAX_NON_BLACK_STEPS +) -> None: + """Step env until Kit viewport camera render product is non-black, bounded by max_steps.""" + camera_path = getattr(kit_visualizer, "_controlled_camera_path", None) + if not camera_path: + pytest.skip("Kit visualizer does not expose a controlled viewport camera path.") + + annotator = None + render_product = None + try: + annotator, render_product = _build_rgb_annotator_for_camera(camera_path) + actions = torch.zeros((env.num_envs, env.action_space.shape[-1]), device=env.device) + last_frame = None + for _ in range(max_steps): + env.step(action=actions) + rgb_data = annotator.get_data() + frame = _annotator_rgb_to_numpy(rgb_data) + last_frame = frame + try: + _assert_non_black_frame_array(frame) + return + except AssertionError: + continue + _assert_non_black_frame_array(last_frame) + finally: + if annotator is not None and render_product is not None: + with contextlib.suppress(Exception): + annotator.detach([render_product]) + + +def _make_cartpole_camera_env(visualizer_kind: str, backend_kind: str) -> CartpoleCameraEnv: + """Create cartpole camera env configured with selected visualizer and physics backend.""" + env_cfg_root = CartpoleCameraPresetsEnvCfg() + env_cfg = getattr(env_cfg_root, "default", None) + if env_cfg is None: + env_cfg = getattr(type(env_cfg_root), "default", None) + if env_cfg is None: + raise RuntimeError( + "CartpoleCameraPresetsEnvCfg does not expose a 'default' preset config. " + f"Available attributes: {sorted(vars(env_cfg_root).keys())}" + ) + env_cfg = copy.deepcopy(env_cfg) + env_cfg.scene.num_envs = 1 + # Avoid hard dependency on replicator graph initialization in this smoke test; + # deterministic seeding is not required for black-frame detection coverage. + env_cfg.seed = None + env_cfg.sim.physics, _ = _get_physics_cfg(backend_kind) + visualizer_cfg, _ = _get_visualizer_cfg(visualizer_kind) + env_cfg.sim.visualizer_cfgs = visualizer_cfg + return CartpoleCameraEnv(env_cfg) + + def _resolve_case(visualizer_kind: str, backend_kind: str): """Resolve (env_cfg, expected_visualizer_cls, expected_backend_substring) for one smoke test. @@ -224,5 +345,57 @@ def test_visualizer_backend_smoke(visualizer_kind: str, backend_kind: str, caplo _run_smoke_test(cfg, expected_viz_cls, expected_backend, caplog) +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("backend_kind", ["physx", "newton"]) +def test_kit_visualizer_non_black_viewport_frame(backend_kind: str): + """Kit visualizer viewport camera output should not be black.""" + env = None + try: + sim_utils.create_new_stage() + env = _make_cartpole_camera_env(visualizer_kind="kit", backend_kind=backend_kind) + env.sim._app_control_on_stop_handle = None # type: ignore[attr-defined] + env.reset() + kit_visualizers = [viz for viz in env.sim.visualizers if isinstance(viz, KitVisualizer)] + assert kit_visualizers, "Expected an initialized Kit visualizer." + _step_until_non_black_kit_viewport(env, kit_visualizers[0], max_steps=_MAX_NON_BLACK_STEPS) + finally: + if env is not None: + env.close() + else: + SimulationContext.clear_instance() + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("backend_kind", ["physx", "newton"]) +def test_newton_visualizer_non_black_viewer_frame(backend_kind: str): + """Newton visualizer should produce at least one non-black viewer frame for Cartpole.""" + env = None + try: + sim_utils.create_new_stage() + env = _make_cartpole_camera_env(visualizer_kind="newton", backend_kind=backend_kind) + env.sim._app_control_on_stop_handle = None # type: ignore[attr-defined] + env.reset() + actions = torch.zeros((env.num_envs, env.action_space.shape[-1]), device=env.device) + for _ in range(_MAX_NON_BLACK_STEPS): + env.step(action=actions) + + newton_visualizers = [viz for viz in env.sim.visualizers if isinstance(viz, NewtonVisualizer)] + assert newton_visualizers, "Expected an initialized Newton visualizer." + viewer = getattr(newton_visualizers[0], "_viewer", None) + assert viewer is not None, "Newton viewer was not created." + + get_frame = getattr(viewer, "get_frame", None) + if not callable(get_frame): + pytest.skip("ViewerGL.get_frame is not available in this Newton version.") + + frame = get_frame() + _assert_non_black_frame_array(frame) + finally: + if env is not None: + env.close() + else: + SimulationContext.clear_instance() + + if __name__ == "__main__": pytest.main([__file__, "-v", "--maxfail=1"])