diff --git a/docs/source/features/isaac_teleop.rst b/docs/source/features/isaac_teleop.rst index c5f492072e1b..22f56d89d626 100644 --- a/docs/source/features/isaac_teleop.rst +++ b/docs/source/features/isaac_teleop.rst @@ -1162,9 +1162,10 @@ can be referenced via constants: - ``0`` - ``0`` -All three profiles set ``NV_CXR_ENABLE_PUSH_DEVICES=0``, which is correct for headset optical hand -tracking (the most common setup). For external push-device peripherals such as Manus gloves, set -this to ``1`` in a custom profile (see below). +The ``cloudxrjs`` profile sets ``NV_CXR_ENABLE_PUSH_DEVICES=1`` to admit its WebXR +input path. The AVP and standalone profiles set it to ``0`` for headset optical +hand tracking. Custom profiles for external push-device peripherals such as +Manus gloves must enable this setting. They also set ``NV_ENABLE_POSE_WAIT=0`` so CloudXR does not throttle the application when frame times spike. This favors lower latency over CloudXR's pose-wait smoothing. @@ -1204,8 +1205,9 @@ Copy a shipped profile and edit it: cp $(uv run --extra teleop,isaacsim python -c \ "from isaaclab_teleop import CLOUDXR_JS_ENV; print(CLOUDXR_JS_ENV)") ~/my-cloudxr.env -Edit ``~/my-cloudxr.env`` to change any values (e.g. ``NV_CXR_ENABLE_PUSH_DEVICES=1`` for -Manus gloves), then pass it via ``--cloudxr_env ~/my-cloudxr.env``. +Edit ``~/my-cloudxr.env`` to change any values, then pass it via +``--cloudxr_env ~/my-cloudxr.env``. Keep ``NV_CXR_ENABLE_PUSH_DEVICES=0`` for headset optical hands; +enable it only for external push-device peripherals such as Manus gloves. Disable auto-launch ~~~~~~~~~~~~~~~~~~~ @@ -1266,6 +1268,33 @@ through this variable -- later runs no longer prompt. The variable applies to ev that launches the runtime, including the process-scoped launcher in ``teleop_replay_agent.py``. +.. _isaac-teleop-ovxr-mcap: + +Replay MCAP with a Kitless OVXR Viewer +-------------------------------------- + +The MCAP replay entrypoint can present recorded input through an independent +OVXR viewer. Follow the :ref:`kitless XR guide ` to install Isaac +Capture with its OVXR extra and select the compatible rendering dependencies. + +.. code-block:: bash + + uv run --extra ovxr python \ + scripts/environments/teleoperation/teleop_replay_agent.py \ + --task IsaacContrib-PickPlace-GR1T2-Abs --physics ovphysx \ + --device cpu --num_envs 1 --ovxr \ + --replay_file /absolute/path/to/session.mcap + +Use ``--physics newton_mjwarp`` for Newton. MCAP supplies tracker and control +input; the connected client supplies the view. A recorded STOP, task success +or failure, or the replay duration limit ends the run. The separate +``isaaclab teleop replay`` command replays HDF5 demonstrations through Kit. + +Add ``--ovxr_pip_camera robot_pov_cam --ovxr_pip_head_locked`` for a camera +panel, or ``--ovxr_desktop_mirror`` for the optional desktop window. See the +kitless XR guide for build prerequisites, camera selection and display requirements. + + .. _isaac-teleop-xr-anchor: Configure the XR Anchor @@ -1518,8 +1547,10 @@ Kit Scene UI presentation PiP presentation uses Kit Scene UI and ``SpatialSource`` placement. Kit imports are deferred until an enabled feed is requested. If the Scene UI modules cannot be imported, the scripts log a warning and continue without PiP; task-owned cameras and recording observations remain unchanged. This keeps -the camera selection configuration usable when a future kitless entry point no longer provides -Scene UI. Render-product tuning is also best-effort: backends without a compatible render product +the camera selection configuration usable without Scene UI. The +:ref:`kitless XR guide ` uses separate OpenXR quad +layers and does not import Scene UI; those layers are visible in the headset/client, not in the +passive eye-image desktop mirror. Render-product tuning is also best-effort: backends without a compatible render product keep the Camera-buffer path, and schema-authoring failures warn while PiP continues. Other configuration, camera-buffer, and panel-initialization errors still fail during startup. diff --git a/docs/source/how-to/cloudxr_teleoperation.rst b/docs/source/how-to/cloudxr_teleoperation.rst index f80695e34da7..79b7aac0cfcf 100644 --- a/docs/source/how-to/cloudxr_teleoperation.rst +++ b/docs/source/how-to/cloudxr_teleoperation.rst @@ -145,9 +145,10 @@ to capture demonstrations, and ``replay`` to play a dataset back. :ref:`The next .. note:: - ``teleop`` cannot be combined with ``ov`` or ``ovphysx`` in a single ``uv run``: the - bundled Isaac Sim pins ``packaging==26.0`` while those runtimes require ``<24``. Install - the OV runtimes separately when you need them. + For XR teleoperation without Isaac Sim, follow the + :ref:`kitless XR guide `. It explains how to select a source-built + Isaac Capture wheel and its optional OVXR dependencies. The Kit setup below + uses the published ``teleop`` extra. Complete these steps first: @@ -695,10 +696,9 @@ Launch a teleoperation session paired with a hand-tracking task, as shown in .. important:: - Manus gloves and other external push-device peripherals require - ``NV_CXR_ENABLE_PUSH_DEVICES=1``. The shipped ``.env`` profiles set this to ``0`` - (optimised for headset optical hand tracking). Create a custom ``.env`` file with the value set - to ``1`` and pass it via ``--cloudxr_env`` instead of the plain command above: + The shipped ``cloudxrjs`` profile sets ``NV_CXR_ENABLE_PUSH_DEVICES=0`` for headset optical + hands. Manus gloves and other external push-device peripherals instead require a custom + ``.env`` file with push devices enabled, passed via ``--cloudxr_env``: .. tab-set:: diff --git a/docs/source/how-to/index.rst b/docs/source/how-to/index.rst index 59c932c48299..d608f60aa7c0 100644 --- a/docs/source/how-to/index.rst +++ b/docs/source/how-to/index.rst @@ -500,6 +500,12 @@ How-to Guides :guide-tag:`PhysX` :guide-tag:`Isaac Sim` + .. container:: guide-entry + + :doc:`Source-built Capture with optional OVXR ` + + Select public OVXR providers and replay MCAP with a source-built Capture wheel. + .. container:: guide-entry :doc:`Setting up Haply teleoperation ` @@ -546,3 +552,8 @@ How-to Guides This collection is a work in progress. If a question is not answered here, open an issue on the `Isaac Lab GitHub repository `_. + +.. toctree:: + :hidden: + + kitless_xr diff --git a/docs/source/how-to/kitless_xr.md b/docs/source/how-to/kitless_xr.md new file mode 100644 index 000000000000..6fca59ee981f --- /dev/null +++ b/docs/source/how-to/kitless_xr.md @@ -0,0 +1,10 @@ + + +(kitless-xr)= + +```{include} ../../../source/isaaclab_teleop/docs/OVXR_RUNTIME.md +``` diff --git a/pyproject.toml b/pyproject.toml index b143612a98e7..5c59d631404d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -410,7 +410,8 @@ environments = [ # ``importers`` extras pin older versions of these packages, so the overrides prevent downgrades. # Direct torch-stack pins also apply to published wheels; overrides reconcile Isaac Sim's older pins. override-dependencies = [ - "numpy>=2", + # Numba requires <2.5; cmeel-boost used by teleop requires >=2.3,<2.4. + "numpy>=2.3,<2.4", "mujoco~=3.15.0", "mujoco-warp~=3.15.0", "newton[sim] @ git+https://github.com/newton-physics/newton.git@71eeba16a717d4dcf0b9dd7056a7b6f863054dc3", diff --git a/scripts/environments/teleoperation/teleop_replay_agent.py b/scripts/environments/teleoperation/teleop_replay_agent.py index 2f40beec2eca..a3462d01ac2a 100644 --- a/scripts/environments/teleoperation/teleop_replay_agent.py +++ b/scripts/environments/teleoperation/teleop_replay_agent.py @@ -7,11 +7,18 @@ This is the non-interactive counterpart to ``teleop_se3_agent.py``. It builds a teleop environment, attaches an :class:`~isaaclab_teleop.IsaacTeleopDevice` -configured in :class:`isacteleop.teleop_session_manager.SessionMode.REPLAY`, +configured in :class:`isaacteleop.teleop_session_manager.SessionMode.REPLAY`, and pumps the simulation loop until the recorded operator presses STOP (or ``--max_replay_duration_s`` elapses, or the simulator is closed). The user-journey teleop script remains ``teleop_se3_agent.py``. +OVXR viewer: + ``--ovxr`` presents replay through an independently owned OVXR session while + tracker and control input remain sourced from MCAP. Optional camera quads + use ``--ovxr_pip_camera NAME --ovxr_pip_head_locked``; desktop mirror uses + ``--ovxr_desktop_mirror``. See ``source/isaaclab_teleop/docs/OVXR_RUNTIME.md`` + for installation and launch instructions. + Inputs: ``--replay_file`` is an MCAP capture produced by Isaac Capture's ``McapRecordingConfig`` path (typically written by ``record_demos.py @@ -19,7 +26,7 @@ messages (head / hands / controllers) plus the ``_teleop_control`` ``MessageChannelTracker`` that captured the operator's START / STOP / RESET gestures. TeleopCore's - :class:`~isacteleop.deviceio_session.ReplaySession` re-emits all of + :class:`~isaacteleop.deviceio_session.ReplaySession` re-emits all of them on the same monotonic-time cadence they were recorded on, so :func:`~isaaclab_teleop.poll_control_events` returns the same edges here that ``record_demos.py``'s loop saw at recording time. @@ -64,28 +71,27 @@ Stats output: The wall-clock deltas between successive ``env.sim.render()`` calls - produced during the active window are recorded as a per-rendered-frame + produced during the active window are recorded as a per-render-call interval series (N gated renders produce N-1 samples). Pre-START render-only frames, warmup ticks, and post-quit render-only spin-down are excluded so the numbers reflect the steady-state replay workload rather than agent bookkeeping overhead. Per-run samples are summarised into mean / p50 / p90 / p95 / p99 / min / max / stddev (under ``cpu_frame_time_ms``) plus - derived FPS metrics (under ``fps``). Both blocks are reported on - a **per-render** basis -- ``cpu_frame_time_ms`` is the wall time - between rendered frames and ``fps`` is the render rate, matching - what the simulator's HUD shows and what the headset wearer / - spectator actually perceives during real-time teleop. The two + derived call-rate metrics (under the legacy ``fps`` key). Both blocks measure + successive application ``env.sim.render()`` calls, not completed renderer + frames, encoded video, or client/headset presentation. The two blocks stay self-consistent: ``fps.mean`` equals ``1000 / cpu_frame_time_ms.mean`` (harmonic mean of FPS = total frames / total step time). Each interval is the wall-clock delta between successive ``env.sim.render`` calls, so a run needs at least two - rendered frames during the active window to contribute. The agent + render calls during the active window to contribute. The agent asserts this per-run inside :func:`_run_single_replay`: if a run stepped the env but produced no usable intervals (0 or 1 renders -- typically the sim is not rendering, or the active window closed too quickly), it raises ``RuntimeError`` immediately, so the remaining replays never start and no stdout summary or JSON file - is written. Partial reports are never emitted: CI relies on + is written. OVXR owns rendering independently, so application render-call + timing statistics remain empty for OVXR replays. Partial reports are never emitted: CI relies on "no JSON output" as an unambiguous failure signal. Each active iteration also emits one ``GpuStatsProvider.sample()`` @@ -117,6 +123,8 @@ "task": "IsaacContrib-PickPlace-GR1T2-Abs", "replay_file": "/tmp/pickplace_gr1t2.mcap", "num_replays": 5, + "requested_num_replays": 5, + "batch_complete": true, "outcomes": {"success": 4, "failure": 1, "incomplete": 0, "timeout": 0}, "success_rate": 0.8, "env_cfg": { @@ -147,7 +155,7 @@ "mean": ..., "p50": ..., "p90": ..., "p95": ..., "p99": ..., "min": ..., "max": ..., "stddev": ..., "n": ... }, - "fps": { # per-render rate (matches simulator HUD) + "fps": { # application render-call rate, not headset presentation "mean": ..., "min_instantaneous": ..., "max_instantaneous": ... }, "gpu_stats": { @@ -251,8 +259,9 @@ """Parse CLI first so we can decide whether to launch Isaac Sim Kit.""" import argparse +import sys -from isaaclab.app import add_launcher_args, launch_simulation +from isaaclab.app import add_launcher_args, launch_simulation, validate_launch parser = argparse.ArgumentParser( description=( @@ -262,6 +271,13 @@ ) parser.add_argument("--task", type=str, required=True, help="Name of the task.") parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to simulate.") +parser.add_argument( + "--ovxr", action="store_true", help="Present recorded motion through kitless OVXR; input stays MCAP." +) +parser.add_argument("--ovxr_pip_camera", nargs="+", default=None, help="Task camera names to show as OVXR panels.") +parser.add_argument("--ovxr_pip_head_locked", action="store_true", help="Use the supported head-locked PiP layout.") +parser.add_argument("--ovxr_desktop_mirror", action="store_true", help="Open the optional OVUI eye mirror.") +parser.add_argument("--ovxr_session_mode", choices=("ar", "vr"), default="ar") parser.add_argument( "--replay_file", type=str, @@ -329,9 +345,9 @@ type=float, default=300.0, help=( - "Safety cap on how long to wait for omni.usd to finish loading the stage before" - " proceeding anyway. Hit only when something is misconfigured (missing asset, slow" - " Nucleus, etc.); a warning is logged and replay continues. Default is 300s." + "Safety cap for initial stage readiness. Kit stage loading warns and continues" + " at this cap; OVXR render-worker bootstrap fails and drains on expiry." + " Default is 300s." ), ) parser.add_argument( @@ -372,8 +388,10 @@ ), ) add_launcher_args(parser) -# forward unrecognized args as Hydra-style task config overrides -args_cli, hydra_overrides = parser.parse_known_args() +from isaaclab_tasks.utils import resolve_task_config, setup_preset_cli + +args_cli, hydra_overrides = setup_preset_cli(parser) +sys.argv = [sys.argv[0]] + hydra_overrides # Enable external camera rendering by default so the replay mimics the production teleop env (perf # parity with live camera rendering); ``--disable_external_cameras`` turns it off. The @@ -381,8 +399,8 @@ # the launcher; this selects a camera-rendering experience that provides RTX/DLSS. # Everywhere else we read ``args_cli.disable_external_cameras`` directly. args_cli.enable_cameras = not args_cli.disable_external_cameras -# the stage-load wait and the RTX settings use Kit APIs directly -args_cli.require_kit = True +# Let the resolved task/preset select Kit; only Kit paths use its stage-load APIs. +args_cli.require_kit = False """Rest everything follows.""" @@ -390,8 +408,8 @@ import json import logging import os +import signal import statistics -import sys import time from collections.abc import Sequence from contextlib import contextmanager @@ -410,17 +428,20 @@ create_isaac_teleop_device, poll_control_events, ) +from isaaclab_teleop.action_policy import initial_hand_tracking_issue, prepare_teleop_action, prepare_teleop_backend from isaaclab.devices.openxr import remove_camera_configs from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.envs.utils._manual_success import _prepare_success_term +from isaaclab.utils.version import has_kit import isaaclab_tasks # noqa: F401 -from isaaclab_tasks.utils import parse_env_cfg logger = logging.getLogger(__name__) # Resolve CLI profile names once; runtime construction receives a path or None. +if args_cli.ovxr and args_cli.cloudxr_env is None: + args_cli.cloudxr_env = "cloudxrjs" if args_cli.cloudxr_env is not None: if not args_cli.cloudxr_env.strip() or args_cli.cloudxr_env.lower() == "none": args_cli.cloudxr_env = None @@ -446,7 +467,7 @@ class _RunStats: """Per-replay performance + outcome record. - ``active_frame_times_ms`` is the per-rendered-frame series: wall-clock + ``active_frame_times_ms`` is the per-render-call series: wall-clock deltas between successive :meth:`SimulationContext.render` calls produced from inside ``env.step`` during the active window (post-START, pre-terminator). Pre-START render-only frames, warmup @@ -469,6 +490,8 @@ class _RunStats: outcome: str = "incomplete" # "success" | "failure" | "incomplete" | "timeout" active_frame_times_ms: list[float] = field(default_factory=list) active_iterations: int = 0 + rejected_action_frames: int = 0 + last_action_rejection: str | None = None active_render_calls: int = 0 active_duration_s: float = 0.0 success_step_count: int = 0 # final consecutive-success counter at terminator @@ -482,6 +505,8 @@ def to_dict(self, run_index: int) -> dict: "run_index": run_index, "outcome": self.outcome, "active_iterations": self.active_iterations, + "rejected_action_frames": self.rejected_action_frames, + "last_action_rejection": self.last_action_rejection, "active_duration_s": round(self.active_duration_s, 6), "success_step_count": self.success_step_count, "cpu_frame_time_ms": _compute_frame_stats(self.active_frame_times_ms), @@ -545,7 +570,7 @@ def _compute_frame_stats(samples_ms: list[float]) -> dict: def _compute_fps_stats(samples_ms: list[float]) -> dict: - """Compute FPS stats from per-rendered-frame intervals (in ms). + """Compute render-call rate stats from application intervals (in ms). All three fields are derived from the same ``samples_ms`` series that feeds ``cpu_frame_time_ms`` and stay self-consistent with it: @@ -556,7 +581,7 @@ def _compute_fps_stats(samples_ms: list[float]) -> dict: ``n / active_duration_s`` (dragged down by inter-step bookkeeping). ``min_instantaneous`` and ``max_instantaneous`` are derived from - the slowest / fastest individual rendered frame respectively. + the slowest / fastest individual render-call interval respectively. """ n = len(samples_ms) if n == 0: @@ -798,6 +823,8 @@ def _build_report(args, env_cfg, all_runs: list[_RunStats]) -> dict: "task": args.task, "replay_file": args.replay_file, "num_replays": len(all_runs), + "requested_num_replays": args.num_replays, + "batch_complete": len(all_runs) == args.num_replays, "outcomes": outcomes_count, "success_rate": success_rate, "env_cfg": env_perf_cfg, @@ -978,8 +1005,7 @@ def _maybe_launch_cloudxr(cloudxr_env_path: str | None, auto_launch: bool): Mirrors the gating in :meth:`TeleopSessionLifecycle._ensure_cloudxr_runtime` (``--cloudxr_env`` set, ``--auto_launch_cloudxr`` enabled, ``ISAACLAB_CXR_SKIP_AUTOLAUNCH=1`` env var not set) so behavior parity - is preserved, including ``ISAACLAB_CXR_ACCEPT_EULA=1`` for the CloudXR - license, which is read through the same shared helper. + is preserved. Args: cloudxr_env_path: Resolved CloudXR ``.env`` file path, or ``None`` @@ -999,13 +1025,12 @@ def _maybe_launch_cloudxr(cloudxr_env_path: str | None, auto_launch: bool): logger.info("CloudXR auto-launch skipped (ISAACLAB_CXR_SKIP_AUTOLAUNCH=1)") return None - from isaaclab_teleop import cloudxr_eula_accepted from isaacteleop.cloudxr import CloudXRLauncher launcher = CloudXRLauncher( install_dir=str(Path.home() / ".cloudxr"), env_config=cloudxr_env_path, - accept_eula=cloudxr_eula_accepted(), + accept_eula=False, ) logger.info("CloudXR runtime launched (process-scoped, shared across replays)") return launcher @@ -1049,13 +1074,17 @@ def _prepare_env_cfg( task: Registered task name to load the env config for. num_envs: Number of parallel environments. device: Simulation device (e.g. ``"cuda:0"``). - overrides: Hydra-style ``key=value`` overrides forwarded to :func:`parse_env_cfg`. + overrides: Hydra-style ``key=value`` overrides passed to :func:`resolve_task_config`. Returns: Tuple ``(env_cfg, success_term)``. ``success_term`` is ``None`` when the env doesn't define a ``success`` termination term. """ - env_cfg = parse_env_cfg(task, device=device, num_envs=num_envs, overrides=overrides) + overrides = list(overrides) + env_cfg, _ = resolve_task_config(task, "", overrides=overrides) + if device is not None: + env_cfg.sim.device = device + env_cfg.scene.num_envs = num_envs env_cfg.env_name = task.split(":")[-1] if not isinstance(env_cfg, ManagerBasedRLEnvCfg): raise ValueError( @@ -1072,27 +1101,77 @@ def _prepare_env_cfg( env_cfg.terminations.time_out = None # Keep camera configs when external cameras are enabled (defaulted on) so the replay # renders them for production parity; otherwise strip them for a lighter headless replay. - if args_cli.disable_external_cameras: + if args_cli.ovxr: + if args_cli.num_envs != 1 or args_cli.xr: + raise ValueError("--ovxr requires one environment and no Kit XR or local visualizer") + if os.environ.setdefault("ISAAC_LAB_OVRTX_USE_OVSTAGE", "1").strip() != "1": + raise ValueError("--ovxr requires ISAAC_LAB_OVRTX_USE_OVSTAGE=1") + from isaaclab_ov.renderers import OVRTXRendererCfg + + from isaaclab.sensors import CameraCfg + + names = args_cli.ovxr_pip_camera or [] + if names and args_cli.disable_external_cameras: + raise ValueError("OVXR PiP conflicts with --disable_external_cameras") + if len(names) > 3 or len(names) != len(set(names)): + raise ValueError("Select up to three unique OVXR PiP cameras") + if names and not args_cli.ovxr_pip_head_locked: + raise ValueError("OVXR replay PiP requires --ovxr_pip_head_locked") + renderer_cfg = OVRTXRendererCfg() + for name in names: + camera = getattr(env_cfg.scene, name, None) + feeds = [feed for feed in env_cfg.isaac_teleop.xr_camera_feeds if feed.enabled and feed.camera_name == name] + if not isinstance(camera, CameraCfg) or len(feeds) != 1: + raise ValueError(f"OVXR camera {name!r} requires a CameraCfg and one enabled XrCameraFeedCfg") + camera.renderer_cfg = renderer_cfg + env_cfg = remove_camera_configs(env_cfg, keep=tuple(names)) + elif args_cli.ovxr_pip_camera or args_cli.ovxr_desktop_mirror or args_cli.ovxr_pip_head_locked: + raise ValueError("OVXR presentation options require --ovxr") + elif args_cli.disable_external_cameras: env_cfg = remove_camera_configs(env_cfg) - return env_cfg, success_term + if env_cfg.sim.physics is None: + from isaaclab.physics import PhysicsCfg + + env_cfg.sim.physics = PhysicsCfg() + configs, kit_sources = validate_launch(env_cfg, vars(args_cli).copy()) + if args_cli.ovxr: + from isaaclab.visualizers import VisualizerBackendCfg + + if kit_sources or any(isinstance(cfg, VisualizerBackendCfg) for cfg in configs): + raise ValueError("--ovxr requires no Kit runtime or local visualizer") + args_cli.require_kit = bool(kit_sources) + prepare_teleop_backend(env_cfg, env_cfg.sim.physics) + return (env_cfg, success_term) + + +def _replay_requires_hard_sim_reset(physics_backend: str | None) -> bool: + """Return whether replay must rebuild the simulator before an environment reset.""" + return physics_backend is None or not physics_backend.startswith("newton") + + +def _reset_replay_environment(env: object, *, hard_sim_reset: bool, reset_recorder: bool) -> None: + """Reset replay-owned environment state in backend-safe order.""" + if hard_sim_reset: + env.sim.reset() + if reset_recorder: + env.recorder_manager.reset() + env.reset() -def _handle_reset(env: gym.Env, teleop_interface: IsaacTeleopDevice) -> None: +def _handle_reset(env: gym.Env, teleop_interface: IsaacTeleopDevice, *, hard_sim_reset: bool) -> None: """Run the full env+teleop reset cycle used by ``record_demos.py``. Mirrors :func:`scripts.tools.record_demos.handle_reset` (sans the instruction-display update, which the headless replay agent doesn't - own). ``env.sim.reset()`` does the hard physics reinit that keeps Pink - IK seeded against fresh articulation views; see the initial-reset note - in :func:`main`. ``env.recorder_manager.reset()`` is a no-op when no - recorders are configured (the default for this script), but kept for - parity with record_demos.py so future recorder additions don't have to - re-derive the call sequence. + own). The optional hard physics reset keeps Pink seeded against fresh + articulation views on PhysX backends. Newton instead retains the solver + constructed before replay. ``env.recorder_manager.reset()`` is a no-op + when no recorders are configured (the default for this script), but kept + for parity with record_demos.py so future recorder additions don't have + to re-derive the call sequence. """ print("Resetting environment...") - env.sim.reset() - env.recorder_manager.reset() - env.reset() + _reset_replay_environment(env, hard_sim_reset=hard_sim_reset, reset_recorder=True) teleop_interface.reset() @@ -1125,22 +1204,147 @@ def _process_success_condition( _RENDERER_SETTLE_FRAMES: int = 30 -"""Number of additional render frames pumped after the USD stage finishes loading. +"""Additional Kit render ticks after asynchronous USD asset loading. -The stage-load status flips to ``count_loading == 0`` as soon as every referenced asset -has been resolved, but the renderer pipeline (shader compilation, articulation-view -binding, material warm-up) typically needs a few more event-loop ticks to converge. Thirty -frames at the default render cadence is ~0.5 s on most machines and is deterministic -per-machine -- unlike a wall-clock delay it does not have to be tuned for hardware. +This legacy Kit warmup is not a kitless renderer-completion or FPS guarantee. """ +class _ReplayPresentation: + """Own a batch's viewer independently of each recorded-input session.""" + + def __init__(self, env_cfg): + from isaaclab_ov.renderers import OVRTXRendererCfg + from isaaclab_teleop import OvLibrariesXrConfig, OvLibrariesXrSession + from isaaclab_teleop.camera_feed import resolve_static_panel_pose + from isaaclab_teleop.isaac_teleop_cfg import XrCameraFeedLayoutCfg + from isaaclab_teleop.ov_libraries_xr import OvLibrariesXrCameraConfig + + cameras = [] + for name in args_cli.ovxr_pip_camera or []: + feed = next( + feed for feed in env_cfg.isaac_teleop.xr_camera_feeds if feed.enabled and feed.camera_name == name + ) + space, position, orientation = resolve_static_panel_pose( + feed, XrCameraFeedLayoutCfg(placement="head_locked") + ) + cameras.append( + OvLibrariesXrCameraConfig( + name=name, + camera_cfg=getattr(env_cfg.scene, name), + max_fps=min(30.0, feed.max_update_hz) if feed.max_update_hz > 0 else 30.0, + panel_width_m=feed.panel_width_m, + position=position, + orientation_xyzw=orientation, + space=space, + ) + ) + self.session = OvLibrariesXrSession.prepare( + env_cfg.sim, + OvLibrariesXrConfig( + application_name=f"isaaclab-ovxr-replay-{args_cli.task}", + session_mode=args_cli.ovxr_session_mode, + cloudxr_env_file=args_cli.cloudxr_env, + auto_launch_cloudxr=args_cli.auto_launch_cloudxr, + desktop_mirror_enabled=args_cli.ovxr_desktop_mirror, + auxiliary_cameras=tuple(cameras), + ), + cameras[0].camera_cfg.renderer_cfg if cameras else OVRTXRendererCfg(), + ) + self.xr_cfg = env_cfg.isaac_teleop.xr_cfg + self.device = None + self.started = False + self.reset_generation = 0 + self.pending = None + self.force = False + + def start(self, env, device): + if not self.started: + if not self.session.start_render_worker( + env, + tracking_anchor_path=device.xr_render_anchor_path, + hidden_visual_prims=self.xr_cfg.hidden_visual_prims, + excluded_visual_meshes=self.xr_cfg.excluded_visual_meshes, + external_anchor_state=device.external_anchor_state, + timeout_s=args_cli.max_stage_load_wait_s, + ): + raise RuntimeError("OVXR viewer shut down before replay startup") + self.started = True + else: + self.session.set_external_anchor_state(device.external_anchor_state) + self.publish(env, changed=True, reset=True) + + def publish(self, env, *, changed=False, reset=False): + if reset: + self.reset_generation += 1 + self.force = True + if changed or reset: + self.pending = (self.reset_generation, env.sim.get_physics_step_count()) + if self.pending is not None and self.session.request_render(self.pending, force=self.force) is not None: + self.pending = None + self.force = False + self.session.report_host_timing(env.sim.get_physics_step_count(), env.sim.get_physics_dt()) + + def close(self): + if self.device is not None and self.device.session_cleanup_failed: + raise RuntimeError("Replay input did not drain; retaining its borrowed environment") + self.session.close() + + +def _warmup_replay(env, run_index: int, presentation) -> None: + """Wait for Kit assets or pump the independently owned viewer before input.""" + # Only an active Kit application owns the asynchronous USD loader. + # Kitless environments finish their stage construction before this call. + if run_index == 0: + if has_kit(): + from isaaclab_physx.renderers.isaac_rtx_renderer_utils import wait_for_stage_load + + print("Waiting for USD stage to finish loading...") + wait_for_stage_load(args_cli.max_stage_load_wait_s, settle_frames=_RENDERER_SETTLE_FRAMES) + + # Optional extra wall-clock buffer on top of the deterministic + # wait. Useful as an escape hatch when the deterministic check + # is not enough (e.g. very slow shader compilation paths). + if args_cli.replay_start_delay_s > 0: + print( + f"Additional warmup buffer: rendering for {args_cli.replay_start_delay_s:.1f}s" + " before consuming MCAP frames." + ) + buffer_start_s = time.monotonic() + while env.sim.is_running() and time.monotonic() - buffer_start_s < args_cli.replay_start_delay_s: + if presentation is None: + env.sim.render() + else: + if presentation.session.shutdown_requested: + raise RuntimeError("OVXR viewer closed before replay started") + presentation.publish(env) + time.sleep(0.01) + + +def _advance_replay_input(device, cfg, presentation): + """Read recorded input using the viewer owner's target transform when needed.""" + target_path = cfg.target_frame_prim_path if presentation is not None else None + if target_path is None: + return device.advance() + return device.advance(target_T_world=presentation.session.get_target_frame_T_world(target_path)) + + +def _render_replay_idle(env, presentation) -> None: + """Keep the selected presentation responsive without advancing physics.""" + if presentation is None: + env.sim.render() + else: + presentation.publish(env) + time.sleep(env.step_dt) + + def _run_single_replay( env: gym.Env, isaac_teleop_cfg, success_term: object | None, run_index: int, total_runs: int, + presentation: _ReplayPresentation | None = None, ) -> _RunStats: """Run a single replay pass against ``env`` and return the per-run stats. @@ -1190,13 +1394,16 @@ def _run_single_replay( # its IPC connection. teleop_interface = create_isaac_teleop_device( isaac_teleop_cfg, - sim_device=args_cli.device, + sim_device=env.sim.device, callbacks={}, cloudxr_env_file=None, auto_launch_cloudxr=False, mcap_replay_path=args_cli.replay_file, enable_debug_visualization=args_cli.enable_debug_visualization, + ovxr_session=presentation is not None, ) + if presentation is not None: + presentation.device = teleop_interface if run_index == 0: print(f"Using teleop device: {teleop_interface}") @@ -1228,45 +1435,29 @@ def _patched_sim_render(): env.sim.render = _orig_sim_render with _patched_sim_render(), teleop_interface: - # Mirror the reset sequence used by ``record_demos.py``: ``sim.reset()`` - # does a hard physics reinit (re-binds articulation views, plays the - # timeline) that ``env.reset()`` alone does not perform. Pink IK reads - # ``data.joint_pos.torch`` every step to seed Pinocchio's configuration - # and to compute ``target = curr + delta``; if the articulation view is - # stale, every IK call produces zero-delta arm targets while the - # hand-finger path (which bypasses IK) keeps tracking. See PR #5507. - env.sim.reset() - env.reset() + # PhysX backends need the hard reset that refreshes articulation views + # before Pink reads them (PR #5507). Newton's constructed solver must + # instead remain alive across the ordinary environment reset. + _reset_replay_environment( + env, + hard_sim_reset=_replay_requires_hard_sim_reset(env.scene.physics_backend), + reset_recorder=False, + ) teleop_interface.reset() - - # Deterministic warmup is only required on the first run -- once the - # stage has been streamed in and the renderer settled, subsequent - # runs share the same stage and only need the per-run ``env.sim.reset`` - # above. Skipping it on later runs saves the renderer-settle frames - # at the cost of doing nothing measurable (the wait returns - # immediately on a fully-loaded stage anyway). - if run_index == 0: - from isaaclab_physx.renderers.isaac_rtx_renderer_utils import wait_for_stage_load - - print("Waiting for USD stage to finish loading...") - wait_for_stage_load(args_cli.max_stage_load_wait_s, settle_frames=_RENDERER_SETTLE_FRAMES) - - # Optional extra wall-clock buffer on top of the deterministic - # wait. Useful as an escape hatch when the deterministic check - # is not enough (e.g. very slow shader compilation paths). - if args_cli.replay_start_delay_s > 0: - print( - f"Additional warmup buffer: rendering for {args_cli.replay_start_delay_s:.1f}s" - " before consuming MCAP frames." - ) - buffer_start_s = time.monotonic() - while env.sim.is_running() and time.monotonic() - buffer_start_s < args_cli.replay_start_delay_s: - env.sim.render() + # Replay has no headset client to send START. Match record_demos.py's + # canonical non-XR lifecycle: its host reset is a pulse and the local + # start command drives the control graph into RUNNING for the capture. + teleop_interface.request_start() + if presentation is not None: + presentation.start(env, teleop_interface) + _warmup_replay(env, run_index, presentation) print( f"Replay {run_index + 1}/{total_runs} started; replaying MCAP from {args_cli.replay_file}" f" (max_replay_duration_s={args_cli.max_replay_duration_s:.1f})." ) + action_state: dict = {} + initialized_hand_tracking: set[str] = set() teleop_active = False teleop_was_active = False # only terminate on STOP after a real START success_step_count = 0 @@ -1297,7 +1488,11 @@ def _patched_sim_render(): stats.outcome = "timeout" break - action = teleop_interface.advance() + if presentation is not None and presentation.session.shutdown_requested: + print("OVXR viewer requested shutdown before replay completed.") + stats.outcome = "incomplete" + break + action = _advance_replay_input(teleop_interface, isaac_teleop_cfg, presentation) ctrl = poll_control_events(teleop_interface) # Track active state from the replayed _teleop_control @@ -1329,24 +1524,56 @@ def _patched_sim_render(): break if ctrl.should_reset: - _handle_reset(env, teleop_interface) + action_state.clear() + initialized_hand_tracking.clear() + _handle_reset( + env, + teleop_interface, + hard_sim_reset=_replay_requires_hard_sim_reset(env.scene.physics_backend), + ) + # _handle_reset() intentionally performs a host reset + # (pause=False). Reassert the canonical headset-free + # start so a recorded reset cannot leave replay paused. + teleop_interface.request_start() success_step_count = 0 + if presentation is not None: + presentation.publish(env, changed=True, reset=True) continue # Gate stepping on the active state (mirrors # teleop_se3_agent.py:309-328). Pre-START operator # setup frames render only; the recorded START flips # us into the stepping branch. + if not teleop_active: + action_state.clear() if action is None or not teleop_active: - env.sim.render() + _render_replay_idle(env, presentation) + continue + + issue = initial_hand_tracking_issue( + teleop_interface, + initialized_hand_tracking, + required=getattr(isaac_teleop_cfg, "require_initial_bimanual_hand_tracking", False), + ) + if issue is None: + action, issue = prepare_teleop_action( + env, + action, + isaac_teleop_cfg.action_conditioner, + isaac_teleop_cfg.action_validator, + action_state, + ) + if issue is not None: + stats.rejected_action_frames += 1 + stats.last_action_rejection = issue + _render_replay_idle(env, presentation) continue # Gate the render shim to this env.step call so # only renders fired from within it contribute # interval samples. iter_start_s = time.perf_counter() - if active_start_s is None: - active_start_s = iter_start_s + active_start_s = iter_start_s if active_start_s is None else active_start_s actions = action.repeat(env.num_envs, 1) sampling_active[0] = True try: @@ -1355,6 +1582,8 @@ def _patched_sim_render(): sampling_active[0] = False last_active_end_s = time.perf_counter() stats.active_iterations += 1 + if presentation is not None: + presentation.publish(env, changed=True) # Snapshot GPU state right after env.step so the # sample reflects the active workload (post-render @@ -1376,7 +1605,7 @@ def _patched_sim_render(): # meaningful replay. End the run with a failure # outcome so the batch's exit code reflects it. if bool(terminated.any().item()) or bool(truncated.any().item()): - print("Replay failure: env terminated/truncated mid-trajectory; ending run.") + print("Replay environment terminated; ending run.") stats.outcome = "failure" break @@ -1425,7 +1654,8 @@ def _patched_sim_render(): # Fail loud before returning so the next replay never starts and # the batch produces no partial report. See _assert_run_measured. - _assert_run_measured(stats, run_index) + if presentation is None: + _assert_run_measured(stats, run_index) return stats @@ -1439,10 +1669,11 @@ def main() -> int: multi-run batches start essentially instantly. Per-replay control flow (see :func:`_run_single_replay` for details): - * Pre-loop warmup: ``wait_for_stage_load`` polls + * First-run Kit warmup: ``wait_for_stage_load`` polls ``omni.usd.UsdContext.get_stage_loading_status`` until the simulator reports zero pending assets, then renders a fixed number of - settle frames (only on ``run_index == 0``). An optional + settle frames only when Kit is active and ``run_index == 0``. + Kitless runs do not import Kit's asset loader. An optional ``--replay_start_delay_s`` buffer can be appended for hardware that needs more grace. ``advance()`` is not called during warmup so ``ReplaySession.update`` does not consume MCAP @@ -1490,16 +1721,18 @@ def main() -> int: persisted to disk; a one-line-per-run stdout summary is always printed. See the module docstring for the full JSON schema. - Resource cleanup is wrapped in a ``try/finally`` so that ``env.close()`` - always runs, even when device construction or any subsequent setup - raises -- otherwise the USD stage would leak across CI runs. + Cleanup drains replay input before the optional OVXR owner releases its + borrowed environment. Without that owner the batch closes the environment + directly. Failed input drain retains borrowed resources and raises rather + than releasing a stage that a worker may still use. Repeated shutdown signals + request owner shutdown without interrupting its drain. Returns: - The host process exit code, mapped from the worst-of-N outcome - across the multi-run batch: ``0`` if every run's - ``success_term`` fired, ``2`` if any run hit - ``--max_replay_duration_s``, otherwise ``1`` (any failure or - incomplete run). + ``1`` if the requested batch did not finish. For a complete batch, + return the worst-of-N outcome: ``0`` if every run met task success, + ``2`` if any run hit + ``--max_replay_duration_s``, otherwise ``1`` (any failure or incomplete + run). The report retains each completed run's outcome in either case. """ if args_cli.num_replays < 1: raise ValueError(f"--num_replays must be >= 1; got {args_cli.num_replays}") @@ -1517,7 +1750,7 @@ def main() -> int: "MCAP replay requires env_cfg.isaac_teleop to be set." ) - with launch_simulation(env_cfg, args_cli): + with launch_simulation(env_cfg, args_cli, cleanup_context=not args_cli.ovxr): exit_code = _run_replay_batch(env_cfg, success_term) return exit_code @@ -1528,8 +1761,14 @@ def _run_replay_batch(env_cfg: ManagerBasedRLEnvCfg, success_term: object | None Returns: The host process exit code, see :func:`main`. """ + from isaaclab.sim import SimulationContext + + previous_context = SimulationContext.instance() env: gym.Env | None = None cloudxr_launcher = None + presentation = None + previous_handlers = {} + shutting_down = False all_runs: list[_RunStats] = [] try: @@ -1540,9 +1779,24 @@ def _run_replay_batch(env_cfg: ManagerBasedRLEnvCfg, success_term: object | None # ``_run_single_replay``). This is what lets ``--num_replays > 1`` # work in XR-active mode without losing the OpenXR runtime # IPC between runs. - cloudxr_launcher = _maybe_launch_cloudxr(args_cli.cloudxr_env, args_cli.auto_launch_cloudxr) - - if not args_cli.disable_external_cameras or args_cli.kit_visualizer or args_cli.xr: + if args_cli.ovxr: + presentation = _ReplayPresentation(env_cfg) + + def request_shutdown(_signum, _frame): + nonlocal shutting_down + if not presentation.started and not shutting_down: + # Begin normal unwinding during construction or peer-gated + # startup; subsequent signals must not interrupt owner drain. + shutting_down = True + raise KeyboardInterrupt + shutting_down = True + presentation.session.request_shutdown() + + previous_handlers = {sig: signal.signal(sig, request_shutdown) for sig in (signal.SIGINT, signal.SIGTERM)} + else: + cloudxr_launcher = _maybe_launch_cloudxr(args_cli.cloudxr_env, args_cli.auto_launch_cloudxr) + + if args_cli.require_kit and (not args_cli.disable_external_cameras or args_cli.kit_visualizer or args_cli.xr): from isaaclab_physx.renderers.isaac_rtx_renderer_utils import apply_isaac_rtx_global_settings apply_isaac_rtx_global_settings(IsaacRtxRendererGlobalSettingsCfg(antialiasing_mode="DLSS")) @@ -1556,18 +1810,33 @@ def _run_replay_batch(env_cfg: ManagerBasedRLEnvCfg, success_term: object | None success_term=success_term, run_index=run_idx, total_runs=args_cli.num_replays, + presentation=presentation, ) all_runs.append(run_stats) print(f"Replay {run_idx + 1}/{args_cli.num_replays} outcome: {run_stats.outcome}") - if not env.sim.is_running(): + if not env.sim.is_running() or (presentation is not None and presentation.session.shutdown_requested): # The simulator was closed externally mid-batch; stop the # outer loop rather than spawning a fresh device against # a dead app. break finally: - if env is not None: - env.close() - print("Environment closed") + shutting_down = True + try: + if presentation is not None: + presentation.close() + if env is not None: + if presentation is None or not presentation.session.async_worker_owns_environment: + env.close() + print("Environment closed") + elif presentation is None or not presentation.session.async_worker_owns_environment: + # Construction may have created a context before returning env. + # Only run after proven input/viewer drain; preserve older contexts. + context = SimulationContext.instance() + if context is not None and context is not previous_context: + SimulationContext.clear_instance() + finally: + for sig, handler in previous_handlers.items(): + signal.signal(sig, handler) if cloudxr_launcher is not None: try: cloudxr_launcher.stop() @@ -1579,6 +1848,8 @@ def _run_replay_batch(env_cfg: ManagerBasedRLEnvCfg, success_term: object | None _print_stdout_summary(report) if args_cli.stats_output_file is not None: _write_json_report(args_cli.stats_output_file, report) + if len(all_runs) != args_cli.num_replays: + return 1 return _exit_code_for_outcomes(all_runs) diff --git a/scripts/environments/teleoperation/teleop_se3_agent.py b/scripts/environments/teleoperation/teleop_se3_agent.py index c6f94f292381..3faae41021c4 100644 --- a/scripts/environments/teleoperation/teleop_se3_agent.py +++ b/scripts/environments/teleoperation/teleop_se3_agent.py @@ -16,7 +16,7 @@ The script automatically detects which stack to use based on the environment config. """ -"""Parse CLI first so we can decide whether to launch Isaac Sim Kit.""" +"""Resolve the task before launching its selected Kit or OVXR runtime.""" # Isaac Lab does not use Warp autodiff; skipping adjoint codegen roughly halves the # time spent building kernels on a cold kernel cache. @@ -25,14 +25,18 @@ wp.config.enable_backward = False import argparse +import math +import os +import signal import sys +import time from collections.abc import Callable -from contextlib import ExitStack -from isaaclab.app import add_launcher_args, launch_simulation +from isaaclab.app import add_launcher_args, launch_simulation, validate_launch from isaaclab.utils.string import string_to_callable -from isaaclab_tasks.utils import setup_preset_cli +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.utils import resolve_task_config, setup_preset_cli # add argparse arguments parser = argparse.ArgumentParser(description="Teleoperation for Isaac Lab environments.") @@ -49,6 +53,12 @@ ) parser.add_argument("--task", type=str, default=None, help="Name of the task.") parser.add_argument("--sensitivity", type=float, default=1.0, help="Sensitivity factor.") +parser.add_argument( + "--step_hz", + type=float, + default=0.0, + help="Maximum host-control rate with --ovxr; 0 leaves active control unthrottled. XR rendering is independent.", +) parser.add_argument( "--cloudxr_env", type=str, @@ -56,7 +66,7 @@ help=( "Path to a CloudXR .env file, or a shorthand: 'cloudxrjs' (Quest/Pico), 'avp' (Apple Vision Pro)," " or 'standalone' (headless, no XR client). Set to 'none' to disable CloudXR auto-launch entirely." - " When unset, defaults to 'cloudxrjs' with --xr and 'standalone' without --xr." + " When unset, defaults to 'cloudxrjs' with --xr or --ovxr and 'standalone' otherwise." ), ) parser.add_argument( @@ -86,19 +96,99 @@ " and improve XR performance)." ), ) +parser.add_argument( + "--ovxr", + dest="using_ovxr", + action="store_true", + default=False, + help="Use the OVXR OVRTX/OVStage/OVXR path instead of Kit XR.", +) +parser.add_argument( + "--ovxr_session_mode", + choices=("ar", "vr"), + default="ar", + help="OVXR session mode. Defaults to 'ar'; pass 'vr' for an opaque background.", +) +parser.add_argument( + "--ovxr_bootstrap_frames", + type=int, + default=1, + help="Bounded blank OVXR frames used to expose shared OpenXR handles before the main loop.", +) +parser.add_argument( + "--ovxr_worker_timeout", + type=float, + default=180.0, + help="Bounded render-worker bootstrap timeout in seconds (default: 180 for cold RTX startup).", +) +parser.add_argument( + "--ovxr_desktop_mirror", + action="store_true", + default=False, + help="Show completed eye or camera images in an optional local OVUI window.", +) +parser.add_argument( + "--ovxr_desktop_mirror_fps", + type=float, + default=30.0, + help="Maximum desktop-mirror update rate (default: 30 Hz).", +) +parser.add_argument( + "--ovxr_pip_camera", + type=str, + nargs="+", + default=None, + help="Show up to three task-configured cameras as application-owned XR panels.", +) +parser.add_argument( + "--ovxr_pip_fps", + type=float, + default=30.0, + help="Maximum OVXR PiP update rate (default: 30 Hz).", +) +parser.add_argument( + "--ovxr_pip_head_locked", + action="store_true", + help="Place the application camera quad in the OpenXR VIEW space.", +) +parser.add_argument( + "--ovxr_pip_view_position", + type=float, + nargs=3, + metavar=("X", "Y", "Z"), + help="Optional VIEW-space camera panel center in meters; requires --ovxr_pip_head_locked.", +) + -# append simulation launcher cli args +# append shared launcher CLI args add_launcher_args(parser) # parse the arguments args_cli, hydra_args = setup_preset_cli(parser) - -# Enable external camera rendering by default (``--disable_external_cameras`` turns it off). The -# ``--enable_cameras`` CLI flag was removed in Isaac Lab 3.0 (see #6656), so pass the intent to -# the launcher; this selects a camera-rendering experience that provides RTX/DLSS support. -# Everywhere else we read ``args_cli.disable_external_cameras`` directly. -args_cli.enable_cameras = not args_cli.disable_external_cameras -# the teleop input devices and the RTX settings use Kit APIs directly -args_cli.require_kit = True +if args_cli.xr and args_cli.using_ovxr: + parser.error("--xr and --ovxr are mutually exclusive OpenXR frame-loop owners") +if not math.isfinite(args_cli.step_hz) or args_cli.step_hz < 0.0: + parser.error("--step_hz must be finite and non-negative; 0 disables active-control pacing") +if args_cli.step_hz > 0.0 and not args_cli.using_ovxr: + parser.error("--step_hz requires --ovxr") +if args_cli.ovxr_bootstrap_frames < 0: + parser.error("--ovxr_bootstrap_frames must be non-negative") +if args_cli.ovxr_worker_timeout <= 0: + parser.error("--ovxr_worker_timeout must be positive") +if args_cli.ovxr_desktop_mirror and not args_cli.using_ovxr: + parser.error("--ovxr_desktop_mirror requires --ovxr") +if args_cli.ovxr_pip_camera is not None and not args_cli.using_ovxr: + parser.error("--ovxr_pip_camera requires --ovxr") +if args_cli.ovxr_pip_camera is not None and args_cli.disable_external_cameras: + parser.error("--ovxr_pip_camera cannot be combined with --disable_external_cameras") +if not math.isfinite(args_cli.ovxr_pip_fps) or args_cli.ovxr_pip_fps <= 0.0: + parser.error("--ovxr_pip_fps must be finite and positive") +if args_cli.ovxr_pip_view_position is not None: + if not args_cli.ovxr_pip_head_locked or args_cli.ovxr_pip_camera is None: + parser.error("--ovxr_pip_view_position requires a head-locked PiP camera") + if not all(math.isfinite(value) for value in args_cli.ovxr_pip_view_position): + parser.error("--ovxr_pip_view_position must be finite") +if not math.isfinite(args_cli.ovxr_desktop_mirror_fps) or args_cli.ovxr_desktop_mirror_fps <= 0: + parser.error("--ovxr_desktop_mirror_fps must be finite and positive") # Call an external callback if requested. if args_cli.external_callback: @@ -114,25 +204,25 @@ import gymnasium as gym import torch -from isaaclab_physx.renderers import IsaacRtxRendererGlobalSettingsCfg from isaaclab_teleop import CLOUDXR_AVP_ENV, CLOUDXR_JS_ENV, CLOUDXR_STANDALONE_ENV +from isaaclab_teleop.action_policy import ( + initial_hand_tracking_issue as _required_hand_tracking_issue, +) +from isaaclab_teleop.action_policy import prepare_teleop_action, prepare_teleop_backend from isaaclab.devices import Se3GamepadCfg, Se3KeyboardCfg, Se3SpaceMouseCfg -from isaaclab.devices.openxr import remove_camera_configs from isaaclab.devices.teleop_device_factory import create_teleop_device from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.utils import instantiate -import isaaclab_tasks # noqa: F401 from isaaclab_tasks.core.lift import mdp -from isaaclab_tasks.utils import resolve_task_config logger = logging.getLogger(__name__) # Resolve CLI profile names once; runtime construction receives a path or None. if args_cli.cloudxr_env is None: - args_cli.cloudxr_env = "cloudxrjs" if args_cli.xr else "standalone" + args_cli.cloudxr_env = "cloudxrjs" if args_cli.xr or args_cli.using_ovxr else "standalone" if not args_cli.cloudxr_env.strip() or args_cli.cloudxr_env.lower() == "none": args_cli.cloudxr_env = None else: @@ -143,19 +233,93 @@ }.get(args_cli.cloudxr_env.lower(), args_cli.cloudxr_env) -def main() -> None: # noqa: C901 +def _pace_ovxr_control_loop( + using_ovxr: bool, + action_applied: bool, + *, + iteration_started_s: float, + action_period_s: float, + step_hz: float = 0.0, + wait_for_shutdown: Callable[[float], bool] | None = None, +) -> None: + """Limit host control independently of the XR frame clock. + + Credit elapsed input, physics and publication work; an overrun adds no wait + or catch-up steps. With no explicit limit, active control remains unpaced + and paused polling retains the environment's action interval. """ - Run teleoperation with an Isaac Lab manipulation environment. + if not using_ovxr or (action_applied and step_hz == 0.0): + return + period_s = 1.0 / step_hz if step_hz > 0.0 else action_period_s + remaining_s = period_s - (time.perf_counter() - iteration_started_s) + if remaining_s > 0.0: + if wait_for_shutdown is None: + time.sleep(remaining_s) + else: + wait_for_shutdown(remaining_s) + + +def _rtx_rendering_requested(args: argparse.Namespace) -> bool: + """Return whether the CLI selects a renderer that actually drives RTX rendering. - Creates the environment, sets up teleoperation interfaces and callbacks, - and runs the main simulation loop until the application is closed. + The RTX/DLSS global settings are only meaningful when something renders through RTX. + That happens when the Kit visualizer is enabled (``--viz kit``), when external cameras + are rendered (on by default; see ``--disable_external_cameras``), or in XR mode (``--xr``). + A pure-headless session with none of these renders nothing. - Returns: - None + This reads the resolved namespace intent rather than any Kit/carb runtime state so the + check keeps working as these scripts grow support for other rendering backends. """ - # Resolve the task configuration through Hydra so CLI presets are applied. - env_cfg, _ = resolve_task_config(args_cli.task, "") - env_cfg.sim.device = args_cli.device + visualizers = getattr(args, "visualizer", None) or [] + external_cameras = not getattr(args, "disable_external_cameras", False) + return external_cameras or ("kit" in visualizers) or bool(getattr(args, "xr", False)) + + +def _make_haptic_io(env, teleop_interface, env_cfg, use_isaac_teleop: bool): + """Return ``(update, stop)`` callables driving controller haptics, or no-ops. + + Keeps haptics opt-in without branching in the main loop: both callables are + no-ops unless the active device is an Isaac Capture device and the env declares + a ``haptic_feedback`` config. ``update`` renders the current contact force; + ``stop`` zeroes it so a stale pulse does not persist while teleop is paused. + """ + noop = lambda: None # noqa: E731 + if not use_isaac_teleop: + return noop, noop + from isaaclab_teleop import create_haptic_feedback_driver + + driver = create_haptic_feedback_driver(env.unwrapped, teleop_interface, env_cfg) + if driver is None: + return noop, noop + return driver.update, driver.stop + + +def _apply_teleop_frame(env, action, issue: str | None, haptic_update, haptic_stop) -> bool: + """Step one valid action, or render-only while an unsafe tracker frame is held.""" + if issue is not None: + env.sim.render() + haptic_stop() + return False + env.step(action.repeat(env.num_envs, 1)) + haptic_update() + return True + + +def _ovxr_publication_identity( + physics_step_id: int, *, reset_generation: int | None = None +) -> int | tuple[str, int, int]: + """Return a dedup identity that also represents a reset at an unchanged physics step.""" + if reset_generation is None: + return physics_step_id + return ("reset", reset_generation, physics_step_id) + + +def _resolve_env_cfg() -> tuple[ManagerBasedRLEnvCfg, bool, bool, tuple[object, object] | None]: + """Resolve task configuration and teleop ownership before runtime launch.""" + overrides = list(sys.argv[1:]) + env_cfg, _ = resolve_task_config(args_cli.task, "", overrides=overrides) + if args_cli.device is not None: + env_cfg.sim.device = args_cli.device env_cfg.scene.num_envs = args_cli.num_envs env_cfg.env_name = args_cli.task if not isinstance(env_cfg, ManagerBasedRLEnvCfg): @@ -177,265 +341,582 @@ def main() -> None: # noqa: C901 use_isaac_teleop = ( not teleop_device_explicitly_set and hasattr(env_cfg, "isaac_teleop") and env_cfg.isaac_teleop is not None ) + if args_cli.using_ovxr and not use_isaac_teleop: + raise ValueError("--ovxr requires a task configured with env_cfg.isaac_teleop") + if args_cli.using_ovxr and args_cli.num_envs != 1: + raise ValueError("--ovxr currently requires --num_envs 1") + if args_cli.using_ovxr: + stage_mode = os.environ.setdefault("ISAAC_LAB_OVRTX_USE_OVSTAGE", "1") + if stage_mode.strip() != "1": + raise ValueError("--ovxr requires OVStage (ISAAC_LAB_OVRTX_USE_OVSTAGE=1)") + + pip_selection = [] + keep_cameras: tuple[str, ...] = () + if args_cli.ovxr_pip_camera is not None: + from isaaclab_ov.renderers import OVRTXRendererCfg + + from isaaclab.sensors import CameraCfg + + names = args_cli.ovxr_pip_camera + if len(names) > 3 or len(names) != len(set(names)): + raise ValueError("Select up to three unique OVXR PiP cameras") + if len(names) > 1 and args_cli.ovxr_pip_view_position is not None: + raise ValueError("A PiP position override requires one camera; multiple panels use task layout") + renderer_cfg = OVRTXRendererCfg() + for camera_name in names: + camera_cfg = getattr(env_cfg.scene, camera_name, None) + if not isinstance(camera_cfg, CameraCfg): + raise ValueError(f"OVXR PiP camera {camera_name!r} is not a configured CameraCfg") + feed_cfgs = [ + cfg for cfg in env_cfg.isaac_teleop.xr_camera_feeds if cfg.enabled and cfg.camera_name == camera_name + ] + if len(feed_cfgs) != 1: + raise ValueError(f"OVXR PiP camera {camera_name!r} must have exactly one enabled XrCameraFeedCfg") + # All selected sensors must share the one leased renderer, not three renderers. + camera_cfg.renderer_cfg = renderer_cfg + pip_selection.append((camera_name, camera_cfg, feed_cfgs[0])) + keep_cameras = tuple(names) + + # The leased OVRTX renderer is the only render path in OVXR. Strip + # every task camera before the launcher scans the config. + if args_cli.using_ovxr or args_cli.disable_external_cameras: + from isaaclab_teleop import remove_camera_configs + + env_cfg = remove_camera_configs(env_cfg, keep=keep_cameras) + + if env_cfg.sim.physics is None: + from isaaclab.physics import PhysicsCfg + + env_cfg.sim.physics = PhysicsCfg() + configs, kit_sources = validate_launch(env_cfg, vars(args_cli).copy()) + if args_cli.using_ovxr: + from isaaclab.visualizers import VisualizerBackendCfg + + if kit_sources or any(isinstance(cfg, VisualizerBackendCfg) for cfg in configs): + raise ValueError("--ovxr requires no Kit runtime or local visualizer") + prepare_teleop_backend(env_cfg, env_cfg.sim.physics) + return env_cfg, teleop_device_explicitly_set, use_isaac_teleop, pip_selection + + +def _install_ovxr_shutdown_handlers(ov_libraries_xr_session) -> dict: + """Make owner-script signals request the same orderly shutdown as OVUI.""" + if ov_libraries_xr_session is None: + return {} + + def request_shutdown(_signum, _frame) -> None: + ov_libraries_xr_session.request_shutdown() + + return {signum: signal.signal(signum, request_shutdown) for signum in (signal.SIGINT, signal.SIGTERM)} + + +def _restore_ovxr_shutdown_handlers(previous_handlers: dict) -> None: + for signum, handler in previous_handlers.items(): + signal.signal(signum, handler) + + +def _close_teleop_resources( + env, + camera_feed_session, + ov_libraries_xr_session, + previous_shutdown_handlers, + teleop_interface=None, + *, + previous_context=None, +) -> None: + """Preserve the first failure and drain XR before freeing its borrowed environment.""" + failure = sys.exception() + cleanup_failure = None + xr_drained = True + camera_drained = True + + def report_cleanup_error(error: Exception) -> None: + nonlocal cleanup_failure + logger.exception("Teleoperation cleanup failed") + if failure is not None: + failure.add_note(f"Cleanup also failed: {error!r}") + elif cleanup_failure is None: + cleanup_failure = error + else: + cleanup_failure.add_note(f"Cleanup also failed: {error!r}") - # XR-rendering setup (camera removal + DLSS) is only needed for the Kit XR - # path. Without --xr, Isaac Capture runs standalone (I/O only) and renders - # normally, so gate on --xr alone. - if args_cli.xr: - # Keep camera configs when external cameras are enabled (defaulted on); otherwise - # strip them so the XR headset view is the sole render product. - if args_cli.disable_external_cameras: - env_cfg = remove_camera_configs(env_cfg) - - with launch_simulation(env_cfg, args_cli), ExitStack() as cleanup: - run_teleoperation(env_cfg, use_isaac_teleop, cleanup) - + try: + input_drain_failed = getattr(teleop_interface, "session_cleanup_failed", False) is True + if input_drain_failed: + xr_drained = False + report_cleanup_error( + RuntimeError("Isaac Capture cleanup failed; borrowed XR/environment drain is unproved") + ) + elif ov_libraries_xr_session is not None: + try: + ov_libraries_xr_session.close() + except Exception as error: + xr_drained = False + report_cleanup_error(error) + if not input_drain_failed: + # A successfully bound feed exits before Capture. This is also the + # cleanup path for preparation that never reached bind(). + try: + camera_feed_session.close() + except Exception as error: + camera_drained = False + report_cleanup_error(error) + if env is not None and xr_drained: + if ov_libraries_xr_session is None or not ov_libraries_xr_session.async_worker_owns_environment: + try: + env.close() + except Exception as error: + report_cleanup_error(error) + elif env is None and xr_drained and camera_drained: + from isaaclab.sim import SimulationContext + + if ov_libraries_xr_session is None or not ov_libraries_xr_session.async_worker_owns_environment: + context = SimulationContext.instance() + if context is not None and context is not previous_context: + try: + SimulationContext.clear_instance() + except Exception as error: + report_cleanup_error(error) + finally: + # Keep repeated SIGINT/SIGTERM on the owner throughout this cleanup attempt. + try: + _restore_ovxr_shutdown_handlers(previous_shutdown_handlers) + except Exception as error: + report_cleanup_error(error) + if cleanup_failure is not None: + raise cleanup_failure + if env is not None and xr_drained: + print("Environment closed") + + +def main( # noqa: C901 + env_cfg: ManagerBasedRLEnvCfg, + teleop_device_explicitly_set: bool, + use_isaac_teleop: bool, + pip_selection: list[tuple[str, object, object]], +) -> None: + """Run teleoperation through the selected Kit or OVXR runtime.""" -def run_teleoperation(env_cfg: ManagerBasedRLEnvCfg, use_isaac_teleop: bool, cleanup: ExitStack) -> None: # noqa: C901 - """Create the environment and teleop device, then run the teleoperation loop until the app is closed.""" - from isaaclab_physx.renderers.isaac_rtx_renderer_utils import apply_isaac_rtx_global_settings from isaaclab_teleop import XrCameraFeedSession + from isaaclab.sim import SimulationContext + + previous_context = SimulationContext.instance() camera_feed_session = XrCameraFeedSession.prepare( env_cfg, enabled=args_cli.xr and use_isaac_teleop, - camera_rendering_enabled=not args_cli.disable_external_cameras, + camera_rendering_enabled=not args_cli.disable_external_cameras and not args_cli.using_ovxr, ) - cleanup.callback(camera_feed_session.close) - - # Apply the RTX/DLSS global settings when an RTX render pipeline will run (Kit visualizer, - # external cameras, or XR). - if not args_cli.disable_external_cameras or args_cli.kit_visualizer or args_cli.xr: - apply_isaac_rtx_global_settings( - IsaacRtxRendererGlobalSettingsCfg( - antialiasing_mode="DLSS", - carb_settings=( - {"/rtx/dldenoiser/responsiveDenoising": True} - if camera_feed_session.requires_responsive_denoising - else None - ), - ), - ) + env = None + teleop_interface = None + ov_libraries_xr_session = None + previous_shutdown_handlers = {} try: - # create environment - env = gym.make(args_cli.task, cfg=env_cfg).unwrapped - # check environment name (for reach , we don't allow the gripper) - if "Reach" in args_cli.task: - logger.warning( - f"The environment '{args_cli.task}' does not support gripper control. The device command will be" - " ignored." + # Apply the RTX/DLSS global settings when an RTX render pipeline will run (Kit visualizer, + # external cameras, or XR). ``apply_isaac_rtx_global_settings`` uses ``omni.replicator``, + # which some experiences do not preload, so ensure it is loaded first. + if not args_cli.using_ovxr and _rtx_rendering_requested(args_cli): + from isaaclab_physx.renderers import IsaacRtxRendererGlobalSettingsCfg + from isaaclab_physx.renderers.isaac_rtx_renderer_utils import apply_isaac_rtx_global_settings + + # The renderer's public helper loads Replicator when its settings need it. + apply_isaac_rtx_global_settings( + IsaacRtxRendererGlobalSettingsCfg( + antialiasing_mode="DLSS", + carb_settings=( + {"/rtx/dldenoiser/responsiveDenoising": True} + if camera_feed_session.requires_responsive_denoising + else None + ), + ), ) - except Exception as e: - logger.error(f"Failed to create environment: {e}") - return - - # Flags for controlling teleoperation flow - should_reset_recording_instance = False - teleoperation_active = True - # Callback handlers - def reset_recording_instance() -> None: - """ - Reset the environment to its initial state. - - Sets a flag to reset the environment on the next simulation step. - - Returns: - None - """ - nonlocal should_reset_recording_instance - should_reset_recording_instance = True - print("Reset triggered - Environment will reset on next step") + if args_cli.using_ovxr: + from isaaclab_ov.renderers import OVRTXRendererCfg + from isaaclab_teleop import OvLibrariesXrConfig, OvLibrariesXrSession + from isaaclab_teleop.camera_feed import resolve_static_panel_pose + from isaaclab_teleop.isaac_teleop_cfg import XrCameraFeedLayoutCfg + from isaaclab_teleop.ov_libraries_xr import OvLibrariesXrCameraConfig + + selected_cameras = [] + for name, camera_cfg, feed_cfg in pip_selection: + layout = ( + XrCameraFeedLayoutCfg(placement="head_locked") + if args_cli.ovxr_pip_head_locked + else env_cfg.isaac_teleop.xr_camera_feed_layout + ) + space, position, orientation = resolve_static_panel_pose(feed_cfg, layout) + if args_cli.ovxr_pip_view_position is not None: + position = tuple(args_cli.ovxr_pip_view_position) + selected_cameras.append( + OvLibrariesXrCameraConfig( + name=name, + camera_cfg=camera_cfg, + # Zero means uncapped in XrCameraFeedCfg, not zero production. + max_fps=( + min(args_cli.ovxr_pip_fps, feed_cfg.max_update_hz) + if feed_cfg.max_update_hz > 0.0 + else args_cli.ovxr_pip_fps + ), + panel_width_m=feed_cfg.panel_width_m, + position=position, + orientation_xyzw=orientation, + space=space, + ) + ) + ov_libraries_xr_session = OvLibrariesXrSession.prepare( + env_cfg.sim, + OvLibrariesXrConfig( + application_name=f"isaaclab-ovxr-teleop-{args_cli.task}", + session_mode=args_cli.ovxr_session_mode, + cloudxr_env_file=args_cli.cloudxr_env, + auto_launch_cloudxr=args_cli.auto_launch_cloudxr, + desktop_mirror_enabled=args_cli.ovxr_desktop_mirror, + desktop_mirror_max_fps=args_cli.ovxr_desktop_mirror_fps, + auxiliary_cameras=tuple(selected_cameras), + ), + renderer_cfg=selected_cameras[0].camera_cfg.renderer_cfg if selected_cameras else OVRTXRendererCfg(), + ) - def start_teleoperation() -> None: - """ - Activate teleoperation control of the robot. + try: + # create environment + env = gym.make(args_cli.task, cfg=env_cfg).unwrapped + # check environment name (for reach , we don't allow the gripper) + if "Reach" in args_cli.task: + logger.warning( + f"The environment '{args_cli.task}' does not support gripper control. The device command will be" + " ignored." + ) - Enables the application of teleoperation commands to the environment. + except Exception: + logger.exception("Failed to create environment") + raise - Returns: - None - """ - nonlocal teleoperation_active + # Flags for controlling teleoperation flow + should_reset_recording_instance = False teleoperation_active = True - print("Teleoperation activated") - - def stop_teleoperation() -> None: - """ - Deactivate teleoperation control of the robot. - - Disables the application of teleoperation commands to the environment. - - Returns: - None - """ - nonlocal teleoperation_active - teleoperation_active = False - print("Teleoperation deactivated") - - # Create device config if not already in env_cfg - teleoperation_callbacks: dict[str, Callable[[], None]] = { - "R": reset_recording_instance, - "START": start_teleoperation, - "STOP": stop_teleoperation, - "RESET": reset_recording_instance, - } - - # For XR devices (hand tracking or Isaac Capture), default to inactive. Without - # --xr, teleop is started locally (see ``request_start`` below) rather than by - # a headset, so it still begins running -- but it flows through the same state - # machine, so keyboard/host pause/resume keeps working. - if use_isaac_teleop or args_cli.xr: - teleoperation_active = env_cfg.isaac_teleop.teleoperation_active_default if use_isaac_teleop else False - else: - # Always active for other devices - teleoperation_active = True - - try: - if use_isaac_teleop: - from isaaclab_teleop import create_isaac_teleop_device, poll_control_events - - teleop_interface = create_isaac_teleop_device( - env_cfg.isaac_teleop, - sim_device=args_cli.device, - callbacks=teleoperation_callbacks, - cloudxr_env_file=args_cli.cloudxr_env, - auto_launch_cloudxr=args_cli.auto_launch_cloudxr, - enable_debug_visualization=args_cli.enable_debug_visualization, - use_kit_xr_bridge=args_cli.xr, - haptic_cfg=getattr(env_cfg, "haptic_feedback", None), - ) - + reset_publication_generation = 0 + + # Callback handlers + def reset_recording_instance() -> None: + """ + Reset the environment to its initial state. + + Sets a flag to reset the environment on the next simulation step. + + Returns: + None + """ + nonlocal should_reset_recording_instance + should_reset_recording_instance = True + print("Reset triggered - Environment will reset on next step") + + def start_teleoperation() -> None: + """ + Activate teleoperation control of the robot. + + Enables the application of teleoperation commands to the environment. + + Returns: + None + """ + nonlocal teleoperation_active + teleoperation_active = True + print("Teleoperation activated") + + def stop_teleoperation() -> None: + """ + Deactivate teleoperation control of the robot. + + Disables the application of teleoperation commands to the environment. + + Returns: + None + """ + nonlocal teleoperation_active + teleoperation_active = False + print("Teleoperation deactivated") + + # Create device config if not already in env_cfg + teleoperation_callbacks: dict[str, Callable[[], None]] = { + "R": reset_recording_instance, + "START": start_teleoperation, + "STOP": stop_teleoperation, + "RESET": reset_recording_instance, + } + + # For XR devices (hand tracking or Isaac Capture), default to inactive. Without + # --xr, teleop is started locally (see ``request_start`` below) rather than by + # a headset, so it still begins running -- but it flows through the same state + # machine, so keyboard/host pause/resume keeps working. + if use_isaac_teleop or args_cli.xr: + teleoperation_active = env_cfg.isaac_teleop.teleoperation_active_default if use_isaac_teleop else False else: - # Task-declared devices take precedence only when explicitly selected. - if args_cli.teleop_device is not None and args_cli.teleop_device in env_cfg.teleop_devices.devices: - teleop_interface = create_teleop_device( - args_cli.teleop_device, env_cfg.teleop_devices.devices, teleoperation_callbacks + # Always active for other devices + teleoperation_active = True + + # Create teleop device based on configuration + try: + if use_isaac_teleop: + from isaaclab_teleop import create_isaac_teleop_device, poll_control_events + + teleop_interface = create_isaac_teleop_device( + env_cfg.isaac_teleop, + sim_device=env.sim.device, + callbacks=teleoperation_callbacks, + cloudxr_env_file=(None if args_cli.using_ovxr else args_cli.cloudxr_env), + auto_launch_cloudxr=args_cli.auto_launch_cloudxr and not args_cli.using_ovxr, + enable_debug_visualization=args_cli.enable_debug_visualization, + use_kit_xr_bridge=args_cli.xr, + external_oxr_handles_provider=( + ov_libraries_xr_session.external_oxr_handles if ov_libraries_xr_session is not None else None + ), + haptic_cfg=getattr(env_cfg, "haptic_feedback", None), ) + else: - device_name = "keyboard" if args_cli.teleop_device is None else args_cli.teleop_device.lower() - device_cfgs = { - "keyboard": Se3KeyboardCfg(pos_sensitivity=0.05, rot_sensitivity=0.05), - "spacemouse": Se3SpaceMouseCfg(pos_sensitivity=0.05, rot_sensitivity=0.05), - "gamepad": Se3GamepadCfg(pos_sensitivity=0.1, rot_sensitivity=0.1), - } - if device_name not in device_cfgs: - logger.error( - f"--teleop_device={args_cli.teleop_device} was passed but no matching entry exists in" - " env_cfg.teleop_devices and it is not a built-in device name. Either remove" - " --teleop_device to use the Isaac Capture pipeline, or add a" - f" '{args_cli.teleop_device}' entry under teleop_devices in the environment config." - " Built-in devices: keyboard, spacemouse, gamepad." + # Task-declared devices take precedence only when explicitly selected. + if args_cli.teleop_device is not None and args_cli.teleop_device in env_cfg.teleop_devices.devices: + teleop_interface = create_teleop_device( + args_cli.teleop_device, env_cfg.teleop_devices.devices, teleoperation_callbacks ) - env.close() - return - device_cfg = device_cfgs[device_name] - device_cfg.pos_sensitivity *= args_cli.sensitivity - device_cfg.rot_sensitivity *= args_cli.sensitivity - teleop_interface = instantiate(device_cfg) - for key, callback in teleoperation_callbacks.items(): - try: - teleop_interface.add_callback(key, callback) - except (ValueError, TypeError) as e: - logger.warning(f"Failed to add callback for key {key}: {e}") - except Exception as e: - logger.error(f"Failed to create teleop device: {e}") - env.close() - return - - print(f"Using teleop device: {teleop_interface}") - - haptic_driver = None - if use_isaac_teleop: - from isaaclab_teleop import create_haptic_feedback_driver - - haptic_driver = create_haptic_feedback_driver(env.unwrapped, teleop_interface, env_cfg) - - # Retain the keyboard so its input subscription lives for the whole session. - if use_isaac_teleop and env.sim.has_gui and not env.sim.get_setting("/isaaclab/xr/auto_start"): - try: - control_keyboard = instantiate(Se3KeyboardCfg(pos_sensitivity=0.0, rot_sensitivity=0.0)) - control_keyboard.add_callback("B", teleop_interface.request_start) - control_keyboard.add_callback("P", teleop_interface.request_stop) - # Inject a reset pulse; the loop owns the environment reset. - control_keyboard.add_callback("R", lambda: teleop_interface.reset(pause=True)) - print("Isaac Capture control keys: [B] start/resume [P] pause [R] reset") - except Exception as e: - logger.warning(f"Control keyboard unavailable ({e}); teleop still auto-starts without --xr") - control_keyboard = None - - def run_loop(): - """Inner function to run the teleop loop with access to nonlocal variables.""" - nonlocal should_reset_recording_instance, teleoperation_active - - # reset environment - env.reset() - teleop_interface.reset() - - # Without --xr there is no headset to send START, so start locally ([B]/[P] can - # still pause/resume). The reset() above is a host reset (a pure pulse), so it does - # not cancel this start. - if use_isaac_teleop and not args_cli.xr: - teleop_interface.request_start() - - stack_name = "Isaac Capture" if use_isaac_teleop else "native" - print(f"{stack_name} teleoperation started. Press 'R' to reset the environment.") - - # simulate environment - while env.sim.is_running(): + else: + device_name = "keyboard" if args_cli.teleop_device is None else args_cli.teleop_device.lower() + device_cfgs = { + "keyboard": Se3KeyboardCfg(pos_sensitivity=0.05, rot_sensitivity=0.05), + "spacemouse": Se3SpaceMouseCfg(pos_sensitivity=0.05, rot_sensitivity=0.05), + "gamepad": Se3GamepadCfg(pos_sensitivity=0.1, rot_sensitivity=0.1), + } + if device_name not in device_cfgs: + logger.error( + f"--teleop_device={args_cli.teleop_device} was passed but no matching entry exists in" + " env_cfg.teleop_devices and it is not a built-in device name. Either remove" + " --teleop_device to use the Isaac Capture pipeline, or add a" + f" '{args_cli.teleop_device}' entry under teleop_devices in the environment config." + " Built-in devices: keyboard, spacemouse, gamepad." + ) + env.close() + return + device_cfg = device_cfgs[device_name] + device_cfg.pos_sensitivity *= args_cli.sensitivity + device_cfg.rot_sensitivity *= args_cli.sensitivity + teleop_interface = instantiate(device_cfg) + for key, callback in teleoperation_callbacks.items(): + try: + teleop_interface.add_callback(key, callback) + except (ValueError, TypeError) as e: + logger.warning(f"Failed to add callback for key {key}: {e}") + except Exception: + logger.exception("Failed to create teleop device") + raise + + if teleop_interface is None: + logger.error("Failed to create teleop interface") + raise RuntimeError("Failed to create teleop interface") + + print(f"Using teleop device: {teleop_interface}") + + # Optional controller haptics: no-ops unless the env declares a + # ``haptic_feedback`` config and the device can render it (Isaac Capture). + haptic_update, haptic_stop = _make_haptic_io(env, teleop_interface, env_cfg, use_isaac_teleop) + + require_initial_bimanual_tracking = ( + env_cfg.isaac_teleop.require_initial_bimanual_hand_tracking if use_isaac_teleop else False + ) + initialized_hand_tracking: set[str] = set() + skipped_tracking_frames = 0 + action_conditioner = env_cfg.isaac_teleop.action_conditioner if use_isaac_teleop else None + action_validator = env_cfg.isaac_teleop.action_validator if use_isaac_teleop else None + action_state: dict = {} + + # Retain the keyboard so its input subscription lives for the whole session. + if use_isaac_teleop and env.sim.has_gui and not env.sim.get_setting("/isaaclab/xr/auto_start"): try: - # run everything in inference mode - with torch.inference_mode(): - # get device command - action = teleop_interface.advance() - - if use_isaac_teleop: - ctrl = poll_control_events(teleop_interface) - if ctrl.is_active is not None: - teleoperation_active = ctrl.is_active - if ctrl.should_reset: - should_reset_recording_instance = True - - # action is None when Isaac Capture session hasn't started yet - # (e.g. waiting for user to click "Start AR") - if action is None or not teleoperation_active: - env.sim.render() - if haptic_driver is not None: - haptic_driver.stop() - else: - # process actions - actions = action.repeat(env.num_envs, 1) - # apply actions - env.step(actions) - # render controller haptics from post-step contact forces - if haptic_driver is not None: - haptic_driver.update() - - if should_reset_recording_instance: - env.reset() - teleop_interface.reset() - camera_feed_session.refresh() - should_reset_recording_instance = False - print("Environment reset complete") + control_keyboard = instantiate(Se3KeyboardCfg(pos_sensitivity=0.0, rot_sensitivity=0.0)) + control_keyboard.add_callback("B", teleop_interface.request_start) + control_keyboard.add_callback("P", teleop_interface.request_stop) + # Inject a reset pulse; the loop owns the environment reset. + control_keyboard.add_callback("R", lambda: teleop_interface.reset(pause=True)) + print("Isaac Capture control keys: [B] start/resume [P] pause [R] reset") except Exception as e: - logger.error(f"Error during simulation step: {e}") - break - - # Run the teleoperation loop - # Isaac Capture requires a context manager, native devices don't - if use_isaac_teleop: - with teleop_interface, camera_feed_session.bind(env): - run_loop() - else: - with camera_feed_session.bind(env): - run_loop() - - # close the simulator - env.close() - print("Environment closed") + logger.warning(f"Control keyboard unavailable ({e}); teleop still auto-starts without --xr") + control_keyboard = None + + def run_loop(): + """Inner function to run the teleop loop with access to nonlocal variables.""" + nonlocal should_reset_recording_instance, skipped_tracking_frames, teleoperation_active + nonlocal reset_publication_generation + + # reset environment + env.reset() + teleop_interface.reset() + initialized_hand_tracking.clear() + action_state.clear() + if ov_libraries_xr_session is not None: + ov_libraries_xr_session.request_render(env.sim.get_physics_step_count()) + pending_publication_identity = None + + # Without --xr there is no headset to send START, so start locally ([B]/[P] can + # still pause/resume). The reset() above is a host reset (a pure pulse), so it does + # not cancel this start. + if use_isaac_teleop and not (args_cli.xr or args_cli.using_ovxr): + teleop_interface.request_start() + + stack_name = "Isaac Capture" if use_isaac_teleop else "native" + print(f"{stack_name} teleoperation started. Press 'R' to reset the environment.") + + # simulate environment + while env.sim.is_running() and not ( + ov_libraries_xr_session is not None and ov_libraries_xr_session.shutdown_requested + ): + iteration_started_s = time.perf_counter() if ov_libraries_xr_session is not None else 0.0 + try: + # run everything in inference mode + with torch.inference_mode(): + # get device command + target_frame_prim_path = ( + env_cfg.isaac_teleop.target_frame_prim_path + if ov_libraries_xr_session is not None and use_isaac_teleop + else None + ) + if target_frame_prim_path is None: + action = teleop_interface.advance() + else: + target_T_world = ov_libraries_xr_session.get_target_frame_T_world(target_frame_prim_path) + action = teleop_interface.advance(target_T_world=target_T_world) + + if use_isaac_teleop: + ctrl = poll_control_events(teleop_interface) + if ctrl.is_active is not None: + teleoperation_active = ctrl.is_active + if ctrl.should_reset: + should_reset_recording_instance = True + + if not teleoperation_active: + action_state.clear() + + # action is None when Isaac Capture session hasn't started yet + # (e.g. waiting for user to click "Start AR") + if action is None: + if ov_libraries_xr_session is None: + env.sim.render() + haptic_stop() + action_applied = False + elif teleoperation_active: + issue = ( + _required_hand_tracking_issue(teleop_interface, initialized_hand_tracking) + if require_initial_bimanual_tracking + else None + ) + if issue is None: + action, issue = prepare_teleop_action( + env, action, action_conditioner, action_validator, action_state + ) + action_applied = _apply_teleop_frame(env, action, issue, haptic_update, haptic_stop) + if issue is not None: + skipped_tracking_frames += 1 + if skipped_tracking_frames <= 5 or skipped_tracking_frames % 200 == 0: + logger.warning( + "Rendering without a physics step for unsafe teleop frame (%s); skipped=%d", + issue, + skipped_tracking_frames, + ) + else: + action_state.clear() + if ov_libraries_xr_session is None: + env.sim.render() + # not stepping: zero haptics so a paused grip stops buzzing + haptic_stop() + action_applied = False + + state_changed = action_applied + force_publication = False + publication_identity = env.sim.get_physics_step_count() + if should_reset_recording_instance: + env.reset() + teleop_interface.reset() + initialized_hand_tracking.clear() + action_state.clear() + camera_feed_session.refresh() + should_reset_recording_instance = False + if ov_libraries_xr_session is not None: + state_changed = True + reset_publication_generation += 1 + force_publication = True + publication_identity = _ovxr_publication_identity( + env.sim.get_physics_step_count(), reset_generation=reset_publication_generation + ) + print("Environment reset complete") + + if ov_libraries_xr_session is not None: + if state_changed: + pending_publication_identity = publication_identity + if pending_publication_identity is not None: + admitted = ov_libraries_xr_session.request_render( + pending_publication_identity, force=force_publication + ) + if admitted is not None: + pending_publication_identity = None + + if ov_libraries_xr_session is not None: + ov_libraries_xr_session.report_host_timing( + env.sim.get_physics_step_count(), env.sim.get_physics_dt() + ) + _pace_ovxr_control_loop( + ov_libraries_xr_session is not None, + action_applied, + iteration_started_s=iteration_started_s, + action_period_s=env.step_dt, + step_hz=args_cli.step_hz, + wait_for_shutdown=( + ov_libraries_xr_session.wait_for_shutdown + if ov_libraries_xr_session is not None + else None + ), + ) + except Exception as e: + logger.exception("Error during simulation step: %s", e) + raise + + # Install after environment/device initialization because embedded native + # libraries may replace inherited signal dispositions during startup. + previous_shutdown_handlers = _install_ovxr_shutdown_handlers(ov_libraries_xr_session) + # IsaacTeleop requires a context manager, native devices do not. + if use_isaac_teleop: + if ov_libraries_xr_session is not None: + # Capture releases borrowed handles before the outer cleanup + # closes OVXR and then the environment. + with teleop_interface, camera_feed_session.bind(env): + render_worker_started = ov_libraries_xr_session.start_render_worker( + env, + tracking_anchor_path=teleop_interface.xr_render_anchor_path, + required_openxr_extensions=teleop_interface.required_openxr_extensions, + hidden_visual_prims=env_cfg.isaac_teleop.xr_cfg.hidden_visual_prims, + excluded_visual_meshes=env_cfg.isaac_teleop.xr_cfg.excluded_visual_meshes, + external_anchor_state=teleop_interface.external_anchor_state, + max_blank_frames=args_cli.ovxr_bootstrap_frames, + timeout_s=args_cli.ovxr_worker_timeout, + ) + if render_worker_started is not False: + run_loop() + else: + with teleop_interface, camera_feed_session.bind(env): + run_loop() + else: + with camera_feed_session.bind(env): + run_loop() + finally: + _close_teleop_resources( + env, + camera_feed_session, + ov_libraries_xr_session, + previous_shutdown_handlers, + teleop_interface, + previous_context=previous_context, + ) if __name__ == "__main__": - # run the main function - main() + resolved_env_cfg, explicit_device, uses_isaac_teleop, pip_selection = _resolve_env_cfg() + with launch_simulation(resolved_env_cfg, args_cli, cleanup_context=not args_cli.using_ovxr): + main(resolved_env_cfg, explicit_device, uses_isaac_teleop, pip_selection) diff --git a/source/isaaclab/changelog.d/kitless-hand-joint-order.fixed.rst b/source/isaaclab/changelog.d/kitless-hand-joint-order.fixed.rst new file mode 100644 index 000000000000..ca76d3f2e2ef --- /dev/null +++ b/source/isaaclab/changelog.d/kitless-hand-joint-order.fixed.rst @@ -0,0 +1 @@ +* Preserved configured hand joint ordering when applying Pink action targets. diff --git a/source/isaaclab/changelog.d/kitless-ovxr-lifecycle.fixed.rst b/source/isaaclab/changelog.d/kitless-ovxr-lifecycle.fixed.rst new file mode 100644 index 000000000000..4274dd1920f9 --- /dev/null +++ b/source/isaaclab/changelog.d/kitless-ovxr-lifecycle.fixed.rst @@ -0,0 +1,2 @@ +* Allowed asynchronous XR owners to drain before releasing the simulation context. +* Supported empty CPU Warp arrays in Torch conversion. diff --git a/source/isaaclab/changelog.d/kitless-ovxr-lifecycle.minor b/source/isaaclab/changelog.d/kitless-ovxr-lifecycle.minor new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab/changelog.d/kitless-ovxr.added.rst b/source/isaaclab/changelog.d/kitless-ovxr.added.rst new file mode 100644 index 000000000000..ae73800ea091 --- /dev/null +++ b/source/isaaclab/changelog.d/kitless-ovxr.added.rst @@ -0,0 +1 @@ +* Exposed the native scene transform device for OVXR anchor and target-frame updates. diff --git a/source/isaaclab/changelog.d/kitless-ovxr.minor b/source/isaaclab/changelog.d/kitless-ovxr.minor new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab/changelog.d/ovxr-outputs.skip b/source/isaaclab/changelog.d/ovxr-outputs.skip new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab/isaaclab/app/sim_launcher.py b/source/isaaclab/isaaclab/app/sim_launcher.py index d832f83b1d0c..74dcad0aa445 100644 --- a/source/isaaclab/isaaclab/app/sim_launcher.py +++ b/source/isaaclab/isaaclab/app/sim_launcher.py @@ -412,31 +412,49 @@ def validate_launch( # noqa: C901 def launch_simulation( cfg, launcher_args: argparse.Namespace | dict | None = None, + *, + cleanup_context: bool = True, ) -> Generator[PhysicsCfg | None, None, None]: """Context manager that launches the appropriate simulation runtime for *cfg*. - Resolves configured consumers before starting their required runtimes. Each runtime starts - once and closes on exit. Cameras are auto-enabled for Kit-renderer sensors. + <<<<<<< ours + Resolves configured consumers before starting their required runtimes. Each runtime starts + once and closes on exit. Cameras are auto-enabled for Kit-renderer sensors. + ======= + Walks the config tree once (resolving ``--physics``, validating the + physics/renderer/visualizer combination, and deciding whether Isaac Sim Kit is + needed), then starts the launcher each required runtime's config names (closed on exit) or + does nothing for kitless ones. Cameras are auto-enabled for Kit-renderer sensors. - Resolve viewer and physics choices on the same config used to construct the simulation:: + On exit, stops and releases a simulation context created inside the scope before closing + its runtimes. An existing context owned by the caller is preserved. Applications + with asynchronous resource owners can set ``cleanup_context=False`` and take + responsibility for context cleanup, including failed construction and drain. + >>>>>>> theirs - sim_cfg = SimulationCfg() - with launch_simulation(sim_cfg, args_cli): - sim = SimulationContext(sim_cfg) + Resolve viewer and physics choices on the same config used to construct the simulation:: - Yields the resolved physics config when a caller needs to adjust solver settings before construction. + sim_cfg = SimulationCfg() + with launch_simulation(sim_cfg, args_cli): + sim = SimulationContext(sim_cfg) - Args: - cfg: Concrete config tree declaring backend, renderer, and sensor requirements. - launcher_args: Parsed launcher arguments, typically the script's ``args_cli``. Besides the - arguments added by :func:`add_launcher_args`, the following keys are read when a script - contributes them: - - * ``physics``: Backend selector applied to every physics config in *cfg*. - * ``require_kit``: Whether the caller needs Kit for a reason *cfg* cannot express, e.g. - a tool that reaches a Kit-only extension API. This is additive -- it can only turn a - kitless launch into a Kit one, never the reverse, so a config that already needs Kit - still launches it when the key is absent or ``False``. + Yields the resolved physics config when a caller needs to adjust solver settings before construction. + + Args: + cfg: Concrete config tree declaring backend, renderer, and sensor requirements. + launcher_args: Parsed launcher arguments, typically the script's ``args_cli``. Besides the + arguments added by :func:`add_launcher_args`, the following keys are read when a script + contributes them: + + * ``physics``: Backend selector applied to every physics config in *cfg*. + * ``require_kit``: Whether the caller needs Kit for a reason *cfg* cannot express, e.g. + a tool that reaches a Kit-only extension API. This is additive -- it can only turn a + kitless launch into a Kit one, never the reverse, so a config that already needs Kit + still launches it when the key is absent or ``False``. + cleanup_context: Automatically release a context created inside this scope. + Disable only when the application owns its cleanup and must retain it + if asynchronous consumers cannot prove they have drained. This controls + context release only; configured runtime launchers still close on exit. """ launcher_args = {} if launcher_args is None else launcher_args launcher_args = vars(launcher_args) if isinstance(launcher_args, argparse.Namespace) else launcher_args @@ -500,7 +518,7 @@ def launch_simulation( finally: # Release runtime consumers before a launcher can terminate the process. context = SimulationContext.instance() - if context is not None and context is not previous_context: + if cleanup_context and context is not None and context is not previous_context: SimulationContext.clear_instance() except KeyboardInterrupt: exit_code = 130 diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py index ebd129bb6f62..454c965562f4 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py @@ -72,8 +72,12 @@ def _initialize_joint_info(self) -> None: self._isaaclab_all_joint_ids = list(range(len(self._asset.data.joint_names))) self.cfg.controller.all_joint_names = self._asset.data.joint_names - # Resolve hand joints - self._hand_joint_ids, self._hand_joint_names = self._asset.find_joints(self.cfg.hand_joint_names) + # Hand targets occupy the action tensor in configuration order. Newton's + # articulation order groups these joints differently, so preserve the + # declared order when resolving ids instead of silently permuting targets. + self._hand_joint_ids, self._hand_joint_names = self._asset.find_joints( + self.cfg.hand_joint_names, preserve_order=True + ) # Combine all joint information self._controlled_joint_ids = self._isaaclab_controlled_joint_ids + self._hand_joint_ids diff --git a/source/isaaclab/isaaclab/scene_data/scene_data_provider.py b/source/isaaclab/isaaclab/scene_data/scene_data_provider.py index d2bec498aa55..0c4a72c554b2 100644 --- a/source/isaaclab/isaaclab/scene_data/scene_data_provider.py +++ b/source/isaaclab/isaaclab/scene_data/scene_data_provider.py @@ -70,6 +70,11 @@ def __init__(self, backend: SceneDataBackend): self._cache: dict[Any, TimestampedBuffer] = {} self._transform_mappings: dict[tuple, wp.array | None] = {} + @property + def transform_device(self) -> wp.Device: + """Return the device of the current native transform publication.""" + return _publication_device(self.backend.get_transforms(SceneDataFormat.Matrix44)) + def get_transforms( self, output: SceneDataFormat.Vec3_Quat diff --git a/source/isaaclab/isaaclab/utils/warp/__init__.py b/source/isaaclab/isaaclab/utils/warp/__init__.py index 2b2eef4ecef1..a9def1c0d538 100644 --- a/source/isaaclab/isaaclab/utils/warp/__init__.py +++ b/source/isaaclab/isaaclab/utils/warp/__init__.py @@ -37,8 +37,9 @@ def _wp_to_torch_with_proxyarray(a, requires_grad=None): The shim routes :class:`ProxyArray` arguments to their cached ``.torch`` view (a zero-copy :class:`torch.Tensor` of the same underlying memory) and - emits a one-shot :class:`DeprecationWarning`. For any other input type, - the original :func:`warp.to_torch` handles the call as before. + emits a one-shot :class:`DeprecationWarning`. Empty CPU arrays use DLPack + because NumPy rejects their null-pointer array interface. Other native + arrays keep the original :func:`warp.to_torch` conversion. """ global _WP_TO_TORCH_WARNED if isinstance(a, ProxyArray): @@ -51,6 +52,19 @@ def _wp_to_torch_with_proxyarray(a, requires_grad=None): stacklevel=2, ) return a.torch + if isinstance(a, wp.array) and a.size == 0 and a.device is not None and a.device.is_cpu: + # Preserve Warp's structured-array rejection before the DLPack fallback. + if isinstance(a.dtype, wp._src.codegen.Struct): + return _WP_TO_TORCH_ORIGINAL(a, requires_grad=requires_grad) + import torch + + if requires_grad is None: + requires_grad = a.requires_grad + tensor = torch.from_dlpack(wp.to_dlpack(a)) + tensor.requires_grad = requires_grad + if requires_grad and a.requires_grad: + tensor.grad = torch.from_dlpack(wp.to_dlpack(a.grad)) + return tensor return _WP_TO_TORCH_ORIGINAL(a, requires_grad=requires_grad) diff --git a/source/isaaclab/test/app/test_launch_simulation_require_kit.py b/source/isaaclab/test/app/test_launch_simulation_require_kit.py index 32d754ba16ec..e8f44054ce19 100644 --- a/source/isaaclab/test/app/test_launch_simulation_require_kit.py +++ b/source/isaaclab/test/app/test_launch_simulation_require_kit.py @@ -256,3 +256,40 @@ def test_legacy_recording_reuses_the_display_producer(kit_branch_taken, visualiz assert cfg.sim.visualizer_cfgs[0].headless == (not visualizers) assert args.get("enable_cameras", False) == (source == "kit") assert kit_branch_taken == [True] * (source == "kit") + + +@pytest.mark.parametrize( + "application_owns_cleanup,drain_failed", [(False, False), (False, True), (True, False), (True, True)] +) +def test_context_cleanup_respects_application_drain_ownership(monkeypatch, application_owns_cleanup, drain_failed): + """An outer launcher must not release a context retained by a failed native drain.""" + from isaaclab.sim import SimulationContext + + state = {"context": None} + released = [] + + def clear(): + released.append(state["context"]) + state["context"] = None + + monkeypatch.setattr(SimulationContext, "instance", lambda: state["context"]) + monkeypatch.setattr(SimulationContext, "clear_instance", clear) + context = object() + + def run(): + with launch_simulation(PhysicsCfg(), cleanup_context=not application_owns_cleanup): + state["context"] = context + if drain_failed: + raise RuntimeError("consumer drain unproved") + if application_owns_cleanup: + # The already-owning environment/worker performs normal release. + SimulationContext.clear_instance() + + if drain_failed: + with pytest.raises(RuntimeError, match="consumer drain unproved"): + run() + else: + run() + retained = application_owns_cleanup and drain_failed + assert released == ([] if retained else [context]) + assert state["context"] is (context if retained else None) diff --git a/source/isaaclab/test/cli/test_teleop_entrypoints.py b/source/isaaclab/test/cli/test_teleop_entrypoints.py index 24402b65ec60..b8f2e38428d8 100644 --- a/source/isaaclab/test/cli/test_teleop_entrypoints.py +++ b/source/isaaclab/test/cli/test_teleop_entrypoints.py @@ -47,12 +47,80 @@ def test_teleop_workflow_help_exposes_task_preset_selectors(source_checkout_root assert "physics=NAME" in result.stdout assert "renderer=NAME" in result.stdout assert "presets=NAME[,NAME,...]" in result.stdout + if command == "run": + assert re.search(r"^\s+--ovxr(?:\s|$)", result.stdout, re.MULTILINE) + assert not re.search(r"^\s+--kitless_xr(?:[,\s]|$)", result.stdout, re.MULTILINE) + assert "--ovxr_pip_camera" in result.stdout + assert "--ovxr_desktop_mirror" in result.stdout + assert "--kitless_xr_" not in result.stdout + + +def test_ovxr_omitted_launch_options_match_documented_defaults(source_checkout_root, monkeypatch): + """The shortened user command retains the profile, launch policy and visualizers.""" + from isaaclab.app import validate_launch + + monkeypatch.delenv("LIVESTREAM", raising=False) + script = source_checkout_root / "scripts/environments/teleoperation/teleop_se3_agent.py" + + def parse(arguments): + monkeypatch.setattr(sys, "argv", [str(script), *arguments]) + return runpy.run_path(str(script))["args_cli"] + + implicit = parse(["--ovxr"]) + explicit = parse(["--ovxr", "--viz", "none", "--auto_launch_cloudxr", "--cloudxr_env", "cloudxrjs"]) + assert implicit.auto_launch_cloudxr is explicit.auto_launch_cloudxr is True + assert implicit.cloudxr_env == explicit.cloudxr_env + assert implicit.cloudxr_env.endswith(".env") + left, right = vars(implicit).copy(), vars(explicit).copy() + from isaaclab.sim import SimulationCfg + + left_cfg, right_cfg = SimulationCfg(), SimulationCfg() + validate_launch(left_cfg, left) + validate_launch(right_cfg, right) + assert left_cfg.visualizer_cfgs == right_cfg.visualizer_cfgs == [] + assert parse(["--ovxr", "--no-auto_launch_cloudxr"]).auto_launch_cloudxr is False + assert parse(["--ovxr", "--cloudxr_env", "none"]).cloudxr_env is None + + +def test_ovxr_output_options_reach_existing_runtime_fields(source_checkout_root, monkeypatch): + """Camera options configure panels, the desktop mirror and XR startup.""" + options = [ + "--ovxr", + "--ovxr_session_mode", + "vr", + "--ovxr_bootstrap_frames", + "2", + "--ovxr_worker_timeout", + "60", + "--ovxr_desktop_mirror", + "--ovxr_desktop_mirror_fps", + "20", + "--ovxr_pip_camera", + "robot_pov_cam", + "--ovxr_pip_fps", + "10", + "--ovxr_pip_head_locked", + "--ovxr_pip_view_position", + "0", + "0.2", + "-0.5", + ] + script = source_checkout_root / "scripts/environments/teleoperation/teleop_se3_agent.py" + monkeypatch.setattr(sys, "argv", [str(script), *options]) + parsed = runpy.run_path(str(script))["args_cli"] + assert parsed.using_ovxr and parsed.ovxr_session_mode == "vr" + assert (parsed.ovxr_bootstrap_frames, parsed.ovxr_worker_timeout) == (2, 60) + assert parsed.ovxr_desktop_mirror + assert parsed.ovxr_desktop_mirror_fps == 20 + assert parsed.ovxr_pip_camera == ["robot_pov_cam"] and parsed.ovxr_pip_fps == 10 + assert parsed.ovxr_pip_head_locked and parsed.ovxr_pip_view_position == [0, 0.2, -0.5] @pytest.mark.parametrize(("command", "script_parts"), TELEOP_WORKFLOWS.items()) def test_teleop_dispatches_to_an_existing_script(source_checkout_root: Path, command, script_parts): """``isaaclab teleop`` forwards the remaining arguments to a script that exists.""" - args = [command, "--task", "IsaacContrib-PickPlace-Locomanipulation-G1-Abs", "--xr"] + selector = "--ovxr" if command == "run" else "--xr" + args = [command, "--task", "IsaacContrib-PickPlace-Locomanipulation-G1-Abs", selector] with ( mock.patch.object(sys, "argv", ["isaaclab", "teleop", *args]), diff --git a/source/isaaclab/test/envs/test_pink_hand_joint_order.py b/source/isaaclab/test/envs/test_pink_hand_joint_order.py new file mode 100644 index 000000000000..dfebf344dcdc --- /dev/null +++ b/source/isaaclab/test/envs/test_pink_hand_joint_order.py @@ -0,0 +1,46 @@ +# 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 + +"""Unit coverage for Pink hand-target ordering across articulation backends.""" + +from types import SimpleNamespace + +import torch + +from isaaclab.envs.mdp.actions.pink_task_space_actions import PinkInverseKinematicsAction +from isaaclab.utils.string import resolve_matching_names + + +def test_pink_hand_commands_reach_the_configured_joints(): + """A non-self-inverse hand permutation must preserve physical joint targets.""" + + class _Asset: + data = SimpleNamespace(joint_names=["arm", "hand_a", "hand_b", "hand_c"]) + + def __init__(self): + self.joint_targets = torch.zeros((1, 4)) + + def find_joints(self, expressions, preserve_order=False): + return resolve_matching_names(expressions, self.data.joint_names, preserve_order=preserve_order) + + def set_joint_position_target_index(self, target, joint_ids): + self.joint_targets[:, joint_ids] = target + + action = PinkInverseKinematicsAction.__new__(PinkInverseKinematicsAction) + action._asset = _Asset() + action.cfg = SimpleNamespace( + pink_controlled_joint_names=["arm"], + hand_joint_names=["hand_c", "hand_a", "hand_b"], + controller=SimpleNamespace(), + enable_gravity_compensation=False, + ) + + action._initialize_joint_info() + + action._compute_ik_solutions = lambda: torch.tensor([[9.0]]) + action._target_hand_joint_positions = torch.tensor([[30.0, 10.0, 20.0]]) + action.apply_actions() + + torch.testing.assert_close(action._asset.joint_targets, torch.tensor([[9.0, 10.0, 20.0, 30.0]])) diff --git a/source/isaaclab/test/utils/warp/test_proxy_array.py b/source/isaaclab/test/utils/warp/test_proxy_array.py index 247644674b71..8e96e56eb5ec 100644 --- a/source/isaaclab/test/utils/warp/test_proxy_array.py +++ b/source/isaaclab/test/utils/warp/test_proxy_array.py @@ -461,7 +461,7 @@ def test_proxy_array_warning_is_one_shot(self): assert not deprecation, "shim must not re-warn after the first call" def test_requires_grad_forwarded_to_raw_wp_array(self): - """``wp.to_torch(wp.array)`` still produces a torch view and forwards ``requires_grad``.""" + """Native and empty CPU views preserve dtype, shape and gradient selection.""" import isaaclab.utils.warp # noqa: F401 # ensure shim is installed arr = wp.array([1.0, 2.0], dtype=wp.float32, device="cpu") @@ -469,3 +469,22 @@ def test_requires_grad_forwarded_to_raw_wp_array(self): assert isinstance(t, torch.Tensor) assert t.shape == (2,) assert t.requires_grad is False + + empty_ids = wp.from_torch(torch.arange(4, dtype=torch.int32)[:0]) + ids = wp.to_torch(empty_ids) + assert ids.shape == (0,) and ids.dtype == torch.int32 + assert ids.data_ptr() == 0 and ids.stride() == (1,) + + for inherited, override, expected, has_grad in ( + (False, None, False, False), + (True, None, True, True), + (True, False, False, False), + (False, True, True, False), + ): + empty = wp.zeros((0, 3), dtype=wp.float32, device="cpu", requires_grad=inherited) + view = wp.to_torch(empty, requires_grad=override) + assert view.shape == (0, 3) and view.dtype == torch.float32 + assert view.requires_grad is expected + assert (view.grad is not None) is has_grad + if has_grad: + assert view.grad.shape == (0, 3) and view.grad.dtype == torch.float32 diff --git a/source/isaaclab_newton/changelog.d/kitless-ovxr-model.fixed.rst b/source/isaaclab_newton/changelog.d/kitless-ovxr-model.fixed.rst new file mode 100644 index 000000000000..7d2140ab9169 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/kitless-ovxr-model.fixed.rst @@ -0,0 +1 @@ +* Completed Newton model finalization before kitless renderer consumers access scene data. diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 9bcff0c94373..488e8026938e 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -256,11 +256,12 @@ def run_query( @staticmethod def capture_graph(device: str, capture_target: Callable[[], None], *, relaxed: bool = False) -> wp.Graph: """Record work without an eager warmup or graph replay, using Warp's public capture API.""" - stream = wp.get_stream(device) + # A blocking capture stream forbids concurrent legacy-stream work even + # on other threads (e.g. OVUI's input upload). ScopedStream preserves + # the incoming dependency; graph replay still uses the caller's stream. + stream = wp.stream_from_torch(torch.cuda.Stream(device=device)) mode = wp.CaptureMode.THREAD_LOCAL if relaxed: - # RTX uses the legacy CUDA stream. A nonblocking stream avoids implicit synchronization. - stream = wp.stream_from_torch(torch.cuda.Stream(device=device)) mode = wp.CaptureMode.RELAXED with NewtonQueries._paused_gc(), wp.ScopedStream(stream): with wp.ScopedCapture(stream=stream, capture_mode=mode) as capture: diff --git a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py index 2dfc71d0cf0d..a92b08556136 100644 --- a/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py +++ b/source/isaaclab_newton/test/physics/test_newton_manager_abstraction.py @@ -22,11 +22,13 @@ import contextlib import logging +import threading from types import SimpleNamespace import isaaclab_newton.physics.newton_manager as newton_manager_module import numpy as np import pytest +import torch import warp as wp from isaaclab_newton.assets.articulation import articulation as articulation_module from isaaclab_newton.assets.rigid_object import rigid_object as rigid_object_module @@ -67,7 +69,7 @@ from isaaclab.physics import PhysicsEvent from isaaclab.scene_data import SceneDataFormat, SceneDataProvider from isaaclab.sim import SimulationCfg, SimulationContext, build_simulation_context -from isaaclab.test.utils import test_devices +from isaaclab.test.utils import DeviceScope, test_devices # --------------------------------------------------------------------------- # Lightweight (no sim) parametrisation @@ -959,7 +961,8 @@ def __exit__(self, exc_type, exc_value, traceback): manager._device = "cuda:1" monkeypatch.setattr(newton_manager_module, "has_kit", lambda: False) stream = SimpleNamespace(device="cuda:1") - monkeypatch.setattr(wp, "get_stream", lambda device: stream if device == "cuda:1" else None) + monkeypatch.setattr(torch.cuda, "Stream", lambda device: stream if device == "cuda:1" else None) + monkeypatch.setattr(wp, "stream_from_torch", lambda stream: stream) monkeypatch.setattr(wp, "ScopedStream", lambda stream: contextlib.nullcontext()) monkeypatch.setattr(wp, "ScopedCapture", FakeScopedCapture) @@ -969,6 +972,52 @@ def __exit__(self, exc_type, exc_value, traceback): assert graph is captured_graph +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) +def test_graph_capture_allows_concurrent_legacy_stream_copy(device): + """A render/UI copy must not invalidate physics capture on the same GPU.""" + source = wp.full(4, 7.0, device=device) + result = wp.zeros(4, device=device) + image = torch.full((4,), 23.0, device=device) + copied = torch.zeros_like(image) + # Setup is complete before capture. No allocations or implicit dependency + # on captured physics work are needed by the independent image consumer. + wp.synchronize_device(device) + torch.cuda.synchronize(device) + entered = threading.Event() + finished = threading.Event() + errors = [] + + def copy_image(): + try: + assert entered.wait(5), "capture did not start" + with torch.cuda.device(device), torch.cuda.stream(torch.cuda.default_stream(device)): + copied.copy_(image) + torch.cuda.default_stream(device).synchronize() + except BaseException as error: + errors.append(error) + finally: + finished.set() + + def record_physics(): + wp.copy(result, source) + entered.set() + assert finished.wait(5), "concurrent image copy did not complete" + + worker = threading.Thread(target=copy_image) + worker.start() + try: + graph = NewtonQueries.capture_graph(device, record_physics) + finally: + entered.set() + worker.join(5) + assert not errors, errors + assert not worker.is_alive() + np.testing.assert_array_equal(result.numpy(), [0.0] * 4) # Recording did not advance physics. + wp.capture_launch(graph) + np.testing.assert_array_equal(result.numpy(), [7.0] * 4) + np.testing.assert_array_equal(copied.cpu().numpy(), [23.0] * 4) + + # --------------------------------------------------------------------------- # Manager state-refresh boundaries (no SimulationContext required) # --------------------------------------------------------------------------- diff --git a/source/isaaclab_ov/changelog.d/kitless-ovxr.added.rst b/source/isaaclab_ov/changelog.d/kitless-ovxr.added.rst new file mode 100644 index 000000000000..0efaa69c6810 --- /dev/null +++ b/source/isaaclab_ov/changelog.d/kitless-ovxr.added.rst @@ -0,0 +1,2 @@ +* Added application-owned OVRTX/OVStage borrowing for OVXR, with render-thread ownership transfer, scene publication, and ordered cleanup. +* Supported CPU scene transforms and camera buffers alongside GPU rendering. diff --git a/source/isaaclab_ov/changelog.d/kitless-ovxr.minor b/source/isaaclab_ov/changelog.d/kitless-ovxr.minor new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_ov/changelog.d/ovxr-camera-products.added.rst b/source/isaaclab_ov/changelog.d/ovxr-camera-products.added.rst new file mode 100644 index 000000000000..0033a33ec79d --- /dev/null +++ b/source/isaaclab_ov/changelog.d/ovxr-camera-products.added.rst @@ -0,0 +1 @@ +* Added leased camera render products and independent camera-panel scheduling to OVXR sessions. diff --git a/source/isaaclab_ov/changelog.d/ovxr-camera-products.minor b/source/isaaclab_ov/changelog.d/ovxr-camera-products.minor new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_ov/isaaclab_ov/cloner/replicate.py b/source/isaaclab_ov/isaaclab_ov/cloner/replicate.py index f350bd6b2626..8a273686d338 100644 --- a/source/isaaclab_ov/isaaclab_ov/cloner/replicate.py +++ b/source/isaaclab_ov/isaaclab_ov/cloner/replicate.py @@ -192,6 +192,9 @@ def prepare(sim: SimulationContext, routing: dict[type[cloner.ReplicateContext], use_ovstage = share_ovstage or value == "1" if share_ovstage and not with_ovphysx: raise ValueError("Shared OVStage cloning requires OVPhysX.") + leased = [renderer for renderer in renderers if renderer._active_ov_libraries_render_lease is not None] + if leased and (not use_ovstage or share_ovstage): + raise RuntimeError("OVXR requires a private rendering OVStage") if share_ovstage and sim.backend_manager.backend is not None and sim.backend_manager.backend.owns_scene: raise RuntimeError("Configure OVRTX cameras before the first OVPhysX reset to share its OVStage.") if use_ovstage: @@ -230,6 +233,15 @@ def prepare(sim: SimulationContext, routing: dict[type[cloner.ReplicateContext], renderer.backend = sim.get_or_create_backend(backend_cfg) renderer.scene = scene if use_ovstage else renderer.backend renderer._use_ovstage = use_ovstage + if renderer in leased: + if renderer.backend._ovxr_session_owner is not None or scene._ovxr_session_owner is not None: + raise RuntimeError("The native pair already has an OVXR lease") + with scene.stage.get_attribute_write_floor() as floor: + floor.wait() + if int(floor.fetch()) != 0: + raise RuntimeError("OVXR session requires an unused private OVStage") + renderer.backend._ovxr_session_owner = renderer + scene._ovxr_session_owner = renderer if renderer.cfg.async_rendering and use_ovstage: logger.warning("Asynchronous OVRTX rendering is unsupported with ovstage; rendering synchronously.") diff --git a/source/isaaclab_ov/isaaclab_ov/ovxr_session.py b/source/isaaclab_ov/isaaclab_ov/ovxr_session.py new file mode 100644 index 000000000000..dd63e8b07227 --- /dev/null +++ b/source/isaaclab_ov/isaaclab_ov/ovxr_session.py @@ -0,0 +1,220 @@ +# 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 + +"""Lifecycle bootstrap for an application-owned OVRTX OVXR pair.""" + +from __future__ import annotations + +import builtins +from typing import TYPE_CHECKING, Any + +from isaaclab.sim import SimulationContext + +from .renderers import OvLibrariesRenderLease, OVRTXRenderer, OVRTXRendererCfg + +_TERMINAL_MODE_SENTINEL = "ISAAC_LAUNCHED_FROM_TERMINAL" + +if TYPE_CHECKING: + import numpy as np + + +def validate_ovxr_session_preparation() -> None: + """Reject ineligible preparation before the caller changes renderer globals.""" + if SimulationContext.instance() is not None: + raise RuntimeError("OVXR bootstrap must create the SimulationContext immediately before the environment") + if hasattr(builtins, _TERMINAL_MODE_SENTINEL) and getattr(builtins, _TERMINAL_MODE_SENTINEL) is not True: + raise RuntimeError("OVXR bootstrap cannot reuse a context in extension mode") + + +class OvxrSessionBootstrap: + """Reserve an OVRTX/OVStage lease before a real environment is constructed.""" + + def __init__(self, sim_cfg: Any, renderer_cfg: OVRTXRendererCfg | None = None): + validate_ovxr_session_preparation() + + self._sim: SimulationContext | None = None + self._lease: OvLibrariesRenderLease | None = None + self._owner_transfer_capability: object | None = None + self._bound = False + self._closed = False + self._installed_terminal_mode_sentinel = False + self._target_frame_readers: dict[str, tuple[Any, Any, Any]] = {} + + if not hasattr(builtins, _TERMINAL_MODE_SENTINEL): + setattr(builtins, _TERMINAL_MODE_SENTINEL, True) + self._installed_terminal_mode_sentinel = True + + try: + sim = SimulationContext(sim_cfg) + # The simulation registry owns the renderer, including cloning and + # cleanup; the XR session only borrows its exact native pair. + renderer = sim.get_or_create_backend(renderer_cfg or OVRTXRendererCfg()) + if not isinstance(renderer, OVRTXRenderer): + raise TypeError(f"OVRTXRendererCfg resolved to unexpected renderer {type(renderer).__name__}") + lease = renderer.acquire_ov_libraries_render_lease() + except Exception: + sim = SimulationContext.instance() + try: + if sim is not None: + sim.clear_instance() + finally: + self._restore_terminal_mode_sentinel() + raise + + self._sim = sim + self._lease = lease + + def capture_scene_publisher(self): + """Capture the application scene publisher before render ownership transfers.""" + self._require_open() + if self._bound or self._lease is None: + raise RuntimeError("Scene publisher is available only before OVXR Stage binding") + return self._lease.capture_scene_publisher() + + def configure_auxiliary_camera(self, camera_cfg: Any, *, max_fps: float | None = None) -> None: + """Authorize one exact camera before environment construction.""" + self._require_open() + if self._bound or self._lease is None: + raise RuntimeError("Auxiliary camera must be configured before OVXR Stage binding") + self._lease.configure_auxiliary_camera(camera_cfg, max_fps=max_fps) + + @property + def simulation_context(self) -> SimulationContext: + """Return the exact pre-created simulation context.""" + self._require_open() + assert self._sim is not None + return self._sim + + def bind_environment(self, env: Any) -> OvLibrariesRenderLease: + """Prepare and bind the completed environment Stage to the reserved pair.""" + self._require_open() + if self._bound: + raise RuntimeError("OVXR bootstrap is already bound") + try: + unwrapped = getattr(env, "unwrapped", env) + if getattr(unwrapped, "sim", None) is not self._sim: + raise RuntimeError("Environment did not reuse the exact pre-created SimulationContext") + scene = getattr(unwrapped, "scene", None) + if scene is None or not hasattr(scene, "stage"): + raise RuntimeError("Environment has no completed InteractiveScene Stage") + num_envs = getattr(unwrapped, "num_envs", None) + if not isinstance(num_envs, int) or num_envs <= 0: + raise RuntimeError(f"Environment has invalid num_envs: {num_envs!r}") + + assert self._sim is not None and self._lease is not None + if not self._lease._materialize_deferred_auxiliary_camera_stage(scene.stage, num_envs): + self._sim.render_context.ensure_prepare_stage(scene.stage, num_envs) + self._lease._validate_live() + self._bound = True + return self._lease + finally: + self._restore_terminal_mode_sentinel() + + def offer_render_owner_transfer(self) -> object: + """Offer the exact unbound OVXR lease before Stage binding.""" + self._require_open() + if self._bound or self._lease is None: + raise RuntimeError("Owner transfer is available only before OVXR Stage binding") + capability = self._lease.offer_owner_transfer() + self._owner_transfer_capability = capability + return capability + + def claim_render_owner_transfer(self, capability: object) -> OvLibrariesRenderLease: + """Claim the offered lease on the worker that will bind, use, and close it.""" + self._require_open() + assert self._lease is not None + self._lease.claim_owner_transfer(capability) + self._owner_transfer_capability = None + return self._lease + + def get_world_transform(self, prim_path: str) -> np.ndarray: + """Read one owned current backend world transform for an exact USD path.""" + self._require_open() + if not self._bound or self._sim is None: + raise RuntimeError("Target-frame transforms require a bound OVXR environment") + if not isinstance(prim_path, str) or not prim_path.startswith("/"): + raise ValueError(f"Target-frame prim path must be absolute: {prim_path!r}") + + import numpy as np + import warp as wp + + from isaaclab.scene_data import SceneDataFormat + + provider = self._sim.get_scene_data_provider() + cached = self._target_frame_readers.get(prim_path) + if cached is None or cached[0] is not provider: + transform_paths = list(provider.backend.transform_paths) + match_count = transform_paths.count(prim_path) + if match_count != 1: + raise RuntimeError( + f"Target-frame prim path {prim_path!r} matched {match_count} live backend transforms" + ) + mapping = provider.create_mapping([prim_path]) + output = SceneDataFormat.Matrix44() + output.matrices = wp.empty(1, dtype=wp.mat44f, device=provider.transform_device) + cached = (provider, mapping, output) + self._target_frame_readers[prim_path] = cached + + provider, mapping, output = cached + if not provider.get_transforms(output, mapping=mapping, allow_passthrough=False, count=1): + raise RuntimeError(f"No SceneData transform conversion is available for target frame {prim_path!r}") + if output.matrices is None: + raise RuntimeError(f"SceneData returned no matrix buffer for target frame {prim_path!r}") + + wp.synchronize_device(str(output.matrices.device)) + world_T_target = np.asarray(output.matrices.numpy()[0], dtype=np.float64) + if world_T_target.shape != (4, 4) or not np.all(np.isfinite(world_T_target)): + raise RuntimeError(f"Target-frame transform {prim_path!r} is missing or non-finite") + if not np.allclose(world_T_target[3], (0.0, 0.0, 0.0, 1.0), atol=1e-5, rtol=0.0): + raise RuntimeError(f"Target-frame transform {prim_path!r} is not homogeneous") + return world_T_target.copy() + + def get_target_frame_T_world(self, prim_path: str) -> np.ndarray: + """Read the current inverse backend transform for one exact USD path.""" + import numpy as np + + world_T_target = self.get_world_transform(prim_path) + try: + target_T_world = np.linalg.inv(world_T_target) + except np.linalg.LinAlgError as exc: + raise RuntimeError(f"Target-frame transform {prim_path!r} is singular") from exc + if not np.all(np.isfinite(target_T_world)): + raise RuntimeError(f"Inverse target-frame transform {prim_path!r} is non-finite") + return target_T_world.astype(np.float32) + + def close(self) -> None: + """Release the lease after every external borrower is destroyed.""" + if self._closed: + return + try: + if self._lease is not None: + if self._owner_transfer_capability is not None: + self._lease.cancel_owner_transfer(self._owner_transfer_capability) + self._owner_transfer_capability = None + self._lease.release() + finally: + self._target_frame_readers.clear() + self._restore_terminal_mode_sentinel() + self._closed = True + + def abort(self) -> None: + """Release and clear a context when environment construction failed.""" + if self._closed: + return + try: + self.close() + finally: + if not self._bound and self._sim is not None and SimulationContext.instance() is self._sim: + self._sim.clear_instance() + + def _require_open(self) -> None: + if self._closed: + raise RuntimeError("OVXR bootstrap is closed") + + def _restore_terminal_mode_sentinel(self) -> None: + if self._installed_terminal_mode_sentinel: + if hasattr(builtins, _TERMINAL_MODE_SENTINEL): + delattr(builtins, _TERMINAL_MODE_SENTINEL) + self._installed_terminal_mode_sentinel = False diff --git a/source/isaaclab_ov/isaaclab_ov/renderers/__init__.pyi b/source/isaaclab_ov/isaaclab_ov/renderers/__init__.pyi index d430a333224a..4706dd080934 100644 --- a/source/isaaclab_ov/isaaclab_ov/renderers/__init__.pyi +++ b/source/isaaclab_ov/isaaclab_ov/renderers/__init__.pyi @@ -7,9 +7,10 @@ __all__ = [ "OVRTXRenderer", "OVRTXRendererCfg", "OVRTXBackendCfg", + "OvLibrariesRenderLease", "Renderer", ] -from .ovrtx_renderer import OVRTXRenderer +from .ovrtx_renderer import OVRTXRenderer, OvLibrariesRenderLease from .ovrtx_renderer import OVRTXRenderer as Renderer from .ovrtx_renderer_cfg import OVRTXBackendCfg, OVRTXRendererCfg diff --git a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py index 09e8260ff24b..f224c63427bd 100644 --- a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py +++ b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py @@ -26,14 +26,18 @@ import logging import math import os +import re import sys import tempfile +import threading import weakref from builtins import ExceptionGroup from collections import deque -from collections.abc import Callable, Iterable, Iterator, Sequence +from collections.abc import Callable, Hashable, Iterable, Iterator, Sequence +from dataclasses import dataclass +from enum import Enum, auto from pathlib import Path -from typing import TYPE_CHECKING, Any, Generic, TypeVar +from typing import TYPE_CHECKING, Any, Generic, TypeVar, cast logger = logging.getLogger(__name__) @@ -107,6 +111,7 @@ if TYPE_CHECKING: from isaaclab_ppisp import PpispPipeline from ovrtx import AttributeBinding, FrameOutput, Operation, PendingFetch, RenderVarOutput + from ovstage import Stage as OvStage from isaaclab.renderers.base_renderer import VisualMaterialBatch @@ -186,12 +191,249 @@ def _write_combined_stage(output_dir: Path, render_product_usd: str) -> None: _write_file(output_dir, "ovrtx_renderer_stage.usda", scene_layer.ExportToString()) +_NO_PHYSICS_STEP_ID = object() + + +class _OvLibrariesRenderLeaseState(Enum): + """Internal lifecycle state for an OVXR pair lease.""" + + UNBOUND = auto() + BOUND = auto() + RELEASED = auto() + + +@dataclass +class _AuxiliaryCamera: + cfg: Any + max_fps: float | None + spec: CameraRenderSpec | None = None + render_data: OVRTXCameraRenderData | None = None + state: tuple[Any, ...] | None = None + product_path: str | None = None + + +class OvLibrariesRenderLease: + """Exclusive non-owning lease over one exact attached OVRTX/OVStage pair. + + The lease is acquired before stage preparation. Stage preparation then + materializes the renderer-owned scene without requiring a camera sensor and + binds this object to the exact live Python ``Renderer`` and ``Stage`` + instances. External consumers must release the lease before the renderer is + closed; they must never detach or destroy either borrowed object. + """ + + def __init__(self) -> None: + raise RuntimeError("OvLibrariesRenderLease must be acquired from OVRTXRenderer") + + @classmethod + def _create_unbound(cls, owner: OVRTXRenderer) -> OvLibrariesRenderLease: + lease = object.__new__(cls) + lease._owner = owner + lease._state = _OvLibrariesRenderLeaseState.UNBOUND + lease._renderer = None + lease._stage = None + lease._generation = 0 + lease._required_environment_scene_partitions = () + lease._external_camera_scene_partitions = () + lease._external_camera_scene_partitions_pending = False + lease._external_scene_partition_proof = None + lease._auxiliary_cameras = [] + return lease + + def configure_auxiliary_camera(self, camera_cfg: Any, *, max_fps: float | None = None) -> None: + """Authorize an exact camera before Stage materialization (at most three).""" + self._validate_active() + if self._state is not _OvLibrariesRenderLeaseState.UNBOUND: + raise RuntimeError("Auxiliary camera configuration must precede Stage materialization") + if camera_cfg is None: + raise ValueError("Auxiliary camera configuration cannot be None") + if any(camera.cfg.prim_path == camera_cfg.prim_path for camera in self._auxiliary_cameras): + raise RuntimeError("This auxiliary camera is already configured") + if len(self._auxiliary_cameras) >= 3: + raise ValueError("At most three auxiliary cameras are supported") + if max_fps is not None and (not math.isfinite(max_fps) or max_fps <= 0.0): + raise ValueError("Auxiliary camera max_fps must be finite and positive [Hz]") + self._auxiliary_cameras.append(_AuxiliaryCamera(camera_cfg, max_fps)) + + @property + def auxiliary_render_product_path(self) -> str | None: + """Return the verified prepared product path, when configured.""" + paths = self.auxiliary_render_product_paths + if len(paths) > 1: + raise RuntimeError("Expected exactly one RenderProduct; use auxiliary_render_product_paths") + return paths[0] if paths else None + + @property + def auxiliary_render_product_paths(self) -> tuple[str, ...]: + self._validate_live() + return cast("OVRTXRenderer", self._owner)._auxiliary_render_product_paths(self) + + def offer_owner_transfer(self) -> object: + """Freeze one unbound lease and return its one-shot owner capability.""" + owner = cast("OVRTXRenderer", self._owner) + return owner._offer_ov_libraries_owner_transfer(self) + + def claim_owner_transfer(self, capability: object) -> None: + cast("OVRTXRenderer", self._owner)._claim_ov_libraries_owner_transfer(self, capability) + + def cancel_owner_transfer(self, capability: object) -> None: + cast("OVRTXRenderer", self._owner)._cancel_ov_libraries_owner_transfer(self, capability) + + def capture_scene_publisher(self) -> OvLibrariesScenePublisher: + """Capture the application-thread scene publisher before render ownership transfers.""" + return cast("OVRTXRenderer", self._owner)._capture_ov_libraries_scene_publisher(self) + + def _materialize_deferred_auxiliary_camera_stage(self, stage: Any, num_envs: int) -> bool: + """Complete the selected camera's deferred Stage on the claimed owner.""" + return cast("OVRTXRenderer", self._owner)._materialize_deferred_auxiliary_camera_stage(self, stage, num_envs) + + def __enter__(self) -> OvLibrariesRenderLease: + self._validate_active() + return self + + def __exit__(self, *_exc: object) -> None: + self.release() + + @property + def ovrtx_renderer(self) -> Renderer: + """Return the exact retained application renderer.""" + self._validate_live() + return cast(Renderer, self._renderer) + + @property + def ovstage(self) -> OvStage: + """Return the exact retained renderer-attached OVStage.""" + self._validate_live() + return cast("OvStage", self._stage) + + @contextlib.contextmanager + def ovstage_owner_lane(self) -> Iterator[None]: + """Serialize one complete external Stage-writing/render operation.""" + self._validate_live() + owner = cast("OVRTXRenderer", self._owner) + with owner._hold_ovstage_owner_lane("external OVXR operation", lease=self): + yield + + def scene_publication_required(self, physics_step_id: Hashable) -> bool: + """Return whether ``physics_step_id`` needs a new scene batch.""" + self._validate_live() + owner = cast("OVRTXRenderer", self._owner) + return owner._external_scene_publication_required(self, physics_step_id) + + def publish_scene(self, physics_step_id: Hashable) -> int: + """Publish one complete live-floor-rebased scene batch. + + This method must be called inside :meth:`ovstage_owner_lane`. Repeating + the same hashable physics-step identity is deduplicated without another + floor query, write, or advance. + """ + self._validate_live() + owner = cast("OVRTXRenderer", self._owner) + return owner._publish_external_scene_ovstage(self, physics_step_id) + + def configure_external_camera_scene_partitions( + self, environment_root_path: str, camera_paths: Sequence[str] + ) -> str: + """Assign the live environment partition token to external cameras. + + This method must be called inside :meth:`ovstage_owner_lane` after the + external camera prims exist. The environment root's resolved token is + authoritative; empty and token-zero values fail closed. + """ + self._validate_live() + owner = cast("OVRTXRenderer", self._owner) + return owner._configure_external_camera_scene_partitions(self, environment_root_path, camera_paths) + + def validate_external_scene_partitions(self, ordinal: int) -> dict[str, str]: + """Read back and prove environment/camera partition equality.""" + self._validate_live() + owner = cast("OVRTXRenderer", self._owner) + return owner._validate_external_scene_partitions_ovstage(self, ordinal) + + def release(self) -> None: + """Release the borrowing intent without destroying either pair member.""" + if self._state is _OvLibrariesRenderLeaseState.RELEASED: + return + owner = cast("OVRTXRenderer", self._owner) + owner._release_ov_libraries_render_lease(self) + self._renderer = None + self._stage = None + self._state = _OvLibrariesRenderLeaseState.RELEASED + + def _bind(self, renderer: Renderer, stage: OvStage, generation: int) -> None: + if self._state is not _OvLibrariesRenderLeaseState.UNBOUND: + raise RuntimeError("OvLibrariesRenderLease can only bind once") + self._renderer = renderer + self._stage = stage + self._generation = generation + self._state = _OvLibrariesRenderLeaseState.BOUND + + def _validate_active(self) -> None: + if self._state is _OvLibrariesRenderLeaseState.RELEASED: + raise RuntimeError("OvLibrariesRenderLease is released") + owner = cast("OVRTXRenderer", self._owner) + owner._validate_active_ov_libraries_render_lease(self) + + def _validate_live(self) -> None: + self._validate_active() + if self._state is not _OvLibrariesRenderLeaseState.BOUND: + raise RuntimeError("OvLibrariesRenderLease is not bound to a prepared Stage") + owner = cast("OVRTXRenderer", self._owner) + owner._validate_live_ov_libraries_render_lease( + self, + cast(Renderer, self._renderer), + cast("OvStage", self._stage), + self._generation, + ) + + +class OvLibrariesScenePublisher: + """Application-thread writer for complete provider-derived OVStage batches.""" + + def __init__(self, owner: OVRTXRenderer, lease: OvLibrariesRenderLease) -> None: + self._owner = owner + self._lease = lease + self._application_thread = threading.current_thread() + self._lock = threading.Lock() + self._closed = False + + def publication_required(self, physics_step_id: Hashable) -> bool: + self._require_application_thread() + return self._owner._producer_scene_publication_required(self._lease, physics_step_id) + + def write_complete_scene(self, ordinal: int, physics_step_id: Hashable) -> None: + """Write, but do not publish, one complete batch at ``ordinal``.""" + self._require_application_thread() + with self._lock: + if self._closed: + raise RuntimeError("OVStage scene publisher is closed") + self._owner._write_producer_scene_batch(self._lease, ordinal, physics_step_id) + + def publish_scene(self, physics_step_id: Hashable) -> int: + """Publish one complete scene batch at the next live OVStage ordinal.""" + self._require_application_thread() + with self._lock: + if self._closed: + raise RuntimeError("OVStage scene publisher is closed") + return self._owner._publish_producer_scene_batch(self._lease, physics_step_id) + + def close(self) -> None: + """Stop scene publication and wait for an in-flight batch to finish.""" + with self._lock: + self._closed = True + + def _require_application_thread(self) -> None: + if threading.current_thread() is not self._application_thread: + raise RuntimeError("OVStage scene publisher must run on its captured application thread") + + class OVRTXBackend: """Own one native renderer, independently of its camera products and optional OVStage.""" def __init__(self, cfg: OVRTXBackendCfg): self.cfg = cfg self.attached = False + self._ovxr_session_owner: OVRTXRenderer | None = None self.render_products: set[str] = set() # Resolve the wheel's native dependency here, including callers without a viewer. dependency = Path(ovstage.__file__).parent / "bin/plugins/libosdCPU.so.3.6.0" @@ -222,12 +464,16 @@ def __init__(self, cfg: OVRTXBackendCfg): def detach(self) -> None: """Release the borrowed stage before it is rebuilt or destroyed.""" + if self._ovxr_session_owner is not None: + raise RuntimeError("Cannot detach a backend with an active OVXR lease") if self.attached: self.renderer.detach_ovstage() self.attached = False def close(self) -> None: """Destroy the engine; the simulation's backend registry releases any borrowed stage afterward.""" + if self._ovxr_session_owner is not None: + raise RuntimeError("Cannot destroy a backend with an active OVXR lease") if self.renderer is not None: # The SDK destroys bindings and detaches only if attached, including partial initialization. self.renderer.destroy() @@ -319,6 +565,20 @@ def supported_output_types(self) -> dict[RenderBufferKind, RenderBufferSpec]: def __init__(self, cfg: OVRTXRendererCfg, *, sim: SimulationContext): super().__init__(cfg, sim=sim) + self._closed = False + self._ov_libraries_render_owner_thread = threading.current_thread() + self._ov_libraries_owner_transfer_lock = threading.Lock() + self._ov_libraries_owner_transfer_capability: object | None = None + self._ov_libraries_owner_transfer_claimed = False + self._active_ov_libraries_render_lease: OvLibrariesRenderLease | None = None + self._ov_libraries_scene_publisher: OvLibrariesScenePublisher | None = None + self._ov_libraries_render_generation = 0 + self._ovstage_owner_lane_operation: str | None = None + self._ovstage_owner_lane_lease: OvLibrariesRenderLease | None = None + self._ovstage_last_scene_physics_step_id: Hashable = _NO_PHYSICS_STEP_ID + self._ovstage_last_scene_ordinal: int | None = None + self._ovstage_pair_poisoned = False + self._auxiliary_camera_stage_deferred = False self._warp_device = wp.get_device(self._sim.device) self._device = str(self._warp_device) self._camera_render_data: list[OVRTXCameraRenderData] = [] @@ -361,6 +621,18 @@ def _create_visual_material_writer(self, batches: tuple[VisualMaterialBatch, ... def prepare_stage(self, stage: Usd.Stage, num_envs: int) -> None: """Load, clone, and bind the shared scene before any camera products are registered.""" + lease = self._active_ov_libraries_render_lease + if lease is not None: + if lease._auxiliary_cameras and not self._ov_libraries_owner_transfer_claimed: + self._auxiliary_camera_stage_deferred = True + return + self._require_ov_libraries_render_owner_thread("prepare the OVXR Stage") + if lease._state is not _OvLibrariesRenderLeaseState.UNBOUND: + raise RuntimeError("Cannot replace the Stage of a bound OVXR pair") + if num_envs != 1: + raise RuntimeError("OVXR session requires exactly one environment") + if not self._use_ovstage or self.backend._ovxr_session_owner is not self: + raise RuntimeError("OVXR requires its reserved private OVStage renderer") logger.info("Preparing stage (%d envs)...", num_envs) env_paths = self.scene.clone_env_paths if self.cfg.temp_usd_dir is not None or not self._use_ovstage or not self.scene.ordinal: @@ -372,8 +644,34 @@ def prepare_stage(self, stage: Usd.Stage, num_envs: int) -> None: ) # fmt: skip if self.cfg.temp_usd_dir is not None: _write_file(Path(self.cfg.temp_usd_dir), "ovrtx_renderer_stage.usda", usd) + if lease is not None and lease._auxiliary_cameras: + usd = self._author_auxiliary_camera_schedules(usd, lease) if self._use_ovstage: - self.scene.populate(usd) + scheduled_paths = ( + [ + camera.spec.camera_prim_paths[0] + for camera in lease._auxiliary_cameras + if camera.max_fps is not None + ] + if lease is not None + else [] + ) + if scheduled_paths: + population = ovstage.population + description = population.Desc( + domains=self.scene.cfg.population_domains, + selectors=[ + population.Selector( + population.PrimPredicate.has_path(*scheduled_paths), + population.PropertyPredicate.has_name( + "omni:sensor:frameRate", "omni:sensor:Core:partialOutputs" + ), + ) + ], + ) + self.scene.populate(usd, population_desc=description) + else: + self.scene.populate(usd) else: self.backend.renderer.open_usd_from_string(usd) ovrtx_replicate(self.backend.renderer, self.scene.clone_copies, env_paths, self.scene.clone_positions) @@ -402,6 +700,11 @@ def prepare_stage(self, stage: Usd.Stage, num_envs: int) -> None: self.scene.resources.callback(self._sim.render_context.reset_stage_prepare_flag) self.scene.resources.callback(self.close) self._initialized_scene = True + if lease is not None: + self._materialize_external_camera_products(lease) + self._auxiliary_camera_stage_deferred = False + self._ov_libraries_render_generation += 1 + lease._bind(self.backend.renderer, self.scene.stage, self._ov_libraries_render_generation) def _setup_xform_bindings(self, stage: Usd.Stage) -> None: """Bind SDP body paths and preserve their authored scales when writing world poses.""" @@ -510,15 +813,34 @@ def _setup_geometry_bindings(self) -> None: self._bindings.callback(self._geometry_points_binding.unbind) def create_render_data(self, spec: CameraRenderSpec) -> OVRTXCameraRenderData: - """Register a camera's product and bindings in the prepared scene.""" + """Register an ordinary camera, or reserve an authorized OVXR camera until handoff.""" + auxiliary = self._accept_auxiliary_camera_spec(spec) + if not auxiliary: + self._raise_if_ov_libraries_render_leased("create ordinary camera render data") if str(wp.get_device(spec.device)) != self._device: raise ValueError("Cameras sharing an OVRTX renderer must use the simulation device.") - render_data = OVRTXCameraRenderData(spec, f"RenderCamera_{self.scene.next_camera_id}") + camera = self._auxiliary_camera_for_spec(spec) if auxiliary else None + if camera is not None and camera.render_data is not None: + raise RuntimeError("Auxiliary camera render data already exists") + data = OVRTXCameraRenderData(spec, f"RenderCamera_{self.scene.next_camera_id}") + if not auxiliary or not self._auxiliary_camera_stage_deferred: + self._register_camera(spec, data) + self.scene.next_camera_id += 1 + self._camera_render_data.append(data) + if camera is not None: + camera.render_data = data + return data + + def _register_camera(self, spec: CameraRenderSpec, render_data: OVRTXCameraRenderData) -> None: + """Bind one product in the already populated simulation-owned scene.""" camera_paths = render_data.camera_paths env_camera = any(camera_paths[0].startswith(path + "/") for path in self.scene.clone_env_paths) scope, product_path = render_data.render_scope_name, render_data.render_product_path usd = build_render_product_as_string( - spec, render_data, device_id=self._warp_device.ordinal, enable_shadows=self.cfg.enable_shadows + spec, + render_data, + device_id=self._warp_device.ordinal if self._warp_device.is_cuda else None, + enable_shadows=self.cfg.enable_shadows, ) try: # Registration mutates the shared scene; finish renders using its existing products first. @@ -603,11 +925,8 @@ def create_render_data(self, spec: CameraRenderSpec) -> OVRTXCameraRenderData: except Exception: render_data.cleanup() raise - self.scene.next_camera_id += 1 - self._camera_render_data.append(render_data) self.backend.render_products.add(render_data.render_product_path) render_data.resources.callback(self.backend.render_products.discard, render_data.render_product_path) - return render_data def set_outputs(self, render_data: OVRTXCameraRenderData, output_data: dict[str, ProxyArray]) -> None: """Register pre-allocated warp output buffers for rendering. @@ -685,7 +1004,7 @@ def _map_render_var_to_dlpack(self, render_var: RenderVarOutput) -> Iterator[wp. Wait for rendering on the consuming stream, or on the host on Linux unless :data:`_DISABLE_LINUX_CUDA_CPU_SYNC_ENV` is ``1``. Consume the zero-copy view inside the context. Unmapping records the consuming stream; native release waits for its queued - reads and the last view to be dropped. + reads and the last view to be dropped. CPU consumers wait on the host and release without a CUDA stream. Args: render_var: OVRTX ``RenderVarOutput`` to map (looked up from ``frame.render_vars``). @@ -693,6 +1012,14 @@ def _map_render_var_to_dlpack(self, render_var: RenderVarOutput) -> Iterator[wp. Yields: The render var's contents as a Warp array, valid for the duration of the context. """ + if not self._warp_device.is_cuda: + mapping = render_var.map(device=Device.CPU) + try: + mapping.wait() + yield wp.from_dlpack(mapping) + finally: + mapping.unmap() + return gpu_side_sync = _gpu_side_render_var_sync_enabled() # OVRTX uses 0 for no synchronization and 1 for Torch's legacy default stream (CUDA handle 0). stream = self._warp_device.stream.cuda_stream or 1 @@ -929,6 +1256,11 @@ def _process_render_products( ) def update_transforms(self) -> None: + """Publish ordinary scene data when the native pair is not leased.""" + self._raise_if_ov_libraries_render_leased("update ordinary transforms") + self._update_transforms() + + def _update_transforms(self) -> None: """Write changed SDP transforms to OVRTX.""" binding = self._object_xform_binding if binding is None: @@ -936,7 +1268,7 @@ def update_transforms(self) -> None: timestamp = self._sdp.backend.transforms_timestamp if self._transforms_timestamp == timestamp: return - stream = self._warp_device.stream + stream = self._warp_device.stream if self._warp_device.is_cuda else None asynchronous = self.cfg.async_rendering and not self._use_ovstage output = self._transform_writes.acquire(stream) if not self._sdp.get_transforms(output, allow_passthrough=not asynchronous, scales=self._object_scales): @@ -952,15 +1284,24 @@ def update_transforms(self) -> None: tensors=xform_tensor_from_warp(matrices), is_array=False, semantic=ovstage.AttributeSemantic.MATRIX, - cuda_stream=stream.cuda_stream or 1, + cuda_stream=(stream.cuda_stream or 1) if stream is not None else None, ).wait() elif asynchronous: self._transform_writes.submit(binding, matrices, stream) else: - binding.write(matrices, data_access=DataAccess.ASYNC, cuda_stream=stream.cuda_stream or 1) + binding.write( + matrices, + data_access=DataAccess.ASYNC, + cuda_stream=(stream.cuda_stream or 1) if stream is not None else None, + ) self._transforms_timestamp = timestamp def update_geometries(self) -> None: + """Publish ordinary scene data when the native pair is not leased.""" + self._raise_if_ov_libraries_render_leased("update ordinary geometries") + self._update_geometries() + + def _update_geometries(self) -> None: """Write changed SDP geometry to OVRTX.""" for callback in self.geometry_callbacks.values(): callback(self) @@ -979,7 +1320,7 @@ def update_geometries(self) -> None: binding = self._geometry_points_binding if binding is None: return - stream = self._warp_device.stream + stream = self._warp_device.stream if self._warp_device.is_cuda else None asynchronous = self.cfg.async_rendering and not self._use_ovstage if asynchronous: output = self._geometry_writes.acquire(stream) @@ -1000,15 +1341,30 @@ def update_geometries(self) -> None: tensors=[points_tensor_from_warp(array) for array in points], is_array=True, semantic=ovstage.AttributeSemantic.POINT, - cuda_stream=stream.cuda_stream or 1, + cuda_stream=(stream.cuda_stream or 1) if stream is not None else None, ).wait() elif asynchronous: self._geometry_writes.submit(binding, points, stream) else: - binding.write(points, data_access=DataAccess.ASYNC, cuda_stream=stream.cuda_stream or 1) + binding.write( + points, + data_access=DataAccess.ASYNC, + cuda_stream=(stream.cuda_stream or 1) if stream is not None else None, + ) self._geometry_timestamp = timestamp - def update_camera( + def update_camera(self, render_data, positions, orientations, intrinsics) -> None: + """Queue selected XR camera poses; ordinary cameras retain their direct update path.""" + lease = self._active_ov_libraries_render_lease + if lease is not None: + for camera in lease._auxiliary_cameras: + if camera.render_data is render_data: + camera.state = (render_data, positions, orientations, intrinsics) + return + self._raise_if_ov_libraries_render_leased("update ordinary cameras") + self._update_camera(render_data, positions, orientations, intrinsics) + + def _update_camera( self, render_data: OVRTXCameraRenderData, positions: ProxyArray, @@ -1019,7 +1375,7 @@ def update_camera( binding = render_data.camera_xform_binding if binding is None: return - stream = self._warp_device.stream + stream = self._warp_device.stream if self._warp_device.is_cuda else None matrices = render_data.camera_writes.acquire(stream) wp.launch( create_camera_transforms_kernel, len(matrices), [positions, orientations, matrices], device=self._device @@ -1032,7 +1388,7 @@ def update_camera( tensors=xform_tensor_from_warp(matrices), is_array=False, semantic=ovstage.AttributeSemantic.MATRIX, - cuda_stream=stream.cuda_stream or 1, + cuda_stream=(stream.cuda_stream or 1) if stream is not None else None, ).wait() else: operation = render_data.camera_writes.submit(binding, matrices, stream) @@ -1040,13 +1396,14 @@ def update_camera( operation.wait() def update_camera_intrinsics(self, render_data: OVRTXCameraRenderData, intrinsics: wp.array, parameters: wp.array): - """Publish calibration columns from GPU memory into the renderer-owned scene.""" + """Publish calibration columns from their producing device into the renderer-owned scene.""" # A runtime calibration change is a scene write for this camera's bindings, so its # in-flight renders must finish first. Other cameras keep their pipelines. + self._raise_if_ov_libraries_render_leased("update ordinary camera intrinsics") errors = self.drain_pending_renders((render_data,)) if errors: raise ExceptionGroup("OVRTX renders failed before calibration update", errors) - stream = wp.get_stream(parameters.device).cuda_stream or 1 + stream = (wp.get_stream(parameters.device).cuda_stream or 1) if parameters.device.is_cuda else None if self._use_ovstage: self.scene.stage.write_attributes( render_data.camera_xform_binding, @@ -1094,6 +1451,7 @@ def render_batch(self, render_data: Sequence[OVRTXCameraRenderData]) -> None: """ if not render_data: return + self._raise_if_ov_libraries_render_leased("render an ordinary camera batch") if not self._initialized_scene: raise RuntimeError("Scene not initialized. Call initialize() first.") if self.backend.renderer is None or not self._camera_render_data: @@ -1162,6 +1520,7 @@ def _remove_usd_reference(self, reference: int) -> None: def close(self) -> None: """Complete borrowed-buffer reads and release bindings; the registry owns the native engine.""" + self._raise_if_ov_libraries_render_leased("close the renderer") try: with contextlib.ExitStack() as resources: resources.callback(self._bindings.close) @@ -1186,6 +1545,525 @@ def close(self) -> None: self.meshes.clear() self.markers.clear() self.geometry_callbacks.clear() + self._closed = True + + def acquire_ov_libraries_render_lease(self) -> OvLibrariesRenderLease: + """Reserve the exact OVRTX/OVStage pair before stage preparation.""" + self._require_ov_libraries_render_owner_thread("acquire a render lease") + if os.environ.get("ISAAC_LAB_OVRTX_USE_OVSTAGE", "0").strip() != "1": + raise RuntimeError("OvLibrariesRenderLease requires ISAAC_LAB_OVRTX_USE_OVSTAGE=1") + if os.environ.get("ISAAC_LAB_SHARE_OVSTAGE") == "1": + raise RuntimeError("OVXR requires a private rendering OVStage") + if self._closed: + raise RuntimeError("Cannot acquire OvLibrariesRenderLease from a closed renderer") + if self._active_ov_libraries_render_lease is not None: + raise RuntimeError("An OvLibrariesRenderLease is already active") + if self.backend is not None or self._initialized_scene or self._ov_libraries_render_generation != 0: + raise RuntimeError("OvLibrariesRenderLease must be acquired before clone preparation") + lease = OvLibrariesRenderLease._create_unbound(self) + self._active_ov_libraries_render_lease = lease + return lease + + def _require_ov_libraries_render_owner_thread(self, operation: str) -> None: + """Reject lease operations outside the renderer construction thread.""" + if self._ov_libraries_owner_transfer_capability is not None: + raise RuntimeError("OVRTXRenderer owner transfer is pending") + if threading.current_thread() is not self._ov_libraries_render_owner_thread: + raise RuntimeError(f"OVRTXRenderer owner thread must {operation}") + + def _offer_ov_libraries_owner_transfer(self, lease: OvLibrariesRenderLease) -> object: + self._require_ov_libraries_render_owner_thread("offer an owner transfer") + with self._ov_libraries_owner_transfer_lock: + if ( + self._active_ov_libraries_render_lease is not lease + or lease._state is not _OvLibrariesRenderLeaseState.UNBOUND + or self._initialized_scene + or (self.backend is not None and self.backend.attached) + or self._ov_libraries_render_generation != 0 + or self._ovstage_owner_lane_operation is not None + or self._ov_libraries_owner_transfer_capability is not None + or self._ov_libraries_owner_transfer_claimed + ): + raise RuntimeError("OVRTXRenderer is not eligible for a pre-use owner transfer") + capability = object() + self._ov_libraries_owner_transfer_capability = capability + return capability + + def _claim_ov_libraries_owner_transfer(self, lease: OvLibrariesRenderLease, capability: object) -> None: + with self._ov_libraries_owner_transfer_lock: + if ( + capability is not self._ov_libraries_owner_transfer_capability + or self._ov_libraries_owner_transfer_claimed + or self._active_ov_libraries_render_lease is not lease + or lease._state is not _OvLibrariesRenderLeaseState.UNBOUND + or self._initialized_scene + or (self.backend is not None and self.backend.attached) + or self._ov_libraries_render_generation != 0 + or self._ovstage_owner_lane_operation is not None + ): + raise RuntimeError("OVRTXRenderer owner transfer capability is invalid") + self._ov_libraries_render_owner_thread = threading.current_thread() + self._ov_libraries_owner_transfer_claimed = True + self._ov_libraries_owner_transfer_capability = None + + def _cancel_ov_libraries_owner_transfer(self, lease: OvLibrariesRenderLease, capability: object) -> None: + with self._ov_libraries_owner_transfer_lock: + # Cancellation is the one original-owner operation allowed while an offer is pending. + if threading.current_thread() is not self._ov_libraries_render_owner_thread: + raise RuntimeError("OVRTXRenderer owner thread must cancel an owner transfer") + if ( + capability is None + or capability is not self._ov_libraries_owner_transfer_capability + or self._active_ov_libraries_render_lease is not lease + ): + raise RuntimeError("OVRTXRenderer owner transfer capability is invalid") + self._ov_libraries_owner_transfer_capability = None + + def _capture_ov_libraries_scene_publisher(self, lease: OvLibrariesRenderLease) -> OvLibrariesScenePublisher: + """Create the scene publisher before render ownership transfers.""" + self._require_ov_libraries_render_owner_thread("capture an OVStage scene publisher") + if ( + self._active_ov_libraries_render_lease is not lease + or lease._state is not _OvLibrariesRenderLeaseState.UNBOUND + or self._ov_libraries_scene_publisher is not None + ): + raise RuntimeError("OVRTXRenderer is not eligible to capture an OVStage scene publisher") + publisher = OvLibrariesScenePublisher(self, lease) + self._ov_libraries_scene_publisher = publisher + return publisher + + @contextlib.contextmanager + def _hold_ovstage_owner_lane( + self, operation: str, *, lease: OvLibrariesRenderLease | None = None + ) -> Iterator[None]: + """Hold the non-reentrant lane shared by IsaacLab and OVXR Stage writers.""" + self._require_ov_libraries_render_owner_thread(f"enter the OVStage owner lane for {operation}") + if self._ovstage_owner_lane_operation is not None: + raise RuntimeError( + "OVStage owner lane is already active for " + f"{self._ovstage_owner_lane_operation}; nested writers are forbidden" + ) + self._ovstage_owner_lane_operation = operation + self._ovstage_owner_lane_lease = lease + try: + yield + finally: + self._ovstage_owner_lane_lease = None + self._ovstage_owner_lane_operation = None + + def _validate_active_ov_libraries_render_lease(self, lease: OvLibrariesRenderLease) -> None: + self._require_ov_libraries_render_owner_thread("access a render lease") + if self._active_ov_libraries_render_lease is not lease: + raise RuntimeError("OvLibrariesRenderLease is not active for this renderer") + if self._closed: + raise RuntimeError("OvLibrariesRenderLease owner is closed or no longer uses OVStage") + + def _validate_live_ov_libraries_render_lease( + self, + lease: OvLibrariesRenderLease, + ovrtx_renderer: Renderer, + ovstage_instance: OvStage, + generation: int, + ) -> None: + """Validate retained Python-object identity for the live attached pair.""" + self._validate_active_ov_libraries_render_lease(lease) + if self._ovstage_pair_poisoned: + raise RuntimeError("OvLibrariesRenderLease owner pair is poisoned") + if generation != self._ov_libraries_render_generation: + raise RuntimeError("OvLibrariesRenderLease has a stale Stage generation") + if ovrtx_renderer is not self.backend.renderer or ovstage_instance is not self.scene.stage: + raise RuntimeError("OvLibrariesRenderLease no longer matches its exact renderer-owned pair") + if not self._initialized_scene or not self.backend.attached: + raise RuntimeError("OvLibrariesRenderLease pair is no longer initialized and attached") + + def _release_ov_libraries_render_lease(self, lease: OvLibrariesRenderLease) -> None: + """Release a borrower after OVXR destruction and rebase the dormant ordinal.""" + self._validate_active_ov_libraries_render_lease(lease) + if self._ovstage_owner_lane_operation is not None: + raise RuntimeError("Cannot release OvLibrariesRenderLease while the owner lane is active") + if lease._state is _OvLibrariesRenderLeaseState.BOUND and (not self._ovstage_pair_poisoned): + with self._hold_ovstage_owner_lane("lease-release ordinal rebase"): + self.scene.ordinal = self._query_next_live_ovstage_ordinal() + self._ovstage_last_scene_physics_step_id = _NO_PHYSICS_STEP_ID + self._ovstage_last_scene_ordinal = None + if self._ov_libraries_scene_publisher is not None: + self._ov_libraries_scene_publisher.close() + self._ov_libraries_scene_publisher = None + self._ov_libraries_owner_transfer_claimed = False + self._active_ov_libraries_render_lease = None + if self.backend is not None: + self.backend._ovxr_session_owner = None + if self.scene is not None: + self.scene._ovxr_session_owner = None + + def _raise_if_ov_libraries_render_leased(self, operation: str) -> None: + """Reject ordinary OVRTX operations while an external lease is active.""" + if getattr(self, "_active_ov_libraries_render_lease", None) is not None or ( + self.backend is not None and self.backend._ovxr_session_owner is not None + ): + raise RuntimeError(f"Cannot {operation} while an OvLibrariesRenderLease is active") + + def _query_next_live_ovstage_ordinal(self) -> int: + """Return ``floor + 1`` from the exact renderer-owned Stage.""" + if self.scene.stage is None: + raise RuntimeError("Cannot query an OVStage floor before scene materialization") + with self.scene.stage.get_attribute_write_floor() as floor_query: + floor_query.wait() + floor = int(floor_query.fetch()) + if floor >= (1 << 64) - 1: + raise RuntimeError("OVStage ordinal range is exhausted") + return floor + 1 + + def _advance_all_ovstage_write_floor(self, ordinal: int) -> None: + """Commit exactly one complete Stage batch.""" + if self.scene.stage is None: + raise RuntimeError("Cannot advance an OVStage floor before scene materialization") + self.scene.stage.advance_write_floor(ordinal=ordinal, scope=ovstage.Scope.ALL).wait() + + def _external_scene_publication_required(self, lease: OvLibrariesRenderLease, physics_step_id: Hashable) -> bool: + self._validate_active_ov_libraries_render_lease(lease) + lease._validate_live() + if self._ovstage_owner_lane_lease is not lease: + raise RuntimeError("scene_publication_required() needs the active lease owner lane") + try: + hash(physics_step_id) + except TypeError as exc: + raise TypeError("physics_step_id must be hashable") from exc + return ( + lease._external_camera_scene_partitions_pending + or self._ovstage_last_scene_physics_step_id != physics_step_id + ) + + def _producer_scene_publication_required(self, lease: OvLibrariesRenderLease, physics_step_id: Hashable) -> bool: + """Check publication on the application thread without entering the render thread.""" + if self._active_ov_libraries_render_lease is not lease or self._ov_libraries_scene_publisher is None: + raise RuntimeError("OVStage scene publisher is not active for this lease") + if self._ovstage_pair_poisoned or lease._state is not _OvLibrariesRenderLeaseState.BOUND: + raise RuntimeError("OVStage scene publisher pair is unavailable") + try: + hash(physics_step_id) + except TypeError as exc: + raise TypeError("physics_step_id must be hashable") from exc + return ( + lease._external_camera_scene_partitions_pending + or self._ovstage_last_scene_physics_step_id != physics_step_id + ) + + def _write_producer_scene_batch( + self, lease: OvLibrariesRenderLease, ordinal: int, physics_step_id: Hashable + ) -> None: + """Write a complete scene batch without advancing its reserved ordinal.""" + if ordinal <= 0: + raise ValueError("OVStage publication ordinal must be nonzero") + if not self._producer_scene_publication_required(lease, physics_step_id): + raise RuntimeError("OVStage scene publication is not required for this complete state") + publish_camera_partitions = lease._external_camera_scene_partitions_pending + try: + self._write_external_scene_components_ovstage( + ordinal, partition_lease=lease if publish_camera_partitions else None + ) + except Exception as exc: + self._ovstage_pair_poisoned = True + raise RuntimeError("OVXR producer scene publication failed; recreate the exact pair") from exc + self._ovstage_last_scene_physics_step_id = physics_step_id + self._ovstage_last_scene_ordinal = ordinal + if publish_camera_partitions: + lease._external_camera_scene_partitions_pending = False + + def _publish_producer_scene_batch(self, lease: OvLibrariesRenderLease, physics_step_id: Hashable) -> int: + """Write and publish a complete scene batch at the next live ordinal.""" + if not self._producer_scene_publication_required(lease, physics_step_id): + if self._ovstage_last_scene_ordinal is None: + raise RuntimeError("OVStage scene deduplication state is inconsistent") + return self._ovstage_last_scene_ordinal + ordinal = self._query_next_live_ovstage_ordinal() + try: + self._write_producer_scene_batch(lease, ordinal, physics_step_id) + self._advance_all_ovstage_write_floor(ordinal) + except Exception as exc: + self._ovstage_pair_poisoned = True + raise RuntimeError("OVXR producer scene publication failed; recreate the exact pair") from exc + self.scene.ordinal = ordinal + 1 + return ordinal + + def _publish_external_scene_ovstage(self, lease: OvLibrariesRenderLease, physics_step_id: Hashable) -> int: + """Write the complete scene at live ``floor + 1`` and advance once.""" + self._validate_active_ov_libraries_render_lease(lease) + lease._validate_live() + if self._ovstage_owner_lane_lease is not lease: + raise RuntimeError("publish_scene() needs the active lease owner lane") + try: + hash(physics_step_id) + except TypeError as exc: + raise TypeError("physics_step_id must be hashable") from exc + if ( + self._ovstage_last_scene_physics_step_id == physics_step_id + and not lease._external_camera_scene_partitions_pending + ): + if self._ovstage_last_scene_ordinal is None: + raise RuntimeError("OVStage scene deduplication state is inconsistent") + return self._ovstage_last_scene_ordinal + + ordinal = self._query_next_live_ovstage_ordinal() + publish_camera_partitions = lease._external_camera_scene_partitions_pending + try: + self._write_external_scene_components_ovstage( + ordinal, partition_lease=lease if publish_camera_partitions else None + ) + self._advance_all_ovstage_write_floor(ordinal) + except Exception as exc: + self._ovstage_pair_poisoned = True + raise RuntimeError("OVXR scene publication failed; recreate the exact pair") from exc + self.scene.ordinal = ordinal + 1 + self._ovstage_last_scene_physics_step_id = physics_step_id + self._ovstage_last_scene_ordinal = ordinal + if publish_camera_partitions: + lease._external_camera_scene_partitions_pending = False + return ordinal + + def _auxiliary_camera_for_spec(self, spec: CameraRenderSpec) -> _AuxiliaryCamera | None: + lease = self._active_ov_libraries_render_lease + if lease is None: + return None + for camera in lease._auxiliary_cameras: + expected = camera.cfg + if spec.num_instances != 1 or spec.view_count != 1 or len(spec.camera_prim_paths) != 1: + raise RuntimeError("OVXR auxiliary camera requires exactly one camera view") + pattern = str(expected.prim_path).replace("{ENV_REGEX_NS}", r"/World/envs/env_[^/]+") + if ( + int(spec.cfg.width) == int(expected.width) + and int(spec.cfg.height) == int(expected.height) + and tuple(spec.cfg.data_types) == tuple(expected.data_types) + and re.fullmatch(f"(?:{pattern})(?:/camera)?", str(spec.camera_prim_paths[0])) is not None + ): + return camera + return None + + def _accept_auxiliary_camera_spec(self, spec: CameraRenderSpec) -> bool: + camera = self._auxiliary_camera_for_spec(spec) + if camera is None: + return False + if camera.spec is None: + camera.spec = spec + elif camera.spec != spec: + raise RuntimeError("Auxiliary camera specification changed after authorization") + return True + + def _materialize_deferred_auxiliary_camera_stage( + self, lease: OvLibrariesRenderLease, stage: Any, num_envs: int + ) -> bool: + """Materialize the selected camera after render-thread ownership transfer.""" + self._require_ov_libraries_render_owner_thread("materialize the auxiliary camera Stage") + if not self._auxiliary_camera_stage_deferred: + return False + if ( + self._active_ov_libraries_render_lease is not lease + or lease._state is not _OvLibrariesRenderLeaseState.UNBOUND + or not self._ov_libraries_owner_transfer_claimed + or not lease._auxiliary_cameras + or any(camera.spec is None or camera.render_data is None for camera in lease._auxiliary_cameras) + ): + raise RuntimeError("Deferred auxiliary camera Stage is not eligible for materialization") + self.prepare_stage(stage, num_envs) + return True + + def _auxiliary_render_product_paths(self, lease: OvLibrariesRenderLease) -> tuple[str, ...]: + """Return products in application selection order, independently of sensor initialization order.""" + self._validate_live_ov_libraries_render_lease(lease, lease.ovrtx_renderer, lease.ovstage, lease._generation) + paths = tuple(camera.product_path for camera in lease._auxiliary_cameras) + if any(not isinstance(path, str) or not path.startswith("/") for path in paths) or len(set(paths)) != len( + paths + ): + raise RuntimeError("Auxiliary camera did not materialize exactly one RenderProduct per selection") + return paths + + def _write_external_scene_components_ovstage( + self, ordinal: int, *, partition_lease: OvLibrariesRenderLease | None + ) -> None: + """Write selected scene components; the caller owns validation and commit.""" + self.scene.ordinal = ordinal + self._update_transforms() + self._update_geometries() + self._update_auxiliary_camera_ovstage() + if partition_lease is not None: + self._write_external_camera_scene_partitions_ovstage(partition_lease, ordinal) + + def _configure_external_camera_scene_partitions( + self, + lease: OvLibrariesRenderLease, + environment_root_path: str, + camera_paths: Sequence[str], + ) -> str: + """Read the authoritative environment token and arm matching camera writes.""" + self._validate_active_ov_libraries_render_lease(lease) + lease._validate_live() + if self._ovstage_owner_lane_lease is not lease: + raise RuntimeError("configure_external_camera_scene_partitions() needs the active lease owner lane") + if not environment_root_path or not camera_paths or any(not path for path in camera_paths): + raise ValueError("One environment root and at least one non-empty external camera path are required") + + floor = self._query_next_live_ovstage_ordinal() - 1 + attribute = "primvars:omni:scenePartition" + actual = self._read_external_scene_partition_tokens_ovstage([environment_root_path], (attribute,), floor) + token = actual.get((environment_root_path, attribute), "") + if token in {"", "0"}: + raise RuntimeError( + f"Environment root {environment_root_path!r} resolved to invalid scene-partition token {token!r}" + ) + + lease._required_environment_scene_partitions = ((environment_root_path, token),) + lease._external_camera_scene_partitions = tuple((str(path), token) for path in camera_paths) + lease._external_camera_scene_partitions_pending = True + lease._external_scene_partition_proof = None + return token + + def _write_external_camera_scene_partitions_ovstage(self, lease: OvLibrariesRenderLease, ordinal: int) -> None: + """Write both OVXR camera tokens inside the complete scene batch.""" + if self.scene.stage is None or self.scene.paths is None: + raise RuntimeError("Cannot write external camera partitions before Stage materialization") + camera_scene_partitions = lease._external_camera_scene_partitions + if not camera_scene_partitions: + raise RuntimeError("No external camera scene partitions are configured") + camera_paths = [path for path, _token in camera_scene_partitions] + token_ids = np.array( + [self.scene.paths.intern_token(token) for _path, token in camera_scene_partitions], dtype=np.uint64 + ) + camera_paths_list = self.scene.paths.create_path_list_from_strings(camera_paths) + camera_query = None + try: + camera_query = self.scene.stage.query_from_path_list(camera_paths_list) + if camera_query is None: + raise RuntimeError("External camera scene-partition query is None") + self.scene.stage.write_attribute( + camera_query, + "omni:scenePartition", + ordinal=ordinal, + tensors=token_ids, + is_array=False, + semantic=ovstage.AttributeSemantic.TOKEN_ID, + ).wait() + finally: + if camera_query is not None: + self.scene.stage.release_query(camera_query).wait() + self.scene.paths.destroy_path_list(camera_paths_list) + + def _read_external_scene_partition_tokens_ovstage( + self, paths: list[str], attributes: tuple[str, ...], ordinal: int + ) -> dict[tuple[str, str], str]: + """Read resolved partition tokens from the exact renderer-owned Stage.""" + if self.scene.stage is None or self.scene.paths is None: + raise RuntimeError("Cannot read scene partitions before Stage materialization") + path_list = self.scene.paths.create_path_list_from_strings(paths) + attribute_tokens = [self.scene.paths.intern_token(attribute) for attribute in attributes] + resolved: dict[tuple[str, str], str] = {} + try: + with self.scene.stage.query_from_path_list(path_list) as query: + query.wait() + with self.scene.stage.read_attributes( + query, attribute_tokens, ovstage.OrdinalRange.latest(ordinal) + ) as read: + read.wait() + for group in read.groups(): + try: + if group.is_delete or group.is_array or group.tensor_count != 1: + continue + attribute = self.scene.paths.token_to_string(group.attribute) + if attribute not in attributes: + continue + group_paths = self.scene.paths.get_path_strings(group.prim_list) + try: + values = np.asarray(group.array(0).copy()).reshape(-1) + except NotImplementedError: + values = torch.from_dlpack(group.dlpack(0)).detach().cpu().numpy().copy().reshape(-1) + logical_count = min(group.prim_count, group.data_count) + for local_index in range(logical_count): + prim_index = group.prim_index(local_index) + data_row_index = group.data_row_index(local_index) + if prim_index >= len(group_paths) or data_row_index >= len(values): + raise RuntimeError("OVStage scene-partition read escaped its group-owned rows") + token_id = int(values[data_row_index]) + token = self.scene.paths.token_to_string(token_id) if token_id else "" + key = (group_paths[prim_index], attribute) + if key in resolved and resolved[key] != token: + raise RuntimeError(f"OVStage returned conflicting scene-partition rows for {key}") + resolved[key] = token + finally: + self.scene.stage.release_group(group) + finally: + self.scene.paths.destroy_path_list(path_list) + return resolved + + def _validate_external_scene_partitions_ovstage( + self, lease: OvLibrariesRenderLease, ordinal: int + ) -> dict[str, str]: + """Prove root and both OVXR eyes resolve to one valid token.""" + self._validate_active_ov_libraries_render_lease(lease) + lease._validate_live() + if self._ovstage_owner_lane_lease is not lease: + raise RuntimeError("validate_external_scene_partitions() needs the active lease owner lane") + expected_rows = [ + *( + (path, "primvars:omni:scenePartition", token) + for path, token in lease._required_environment_scene_partitions + ), + *((path, "omni:scenePartition", token) for path, token in lease._external_camera_scene_partitions), + ] + if len(lease._required_environment_scene_partitions) != 1: + raise RuntimeError("Exactly one authoritative environment scene partition is required") + if len(lease._external_camera_scene_partitions) != 2: + raise RuntimeError("Exactly two external OVXR camera scene partitions are required") + paths = list(dict.fromkeys(path for path, _attribute, _token in expected_rows)) + attributes = tuple(dict.fromkeys(attribute for _path, attribute, _token in expected_rows)) + actual = self._read_external_scene_partition_tokens_ovstage(paths, attributes, ordinal) + partitions: dict[str, str] = {} + mismatches = [] + for path, attribute, expected_token in expected_rows: + actual_token = actual.get((path, attribute), "") + partitions[path] = actual_token + if actual_token in {"", "0"} or actual_token != expected_token: + mismatches.append(f"{path}.{attribute}: expected {expected_token!r}, got {actual_token!r}") + if mismatches: + raise RuntimeError("OVXR scene-partition proof failed: " + "; ".join(mismatches)) + lease._external_scene_partition_proof = dict(partitions) + return partitions + + def _author_auxiliary_camera_schedules(self, usd: str, lease: OvLibrariesRenderLease) -> str: + """Author selected native camera rates on the private export, preserving the application stage.""" + from pxr import Sdf + + layer = Sdf.Layer.CreateAnonymous("ovxr-session.usda") + if not layer.ImportFromString(usd): + raise RuntimeError("Cannot parse the exported OVXR Stage") + stage = Usd.Stage.Open(layer) + for camera in lease._auxiliary_cameras: + if camera.max_fps is None: + continue + if camera.spec is None: + raise RuntimeError("Native-rate auxiliary camera was not prepared") + prim = stage.GetPrimAtPath(camera.spec.camera_prim_paths[0]) + if not prim.IsValid(): + raise RuntimeError("Native-rate auxiliary camera is absent from the exported Stage") + prim.CreateAttribute("omni:sensor:frameRate", Sdf.ValueTypeNames.Double2).Set(Gf.Vec2d(camera.max_fps, 1.0)) + prim.CreateAttribute("omni:sensor:Core:partialOutputs", Sdf.ValueTypeNames.Bool).Set(False) + return stage.ExportToString() + + def _materialize_external_camera_products(self, lease: OvLibrariesRenderLease) -> None: + for camera in lease._auxiliary_cameras: + if camera.spec is None or camera.render_data is None: + raise RuntimeError("Selected auxiliary camera was not prepared") + self._register_camera(camera.spec, camera.render_data) + camera.product_path = camera.render_data.render_product_path + if lease._auxiliary_cameras: + self.scene.commit() + + def _update_auxiliary_camera_ovstage(self) -> None: + lease = self._active_ov_libraries_render_lease + if lease is None: + return + for camera in lease._auxiliary_cameras: + if camera.state is None or camera.render_data.camera_xform_binding is None: + raise RuntimeError("Auxiliary camera has no live world-pose sample") + self._update_camera(*camera.state) class OVRTXMesh: @@ -1516,27 +2394,29 @@ def close(self) -> None: class _AsyncWriteBuffers(Generic[_BufferT]): - """Retain GPU buffers until native writes finish; reuse them in submission order.""" + """Retain buffers until native writes finish; reuse them in submission order.""" def __init__(self, buffers: Iterable[_BufferT] = ()): self._writes: deque[tuple[_BufferT, Operation[bool] | None, wp.Stream | None]] = deque( (buffer, None, None) for buffer in buffers ) - def acquire(self, stream: wp.Stream) -> _BufferT: + def acquire(self, stream: wp.Stream | None) -> _BufferT: """Wait until the next buffer is writable on the given stream.""" buffer, operation, producer = self._writes[0] if operation is not None: operation.wait() - if producer != stream: + if stream is not None and producer is not None and producer != stream: stream.wait_stream(producer) return buffer def submit( - self, binding: AttributeBinding, values: wp.array | list[wp.array], stream: wp.Stream + self, binding: AttributeBinding, values: wp.array | list[wp.array], stream: wp.Stream | None ) -> Operation[bool]: """Submit a write and retain its inputs; a failed submission does not advance the buffers.""" - operation = binding.write_async(values, data_access=DataAccess.ASYNC, cuda_stream=stream.cuda_stream or 1) + operation = binding.write_async( + values, data_access=DataAccess.ASYNC, cuda_stream=(stream.cuda_stream or 1) if stream is not None else None + ) self._writes[0] = (self._writes[0][0], operation, stream) self._writes.rotate(-1) return operation @@ -1547,5 +2427,6 @@ def close(self) -> None: writes.callback(self._writes.clear) for _, operation, producer in self._writes: if operation is not None: - writes.callback(wp.synchronize_stream, producer) + if producer is not None: + writes.callback(wp.synchronize_stream, producer) writes.callback(operation.wait) diff --git a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_usd.py b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_usd.py index 85032bcbd42a..30094ae61f29 100644 --- a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_usd.py +++ b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_usd.py @@ -144,7 +144,7 @@ def build_render_product_as_string( spec: CameraRenderSpec, render_data: OVRTXCameraRenderData, *, - device_id: int, + device_id: int | None = None, enable_shadows: bool = False, ) -> str: """Build a complete render product USD layer as a string. @@ -160,6 +160,7 @@ def build_render_product_as_string( render_data: Camera's render scope and product identity. device_id: CUDA device index the render product is pinned to, so its render var buffers are allocated on the same device as the Warp kernels that read them. + When None, leave native GPU selection automatic for CPU consumers. enable_shadows: Whether lights cast shadows. Defaults to False. Only honored for the ``simple_shading_*`` data types, which are the ones that select RTX Minimal mode. @@ -210,6 +211,8 @@ def build_render_product_as_string( for _, name, source in render_var_configs ) + device_ids_line = "" if device_id is None else f"uint[] deviceIds = [{device_id}]" + layer = f'''#usda 1.0 (defaultPrim = "{render_data.render_scope_name}") @@ -219,7 +222,7 @@ def RenderProduct "RenderProduct" ( prepend apiSchemas = ["OmniRtxSettingsCommonAdvancedAPI_1"] ) {{ rel camera = [<{render_data.camera_paths[0]}>] - uint[] deviceIds = [{device_id}] + {device_ids_line} {bg_type_line} {render_mode_block} token[] omni:rtx:waitForEvents = ["AllLoadingFinished", "OnlyOnFirstRequest"] diff --git a/source/isaaclab_ov/isaaclab_ov/stage.py b/source/isaaclab_ov/isaaclab_ov/stage.py index 3756fe88fa23..1e0c9889d33d 100644 --- a/source/isaaclab_ov/isaaclab_ov/stage.py +++ b/source/isaaclab_ov/isaaclab_ov/stage.py @@ -175,11 +175,12 @@ def __init__(self, cfg: OvstageBackendCfg): self.clone_env_paths: list[str] = [] self.population_env_paths: list[str] = [] self.clone_positions: np.ndarray | None = None + self._ovxr_session_owner = None self.resources = contextlib.ExitStack() """Release borrowers before paths and stage, in reverse registration order.""" self.reset() - def populate(self, usda: str) -> None: + def populate(self, usda: str, *, population_desc: ovstage.population.Desc | None = None) -> None: """Import the exported prototypes and apply the clone context's prepared operations. Population commits ordinal 1. Consumers may then author render products and poses @@ -187,12 +188,18 @@ def populate(self, usda: str) -> None: Args: usda: USD scene containing the routed prototypes and their materials. + population_desc: Optional native property selection for this scene population. """ if self.ordinal: return # Ordinal 0 is the empty state; population and cloning form the first committed write. - ovstage.population.open_usd_from_string(self.stage, usda, ordinal=1, domains=self.cfg.population_domains) + if population_desc is None: + ovstage.population.open_usd_from_string(self.stage, usda, ordinal=1, domains=self.cfg.population_domains) + else: + ovstage.population.open_usd_from_string_with_desc( + self.stage, usda, ordinal=1, time=float("nan"), descs=population_desc + ) ovstage_replicate( self.stage, self.paths, self.clone_copies, self.clone_env_paths, self.clone_positions, ordinal=1 ) @@ -225,6 +232,8 @@ def commit(self) -> int: def close(self) -> None: """Release borrowers, then the path dictionary and stage.""" + if self._ovxr_session_owner is not None: + raise RuntimeError("Cannot destroy or reset a stage with an active OVXR lease") self.resources.close() self.stage = self.paths = None @@ -270,9 +279,11 @@ def export_stage_to_string( settings_path = export_stage.GetMetadata("renderSettingsPrimPath") or "/Render/IsaacLabSettings" settings = UsdRender.Settings.Define(export_stage, settings_path) settings.GetPrim().AddAppliedSchema("OmniRtxSettingsGlobalCommonAPI_1") - settings.GetPrim().CreateAttribute( + show_all = settings.GetPrim().CreateAttribute( "omni:rtx:scenePartitioning:showAllPartitionsByDefault", Sdf.ValueTypeNames.Bool - ).Set(True) + ) + if not show_all.HasAuthoredValueOpinion(): + show_all.Set(True) export_stage.SetMetadata("renderSettingsPrimPath", settings_path) if num_envs > 1: envs_path = Sdf.Path("/World/envs") diff --git a/source/isaaclab_ov/test/test_ovrtx_clone_plan.py b/source/isaaclab_ov/test/test_ovrtx_clone_plan.py index edb38ad2b0ca..0df041f6881c 100644 --- a/source/isaaclab_ov/test/test_ovrtx_clone_plan.py +++ b/source/isaaclab_ov/test/test_ovrtx_clone_plan.py @@ -88,7 +88,12 @@ def _simulation(monkeypatch): def _make_ovrtx_renderer_without_backend() -> OVRTXRenderer: renderer = OVRTXRenderer(OVRTXRendererCfg(), sim=SimulationContext.instance()) renderer.backend = SimpleNamespace( - clone_copies=[], clone_env_paths=[], population_env_paths=[], clone_positions=None, render_products=set() + clone_copies=[], + clone_env_paths=[], + population_env_paths=[], + clone_positions=None, + render_products=set(), + _ovxr_session_owner=None, ) renderer.scene = renderer.backend renderer.backend.renderer = SimpleNamespace( @@ -99,7 +104,7 @@ def _make_ovrtx_renderer_without_backend() -> OVRTXRenderer: write_array_attribute=lambda *args, **kwargs: None, write_attribute=lambda *args, **kwargs: None, ) - renderer._warp_device = SimpleNamespace(ordinal=0) + renderer._warp_device = SimpleNamespace(ordinal=0, is_cuda=True) renderer.scene.next_camera_id = 0 return renderer @@ -279,6 +284,7 @@ def test_create_render_data_writes_combined_stage_on_the_simulation_device(tmp_p class Device: ordinal = 1 + is_cuda = True def __str__(self): return "cuda:1" diff --git a/source/isaaclab_ov/test/test_ovrtx_deformable_bindings.py b/source/isaaclab_ov/test/test_ovrtx_deformable_bindings.py index 529ab0a469b5..d63022ee609d 100644 --- a/source/isaaclab_ov/test/test_ovrtx_deformable_bindings.py +++ b/source/isaaclab_ov/test/test_ovrtx_deformable_bindings.py @@ -42,11 +42,11 @@ def _simulation(monkeypatch): def _make_renderer_without_backend() -> tuple[OVRTXRenderer, MagicMock]: renderer = OVRTXRenderer(OVRTXRendererCfg(), sim=SimulationContext.instance()) - renderer.backend = SimpleNamespace(renderer=MagicMock()) + renderer.backend = SimpleNamespace(renderer=MagicMock(), _ovxr_session_owner=None) renderer.scene = renderer.backend renderer.scene.query = OvstageBackend.query.__get__(renderer.scene) renderer._device = "cpu" - renderer._warp_device = SimpleNamespace(ordinal=0, stream=SimpleNamespace(cuda_stream=99)) + renderer._warp_device = SimpleNamespace(ordinal=0, is_cuda=True, stream=SimpleNamespace(cuda_stream=99)) return renderer, renderer.backend.renderer @@ -57,7 +57,7 @@ def test_geometry_bindings_follow_mixed_sdp_points_and_pointer_swaps(mode): renderer.cfg.async_rendering = mode == "async" use_ovstage = mode == "ovstage" renderer._use_ovstage = use_ovstage - renderer._warp_device = SimpleNamespace(stream=SimpleNamespace(cuda_stream=42)) + renderer._warp_device = SimpleNamespace(is_cuda=True, stream=SimpleNamespace(cuda_stream=42)) points = { path: wp.array(np.arange(count * 3, dtype=np.float32).reshape(count, 3), dtype=wp.vec3f, device="cpu") for path, count in ( @@ -189,7 +189,7 @@ def test_update_transforms_consumes_sdp_matrices_once_per_publication(monkeypatc stage = Usd.Stage.CreateInMemory() UsdGeom.Xform.Define(stage, paths[0]).AddScaleOp().Set(Gf.Vec3d(2, 3, 4) if scaled else Gf.Vec3d(1)) renderer.scene.clone_copies = [(paths[0], [])] - renderer._warp_device = SimpleNamespace(stream=SimpleNamespace(cuda_stream=0)) + renderer._warp_device = SimpleNamespace(is_cuda=True, stream=SimpleNamespace(cuda_stream=0)) renderer._use_ovstage = use_ovstage renderer.scene.ordinal = 5 writes = [] diff --git a/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py b/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py index 9831b5931465..e303972f6433 100644 --- a/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py +++ b/source/isaaclab_ov/test/test_ovrtx_renderer_contract.py @@ -81,6 +81,7 @@ def _make_ovrtx_camera_render_data() -> OVRTXCameraRenderData: def _make_ovrtx_renderer_without_backend() -> OVRTXRenderer: renderer = OVRTXRenderer(OVRTXRendererCfg(), sim=SimulationContext.instance()) renderer.backend = OVRTXBackend.__new__(OVRTXBackend) + renderer.backend._ovxr_session_owner = None renderer.scene = renderer.backend from isaaclab_ov.stage import OvstageBackend @@ -960,7 +961,7 @@ def test_ovrtx_render_var_sync_rejects_non_boolean_values(monkeypatch, value): @pytest.mark.parametrize( ("gpu_side", "cuda_stream", "expected_stream", "consumer_fails"), - [(True, 0, 1, False), (True, 99, 99, True), (False, 0, 1, False)], + [(True, 0, 1, False), (True, 99, 99, True), (False, 0, 1, False), (False, None, None, True)], ) def test_ovrtx_map_render_var_orders_the_read_against_render_completion( monkeypatch, gpu_side, cuda_stream, expected_stream, consumer_fails @@ -968,15 +969,17 @@ def test_ovrtx_map_render_var_orders_the_read_against_render_completion( """Order reads after rendering and release after reads, including when the consumer raises.""" events = [] mapping = types.SimpleNamespace( - wait=lambda: events.append("wait"), unmap=lambda *, stream: events.append(("release", stream)) + wait=lambda: events.append("wait"), unmap=lambda *, stream=None: events.append(("release", stream)) ) render_var = types.SimpleNamespace(map=MagicMock(return_value=mapping)) sentinel = object() monkeypatch.setattr(ovrtx_renderer_module, "_gpu_side_render_var_sync_enabled", lambda: gpu_side) - monkeypatch.setattr(ovrtx_renderer_module.wp, "from_dlpack", lambda mapping: sentinel) + monkeypatch.setattr(ovrtx_renderer_module.wp, "from_dlpack", lambda mapping: events.append("wrap") or sentinel) renderer = _make_ovrtx_renderer_without_backend() - renderer._warp_device = types.SimpleNamespace(stream=types.SimpleNamespace(cuda_stream=cuda_stream)) + renderer._warp_device = types.SimpleNamespace( + is_cuda=cuda_stream is not None, stream=types.SimpleNamespace(cuda_stream=cuda_stream) + ) with pytest.raises(ValueError, match="consumer failed") if consumer_fails else contextlib.nullcontext(): with renderer._map_render_var_to_dlpack(render_var) as array: assert array is sentinel @@ -984,8 +987,11 @@ def test_ovrtx_map_render_var_orders_the_read_against_render_completion( if consumer_fails: raise ValueError("consumer failed") - assert render_var.map.call_args.kwargs["sync_stream"] == (expected_stream if gpu_side else 0) - assert events == ([] if gpu_side else ["wait"]) + ["consume", ("release", expected_stream)] + if cuda_stream is None: + assert render_var.map.call_args.kwargs == {"device": ovrtx_renderer_module.Device.CPU} + else: + assert render_var.map.call_args.kwargs["sync_stream"] == (expected_stream if gpu_side else 0) + assert events == ([] if gpu_side else ["wait"]) + ["wrap", "consume", ("release", expected_stream)] @pytest.mark.parametrize( @@ -1091,7 +1097,7 @@ def test_pose_and_intrinsic_updates_target_the_given_camera(monkeypatch, use_ovs assert not vars(renderer).keys() & {"_camera_xform_binding", "_camera_xform_query", "_camera_paths_list"} monkeypatch.setattr(wp, "get_stream", lambda device: types.SimpleNamespace(cuda_stream=99)) monkeypatch.setattr(wp, "synchronize_stream", lambda stream: None) - renderer._warp_device = types.SimpleNamespace(stream=types.SimpleNamespace(cuda_stream=99)) + renderer._warp_device = types.SimpleNamespace(is_cuda=True, stream=types.SimpleNamespace(cuda_stream=99)) monkeypatch.setattr(ovrtx_renderer_module, "xform_tensor_from_warp", lambda value: value) positions = wp.array([[1, 2, 3], [4, 5, 6]], dtype=wp.vec3f, device="cpu") rotations = np.array([[1, 2, 3, 4], [0, 0, 0, 1]], dtype=np.float32) @@ -1115,6 +1121,7 @@ def test_pose_and_intrinsic_updates_target_the_given_camera(monkeypatch, use_ovs renderer.update_camera_intrinsics(cameras[1], wp.zeros(2, dtype=wp.mat33f, device="cpu"), parameters) if use_ovstage: assert renderer.scene.stage.write_attributes.call_args.args[0] is cameras[1].camera_xform_binding + assert all(desc.cuda_stream is None for desc in renderer.scene.stage.write_attributes.call_args.args[1]) bound_paths = [call.args[0] for call in renderer.scene.paths.create_path_list_from_strings.call_args_list] assert bound_paths == [ entry @@ -1173,6 +1180,7 @@ def query(paths): events.append(f"release_query:{paths}") renderer.scene = OvstageBackend.__new__(OvstageBackend) + renderer.scene._ovxr_session_owner = None SimulationContext.instance()._backend_registry.insert( 0, (OvstageBackendCfg(scene_key=renderer.cfg), renderer.scene) ) @@ -1234,3 +1242,566 @@ def fail_release(): events.clear() renderer.close() assert events == [] + + +@pytest.fixture +def ovxr_session_renderer(monkeypatch): + """Exercise real deferred resource acquisition with native SDK endpoints doubled.""" + import isaaclab_ov.stage as stage_module + + monkeypatch.setenv("ISAAC_LAB_OVRTX_USE_OVSTAGE", "1") + native, stage, paths = MagicMock(), MagicMock(), MagicMock() + floor = stage.get_attribute_write_floor.return_value.__enter__.return_value + floor.fetch.return_value = 0 + + def advance(*, ordinal, **kwargs): + return types.SimpleNamespace(wait=lambda: setattr(floor.fetch, "return_value", ordinal)) + + stage.advance_write_floor.side_effect = advance + monkeypatch.setattr(ovrtx_renderer_module, "Renderer", lambda _cfg: native) + monkeypatch.setattr(ovrtx_renderer_module, "redirect_shader_cache", lambda _cfg: None) + monkeypatch.setattr(stage_module, "create_ovstage", lambda _name: contextlib.nullcontext(stage)) + monkeypatch.setattr(stage_module.ovstage, "PathDictionary", lambda _: contextlib.nullcontext(paths)) + monkeypatch.setattr(stage_module.ovstage.population, "open_usd_from_string", lambda *args, **kwargs: None) + renderer = SimulationContext.instance().get_or_create_backend(OVRTXRendererCfg()) + assert renderer.backend is renderer.scene is None + return renderer + + +def _prepare_ovxr_renderer(renderer, *, bind=True): + from isaaclab_ov.cloner import OvrtxReplicateContext + + OvrtxReplicateContext.prepare(renderer._sim, {}) + if bind: + renderer.prepare_stage(renderer._sim.stage, 1) + + +def test_ovxr_session_lease_borrows_registry_pair_and_blocks_shared_users(ovxr_session_renderer): + renderer = ovxr_session_renderer + shared = SimulationContext.instance().get_or_create_backend(renderer.cfg) + assert shared is renderer + lease = renderer.acquire_ov_libraries_render_lease() + with pytest.raises(RuntimeError, match="already active"): + shared.acquire_ov_libraries_render_lease() + _prepare_ovxr_renderer(renderer) + with pytest.raises(RuntimeError, match="Lease"): + shared.render_batch((object(),)) + shared.render_batch(()) + with pytest.raises(RuntimeError, match="active OVXR lease"): + SimulationContext.instance().close_backend(renderer.backend) + with pytest.raises(RuntimeError, match="active OVXR lease"): + renderer.scene.reset() + native = lease.ovrtx_renderer + assert native is shared.backend.renderer + assert lease.ovstage is shared.scene.stage + lease.release() + lease.release() + assert renderer.backend._ovxr_session_owner is None + assert renderer.scene._ovxr_session_owner is None + native.destroy.assert_not_called() + SimulationContext.instance().close_backend(renderer.backend) + native.destroy.assert_called_once_with() + + +def test_ovxr_session_rejects_acquisition_after_clone_preparation(ovxr_session_renderer): + renderer = ovxr_session_renderer + _prepare_ovxr_renderer(renderer, bind=False) + with pytest.raises(RuntimeError, match="before clone preparation"): + renderer.acquire_ov_libraries_render_lease() + assert renderer.backend._ovxr_session_owner is None + + +def test_ovxr_session_lease_rejects_calibration_before_scene_access(ovxr_session_renderer, monkeypatch): + """An ordinary calibration write cannot bypass the external owner's publication lane.""" + renderer = ovxr_session_renderer + lease = renderer.acquire_ov_libraries_render_lease() + camera = _make_ovrtx_camera_render_data() + intrinsics = wp.zeros(2, dtype=wp.mat33f, device="cpu") + parameters = wp.zeros((5, 2), dtype=wp.float32, device="cpu") + drain = MagicMock(return_value=[]) + monkeypatch.setattr(renderer, "drain_pending_renders", drain) + monkeypatch.setattr(wp, "get_stream", lambda device: types.SimpleNamespace(cuda_stream=99)) + try: + with pytest.raises(RuntimeError, match="Lease"): + renderer.update_camera_intrinsics(camera, intrinsics, parameters) + drain.assert_not_called() + assert renderer.backend is renderer.scene is None + finally: + lease.release() + + +def test_ovxr_session_owner_transfer_is_single_use(ovxr_session_renderer): + import threading + + renderer = ovxr_session_renderer + lease = renderer.acquire_ov_libraries_render_lease() + token = lease.offer_owner_transfer() + errors = [] + + def owner(): + try: + lease.claim_owner_transfer(token) + with pytest.raises(RuntimeError, match="capability is invalid"): + lease.claim_owner_transfer(token) + lease.release() + except BaseException as error: + errors.append(error) + + thread = threading.Thread(target=owner) + thread.start() + thread.join(timeout=5) + assert not thread.is_alive() + assert not errors + assert renderer._ov_libraries_render_owner_thread is thread + assert renderer.backend is renderer.scene is None + + +def test_ovxr_session_publication_rebases_deduplicates_and_poison_fails_closed(ovxr_session_renderer, monkeypatch): + renderer = ovxr_session_renderer + lease = renderer.acquire_ov_libraries_render_lease() + _prepare_ovxr_renderer(renderer) + floor = renderer.scene.stage.get_attribute_write_floor.return_value.__enter__.return_value + floor.fetch.return_value = 17 + transforms = MagicMock() + monkeypatch.setattr(renderer, "_update_transforms", transforms) + monkeypatch.setattr(renderer, "_update_geometries", lambda: None) + with lease.ovstage_owner_lane(): + assert lease.publish_scene("step-1") == 18 + assert lease.publish_scene("step-1") == 18 + transforms.assert_called_once_with() + transforms.side_effect = ValueError("write failed") + with lease.ovstage_owner_lane(): + with pytest.raises(RuntimeError, match="scene publication failed"): + lease.publish_scene("step-2") + with pytest.raises(RuntimeError, match="poisoned"): + _ = lease.ovstage + lease.release() + + +@pytest.fixture +def complete_scene_publication(ovxr_session_renderer, monkeypatch, request): + """Use public A/B entry points with only native component endpoints doubled.""" + renderer = ovxr_session_renderer + route = request.param + lease = renderer.acquire_ov_libraries_render_lease() + publisher = lease.capture_scene_publisher() if route != "publish-B" else None + _prepare_ovxr_renderer(renderer) + assert (renderer._ov_libraries_scene_publisher is None) == (route == "publish-B") + lease._external_camera_scene_partitions = (("/Camera/left", "env"), ("/Camera/right", "env")) + lease._external_camera_scene_partitions_pending = True + stage = renderer.scene.stage + floor = stage.get_attribute_write_floor.return_value.__enter__.return_value + floor.fetch.return_value = 17 + stage.get_attribute_write_floor.reset_mock() + state = types.SimpleNamespace( + renderer=renderer, + lease=lease, + publisher=publisher, + route=route, + ordinal=23 if route == "raw-A" else 18, + events=[], + failure=None, + cause=ValueError("component endpoint failed"), + initial_id=renderer._ovstage_last_scene_physics_step_id, + ) + + def component(name): + state.events.append((name, renderer.scene.ordinal)) + if state.failure == name: + raise state.cause + + monkeypatch.setattr(renderer, "_update_transforms", lambda: component("transform")) + monkeypatch.setattr(renderer, "_update_geometries", lambda: component("geometry")) + monkeypatch.setattr(renderer, "_update_auxiliary_camera_ovstage", lambda: component("auxiliary")) + + def partitions(selected, ordinal): + assert selected is lease + assert selected._external_camera_scene_partitions == (("/Camera/left", "env"), ("/Camera/right", "env")) + assert ordinal == renderer.scene.ordinal + component("partition") + + monkeypatch.setattr(renderer, "_write_external_camera_scene_partitions_ovstage", partitions) + + def advance(*, ordinal, scope): + assert scope is ovrtx_renderer_module.ovstage.Scope.ALL + assert ordinal == renderer.scene.ordinal + + def wait(): + component("seal") + # A's convenience method has already committed its raw batch; B has not. + assert renderer._ovstage_last_scene_physics_step_id == ( + "step" if route == "publish-A" else state.initial_id + ) + assert lease._external_camera_scene_partitions_pending == (False if route == "publish-A" else state.pending) + floor.fetch.return_value = ordinal + + return types.SimpleNamespace(wait=wait) + + stage.advance_write_floor.reset_mock() + stage.advance_write_floor.side_effect = advance + + def publish(step_id="step"): + if route == "raw-A": + return publisher.write_complete_scene(state.ordinal, step_id) + if route == "publish-A": + return publisher.publish_scene(step_id) + with lease.ovstage_owner_lane(): + return lease.publish_scene(step_id) + + state.publish = publish + state.pending = True + try: + yield state + finally: + lease.release() + + +@pytest.mark.parametrize("complete_scene_publication", ["raw-A", "publish-A", "publish-B"], indirect=True) +@pytest.mark.parametrize("pending", [False, True]) +def test_ovxr_session_complete_scene_components_share_the_supplied_ordinal(complete_scene_publication, pending): + state = complete_scene_publication + renderer, lease = state.renderer, state.lease + state.pending = lease._external_camera_scene_partitions_pending = pending + result = state.publish() + assert result == (None if state.route == "raw-A" else state.ordinal) + names = ["transform", "geometry", "auxiliary"] + (["partition"] if pending else []) + if state.route != "raw-A": + names.append("seal") + assert state.events == [(name, state.ordinal) for name in names] + assert renderer._ovstage_last_scene_physics_step_id == "step" + assert renderer._ovstage_last_scene_ordinal == state.ordinal + assert not lease._external_camera_scene_partitions_pending + assert renderer.scene.ordinal == state.ordinal + (state.route != "raw-A") + assert renderer.scene.stage.get_attribute_write_floor.call_count == (state.route != "raw-A") + assert renderer.scene.stage.advance_write_floor.call_count == (state.route != "raw-A") + if state.route == "raw-A": + with pytest.raises(RuntimeError, match="publication is not required"): + state.publish() + else: + assert state.publish() == state.ordinal + assert state.events == [(name, state.ordinal) for name in names] + assert renderer.scene.stage.get_attribute_write_floor.call_count == (state.route != "raw-A") + assert renderer.scene.stage.advance_write_floor.call_count == (state.route != "raw-A") + assert not renderer._ovstage_pair_poisoned + + +@pytest.mark.parametrize( + "complete_scene_publication,failure", + [ + (route, failure) + for route in ("raw-A", "publish-A", "publish-B") + for failure in ("transform", "geometry", "auxiliary", "partition") + ] + + [(route, "seal") for route in ("publish-A", "publish-B")], + indirect=["complete_scene_publication"], +) +def test_ovxr_session_complete_scene_failure_preserves_route_commit_boundary(complete_scene_publication, failure): + state = complete_scene_publication + renderer, lease = state.renderer, state.lease + state.failure = failure + message = "producer scene publication failed" if state.route != "publish-B" else "scene publication failed" + with pytest.raises(RuntimeError, match=message) as caught: + state.publish() + cause = caught.value + while cause.__cause__ is not None: + cause = cause.__cause__ + assert cause is state.cause + order = ["transform", "geometry", "auxiliary", "partition", "seal"] + assert state.events == [(name, state.ordinal) for name in order[: order.index(failure) + 1]] + committed = failure == "seal" and state.route == "publish-A" + assert renderer._ovstage_last_scene_physics_step_id == ("step" if committed else state.initial_id) + assert renderer._ovstage_last_scene_ordinal == (state.ordinal if committed else None) + assert lease._external_camera_scene_partitions_pending == (not committed) + assert renderer.scene.ordinal == state.ordinal + assert renderer.scene.stage.advance_write_floor.call_count == (failure == "seal") + assert renderer.scene.stage.get_attribute_write_floor.call_count == (state.route != "raw-A") + assert renderer._ovstage_pair_poisoned + with pytest.raises(RuntimeError, match="poisoned"): + _ = lease.ovstage + previous_events = list(state.events) + with pytest.raises(RuntimeError, match="unavailable|poisoned"): + state.publish("next-step") + assert state.events == previous_events + assert renderer.scene.stage.advance_write_floor.call_count == (failure == "seal") + assert renderer.scene.stage.get_attribute_write_floor.call_count == (state.route != "raw-A") + + +@pytest.mark.parametrize("complete_scene_publication", ["raw-A", "publish-A", "publish-B"], indirect=True) +def test_ovxr_session_complete_scene_public_authority_precedes_components(complete_scene_publication): + import threading + + state = complete_scene_publication + errors = [] + + def foreign_thread(): + try: + state.publish() + except BaseException as exc: + errors.append(exc) + + thread = threading.Thread(target=foreign_thread) + thread.start() + thread.join(timeout=5) + assert not thread.is_alive() + assert len(errors) == 1 and isinstance(errors[0], RuntimeError) + assert "thread" in str(errors[0]) + with pytest.raises(TypeError, match="physics_step_id must be hashable"): + state.publish([]) + if state.route == "raw-A": + with pytest.raises(ValueError, match="ordinal must be nonzero"): + state.publisher.write_complete_scene(0, "step") + if state.route == "publish-B": + assert state.publisher is None + with pytest.raises(RuntimeError, match="active lease owner lane"): + state.lease.publish_scene("step") + else: + state.publisher.close() + with pytest.raises(RuntimeError, match="publisher is closed"): + state.publish() + assert not state.events + state.renderer.scene.stage.get_attribute_write_floor.assert_not_called() + state.renderer.scene.stage.advance_write_floor.assert_not_called() + assert not state.renderer._ovstage_pair_poisoned + + +@pytest.mark.parametrize("use_ovstage, asynchronous", [(True, False), (False, False), (False, True)]) +def test_cpu_scene_publication_uses_host_tensors_without_a_cuda_stream(use_ovstage, asynchronous): + renderer = _make_ovrtx_renderer_without_backend() + renderer._use_ovstage = use_ovstage + renderer.cfg = OVRTXRendererCfg(async_rendering=asynchronous) + renderer._warp_device = wp.get_device("cpu") + renderer._device = "cpu" + renderer._object_xform_binding = MagicMock() + renderer._geometry_points_binding = MagicMock() + renderer._transforms_timestamp = renderer._geometry_timestamp = None + renderer._object_scales = None + renderer._geometry_paths = ["/World/Object"] + matrices = wp.array(np.eye(4, dtype=np.float64)[None], dtype=wp.mat44d, device="cpu") + points = wp.array([[1.0, 2.0, 3.0]], dtype=wp.vec3f, device="cpu") + renderer._transform_writes = _AsyncWriteBuffers([types.SimpleNamespace(matrices=matrices)]) + renderer._geometry_writes = _AsyncWriteBuffers([wp.empty_like(points)]) + renderer._geometry_offsets = {"/World/Object": 0} + + def get_points(*, output=None, offsets=None): + if output is None: + return {"/World/Object": points} + wp.copy(output, points) + return None + + renderer._sdp = types.SimpleNamespace( + backend=types.SimpleNamespace(transforms_timestamp=1, geometry_timestamp=1), + get_transforms=lambda *_args, **_kwargs: True, + get_geometry_points=get_points, + ) + stage = MagicMock() + renderer.scene = types.SimpleNamespace(stage=stage, ordinal=11) + renderer.update_transforms() + renderer.update_geometries() + bindings = (renderer._object_xform_binding, renderer._geometry_points_binding) + writes = [getattr(binding, "write_async" if asynchronous else "write") for binding in bindings] + calls = stage.write_attribute.call_args_list if use_ovstage else [write.call_args for write in writes] + assert len(calls) == 2 and all(c.kwargs["cuda_stream"] is None for c in calls) + if use_ovstage: + assert [c.args[1] for c in calls] == ["omni:xform", "points"] + assert all(c.kwargs["ordinal"] == 11 for c in calls) + else: + np.testing.assert_array_equal(calls[0].args[0].numpy(), matrices.numpy()) + np.testing.assert_array_equal(calls[1].args[0][0].numpy(), points.numpy()) + # A repeated publication of unchanged state must not duplicate either write. + renderer.update_transforms() + renderer.update_geometries() + assert stage.write_attribute.call_count == 2 if use_ovstage else all(write.call_count == 1 for write in writes) + camera = _make_ovrtx_camera_render_data() + camera.camera_xform_binding = MagicMock() + camera.camera_writes = _AsyncWriteBuffers([wp.empty(2, dtype=wp.mat44d, device="cpu")]) + positions = wp.array([[1.0, 2.0, 3.0]] * 2, dtype=wp.vec3f, device="cpu") + orientations = wp.array([[0.0, 0.0, 0.0, 1.0]] * 2, dtype=wp.quatf, device="cpu") + update = renderer.update_camera + update(camera, positions, orientations, None) + call = stage.write_attribute.call_args if use_ovstage else camera.camera_xform_binding.write_async.call_args + assert call.kwargs["cuda_stream"] is None + if use_ovstage: + assert call.args == (camera.camera_xform_binding, "omni:xform") + assert call.kwargs["ordinal"] == 11 + else: + # Reusing the camera ring waits for previous writes without a CUDA stream. + update(camera, positions, orientations, None) + update(camera, positions, orientations, None) + camera.camera_writes.close() + renderer._transform_writes.close() + renderer._geometry_writes.close() + + +@pytest.mark.parametrize("cuda_ordinal", [None, 2]) +def test_camera_registration_authors_valid_render_device_selection(monkeypatch, cuda_ordinal): + """CPU camera consumers leave GPU selection automatic; CUDA consumers retain their device.""" + from pxr import Usd + + class AuthoredProduct(Exception): + pass + + renderer = _make_ovrtx_renderer_without_backend() + renderer.cfg = OVRTXRendererCfg() + + class Device: + is_cuda = cuda_ordinal is not None + ordinal = cuda_ordinal + + def __str__(self): + return f"cuda:{cuda_ordinal}" if self.is_cuda else "cpu" + + renderer._warp_device = Device() + renderer._device = str(renderer._warp_device) + monkeypatch.setattr(ovrtx_renderer_module.wp, "get_device", lambda _device: renderer._warp_device) + renderer.scene.clone_env_paths = [] + renderer.drain_pending_renders = lambda: [] + spec = types.SimpleNamespace( + cfg=_make_camera_cfg(["rgb"]), + device=renderer._device, + num_instances=1, + camera_prim_paths=["/World/envs/env_0/Camera"], + render_settings={}, + ) + build_product = ovrtx_renderer_module.build_render_product_as_string + + def consume_product(spec, render_data, **kwargs): + stage = Usd.Stage.CreateInMemory() + assert stage.GetRootLayer().ImportFromString(build_product(spec, render_data, **kwargs)) + product = stage.GetPrimAtPath(render_data.render_product_path) + devices = product.GetAttribute("deviceIds") + if cuda_ordinal is None: + assert not devices.HasAuthoredValueOpinion() + else: + assert list(devices.Get()) == [cuda_ordinal] + # Native population is outside this parser/selection regression. + raise AuthoredProduct + + monkeypatch.setattr(ovrtx_renderer_module, "build_render_product_as_string", consume_product) + with pytest.raises(AuthoredProduct): + renderer.create_render_data(spec) + + +def test_external_camera_native_rate_is_authored_only_on_export(ovxr_session_renderer): + from pxr import Usd, UsdGeom + + renderer = ovxr_session_renderer + lease = renderer.acquire_ov_libraries_render_lease() + cfg = _make_camera_cfg(["rgb"]) + cfg.prim_path = "/World/envs/env_0/Camera" + lease.configure_auxiliary_camera(cfg, max_fps=20.0) + lease._auxiliary_cameras[0].spec = types.SimpleNamespace(camera_prim_paths=[cfg.prim_path]) + stage = Usd.Stage.CreateInMemory() + camera = UsdGeom.Camera.Define(stage, cfg.prim_path).GetPrim() + exported = Usd.Stage.CreateInMemory() + assert exported.GetRootLayer().ImportFromString( + renderer._author_auxiliary_camera_schedules(stage.ExportToString(), lease) + ) + assert not camera.HasAttribute("omni:sensor:frameRate") + assert not camera.HasAttribute("omni:sensor:Core:partialOutputs") + prim = exported.GetPrimAtPath(cfg.prim_path) + assert tuple(prim.GetAttribute("omni:sensor:frameRate").Get()) == (20.0, 1.0) + assert prim.GetAttribute("omni:sensor:Core:partialOutputs").Get() is False + lease.release() + + +def test_selected_external_camera_order_is_independent_of_sensor_preparation(ovxr_session_renderer): + renderer = ovxr_session_renderer + lease = renderer.acquire_ov_libraries_render_lease() + specs = [] + for name, hz in zip(("Head", "Left", "Right"), (20, 10, 10), strict=True): + cfg = types.SimpleNamespace(prim_path=f"/World/{name}", width=640, height=480, data_types=("rgb",)) + lease.configure_auxiliary_camera(cfg, max_fps=hz) + specs.append(types.SimpleNamespace(cfg=cfg, num_instances=1, view_count=1, camera_prim_paths=(cfg.prim_path,))) + for spec in reversed(specs): + assert renderer._accept_auxiliary_camera_spec(spec) + assert [camera.spec for camera in lease._auxiliary_cameras] == specs + assert [camera.max_fps for camera in lease._auxiliary_cameras] == [20, 10, 10] + lease.release() + + +def test_external_camera_schedules_reach_native_stage(monkeypatch): + """The renderer's real population path retains only selected camera scheduling properties.""" + import ovrtx + import ovstage + + ovrtx.register_schema_paths() + camera_paths = [f"/World/envs/env_0/{name}" for name in ("Head", "Wrist", "Unselected")] + source = """#usda 1.0 +def Xform "World" { + def Xform "envs" { + def Xform "env_0" { + def Camera "Head" { + double2 omni:sensor:frameRate = (20, 1) + bool omni:sensor:Core:partialOutputs = false + } + def Camera "Wrist" { + double2 omni:sensor:frameRate = (10, 1) + bool omni:sensor:Core:partialOutputs = false + } + def Camera "Unselected" { + double2 omni:sensor:frameRate = (999, 1) + } + } + } +} +""" + cfg = _make_camera_cfg(["rgb"]) + cfg.prim_path = camera_paths[0] + spec = types.SimpleNamespace(cfg=cfg, num_instances=1, camera_prim_paths=[camera_paths[0]], render_settings={}) + data = OVRTXCameraRenderData(spec, "RenderCamera_0") + from isaaclab_ov.stage import OvstageBackend, OvstageBackendCfg + + renderer = _make_ovrtx_renderer_without_backend() + renderer._use_ovstage = True + renderer._auxiliary_camera_stage_deferred = False + renderer._active_ov_libraries_render_lease = None + renderer.backend.renderer = MagicMock() + with contextlib.ExitStack() as resources: + renderer.scene = OvstageBackend(OvstageBackendCfg()) + resources.callback(renderer.scene.close) + stage, paths = renderer.scene.stage, renderer.scene.paths + population = ovstage.population + description = population.Desc( + domains=ovstage.PopulationDomain.RENDERING, + selectors=[ + population.Selector( + population.PrimPredicate.has_path(*camera_paths[:2]), + population.PropertyPredicate.has_name("omni:sensor:frameRate", "omni:sensor:Core:partialOutputs"), + ) + ], + ) + renderer.scene.populate(source, population_desc=description) + try: + renderer._register_camera(spec, data) + renderer.scene.commit() + resolved = {} + plist = paths.create_path_list_from_strings(camera_paths) + try: + with stage.query_from_path_list(plist) as query: + query.wait() + names = ("omni:sensor:frameRate", "omni:sensor:Core:partialOutputs") + with stage.read_attributes( + query, + [paths.intern_token(name) for name in names], + ovstage.OrdinalRange.latest(renderer.scene.ordinal - 1), + ) as read: + read.wait() + for group in read.groups(): + try: + values = np.asarray(group.array(0).copy()).reshape(group.data_count, -1) + group_paths = paths.get_path_strings(group.prim_list) + attribute = paths.token_to_string(group.attribute) + for index in range(min(group.prim_count, group.data_count)): + path = group_paths[group.prim_index(index)] + resolved[path, attribute] = values[group.data_row_index(index)].tolist() + finally: + stage.release_group(group) + finally: + paths.destroy_path_list(plist) + assert resolved.get((camera_paths[0], names[0])) == [20.0, 1.0] + assert resolved.get((camera_paths[1], names[0])) == [10.0, 1.0] + assert resolved.get((camera_paths[0], names[1])) == [False] + assert resolved.get((camera_paths[1], names[1])) == [False] + assert (camera_paths[2], names[0]) not in resolved + finally: + data.resources.close() diff --git a/source/isaaclab_ov/test/test_ovrtx_usd.py b/source/isaaclab_ov/test/test_ovrtx_usd.py index 698d13612af1..856b2dd8805d 100644 --- a/source/isaaclab_ov/test/test_ovrtx_usd.py +++ b/source/isaaclab_ov/test/test_ovrtx_usd.py @@ -435,11 +435,11 @@ def test_export_stage_homogeneous_keeps_only_env0_prototype(): def test_export_stage_without_keep_env_roots_trims_non_source_env_roots(): - """The ovstage clone path also trims the non-source env roots themselves. + """Whole-environment cloning trims the non-source environment roots themselves. ``ovstage.Stage.clone`` requires every target path to not already exist, so the exported stage - must not retain env roots that the clone will recreate. This is the only difference from the - legacy ``renderer.clone_usd`` path, which keeps the roots as placeholders. + must not retain environment roots that the clone will recreate. Asset-level clone plans + retain typed parent roots on both renderer paths. """ num_envs = 4 stage = _make_multi_env_stage(num_envs) @@ -480,14 +480,20 @@ def test_export_stage_heterogeneous_keeps_multiple_sources(): assert 'def Camera "Camera"' not in exported -@pytest.mark.parametrize("settings_path", [None, "/Render/AuthoredSettings"]) -def test_export_stage_preserves_authored_scene(settings_path): +@pytest.mark.parametrize( + "settings_path, authored_show_all", + [(None, None), ("/Render/AuthoredSettings", False), ("/Render/AuthoredSettings", True)], +) +def test_export_stage_preserves_authored_scene(settings_path, authored_show_all): """Renderer settings and clone trimming are confined to the exported scene.""" num_envs = 4 stage = _make_multi_env_stage(num_envs) if settings_path is not None: settings = UsdRender.Settings.Define(stage, settings_path) settings.CreateResolutionAttr(Gf.Vec2i(1280, 720)) + settings.GetPrim().CreateAttribute( + "omni:rtx:scenePartitioning:showAllPartitionsByDefault", Sdf.ValueTypeNames.Bool + ).Set(authored_show_all) stage.SetMetadata("renderSettingsPrimPath", settings_path) before = stage.ExportToString() layer = Sdf.Layer.CreateAnonymous() @@ -496,7 +502,7 @@ def test_export_stage_preserves_authored_scene(settings_path): assert exported.HasAuthoredMetadata("renderSettingsPrimPath") settings = exported.GetPrimAtPath(exported.GetMetadata("renderSettingsPrimPath")) show_all = settings.GetAttribute("omni:rtx:scenePartitioning:showAllPartitionsByDefault") - assert show_all and show_all.Get() is True + assert show_all and show_all.Get() is (True if authored_show_all is None else authored_show_all) if settings_path is not None: assert settings.GetPath() == settings_path assert settings.GetAttribute("resolution").Get() == (1280, 720) diff --git a/source/isaaclab_tasks/changelog.d/humanoid-teleop-physics-presets.fixed.rst b/source/isaaclab_tasks/changelog.d/humanoid-teleop-physics-presets.fixed.rst new file mode 100644 index 000000000000..417f143d489b --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/humanoid-teleop-physics-presets.fixed.rst @@ -0,0 +1 @@ +* Fixed public physics preset selection for GR1T2 and G1 teleoperation while retaining the Kit default and task-owned backend preparation. diff --git a/source/isaaclab_tasks/changelog.d/kitless-robot-tasks.added.rst b/source/isaaclab_tasks/changelog.d/kitless-robot-tasks.added.rst new file mode 100644 index 000000000000..edc464914920 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/kitless-robot-tasks.added.rst @@ -0,0 +1 @@ +* Added G1 and GR1T2 kitless teleoperation setup, including portable GR1T2 kinematics, backend-specific actuators, collision preparation, and bounded action targets. diff --git a/source/isaaclab_tasks/changelog.d/kitless-robot-tasks.minor b/source/isaaclab_tasks/changelog.d/kitless-robot-tasks.minor new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_tasks/changelog.d/kitless-table-collisions.fixed.rst b/source/isaaclab_tasks/changelog.d/kitless-table-collisions.fixed.rst new file mode 100644 index 000000000000..c609b3ebe305 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/kitless-table-collisions.fixed.rst @@ -0,0 +1 @@ +* Prepared packing-table mesh collisions for kitless Newton and OVPhysX tasks. diff --git a/source/isaaclab_tasks/changelog.d/ovxr-camera-feedback.added.rst b/source/isaaclab_tasks/changelog.d/ovxr-camera-feedback.added.rst new file mode 100644 index 000000000000..d47e8aebeb72 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/ovxr-camera-feedback.added.rst @@ -0,0 +1 @@ +* Added G1 feedback cameras and isolated robot head visuals for OVXR headset and camera views. diff --git a/source/isaaclab_tasks/changelog.d/selector-test-ownership.skip b/source/isaaclab_tasks/changelog.d/selector-test-ownership.skip new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_tasks/changelog.d/so101-capture-clutch-api.fixed.rst b/source/isaaclab_tasks/changelog.d/so101-capture-clutch-api.fixed.rst new file mode 100644 index 000000000000..90cdff6697b2 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/so101-capture-clutch-api.fixed.rst @@ -0,0 +1,3 @@ +* Fixed SO101 XR pipeline construction with the supported Capture clutch API. Seeded its + engage-relative position and orientation from the complete nominal gripper reset pose, + preserved the supported isaacteleop 1.4 absolute-grip calibration, and kept IK task resets consistent with that pose without joint randomization. diff --git a/source/isaaclab_tasks/changelog.d/so101-physics-presets.fixed.rst b/source/isaaclab_tasks/changelog.d/so101-physics-presets.fixed.rst new file mode 100644 index 000000000000..5c667d62f215 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/so101-physics-presets.fixed.rst @@ -0,0 +1 @@ +* Fixed standalone OVPhysX preset selection for SO101 IK teleoperation while retaining the Kit default, Newton solver settings, and matching USD Physics variants. diff --git a/source/isaaclab_tasks/docs/README.md b/source/isaaclab_tasks/docs/README.md index 2ccb195fb5d7..d2a3be409f81 100644 --- a/source/isaaclab_tasks/docs/README.md +++ b/source/isaaclab_tasks/docs/README.md @@ -58,3 +58,31 @@ gym.register( > **Note:** As a practice, we specify all the environments in a single file to avoid name conflicts between different > tasks or environments. However, this practice is debatable and we are open to suggestions to deal with a large > scaling in the number of tasks or environments. + +## Fixed-base G1 manipulation cameras + +For standalone OVPhysX, the task's backend preparation hook installs explicit +tabletop and steering-wheel collision shapes, shared with GR1T2. The bin's +existing primitive colliders and the object's drop termination remain enabled. +The teleoperation launcher invokes this hook when selecting `--physics ovphysx`; +custom launchers must apply `stabilize_for_kitless_ovphysx()` before creating the +environment. Newton uses its separate backend preparation. + +`IsaacContrib-PickPlace-FixedBaseUpperBodyIK-G1-Abs` provides three 640×480 RGB +cameras: `robot_pov_cam` at 20 Hz, and `left_wrist_cam` and `right_wrist_cam` at +10 Hz each. The head uses the calibrated G1 head-camera pose; wrist cameras +are mounted on their respective wrist-yaw links and look along wrist-local +X +with +Z up. Their offsets are configurable in the task's scene configuration. + +The same names identify policy image observations and the task's XR camera +feeds, with the head panel centered and wrist panels to its left and right. +Sensor `update_period` controls image sampling in simulation time; feed +`max_update_hz` caps uploads. Actual delivery also depends on simulation and +renderer throughput. Camera sampling rates are configuration targets, not +measured headset frame rates. + +The teleoperation launcher's `--disable_external_cameras` option removes these +cameras and their image observations. The kitless XR launcher accepts up to +three camera names with `--ovxr_pip_camera`. Camera feeds are opt-in: without +this option, live OVXR and MCAP replay omit task cameras and PiP layers. +Selected feeds keep their own sampling cadence. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/locomanip_pick_place/fixed_base_upper_body_ik_g1_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/locomanip_pick_place/fixed_base_upper_body_ik_g1_env_cfg.py index e4e9a1a1e59d..dabec38c2b3a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/locomanip_pick_place/fixed_base_upper_body_ik_g1_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/locomanip_pick_place/fixed_base_upper_body_ik_g1_env_cfg.py @@ -13,6 +13,7 @@ from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.scene import InteractiveSceneCfg from isaaclab.sensors import ContactSensorCfg +from isaaclab.sim import SimulationCfg from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg from isaaclab.utils import configclass, replace from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR @@ -21,12 +22,14 @@ from isaaclab_tasks.contrib.locomanip_pick_place import mdp as locomanip_mdp from isaaclab_tasks.contrib.pick_place import mdp as manip_mdp from isaaclab_tasks.contrib.robot_pov_camera_cfg import g1_robot_pov_camera_cfg +from isaaclab_tasks.contrib.teleop_physics_cfg import TeleopPhysicsCfg from isaaclab_assets.robots.unitree import G1_29DOF_CFG from isaaclab_tasks.contrib.locomanip_pick_place.configs.pink_controller_cfg import ( # isort: skip G1_UPPER_BODY_IK_ACTION_CFG, ) +from isaaclab_tasks.contrib.robot_pov_camera_cfg import robot_pov_camera_cfg # isort: skip def _build_g1_upper_body_pipeline(): @@ -261,18 +264,35 @@ class FixedBaseUpperBodyIKG1SceneCfg(InteractiveSceneCfg): # Unitree G1 Humanoid robot - fixed base configuration robot: ArticulationCfg = replace(G1_29DOF_CFG, prim_path="{ENV_REGEX_NS}/Robot") - robot_pov_cam = g1_robot_pov_camera_cfg() + # Share the calibrated head view with the locomomanipulation G1 task. + robot_pov_cam = g1_robot_pov_camera_cfg().replace(update_period=1.0 / 20.0) + + # Mount above each hand, looking along wrist-local +X with +Z up. + left_wrist_cam = robot_pov_camera_cfg( + parent_prim_path="{ENV_REGEX_NS}/Robot/left_wrist_yaw_link", + offset_pos=(0.06, 0.0, 0.06), + offset_rot=(-0.5, 0.5, -0.5, 0.5), + ).replace( + prim_path="{ENV_REGEX_NS}/Robot/left_wrist_yaw_link/LeftWristCam", + update_period=1.0 / 10.0, + height=480, + width=640, + spawn=sim_utils.PinholeCameraCfg(focal_length=15.0, horizontal_aperture=20.955, clipping_range=(0.01, 5.0)), + ) + right_wrist_cam = left_wrist_cam.replace( + prim_path="{ENV_REGEX_NS}/Robot/right_wrist_yaw_link/RightWristCam", + ) # Per-hand contact sensors over all finger links, used to drive controller # haptics (see HapticFeedbackCfg below). Requires activate_contact_sensors # on the robot spawn, enabled in the env __post_init__. left_hand_contact = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Robot/left_hand_[^/]*_link", + prim_path="{ENV_REGEX_NS}/Robot/left_hand/left_hand_[^/]*_link", update_period=0.0, history_length=3, ) right_hand_contact = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Robot/right_hand_[^/]*_link", + prim_path="{ENV_REGEX_NS}/Robot/right_hand/right_hand_[^/]*_link", update_period=0.0, history_length=3, ) @@ -304,9 +324,7 @@ class ActionsCfg: @configclass class ObservationsCfg: - """Observation specifications for the MDP. - This class is required by the environment configuration but not used in this implementation - """ + """State and camera observations for fixed-base manipulation.""" @configclass class PolicyCfg(ObsGroup): @@ -336,6 +354,28 @@ class PolicyCfg(ObsGroup): params={"left_eef_link_name": "left_wrist_yaw_link", "right_eef_link_name": "right_wrist_yaw_link"}, ) + robot_pov_cam = ObsTerm( + func=base_mdp.image_rgb, + params={ + "sensor_cfg": SceneEntityCfg("robot_pov_cam"), + "normalize": False, + }, + ) + left_wrist_cam = ObsTerm( + func=base_mdp.image_rgb, + params={ + "sensor_cfg": SceneEntityCfg("left_wrist_cam"), + "normalize": False, + }, + ) + right_wrist_cam = ObsTerm( + func=base_mdp.image_rgb, + params={ + "sensor_cfg": SceneEntityCfg("right_wrist_cam"), + "normalize": False, + }, + ) + def __post_init__(self): self.enable_corruption = False self.concatenate_terms = False @@ -374,6 +414,8 @@ class FixedBaseUpperBodyIKG1EnvCfg(ManagerBasedRLEnvCfg): controlled using upper body IK. """ + sim: SimulationCfg = SimulationCfg(physics=TeleopPhysicsCfg()) + # Scene settings scene: FixedBaseUpperBodyIKG1SceneCfg = FixedBaseUpperBodyIKG1SceneCfg(num_envs=1, env_spacing=2.5) # MDP settings @@ -394,6 +436,7 @@ def __post_init__(self): # simulation settings self.sim.dt = 1 / 200 # 200Hz self.sim.render_interval = 2 + self.num_rerenders_on_reset = 3 # Set the URDF and mesh paths for the IK controller urdf_omniverse_path = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/LocomanipulationAssets/unitree_g1_kinematics_asset/g1_29dof_with_hand_only_kinematics.urdf" # noqa: E501 @@ -418,6 +461,7 @@ def __post_init__(self): self.xr = XrCfg( anchor_pos=(0.0, 0.0, -0.30), anchor_rot=(0.0, 0.0, 0.0, 1.0), + excluded_visual_meshes=["/World/envs/env_0/Robot/torso_link/visuals/head_link/mesh"], ) self.isaac_teleop = IsaacTeleopCfg( @@ -429,11 +473,26 @@ def __post_init__(self): xr_camera_feeds=[ XrCameraFeedCfg( camera_name="robot_pov_cam", + label="Head", enable_dlss_ray_reconstruction=True, dlss_exec_mode="quality", offset_m=(0.0, -0.15), - max_update_hz=0.0, - ) + max_update_hz=20.0, + ), + XrCameraFeedCfg( + camera_name="left_wrist_cam", + label="Left wrist", + panel_width_m=0.30, + offset_m=(-0.43, -0.15), + max_update_hz=10.0, + ), + XrCameraFeedCfg( + camera_name="right_wrist_cam", + label="Right wrist", + panel_width_m=0.30, + offset_m=(0.43, -0.15), + max_update_hz=10.0, + ), ], ) @@ -442,3 +501,15 @@ def __post_init__(self): left_sensor_name="left_hand_contact", right_sensor_name="right_hand_contact", ) + + def stabilize_for_kitless_ovphysx(self) -> None: + """Prepare the table collision authoring; let OVPhysX cook the wheel meshes.""" + from isaaclab_tasks.contrib.packing_table import condition_packing_table_for_kitless_ovphysx + + condition_packing_table_for_kitless_ovphysx(self.scene.packing_table) + + def stabilize_for_kitless_newton_mjwarp(self) -> None: + """Configure G1 actions, assets and contacts for kitless Newton.""" + from .g1_kitless_newton import configure_g1_for_kitless_newton + + configure_g1_for_kitless_newton(self) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/locomanip_pick_place/g1_kitless_newton.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/locomanip_pick_place/g1_kitless_newton.py new file mode 100644 index 000000000000..25d704d21df3 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/locomanip_pick_place/g1_kitless_newton.py @@ -0,0 +1,230 @@ +# Copyright (c) 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 + +"""G1 action and actuator boundaries for kitless Newton teleoperation.""" + +from __future__ import annotations + +from typing import Any + +import numpy as np +import torch + +from isaaclab.envs.mdp.actions.pink_task_space_actions import PinkInverseKinematicsAction + +POSITION_MAX_STEP_M = 0.10 +"""Maximum Cartesian wrist-target change accepted in one active frame.""" + +ROTATION_MAX_STEP_RAD = 0.60 +"""Maximum wrist-target rotation accepted in one active frame.""" + +JOINT_TARGET_MAX_STEP_RAD = 0.10 +"""Maximum Pink joint-target change accepted in one active frame.""" + +JOINT_LIMIT_MARGIN_RAD = 1.0e-3 +"""Margin retained inside the common Pink/articulation joint limits.""" + +EXPECTED_ACTION_DIM = 28 +"""Two 7-D wrist poses followed by fourteen G1 hand targets.""" + +ACTUATOR_PROFILE = { + "waist": {"stiffness": 500.0, "damping": 50.0, "armature": 0.10, "joint_effort_limit": 250.0}, + "arms": {"stiffness": 1200.0, "damping": 60.0, "armature": 0.30, "joint_effort_limit": 800.0}, + "hands": {"stiffness": 5.0, "damping": 0.5, "armature": 0.02, "joint_effort_limit": 20.0}, +} +"""Measured current-stack Newton implicit-drive contract for G1-owned groups.""" + + +def teleop_action_target_issue(action: torch.Tensor) -> str | None: + """Return why an incoming G1 controller action must not reach Pink/Newton.""" + flat = action.detach().flatten() + if flat.numel() != EXPECTED_ACTION_DIM: + return f"unexpected G1 controller action size {flat.numel()} (expected {EXPECTED_ACTION_DIM})" + if not bool(torch.isfinite(flat).all()): + return "non-finite action target" + + wrists = flat[:14].reshape(2, 7) + quaternion_norms = torch.linalg.norm(wrists[:, 3:7], dim=1) + if bool(torch.any(quaternion_norms < 0.25)) or bool(torch.any(quaternion_norms > 2.0)): + return f"invalid wrist quaternion norms {quaternion_norms.detach().cpu().tolist()}" + if float(torch.max(torch.abs(flat[14:])).detach().cpu()) > 6.5: + return "unreasonable hand joint target magnitude" + return None + + +def bound_pink_joint_targets( + current: torch.Tensor, + proposed: torch.Tensor, + lower: torch.Tensor, + upper: torch.Tensor, +) -> torch.Tensor: + """Keep one Pink update finite, inside limits, and close to current state.""" + if not bool( + torch.isfinite(current).all() + and torch.isfinite(proposed).all() + and torch.isfinite(lower).all() + and torch.isfinite(upper).all() + ): + raise RuntimeError("non-finite Pink joint target boundary") + + safe_lower = lower + JOINT_LIMIT_MARGIN_RAD + safe_upper = upper - JOINT_LIMIT_MARGIN_RAD + if bool(torch.any(safe_lower > safe_upper)): + raise RuntimeError("Pink/articulation joint-limit intersection is empty") + + stepped = torch.clamp(proposed, min=current - JOINT_TARGET_MAX_STEP_RAD, max=current + JOINT_TARGET_MAX_STEP_RAD) + return torch.maximum(torch.minimum(stepped, safe_upper), safe_lower) + + +class G1NewtonBoundedPinkInverseKinematicsAction(PinkInverseKinematicsAction): + """Bound Pink outputs without changing G1's public 28-D action layout.""" + + def __init__(self, cfg, env): + super().__init__(cfg, env) + self._pink_joint_limits = None + + def _compute_ik_solutions(self) -> torch.Tensor: + proposed = super()._compute_ik_solutions() + current = self._asset.data.joint_pos.torch[:, self._isaaclab_controlled_joint_ids] + articulation_limits = self._asset.data.joint_pos_limits.torch[:, self._isaaclab_controlled_joint_ids, :] + + # Pink's controlled model/order are immutable for this action's lifetime. + # Keep articulation limits live: users may update them during a run. + if self._pink_joint_limits is None: + self._pink_joint_limits = tuple( + ( + torch.as_tensor( + np.asarray(controller.pink_configuration.controlled_model.lowerPositionLimit)[ + controller.pink_to_isaac_lab_controlled_ordering + ], + device=self.device, + dtype=proposed.dtype, + ), + torch.as_tensor( + np.asarray(controller.pink_configuration.controlled_model.upperPositionLimit)[ + controller.pink_to_isaac_lab_controlled_ordering + ], + device=self.device, + dtype=proposed.dtype, + ), + ) + for controller in self._ik_controllers + ) + bounded = [] + for env_index, (pink_lower, pink_upper) in enumerate(self._pink_joint_limits): + lower = torch.maximum(pink_lower, articulation_limits[env_index, :, 0]) + upper = torch.minimum(pink_upper, articulation_limits[env_index, :, 1]) + bounded.append(bound_pink_joint_targets(current[env_index], proposed[env_index], lower, upper)) + return torch.stack(bounded) + + +def _wrist_poses(env, state: dict[str, Any]): + """Return current task-selected wrist poses with body indices cached in state.""" + try: + robot = env.scene["robot"] + if "_wrist_indices" in state: + left_index, right_index = state["_wrist_indices"] + else: + body_names = list(getattr(robot, "body_names", None) or getattr(robot.data, "body_names", [])) + target_links = env.cfg.actions.upper_body_ik.target_eef_link_names + left_index = body_names.index(target_links["left_wrist"]) + right_index = body_names.index(target_links["right_wrist"]) + state["_wrist_indices"] = (left_index, right_index) + pose_w = robot.data.body_link_pose_w + pose_w = (pose_w.torch if hasattr(pose_w, "torch") else pose_w)[0] + except (AttributeError, KeyError, ValueError): + return None + return ( + pose_w[left_index, :3].clone(), + pose_w[left_index, 3:7].clone(), + pose_w[right_index, :3].clone(), + pose_w[right_index, 3:7].clone(), + ) + + +def _step_vector_towards(current: torch.Tensor, target: torch.Tensor, max_step: float) -> torch.Tensor: + delta = target - current + distance = torch.linalg.norm(delta) + alpha = torch.clamp(max_step / torch.clamp(distance, min=1.0e-6), max=1.0) + return current + delta * alpha + + +def _step_quaternion_towards(current: torch.Tensor, target: torch.Tensor, max_step: float) -> torch.Tensor: + current = torch.nn.functional.normalize(current, dim=0) + target = torch.nn.functional.normalize(target, dim=0) + target = torch.where(torch.sum(current * target) < 0.0, -target, target) + dot = torch.clamp(torch.abs(torch.sum(current * target)), 0.0, 1.0) + angle = 2.0 * torch.acos(dot) + alpha = torch.clamp(max_step / torch.clamp(angle, min=1.0e-6), max=1.0) + half_angle = angle * 0.5 + denominator = torch.clamp(torch.sin(half_angle), min=1.0e-6) + interpolated = ( + torch.sin((1.0 - alpha) * half_angle) * current + torch.sin(alpha * half_angle) * target + ) / denominator + # Coincident quaternions need no interpolation and avoid a zero denominator. + return torch.nn.functional.normalize(torch.where(angle < 1.0e-6, target, interpolated), dim=0) + + +def condition_kitless_newton_g1_action(env, action: torch.Tensor, state: dict[str, Any]) -> torch.Tensor: + """Ramp both wrist targets from the current G1 poses before Pink solves IK.""" + flat = action.detach().flatten() + if flat.numel() != EXPECTED_ACTION_DIM: + state.clear() + return action + + conditioned = action.clone() + conditioned_flat = conditioned.flatten() + if "left_pos" not in state: + current_wrist_poses = _wrist_poses(env, state) + if current_wrist_poses is None: + state.clear() + raise RuntimeError("could not resolve the task-selected G1 wrist poses") + left_pos, left_quat, right_pos, right_quat = current_wrist_poses + state["left_pos"] = left_pos.to(device=conditioned_flat.device, dtype=conditioned_flat.dtype) + state["left_quat"] = left_quat.to(device=conditioned_flat.device, dtype=conditioned_flat.dtype) + state["right_pos"] = right_pos.to(device=conditioned_flat.device, dtype=conditioned_flat.dtype) + state["right_quat"] = right_quat.to(device=conditioned_flat.device, dtype=conditioned_flat.dtype) + + state["left_pos"] = _step_vector_towards(state["left_pos"], flat[0:3], POSITION_MAX_STEP_M) + state["left_quat"] = _step_quaternion_towards(state["left_quat"], flat[3:7], ROTATION_MAX_STEP_RAD) + state["right_pos"] = _step_vector_towards(state["right_pos"], flat[7:10], POSITION_MAX_STEP_M) + state["right_quat"] = _step_quaternion_towards(state["right_quat"], flat[10:14], ROTATION_MAX_STEP_RAD) + conditioned_flat[0:3] = state["left_pos"] + conditioned_flat[3:7] = state["left_quat"] + conditioned_flat[7:10] = state["right_pos"] + conditioned_flat[10:14] = state["right_quat"] + return conditioned + + +def apply_g1_newton_actuator_profile(robot_cfg) -> None: + """Configure the G1 actuator groups used by teleoperation.""" + groups = robot_cfg.actuators + if not set(ACTUATOR_PROFILE).issubset(groups): + raise RuntimeError(f"Expected G1 waist/arms/hands actuator groups; found {sorted(groups)}") + for name, selected in ACTUATOR_PROFILE.items(): + actuator = groups[name] + for key, value in selected.items(): + setattr(actuator, key, value) + + +def configure_g1_for_kitless_newton(env_cfg) -> None: + """Configure the controller, asset and solver for kitless Newton.""" + from isaaclab_tasks.contrib.packing_table import configure_packing_table_collisions + from isaaclab_tasks.contrib.steering_wheel import condition_newton_steering_wheel_detail_collisions + + solver_cfg = getattr(getattr(env_cfg.sim, "physics", None), "solver_cfg", None) + if solver_cfg is None: + raise RuntimeError("G1 kitless Newton stabilization requires an MJWarp solver config") + solver_cfg.nconmax = 4096 + solver_cfg.njmax = 1536 + + apply_g1_newton_actuator_profile(env_cfg.scene.robot) + env_cfg.actions.upper_body_ik.controller.fail_on_joint_limit_violation = False + env_cfg.actions.upper_body_ik.class_type = G1NewtonBoundedPinkInverseKinematicsAction + env_cfg.isaac_teleop.action_conditioner = condition_kitless_newton_g1_action + env_cfg.isaac_teleop.action_validator = teleop_action_target_issue + + configure_packing_table_collisions(env_cfg.scene.packing_table) + condition_newton_steering_wheel_detail_collisions(env_cfg.scene.object) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/locomanip_pick_place/locomanipulation_g1_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/locomanip_pick_place/locomanipulation_g1_env_cfg.py index 23404da5ff32..16ce649d3f51 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/locomanip_pick_place/locomanipulation_g1_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/locomanip_pick_place/locomanipulation_g1_env_cfg.py @@ -13,6 +13,7 @@ from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.scene import InteractiveSceneCfg from isaaclab.sensors import ContactSensorCfg +from isaaclab.sim import SimulationCfg from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg from isaaclab.utils import configclass, replace from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR @@ -23,7 +24,9 @@ from isaaclab_tasks.contrib.locomanip_pick_place.configs.agile_locomotion_observation_cfg import ( AgileTeacherPolicyObservationsCfg, ) +from isaaclab_tasks.contrib.packing_table import condition_packing_table_for_kitless_ovphysx from isaaclab_tasks.contrib.pick_place import mdp as manip_mdp +from isaaclab_tasks.contrib.teleop_physics_cfg import TeleopPhysicsCfg from isaaclab_assets.robots.unitree import G1_29DOF_CFG @@ -296,12 +299,12 @@ class LocomanipulationG1SceneCfg(InteractiveSceneCfg): # haptics (see HapticFeedbackCfg below). Requires activate_contact_sensors # on the robot spawn, enabled in the env __post_init__. left_hand_contact = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Robot/left_hand_[^/]*_link", + prim_path="{ENV_REGEX_NS}/Robot/left_hand/left_hand_[^/]*_link", update_period=0.0, history_length=3, ) right_hand_contact = ContactSensorCfg( - prim_path="{ENV_REGEX_NS}/Robot/right_hand_[^/]*_link", + prim_path="{ENV_REGEX_NS}/Robot/right_hand/right_hand_[^/]*_link", update_period=0.0, history_length=3, ) @@ -430,6 +433,8 @@ class LocomanipulationG1EnvCfg(ManagerBasedRLEnvCfg): behaviors. """ + sim: SimulationCfg = SimulationCfg(physics=TeleopPhysicsCfg()) + # Scene settings scene: LocomanipulationG1SceneCfg = LocomanipulationG1SceneCfg(num_envs=1, env_spacing=2.5) # MDP settings @@ -444,6 +449,8 @@ class LocomanipulationG1EnvCfg(ManagerBasedRLEnvCfg): def __post_init__(self): """Post initialization.""" + # The pretrained Agile policy consumes and produces PhysX-ordered joints. + self.scene.robot.joint_ordering = "physx" # general settings self.decimation = 4 self.episode_length_s = 20.0 @@ -483,7 +490,6 @@ def __post_init__(self): pipeline_builder=_build_g1_locomanipulation_pipeline, sim_device=self.sim.device, xr_cfg=self.xr, - xr_camera_feed_layout=XrCameraFeedLayoutCfg(placement="head_locked", use_scene_partition=True), xr_camera_feeds=[ XrCameraFeedCfg( camera_name="robot_pov_cam", @@ -493,6 +499,7 @@ def __post_init__(self): max_update_hz=0.0, ) ], + xr_camera_feed_layout=XrCameraFeedLayoutCfg(placement="head_locked", use_scene_partition=True), ) # Drive controller haptics from the per-hand contact sensors. @@ -500,3 +507,18 @@ def __post_init__(self): left_sensor_name="left_hand_contact", right_sensor_name="right_hand_contact", ) + + def stabilize_for_kitless_ovphysx(self) -> None: + """Provide the packing-table support shape required by standalone OVPhysX.""" + condition_packing_table_for_kitless_ovphysx(self.scene.packing_table) + + def stabilize_for_kitless_newton_mjwarp(self) -> None: + """Prepare shared task collision geometry without replacing the locomotion policy.""" + from isaaclab_tasks.contrib.packing_table import configure_packing_table_collisions + from isaaclab_tasks.contrib.steering_wheel import condition_newton_steering_wheel_detail_collisions + + # Match the common locomotion integrator for the floating-base robot. + self.sim.physics.solver_cfg.integrator = "implicitfast" + self.sim.physics.num_substeps = 2 + configure_packing_table_collisions(self.scene.packing_table) + condition_newton_steering_wheel_detail_collisions(self.scene.object) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/packing_table.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/packing_table.py new file mode 100644 index 000000000000..bfc8999d9350 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/packing_table.py @@ -0,0 +1,71 @@ +# Copyright (c) 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 + +"""Shared collision preparation for the packing-table USD asset.""" + +from isaaclab.assets import AssetBaseCfg + + +def configure_packing_table_collisions(table_cfg: AssetBaseCfg, *, physx_offsets: bool = False) -> None: + """Move the asset's non-geometric collision anchor onto its existing meshes. + + The source USD attaches a bounding-cube collision approximation to an Xform. + Both standalone backends need concrete geometry. Preserve the authored mesh + transforms and approximation, then use the common schema writers for each + mesh. The five bin colliders are left intact; no replacement geometry or + backend-specific cooking implementation is needed. + """ + if table_cfg.spawn is None: + return + table_cfg.spawn._packing_table_physx_offsets = physx_offsets + if getattr(table_cfg.spawn, "_packing_table_collision_prepared", False): + return + original_spawn = table_cfg.spawn.func + + def _spawn_with_mesh_collisions(prim_path, cfg, *args, **kwargs): + from pxr import Usd, UsdGeom, UsdPhysics + + import isaaclab.sim as sim_utils + + prim = original_spawn(prim_path, cfg, *args, **kwargs) + stage = prim.GetStage() + sim_utils.make_uninstanceable(str(prim.GetPath()), stage=stage) + for child in list(Usd.PrimRange(prim)): + if not child.HasAPI(UsdPhysics.MeshCollisionAPI) or child.IsA(UsdGeom.Gprim): + continue + approximation = UsdPhysics.MeshCollisionAPI(child).GetApproximationAttr().Get() + child.RemoveAPI(UsdPhysics.MeshCollisionAPI) + child.RemoveAPI(UsdPhysics.CollisionAPI) + for mesh in Usd.PrimRange(child): + if mesh.IsA(UsdGeom.Mesh): + path = str(mesh.GetPath()) + sim_utils.apply_collision_properties( + path, + [sim_utils.UsdPhysicsCollisionCfg(collision_enabled=True)], + stage=stage, + create_if_missing=True, + ) + sim_utils.apply_mesh_collision_properties( + path, + [sim_utils.UsdPhysicsMeshCollisionCfg(mesh_approximation_name=approximation or "boundingCube")], + stage=stage, + ) + if cfg._packing_table_physx_offsets: + from isaaclab_physx.sim.schemas import PhysxCollisionCfg + + sim_utils.apply_collision_properties( + str(prim.GetPath()) + "/.*", + [PhysxCollisionCfg(contact_offset=0.001, rest_offset=0.0)], + stage=stage, + ) + return prim + + table_cfg.spawn.func = _spawn_with_mesh_collisions + table_cfg.spawn._packing_table_collision_prepared = True + + +def condition_packing_table_for_kitless_ovphysx(table_cfg: AssetBaseCfg) -> None: + """Prepare shared mesh collisions with the accepted PhysX contact offsets.""" + configure_packing_table_collisions(table_cfg, physx_offsets=True) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/assets/GR1T2_fourier_hand_6dof_kinematics.urdf b/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/assets/GR1T2_fourier_hand_6dof_kinematics.urdf new file mode 100644 index 000000000000..f8f8af75677d --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/assets/GR1T2_fourier_hand_6dof_kinematics.urdf @@ -0,0 +1,778 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/gr1t2_kitless_action.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/gr1t2_kitless_action.py new file mode 100644 index 000000000000..ec8137c93e7c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/gr1t2_kitless_action.py @@ -0,0 +1,172 @@ +# 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 + +"""GR1T2 Pink action conditioning for the kitless OVPhysX backend.""" + +from __future__ import annotations + +import math +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +from isaaclab.envs.mdp.actions.pink_task_space_actions import PinkInverseKinematicsAction + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from .gr1t2_kitless_action_cfg import GR1T2KitlessPinkInverseKinematicsActionCfg + + +def bound_ik_joint_targets( + targets: torch.Tensor, + current: torch.Tensor, + max_delta_rad: float, + max_magnitude_rad: float, +) -> torch.Tensor: + """Hold invalid rows and bound finite IK targets relative to measured state.""" + finite_rows = torch.isfinite(targets).all(dim=1, keepdim=True) + safe_targets = torch.where(finite_rows, targets, current) + delta = torch.clamp(safe_targets - current, -max_delta_rad, max_delta_rad) + bounded = torch.clamp(current + delta, -max_magnitude_rad, max_magnitude_rad) + return torch.where(finite_rows, bounded, current) + + +def constrain_hand_joint_targets( + targets: torch.Tensor, + mimic_pairs: tuple[tuple[int, int, float, float], ...], + lower: torch.Tensor, + upper: torch.Tensor, +) -> torch.Tensor: + """Apply software mimic relations and anatomical bounds to hand targets.""" + constrained = targets.clone() + for mimic_index, reference_index, factor, bias in mimic_pairs: + constrained[:, mimic_index] = factor * constrained[:, reference_index] + bias + return torch.maximum(torch.minimum(constrained, upper), lower) + + +class _HandTargetSlewState: + """Per-environment hand target history, independent of measured joint state.""" + + def __init__(self, prototype: torch.Tensor, max_step_rad: float) -> None: + self._previous = torch.zeros_like(prototype) + self._initialized = torch.zeros(prototype.shape[0], dtype=torch.bool, device=prototype.device) + self._max_step_rad = max_step_rad + + def condition(self, targets: torch.Tensor) -> torch.Tensor: + slewed = self._previous + torch.clamp(targets - self._previous, -self._max_step_rad, self._max_step_rad) + result = torch.where(self._initialized[:, None], slewed, targets) + self._previous.copy_(result) + self._initialized.fill_(True) + return result + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + if env_ids is None: + env_ids = slice(None) + self._previous[env_ids] = 0.0 + self._initialized[env_ids] = False + + +class GR1T2KitlessPinkInverseKinematicsAction(PinkInverseKinematicsAction): + """Pink action that owns the GR1T2 kitless target-stability boundary. + + Kit realizes the hand's PhysX mimic constraints and sanitizes the raw asset. + The kitless task removes those hard constraints before physics parsing, so + this term reproduces their target relation in software and supplies the + software limits that replace the widened physical hand limits. It also + bounds cold-start IK demands against measured state to avoid a first-step + drive impulse. With the feature disabled, behavior is the base Pink action. + """ + + cfg: GR1T2KitlessPinkInverseKinematicsActionCfg + + def __init__(self, cfg: GR1T2KitlessPinkInverseKinematicsActionCfg, env: ManagerBasedEnv) -> None: + super().__init__(cfg, env) + self._kitless_ovphysx_enabled = cfg.kitless_ovphysx_enabled + self._mimic_pairs: tuple[tuple[int, int, float, float], ...] = () + self._hand_lower: torch.Tensor | None = None + self._hand_upper: torch.Tensor | None = None + self._hand_target_slew: _HandTargetSlewState | None = None + if not self._kitless_ovphysx_enabled: + return + + for name, value in ( + ("max_ik_joint_delta_rad", cfg.max_ik_joint_delta_rad), + ("max_ik_joint_magnitude_rad", cfg.max_ik_joint_magnitude_rad), + ("max_hand_target_step_rad", cfg.max_hand_target_step_rad), + ): + if not math.isfinite(value) or value <= 0.0: + raise ValueError(f"{name} must be finite and positive.") + if not math.isfinite(cfg.hand_target_margin_rad) or cfg.hand_target_margin_rad < 0.0: + raise ValueError("hand_target_margin_rad must be finite and non-negative.") + + hand_indices = {name: index for index, name in enumerate(self._hand_joint_names)} + missing_limits = [name for name in self._hand_joint_names if name not in cfg.hand_joint_limits] + if missing_limits: + raise ValueError(f"Missing pre-conditioned hand limits for: {missing_limits}") + + pairs = [] + for mimic_name, (reference_name, gearing, offset) in cfg.hand_mimic_relations.items(): + if mimic_name not in hand_indices or reference_name not in hand_indices: + raise ValueError( + f"Mimic relation {mimic_name!r} -> {reference_name!r} is outside the Pink hand joint set." + ) + if not all(math.isfinite(value) for value in (gearing, offset)): + raise ValueError(f"Mimic relation for {mimic_name!r} must be finite.") + # PhysX authors q_mimic + gearing * q_reference + offset = 0. + pairs.append((hand_indices[mimic_name], hand_indices[reference_name], -gearing, -offset)) + self._mimic_pairs = tuple(pairs) + + lower = [] + upper = [] + for name in self._hand_joint_names: + joint_lower, joint_upper = cfg.hand_joint_limits[name] + if not all(math.isfinite(value) for value in (joint_lower, joint_upper)) or joint_lower > joint_upper: + raise ValueError(f"Invalid hand limits for {name!r}: {(joint_lower, joint_upper)}") + lower.append(joint_lower - cfg.hand_target_margin_rad) + upper.append(joint_upper + cfg.hand_target_margin_rad) + self._hand_lower = torch.tensor(lower, dtype=self._raw_actions.dtype, device=self.device) + self._hand_upper = torch.tensor(upper, dtype=self._raw_actions.dtype, device=self.device) + self._hand_target_slew = _HandTargetSlewState( + torch.zeros( + (self.num_envs, len(self._hand_joint_names)), + dtype=self._raw_actions.dtype, + device=self.device, + ), + cfg.max_hand_target_step_rad, + ) + + def process_actions(self, actions: torch.Tensor) -> None: + """Process input without mutating the action-manager input during coupling.""" + super().process_actions(actions) + if self._kitless_ovphysx_enabled: + assert self._hand_lower is not None and self._hand_upper is not None + assert self._hand_target_slew is not None + constrained = constrain_hand_joint_targets( + self._target_hand_joint_positions, + self._mimic_pairs, + self._hand_lower, + self._hand_upper, + ) + self._target_hand_joint_positions = self._hand_target_slew.condition(constrained) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Clear commanded-target history when the owning session/environment resets.""" + super().reset(env_ids) + if self._hand_target_slew is not None: + self._hand_target_slew.reset(env_ids) + + def _compute_ik_solutions(self) -> torch.Tensor: + targets = super()._compute_ik_solutions() + if not self._kitless_ovphysx_enabled: + return targets + current = self._asset.data.joint_pos.torch[:, self._isaaclab_controlled_joint_ids] + return bound_ik_joint_targets( + targets, + current, + self.cfg.max_ik_joint_delta_rad, + self.cfg.max_ik_joint_magnitude_rad, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/gr1t2_kitless_action_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/gr1t2_kitless_action_cfg.py new file mode 100644 index 000000000000..a849a6c221bf --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/gr1t2_kitless_action_cfg.py @@ -0,0 +1,52 @@ +# 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 + +"""Pure-data configuration for the GR1T2 kitless OVPhysX action boundary.""" + +from __future__ import annotations + +from dataclasses import field +from typing import TYPE_CHECKING + +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg +from isaaclab.utils.configclass import configclass + +if TYPE_CHECKING: + from .gr1t2_kitless_action import GR1T2KitlessPinkInverseKinematicsAction + + +@configclass +class GR1T2KitlessPinkInverseKinematicsActionCfg(PinkInverseKinematicsActionCfg): + """Pink action with opt-in, backend-specific target conditioning for GR1T2. + + The feature is disabled by default, preserving the regular Kit/PhysX action + path. The task enables it for kitless OVPhysX or Newton and fills the + hand metadata while conditioning the spawned USD before physics parses it. + """ + + class_type: type[GR1T2KitlessPinkInverseKinematicsAction] | str = ( + "{DIR}.gr1t2_kitless_action:GR1T2KitlessPinkInverseKinematicsAction" + ) + + kitless_ovphysx_enabled: bool = False + """Enable the shared raw-GR1T2 joint/target policy for standalone OVPhysX and Newton.""" + + max_ik_joint_delta_rad: float = 0.2 + """Maximum commanded IK-joint change from measured position per policy step [rad].""" + + max_ik_joint_magnitude_rad: float = 3.5 + """Symmetric absolute bound for commanded IK-joint positions [rad].""" + + max_hand_target_step_rad: float = 0.079 + """Maximum hand-target change per policy step [rad].""" + + hand_target_margin_rad: float = 0.5 + """Extra target travel beyond anatomical limits, used to sustain grip effort [rad].""" + + hand_mimic_relations: dict[str, tuple[str, float, float]] = field(default_factory=dict) + """Mimic joint to ``(reference joint, gearing, offset)`` from the spawned USD.""" + + hand_joint_limits: dict[str, tuple[float, float]] = field(default_factory=dict) + """Original anatomical hand-joint limits recorded before physical limits are widened [rad].""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/pickplace_gr1t2_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/pickplace_gr1t2_env_cfg.py index 2a049e692bc5..38e122fc011d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/pickplace_gr1t2_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/pickplace_gr1t2_env_cfg.py @@ -6,12 +6,14 @@ import os import tempfile +from isaaclab_physx.sim.schemas import PhysxArticulationCfg, PhysxRigidBodyCfg + import isaaclab.envs.mdp as base_mdp import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg from isaaclab.controllers.pink_ik import DampingTaskCfg, FrameTaskCfg, NullSpacePostureTaskCfg, PinkIKControllerCfg from isaaclab.envs import ManagerBasedRLEnvCfg -from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import ObservationGroupCfg as ObsGroup from isaaclab.managers import ObservationTermCfg as ObsTerm @@ -19,17 +21,25 @@ from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.scene import InteractiveSceneCfg from isaaclab.sensors import ContactSensorCfg +from isaaclab.sim import SimulationCfg from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg from isaaclab.utils import configclass, replace from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR, retrieve_file_path from isaaclab.visualizers import VisualizerCfg from isaaclab_tasks.contrib import ISAACLAB_TELEOP_AVAILABLE +from isaaclab_tasks.contrib.steering_wheel import ( + condition_newton_steering_wheel_detail_collisions as _condition_newton_steering_wheel_detail_collisions, +) +from isaaclab_tasks.contrib.teleop_physics_cfg import TeleopPhysicsCfg from . import mdp +from .gr1t2_kitless_action_cfg import GR1T2KitlessPinkInverseKinematicsActionCfg from isaaclab_assets.robots.fourier import GR1T2_HIGH_PD_CFG # isort: skip + from isaaclab_tasks.contrib.robot_pov_camera_cfg import robot_pov_camera_cfg # isort: skip +from isaaclab_tasks.contrib.packing_table import condition_packing_table_for_kitless_ovphysx # isort: skip def build_gr1t2_pickplace_pipeline(): @@ -259,7 +269,13 @@ def build_gr1t2_pickplace_pipeline(): } ) - pipeline = OutputCombiner({"action": connected_reorderer.output("output")}) + pipeline = OutputCombiner( + { + "action": connected_reorderer.output("output"), + HandsSource.LEFT: hands.output(HandsSource.LEFT), + HandsSource.RIGHT: hands.output(HandsSource.RIGHT), + } + ) return pipeline, [left_dex, right_dex] @@ -383,7 +399,7 @@ class PickPlaceGR1T2SceneCfg(ObjectTableSceneCfg): class ActionsCfg: """Action specifications for the MDP.""" - upper_body_ik = PinkInverseKinematicsActionCfg( + upper_body_ik = GR1T2KitlessPinkInverseKinematicsActionCfg( pink_controlled_joint_names=[ "left_shoulder_pitch_joint", "left_shoulder_roll_joint", @@ -582,6 +598,8 @@ class EventCfg: class PickPlaceGR1T2EnvCfg(ManagerBasedRLEnvCfg): """Configuration for the GR1T2 environment.""" + sim: SimulationCfg = SimulationCfg(physics=TeleopPhysicsCfg()) + # Scene settings scene: PickPlaceGR1T2SceneCfg = PickPlaceGR1T2SceneCfg(num_envs=1, env_spacing=2.5) # Basic settings @@ -660,12 +678,21 @@ def __post_init__(self): self.image_obs_list = ["robot_pov_cam"] if ISAACLAB_TELEOP_AVAILABLE: - from isaaclab_teleop import GloveHapticFeedbackCfg, IsaacTeleopCfg, XrCameraFeedCfg, XrCfg + from isaaclab_teleop import ( + GloveHapticFeedbackCfg, + IsaacTeleopCfg, + XrCameraFeedCfg, + XrCameraFeedLayoutCfg, + XrCfg, + ) # Isaac Capture-based teleoperation pipeline. self.xr = XrCfg( anchor_pos=(0.0, 0.0, 0.0), anchor_rot=(0.0, 0.0, 0.0, 1.0), + # The shared anchor places the default eye inside the robot head. + # Exclude visual meshes from the eye partition, not source visibility. + excluded_visual_meshes=["/World/envs/env_0/Robot/head_.*_link/visuals/.*"], ) self.isaac_teleop = IsaacTeleopCfg( pipeline_builder=lambda: build_gr1t2_pickplace_pipeline()[0], @@ -680,6 +707,11 @@ def __post_init__(self): max_update_hz=0.0, ) ], + xr_camera_feed_layout=XrCameraFeedLayoutCfg( + placement="world", + world_position_m=(0.0, 0.8, 1.6), + world_orientation_xyzw=(0.70710678, 0.0, 0.0, 0.70710678), + ), ) # Per-finger haptic glove feedback: vibrate each finger of the operator's @@ -690,3 +722,226 @@ def __post_init__(self): left_sensor_name="left_hand_contact", right_sensor_name="right_hand_contact", ) + + def stabilize_for_kitless_ovphysx(self) -> None: + """Configure the GR1T2 asset and action boundaries for kitless OVPhysX.""" + action_cfg = self.actions.upper_body_ik + action_cfg.controller.urdf_path = os.path.join( + os.path.dirname(__file__), "assets", "GR1T2_fourier_hand_6dof_kinematics.urdf" + ) + action_cfg.kitless_ovphysx_enabled = True + # Do not apply retargeter defaults before hand tracking starts. + self.isaac_teleop.require_initial_bimanual_hand_tracking = True + _apply_kitless_ovphysx_drive_gains(self.scene.robot) + _condition_kitless_ovphysx_robot_spawn_properties(self.scene.robot) + _condition_kitless_ovphysx_gr1t2_stage(self.scene.robot, action_cfg) + condition_packing_table_for_kitless_ovphysx(self.scene.packing_table) + + def stabilize_for_kitless_newton_mjwarp(self) -> None: + """Configure GR1T2 actuators and contacts for kitless Newton.""" + action_cfg = self.actions.upper_body_ik + action_cfg.controller.urdf_path = os.path.join( + os.path.dirname(__file__), "assets", "GR1T2_fourier_hand_6dof_kinematics.urdf" + ) + action_cfg.kitless_ovphysx_enabled = True + self.isaac_teleop.require_initial_bimanual_hand_tracking = True + _stabilize_gr1t2_for_kitless_newton_mjwarp(self) + _condition_kitless_ovphysx_gr1t2_stage(self.scene.robot, action_cfg, widen_hand_limits=False) + _wrap_packing_table_collision_for_newton(self.scene.packing_table) + + +def _stabilize_gr1t2_for_kitless_newton_mjwarp(env_cfg: ManagerBasedRLEnvCfg) -> None: + """Apply Newton actuator, allocation-budget, and wheel-collision preparation.""" + from isaaclab_newton.sim.schemas import NewtonArticulationCfg + + # Match the kitless OVPhysX task: internal finger contacts must not force + # compliant thumb drives away from their commands. Object contacts remain. + fragments = env_cfg.scene.robot.spawn.articulation_props + for fragment in fragments if isinstance(fragments, list) else [fragments]: + if isinstance(fragment, NewtonArticulationCfg): + fragment.self_collision_enabled = False + + groups = env_cfg.scene.robot.actuators + contracts = { + "head": (["head_.*"], 80.0, 8.0, 0.03, 80.0), + "trunk": (["waist_.*"], 500.0, 50.0, 0.10, 250.0), + "legs": ([".*_hip_.*", ".*_knee_.*", ".*_ankle_.*"], 300.0, 30.0, 0.125, 250.0), + "right-arm": (["right_shoulder_.*", "right_elbow_.*", "right_wrist_.*"], 1200.0, 60.0, 0.30, 800.0), + "left-arm": (["left_shoulder_.*", "left_elbow_.*", "left_wrist_.*"], 1200.0, 60.0, 0.30, 800.0), + } + for name, (joint_names, stiffness, damping, armature, effort_limit) in contracts.items(): + actuator = groups.get(name) + if actuator is None: + actuator = ImplicitActuatorCfg(joint_names_expr=joint_names) + groups[name] = actuator + actuator.joint_names_expr = joint_names + actuator.stiffness = stiffness + actuator.damping = damping + actuator.armature = armature + actuator.effort_limit_sim = effort_limit + + for side, name in (("L", "left-hand"), ("R", "right-hand")): + # The action conditions all eleven targets to the asset's mimic + # relation. Driving the whole hand gives Newton the same compliant + # hand targets as the OVPhysX path. + hand = groups[name] + hand.joint_names_expr = [f"{side}_.*"] + hand.stiffness = 5.0 + hand.damping = 0.5 + hand.armature = 0.02 + hand.effort_limit_sim = 20.0 + groups.pop(f"{name}-slaved", None) + + solver_cfg = env_cfg.sim.physics.solver_cfg + solver_cfg.nconmax = 4096 + solver_cfg.njmax = 1536 + _condition_newton_steering_wheel_detail_collisions(env_cfg.scene.object) + + +def _wrap_packing_table_collision_for_newton(asset_cfg: AssetBaseCfg) -> None: + """Use the shared USD collision preparation for the packing-table asset.""" + from isaaclab_tasks.contrib.packing_table import configure_packing_table_collisions + + configure_packing_table_collisions(asset_cfg) + + +def _apply_kitless_ovphysx_drive_gains(robot_cfg: ArticulationCfg) -> None: + """Restore stable actuator coverage around the Kit-tuned arm drives.""" + from isaaclab.actuators import ImplicitActuatorCfg + + groups = { + "head": (["head_.*"], 80.0, 8.0, 0.03, 80.0), + "legs": ([".*_hip_.*", ".*_knee_.*", ".*_ankle_.*"], 300.0, 30.0, 0.10, 250.0), + "right-hand": (None, 5.0, 0.5, 0.001, 20.0), + "left-hand": (None, 5.0, 0.5, 0.001, 20.0), + } + for group_name, (joint_expr, stiffness, damping, armature, effort_limit) in groups.items(): + actuator = robot_cfg.actuators.get(group_name) + if actuator is None: + assert joint_expr is not None + actuator = ImplicitActuatorCfg(joint_names_expr=list(joint_expr)) + robot_cfg.actuators[group_name] = actuator + actuator.stiffness = stiffness + actuator.damping = damping + actuator.armature = armature + actuator.effort_limit_sim = effort_limit + + if robot_cfg.spawn is not None and robot_cfg.spawn.articulation_props is not None: + fragments = robot_cfg.spawn.articulation_props + for fragment in fragments if isinstance(fragments, list) else [fragments]: + if isinstance(fragment, PhysxArticulationCfg): + fragment.enabled_self_collisions = False + + +def _condition_kitless_ovphysx_robot_spawn_properties(robot_cfg: ArticulationCfg) -> None: + """Limit robot contact depenetration for kitless OVPhysX.""" + if robot_cfg.spawn is not None and robot_cfg.spawn.rigid_props is not None: + fragments = robot_cfg.spawn.rigid_props + for fragment in fragments if isinstance(fragments, list) else [fragments]: + if isinstance(fragment, PhysxRigidBodyCfg): + fragment.max_depenetration_velocity = 0.1 + + +def _author_kitless_ovphysx_contact_offsets(prim) -> bool: + """Author Kit-parity offsets on a collision prim before OVPhysX parses it.""" + from pxr import Sdf + + schemas = prim.GetMetadata("apiSchemas") + applied = [str(schema) for schema in (schemas.GetAddedOrExplicitItems() if schemas else [])] + if not any(schema.startswith("PhysicsCollisionAPI") for schema in applied): + return False + if not any(schema.startswith("PhysxCollisionAPI") for schema in applied): + list_op = Sdf.TokenListOp() + list_op.explicitItems = [*applied, "PhysxCollisionAPI"] + prim.SetMetadata("apiSchemas", list_op) + prim.CreateAttribute("physxCollision:contactOffset", Sdf.ValueTypeNames.Float).Set(0.001) + prim.CreateAttribute("physxCollision:restOffset", Sdf.ValueTypeNames.Float).Set(0.0) + return True + + +def _condition_kitless_ovphysx_gr1t2_stage( + robot_cfg: ArticulationCfg, + action_cfg: GR1T2KitlessPinkInverseKinematicsActionCfg, + *, + widen_hand_limits: bool = True, +) -> None: + """Condition the spawned GR1T2 hand description before kitless physics parses it. + + The raw asset has near-singular finger inertia, hard PhysX-only mimic + constraints, and hand rest poses exactly on physical limits. Those inputs + are stable after Kit's asset preparation but are ill-conditioned when read + directly by OVPhysX. This spawn boundary floors inertia, removes the hard + constraints that conflict with direct hand drives, and optionally keeps the + physical limits inactive for OVPhysX. Newton retains the anatomical limits + because bounding drive targets alone does not bound simulated positions. The + original mimic relations and anatomical limits are recorded on the typed + action config for target conditioning. The source USD and normal Kit path + remain unchanged. + """ + if robot_cfg.spawn is None or getattr(robot_cfg.spawn, "_kitless_ovphysx_doctored", False): + return + original_spawn = robot_cfg.spawn.func + + def _spawn_and_condition(prim_path, cfg, *args, **kwargs): + import math + + from pxr import Gf, Sdf, Usd + + prim = original_spawn(prim_path, cfg, *args, **kwargs) + action_cfg.hand_joint_limits.clear() + action_cfg.hand_mimic_relations.clear() + + # Collision meshes below instanceable ``collisions`` scopes are + # immutable proxies. De-instance those scopes so the parser-visible + # collision prims can receive the OVPhysX contact offsets. + for child in Usd.PrimRange(prim): + if child.GetName() == "collisions" and child.IsInstanceable(): + child.SetInstanceable(False) + + for child in Usd.PrimRange(prim): + _author_kitless_ovphysx_contact_offsets(child) + inertia = child.GetAttribute("physics:diagonalInertia") + if inertia and inertia.HasValue(): + value = inertia.Get() + if min(value) < 1.0e-4: + inertia.Set(Gf.Vec3f(*(max(component, 1.0e-4) for component in value))) + + is_hand_joint = child.GetName()[:2] in ("L_", "R_") and child.GetTypeName() == "PhysicsRevoluteJoint" + schemas = child.GetMetadata("apiSchemas") + applied = list(schemas.GetAddedOrExplicitItems()) if schemas else [] + kept = [schema for schema in applied if not str(schema).startswith("PhysxMimicJointAPI")] + if len(kept) != len(applied): + gearing = child.GetAttribute("physxMimicJoint:rotZ:gearing") + offset = child.GetAttribute("physxMimicJoint:rotZ:offset") + reference = child.GetRelationship("physxMimicJoint:rotZ:referenceJoint") + targets = reference.GetTargets() if reference else [] + if gearing and gearing.HasValue() and targets: + action_cfg.hand_mimic_relations[child.GetName()] = ( + targets[0].name, + float(gearing.Get()), + float(offset.Get()) if offset and offset.HasValue() else 0.0, + ) + list_op = Sdf.TokenListOp() + list_op.explicitItems = kept + child.SetMetadata("apiSchemas", list_op) + for property_name in [name for name in child.GetPropertyNames() if name.startswith("physxMimicJoint:")]: + if not child.RemoveProperty(property_name): + mimic_attr = child.GetAttribute(property_name) + if mimic_attr and mimic_attr.GetTypeName() == Sdf.ValueTypeNames.Float: + mimic_attr.Set(0.0) + + if is_hand_joint: + lower = child.GetAttribute("physics:lowerLimit") + upper = child.GetAttribute("physics:upperLimit") + if lower and upper and lower.HasValue() and upper.HasValue(): + action_cfg.hand_joint_limits[child.GetName()] = ( + math.radians(float(lower.Get())), + math.radians(float(upper.Get())), + ) + if widen_hand_limits: + lower.Set(-720.0) + upper.Set(720.0) + return prim + + robot_cfg.spawn.func = _spawn_and_condition + robot_cfg.spawn._kitless_ovphysx_doctored = True diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py index 9e04c5afe254..f01afd90de8b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py @@ -3,6 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause +import os import tempfile from isaaclab.envs import ManagerBasedRLEnvCfg @@ -16,7 +17,13 @@ ObjectTableSceneCfg, ObservationsCfg, TerminationsCfg, + _apply_kitless_ovphysx_drive_gains, + _condition_kitless_ovphysx_gr1t2_stage, + _condition_kitless_ovphysx_robot_spawn_properties, + _stabilize_gr1t2_for_kitless_newton_mjwarp, + _wrap_packing_table_collision_for_newton, build_gr1t2_pickplace_pipeline, + condition_packing_table_for_kitless_ovphysx, ) @@ -66,6 +73,8 @@ def __post_init__(self): self.xr = XrCfg( anchor_pos=(0.0, 0.0, 0.0), anchor_rot=(0.0, 0.0, 0.0, 1.0), + # Exclude head visuals from the operator camera, not the source scene. + excluded_visual_meshes=["/World/envs/env_0/Robot/head_.*_link/visuals/.*"], ) self.isaac_teleop = IsaacTeleopCfg( pipeline_builder=lambda: build_gr1t2_pickplace_pipeline()[0], @@ -81,3 +90,25 @@ def __post_init__(self): left_sensor_name="left_hand_contact", right_sensor_name="right_hand_contact", ) + + def stabilize_for_kitless_ovphysx(self) -> None: + """Prepare the existing GR asset while retaining the movable waist.""" + action_cfg = self.actions.upper_body_ik + action_cfg.controller.urdf_path = os.path.join( + os.path.dirname(__file__), "assets", "GR1T2_fourier_hand_6dof_kinematics.urdf" + ) + action_cfg.kitless_ovphysx_enabled = True + self.isaac_teleop.require_initial_bimanual_hand_tracking = True + _apply_kitless_ovphysx_drive_gains(self.scene.robot) + _condition_kitless_ovphysx_robot_spawn_properties(self.scene.robot) + _condition_kitless_ovphysx_gr1t2_stage(self.scene.robot, action_cfg) + condition_packing_table_for_kitless_ovphysx(self.scene.packing_table) + + def stabilize_for_kitless_newton_mjwarp(self) -> None: + """Configure GR1T2 actuators and contacts for kitless Newton.""" + # Keep the USD exporter as the default; select this model for Newton preparation. + self.actions.upper_body_ik.controller.urdf_path = os.path.join( + os.path.dirname(__file__), "assets", "GR1T2_fourier_hand_6dof_kinematics.urdf" + ) + _stabilize_gr1t2_for_kitless_newton_mjwarp(self) + _wrap_packing_table_collision_for_newton(self.scene.packing_table) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/stack/config/so101/stack_ik_abs_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/stack/config/so101/stack_ik_abs_env_cfg.py index 9b7e8a9c106a..3a7317c7302d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/stack/config/so101/stack_ik_abs_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/stack/config/so101/stack_ik_abs_env_cfg.py @@ -3,9 +3,11 @@ # # SPDX-License-Identifier: BSD-3-Clause +import inspect from dataclasses import MISSING import numpy as np +from isaaclab_ov.physics import OvPhysxCfg from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.utils import configclass, replace @@ -31,30 +33,17 @@ _SO101_GRIPPER_OFFSET = SO101_GRIPPER_OPEN # [rad] closedness c=0 -> open _SO101_GRIPPER_SCALE = SO101_GRIPPER_CLOSE - SO101_GRIPPER_OPEN # [rad] c=1 -> close -# Clutch EE reset-origin home: the ``base_T_ee`` transform [m] for the gripper's pose in the -# robot base frame at the seated reset pose. The clutch seeds its home from this on reset / first -# engage (no live end-effector is read; the base rebase is handled upstream by -# ``target_frame_prim_path`` and the EE state forward is the clutch's own running home). The robot -# ``init_state`` MUST forward-kinematics to this value, or engaging snaps the arm -- this is the -# forward kinematics of the seated ``_SO101_STACK_INIT_JOINT_POS`` measured in sim, so re-measure -# (read ``ee_frame.data.target_pos_source`` at reset) if those init joints change. Only the -# translation drives the IK position command; the orientation command is the live controller grip -# orientation composed with the clutch's fixed calibration offset, not this transform's rotation. -_BASE_T_GRIPPER_HOME = np.eye(4, dtype=np.float32) -_BASE_T_GRIPPER_HOME[:3, 3] = (0.01918, -0.18852, 0.18887) - -# Fixed controller-grip -> gripper_link orientation calibration offset [x, y, z, w] (xyzw), -# passed to the clutch as a body-frame right multiply. ``None`` uses the clutch default (Rz(pi)). -# Derivation: with ``q_grip`` the controller grip orientation and ``q_G0`` the gripper_link -# orientation in the base frame (both xyzw) at the reset pose, the offset is -# ``quat_inv(q_grip) (x) q_G0`` (xyzw); tuning it here needs no Teleop rebuild. -# Set to RPY (roll, pitch, yaw) = (-90, 0, 60) degrees (intrinsic XYZ), i.e. -# ``Rotation.from_euler("XYZ", [-90, 0, 60], degrees=True).as_quat()``. -_SO101_ORIENTATION_OFFSET_XYZW: tuple[float, float, float, float] | None = ( - -0.61237244, - 0.35355339, - 0.35355339, - 0.61237244, +# Nominal Robot/gripper reset pose in Robot/base, derived from the asset's USD joint frames +# at _SO101_STACK_INIT_JOINT_POS. The engage-relative clutch uses both translation and rotation. +# Update the complete transform if the asset or reset joints change; world seating is not included. +_BASE_T_GRIPPER_HOME = np.array( + [ + [0.00000711, 1.0, -0.00000682, 0.02061444], + [-0.71735179, 0.00000985, 0.69671114, -0.18883384], + [0.69671114, -0.00000006, 0.71735179, 0.19226513], + [0.0, 0.0, 0.0, 1.0], + ], + dtype=np.float32, ) @@ -66,8 +55,9 @@ def _build_so101_stack_pipeline(): tensor via TensorReorderer. The SO-101 is 5-DOF and driven by a full-SE3-pose IK over all 5 arm joints (shoulder_pan, - shoulder_lift, elbow_flex, wrist_flex, wrist_roll): the clutch emits the controller pose - (position + grip orientation composed with a fixed calibration offset) and the IK tracks + shoulder_lift, elbow_flex, wrist_flex, wrist_roll): the clutch applies controller motion + relative to each engage pose to its full home pose (Capture 1.7). The supported + isaacteleop 1.4 API instead retains calibrated absolute grip orientation. The IK tracks position exactly while best-effort tracking orientation (soft-weighted). The gripper jaw tracks the trigger proportionally (analog), and the EE position is clutch-rebased around a captured origin so engaging teleop does not teleport the arm. @@ -99,22 +89,20 @@ def _build_so101_stack_pipeline(): # receives base-frame data matching the absolute-pose IK command frame. transformed_controllers = controllers.transformed(transform_input.output(ValueInput.VALUE)) - # Clutch (relative-origin) EE-pose retargeter (right hand). Emits the same absolute 7D - # "ee_pose" contract as Se3AbsRetargeter (node name + output key "ee_pose"), but rebases - # controller motion around an origin captured on engage. It seeds the home from the static - # ``home_base_T_ee`` reset-origin on reset / first engage and keeps its own running home - # thereafter (so a mid-task re-clutch resumes from the last commanded pose). The robot - # ``init_state`` must reset the arm to ``_BASE_T_GRIPPER_HOME`` so the first engage does not - # jump. The full pose (position + orientation) drives the 5-joint SE3 IK below. + # Both APIs rebase position around a captured origin and retain the commanded position + # across re-engages. Capture 1.7 additionally clutches orientation around the full home; + # isaacteleop 1.4 retains absolute grip orientation with its existing body-frame calibration. + # Pass the calibration only to the API that explicitly accepts it. + clutch_options = {} + if "orientation_offset" in inspect.signature(SO101ClutchRetargeter).parameters: + clutch_options["orientation_offset"] = np.array( + [-0.61237244, 0.35355339, 0.35355339, 0.61237244], dtype=np.float64 + ) # intrinsic XYZ (-90, 0, 60) degrees, xyzw clutch = SO101ClutchRetargeter( name="ee_pose", input_device=ControllersSource.RIGHT, home_base_T_ee=_BASE_T_GRIPPER_HOME, - orientation_offset=( - np.array(_SO101_ORIENTATION_OFFSET_XYZW, dtype=np.float64) - if _SO101_ORIENTATION_OFFSET_XYZW is not None - else None - ), + **clutch_options, ) connected_clutch = clutch.connect( { @@ -202,6 +190,16 @@ class SO101CubeStackEnvCfg(stack_joint_pos_env_cfg.SO101CubeStackEnvCfg): def __post_init__(self): # post init of parent super().__post_init__() + # The clutch resets to a configured full pose, so the arm must return to the same + # nominal joints on every reset. Keep the reset event active, without joint noise. + self.events.randomize_joint_state.params["std"] = 0.0 + self.sim.physics = preset( + default=self.sim.physics.default, + isaacsim_physx=self.sim.physics.isaacsim_physx, + physx=self.sim.physics.physx, + newton_mjwarp=self.sim.physics.newton_mjwarp, + ovphysx=OvPhysxCfg(), + ) # Switch to a stiffer PD controller so the arm tracks IK targets well. Replace the robot # configured by the parent so its task-specific seated root and stack joint pose are retained. @@ -222,7 +220,9 @@ def __post_init__(self): variants={ "Robot": "robot", "Sensor": "sensors", - "Physics": preset(default="physx", isaacsim_physx="physx", physx="physx", newton_mjwarp="physics"), + "Physics": preset( + default="physx", isaacsim_physx="physx", physx="physx", ovphysx="physx", newton_mjwarp="physics" + ), }, ), actuators={ diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/steering_wheel.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/steering_wheel.py new file mode 100644 index 000000000000..b518e0f341de --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/steering_wheel.py @@ -0,0 +1,50 @@ +# Copyright (c) 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 + +"""Shared collision preparation for the contributed steering-wheel asset.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import RigidObjectCfg + +# These are the graspable wheel surfaces. Their decomposition preserves the +# rim opening and spoke gaps that a single hull would fill. +_STEERING_WHEEL_DECOMPOSED_MESHES = frozenset( + { + "sm_steeringwheel_a01_wheel_rim_01", + "sm_steeringwheel_a01_wheel_spokes_01", + } +) + + +def condition_newton_steering_wheel_detail_collisions(object_cfg: RigidObjectCfg) -> None: + """Hull only non-graspable wheel details that MuJoCo cannot compile as solids. + + The authored wheel uses convex decomposition throughout. Tiny hub and + quick-release decomposition pieces can have near-zero volume, which MuJoCo + rejects for solid inertia. The rim and spokes remain decomposed so the + wheel remains graspable; only detail meshes are switched to convex hulls. + """ + if object_cfg.spawn is None or getattr(object_cfg.spawn, "_kitless_newton_wheel_collision", False): + return + original_spawn = object_cfg.spawn.func + + def _spawn_with_selective_collision(prim_path, cfg, *args, **kwargs): + from pxr import Usd, UsdGeom + + prim = original_spawn(prim_path, cfg, *args, **kwargs) + for root_path in sim_utils.find_matching_prim_paths(prim_path): + root = prim.GetStage().GetPrimAtPath(root_path) + if not root.IsValid(): + continue + for mesh_prim in Usd.PrimRange(root): + if not mesh_prim.IsA(UsdGeom.Mesh) or mesh_prim.GetName() in _STEERING_WHEEL_DECOMPOSED_MESHES: + continue + approximation = mesh_prim.GetAttribute("physics:approximation") + if approximation and approximation.Get() is not None: + approximation.Set("convexHull") + return prim + + object_cfg.spawn.func = _spawn_with_selective_collision + object_cfg.spawn._kitless_newton_wheel_collision = True diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/teleop_physics_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/teleop_physics_cfg.py new file mode 100644 index 000000000000..eb3f7015b382 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/teleop_physics_cfg.py @@ -0,0 +1,26 @@ +# Copyright (c) 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 + +"""Backend selection for humanoid teleoperation tasks with task-owned preparation.""" + +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg +from isaaclab_ov.physics import OvPhysxCfg +from isaaclab_physx.physics import PhysxCfg + +from isaaclab.physics import PhysxAutoCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.utils import PresetCfg + + +@configclass +class TeleopPhysicsCfg(PresetCfg): + """Keep Kit as the default and expose the supported standalone backends.""" + + isaacsim_physx = PhysxCfg() + ovphysx = OvPhysxCfg() + newton_mjwarp = NewtonCfg(solver_cfg=MJWarpSolverCfg()) + physx = PhysxAutoCfg(isaacsim_physx=isaacsim_physx, ovphysx=ovphysx) + default = isaacsim_physx diff --git a/source/isaaclab_tasks/pyproject.toml b/source/isaaclab_tasks/pyproject.toml index 0fdc21f47ddf..a6be4bfb4fc2 100644 --- a/source/isaaclab_tasks/pyproject.toml +++ b/source/isaaclab_tasks/pyproject.toml @@ -39,3 +39,4 @@ include = ["isaaclab_tasks", "isaaclab_tasks.*"] [tool.setuptools.package-data] "*" = ["*.pyi"] +"isaaclab_tasks.contrib.pick_place" = ["assets/*.urdf"] diff --git a/source/isaaclab_tasks/test/contrib/stack/test_so101_pose_ik.py b/source/isaaclab_tasks/test/contrib/stack/test_so101_pose_ik.py index 01fbd819f859..1ad92552a89f 100644 --- a/source/isaaclab_tasks/test/contrib/stack/test_so101_pose_ik.py +++ b/source/isaaclab_tasks/test/contrib/stack/test_so101_pose_ik.py @@ -132,3 +132,53 @@ def test_env_cfg_arm_action_is_pose_and_ordering_matches_pipeline(): "arm_action", "gripper_action", ] + + +def test_ik_reset_restores_nominal_joints_without_changing_joint_control_randomization(): + """Repeated IK resets agree with the configured clutch home, rather than a random arm pose.""" + from types import SimpleNamespace + + from isaaclab_tasks.contrib.stack.config.so101.stack_ik_abs_env_cfg import SO101CubeStackEnvCfg + from isaaclab_tasks.contrib.stack.config.so101.stack_joint_pos_env_cfg import ( + _SO101_STACK_INIT_JOINT_POS, + ) + from isaaclab_tasks.contrib.stack.config.so101.stack_joint_pos_env_cfg import ( + SO101CubeStackEnvCfg as JointPositionCfg, + ) + + cfg = SO101CubeStackEnvCfg() + assert JointPositionCfg().events.randomize_joint_state.params["std"] == 0.02 + defaults = torch.tensor([list(_SO101_STACK_INIT_JOINT_POS.values())]) + written = {} + + class Robot: + data = SimpleNamespace( + default_joint_pos=SimpleNamespace(torch=defaults), + default_joint_vel=SimpleNamespace(torch=torch.zeros_like(defaults)), + soft_joint_pos_limits=SimpleNamespace(torch=torch.tensor([[[-10.0, 10.0]] * defaults.shape[1]])), + ) + + def find_joints(self, names): + return [list(_SO101_STACK_INIT_JOINT_POS).index(name) for name in names], names + + def set_joint_position_target_index(self, target, env_ids): + written["target"] = target.clone() + + def set_joint_velocity_target_index(self, target, env_ids): + written["velocity_target"] = target.clone() + + def write_joint_position_to_sim_index(self, position, env_ids): + written["position"] = position.clone() + + def write_joint_velocity_to_sim_index(self, velocity, env_ids): + written["velocity"] = velocity.clone() + + env = SimpleNamespace(cfg=cfg, scene={"robot": Robot()}) + event = cfg.events.randomize_joint_state + for _ in range(2): + written.clear() + event.func(env, torch.tensor([0]), **event.params) + torch.testing.assert_close(written["position"], defaults) + torch.testing.assert_close(written["target"], defaults) + torch.testing.assert_close(written["velocity"], torch.zeros_like(defaults)) + torch.testing.assert_close(written["velocity_target"], torch.zeros_like(defaults)) diff --git a/source/isaaclab_tasks/test/contrib/test_common_packing_collisions.py b/source/isaaclab_tasks/test/contrib/test_common_packing_collisions.py new file mode 100644 index 000000000000..3884d39b06c5 --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/test_common_packing_collisions.py @@ -0,0 +1,67 @@ +# Copyright (c) 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 + +"""Geometry-preserving common collision preparation, independent of a physics engine.""" + +from types import SimpleNamespace + +import pytest + +from pxr import Usd, UsdGeom, UsdPhysics + +from isaaclab_tasks.contrib.packing_table import configure_packing_table_collisions + + +@pytest.mark.parametrize("physx_offsets", [False, True]) +def test_existing_mesh_transforms_and_bin_colliders_are_preserved(physx_offsets): + stage = Usd.Stage.CreateInMemory() + expected = {} + + def spawn(path, cfg): + root = stage.DefinePrim(path, "Xform") + parent = stage.DefinePrim(path + "/Table", "Xform") + parent.SetInstanceable(True) + UsdGeom.Xformable(parent).AddTranslateOp().Set((0.3, -0.2, 0.4)) + UsdPhysics.CollisionAPI.Apply(parent) + UsdPhysics.MeshCollisionAPI.Apply(parent).CreateApproximationAttr().Set("boundingCube") + for index in range(4): + mesh = UsdGeom.Mesh.Define(stage, path + f"/Table/Mesh_{index}") + mesh.CreatePointsAttr().Set([(0, 0, 0), (1, 0, 0), (0, 1, 0)]) + mesh.CreateFaceVertexCountsAttr().Set([3]) + mesh.CreateFaceVertexIndicesAttr().Set([0, 1, 2]) + UsdGeom.Xformable(mesh).AddTranslateOp().Set((index, index / 2, index / 4)) + for index in range(5): + cube = UsdGeom.Cube.Define(stage, path + f"/Bin/Cube_{index}") + cube.CreateSizeAttr().Set(0.1 + index) + UsdPhysics.CollisionAPI.Apply(cube.GetPrim()) + cache = UsdGeom.XformCache() + for prim in Usd.PrimRange(root): + if prim.IsA(UsdGeom.Gprim): + expected[str(prim.GetPath())] = cache.GetLocalToWorldTransform(prim) + return root + + cfg = SimpleNamespace(spawn=SimpleNamespace(func=spawn)) + configure_packing_table_collisions(cfg, physx_offsets=physx_offsets) + prepared = cfg.spawn.func + configure_packing_table_collisions(cfg, physx_offsets=physx_offsets) + assert cfg.spawn.func is prepared + root = prepared("/World/PackingTable", cfg.spawn) + shapes = [p for p in Usd.PrimRange(root) if p.HasAPI(UsdPhysics.CollisionAPI)] + assert len(shapes) == 9 + assert {str(p.GetPath()) for p in shapes} == set(expected) + assert sum(p.IsA(UsdGeom.Mesh) for p in shapes) == 4 + assert sum(p.IsA(UsdGeom.Cube) for p in shapes) == 5 + assert not stage.GetPrimAtPath("/World/PackingTable/Table").HasAPI(UsdPhysics.CollisionAPI) + cache = UsdGeom.XformCache() + for shape in shapes: + assert cache.GetLocalToWorldTransform(shape) == expected[str(shape.GetPath())] + assert UsdPhysics.CollisionAPI(shape).GetCollisionEnabledAttr().Get() + assert UsdGeom.Imageable(shape).GetPurposeAttr().Get() == UsdGeom.Tokens.default_ + if shape.IsA(UsdGeom.Mesh): + assert UsdPhysics.MeshCollisionAPI(shape).GetApproximationAttr().Get() == "boundingCube" + assert list(UsdGeom.Mesh(shape).GetFaceVertexIndicesAttr().Get()) == [0, 1, 2] + if physx_offsets: + assert shape.GetAttribute("physxCollision:contactOffset").Get() == pytest.approx(0.001) + assert shape.GetAttribute("physxCollision:restOffset").Get() == 0.0 diff --git a/source/isaaclab_tasks/test/contrib/test_g1_kitless_newton.py b/source/isaaclab_tasks/test/contrib/test_g1_kitless_newton.py new file mode 100644 index 000000000000..fa77bf40da4e --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/test_g1_kitless_newton.py @@ -0,0 +1,245 @@ +# Copyright (c) 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 + +"""Sim-free tests for the G1 kitless Newton task boundary.""" + +import math +import re +from types import SimpleNamespace + +import pytest +import torch +from isaaclab_newton.physics import NewtonCfg + +from isaaclab_tasks.contrib.locomanip_pick_place.fixed_base_upper_body_ik_g1_env_cfg import ( + FixedBaseUpperBodyIKG1EnvCfg, +) +from isaaclab_tasks.contrib.locomanip_pick_place.g1_kitless_newton import ( + ACTUATOR_PROFILE, + JOINT_LIMIT_MARGIN_RAD, + JOINT_TARGET_MAX_STEP_RAD, + POSITION_MAX_STEP_M, + ROTATION_MAX_STEP_RAD, + G1NewtonBoundedPinkInverseKinematicsAction, + bound_pink_joint_targets, + condition_kitless_newton_g1_action, + teleop_action_target_issue, +) + +RECORDED_FIRST_ACTION = torch.tensor( + [ + -0.11201994, + 0.43588385, + 0.89166349, + -0.12487421, + -0.11215594, + 0.69946498, + 0.69467705, + 0.14433567, + 0.41181007, + 0.91755670, + 0.00914924, + -0.05320583, + 0.65801126, + 0.75107032, + *([0.0] * 14), + ], + dtype=torch.float32, +) + + +def _quat_angle(a: torch.Tensor, b: torch.Tensor) -> float: + a = torch.nn.functional.normalize(a, dim=0) + b = torch.nn.functional.normalize(b, dim=0) + return float(2.0 * torch.acos(torch.clamp(torch.abs(torch.sum(a * b)), 0.0, 1.0))) + + +def _fake_env(): + left = torch.tensor([0.23491086, 0.21602505, 0.63518274, -0.13474128, 0.79248637, -0.02669795, -0.59422028]) + right = torch.tensor([-0.30430639, 0.52459145, 0.78784978, 0.19782960, 0.52823287, 0.41972736, 0.71109974]) + poses = torch.stack((left, right)).unsqueeze(0) + robot = SimpleNamespace( + body_names=["left_wrist_yaw_link", "right_wrist_yaw_link"], + data=SimpleNamespace(body_link_pose_w=poses), + ) + cfg = SimpleNamespace( + actions=SimpleNamespace( + upper_body_ik=SimpleNamespace( + target_eef_link_names={ + "left_wrist": "left_wrist_yaw_link", + "right_wrist": "right_wrist_yaw_link", + } + ) + ) + ) + return SimpleNamespace(scene={"robot": robot}, cfg=cfg), left, right + + +def test_g1_newton_contract_is_backend_scoped_and_complete() -> None: + default_cfg = FixedBaseUpperBodyIKG1EnvCfg() + newton_cfg = FixedBaseUpperBodyIKG1EnvCfg() + newton_cfg.sim.physics = NewtonCfg() + newton_cfg.stabilize_for_kitless_newton_mjwarp() + + assert newton_cfg.sim.physics.solver_cfg.nconmax == 4096 + assert newton_cfg.sim.physics.solver_cfg.njmax == 1536 + for name, selected in ACTUATOR_PROFILE.items(): + actuator = newton_cfg.scene.robot.actuators[name] + assert {key: getattr(actuator, key) for key in selected} == selected + assert newton_cfg.actions.upper_body_ik.class_type is G1NewtonBoundedPinkInverseKinematicsAction + assert newton_cfg.isaac_teleop.action_conditioner is condition_kitless_newton_g1_action + assert newton_cfg.isaac_teleop.action_validator is teleop_action_target_issue + assert newton_cfg.scene.packing_table.spawn._packing_table_collision_prepared is True + assert newton_cfg.scene.object.spawn._kitless_newton_wheel_collision is True + + assert default_cfg.isaac_teleop.action_conditioner is None + assert default_cfg.isaac_teleop.action_validator is None + assert not hasattr(default_cfg.scene.packing_table.spawn, "_packing_table_collision_prepared") + assert not hasattr(default_cfg.scene.object.spawn, "_kitless_newton_wheel_collision") + + +@pytest.mark.parametrize("task", ["fixed_base", "locomanipulation"]) +def test_g1_contact_mappings_are_bilateral_and_nested(task) -> None: + from isaaclab_tasks.contrib.locomanip_pick_place.locomanipulation_g1_env_cfg import LocomanipulationG1EnvCfg + + cfg = FixedBaseUpperBodyIKG1EnvCfg() if task == "fixed_base" else LocomanipulationG1EnvCfg() + + assert cfg.scene.left_hand_contact.prim_path == "{ENV_REGEX_NS}/Robot/left_hand/left_hand_[^/]*_link" + assert cfg.scene.right_hand_contact.prim_path == "{ENV_REGEX_NS}/Robot/right_hand/right_hand_[^/]*_link" + assert cfg.haptic_feedback.left_sensor_name == "left_hand_contact" + assert cfg.haptic_feedback.right_sensor_name == "right_hand_contact" + + compiled_labels = ( + "/World/envs/env_0/Robot/left_hand/left_hand_index_0_link", + "/World/envs/env_0/Robot/right_hand/right_hand_thumb_2_link", + "/World/envs/env_0/Robot/torso_link", + ) + left_pattern = cfg.scene.left_hand_contact.prim_path.replace("{ENV_REGEX_NS}", r"/World/envs/env_[^/]+") + right_pattern = cfg.scene.right_hand_contact.prim_path.replace("{ENV_REGEX_NS}", r"/World/envs/env_[^/]+") + left = [label for label in compiled_labels if re.fullmatch(left_pattern, label)] + right = [label for label in compiled_labels if re.fullmatch(right_pattern, label)] + assert left == [compiled_labels[0]] + assert right == [compiled_labels[1]] + assert set(left).isdisjoint(right) + + +def test_locomanipulation_preserves_agile_policy_order_and_action_contract() -> None: + from isaaclab_tasks.contrib.locomanip_pick_place.locomanipulation_g1_env_cfg import LocomanipulationG1EnvCfg + + cfg = LocomanipulationG1EnvCfg() + controller = cfg.actions.lower_body_joint_pos.copy() + upper_body_class = cfg.actions.upper_body_ik.class_type + cfg.sim.physics = NewtonCfg() + cfg.stabilize_for_kitless_newton_mjwarp() + + assert cfg.scene.robot.joint_ordering == "physx" + assert cfg.actions.lower_body_joint_pos == controller + assert cfg.actions.upper_body_ik.class_type == upper_body_class + assert cfg.isaac_teleop.action_conditioner is None + assert cfg.sim.physics.solver_cfg.integrator == "implicitfast" + assert cfg.sim.physics.num_substeps == 2 + + +def test_recorded_first_action_is_ramped_and_finite() -> None: + env, left, right = _fake_env() + conditioned = condition_kitless_newton_g1_action(env, RECORDED_FIRST_ACTION, {}) + + assert teleop_action_target_issue(conditioned) is None + assert torch.isfinite(conditioned).all() + assert float(torch.linalg.norm(conditioned[:3] - left[:3])) <= POSITION_MAX_STEP_M + 1.0e-6 + assert float(torch.linalg.norm(conditioned[7:10] - right[:3])) <= POSITION_MAX_STEP_M + 1.0e-6 + assert _quat_angle(conditioned[3:7], left[3:7]) <= ROTATION_MAX_STEP_RAD + 1.0e-5 + assert _quat_angle(conditioned[10:14], right[3:7]) <= ROTATION_MAX_STEP_RAD + 1.0e-5 + assert torch.equal(conditioned[14:], RECORDED_FIRST_ACTION[14:]) + + +@pytest.mark.parametrize( + "angle, axis, sign", [(0.9, (1, 0, 0), 1), (1.2, (0, 1, 1), -1), (0.1, (0, 0, 1), 1), (0, (1, 0, 0), -1)] +) +def test_conditioned_wrist_rotation_obeys_angular_cap(angle, axis, sign): + """The public action conditioner follows the short rotation without exceeding its cap.""" + env, _, _ = _fake_env() + pose = env.scene["robot"].data.body_link_pose_w + pose[0, :, 3:7] = torch.tensor([1.0, 0, 0, 0]) + direction = torch.nn.functional.normalize(torch.tensor(axis, dtype=torch.float32), dim=0) + target = sign * torch.cat((torch.tensor([math.cos(angle / 2)]), direction * math.sin(angle / 2))) + action = RECORDED_FIRST_ACTION.clone() + action[3:7] = action[10:14] = target + conditioned = condition_kitless_newton_g1_action(env, action, {}) + identity = torch.tensor([1.0, 0, 0, 0]) + for result in (conditioned[3:7], conditioned[10:14]): + assert torch.isfinite(result).all() + assert torch.linalg.norm(result).item() == pytest.approx(1, abs=1.0e-6) + assert _quat_angle(identity, result) <= min(angle, ROTATION_MAX_STEP_RAD) + 1.0e-5 + assert _quat_angle(result, target) == pytest.approx(max(0, angle - ROTATION_MAX_STEP_RAD), abs=1.0e-5) + + +def test_invalid_actions_fail_closed() -> None: + nonfinite = RECORDED_FIRST_ACTION.clone() + nonfinite[0] = float("nan") + assert teleop_action_target_issue(nonfinite) == "non-finite action target" + + oversized = RECORDED_FIRST_ACTION.clone() + oversized[14] = 7.0 + assert teleop_action_target_issue(oversized) == "unreasonable hand joint target magnitude" + assert "unexpected G1 controller action size" in teleop_action_target_issue(torch.zeros(27)) + + missing_wrist_env = SimpleNamespace(scene={}, cfg=SimpleNamespace()) + with pytest.raises(RuntimeError, match="could not resolve the task-selected G1 wrist poses"): + condition_kitless_newton_g1_action(missing_wrist_env, RECORDED_FIRST_ACTION, {}) + + +def test_pink_targets_are_slew_limited_and_clamped_to_common_limits() -> None: + current = torch.tensor([0.25, 0.0, -0.5]) + proposed = torch.tensor([0.5215, -4.0, 4.0]) + lower = torch.tensor([-0.2617994, -2.0, -1.0]) + upper = torch.tensor([0.2617994, 2.0, 1.0]) + + bounded = bound_pink_joint_targets(current, proposed, lower, upper) + + assert torch.all(bounded >= lower + JOINT_LIMIT_MARGIN_RAD) + assert torch.all(bounded <= upper - JOINT_LIMIT_MARGIN_RAD) + assert float(bounded[0]) == pytest.approx(0.2617994 - JOINT_LIMIT_MARGIN_RAD) + assert float(bounded[1]) == pytest.approx(-JOINT_TARGET_MAX_STEP_RAD) + assert float(bounded[2]) == pytest.approx(-0.5 + JOINT_TARGET_MAX_STEP_RAD) + + +def test_nonfinite_pink_target_fails_closed() -> None: + with pytest.raises(RuntimeError, match="non-finite Pink joint target boundary"): + bound_pink_joint_targets(torch.zeros(1), torch.tensor([float("nan")]), -torch.ones(1), torch.ones(1)) + + +def test_pink_limit_cache_keeps_live_articulation_limits(monkeypatch): + from isaaclab.envs.mdp.actions.pink_task_space_actions import PinkInverseKinematicsAction + + proposed = torch.tensor([[0.4]]) + monkeypatch.setattr(PinkInverseKinematicsAction, "_compute_ik_solutions", lambda self: proposed) + action = G1NewtonBoundedPinkInverseKinematicsAction.__new__(G1NewtonBoundedPinkInverseKinematicsAction) + action._env = SimpleNamespace(device="cpu") + limits = torch.tensor([[[-1.0, 1.0]]]) + action._asset = SimpleNamespace( + data=SimpleNamespace( + joint_pos=SimpleNamespace(torch=torch.tensor([[0.0]])), + joint_pos_limits=SimpleNamespace(torch=limits), + ) + ) + action._isaaclab_controlled_joint_ids = [0] + action._ik_controllers = [ + SimpleNamespace( + pink_configuration=SimpleNamespace( + controlled_model=SimpleNamespace( + lowerPositionLimit=[-0.5], + upperPositionLimit=[0.5], + ) + ), + pink_to_isaac_lab_controlled_ordering=[0], + ) + ] + action._pink_joint_limits = None + torch.testing.assert_close(action._compute_ik_solutions(), torch.tensor([[0.1]])) + cached = action._pink_joint_limits + limits[0, 0, 1] = 0.05 + torch.testing.assert_close(action._compute_ik_solutions(), torch.tensor([[0.049]])) + assert action._pink_joint_limits is cached diff --git a/source/isaaclab_tasks/test/contrib/test_g1_kitless_ovphysx.py b/source/isaaclab_tasks/test/contrib/test_g1_kitless_ovphysx.py new file mode 100644 index 000000000000..bcea51264e0d --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/test_g1_kitless_ovphysx.py @@ -0,0 +1,146 @@ +# Copyright (c) 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 + +"""Focused regressions for the G1 kitless OVPhysX packing-table contract.""" + +import gymnasium as gym +import pytest +import torch +from isaaclab_ov.physics import OvPhysxCfg + +from isaaclab.app import launch_simulation + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.contrib.locomanip_pick_place.fixed_base_upper_body_ik_g1_env_cfg import ( + FixedBaseUpperBodyIKG1EnvCfg, +) +from isaaclab_tasks.contrib.locomanip_pick_place.locomanipulation_g1_env_cfg import LocomanipulationG1EnvCfg +from isaaclab_tasks.contrib.pick_place.pickplace_gr1t2_env_cfg import PickPlaceGR1T2EnvCfg + +_TASK = "IsaacContrib-PickPlace-Locomanipulation-G1-Abs" + + +def test_fixed_base_g1_ovphysx_conditions_assets_without_changing_other_backends() -> None: + cfg = FixedBaseUpperBodyIKG1EnvCfg() + default_cfg = FixedBaseUpperBodyIKG1EnvCfg() + action_type = cfg.actions.upper_body_ik.class_type + drop_term = cfg.terminations.object_dropping.func + + cfg.stabilize_for_kitless_ovphysx() + table_spawn = cfg.scene.packing_table.spawn.func + wheel_spawn = cfg.scene.object.spawn.func + cfg.stabilize_for_kitless_ovphysx() + + assert cfg.scene.packing_table.spawn._packing_table_collision_prepared is True + assert cfg.scene.object.spawn.func == default_cfg.scene.object.spawn.func + assert cfg.scene.object.spawn.usd_path == default_cfg.scene.object.spawn.usd_path + assert cfg.scene.packing_table.spawn.func is table_spawn + assert cfg.scene.object.spawn.func is wheel_spawn + assert cfg.actions.upper_body_ik.class_type is action_type + assert cfg.terminations.object_dropping.func is drop_term + assert not hasattr(default_cfg.scene.packing_table.spawn, "_packing_table_collision_prepared") + assert not hasattr(default_cfg.scene.object.spawn, "_packing_table_collision_prepared") + + +def test_g1_public_ovphysx_hook_reuses_the_gr1t2_table_conditioner() -> None: + g1_cfg = LocomanipulationG1EnvCfg() + gr1t2_cfg = PickPlaceGR1T2EnvCfg() + + assert not hasattr(g1_cfg.scene.packing_table.spawn, "_packing_table_collision_prepared") + + g1_cfg.stabilize_for_kitless_ovphysx() + gr1t2_cfg.stabilize_for_kitless_ovphysx() + + assert g1_cfg.scene.packing_table.spawn._packing_table_collision_prepared is True + assert gr1t2_cfg.scene.packing_table.spawn._packing_table_collision_prepared is True + assert g1_cfg.scene.packing_table.spawn.func.__module__ == "isaaclab_tasks.contrib.packing_table" + assert gr1t2_cfg.scene.packing_table.spawn.func.__module__ == "isaaclab_tasks.contrib.packing_table" + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize( + ("cfg_type", "task"), + [ + (LocomanipulationG1EnvCfg, _TASK), + (FixedBaseUpperBodyIKG1EnvCfg, "IsaacContrib-PickPlace-FixedBaseUpperBodyIK-G1-Abs"), + ], + ids=["locomanipulation", "fixed_base"], +) +def test_g1_wheel_remains_supported_on_ovphysx_cpu(cfg_type, task) -> None: + pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") + + env_cfg = cfg_type() + env_cfg.scene.num_envs = 1 + env_cfg.sim.device = "cpu" + env_cfg.sim.physics = OvPhysxCfg() + env_cfg.scene.robot_pov_cam = None + env_cfg.observations.policy.robot_pov_cam = None + if isinstance(env_cfg, FixedBaseUpperBodyIKG1EnvCfg): + env_cfg.scene.left_wrist_cam = None + env_cfg.scene.right_wrist_cam = None + env_cfg.observations.policy.left_wrist_cam = None + env_cfg.observations.policy.right_wrist_cam = None + env_cfg.image_obs_list = [] + env_cfg.stabilize_for_kitless_ovphysx() + + env = None + launcher_args = { + "physics": None, + "device": "cpu", + "visualizer": None, + "visualizer_explicit": True, + } + try: + with launch_simulation(env_cfg, launcher_args): + env = gym.make(task, cfg=env_cfg).unwrapped + phases = ("table", "reset", "bin") if cfg_type is FixedBaseUpperBodyIKG1EnvCfg else ("table",) + for phase in phases: + env.reset() + wheel = env.scene["object"] + if phase == "bin": + from pxr import Usd, UsdGeom + + from isaaclab.sim import get_current_stage + + floor = get_current_stage().GetPrimAtPath( + "/World/envs/env_0/PackingTable/container_h20/container_h20_inst/colliders/Cube" + ) + assert floor.IsValid() + # Include invisible collision geometry when measuring the bin floor. + bounds = ( + UsdGeom.BBoxCache(Usd.TimeCode.Default(), [UsdGeom.Tokens.default_], False, True) + .ComputeWorldBound(floor) + .ComputeAlignedRange() + ) + center = bounds.GetMidpoint() + floor_top = bounds.GetMax()[2] + pose = wheel.data.root_pose_w.torch.clone() + pose[0, :3] = torch.tensor([center[0], center[1], floor_top + 0.10]) + wheel.write_root_pose_to_sim_index(root_pose=pose) + wheel.write_root_velocity_to_sim_index(root_velocity=torch.zeros((1, 6))) + + minimum_z = float("inf") + with torch.inference_mode(): + for _ in range(300): + env.sim.step(render=False) + env.scene.update(env_cfg.sim.dt) + minimum_z = min(minimum_z, float(wheel.data.root_pos_w.torch[0, 2].item())) + assert torch.isfinite(wheel.data.root_pose_w.torch).all() + assert torch.isfinite(wheel.data.root_vel_w.torch).all() + object_dropping = env_cfg.terminations.object_dropping.func( + env, **env_cfg.terminations.object_dropping.params + ) + assert not bool(object_dropping[0].item()), phase + + assert 300 * env_cfg.sim.dt == pytest.approx(1.5) + assert minimum_z > env_cfg.terminations.object_dropping.params["minimum_height"] + if phase == "bin": + final_position = wheel.data.root_pos_w.torch[0] + assert float(final_position[2]) >= floor_top - 0.005 + for axis in (0, 1): + assert bounds.GetMin()[axis] < float(final_position[axis]) < bounds.GetMax()[axis] + finally: + if env is not None: + env.close() diff --git a/source/isaaclab_tasks/test/contrib/test_gr1t2_kitless_action.py b/source/isaaclab_tasks/test/contrib/test_gr1t2_kitless_action.py new file mode 100644 index 000000000000..b739b59239c2 --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/test_gr1t2_kitless_action.py @@ -0,0 +1,372 @@ +# 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 + +"""Sim-free tests for the GR1T2 kitless OVPhysX action boundary.""" + +from types import SimpleNamespace + +import numpy as np +import pytest +import torch +from isaaclab_newton.physics import NewtonCfg +from isaaclab_ov.physics import OvPhysxCfg + +from pxr import Sdf, Usd, UsdGeom, UsdPhysics + +from isaaclab_tasks.contrib.pick_place import pickplace_gr1t2_env_cfg as gr1t2_env_cfg +from isaaclab_tasks.contrib.pick_place.gr1t2_kitless_action import ( + _HandTargetSlewState, + bound_ik_joint_targets, + constrain_hand_joint_targets, +) +from isaaclab_tasks.contrib.pick_place.gr1t2_kitless_action_cfg import ( + GR1T2KitlessPinkInverseKinematicsActionCfg, +) +from isaaclab_tasks.contrib.pick_place.pickplace_gr1t2_env_cfg import ( + PickPlaceGR1T2EnvCfg, + _condition_kitless_ovphysx_gr1t2_stage, + _condition_kitless_ovphysx_robot_spawn_properties, +) +from isaaclab_tasks.contrib.pick_place.pickplace_gr1t2_waist_enabled_env_cfg import PickPlaceGR1T2WaistEnabledEnvCfg + + +def test_newton_contract_is_backend_scoped_and_complete(): + default_cfg = PickPlaceGR1T2EnvCfg() + newton_cfg = PickPlaceGR1T2EnvCfg() + + newton_cfg.sim.physics = NewtonCfg() + newton_cfg.stabilize_for_kitless_newton_mjwarp() + + expected = { + "head": (80.0, 8.0, 0.03, 80.0), + "trunk": (500.0, 50.0, 0.10, 250.0), + "legs": (300.0, 30.0, 0.125, 250.0), + "right-arm": (1200.0, 60.0, 0.30, 800.0), + "left-arm": (1200.0, 60.0, 0.30, 800.0), + "right-hand": (5.0, 0.5, 0.02, 20.0), + "left-hand": (5.0, 0.5, 0.02, 20.0), + } + assert set(newton_cfg.scene.robot.actuators) == set(expected) + for name, values in expected.items(): + actuator = newton_cfg.scene.robot.actuators[name] + assert (actuator.stiffness, actuator.damping, actuator.armature, actuator.effort_limit_sim) == pytest.approx( + values + ) + assert newton_cfg.scene.robot.actuators["left-hand"].joint_names_expr == ["L_.*"] + assert newton_cfg.scene.robot.actuators["right-hand"].joint_names_expr == ["R_.*"] + assert newton_cfg.sim.physics.solver_cfg.nconmax == 4096 + assert newton_cfg.sim.physics.solver_cfg.njmax == 1536 + assert newton_cfg.isaac_teleop.require_initial_bimanual_hand_tracking is True + assert newton_cfg.actions.upper_body_ik.kitless_ovphysx_enabled is True + assert default_cfg.actions.upper_body_ik.kitless_ovphysx_enabled is False + assert newton_cfg.scene.robot.spawn._kitless_ovphysx_doctored is True + assert not hasattr(default_cfg.scene.robot.spawn, "_kitless_ovphysx_doctored") + assert default_cfg.isaac_teleop.require_initial_bimanual_hand_tracking is False + + assert set(default_cfg.scene.robot.actuators) == { + "trunk", + "right-arm", + "left-arm", + "right-hand", + "left-hand", + } + assert "legs" not in default_cfg.scene.robot.actuators + + +def test_newton_teleop_self_collision_matches_ovphysx_without_changing_asset_default(): + from isaaclab_newton.sim.schemas import NewtonArticulationCfg + from isaaclab_physx.sim.schemas import PhysxArticulationCfg + + default_cfg = PickPlaceGR1T2EnvCfg() + for backend, fragment_type, attribute in ( + ("newton_mjwarp", NewtonArticulationCfg, "self_collision_enabled"), + ("ovphysx", PhysxArticulationCfg, "enabled_self_collisions"), + ): + cfg = PickPlaceGR1T2EnvCfg() + cfg.sim.physics = OvPhysxCfg() if backend == "ovphysx" else NewtonCfg() + getattr(cfg, f"stabilize_for_kitless_{backend}")() + fragment = next(p for p in cfg.scene.robot.spawn.articulation_props if isinstance(p, fragment_type)) + original = next(p for p in default_cfg.scene.robot.spawn.articulation_props if isinstance(p, fragment_type)) + assert getattr(fragment, attribute) is False + assert getattr(original, attribute) is True + + +@pytest.mark.parametrize("backend", ["ovphysx", "newton_mjwarp"]) +def test_hand_physical_limits_are_widened_only_for_ovphysx(backend): + stage = Usd.Stage.CreateInMemory() + + def spawn_robot(path, _cfg): + root = stage.DefinePrim(path, "Xform") + joint = UsdPhysics.RevoluteJoint.Define(stage, path + "/R_thumb_distal_joint") + joint.CreateLowerLimitAttr().Set(-15.0) + joint.CreateUpperLimitAttr().Set(80.0) + return root + + cfg = PickPlaceGR1T2EnvCfg() + cfg.sim.physics = OvPhysxCfg() if backend == "ovphysx" else NewtonCfg() + cfg.scene.robot.spawn.func = spawn_robot + getattr(cfg, f"stabilize_for_kitless_{backend}")() + cfg.scene.robot.spawn.func("/World/Robot", cfg.scene.robot.spawn) + joint = UsdPhysics.RevoluteJoint(stage.GetPrimAtPath("/World/Robot/R_thumb_distal_joint")) + expected = (-720.0, 720.0) if backend == "ovphysx" else (-15.0, 80.0) + assert (joint.GetLowerLimitAttr().Get(), joint.GetUpperLimitAttr().Get()) == pytest.approx(expected) + assert cfg.actions.upper_body_ik.hand_joint_limits["R_thumb_distal_joint"] == pytest.approx( + (torch.deg2rad(torch.tensor(-15.0)).item(), torch.deg2rad(torch.tensor(80.0)).item()) + ) + + +def test_newton_table_wrapper_relocates_xform_collision_to_four_meshes(): + stage = Usd.Stage.CreateInMemory() + + def spawn_table(prim_path, _cfg): + table = stage.DefinePrim(prim_path, "Xform") + table.SetInstanceable(True) + source = stage.DefinePrim(f"{prim_path}/TableCollision", "Xform") + UsdGeom.Xformable(source).AddTranslateOp().Set((0.3, -0.2, 0.4)) + UsdPhysics.CollisionAPI.Apply(source) + UsdPhysics.MeshCollisionAPI.Apply(source).CreateApproximationAttr().Set(UsdPhysics.Tokens.boundingCube) + source.CreateAttribute("horde:filterOwner", Sdf.ValueTypeNames.String).Set("table") + for index in range(4): + mesh = UsdGeom.Mesh.Define(stage, f"{source.GetPath()}/Mesh_{index}") + mesh.CreatePointsAttr().Set([(0.0, 0.0, 0.0), (1.0, 0.0, 0.0), (0.0, 1.0, 0.0)]) + mesh.CreateFaceVertexCountsAttr().Set([3]) + mesh.CreateFaceVertexIndicesAttr().Set([0, 1, 2]) + bin_scope = stage.DefinePrim(f"{prim_path}/Bin", "Xform") + bin_scope.SetInstanceable(True) + for index in range(5): + cube = UsdGeom.Cube.Define(stage, f"{bin_scope.GetPath()}/Cube_{index}").GetPrim() + UsdPhysics.CollisionAPI.Apply(cube) + return table + + spawn = SimpleNamespace(func=spawn_table) + table_cfg = SimpleNamespace(spawn=spawn) + assert hasattr(gr1t2_env_cfg, "_wrap_packing_table_collision_for_newton") + gr1t2_env_cfg._wrap_packing_table_collision_for_newton(table_cfg) + wrapped_spawn = spawn.func + wrapped_spawn("/World/Table", spawn) + + source = stage.GetPrimAtPath("/World/Table/TableCollision") + meshes = [prim for prim in Usd.PrimRange(source) if prim.IsA(UsdGeom.Mesh)] + bin_cubes = [prim for prim in Usd.PrimRange(stage.GetPrimAtPath("/World/Table/Bin")) if prim.IsA(UsdGeom.Cube)] + assert not source.HasAPI(UsdPhysics.CollisionAPI) + assert not source.HasAPI(UsdPhysics.MeshCollisionAPI) + assert source.GetAttribute("horde:filterOwner").Get() == "table" + assert len(meshes) == 4 + assert all(prim.HasAPI(UsdPhysics.CollisionAPI) for prim in meshes) + assert all( + UsdPhysics.MeshCollisionAPI(prim).GetApproximationAttr().Get() == UsdPhysics.Tokens.boundingCube + for prim in meshes + ) + assert len(bin_cubes) == 5 + assert all(prim.HasAPI(UsdPhysics.CollisionAPI) for prim in bin_cubes) + assert not any( + prim.IsInstance() or prim.IsInstanceProxy() for prim in Usd.PrimRange(stage.GetPrimAtPath("/World/Table")) + ) + + gr1t2_env_cfg._wrap_packing_table_collision_for_newton(table_cfg) + assert spawn.func is wrapped_spawn + + +@pytest.mark.parametrize("config_type", [PickPlaceGR1T2EnvCfg, PickPlaceGR1T2WaistEnabledEnvCfg]) +def test_newton_wheel_wrapper_hulls_only_non_graspable_details(monkeypatch, config_type): + stage = Usd.Stage.CreateInMemory() + + def spawn_wheel(prim_path, _cfg): + root = stage.DefinePrim(prim_path, "Xform") + for name in ( + "sm_steeringwheel_a01_wheel_rim_01", + "sm_steeringwheel_a01_wheel_spokes_01", + "sm_steeringwheel_a01_hub_detail_01", + "sm_steeringwheel_a01_quick_release_detail_01", + ): + mesh = UsdGeom.Mesh.Define(stage, f"{prim_path}/{name}").GetPrim() + UsdPhysics.MeshCollisionAPI.Apply(mesh).CreateApproximationAttr().Set(UsdPhysics.Tokens.convexDecomposition) + return root + + cfg = config_type() + controlled_joints = list(cfg.actions.upper_body_ik.pink_controlled_joint_names) + if config_type is PickPlaceGR1T2WaistEnabledEnvCfg: + assert {"waist_yaw_joint", "waist_pitch_joint", "waist_roll_joint"} <= set(controlled_joints) + cfg.sim.physics = NewtonCfg() + spawn = cfg.scene.object.spawn + spawn.func = spawn_wheel + monkeypatch.setattr(gr1t2_env_cfg.sim_utils, "find_matching_prim_paths", lambda _path: ["/World/Wheel"]) + + cfg.stabilize_for_kitless_newton_mjwarp() + wrapped_spawn = spawn.func + wrapped_spawn("/World/Wheel", spawn) + + for name in ("sm_steeringwheel_a01_wheel_rim_01", "sm_steeringwheel_a01_wheel_spokes_01"): + assert stage.GetPrimAtPath(f"/World/Wheel/{name}").GetAttribute("physics:approximation").Get() == ( + UsdPhysics.Tokens.convexDecomposition + ) + for name in ("sm_steeringwheel_a01_hub_detail_01", "sm_steeringwheel_a01_quick_release_detail_01"): + assert stage.GetPrimAtPath(f"/World/Wheel/{name}").GetAttribute("physics:approximation").Get() == ( + UsdPhysics.Tokens.convexHull + ) + + gr1t2_env_cfg._condition_newton_steering_wheel_detail_collisions(cfg.scene.object) + assert spawn.func is wrapped_spawn + assert cfg.actions.upper_body_ik.pink_controlled_joint_names == controlled_joints + assert not hasattr(config_type().scene.object.spawn, "_kitless_newton_wheel_collision") + + +@pytest.mark.parametrize("config_type", [PickPlaceGR1T2EnvCfg, PickPlaceGR1T2WaistEnabledEnvCfg]) +@pytest.mark.parametrize("backend", ["newton_mjwarp", "ovphysx"]) +def test_kitless_kinematics_model_loads_without_the_kit_exporter(config_type, backend): + from isaaclab.controllers.pink_ik.pink_kinematics_configuration import PinkKinematicsConfiguration + + cfg = config_type() + assert cfg.actions.upper_body_ik.controller.urdf_path is None + controlled_joints = list(cfg.actions.upper_body_ik.pink_controlled_joint_names) + spawn_usd = cfg.scene.robot.spawn.usd_path + cfg.sim.physics = OvPhysxCfg() if backend == "ovphysx" else NewtonCfg() + getattr(cfg, f"stabilize_for_kitless_{backend}")() + action = cfg.actions.upper_body_ik + assert action.pink_controlled_joint_names == controlled_joints + assert cfg.scene.robot.spawn.usd_path == spawn_usd + assert action.controller.urdf_path is not None, "Kitless IK must not invoke Isaac Sim's USD-to-URDF exporter" + kinematics = PinkKinematicsConfiguration( + controlled_joint_names=action.pink_controlled_joint_names, + urdf_path=action.controller.urdf_path, + ) + assert set(kinematics.controlled_joint_names_pinocchio_order) == set(action.pink_controlled_joint_names) + for frame in ("left_hand_pitch_link", "right_hand_pitch_link"): + assert np.isfinite(kinematics.get_transform_frame_to_world(frame).homogeneous).all() + jacobian = kinematics.get_frame_jacobian(frame) + assert jacobian.shape == (6, len(action.pink_controlled_joint_names)) + assert np.isfinite(jacobian).all() + if config_type is PickPlaceGR1T2WaistEnabledEnvCfg: + waist_yaw = kinematics.controlled_joint_names_pinocchio_order.index("waist_yaw_joint") + # A real movable waist must rotate either hand about its vertical axis. + world_axis = kinematics.get_transform_frame_to_world(frame).rotation @ jacobian[3:, waist_yaw] + np.testing.assert_allclose(world_axis, [0.0, 0.0, 1.0], atol=1e-6) + + +def test_kitless_conditioning_is_opt_in_and_metadata_is_not_shared(): + first = GR1T2KitlessPinkInverseKinematicsActionCfg() + second = GR1T2KitlessPinkInverseKinematicsActionCfg() + + first.hand_joint_limits["joint"] = (-1.0, 1.0) + + assert not first.kitless_ovphysx_enabled + assert second.hand_joint_limits == {} + + +def test_ik_guard_is_per_environment_and_holds_invalid_rows(): + current = torch.tensor([[0.0, 0.1], [0.25, -0.25], [-0.1, 0.1]]) + targets = torch.tensor([[1.0, -1.0], [float("nan"), 0.0], [-0.15, 0.15]]) + + actual = bound_ik_joint_targets(targets, current, max_delta_rad=0.2, max_magnitude_rad=0.3) + + expected = torch.tensor([[0.2, -0.1], [0.25, -0.25], [-0.15, 0.15]]) + torch.testing.assert_close(actual, expected) + + +def test_hand_constraints_apply_mimic_equation_and_bounds_without_mutating_input(): + targets = torch.tensor([[0.4, 9.0, -0.9], [-0.3, 9.0, 0.8]]) + original = targets.clone() + # q_mimic = 2 * q_reference - 0.1; the first row then reaches its upper bound. + mimic_pairs = ((1, 0, 2.0, -0.1),) + lower = torch.tensor([-0.5, -0.6, -0.7]) + upper = torch.tensor([0.5, 0.6, 0.7]) + + actual = constrain_hand_joint_targets(targets, mimic_pairs, lower, upper) + + torch.testing.assert_close(actual, torch.tensor([[0.4, 0.6, -0.7], [-0.3, -0.6, 0.7]])) + torch.testing.assert_close(targets, original) + + +def test_hand_target_slew_clamps_discontinuity_against_previous_command(): + state = _HandTargetSlewState(torch.zeros((1, 2)), max_step_rad=0.079) + + torch.testing.assert_close(state.condition(torch.tensor([[0.2, -0.2]])), torch.tensor([[0.2, -0.2]])) + torch.testing.assert_close(state.condition(torch.tensor([[0.6, -0.5]])), torch.tensor([[0.279, -0.279]])) + + +def test_hand_target_slew_preserves_smooth_input_without_command_chasing(): + state = _HandTargetSlewState(torch.zeros((1, 2)), max_step_rad=0.08) + state.condition(torch.tensor([[0.2, -0.2]])) + + # The conditioner has no measured-state input: a smooth command passes exactly. + commanded = torch.tensor([[0.25, -0.24]]) + torch.testing.assert_close(state.condition(commanded), commanded) + + +def test_hand_target_slew_reset_clears_previous_command_history(): + state = _HandTargetSlewState(torch.zeros((2, 1)), max_step_rad=0.08) + state.condition(torch.tensor([[0.2], [-0.2]])) + state.condition(torch.tensor([[0.6], [-0.6]])) + + state.reset([0]) + + torch.testing.assert_close(state.condition(torch.tensor([[0.7], [-0.7]])), torch.tensor([[0.7], [-0.36]])) + + +def test_robot_conditioning_authors_collision_offsets_and_deinstances_scopes(): + stage = Usd.Stage.CreateInMemory() + + def spawn_robot(prim_path, _cfg): + robot = stage.DefinePrim(prim_path, "Xform") + collisions = stage.DefinePrim(f"{prim_path}/collisions", "Scope") + collisions.SetInstanceable(True) + collision = UsdGeom.Cube.Define(stage, f"{prim_path}/collision_shape").GetPrim() + UsdPhysics.CollisionAPI.Apply(collision) + return robot + + from isaaclab_physx.sim.schemas import PhysxRigidBodyCfg + + spawn = SimpleNamespace( + func=spawn_robot, + rigid_props=[PhysxRigidBodyCfg(max_depenetration_velocity=1.0)], + ) + robot_cfg = SimpleNamespace(spawn=spawn) + action_cfg = GR1T2KitlessPinkInverseKinematicsActionCfg() + + _condition_kitless_ovphysx_robot_spawn_properties(robot_cfg) + _condition_kitless_ovphysx_gr1t2_stage(robot_cfg, action_cfg) + spawn.func("/World/Robot", spawn) + + collisions = stage.GetPrimAtPath("/World/Robot/collisions") + collision = stage.GetPrimAtPath("/World/Robot/collision_shape") + schemas = collision.GetMetadata("apiSchemas") + applied = [str(schema) for schema in schemas.GetAddedOrExplicitItems()] + assert not collisions.IsInstanceable() + assert "PhysxCollisionAPI" in applied + assert collision.GetAttribute("physxCollision:contactOffset").Get() == pytest.approx(0.001) + assert collision.GetAttribute("physxCollision:restOffset").Get() == pytest.approx(0.0) + assert spawn.rigid_props[0].max_depenetration_velocity == pytest.approx(0.1) + + +def test_wheel_mesh_collision_geometry_is_not_replaced(): + stage = Usd.Stage.CreateInMemory() + original = {} + + def spawn_wheel(path, cfg): + root = stage.DefinePrim(path, "Xform") + body = stage.DefinePrim(path + "/Body", "Xform") + UsdPhysics.RigidBodyAPI.Apply(body) + for name in ("Rim", "Spokes", "Hub", "QuickRelease"): + mesh = UsdGeom.Mesh.Define(stage, path + "/Body/" + name) + mesh.CreatePointsAttr().Set([(0, 0, 0), (1, 0, 0), (0, 1, 0)]) + mesh.CreateFaceVertexCountsAttr().Set([3]) + mesh.CreateFaceVertexIndicesAttr().Set([0, 1, 2]) + UsdPhysics.CollisionAPI.Apply(mesh.GetPrim()) + UsdPhysics.MeshCollisionAPI.Apply(mesh.GetPrim()).CreateApproximationAttr().Set("convexDecomposition") + original["layer"] = stage.GetRootLayer().ExportToString() + return root + + cfg = PickPlaceGR1T2EnvCfg() + cfg.scene.object.spawn.func = spawn_wheel + cfg.stabilize_for_kitless_ovphysx() + root = cfg.scene.object.spawn.func("/World/Wheel", cfg.scene.object.spawn) + assert stage.GetRootLayer().ExportToString() == original["layer"] + colliders = [p for p in Usd.PrimRange(root) if p.HasAPI(UsdPhysics.CollisionAPI)] + assert len(colliders) == 4 + assert {p.GetName() for p in colliders} == {"Rim", "Spokes", "Hub", "QuickRelease"} + assert all(p.IsA(UsdGeom.Mesh) for p in colliders) + assert all(UsdPhysics.CollisionAPI(p).GetCollisionEnabledAttr().Get() for p in colliders) + assert all(UsdPhysics.MeshCollisionAPI(p).GetApproximationAttr().Get() == "convexDecomposition" for p in colliders) + assert not stage.GetPrimAtPath("/World/Wheel/Body/KitlessPrimitiveCollision") diff --git a/source/isaaclab_tasks/test/contrib/test_so101_teleop_pipeline.py b/source/isaaclab_tasks/test/contrib/test_so101_teleop_pipeline.py index 88e52e338689..9a3b5c7bf0ac 100644 --- a/source/isaaclab_tasks/test/contrib/test_so101_teleop_pipeline.py +++ b/source/isaaclab_tasks/test/contrib/test_so101_teleop_pipeline.py @@ -3,61 +3,98 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Sim-free tests for the SO-101 XR teleop pipeline wiring. +"""Exercise the real SO-101 Capture graph after hardware controller conversion, without XR/GPU.""" -These tests build the Isaac Capture pipeline standalone (no ``gym.make``, USD, GPU, or XR -device) and verify the flattened action width and element order. - -The retargeter math itself (trigger -> closedness, clutch rebasing, orientation calibration) is -unit tested in Isaac Capture (``test_so101_retargeters.py``); this file guards only the Lab-side -wiring: - -- The pose retargeter emits a fixed 7-element output; the flattened action passes the full pose - through and concatenates the gripper channel into the 8D action - ``[pos_x, pos_y, pos_z, quat_x, quat_y, quat_z, quat_w, gripper]``. -- Each entry in ``output_order`` must resolve to a declared element name; an undeclared - name resolves silently to a constant 0.0 instead of raising. -""" +import inspect +import numpy as np import pytest +from scipy.spatial.transform import Rotation pytest.importorskip("isaacteleop") -from isaacteleop.retargeters.SO101.gripper_retargeter import GRIPPER_ELEMENT_LABEL - -from isaaclab_tasks.contrib.stack.config.so101.stack_ik_abs_env_cfg import _build_so101_stack_pipeline - +from isaacteleop.retargeters import SO101ClutchRetargeter +from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource +from isaacteleop.retargeting_engine.interface import ( + ComputeContext, + ExecutionEvents, + ExecutionState, + OptionalTensorGroup, + TensorGroup, +) +from isaacteleop.retargeting_engine.interface.retargeter_core_types import ExecutionCache, GraphTime +from isaacteleop.retargeting_engine.tensor_types import ControllerInput, ControllerInputIndex, TransformMatrix -def test_so101_pipeline_action_is_8d(): - """The SO-101 pipeline flattens to an 8D action ``[pos_xyz, quat_xyzw, gripper]``.""" - combiner = _build_so101_stack_pipeline() +from isaaclab_tasks.contrib.stack.config.so101.stack_ik_abs_env_cfg import ( + _BASE_T_GRIPPER_HOME, + _build_so101_stack_pipeline, +) - action_type = combiner.output_types()["action"].types[0] - assert action_type.shape == (8,), f"expected an 8D action, got shape {action_type.shape}" +def test_so101_pipeline_full_pose_clutch_and_action_order(): + """Home, motion, re-clutch and reset survive the real transform/retarget/flatten graph.""" + legacy = "orientation_offset" in inspect.signature(SO101ClutchRetargeter).parameters + calibration = Rotation.from_euler("XYZ", [-90, 0, 60], degrees=True) + pipeline = _build_so101_stack_pipeline() + controllers = next(node for node in pipeline.get_leaf_nodes() if isinstance(node, ControllersSource)) + anchor = TensorGroup(TransformMatrix()) + anchor[0] = np.eye(4, dtype=np.float32) -def test_so101_pipeline_output_order(): - """The action elements resolve to the declared labels in order (not silent 0.0 fillers).""" - combiner = _build_so101_stack_pipeline() + def step(position, rotation, *, running=True, trigger=0.0, reset=False): + controller = TensorGroup(ControllerInput()) + # Populate the same complete typed input as ControllersSource, replacing hardware polling + # only. Cached source output still traverses the real frame transform and all retargeters. + for index in ControllerInputIndex: + if index in (ControllerInputIndex.GRIP_POSITION, ControllerInputIndex.AIM_POSITION): + controller[index] = np.asarray(position, dtype=np.float32) + elif index in (ControllerInputIndex.GRIP_ORIENTATION, ControllerInputIndex.AIM_ORIENTATION): + controller[index] = rotation.as_quat().astype(np.float32) + elif index in (ControllerInputIndex.GRIP_IS_VALID, ControllerInputIndex.AIM_IS_VALID): + controller[index] = True + else: + controller[index] = 0.0 + controller[ControllerInputIndex.SQUEEZE_VALUE] = float(running) + controller[ControllerInputIndex.TRIGGER_VALUE] = trigger + context = ComputeContext( + graph_time=GraphTime(sim_time_ns=0, real_time_ns=0), + execution_events=ExecutionEvents( + reset=reset, execution_state=ExecutionState.RUNNING if running else ExecutionState.STOPPED + ), + ) + cache = ExecutionCache({"world_T_anchor": {"value": anchor}}, context) + cache.cache( + id(controllers), + {ControllersSource.LEFT: OptionalTensorGroup(ControllerInput()), ControllersSource.RIGHT: controller}, + ) + action = np.from_dlpack(pipeline.execute_pipeline_with_cache(cache)["action"][0]).copy() + assert action.shape == (8,) + assert np.isfinite(action).all() + return action - # Walk the OutputCombiner -> subgraph -> TensorReorderer to read the resolved output order. - # This depends on isaacteleop internals (no public accessor); fail rather than skip if they change, - # since this is the only check of the element order. - reorderer = combiner.output_mapping["action"].module._target_module - output_order = reorderer._output_order + def check(action, position, rotation, gripper): + np.testing.assert_allclose(action[:3], position, atol=1e-6) + np.testing.assert_allclose(Rotation.from_quat(action[3:7]).as_matrix(), rotation.as_matrix(), atol=1e-6) + assert action[7] == pytest.approx(gripper) - expected_order = [ - "pos_x", - "pos_y", - "pos_z", - "quat_x", - "quat_y", - "quat_z", - "quat_w", - GRIPPER_ELEMENT_LABEL, - ] - assert output_order == expected_order, ( - f"unexpected output order {output_order}; the pose/gripper elements must resolve to the" - f" declared pos/quat/{GRIPPER_ELEMENT_LABEL!r} labels so they do not become silent 0.0" - " fillers" - ) + home_position = _BASE_T_GRIPPER_HOME[:3, 3] + home_rotation = Rotation.from_matrix(_BASE_T_GRIPPER_HOME[:3, :3]) + origin = Rotation.from_euler("xyz", [0.2, -0.4, 0.3]) + position = np.array([0.3, -0.1, 0.5]) + reset_rotation = Rotation.identity() if legacy else home_rotation + engaged_rotation = origin * calibration if legacy else home_rotation + check(step(position, origin, running=False, reset=True), home_position, reset_rotation, 0.0) + check(step(position, origin), home_position, engaged_rotation, 0.0) + delta = Rotation.from_rotvec([0.0, 0.0, 0.3]) + displacement = np.array([0.02, -0.03, 0.01]) + moved_position = home_position + displacement + moved_rotation = delta * engaged_rotation + check(step(position + displacement, delta * origin, trigger=1.0), moved_position, moved_rotation, 1.0) + other = Rotation.from_euler("xyz", [-0.5, 0.2, 0.8]) + check(step(-position, other, running=False), moved_position, moved_rotation, 0.0) + resumed_rotation = other * calibration if legacy else moved_rotation + check(step(-position, other), moved_position, resumed_rotation, 0.0) + # Modern Capture composes from the held full pose; 1.4 preserves absolute grip calibration. + delta2 = Rotation.from_rotvec([0.2, 0.0, 0.0]) + check(step(-position + displacement, delta2 * other), moved_position + displacement, delta2 * resumed_rotation, 0.0) + check(step(position, origin, running=False, reset=True), home_position, reset_rotation, 0.0) + check(step(-position, other), home_position, other * calibration if legacy else home_rotation, 0.0) diff --git a/source/isaaclab_tasks/test/core/test_preset_cli.py b/source/isaaclab_tasks/test/core/test_preset_cli.py index e7e9f9e6e78e..76dc3ae8fc37 100644 --- a/source/isaaclab_tasks/test/core/test_preset_cli.py +++ b/source/isaaclab_tasks/test/core/test_preset_cli.py @@ -269,3 +269,41 @@ def load(task, entry): for phrase in expected_phrases: assert phrase in flat, f"Missing phrase: {phrase!r}" + + +@pytest.mark.parametrize( + "task_name", + [ + "IsaacContrib-PickPlace-GR1T2-Abs", + "IsaacContrib-PickPlace-Locomanipulation-G1-Abs", + "IsaacContrib-PickPlace-FixedBaseUpperBodyIK-G1-Abs", + "IsaacContrib-Stack-Cube-SO101-IK-Abs-v0", + ], +) +@pytest.mark.parametrize("physics", [None, "isaacsim_physx", "ovphysx", "newton_mjwarp"]) +def test_teleop_tasks_resolve_public_physics_selectors(task_name, physics): + """Resolve registered tasks through the public flag parser before backend preparation.""" + from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg + from isaaclab_ov.physics import OvPhysxCfg + from isaaclab_physx.physics import PhysxCfg + + import isaaclab_tasks # noqa: F401 -- registers the public task IDs + from isaaclab_tasks.utils import parse_env_cfg + from isaaclab_tasks.utils.hydra import setup_preset_cli + + arguments = ["--task", task_name] + if physics is not None: + arguments.extend(["--physics", physics]) + _, selections = setup_preset_cli(_make_parser(), arguments) + cfg = parse_env_cfg(task_name, device="cpu", num_envs=1, overrides=selections) + expected_type = {None: PhysxCfg, "isaacsim_physx": PhysxCfg, "ovphysx": OvPhysxCfg, "newton_mjwarp": NewtonCfg} + assert isinstance(cfg.sim.physics, expected_type[physics]) + assert cfg.scene.num_envs == 1 and cfg.sim.device == "cpu" + if "SO101" in task_name: + assert cfg.scene.robot.spawn.variants["Physics"] == ("physics" if physics == "newton_mjwarp" else "physx") + if physics == "newton_mjwarp": + assert isinstance(cfg.sim.physics.solver_cfg, MJWarpSolverCfg) + if "SO101" in task_name: + assert cfg.sim.physics.solver_cfg.iterations == 100 + assert cfg.sim.physics.solver_cfg.impratio == 10.0 + assert cfg.sim.physics.solver_cfg.nconmax == 600 diff --git a/source/isaaclab_tasks/test/core/test_xr_camera_feed_presets.py b/source/isaaclab_tasks/test/core/test_xr_camera_feed_presets.py index 67630cca26c5..69064d31d635 100644 --- a/source/isaaclab_tasks/test/core/test_xr_camera_feed_presets.py +++ b/source/isaaclab_tasks/test/core/test_xr_camera_feed_presets.py @@ -3,15 +3,20 @@ # # SPDX-License-Identifier: BSD-3-Clause +import re + +import numpy as np import pytest from isaaclab_ov.renderers import OVRTXRendererCfg from isaaclab_physx.renderers import IsaacRtxRendererCfg +from isaaclab_teleop import xr_anchor_manager from isaaclab_tasks.contrib.locomanip_pick_place.fixed_base_upper_body_ik_g1_env_cfg import ( FixedBaseUpperBodyIKG1EnvCfg, ) from isaaclab_tasks.contrib.locomanip_pick_place.locomanipulation_g1_env_cfg import LocomanipulationG1EnvCfg from isaaclab_tasks.contrib.pick_place.pickplace_gr1t2_env_cfg import PickPlaceGR1T2EnvCfg +from isaaclab_tasks.contrib.pick_place.pickplace_gr1t2_waist_enabled_env_cfg import PickPlaceGR1T2WaistEnabledEnvCfg from isaaclab_tasks.utils.hydra import resolve_presets @@ -48,18 +53,27 @@ def test_g1_xr_camera_uses_calibration_and_head_locked_panel(env_cfg_type): assert feed_cfg.distance_m == 0.8 assert feed_cfg.enable_dlss_ray_reconstruction is True assert feed_cfg.dlss_exec_mode == "quality" - assert feed_cfg.max_update_hz == 0.0 + assert feed_cfg.max_update_hz == (20.0 if env_cfg_type is FixedBaseUpperBodyIKG1EnvCfg else 0.0) assert camera_cfg.renderer_cfg.default == IsaacRtxRendererCfg() assert camera_cfg.renderer_cfg.isaacsim_rtx == IsaacRtxRendererCfg() def test_fixed_base_g1_pip_preserves_policy_observations_and_fixed_root(): - """PiP adds a sensor without changing the fixed-base policy observation schema.""" + """Retain the existing three-camera observation contract and fixed-base root.""" cfg = FixedBaseUpperBodyIKG1EnvCfg() assert hasattr(cfg.scene, "robot_pov_cam") - assert not hasattr(cfg.observations.policy, "robot_pov_cam") + for name in ("robot_pov_cam", "left_wrist_cam", "right_wrist_cam"): + assert hasattr(cfg.observations.policy, name) assert cfg.scene.robot.spawn.fix_root_link is True + assert [feed.camera_name for feed in cfg.isaac_teleop.xr_camera_feeds] == [ + "robot_pov_cam", + "left_wrist_cam", + "right_wrist_cam", + ] + assert [feed.max_update_hz for feed in cfg.isaac_teleop.xr_camera_feeds] == [20.0, 10.0, 10.0] + assert cfg.xr.hidden_visual_prims == [] + assert cfg.xr.excluded_visual_meshes def test_locomanipulation_g1_retains_recorded_camera(): @@ -71,6 +85,30 @@ def test_locomanipulation_g1_retains_recorded_camera(): assert cfg.image_obs_list == ["robot_pov_cam"] +@pytest.mark.parametrize("env_cfg_type", [PickPlaceGR1T2EnvCfg, PickPlaceGR1T2WaistEnabledEnvCfg]) +def test_gr1t2_xr_anchor_preserves_operator_view_and_head_partition(monkeypatch, env_cfg_type): + """The operator faces the table; head exclusion remains camera-selective.""" + cfg = env_cfg_type() + assert cfg.xr.anchor_pos == (0.0, 0.0, 0.0) + assert cfg.xr.render_anchor_pos is None + assert cfg.xr.render_anchor_rot is None + assert not cfg.xr.hidden_visual_prims + assert any( + re.fullmatch(pattern, "/World/envs/env_0/Robot/head_yaw_link/visuals/head_yaw_link/mesh") + for pattern in cfg.xr.excluded_visual_meshes + ) + monkeypatch.setattr(xr_anchor_manager, "XRCore", None) + monkeypatch.setattr(xr_anchor_manager, "_xr_anchor_prim_exists", lambda _path: True) + anchor = xr_anchor_manager.XrAnchorManager(cfg.xr) + assert anchor.render_anchor_path == anchor.anchor_headset_path + transform = anchor.get_world_matrix() + eye = transform @ np.array([0.0, 1.6, 0.0, 1.0]) + forward = transform @ np.array([0.0, 0.0, -1.0, 0.0]) + np.testing.assert_allclose(eye, (0.0, 0.0, 1.6, 1.0)) + np.testing.assert_allclose(forward, (0.0, 1.0, 0.0, 0.0)) + assert np.dot(np.asarray(cfg.scene.packing_table.init_state.pos) - eye[:3], forward[:3]) > 0.0 + + def test_xr_camera_reference_renderer_resolves_for_supported_backends(): """Reference cameras retain Isaac RTX defaults and OVRTX compatibility.""" camera_renderer_cfg = PickPlaceGR1T2EnvCfg().scene.robot_pov_cam.renderer_cfg diff --git a/source/isaaclab_teleop/changelog.d/humanoid-preparation-coverage.skip b/source/isaaclab_teleop/changelog.d/humanoid-preparation-coverage.skip new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_teleop/changelog.d/kitless-ovxr.added.rst b/source/isaaclab_teleop/changelog.d/kitless-ovxr.added.rst new file mode 100644 index 000000000000..134a958ac8cb --- /dev/null +++ b/source/isaaclab_teleop/changelog.d/kitless-ovxr.added.rst @@ -0,0 +1 @@ +* Added kitless OVXR teleoperation and MCAP replay with independently paced rendering, backend-derived anchors, and ordered shutdown. diff --git a/source/isaaclab_teleop/changelog.d/kitless-ovxr.minor b/source/isaaclab_teleop/changelog.d/kitless-ovxr.minor new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_teleop/changelog.d/ovxr-outputs.added.rst b/source/isaaclab_teleop/changelog.d/ovxr-outputs.added.rst new file mode 100644 index 000000000000..023b61933726 --- /dev/null +++ b/source/isaaclab_teleop/changelog.d/ovxr-outputs.added.rst @@ -0,0 +1 @@ +* Added optional OVXR camera panels and an injected desktop mirror using completed render products. diff --git a/source/isaaclab_teleop/changelog.d/ovxr-outputs.minor b/source/isaaclab_teleop/changelog.d/ovxr-outputs.minor new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_teleop/changelog.d/ovxr-renderer-transfer.skip b/source/isaaclab_teleop/changelog.d/ovxr-renderer-transfer.skip new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_teleop/changelog.d/premerge-workspace-pin-guide.skip b/source/isaaclab_teleop/changelog.d/premerge-workspace-pin-guide.skip new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_teleop/changelog.d/schema-install-and-test-ownership.skip b/source/isaaclab_teleop/changelog.d/schema-install-and-test-ownership.skip new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_teleop/changelog.d/teamfood-install-guide.skip b/source/isaaclab_teleop/changelog.d/teamfood-install-guide.skip new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_teleop/docs/OVXR_RUNTIME.md b/source/isaaclab_teleop/docs/OVXR_RUNTIME.md new file mode 100644 index 000000000000..e6eda94fb7c9 --- /dev/null +++ b/source/isaaclab_teleop/docs/OVXR_RUNTIME.md @@ -0,0 +1,202 @@ +# Kitless XR teleoperation + +OVXR presents Isaac Lab's OVRTX renderer and OVStage scene while Isaac Capture +provides tracking and retargeting. This workflow supports Linux x86-64 with +Newton or OVPhysX physics and one environment. + +## Installation + +Use Isaac Capture and Isaac Lab checkouts containing the OVXR integration. +Build Isaac Capture with Python 3.12, CMake 3.24 or newer, a C++20 compiler +and `patchelf` available on `PATH`, using its +[build guide](https://github.com/NVIDIA/IsaacCapture/blob/main/docs/source/development/build_from_source/index.rst). +For camera panels, enable `BUILD_OVXR_CAMERA_QUADS=ON`; SDK discovery and +optional build-managed acquisition are described in the +[OVXR guide](https://github.com/NVIDIA/IsaacCapture/blob/main/docs/source/getting_started/ovxr.rst). +Review the licenses supplied with the selected packages. + +From Isaac Lab, select the built wheel in a local project extra: + +```bash +CAPTURE_WHEEL=/absolute/path/to/isaaccapture-1.7+local-cp312-cp312-linux_x86_64.whl +uv add --optional ovxr --frozen \ + "isaaccapture[ovxr,retargeters,ui,cloudxr] @ file://${CAPTURE_WHEEL} ; platform_system == 'Linux' and platform_machine == 'x86_64'" +uv add --optional ovxr --frozen \ + "isaaclab-dev[ov] ; platform_system == 'Linux' and platform_machine == 'x86_64'" \ + "isaaclab-teleop ; platform_system == 'Linux' and platform_machine == 'x86_64'" \ + "isaaclab-mimic ; platform_system == 'Linux' and platform_machine == 'x86_64'" \ + "ipywidgets>=8.1.5 ; platform_system == 'Linux' and platform_machine == 'x86_64'" \ + "dex-retargeting==0.5.0 ; platform_system == 'Linux' and platform_machine == 'x86_64'" \ + "physx-usd-schemas==25.11.2.72696984 ; platform_system == 'Linux' and platform_machine == 'x86_64'" +``` + +Add `extra = "ovxr"` to the generated `isaaccapture` entry in +`tool.uv.sources`, preserving its wheel path and platform marker. In the same +table, map the optional external PhysX schemas to the existing public PyPI index: + +```toml +physx-usd-schemas = { index = "pypi-public", extra = "ovxr" } +``` + +OvPhysX 0.7 loads these schemas at runtime but does not declare the package as +a dependency, so the optional requirement above is explicit. +Add the following pair to `tool.uv.conflicts`, preserving existing entries: + +```toml +[tool.uv] +conflicts = [ + [{ extra = "ovxr" }, { extra = "teleop" }], +] +``` + +The general `teleop` extra selects an older Isaac Capture package. Use the local +`ovxr` extra for this workflow. Keep local wheel paths and lock changes out of +commits. To install the optional desktop mirror, include `desktop-mirror` among +the extras on the same wheel requirement. + +Until the coordinated OV provider upgrade lands, a local premerge selection +also needs the following entries in the existing `tool.uv.override-dependencies` +list, preserving its other entries. These overrides and the workspace edit +below are temporary consuming-checkout settings. Do not commit them; the default +provider pins in these PRs remain unchanged pending the coordinated upgrade: + +```toml +"ovphysx==0.7.0.74035321", +"ovrtx==0.6.0.1791947691", +"ovstage==0.3.0.1791986420", +``` + +Before locking or synchronizing, also change `ovstage==0.2.1.385922` to +`ovstage==0.3.0.1791986420` in `[project].dependencies` in your local +`source/isaaclab_ov/pyproject.toml`. A root override controls resolution but does +not rewrite the workspace package's installed `Requires-Dist`. Refresh the +editable installation with the scoped reinstall command below so `uv pip check` +sees the matching declaration. Keep this local dependency edit out of commits. + +The project selects NumPy `>=2.3,<2.4`, the compatible range for the selected +Numba and cmeel-boost dependencies. Keep this range when adding local overrides. + +Resolve the selected environment and check its dependencies: + +```bash +uv lock --refresh-package isaaccapture --upgrade-package isaaccapture --upgrade-package packaging==23.2 +uv sync --locked --extra ovxr --reinstall-package isaaclab-ov +uv run --extra ovxr python -c 'import importlib.metadata as m; print(m.version("isaaccapture"), m.version("ovxr"))' +uv pip check +``` + +OVPhysX declares `packaging<24`; the explicit selection above satisfies its +metadata in this non-Kit environment. The project deliberately widens this +requirement to allow Isaac Sim's packaging version too; leave that upstream +override unchanged. A metadata mismatch alone does not establish a runtime +failure. Kit uses a different environment or resolution. +Repeat this selection if a later lock upgrade replaces it. Isaac Capture's +`ovxr` extra selects OVXR and its compatible OVRTX/OVStage dependencies; see its +OVXR guide for application version overrides. + +Stop applications using the environment before replacing a wheel. After a +rebuild, refresh its lock identity before synchronizing. + +## Live teleoperation + +```bash +uv run --extra ovxr isaaclab teleop run \ + --task IsaacContrib-PickPlace-GR1T2-Abs --num_envs 1 \ + --device cuda:0 --physics newton_mjwarp --ovxr +``` + +The same registered-task selectors apply to live input and MCAP replay: + +| Task | Physics selectors | +| --- | --- | +| `IsaacContrib-PickPlace-GR1T2-Abs` | `ovphysx`, `newton_mjwarp`, `isaacsim_physx` | +| `IsaacContrib-PickPlace-Locomanipulation-G1-Abs` | `ovphysx`, `newton_mjwarp`, `isaacsim_physx` | +| `IsaacContrib-PickPlace-FixedBaseUpperBodyIK-G1-Abs` | `ovphysx`, `newton_mjwarp`, `isaacsim_physx` | +| `IsaacContrib-Stack-Cube-SO101-IK-Abs-v0` | `ovphysx`, `newton_mjwarp`, `isaacsim_physx` | + +Omitting `--physics` retains the Kit/Isaac Sim default. Use `--physics ovphysx +--device cpu` for standalone CPU physics, or `--physics newton_mjwarp --device +cuda:0` for Newton; these standalone selections support `--ovxr`. Kit's +`isaacsim_physx` selection uses its separate `--xr` workflow and Isaac Sim +installation, not `--ovxr`. The task presets retain backend-specific solver +settings and, for SO101, choose the matching USD Physics variant. + +For example, replace the task in this command with any row above: + +```bash +uv run --python 3.12 --extra ovxr --locked isaaclab teleop run \ + --task IsaacContrib-PickPlace-GR1T2-Abs --num_envs 1 \ + --device cpu --physics ovphysx --ovxr --cloudxr_env cloudxrjs +``` + + `--ovxr` selects OVStage +and starts CloudXR with the `cloudxrjs` profile by default. Connect a supported +CloudXR client, then use Play, Pause and Reset as in Kit-based teleoperation. +`--ovxr` and Kit's `--xr` select separate XR runtimes and are mutually exclusive. +An explicit `ISAAC_LAB_OVRTX_USE_OVSTAGE=0` is incompatible with OVXR. + +No local window opens by default. Headset rendering still uses the GPU and works +with `DISPLAY` unset. The teleoperation CLI does not need a `--headless` flag. +`--ovxr_worker_timeout` bounds renderer startup; it does not bound waiting for a +client to establish its OpenXR session. Building a Capture wheel and resolving +Python packages do not warm application-specific RTX shaders or Newton/Warp +physics kernels. First runtime preparation and the first physics step can take +longer than subsequent runs. Retain startup logs and distinguish stage/render +preparation from client connection and active simulation; do not treat a warm +retry as proof that an earlier timeout was fixed. + +Use `--step_hz 20` to cap live control at 20 updates per second while XR rendering +runs independently. Time spent reading input, stepping physics and publishing +the scene counts toward that interval. Slow steps do not cause catch-up steps. +Omit the option or use zero for unthrottled control. Paused polling uses the +environment action interval when no explicit rate is set. This option requires +`--ovxr` and does not change physics dt or decimation. + +## MCAP replay + +```bash +uv run --extra ovxr python scripts/environments/teleoperation/teleop_replay_agent.py \ + --task IsaacContrib-PickPlace-GR1T2-Abs --num_envs 1 \ + --device cpu --physics ovphysx --ovxr \ + --replay_file /absolute/path/to/session.mcap +``` + +MCAP supplies tracker and control input; the connected headset supplies the +view. A recorded STOP, task success/failure, or `--max_replay_duration_s` ends +the run. Reset does not rewind recorded input. `--max_stage_load_wait_s` bounds +renderer startup separately; the replay duration starts after warmup and +includes any lazy first-step physics initialization. Deadlines are checked +between steps. Application render-call timing statistics are unavailable when +OVXR owns rendering independently. + +HDF5 demonstration recording and replay continue to use the Kit/Isaac Sim path. + +## Camera panels and desktop mirror + +Select up to three task cameras and a head-locked layout: + +```bash +uv run --extra ovxr isaaclab teleop run --ovxr \ + --task IsaacContrib-PickPlace-FixedBaseUpperBodyIK-G1-Abs \ + --num_envs 1 --device cpu --physics ovphysx \ + --ovxr_pip_camera robot_pov_cam left_wrist_cam right_wrist_cam \ + --ovxr_pip_head_locked +``` + +`--ovxr_pip_fps` limits panel updates. `--ovxr_pip_view_position X Y Z` overrides +one panel's position; multiple panels use the task layout. Panel cameras must be +enabled in the task's `xr_camera_feeds`. World and viewer-start layouts are not +supported by the OVXR presenter. The session snapshots its camera selection at +preparation; create a new session to change it. Panels use the same renderer as +the headset and do not require a desktop display. + +Add `--ovxr_desktop_mirror` to view completed eye or camera images locally. +`--ovxr_desktop_mirror_fps` limits its update rate. The mirror requires the +`desktop-mirror` extra and a usable display. It opens after the first submitted +XR image; closing it requests application shutdown. Eye images exclude the +separate OpenXR camera panels; select a camera product to inspect that feed. + +The mirror defaults `OMNIUI_BACKEND` to `opengl` before OVUI initialization and +preserves an explicit application setting. The setting persists after shutdown; +an already initialized OVUI backend cannot be reconfigured. The Vulkan backend +is unsupported by the mirror copy path. diff --git a/source/isaaclab_teleop/docs/README.md b/source/isaaclab_teleop/docs/README.md index 266f5470dda9..c88e71e0d356 100644 --- a/source/isaaclab_teleop/docs/README.md +++ b/source/isaaclab_teleop/docs/README.md @@ -19,8 +19,9 @@ XR anchor synchronization, retargeting pipelines, and action-tensor generation. modes for controlling how the anchor orientation tracks the reference prim. - **Retargeting tuning UI** -- optional ImGui window for real-time adjustment of retargeter parameters when `retargeters_to_tune` is provided. -- **XR camera feedback** -- selected scene cameras can be shown through Kit Scene UI using a - feed-owned CUDA view of their existing RTX render product, with camera-buffer fallback. +- **XR camera feedback** -- selected scene cameras use Kit Scene UI or the optional + [OVXR path](OVXR_RUNTIME.md)'s OpenXR quad layers. The latter also + supports a passive desktop eye-image mirror; separate quad layers are not in that image. ## Architecture @@ -72,7 +73,8 @@ class MyEnvCfg(ManagerBasedRLEnvCfg): Create a function that builds your Isaac Capture retargeting pipeline. The builder should return an `OutputCombiner` with an `"action"` key containing the flattened action tensor (typically via -`TensorReorderer`). Optionally return a list of retargeters to expose in the tuning UI: +`TensorReorderer`). A setup factory may also return retargeters for the tuning UI; +unpack those before assigning the two configuration callbacks: ```python from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource @@ -98,8 +100,11 @@ uv run python scripts/environments/teleoperation/teleop_se3_agent.py \ --task My-Isaac-Capture-Env-v0 --xr ``` +For kitless teleoperation, installation, MCAP replay, camera panels and the +optional desktop mirror, see [Kitless XR teleoperation](OVXR_RUNTIME.md). + See [XR Camera Feedback](../../../docs/source/features/isaac_teleop.rst#xr-camera-feedback) for -reference tasks, camera selection, layout, placement, renderer, disable, and kitless behavior. +reference tasks, camera selection, layout, placement, renderer, disable, and OVXR behavior. ### 4. Programmatic Usage @@ -108,7 +113,11 @@ reference tasks, camera selection, layout, placement, renderer, disable, and kit ```python from isaaclab_teleop import IsaacTeleopCfg, IsaacTeleopDevice -cfg = IsaacTeleopCfg(pipeline_builder=my_pipeline_builder) +pipeline, retargeters = my_pipeline_builder() +cfg = IsaacTeleopCfg( + pipeline_builder=lambda: pipeline, + retargeters_to_tune=lambda: retargeters, +) with IsaacTeleopDevice(cfg) as device: device.add_callback("RESET", env.reset) @@ -136,7 +145,10 @@ rendering without blocking. | `sim_device` | `str` | `"cuda:0"` | Torch device for output action tensors | | `retargeting_execution` | `RetargetingExecutionConfig \| None` | `None` (resolved at session start to `mode="pipelined", pacing=DeadlinePacingConfig(safety_margin_s=0.025)`) | Isaac Capture retargeting execution settings; deferred so the config imports without `isaacteleop` | | `teleoperation_active_default` | `bool` | `False` | Whether teleoperation is active on session start | -| `app_name` | `str` | `"IsaacLabTeleop"` | Application name for the Isaac Capture session | +| `require_initial_bimanual_hand_tracking` | `bool` | `False` | Wait for initial hand presence before stepping | +| `action_conditioner` | `Callable \| None` | `None` | Task-owned stateful pre-step action conditioning | +| `action_validator` | `Callable \| None` | `None` | Task-owned fail-closed action validation | +| `app_name` | `str` | `"IsaacLabTeleop"` | Application name for the IsaacTeleop session | The 25 ms `DeadlinePacingConfig` safety margin staggers Isaac Capture's Python work behind Isaac Lab's step Python, giving native work such as rendering time to overlap instead of having both Python stacks @@ -148,13 +160,34 @@ contend for the GIL at the start of the step. |---|---|---|---| | `anchor_pos` | `tuple[float, float, float]` | `(0, 0, 0)` | XR anchor position in world frame | | `anchor_rot` | `tuple[float, float, float, float]` | `(0, 0, 0, 1)` | XR anchor rotation (quaternion xyzw) | -| `anchor_prim_path` | `str \| None` | `None` | Prim to attach anchor to for dynamic positioning | +| `render_anchor_pos` | `tuple[float, float, float] \| None` | `None` | Optional render-only anchor position | +| `render_anchor_rot` | `tuple[float, float, float, float] \| None` | `None` | Optional render-only anchor rotation | +| `hidden_visual_prims` | `list[str]` | `[]` | Prim-path regexes hidden before OVXR session materialization | +| `excluded_visual_meshes` | `list[str]` | `[]` | Visual-mesh regexes assigned to a separate render-export partition, without changing source USD visibility | +| `anchor_prim_path` | `str \| None` | `None` | Reference prim for dynamic positioning; OVXR reads committed physics poses and shares the same anchor policy | | `anchor_rotation_mode` | `XrAnchorRotationMode` | `FIXED` | How anchor rotation tracks the reference prim | | `anchor_rotation_smoothing_time` | `float` | `1.0` | Slerp time constant (seconds) for `FOLLOW_PRIM_SMOOTHED` mode | | `anchor_rotation_custom_func` | `Callable` | identity | Custom rotation function for `CUSTOM` mode | | `near_plane` | `float` | `0.15` | Near clipping plane distance for the XR device | | `fixed_anchor_height` | `bool` | `True` | Fix anchor height to initial value of the reference prim | +For single-environment OVXR, prefer `excluded_visual_meshes` for a robot +head surrounding the eyes. Match only visual mesh paths, not head links or +collision geometry. Environment cameras select `env_0`; excluded meshes use +`xr_excluded`, with `showAllPartitionsByDefault=false`. This also excludes the +head from PiP cameras selecting `env_0`; other camera views require an explicit +partition policy. The source Stage's visibility and instancing are restored after +render export. GR1T2 and fixed-base G1 use this policy instead of global hiding. + +## Capture and replay boundaries + +MCAP records DeviceIO tracker/control input, not historical simulation scenes. +Environment/control reset does not rewind MCAP input. HDF5 +demonstration scripts use the Kit/Isaac Sim application path; these OVXR +commands do not qualify HDF5 recording/replay. PiP and mirror consume latest +completed images rather than a history of simulation scenes; they are not lossless +recorders or rolling-shutter playback. + ## Utilities - **`remove_camera_configs(env_cfg)`** -- strips camera sensors and their associated policy @@ -180,5 +213,8 @@ run automatically. ## Dependencies - **`isaaclab`** -- core Isaac Lab framework -- **`isaacteleop`** -- Isaac Capture retargeting engine, device I/O, and session management -- **`isaacsim`** -- Isaac Sim runtime (provides the Kit XR bridge for OpenXR handle acquisition) +- **`isaaclab_ov`** -- required by the optional OVXR session adapter; the standalone `isaaclab_teleop` package does not install this backend automatically +- **`isaacteleop`** -- IsaacTeleop retargeting engine, device I/O, and session management +- **`isaacsim`** -- required for the Kit XR bridge, not for the OVXR session path +- **OVXR / OVRTX / OVStage** -- matched native runtime and stock peers for OVXR +- **OVUI** -- optional desktop-mirror extra; headset-only operation does not import it diff --git a/source/isaaclab_teleop/isaaclab_teleop/__init__.pyi b/source/isaaclab_teleop/isaaclab_teleop/__init__.pyi index eb5c4f82838b..56fc24de33ca 100644 --- a/source/isaaclab_teleop/isaaclab_teleop/__init__.pyi +++ b/source/isaaclab_teleop/isaaclab_teleop/__init__.pyi @@ -15,6 +15,8 @@ __all__ = [ "HapticFeedbackReceiver", "IsaacTeleopCfg", "IsaacTeleopDevice", + "OvLibrariesXrConfig", + "OvLibrariesXrSession", "SupportsControlEvents", "SystemCheckItem", "SystemCheckResult", @@ -52,6 +54,7 @@ from .isaac_teleop_cfg import ( XrCameraFeedLayoutCfg, ) from .isaac_teleop_device import IsaacTeleopDevice, create_isaac_teleop_device +from .ov_libraries_xr import OvLibrariesXrConfig, OvLibrariesXrSession from .session_lifecycle import cloudxr_eula_accepted from .system_check import SystemCheckItem, SystemCheckResult, check_system_requirements from .xr_anchor_utils import XrAnchorSynchronizer diff --git a/source/isaaclab_teleop/isaaclab_teleop/action_policy.py b/source/isaaclab_teleop/isaaclab_teleop/action_policy.py new file mode 100644 index 000000000000..4c62c725ee65 --- /dev/null +++ b/source/isaaclab_teleop/isaaclab_teleop/action_policy.py @@ -0,0 +1,48 @@ +# 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 + + +"""Task-owned action validation and backend preparation shared by live and replay.""" + + +def prepare_teleop_action(env, action, conditioner, validator, state: dict): + """Validate and condition one task-owned action without stepping physics.""" + issue = validator(action) if validator is not None else None + if issue is None and conditioner is not None: + action = conditioner(env, action, state) + issue = validator(action) if validator is not None else None + return action, issue + + +def prepare_teleop_backend(env_cfg, physics_cfg) -> None: + """Apply task-owned preparation for the resolved physics configuration.""" + from isaaclab_ov.physics import OvPhysxCfg + + backend = None + if isinstance(physics_cfg, OvPhysxCfg): + backend = "ovphysx" + elif getattr(getattr(physics_cfg, "solver_cfg", None), "solver_type", None) == "mujoco_warp": + backend = "newton_mjwarp" + if backend is not None: + stabilize = getattr(env_cfg, f"stabilize_for_kitless_{backend}", None) + if callable(stabilize): + stabilize() + + +def initial_hand_tracking_issue(teleop_interface, initialized_sides: set[str], *, required: bool = True) -> str | None: + """Wait for initial hand presence without changing later tracking-loss behavior.""" + required_sides = {"left", "right"} + if not required or required_sides.issubset(initialized_sides): + return None + lifecycle = teleop_interface._session_lifecycle + result = lifecycle.last_step_result + if result is None: + return "hand-tracking presence is unavailable" + hands = (result.get(lifecycle.HAND_LEFT_KEY), result.get(lifecycle.HAND_RIGHT_KEY)) + if any(hand is None for hand in hands): + return "hand-tracking presence is unavailable" + initialized_sides.update(side for side, hand in zip(("left", "right"), hands, strict=True) if not hand.is_none) + missing = sorted(required_sides - initialized_sides) + return f"waiting for initial {'/'.join(missing)} hand tracking" if missing else None diff --git a/source/isaaclab_teleop/isaaclab_teleop/camera_feed.py b/source/isaaclab_teleop/isaaclab_teleop/camera_feed.py index 450f79f74d07..2af6122bfe25 100644 --- a/source/isaaclab_teleop/isaaclab_teleop/camera_feed.py +++ b/source/isaaclab_teleop/isaaclab_teleop/camera_feed.py @@ -16,7 +16,7 @@ from copy import deepcopy from dataclasses import dataclass from functools import lru_cache -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING, Any, Literal import torch @@ -453,6 +453,49 @@ def _panel_descriptor(cfg: XrCameraFeedCfg, layout_cfg: XrCameraFeedLayoutCfg) - ) +def resolve_static_panel_pose( + cfg: XrCameraFeedCfg, layout_cfg: XrCameraFeedLayoutCfg +) -> tuple[ + Literal["viewer_start", "head_locked", "world"], + tuple[float, float, float], + tuple[float, float, float, float], +]: + """Resolve one application-owned static panel pose for a kitless presenter. + + Viewer-start and head-locked retain the feed's local offset policy. OVXR + captures viewer-start once and resolves head-locked against the current + headset pose on every rendered frame. World placement applies that offset + in the configured panel plane and returns one fixed stage-world pose. + """ + _validate_layout_cfg(layout_cfg) + if layout_cfg.placement in {"viewer_start", "head_locked"}: + return ( + layout_cfg.placement, + (float(cfg.offset_m[0]), float(cfg.offset_m[1]), -float(cfg.distance_m)), + (0.0, 0.0, 0.0, 1.0), + ) + + assert layout_cfg.world_position_m is not None + quaternion = tuple(float(value) for value in layout_cfg.world_orientation_xyzw) + norm = math.sqrt(sum(value * value for value in quaternion)) + qx, qy, qz, qw = (value / norm for value in quaternion) + ox, oy = (float(value) for value in cfg.offset_m) + # Rotate the panel-local (x, y, 0) offset by the world orientation. + tx = 2.0 * (-qz * oy) + ty = 2.0 * (qz * ox) + tz = 2.0 * (qx * oy - qy * ox) + rotated = ( + ox + qw * tx + qy * tz - qz * ty, + oy + qw * ty + qz * tx - qx * tz, + qw * tz + qx * ty - qy * tx, + ) + return ( + "world", + tuple(float(base) + offset for base, offset in zip(layout_cfg.world_position_m, rotated, strict=True)), + (qx, qy, qz, qw), + ) + + @dataclass class _ActiveFeed: cfg: XrCameraFeedCfg diff --git a/source/isaaclab_teleop/isaaclab_teleop/cloudxrjs-cloudxr.env b/source/isaaclab_teleop/isaaclab_teleop/cloudxrjs-cloudxr.env index fc46fa8d9c22..a4e492be1d42 100644 --- a/source/isaaclab_teleop/isaaclab_teleop/cloudxrjs-cloudxr.env +++ b/source/isaaclab_teleop/isaaclab_teleop/cloudxrjs-cloudxr.env @@ -1,3 +1,3 @@ NV_DEVICE_PROFILE=auto-webrtc -NV_CXR_ENABLE_PUSH_DEVICES=0 +NV_CXR_ENABLE_PUSH_DEVICES=1 NV_ENABLE_POSE_WAIT=0 diff --git a/source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_cfg.py b/source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_cfg.py index dd5e4f651ba5..a8dc45033f11 100644 --- a/source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_cfg.py +++ b/source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_cfg.py @@ -10,7 +10,7 @@ from collections.abc import Callable from dataclasses import MISSING, field from pathlib import Path -from typing import TYPE_CHECKING, Literal +from typing import TYPE_CHECKING, Any, Literal from isaaclab.utils import configclass @@ -246,6 +246,27 @@ def build_pipeline(): ``"START"`` command is received from xr_core via the message bus. """ + require_initial_bimanual_hand_tracking: bool = False + """Wait for both hands to be present before the first environment step. + + An environment or device reset clears the initialization latch. Later + tracking loss follows the configured retargeting pipeline's behavior. + """ + + action_conditioner: Callable[[Any, Any, dict[str, Any]], Any] | None = None + """Optional task-owned pre-step action conditioner. + + The callable receives ``(env, action, state)``. The launcher retains the + state dictionary across active frames and clears it on pause or reset. + """ + + action_validator: Callable[[Any], str | None] | None = None + """Optional task-owned fail-closed action validator. + + Return ``None`` for a valid action or a concise reason to render without a + physics step. Validation runs both before and after conditioning. + """ + retargeters_to_tune: Callable[[], list[BaseRetargeter]] | None = None """Optional callable returning retargeters to expose in the tuning UI. diff --git a/source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_device.py b/source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_device.py index 2170a29207db..ff85c4391629 100644 --- a/source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_device.py +++ b/source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_device.py @@ -116,6 +116,9 @@ def __init__( mcap_replay_path: str | None = None, enable_debug_visualization: bool = False, haptic_cfg: HapticFeedbackCfg | None = None, + *, + external_oxr_handles_provider: Callable[[], object | None] | None = None, + ovxr_session: bool = False, ): """Initialize the Isaac Capture device. @@ -133,6 +136,13 @@ def __init__( standalone (``False``) with ``isaacteleop`` owning its own OpenXR session through the CloudXR runtime -- teleop I/O with no Kit XR rendering. Typically wired to the ``--xr`` CLI flag. + external_oxr_handles_provider: Optional callback returning handles + borrowed from an application-owned OVXR OVXR context. + Supplying this capability delegates XR rendering/anchoring to + that owner, not Kit XRCore; only a static anchor is supported. + ovxr_session: Use application-owned presentation and static + anchoring without borrowing live input handles. This allows + recorded input to drive a separately presented scene. mcap_record_path: Optional MCAP file path to record the live teleop session into. Mutually exclusive with *mcap_replay_path*. Debug-grade only -- the produced file @@ -155,14 +165,19 @@ def __init__( """ self._cfg = cfg self._target_world_matrix = None + self._ovxr_session = ovxr_session or external_oxr_handles_provider is not None - self._anchor_manager = XrAnchorManager(cfg.xr_cfg) + # Borrowed OVXR handles imply external anchor ownership. + self._anchor_manager = XrAnchorManager( + cfg.xr_cfg, ovxr_session=ovxr_session or external_oxr_handles_provider is not None + ) self._command_handler = CommandHandler() self._session_lifecycle = TeleopSessionLifecycle( cfg, cloudxr_env_file=cloudxr_env_file, auto_launch_cloudxr=auto_launch_cloudxr, use_kit_xr_bridge=use_kit_xr_bridge, + external_oxr_handles_provider=external_oxr_handles_provider, mcap_record_path=mcap_record_path, mcap_replay_path=mcap_replay_path, enable_debug_visualization=enable_debug_visualization, @@ -227,25 +242,71 @@ def __enter__(self) -> IsaacTeleopDevice: Raises: ValueError: If the configured target frame does not exist. """ - if self._cfg.target_frame_prim_path is not None: - import usdrt - from usdrt import Rt - - stage = usdrt.Usd.Stage.Attach(get_current_stage_id()) - prim = stage.GetPrimAtPath(self._cfg.target_frame_prim_path) - if not prim.IsValid(): - raise ValueError(f"No target frame at {self._cfg.target_frame_prim_path!r}.") - self._target_world_matrix = Rt.Xformable(prim).GetFabricHierarchyWorldMatrixAttr() - self._session_lifecycle.start() + try: + if not self._ovxr_session and self._cfg.target_frame_prim_path is not None: + import usdrt + from usdrt import Rt + + stage = usdrt.Usd.Stage.Attach(get_current_stage_id()) + prim = stage.GetPrimAtPath(self._cfg.target_frame_prim_path) + if not prim.IsValid(): + raise ValueError(f"No target frame at {self._cfg.target_frame_prim_path!r}.") + self._target_world_matrix = Rt.Xformable(prim).GetFabricHierarchyWorldMatrixAttr() + self._session_lifecycle.start() + except BaseException as error: + # A failed __enter__ has no automatic __exit__; unwind any runtime, + # pipeline or UI acquired before the failure without replacing it. + self.__exit__(type(error), error, error.__traceback__) + raise return self def __exit__(self, exc_type, exc_val, exc_tb): """Exit the context manager and clean up the Isaac Capture session.""" self._target_world_matrix = None - self._anchor_manager.cleanup() - self._session_lifecycle.stop(exc_type, exc_val, exc_tb) + error = exc_val + try: + self._session_lifecycle.stop(exc_type, exc_val, exc_tb) + except Exception as cleanup_error: + if error is None: + error = cleanup_error + else: + error.add_note(f"Isaac Capture cleanup failed: {cleanup_error!r}") + try: + self._anchor_manager.cleanup() + except Exception as cleanup_error: + if error is None: + error = cleanup_error + else: + error.add_note(f"XR anchor cleanup failed: {cleanup_error!r}") + if exc_val is None and error is not None: + raise error return False + @property + def xr_anchor_path(self) -> str: + """Return the exact anchor prim path consumed by external OVXR.""" + return self._anchor_manager.anchor_headset_path + + @property + def xr_render_anchor_path(self) -> str: + """Return the anchor prim path used to frame XR cameras.""" + return self._anchor_manager.render_anchor_path + + @property + def external_anchor_state(self) -> XrAnchorManager | None: + """The dynamic anchor capability consumed by Lab's external scene publisher.""" + return self._anchor_manager if self._anchor_manager.external_reference_path is not None else None + + @property + def required_openxr_extensions(self) -> tuple[str, ...]: + """Return extensions required by the built teleop graphs.""" + return self._session_lifecycle.required_openxr_extensions() + + @property + def session_cleanup_failed(self) -> bool: + """Whether borrowed XR handles could not be safely drained.""" + return self._session_lifecycle.session_cleanup_failed + def reset(self, pause: bool = False) -> None: """Reset the device state. @@ -511,6 +572,9 @@ def create_isaac_teleop_device( mcap_replay_path: str | None = None, enable_debug_visualization: bool = False, haptic_cfg: HapticFeedbackCfg | None = None, + *, + external_oxr_handles_provider: Callable[[], object | None] | None = None, + ovxr_session: bool = False, ) -> IsaacTeleopDevice: """Create an :class:`IsaacTeleopDevice` with required Omniverse extension setup. @@ -550,6 +614,10 @@ def create_isaac_teleop_device( ``isaacteleop`` creates its own OpenXR session through the CloudXR runtime, so teleop I/O works headless without Kit XR rendering. Typically wired to the ``--xr`` CLI flag. + external_oxr_handles_provider: Optional callback returning handles + borrowed from an external OVXR context. + ovxr_session: Use OVXR presentation with a static anchor even + when input comes from an MCAP reader rather than live XR handles. mcap_record_path: Optional MCAP file path to record the live teleop session into. Debug-grade only. Mutually exclusive with *mcap_replay_path*. @@ -594,6 +662,8 @@ def create_isaac_teleop_device( cloudxr_env_file=cloudxr_env_file, auto_launch_cloudxr=auto_launch_cloudxr, use_kit_xr_bridge=use_kit_xr_bridge, + external_oxr_handles_provider=external_oxr_handles_provider, + ovxr_session=ovxr_session, mcap_record_path=mcap_record_path, mcap_replay_path=mcap_replay_path, enable_debug_visualization=enable_debug_visualization, diff --git a/source/isaaclab_teleop/isaaclab_teleop/ov_libraries_xr.py b/source/isaaclab_teleop/isaaclab_teleop/ov_libraries_xr.py new file mode 100644 index 000000000000..b3c128217996 --- /dev/null +++ b/source/isaaclab_teleop/isaaclab_teleop/ov_libraries_xr.py @@ -0,0 +1,797 @@ +# 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 + +"""Isaac Lab host adapter for IsaacTeleop-owned OVXR presentation.""" + +from __future__ import annotations + +import contextlib +import logging +import math +import re +import threading +import time +from collections import deque +from collections.abc import Hashable, Iterable +from dataclasses import dataclass, field, replace +from typing import Any, Literal + +from isaaccapture.ovxr_session_manager import OvxrApplicationInterface, StageOrdinalLedger +from isaaclab_ov.ovxr_session import OvxrSessionBootstrap, validate_ovxr_session_preparation +from isaaclab_ov.renderers import OVRTXRendererCfg + +logger = logging.getLogger(__name__) + +_DEFAULT_OVXR_CAMERA_PATHS = ( + "/OVXRGeneratedCameras/OVXR_LeftEyeCamera", + "/OVXRGeneratedCameras/OVXR_RightEyeCamera", +) +_DEFAULT_OVXR_RENDER_PRODUCT_PATHS = ( + "/Render/OmniverseKit/HydraTextures/omni_kit_widget_viewport_ViewportTexture_0_OVXR_Left", + "/Render/OmniverseKit/HydraTextures/omni_kit_widget_viewport_ViewportTexture_0_OVXR_Right", +) +_DEFAULT_OVXR_RENDER_VAR_PATHS = ( + "/Render/Vars/LdrColor", + "/Render/Vars/XRDepth", +) +_DEFAULT_OVXR_RENDER_SETTINGS_PATH = "/Render/OmniverseGlobalRenderSettings" +_ENVIRONMENT_ROOT_PATH = "/World/envs/env_0" + + +class _CounterRate: + """Measure a cumulative application counter over a recent wall-clock window.""" + + def __init__(self, window_s: float = 1.0) -> None: + self._window_ns = int(window_s * 1_000_000_000) + self._samples: deque[tuple[int, int]] = deque() + + def sample(self, count: int, now_ns: int | None = None) -> float: + now_ns = time.perf_counter_ns() if now_ns is None else now_ns + if self._samples and count < self._samples[-1][1]: + self._samples.clear() + self._samples.append((now_ns, count)) + while len(self._samples) > 2 and self._samples[1][0] <= now_ns - self._window_ns: + self._samples.popleft() + if len(self._samples) < 2: + return 0.0 + elapsed_s = (self._samples[-1][0] - self._samples[0][0]) / 1_000_000_000 + count_delta = self._samples[-1][1] - self._samples[0][1] + return 0.0 if elapsed_s <= 0.0 else max(0, count_delta) / elapsed_s + + +def _apply_hidden_visual_prims(env: Any, patterns: Iterable[str]) -> tuple[str, ...]: + """Author persistent application-stage visibility before materialization. + + This affects every view of the application scene, not just the XR eyes. + Use scene partitions when source visibility must remain unchanged. + """ + patterns = tuple(patterns) + if not patterns: + return () + + from pxr import UsdGeom + + unwrapped = getattr(env, "unwrapped", env) + scene = getattr(unwrapped, "scene", None) + stage = getattr(scene, "stage", None) + if stage is None: + raise RuntimeError("Configured hidden visual prims require an environment Stage") + + compiled = [(pattern, re.compile(pattern)) for pattern in patterns] + matches: dict[str, list[Any]] = {pattern: [] for pattern in patterns} + for prim in stage.Traverse(): + if not prim.IsA(UsdGeom.Imageable): + continue + path = str(prim.GetPath()) + for pattern, expression in compiled: + if expression.fullmatch(path): + matches[pattern].append(prim) + + missing = [pattern for pattern, prims in matches.items() if not prims] + if missing: + raise RuntimeError(f"Hidden visual prim patterns matched no imageable prims: {missing}") + + hidden_paths: list[str] = [] + hidden_path_set: set[str] = set() + for pattern in patterns: + for prim in matches[pattern]: + path = str(prim.GetPath()) + if path in hidden_path_set: + continue + UsdGeom.Imageable(prim).MakeInvisible() + hidden_paths.append(path) + hidden_path_set.add(path) + logger.info("Hid %d prim(s) from all camera views (%s)", len(hidden_paths), list(patterns)) + return tuple(hidden_paths) + + +def _declare_ovxr_runtime_prims(env: Any, cfg: OvLibrariesXrConfig) -> tuple[tuple[str, str], ...]: + """Declare type-only OVXR prims for the application Stage's lifetime.""" + if not cfg.create_render_products: + return () + + unwrapped = getattr(env, "unwrapped", env) + scene = getattr(unwrapped, "scene", None) + stage = getattr(scene, "stage", None) + if stage is None: + raise RuntimeError("OVXR runtime prim declaration requires an environment Stage") + + render_products = cfg.render_products or _DEFAULT_OVXR_RENDER_PRODUCT_PATHS + declarations = ( + *((path, "Camera") for path in cfg.camera_paths), + *((path, "RenderProduct") for path in render_products), + *((path, "RenderVar") for path in _DEFAULT_OVXR_RENDER_VAR_PATHS), + (_DEFAULT_OVXR_RENDER_SETTINGS_PATH, "RenderSettings"), + ) + expected_types: dict[str, str] = {} + for path, type_name in declarations: + if not path.startswith("/") or path == "/": + raise ValueError(f"OVXR runtime prim path must be absolute and non-root: {path!r}") + previous_type = expected_types.setdefault(path, type_name) + if previous_type != type_name: + raise ValueError(f"OVXR runtime prim path {path!r} has conflicting requested types") + + for path, type_name in expected_types.items(): + existing = stage.GetPrimAtPath(path) + existing_type = str(existing.GetTypeName()) if existing and existing.IsValid() else "" + if existing_type and existing_type != type_name: + raise RuntimeError( + f"OVXR runtime prim {path!r} already has incompatible type {existing_type!r}; expected {type_name!r}" + ) + stage.DefinePrim(path, type_name) + return tuple(expected_types.items()) + + +def _import_ovxr(): + import ovxr + + return ovxr + + +class _StageOrdinalLedger(StageOrdinalLedger): + """Reserve explicit OVStage ordinals for the application publication lane.""" + + def __init__(self, stage: Any) -> None: + self._stage = stage + self._state_lock = threading.Lock() + self._active_ordinal: int | None = None + self._closed = False + self._floor = self._read_floor() + + def _read_floor(self) -> int: + with self._stage.get_attribute_write_floor() as query: + query.wait() + floor = int(query.fetch()) + if floor < 0 or floor > (1 << 64) - 1: + raise RuntimeError(f"OVStage returned an invalid write floor: {floor}") + return floor + + def _require_active(self, ordinal: int) -> None: + if self._active_ordinal != ordinal: + raise RuntimeError( + f"OVStage ordinal {ordinal} is not the active application reservation ({self._active_ordinal=})" + ) + + def reserve(self) -> int: + """Reserve exactly ``floor + 1`` while the application publication guard is held.""" + with self._state_lock: + if self._closed: + raise RuntimeError("OVStage ordinal ledger is closed") + if self._active_ordinal is not None: + raise RuntimeError(f"OVStage ordinal {self._active_ordinal} is still unresolved") + if self._floor == (1 << 64) - 1: + raise RuntimeError("OVStage ordinal range is exhausted") + self._active_ordinal = self._floor + 1 + return self._active_ordinal + + def publish(self, ordinal: int) -> None: + """Seal one application-written reservation and mark it resolved.""" + with self._state_lock: + self._require_active(ordinal) + self._stage.advance_write_floor(ordinal=ordinal).wait() + self._floor = ordinal + self._active_ordinal = None + + def resolve_external(self, ordinal: int, *, expected_published: bool | None) -> bool: + """Resolve an external write and verify its documented publication result.""" + with self._state_lock: + self._require_active(ordinal) + if expected_published is True: + published = True + else: + floor = self._read_floor() + if floor < self._floor or floor > ordinal: + raise RuntimeError( + f"OVStage write floor {floor} is outside the active ledger range [{self._floor}, {ordinal}]" + ) + published = floor == ordinal + # Record native side effects even when the response contradicts the + # expected result. A reported error cannot make a sealed ordinal reusable. + if published: + self._floor = ordinal + if expected_published is not None and published != expected_published: + self._active_ordinal = None + state = "sealed" if published else "unsealed" + expected = "sealed" if expected_published else "unsealed" + raise RuntimeError(f"OVStage ordinal {ordinal} is {state} after an external write; expected {expected}") + self._active_ordinal = None + return published + + def abandon(self, ordinal: int) -> None: + """Reconcile a failed writer before releasing its reservation.""" + with self._state_lock: + self._require_active(ordinal) + # A publication wait can fail after the native Stage has sealed it. + # Query only on the failure/abandon path; an unreadable or unrelated + # floor leaves the reservation unresolved so it cannot be reused. + floor = self._read_floor() + if floor < self._floor or floor > ordinal: + raise RuntimeError( + f"OVStage write floor {floor} is outside the active ledger range [{self._floor}, {ordinal}]" + ) + self._floor = floor + self._active_ordinal = None + + def destroy(self) -> None: + """Close the ledger after every application and external write is resolved.""" + with self._state_lock: + if self._active_ordinal is not None: + raise RuntimeError(f"Cannot close with unresolved OVStage ordinal {self._active_ordinal}") + self._closed = True + + +@dataclass(frozen=True) +class OvLibrariesXrCameraConfig: + """One application-selected sensor and its camera-panel policy.""" + + name: str + camera_cfg: Any = field(repr=False) + max_fps: float = 30.0 + panel_width_m: float = 0.48 + position: tuple[float, float, float] = (0.0, -0.15, -0.8) + orientation_xyzw: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0) + space: Literal["head_locked"] = "head_locked" + + def __post_init__(self) -> None: + if not self.name or self.camera_cfg is None: + raise ValueError("camera name and configuration are required") + from isaaccapture.ovxr_session_manager.ovxr_session import AuxiliaryRasterConfig + + AuxiliaryRasterConfig( + max_fps=self.max_fps, + panel_width_m=self.panel_width_m, + position=self.position, + orientation_xyzw=self.orientation_xyzw, + space=self.space, + ) + + +@dataclass +class OvLibrariesXrConfig: + """Isaac Lab configuration for an IsaacTeleop-owned OVXR session.""" + + application_name: str = "isaaclab-kitless-teleop" + session_mode: Literal["ar", "vr"] = "ar" + renderer_settings: str = "--/app/hydra/aperture/conform=stretch" + cloudxr_env_file: str | None = None + auto_launch_cloudxr: bool = True + render_products: tuple[str, ...] = () + camera_paths: tuple[str, ...] = _DEFAULT_OVXR_CAMERA_PATHS + create_render_products: bool = True + meters_per_unit: float = 1.0 + depth_near_z: float = 0.0 + depth_far_z: float = 0.0 + tracking_anchor_path: str = "" + host_wait_render_output: bool = True + desktop_mirror_enabled: bool = False + desktop_mirror_max_fps: float = 30.0 + auxiliary_cameras: tuple[OvLibrariesXrCameraConfig, ...] = () + extra_openxr_extensions: tuple[str, ...] = () + + def __post_init__(self) -> None: + cameras = self.auxiliary_cameras + if len(cameras) > 3 or len({camera.name for camera in cameras}) != len(cameras): + raise ValueError("Select at most three uniquely named auxiliary cameras") + if self.session_mode not in ("ar", "vr"): + raise ValueError("session_mode must be 'ar' or 'vr'") + if self.meters_per_unit <= 0: + raise ValueError("meters_per_unit must be positive") + if not math.isfinite(self.desktop_mirror_max_fps) or self.desktop_mirror_max_fps <= 0.0: + raise ValueError("desktop_mirror_max_fps must be finite and positive") + if (self.depth_near_z, self.depth_far_z) != (0.0, 0.0) and ( + not math.isfinite(self.depth_near_z) + or not math.isfinite(self.depth_far_z) + or self.depth_near_z <= 0.0 + or self.depth_far_z <= self.depth_near_z + ): + raise ValueError("depth_near_z and depth_far_z must both be zero or finite with 0 < near < far") + if len(self.camera_paths) != 2 or any(not path for path in self.camera_paths): + raise ValueError("Kitless stereo requires exactly two non-empty OVXR camera paths") + + +class _IsaacLabOvxrApplicationInterface(OvxrApplicationInterface): + """Supply only the renderer/Stage/coordinator and simulation publication.""" + + def __init__( + self, + bootstrap: OvxrSessionBootstrap, + env: Any, + cfg: OvLibrariesXrConfig, + hidden_visual_prims: Iterable[str], + excluded_visual_meshes: Iterable[str] = (), + external_anchor_state=None, + ) -> None: + self._bootstrap = bootstrap + self._env = env + self._cfg = cfg + self._hidden_visual_prims = tuple(hidden_visual_prims) + self._excluded_visual_meshes = tuple(excluded_visual_meshes) + self._publication_lock = threading.Lock() + self._scene_publisher = None + self._lease = None + self._ordinal_ledger = None + self._last_published_ordinal: int | None = None + self._publication_closed = False + self._auxiliary_cameras = cfg.auxiliary_cameras + self._external_anchor = external_anchor_state + self._anchor_snapshot = None + self._anchor_resources = contextlib.ExitStack() + self._anchor_query = None + self._anchor_stage = None + self._anchor_retirement_failed = False + + def offer_render_owner_transfer(self) -> object: + self._scene_publisher = self._bootstrap.capture_scene_publisher() + return self._bootstrap.offer_render_owner_transfer() + + def attach_on_render_thread(self, capability: object): + from isaaccapture.ovxr_session_manager.ovxr_session import OvxrSessionBinding + + self._bootstrap.claim_render_owner_transfer(capability) + unwrapped = getattr(self._env, "unwrapped", self._env) + _apply_hidden_visual_prims(self._env, self._hidden_visual_prims) + _declare_ovxr_runtime_prims(self._env, self._cfg) + from .xr_scene_partition import partition_visual_meshes + + stage = getattr(getattr(unwrapped, "scene", None), "stage", None) + with partition_visual_meshes(stage, self._excluded_visual_meshes): + self._lease = self._bootstrap.bind_environment(self._env) + auxiliary_paths = () + if self._auxiliary_cameras: + sensors = getattr(getattr(unwrapped, "scene", None), "sensors", {}) + for camera in self._auxiliary_cameras: + if sensors.get(camera.name) is None: + raise RuntimeError(f"Configured auxiliary camera {camera.name!r} is not present in the environment") + auxiliary_paths = ( + (self._lease.auxiliary_render_product_path,) + if len(self._auxiliary_cameras) == 1 + else self._lease.auxiliary_render_product_paths + ) + with self._lease.ovstage_owner_lane(): + self._lease.configure_external_camera_scene_partitions(_ENVIRONMENT_ROOT_PATH, self._cfg.camera_paths) + self._ordinal_ledger = _StageOrdinalLedger(self._lease.ovstage) + if self._external_anchor is not None: + import ovstage + + self._anchor_stage = self._lease.ovstage + try: + paths = self._anchor_resources.enter_context(ovstage.PathDictionary(self._anchor_stage)) + path_list = paths.create_path_list_from_strings(self._external_anchor.external_anchor_paths) + self._anchor_resources.callback(paths.destroy_path_list, path_list) + self._anchor_query = self._anchor_resources.enter_context( + self._anchor_stage.query_from_path_list(path_list) + ) + self._anchor_query.wait() + except BaseException as setup_error: + try: + self._anchor_resources.close() + except BaseException as cleanup_error: + self._anchor_retirement_failed = True + setup_error.add_note(f"Anchor query cleanup also failed: {cleanup_error!r}") + raise + return OvxrSessionBinding( + renderer=self._lease.ovrtx_renderer, + stage=self._lease.ovstage, + ordinal_ledger=self._ordinal_ledger, + auxiliary_render_product_path=auxiliary_paths[0] if len(auxiliary_paths) == 1 else None, + auxiliary_render_product_paths=auxiliary_paths if len(auxiliary_paths) > 1 else (), + prepare_frame_scene=self.prepare_frame_scene if self._external_anchor is not None else None, + ) + + def publication_guard(self): + return self._publication_lock + + def owner_lane(self): + if self._lease is None: + raise RuntimeError("OVRTX/OVStage lease is not attached") + return self._lease.ovstage_owner_lane() + + def publish_complete_scene(self, publication_id: Hashable) -> int: + if self._publication_closed or self._scene_publisher is None or self._ordinal_ledger is None: + raise RuntimeError("IsaacLab scene publication is not available") + anchor_epoch = None + if self._external_anchor is not None: + # A failed seed can follow the publisher recording its identity. + # Check the terminal policy before dedup may return an older seal. + anchor_epoch = self._external_anchor.external_epoch + if not self._scene_publisher.publication_required(publication_id): + if self._last_published_ordinal is None: + raise RuntimeError("OVStage publication deduplicated before a sealed ordinal exists") + return self._last_published_ordinal + if self._auxiliary_cameras: + unwrapped = getattr(self._env, "unwrapped", self._env) + for camera in self._auxiliary_cameras: + # OVXR presentation bypasses Camera's image-update path. Refresh only poses + # after simulation/reset kinematics, before publishing the matching scene. + unwrapped.scene.sensors[camera.name]._update_poses() + snapshot = None + seed = False + if self._external_anchor is not None: + epoch, seed = anchor_epoch + reference = self._bootstrap.get_world_transform(self._external_anchor.external_reference_path) + unwrapped = getattr(self._env, "unwrapped", self._env) + snapshot = (epoch, reference, unwrapped.sim.get_rendering_dt()) + ordinal = self._ordinal_ledger.reserve() + try: + self._scene_publisher.write_complete_scene(ordinal, publication_id) + except BaseException as write_error: + try: + self._ordinal_ledger.abandon(ordinal) + except BaseException as abandon_error: + write_error.add_note(f"Abandon after failed scene write also failed: {abandon_error!r}") + raise + try: + if seed: + epoch, reference, dt = snapshot + + def commit(matrices): + self._write_anchor_matrices(matrices, ordinal) + self._ordinal_ledger.publish(ordinal) + + if not self._external_anchor.commit_external_pose(reference, dt, epoch, commit): + raise RuntimeError("XR anchor reset changed during complete scene publication") + else: + self._ordinal_ledger.publish(ordinal) + except BaseException as publish_error: + try: + self._ordinal_ledger.abandon(ordinal) + except BaseException as abandon_error: + publish_error.add_note(f"Abandon after failed scene publication also failed: {abandon_error!r}") + raise + self._last_published_ordinal = ordinal + self._anchor_snapshot = snapshot + return ordinal + + def _write_anchor_matrices(self, matrices, ordinal: int) -> None: + import numpy as np + import ovstage + from isaaclab_ov.stage import xform_tensor_from_numpy + + # SDP uses column vectors; the Stage's omni:xform stores USD row matrices. + rows = np.ascontiguousarray(matrices.transpose(0, 2, 1), dtype=np.float64) + tensor = xform_tensor_from_numpy(rows) + self._anchor_stage.write_attribute( + self._anchor_query, + "omni:xform", + ordinal=ordinal, + tensors=tensor, + semantic=ovstage.AttributeSemantic.MATRIX, + is_array=False, + ).wait() + self._anchor_stage.write_attribute( + self._anchor_query, + "omni:resetXformStack", + ordinal=ordinal, + tensors=np.ones(len(rows), dtype=np.bool_), + is_array=False, + ).wait() + + def prepare_frame_scene(self) -> None: + """Advance only from the latest committed A snapshot, once per begun XR frame.""" + if self._anchor_snapshot is None: + return + epoch, reference, dt = self._anchor_snapshot + + def commit(matrices): + ordinal = self._ordinal_ledger.reserve() + try: + self._write_anchor_matrices(matrices, ordinal) + # OVXR reads sealed anchor state at bind; bind reserves the next ordinal. + self._ordinal_ledger.publish(ordinal) + except BaseException as publication_error: + try: + self._ordinal_ledger.abandon(ordinal) + except BaseException as abandon_error: + publication_error.add_note( + f"Abandon after failed anchor publication also failed: {abandon_error!r}" + ) + raise + + self._external_anchor.commit_external_pose(reference, dt, epoch, commit) + + def set_external_anchor_state(self, state) -> None: + """Replace a replay device's policy while preserving native query ownership.""" + if self._external_anchor is not None: + # A new replay manager must not revive a failed borrowed native pair. + _ = self._external_anchor.external_epoch + if (state is None) != (self._external_anchor is None) or ( + state is not None and state.external_anchor_paths != self._external_anchor.external_anchor_paths + ): + raise ValueError("A reused XR environment must preserve its anchor paths") + self._external_anchor = state + self._anchor_snapshot = None + + def validate_first_published_scene(self, ordinal: int) -> None: + if self._lease is None: + raise RuntimeError("OVRTX/OVStage lease is not attached") + with self._lease.ovstage_owner_lane(): + self._lease.validate_external_scene_partitions(ordinal) + + def get_target_frame_T_world(self, prim_path: str): + return self._bootstrap.get_target_frame_T_world(prim_path) + + def close_publication(self) -> None: + if self._publication_closed: + return + self._publication_closed = True + if self._scene_publisher is not None: + self._scene_publisher.close() + + def close_on_render_thread(self) -> None: + first_error: BaseException | None = None + try: + self._anchor_resources.close() + except BaseException: + self._anchor_retirement_failed = True + raise + if self._anchor_retirement_failed: + raise RuntimeError("Anchor query retirement is unproved; retaining its borrowed Stage") + if self._ordinal_ledger is not None: + try: + self._ordinal_ledger.destroy() + except BaseException as exc: + if first_error is None: + first_error = exc + finally: + self._ordinal_ledger = None + try: + self._bootstrap.close() + except BaseException as exc: + if first_error is None: + first_error = exc + if first_error is not None: + raise first_error + + def close_environment_on_render_thread(self) -> None: + if self._anchor_retirement_failed: + raise RuntimeError("Anchor query retirement is unproved; retaining its environment") + self._env.close() + + def abort(self) -> None: + self._bootstrap.abort() + + +class OvLibrariesXrSession: + """Thin Isaac Lab adapter around IsaacTeleop's OVXR session.""" + + def __init__(self, bootstrap: OvxrSessionBootstrap, cfg: OvLibrariesXrConfig): + self._bootstrap = bootstrap + self._cfg = cfg + self._owner = None + self._desktop_mirror = None + self._host = None + self._closed = False + self._physics_rate = _CounterRate() + + @classmethod + def prepare( + cls, + sim_cfg: Any, + cfg: OvLibrariesXrConfig, + renderer_cfg: OVRTXRendererCfg | None = None, + ) -> OvLibrariesXrSession: + """Snapshot session selection, create simulation and reserve its OVRTX lease. + + Renderer settings are process-global on the stock renderer interface; + use one configured session per process. No supported readback exists + here to restore a previous renderer's settings on close. + """ + cameras = tuple(cfg.auxiliary_cameras) + for camera in cameras: + if camera.space != "head_locked": + raise ValueError("Kitless XR camera quads currently require head_locked placement") + validate_ovxr_session_preparation() + # Stock settings are applied before Renderer construction. Preserve explicit caller choices. + _import_ovxr().apply_renderer_settings(cfg.renderer_settings.strip()) + cfg = replace(cfg, auxiliary_cameras=cameras) + bootstrap = OvxrSessionBootstrap(sim_cfg, renderer_cfg) + try: + for camera in cameras: + bootstrap.configure_auxiliary_camera(camera.camera_cfg, max_fps=camera.max_fps) + return cls(bootstrap, cfg) + except BaseException as preparation_error: + try: + bootstrap.abort() + except BaseException as abort_error: + # Cleanup follows the failure; retain its original cause. + preparation_error.add_note(f"Cleanup after failed XR preparation also failed: {abort_error!r}") + raise + + def start_render_worker( + self, + env: Any, + *, + tracking_anchor_path: str | None = None, + required_openxr_extensions: Iterable[str] = (), + hidden_visual_prims: Iterable[str] = (), + excluded_visual_meshes: Iterable[str] = (), + max_blank_frames: int = 1, + timeout_s: float = 30.0, + external_anchor_state=None, + ) -> bool: + """Give IsaacTeleop the application pair and report whether XR started.""" + self._require_open() + if self._owner is not None: + raise RuntimeError("OVXR async render worker is already started") + from isaaccapture.ovxr_session_manager.desktop_mirror import DesktopMirrorConfig + from isaaccapture.ovxr_session_manager.ovxr_session import ( + AuxiliaryRasterConfig, + OvxrSession, + OvxrSessionConfig, + ) + from isaaccapture.ovxr_session_manager.owned_mirror import OwnedDesktopMirror + + from .session_lifecycle import cloudxr_eula_accepted + + anchor_path = self._cfg.tracking_anchor_path if tracking_anchor_path is None else tracking_anchor_path + host = _IsaacLabOvxrApplicationInterface( + self._bootstrap, env, self._cfg, hidden_visual_prims, excluded_visual_meshes, external_anchor_state + ) + self._host = host + rasters = tuple( + AuxiliaryRasterConfig( + label=camera.name, + max_fps=camera.max_fps, + max_image_bytes=int(camera.camera_cfg.width) * int(camera.camera_cfg.height) * 4, + panel_width_m=camera.panel_width_m, + position=camera.position, + orientation_xyzw=camera.orientation_xyzw, + space=camera.space, + ) + for camera in self._cfg.auxiliary_cameras + ) + if self._cfg.desktop_mirror_enabled: + self._desktop_mirror = OwnedDesktopMirror( + DesktopMirrorConfig( + max_fps=self._cfg.desktop_mirror_max_fps, + initial_status_text="Waiting for headset", + ) + ) + self._owner = OvxrSession( + host, + OvxrSessionConfig( + application_name=self._cfg.application_name, + session_mode=self._cfg.session_mode, + cloudxr_env_file=self._cfg.cloudxr_env_file, + auto_launch_cloudxr=self._cfg.auto_launch_cloudxr, + accept_eula=cloudxr_eula_accepted(), + render_products=self._cfg.render_products, + camera_paths=self._cfg.camera_paths, + create_render_products=self._cfg.create_render_products, + meters_per_unit=self._cfg.meters_per_unit, + depth_near_z=self._cfg.depth_near_z, + depth_far_z=self._cfg.depth_far_z, + tracking_anchor_path=anchor_path, + floor_aligned_anchor=False, + host_wait_render_output=self._cfg.host_wait_render_output, + auxiliary_raster=rasters[0] if len(rasters) == 1 else None, + auxiliary_rasters=rasters if len(rasters) > 1 else (), + extra_openxr_extensions=self._cfg.extra_openxr_extensions, + ), + required_openxr_extensions=required_openxr_extensions, + max_blank_frames=max_blank_frames, + frame_consumers=(self._desktop_mirror,) if self._desktop_mirror is not None else (), + ) + return self._owner.start(timeout_s=timeout_s) + + def external_oxr_handles(self): + """Return IsaacTeleop's typed external handles once its owner is ready.""" + self._require_open() + return None if self._owner is None else self._owner.external_oxr_handles() + + def request_render(self, physics_step_id: Hashable, *, force: bool = False) -> int | None: + """Offer one complete application generation for IsaacTeleop to render. + + Return the admitted ordinal, or ``None`` when render demand is already + satisfied. Retry the latest dirty generation until admitted, including + while paused. ``force`` preserves reset/lifecycle discontinuities. + """ + self._require_started() + return self._owner.request_render(physics_step_id, force=force) + + def report_host_timing(self, physics_step_count: int, physics_dt_s: float | None = None) -> None: + """Publish IsaacLab-owned status text for the optional desktop mirror.""" + self._require_started() + if self._desktop_mirror is None: + return + self._desktop_mirror.record_host_frame() + physics_hz = self._physics_rate.sample(physics_step_count) + if physics_dt_s is not None and math.isfinite(physics_dt_s) and physics_dt_s > 0.0: + target_hz = 1.0 / physics_dt_s + physics_text = f"Physics {physics_hz:5.1f}/{target_hz:.0f} Hz ({physics_hz / target_hz:.2f}× RT)" + else: + physics_text = f"Physics {physics_hz:5.1f} Hz" + mirror_status = self._desktop_mirror.status + if mirror_status is not None: + self._desktop_mirror.set_status_text( + f"Main {mirror_status.main_thread_fps:5.1f} FPS " + f"{physics_text} Render {mirror_status.render_thread_fps:5.1f} FPS" + ) + + def get_target_frame_T_world(self, prim_path: str): + """Read a target transform through the thin simulation host adapter.""" + self._require_started() + return self._owner.get_target_frame_T_world(prim_path) + + def close(self) -> None: + """Close IsaacTeleop's owner before releasing the application environment.""" + if self._closed: + return + if self._owner is None: + self._bootstrap.abort() + else: + self._owner.close() + self._closed = True + + @property + def async_worker_owns_environment(self) -> bool: + """Whether IsaacTeleop accepted environment teardown ownership.""" + return self._owner is not None and self._owner.owns_environment + + @property + def shutdown_requested(self) -> bool: + """Whether the OVXR session requested application shutdown.""" + return self._owner is not None and self._owner.shutdown_requested + + def request_shutdown(self) -> None: + """Request orderly OVXR shutdown.""" + if self._owner is not None: + self._owner.request_shutdown() + + def set_external_anchor_state(self, state) -> None: + """Adopt the current replay device's anchor policy between runs.""" + self._require_started() + with self._host.publication_guard(): + self._host.set_external_anchor_state(state) + + def wait_for_shutdown(self, timeout_s: float) -> bool: + """Wait for application cancellation without delaying orderly teardown.""" + self._require_started() + return self._owner.wait_for_shutdown(timeout_s) + + @property + def render_worker_bootstrap_ready(self) -> bool: + """Whether IsaacTeleop claimed the renderer before peer-gated startup.""" + return self._owner is not None and self._owner.render_worker_bootstrap_ready + + def abort(self) -> None: + """Tear down a failed pre-environment bootstrap.""" + self.close() + + def _require_open(self) -> None: + if self._closed: + raise RuntimeError("OVXR session is closed") + + def _require_started(self) -> None: + self._require_open() + if self._owner is None: + raise RuntimeError("OVXR session is not started") + + def __enter__(self) -> OvLibrariesXrSession: + self._require_open() + return self + + def __exit__(self, *_exc: object) -> None: + self.close() diff --git a/source/isaaclab_teleop/isaaclab_teleop/session_lifecycle.py b/source/isaaclab_teleop/isaaclab_teleop/session_lifecycle.py index 75f34b9310ae..3d3936e6d034 100644 --- a/source/isaaclab_teleop/isaaclab_teleop/session_lifecycle.py +++ b/source/isaaclab_teleop/isaaclab_teleop/session_lifecycle.py @@ -212,12 +212,10 @@ class TeleopSessionLifecycle: Only chained when debug visualization is enabled.""" HAND_LEFT_KEY = "hand_left" - """Pipeline output key for the left hand ``TensorGroup`` when the debug - outputs are chained (matches ``HandsSource.LEFT``).""" + """Pipeline output key for left-hand presence when ``HandsSource`` exists.""" HAND_RIGHT_KEY = "hand_right" - """Pipeline output key for the right hand ``TensorGroup`` when the debug - outputs are chained (matches ``HandsSource.RIGHT``).""" + """Pipeline output key for right-hand presence when ``HandsSource`` exists.""" _STEP_FAILURE_RESTART_COOLDOWN_S = 1.0 """Minimum delay [s] before recreating the session after a step failure @@ -237,6 +235,8 @@ def __init__( mcap_replay_path: str | None = None, enable_debug_visualization: bool = False, haptic_cfg: HapticFeedbackCfg | None = None, + *, + external_oxr_handles_provider: Callable[[], OpenXRSessionHandles | None] | None = None, ): """Initialize the session lifecycle manager. @@ -258,6 +258,10 @@ def __init__( session through the CloudXR runtime, so teleop I/O works headless without Kit XR rendering. Ignored in replay mode (which never touches the XR runtime). + external_oxr_handles_provider: Optional callback returning handles + borrowed from an external OVXR context. Requires + ``use_kit_xr_bridge=False`` and defers session creation while + the callback returns ``None``. mcap_record_path: Optional path to an MCAP file the live teleop session should be recorded into. Mutually exclusive with *mcap_replay_path*. Debug-grade only -- see the Isaac Lab @@ -287,12 +291,17 @@ def __init__( "mcap_record_path and mcap_replay_path are mutually exclusive; " "set at most one to switch the session between LIVE recording and REPLAY playback." ) + if external_oxr_handles_provider is not None and use_kit_xr_bridge: + raise ValueError("An external OpenXR handle provider requires use_kit_xr_bridge=False") + if external_oxr_handles_provider is not None and mcap_replay_path is not None: + raise ValueError("An external OpenXR handle provider is incompatible with replay mode") self._cfg = cfg self._device = torch.device(cfg.sim_device) self._cloudxr_env_file = cloudxr_env_file self._auto_launch_cloudxr = auto_launch_cloudxr self._use_kit_xr_bridge = use_kit_xr_bridge + self._external_oxr_handles_provider = external_oxr_handles_provider self._mcap_record_path = mcap_record_path self._mcap_replay_path = mcap_replay_path self._is_replay = mcap_replay_path is not None @@ -312,6 +321,7 @@ def __init__( # Session state (populated during start) self._session: TeleopSession | None = None + self._session_cleanup_error: BaseException | None = None self._pipeline = None self._teleop_control_pipeline = None self._message_processor: TeleopMessageProcessor | None = None @@ -324,6 +334,8 @@ def __init__( self._last_right_controller = None self._last_left_controller = None self._last_step_result: dict | None = None + self._last_control_events = _NO_OP_EVENTS + self._last_control_event_frame_id: int | None = None self._session_start_deferred_logged = False # Monotonic deadline gating session re-creation after a step failure self._restart_holdoff_until = 0.0 @@ -404,7 +416,12 @@ def __init__( @property def is_active(self) -> bool: """Whether the teleop session is currently running.""" - return self._session is not None + return self._session is not None and not self.session_cleanup_failed + + @property + def session_cleanup_failed(self) -> bool: + """Whether native/session drain failed and further use is prohibited.""" + return self._session_cleanup_error is not None @property def pipeline(self): @@ -434,9 +451,11 @@ def last_left_controller(self): def last_step_result(self) -> dict | None: """Full pipeline output from the most recent :meth:`step`, or ``None``. - Contains at least ``"action"`` and the right-controller entry. When - debug visualization was enabled at session start, :attr:`HAND_LEFT_KEY` - and :attr:`HAND_RIGHT_KEY` are included when a ``HandsSource`` exists. + Contains at least ``"action"`` and the right-controller entry. + :attr:`HAND_LEFT_KEY` and :attr:`HAND_RIGHT_KEY` are included whenever + a ``HandsSource`` exists; a user pipeline may explicitly provide those + outputs after applying a validity policy. Controller-left and visual + updates remain debug-only. """ return self._last_step_result @@ -449,19 +468,11 @@ def has_control_channel(self) -> bool: def last_control_events(self) -> ControlEvents: """Control events from the most recent :meth:`step`. - When a ``teleop_control_pipeline`` is configured, derives - :class:`ControlEvents` from - ``session.last_context.execution_events``. Otherwise returns a - default (no-op) :class:`ControlEvents`. + Reset is an edge-triggered event. In pipelined execution, multiple app + steps can return the same completed retargeting frame, so the event is + latched by returned frame ID and emitted only on its first return. """ - if self._message_processor is None: - return _NO_OP_EVENTS - if self._session is None: - return _NO_OP_EVENTS - ctx = self._session.last_context - if ctx is None: - return _NO_OP_EVENTS - return _execution_events_to_control(ctx.execution_events) + return self._last_control_events def request_reset(self, pause: bool = False) -> None: """Schedule a reset for the next pipeline step. @@ -528,6 +539,8 @@ def start(self) -> None: "Start AR"), session creation is deferred and will be retried on each :meth:`step` call. """ + if self._session_cleanup_error is not None: + raise self._session_cleanup_error # Measure the workstation before anything expensive starts. Any notice is # queued now and delivered when the client connects. The queue is not # cleared here: send_client_message promises that messages queued before @@ -557,6 +570,8 @@ def start(self) -> None: self._last_right_controller = None self._last_left_controller = None self._last_step_result = None + self._last_control_events = _NO_OP_EVENTS + self._last_control_event_frame_id = None self._pipeline = self._build_combined_pipeline(user_pipeline) @@ -599,29 +614,52 @@ def stop(self, exc_type=None, exc_val=None, exc_tb=None) -> None: """Shut down the session and clean up resources. Closes the retargeting tuning UI and exits the ``TeleopSession`` - context manager. If the underlying OpenXR session was already torn - down externally (e.g. "Stop AR"), cleanup errors are suppressed. + context manager before stopping CloudXR. Cleanup failures propagate + (or are attached to the caller's original exception). If native drain + fails, latch the error and prohibit reuse or restart. The retained session + reference does not prove native drain; the application must not release + borrowed resources or stop the runtime after that failure. Native owners + classify expected disconnects. Args: exc_type: Exception type (for context manager protocol). exc_val: Exception value. exc_tb: Exception traceback. """ - # Close the retargeting tuning UI first + error = exc_val + + def record_failure(cleanup_error: BaseException) -> None: + nonlocal error + if error is None: + error = cleanup_error + else: + error.add_note(f"Isaac Capture cleanup failed: {cleanup_error!r}") + + # A UI failure must not prevent native/session cleanup. if self._retargeting_ui_ctx is not None: - self._retargeting_ui_ctx.__exit__(exc_type, exc_val, exc_tb) - self._retargeting_ui_ctx = None - self._retargeting_ui = None + try: + self._retargeting_ui_ctx.__exit__(exc_type, exc_val, exc_tb) + except BaseException as cleanup_error: + record_failure(cleanup_error) + else: + self._retargeting_ui_ctx = None + self._retargeting_ui = None if self._session is not None: + if self._session_cleanup_error is not None: + if self._session_cleanup_error is not exc_val: + record_failure(self._session_cleanup_error) + if exc_val is None: + raise error + return try: self._session.__exit__(exc_type, exc_val, exc_tb) - except Exception as e: - # The OpenXR session may have already been torn down externally - # (e.g. user clicked "Stop AR"), so destroying spaces/action - # sets will fail with XR_ERROR_HANDLE_INVALID. This is - # expected and safe to suppress. - logger.debug(f"Suppressed error during Isaac Capture session cleanup: {e}") + except BaseException as cleanup_error: + self._session_cleanup_error = cleanup_error + record_failure(cleanup_error) + if exc_val is None: + raise error + return self._session = None # Always clear pipeline state (session may never have been created if @@ -631,6 +669,8 @@ def stop(self, exc_type=None, exc_val=None, exc_tb=None) -> None: self._message_processor = None self._last_step_result = None self._last_left_controller = None + self._last_control_events = _NO_OP_EVENTS + self._last_control_event_frame_id = None self._haptic_sink = None self._haptic_tracker = None # Undelivered notices belong to the session that queued them; a later @@ -640,12 +680,14 @@ def stop(self, exc_type=None, exc_val=None, exc_tb=None) -> None: if self._cloudxr_launcher is not None: try: self._cloudxr_launcher.stop() - except RuntimeError: - logger.warning("CloudXR runtime process could not be terminated; handle retained for atexit cleanup") + except RuntimeError as cleanup_error: + record_failure(cleanup_error) else: self._cloudxr_launcher = None logger.info("CloudXR runtime stopped") + if exc_val is None and error is not None: + raise error logger.info("Isaac Capture session ended") # ------------------------------------------------------------------ @@ -678,21 +720,19 @@ def _build_combined_pipeline(self, user_pipeline): "action": user_pipeline.output("action"), self._CONTROLLER_RIGHT_KEY: self._button_controllers.output(ControllersSource.RIGHT), } + self._chain_hand_presence_outputs(user_pipeline, pipeline_outputs) if self._enable_debug_visualization: pipeline_outputs[self._CONTROLLER_LEFT_KEY] = self._button_controllers.output(ControllersSource.LEFT) - self._chain_hand_debug_outputs(user_pipeline, pipeline_outputs) return OutputCombiner(pipeline_outputs) @staticmethod - def _chain_hand_debug_outputs(user_pipeline, pipeline_outputs: dict) -> None: - """Auto-discover a ``HandsSource`` and chain its raw outputs. + def _chain_hand_presence_outputs(user_pipeline, pipeline_outputs: dict) -> None: + """Chain explicit filtered hand outputs or auto-discover raw source outputs. - Walks the leaf nodes of *user_pipeline*; if a ``HandsSource`` is - found, its raw (XR-anchor-frame) ``hand_left`` / ``hand_right`` - outputs are added to *pipeline_outputs* so they appear in the step - result. The anchor-to-world transform is applied on the Isaac Lab - side by the visualizer, so the markers stay world-frame correct even - when a ``target_T_world`` rebase is active in the pipeline. + A user pipeline may expose its own ``hand_left`` / ``hand_right`` + outputs when validity or another application-neutral policy must be + applied before session safety observes presence. Otherwise walk the + leaf nodes and retain the legacy raw ``HandsSource`` behavior. Args: user_pipeline: The pipeline returned by ``pipeline_builder()``. @@ -701,15 +741,24 @@ def _chain_hand_debug_outputs(user_pipeline, pipeline_outputs: dict) -> None: """ from isaacteleop.retargeting_engine.deviceio_source_nodes import HandsSource + output_types_fn = getattr(user_pipeline, "output_types", None) + output_types = output_types_fn() if callable(output_types_fn) else {} + hand_keys = (TeleopSessionLifecycle.HAND_LEFT_KEY, TeleopSessionLifecycle.HAND_RIGHT_KEY) + if all(key in output_types for key in hand_keys): + for key in hand_keys: + pipeline_outputs[key] = user_pipeline.output(key) + logger.debug("Chained explicit pipeline hand-presence outputs") + return + # HandsSource owns its HandTracker; the tracker API has no reverse # source-node lookup, so use the graph's public leaf discovery API. for leaf in user_pipeline.get_leaf_nodes(): if isinstance(leaf, HandsSource): pipeline_outputs[TeleopSessionLifecycle.HAND_LEFT_KEY] = leaf.output(HandsSource.LEFT) pipeline_outputs[TeleopSessionLifecycle.HAND_RIGHT_KEY] = leaf.output(HandsSource.RIGHT) - logger.debug(f"Auto-discovered HandsSource '{leaf.name}'; chained hand debug outputs") + logger.debug(f"Auto-discovered HandsSource '{leaf.name}'; chained raw hand-presence outputs") return - logger.info("No HandsSource found in the teleop pipeline; hand joint visualization unavailable") + logger.info("No HandsSource found in the teleop pipeline; raw hand presence unavailable") # ------------------------------------------------------------------ # Control pipeline construction @@ -956,6 +1005,10 @@ def _on_request_required_extensions(self) -> list[str]: logger.info(f"Required extensions: {required_extensions}") return required_extensions + def required_openxr_extensions(self) -> tuple[str, ...]: + """Return extensions required by the fully built teleop graphs.""" + return tuple(self._on_request_required_extensions()) + def _on_xr_enabled_changed(self, item, event_type): import carb.settings @@ -1027,6 +1080,8 @@ def _try_start_session(self) -> bool: ``True`` if the session was successfully started (or was already running), ``False`` if session creation was deferred. """ + if self._session_cleanup_error is not None: + raise self._session_cleanup_error if self._session is not None: return True @@ -1056,6 +1111,13 @@ def _try_start_session(self) -> bool: ) self._session_start_deferred_logged = True return False + elif self._external_oxr_handles_provider is not None: + oxr_handles = self._external_oxr_handles_provider() + if oxr_handles is None: + if not self._session_start_deferred_logged: + logger.info("External OpenXR handles are not ready; IsaacTeleop session creation deferred") + self._session_start_deferred_logged = True + return False else: # Standalone mode (e.g. no ``--xr``): do not touch Kit's XR bridge. # Passing ``oxr_handles=None`` makes ``TeleopSession`` create and own @@ -1086,8 +1148,9 @@ def _try_start_session(self) -> bool: ) # Create and enter the TeleopSession - self._session = TeleopSession(session_config) - self._session.__enter__() + session = TeleopSession(session_config) + self._enter_session(session) + self._session = session if self._mcap_record_path is not None: logger.info(f"Isaac Capture session started: {self._cfg.app_name} (recording to {self._mcap_record_path})") @@ -1136,12 +1199,25 @@ def _start_replay_session(self) -> bool: sinks=[], ) - self._session = TeleopSession(session_config) - self._session.__enter__() + session = TeleopSession(session_config) + self._enter_session(session) + self._session = session logger.info(f"Isaac Capture replay session started: {self._cfg.app_name} (replaying {self._mcap_replay_path})") return True + def _enter_session(self, session: TeleopSession) -> None: + """Latch Capture's explicit failed-rollback signal before releasing borrowed owners.""" + try: + session.__enter__() + except BaseException as error: + # Capture also rolls back interrupts. Preserve its failed-drain + # signal without converting the original interrupt into an error. + if getattr(session, "cleanup_failed", False) is True: + self._session = session + self._session_cleanup_error = error + raise + # ------------------------------------------------------------------ # Stepping # ------------------------------------------------------------------ @@ -1183,7 +1259,11 @@ def step( Raises: RuntimeError: If called before :meth:`start`. + Exception: If an earlier session cleanup failed, or replay stepping + fails. A failed cleanup prohibits further use and restart. """ + if self._session_cleanup_error is not None: + raise self._session_cleanup_error if self._pipeline is None: raise RuntimeError("TeleopSessionLifecycle.start() must be called before step()") @@ -1207,7 +1287,6 @@ def step( from isaacteleop.retargeting_engine.interface.execution_events import ExecutionEvents, ExecutionState execution_events = ExecutionEvents(reset=True, execution_state=ExecutionState.RUNNING) - self._pending_reset = False # Execute one step of the teleop session. # If the underlying OpenXR session was destroyed externally (e.g. @@ -1224,18 +1303,32 @@ def step( # The async retarget worker dies permanently on any pipeline # exception, so session re-entry is the only recovery either way; # what differs is the cause and the restart pacing. - if not self._is_replay and self._kit_xr_session_is_active(): + if not self._is_replay and ( + self._external_oxr_handles_provider is not None or self._kit_xr_session_is_active() + ): logger.warning( - "Isaac Capture retargeting step failed (pipeline error, XR session still active); " + "IsaacTeleop retargeting step failed (owned/existing XR session); " f"restarting the teleop session in {self._STEP_FAILURE_RESTART_COOLDOWN_S:.0f}s: {e}" ) self._restart_holdoff_until = time.monotonic() + self._STEP_FAILURE_RESTART_COOLDOWN_S else: logger.warning(f"Isaac Capture session step failed (XR session likely torn down): {e}") - self._teardown_dead_session() + try: + self._teardown_dead_session() + except BaseException as cleanup_error: + # Failed drain retains the owner; never restart over live resources. + self._session_cleanup_error = e + e.add_note(f"Teleop session drain also failed: {cleanup_error!r}") + raise e from cleanup_error + if self._is_replay: + # Re-entering replay would silently rewind a failed recording. + raise return None + if execution_events is not None: + self._pending_reset = False self._last_step_result = result + self._update_control_events() # Store the controller TensorGroups for button polling / debug viz self._last_right_controller = result.get(self._CONTROLLER_RIGHT_KEY) @@ -1250,6 +1343,21 @@ def step( return action + def _update_control_events(self) -> None: + """Cache the control snapshot associated with the returned retargeting frame.""" + if self._message_processor is None or self._session is None or self._session.last_context is None: + self._last_control_events = _NO_OP_EVENTS + return + + events = _execution_events_to_control(self._session.last_context.execution_events) + step_info = getattr(self._session, "last_step_info", None) + frame_id = getattr(step_info, "returned_frame_id", None) + if frame_id is not None and frame_id == self._last_control_event_frame_id: + events = ControlEvents(is_active=events.is_active, should_reset=False) + else: + self._last_control_event_frame_id = frame_id + self._last_control_events = events + # ------------------------------------------------------------------ # Dead session teardown # ------------------------------------------------------------------ @@ -1259,20 +1367,24 @@ def _teardown_dead_session(self) -> None: This is called when :meth:`step` detects that the XR session was destroyed externally (e.g. user clicked "Stop AR"). The - ``TeleopSession`` is exited with error suppression (since its XR - resources are already gone), and the internal state is reset so that - the deferred-start logic in :meth:`step` can re-acquire handles if - the user restarts AR. + ``TeleopSession`` classifies expected native disconnects. Failed drain + propagates and latches a terminal error; only successful drain resets + state so deferred start can re-acquire handles when the user restarts AR. """ if self._session is not None: try: self._session.__exit__(None, None, None) - except Exception as e: - logger.debug(f"Suppressed error tearing down dead session: {e}") + except BaseException as error: + # Exit may already be terminal. Never infer successful drain from + # a later no-op __exit__, or resume/restart after this failure. + self._session_cleanup_error = error + raise self._session = None self._session_start_deferred_logged = False self._last_step_result = None - logger.info("Isaac Capture session torn down after external XR shutdown") + self._last_control_events = _NO_OP_EVENTS + self._last_control_event_frame_id = None + logger.info("IsaacTeleop session torn down after external XR shutdown") # ------------------------------------------------------------------ # External input building diff --git a/source/isaaclab_teleop/isaaclab_teleop/xr_anchor_manager.py b/source/isaaclab_teleop/isaaclab_teleop/xr_anchor_manager.py index 626c0f803688..55d37f7387ee 100644 --- a/source/isaaclab_teleop/isaaclab_teleop/xr_anchor_manager.py +++ b/source/isaaclab_teleop/isaaclab_teleop/xr_anchor_manager.py @@ -9,28 +9,45 @@ import contextlib import logging +import threading import numpy as np from scipy.spatial.transform import Rotation -from .xr_anchor_utils import XrAnchorSynchronizer +from .xr_anchor_utils import XrAnchorSynchronizer, _XrAnchorPosePolicy from .xr_cfg import XrCfg # Import Kit components with fallback for sessions without Kit -carb = None -with contextlib.suppress(ModuleNotFoundError): - import carb - XRCore = None XRCoreEventType = None with contextlib.suppress(ModuleNotFoundError): from omni.kit.xr.core import XRCore, XRCoreEventType from isaaclab.sim.utils.prims import create_prim as _create_prim +from isaaclab.sim.utils.stage import get_current_stage +from isaaclab.sim.utils.transforms import convert_world_pose_to_local, standardize_xform_ops logger = logging.getLogger(__name__) +def _create_or_update_xr_anchor( + prim_path: str, position: tuple[float, float, float], orientation: tuple[float, float, float, float] +) -> None: + """Apply the configured world pose to a stage-scoped XR anchor.""" + if not _xr_anchor_prim_exists(prim_path): + _create_prim( + prim_path, + prim_type="Xform", + position=np.asarray(position, dtype=np.float64), + orientation=np.asarray(orientation, dtype=np.float64), + ) + return + prim = get_current_stage().GetPrimAtPath(prim_path) + translation, rotation = convert_world_pose_to_local(tuple(position), tuple(orientation), prim.GetParent()) + if not standardize_xform_ops(prim, translation, rotation): + raise ValueError(f"XR anchor {prim_path!r} is not transformable") + + def _xr_anchor_prim_exists(prim_path: str) -> bool: """Return True when ``prim_path`` is already a valid prim on the active stage. @@ -38,16 +55,11 @@ def _xr_anchor_prim_exists(prim_path: str) -> bool: :class:`XrAnchorManager` construction in multi-replay batches (the replay agent rebuilds the device per run, but the prim is stage-scoped). Best-effort: returns ``False`` if the stage cannot - be inspected (e.g. unit tests without omni.usd) so the caller falls + be inspected (e.g. before stage creation) so the caller falls through to the create-and-warn path used historically. """ try: - import omni.usd - - context = omni.usd.get_context() - if context is None: - return False - stage = context.get_stage() + stage = get_current_stage() if stage is None: return False return stage.GetPrimAtPath(prim_path).IsValid() @@ -79,19 +91,36 @@ class XrAnchorManager: dtype=np.float64, ) - def __init__(self, xr_cfg: XrCfg): + def __init__(self, xr_cfg: XrCfg, *, ovxr_session: bool = False): """Initialize the XR anchor manager. - Creates the anchor prim, configures carb XR settings, and sets up - the optional anchor synchronizer for dynamic anchoring. + Creates or refreshes the stage-scoped anchor's configured world pose, + configures carb XR settings, and sets up the optional anchor + synchronizer for dynamic anchoring. Reusing the Stage across sessions + does not retain a previous session's anchor configuration. Args: xr_cfg: XR configuration specifying anchor position, rotation, and optional dynamic anchoring prim path. + ovxr_session: An application-owned renderer/Stage supplies XR. + This path must not configure or synchronize Kit XRCore. + + Raises: + ValueError: An existing anchor prim is not transformable. Other anchor + authoring errors also propagate for OVXR presentation, so + rendering cannot continue with a pose that differs from tracking. """ self._xr_cfg = xr_cfg - self._xr_core = XRCore.get_singleton() if XRCore is not None else None + self._xr_core = XRCore.get_singleton() if XRCore is not None and not ovxr_session else None self._xr_pre_sync_update_subscription = None + self._external_lock = threading.RLock() + self._external_policy = ( + _XrAnchorPosePolicy(xr_cfg) if ovxr_session and xr_cfg.anchor_prim_path is not None else None + ) + self._external_epoch = 0 + self._external_world_matrix = None + self._external_error = None + self._external_render_local = None # Resolve the headset anchor path if self._xr_cfg.anchor_prim_path is not None: @@ -102,26 +131,56 @@ def __init__(self, xr_cfg: XrCfg): else: self._xr_anchor_headset_path = "/World/XRAnchor" - # Create the XR anchor prim in USD if it does not already exist. - # The check matters for multi-replay batches (the replay agent - # rebuilds ``IsaacTeleopDevice`` -- and therefore this manager -- - # for each run, but the prim is stage-scoped and survives - # per-run device teardown). XrCfg.anchor_rot is xyzw; create_prim - # orientation expects xyzw. - if not _xr_anchor_prim_exists(self._xr_anchor_headset_path): - x, y, z, w = self._xr_cfg.anchor_rot + render_anchor_overridden = ( + self._xr_cfg.render_anchor_pos is not None or self._xr_cfg.render_anchor_rot is not None + ) + if render_anchor_overridden: + if self._xr_cfg.anchor_prim_path is not None: + anchor_path = self._xr_cfg.anchor_prim_path.rstrip("/") + self._xr_render_anchor_path = f"{anchor_path}/XRRenderAnchor" + else: + self._xr_render_anchor_path = "/World/XRRenderAnchor" + else: + self._xr_render_anchor_path = self._xr_anchor_headset_path + + # Anchors survive per-run device teardown. Reuse their paths while + # applying this run's configuration, rather than retaining a stale pose. + try: + _create_or_update_xr_anchor(self._xr_anchor_headset_path, self._xr_cfg.anchor_pos, self._xr_cfg.anchor_rot) + except Exception as e: + if ovxr_session: + raise + logger.warning(f"Failed to create or update XR anchor prim: {e}") + + if render_anchor_overridden: + render_pos = ( + self._xr_cfg.anchor_pos if self._xr_cfg.render_anchor_pos is None else self._xr_cfg.render_anchor_pos + ) + render_rot = ( + self._xr_cfg.anchor_rot if self._xr_cfg.render_anchor_rot is None else self._xr_cfg.render_anchor_rot + ) try: - pos = np.asarray(self._xr_cfg.anchor_pos, dtype=np.float64) - quat_xyzw = np.asarray([x, y, z, w], dtype=np.float64) - _create_prim(self._xr_anchor_headset_path, prim_type="Xform", position=pos, orientation=quat_xyzw) + _create_or_update_xr_anchor(self._xr_render_anchor_path, render_pos, render_rot) except Exception as e: - logger.warning(f"Failed to create XR anchor prim: {e}") + if ovxr_session: + raise + logger.warning(f"Failed to create or update XR render anchor prim: {e}") + + if self._external_policy is not None and render_anchor_overridden: + from pxr import UsdGeom + + prim = get_current_stage().GetPrimAtPath(self._xr_render_anchor_path) + self._external_render_local = np.asarray(UsdGeom.Xformable(prim).GetLocalTransformation()).T.copy() + + # Configure settings only when Kit owns a live XRCore. Kitless external- + # present sessions retain this anchor but have no carb settings interface. + if self._xr_core is not None: + import carb.settings # noqa: PLC0415 - # Configure carb settings for XR rendering - if carb is not None and hasattr(carb, "settings"): - carb.settings.get_settings().set_float("/persistent/xr/render/nearPlane", self._xr_cfg.near_plane) - carb.settings.get_settings().set_string("/persistent/xr/anchorMode", "custom anchor") - carb.settings.get_settings().set_string("/xrstage/customAnchor", self._xr_anchor_headset_path) + settings = carb.settings.get_settings() + settings.set_float("/persistent/xr/render/nearPlane", self._xr_cfg.near_plane) + settings.set_string("/persistent/xr/anchorMode", "custom anchor") + settings.set_string("/xrstage/customAnchor", self._xr_render_anchor_path) self._anchor_sync: XrAnchorSynchronizer | None = None if self._xr_core is not None: @@ -156,6 +215,11 @@ def anchor_headset_path(self) -> str: """The USD path of the XR anchor prim.""" return self._xr_anchor_headset_path + @property + def render_anchor_path(self) -> str: + """The USD path consumed by XR rendering.""" + return self._xr_render_anchor_path + def get_world_matrix(self) -> np.ndarray: """Build the combined 4x4 transform from OpenXR local space to Isaac Lab world. @@ -179,19 +243,32 @@ def get_world_matrix(self) -> np.ndarray: core via ``set_world_transform_matrix`` during the most recent ``pre_sync_update``. This works for both dynamic anchoring (``anchor_prim_path`` set) and static anchoring. - * Falls back to raw ``anchor_pos`` / ``anchor_rot`` config values - only when the XR core is unavailable (e.g. unit tests). + * External dynamic sessions read the cached pose sealed by their + frame owner, using application-published physics references. + Unseeded or failed reset epochs raise instead of returning a static pose. + * Static and standalone sessions without Kit retain configured poses. Returns: A (4, 4) float32 numpy array. """ + if self._external_policy is not None: + with self._external_lock: + if self._external_error is not None: + raise RuntimeError( + "Dynamic XR anchor publication failed; recreate the session" + ) from self._external_error + if self._external_world_matrix is None: + raise RuntimeError("Dynamic XR anchor requires a committed post-reset scene pose") + matrix = self._external_world_matrix.copy() + matrix[:3, :3] = matrix[:3, :3] @ self._OXR_TO_USD_ROTATION + return matrix.astype(np.float32) if self._anchor_sync is not None: xform = self._anchor_sync.get_world_transform() if xform is not None: pos, quat_xyzw = xform return self._build_matrix(pos, quat_xyzw) - # Fallback when XR core is unavailable (e.g. unit tests). + # Static and standalone sessions retain their configured-pose fallback. return self._build_matrix(self._xr_cfg.anchor_pos, self._xr_cfg.anchor_rot) def _build_matrix(self, pos, quat_xyzw) -> np.ndarray: @@ -221,14 +298,69 @@ def _build_matrix(self, pos, quat_xyzw) -> np.ndarray: def reset(self) -> None: """Reset the anchor synchronizer state.""" + if self._external_policy is not None: + with self._external_lock: + self._external_epoch += 1 + self._external_policy.reset() + self._external_world_matrix = None if self._anchor_sync is not None: self._anchor_sync.reset() def toggle_anchor_rotation(self) -> None: """Toggle anchor rotation following on the synchronizer.""" + if self._external_policy is not None: + with self._external_lock: + self._external_policy.toggle_anchor_rotation() if self._anchor_sync is not None: self._anchor_sync.toggle_anchor_rotation() + @property + def external_reference_path(self) -> str | None: + """The exact physics reference path, when external dynamic anchoring is active.""" + return self._xr_cfg.anchor_prim_path if self._external_policy is not None else None + + @property + def external_epoch(self) -> tuple[int, bool]: + """Return reset identity and whether the current epoch needs its first publication.""" + with self._external_lock: + if self._external_error is not None: + raise RuntimeError( + "Dynamic XR anchor publication failed; recreate the session" + ) from self._external_error + return self._external_epoch, self._external_world_matrix is None + + @property + def external_anchor_paths(self) -> tuple[str, ...]: + """Native Xforms written together by the OVXR presentation host.""" + return tuple(dict.fromkeys((self._xr_anchor_headset_path, self._xr_render_anchor_path))) + + def commit_external_pose(self, reference, rendering_dt: float, epoch: int, commit) -> bool: + """Advance the shared policy once and expose its input pose only after native seal. + + The host serializes this operation with scene publication. Old committed + references cannot reseed a new reset epoch. A failed write invalidates the + cached input pose and propagates; it never falls back to configured static data. + """ + with self._external_lock: + if self._external_error is not None: + raise RuntimeError( + "Dynamic XR anchor publication failed; recreate the session" + ) from self._external_error + if epoch != self._external_epoch: + return False + try: + matrix = np.asarray(self._external_policy.update(reference, rendering_dt)).T.copy() + matrices = [matrix] + if self._external_render_local is not None: + matrices.append(reference @ self._external_render_local) + commit(np.stack(matrices)) + except BaseException as exc: + self._external_world_matrix = None + self._external_error = exc + raise + self._external_world_matrix = matrix + return True + def cleanup(self) -> None: """Release event subscriptions.""" self._xr_pre_sync_update_subscription = None diff --git a/source/isaaclab_teleop/isaaclab_teleop/xr_anchor_utils.py b/source/isaaclab_teleop/isaaclab_teleop/xr_anchor_utils.py index 6e6671a68e8e..6ef628180b30 100644 --- a/source/isaaclab_teleop/isaaclab_teleop/xr_anchor_utils.py +++ b/source/isaaclab_teleop/isaaclab_teleop/xr_anchor_utils.py @@ -10,12 +10,15 @@ import contextlib import logging import math +from collections.abc import Callable from typing import Any import numpy as np logger = logging.getLogger(__name__) +from pxr import Gf as pxrGf + from isaaclab.sim import SimulationContext from isaaclab.sim.utils.stage import get_current_stage, get_current_stage_id @@ -23,35 +26,134 @@ with contextlib.suppress(ModuleNotFoundError): import usdrt - from pxr import Gf as pxrGf from usdrt import Rt -class XrAnchorSynchronizer: - """Keeps the XR anchor prim aligned with a reference prim according to XR config.""" +class _XrAnchorPosePolicy: + """Own the shared reference-relative anchor policy, without Kit or Stage I/O.""" - def __init__(self, xr_core: Any, xr_cfg: Any, xr_anchor_headset_path: str): - self._xr_core = xr_core + def __init__(self, xr_cfg): self._xr_cfg = xr_cfg - self._xr_anchor_headset_path = xr_anchor_headset_path + self.reset() + def reset(self): self.__anchor_prim_initial_quat = None self.__anchor_prim_initial_height = None self.__smoothed_anchor_quat = None self.__last_anchor_quat = None self.__anchor_rotation_enabled = True + self.__cached_world_pos = None + self.__cached_world_quat_xyzw = None + + def toggle_anchor_rotation(self): + self.__anchor_rotation_enabled = not self.__anchor_rotation_enabled + logger.info("XR: Toggling anchor rotation: %s", self.__anchor_rotation_enabled) + + def get_world_transform(self): + if self.__cached_world_pos is None: + return None + return self.__cached_world_pos.copy(), self.__cached_world_quat_xyzw.copy() + + def update(self, world_T_reference, dt: float | Callable[[], float], previous_headpose=None): + if world_T_reference is not None: + rt_matrix = pxrGf.Matrix4d(world_T_reference.T.tolist()) + rt_pos = rt_matrix.ExtractTranslation() + if self.__anchor_prim_initial_quat is None: + self.__anchor_prim_initial_quat = rt_matrix.ExtractRotationQuat() + if self._xr_cfg.fixed_anchor_height: + if self.__anchor_prim_initial_height is None: + self.__anchor_prim_initial_height = rt_pos[2] + rt_pos[2] = self.__anchor_prim_initial_height + pxr_anchor_pos = rt_pos + pxrGf.Vec3d(*self._xr_cfg.anchor_pos) + else: + rt_matrix = None + pxr_anchor_pos = pxrGf.Vec3d(*self._xr_cfg.anchor_pos) + x, y, z, w = self._xr_cfg.anchor_rot + pxr_cfg_quat = pxrGf.Quatd(w, pxrGf.Vec3d(x, y, z)) + + pxr_anchor_quat = pxr_cfg_quat + + if rt_matrix is not None: + if self._xr_cfg.anchor_rotation_mode in ( + XrAnchorRotationMode.FOLLOW_PRIM, + XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED, + ): + rt_prim_quat = rt_matrix.ExtractRotationQuat() + rt_delta_quat = rt_prim_quat * self.__anchor_prim_initial_quat.GetInverse() + pxr_delta_quat = pxrGf.Quatd(rt_delta_quat.GetReal(), pxrGf.Vec3d(*rt_delta_quat.GetImaginary())) + + # yaw-only about Z (right-handed, Z-up) + wq = pxr_delta_quat.GetReal() + ix, iy, iz = pxr_delta_quat.GetImaginary() + yaw = math.atan2(2.0 * (wq * iz + ix * iy), 1.0 - 2.0 * (iy * iy + iz * iz)) + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + pxr_delta_yaw_only_quat = pxrGf.Quatd(cy, pxrGf.Vec3d(0.0, 0.0, sy)) + pxr_anchor_quat = pxr_delta_yaw_only_quat * pxr_cfg_quat + + if self._xr_cfg.anchor_rotation_mode == XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED: + if self.__smoothed_anchor_quat is None: + self.__smoothed_anchor_quat = pxr_anchor_quat + else: + step_dt = dt() if callable(dt) else dt + alpha = 1.0 - math.exp(-step_dt / max(self._xr_cfg.anchor_rotation_smoothing_time, 1e-6)) + alpha = min(1.0, max(0.05, alpha)) + self.__smoothed_anchor_quat = pxrGf.Slerp(alpha, self.__smoothed_anchor_quat, pxr_anchor_quat) + pxr_anchor_quat = self.__smoothed_anchor_quat + + elif self._xr_cfg.anchor_rotation_mode == XrAnchorRotationMode.CUSTOM: + if self._xr_cfg.anchor_rotation_custom_func is not None: + rt_prim_quat = rt_matrix.ExtractRotationQuat() + anchor_prim_pose = np.array( + [ + rt_pos[0], + rt_pos[1], + rt_pos[2], + rt_prim_quat.GetImaginary()[0], + rt_prim_quat.GetImaginary()[1], + rt_prim_quat.GetImaginary()[2], + rt_prim_quat.GetReal(), + ], + dtype=np.float64, + ) + prev_head = previous_headpose if previous_headpose is not None else np.zeros(7, dtype=np.float64) + np_array_quat = self._xr_cfg.anchor_rotation_custom_func(prev_head, anchor_prim_pose) + x, y, z, w = np_array_quat + pxr_anchor_quat = pxrGf.Quatd(w, pxrGf.Vec3d(x, y, z)) + + pxr_mat = pxrGf.Matrix4d() + pxr_mat.SetTranslateOnly(pxr_anchor_pos) + + if self.__anchor_rotation_enabled: + pxr_final_quat = pxr_anchor_quat + self.__last_anchor_quat = pxr_anchor_quat + else: + if self.__last_anchor_quat is None: + self.__last_anchor_quat = pxr_anchor_quat + pxr_final_quat = self.__last_anchor_quat + self.__smoothed_anchor_quat = self.__last_anchor_quat + + pxr_mat.SetRotateOnly(pxr_final_quat) - # Cached anchor world transform (pos, quat_xyzw) set by sync_headset_to_anchor(). - # Reading back from the prim hierarchy is unreliable when the anchor is a child of a - # physics-driven prim (e.g. the pelvis) because Fabric computes the hierarchy world - # matrix using the physics-updated parent while the local xform was decomposed against - # the USD-level parent, which can diverge. - self.__cached_world_pos: np.ndarray | None = None - self.__cached_world_quat_xyzw: np.ndarray | None = None + self.__cached_world_pos = np.array([pxr_anchor_pos[0], pxr_anchor_pos[1], pxr_anchor_pos[2]], dtype=np.float64) + fq_img = pxr_final_quat.GetImaginary() + self.__cached_world_quat_xyzw = np.array( + [fq_img[0], fq_img[1], fq_img[2], pxr_final_quat.GetReal()], dtype=np.float64 + ) + return pxr_mat + + +class XrAnchorSynchronizer: + """Adapt the shared anchor policy to Kit Fabric and XRCore pre-sync events.""" + def __init__(self, xr_core: Any, xr_cfg: Any, xr_anchor_headset_path: str): + self._xr_core = xr_core + self._xr_cfg = xr_cfg + self._xr_anchor_headset_path = xr_anchor_headset_path + self._policy = _XrAnchorPosePolicy(xr_cfg) stage = get_current_stage() anchor = stage.GetPrimAtPath(xr_anchor_headset_path) - self.__anchor_headset_layer_identifier = anchor.GetPrimStack()[0].layer.identifier + self._layer_identifier = anchor.GetPrimStack()[0].layer.identifier self._reference_world_matrix = None if xr_cfg.anchor_prim_path is not None: rt_stage = usdrt.Usd.Stage.Attach(get_current_stage_id()) @@ -61,149 +163,27 @@ def __init__(self, xr_core: Any, xr_cfg: Any, xr_anchor_headset_path: str): self._reference_world_matrix = Rt.Xformable(reference).GetFabricHierarchyWorldMatrixAttr() def reset(self): - self.__anchor_prim_initial_quat = None - self.__anchor_prim_initial_height = None - self.__smoothed_anchor_quat = None - self.__last_anchor_quat = None - self.__anchor_rotation_enabled = True - self.__cached_world_pos = None - self.__cached_world_quat_xyzw = None + self._policy.reset() self.sync_headset_to_anchor() def toggle_anchor_rotation(self): - self.__anchor_rotation_enabled = not self.__anchor_rotation_enabled - logger.info(f"XR: Toggling anchor rotation: {self.__anchor_rotation_enabled}") - - def get_world_transform(self) -> tuple[np.ndarray, np.ndarray] | None: - """Return the anchor world transform. - - Returns the cached world transform that was computed by the most recent - call to :meth:`sync_headset_to_anchor`. Using the cached value avoids - a Fabric/USD layer mismatch: when the XR anchor prim is a child of a - physics-driven prim (e.g. the robot pelvis), reading - ``GetFabricHierarchyWorldMatrixAttr`` would compose the Fabric-side - parent transform (updated by physics) with a local xform that was - decomposed against the USD-side parent (which can lag behind), - producing an incorrect world matrix that drifts as the robot moves. - - Returns: - A ``(position, quat_xyzw)`` tuple of numpy float64 arrays, - or ``None`` if :meth:`sync_headset_to_anchor` has not run yet. - """ - if self.__cached_world_pos is not None and self.__cached_world_quat_xyzw is not None: - return self.__cached_world_pos, self.__cached_world_quat_xyzw - return None + self._policy.toggle_anchor_rotation() + + def get_world_transform(self): + return self._policy.get_world_transform() def sync_headset_to_anchor(self): - """Sync XR anchor pose in USD for both dynamic and static anchoring. - - For **dynamic** anchoring (``anchor_prim_path`` is set), the reference - prim's world position is read from Fabric and ``anchor_pos`` is added - as an offset. For **static** anchoring (no prim path), ``anchor_pos`` - is used directly as the world position. - - In both cases the function calls ``set_world_transform_matrix`` on the - XR core so that the rendering anchor and the pipeline's - ``world_T_anchor`` matrix are guaranteed to agree, and caches the - world transform for :meth:`get_world_transform`. - """ + """Read the Fabric reference and update the Kit anchor once per pre-sync.""" try: + reference = None if self._reference_world_matrix is not None: - rt_matrix = self._reference_world_matrix.Get() - rt_pos = rt_matrix.ExtractTranslation() - - if self.__anchor_prim_initial_quat is None: - self.__anchor_prim_initial_quat = rt_matrix.ExtractRotationQuat() - - if self._xr_cfg.fixed_anchor_height: - if self.__anchor_prim_initial_height is None: - self.__anchor_prim_initial_height = rt_pos[2] - rt_pos[2] = self.__anchor_prim_initial_height - - pxr_anchor_pos = pxrGf.Vec3d(*rt_pos) + pxrGf.Vec3d(*self._xr_cfg.anchor_pos) - else: - rt_matrix = None - pxr_anchor_pos = pxrGf.Vec3d(*self._xr_cfg.anchor_pos) - - x, y, z, w = self._xr_cfg.anchor_rot - pxr_cfg_quat = pxrGf.Quatd(w, pxrGf.Vec3d(x, y, z)) - - pxr_anchor_quat = pxr_cfg_quat - - if rt_matrix is not None: - if self._xr_cfg.anchor_rotation_mode in ( - XrAnchorRotationMode.FOLLOW_PRIM, - XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED, - ): - rt_prim_quat = rt_matrix.ExtractRotationQuat() - rt_delta_quat = rt_prim_quat * self.__anchor_prim_initial_quat.GetInverse() - pxr_delta_quat = pxrGf.Quatd(rt_delta_quat.GetReal(), pxrGf.Vec3d(*rt_delta_quat.GetImaginary())) - - # yaw-only about Z (right-handed, Z-up) - wq = pxr_delta_quat.GetReal() - ix, iy, iz = pxr_delta_quat.GetImaginary() - yaw = math.atan2(2.0 * (wq * iz + ix * iy), 1.0 - 2.0 * (iy * iy + iz * iz)) - cy = math.cos(yaw * 0.5) - sy = math.sin(yaw * 0.5) - pxr_delta_yaw_only_quat = pxrGf.Quatd(cy, pxrGf.Vec3d(0.0, 0.0, sy)) - pxr_anchor_quat = pxr_delta_yaw_only_quat * pxr_cfg_quat - - if self._xr_cfg.anchor_rotation_mode == XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED: - if self.__smoothed_anchor_quat is None: - self.__smoothed_anchor_quat = pxr_anchor_quat - else: - dt = SimulationContext.instance().get_rendering_dt() - alpha = 1.0 - math.exp(-dt / max(self._xr_cfg.anchor_rotation_smoothing_time, 1e-6)) - alpha = min(1.0, max(0.05, alpha)) - self.__smoothed_anchor_quat = pxrGf.Slerp( - alpha, self.__smoothed_anchor_quat, pxr_anchor_quat - ) - pxr_anchor_quat = self.__smoothed_anchor_quat - - elif self._xr_cfg.anchor_rotation_mode == XrAnchorRotationMode.CUSTOM: - if self._xr_cfg.anchor_rotation_custom_func is not None: - rt_prim_quat = rt_matrix.ExtractRotationQuat() - anchor_prim_pose = np.array( - [ - rt_pos[0], - rt_pos[1], - rt_pos[2], - rt_prim_quat.GetImaginary()[0], - rt_prim_quat.GetImaginary()[1], - rt_prim_quat.GetImaginary()[2], - rt_prim_quat.GetReal(), - ], - dtype=np.float64, - ) - prev_head = getattr(self, "_previous_headpose", np.zeros(7, dtype=np.float64)) - np_array_quat = self._xr_cfg.anchor_rotation_custom_func(prev_head, anchor_prim_pose) - x, y, z, w = np_array_quat - pxr_anchor_quat = pxrGf.Quatd(w, pxrGf.Vec3d(x, y, z)) - - pxr_mat = pxrGf.Matrix4d() - pxr_mat.SetTranslateOnly(pxr_anchor_pos) - - if self.__anchor_rotation_enabled: - pxr_final_quat = pxr_anchor_quat - self.__last_anchor_quat = pxr_anchor_quat - else: - if self.__last_anchor_quat is None: - self.__last_anchor_quat = pxr_anchor_quat - pxr_final_quat = self.__last_anchor_quat - self.__smoothed_anchor_quat = self.__last_anchor_quat - - pxr_mat.SetRotateOnly(pxr_final_quat) - - self.__cached_world_pos = np.array( - [pxr_anchor_pos[0], pxr_anchor_pos[1], pxr_anchor_pos[2]], dtype=np.float64 - ) - fq_img = pxr_final_quat.GetImaginary() - self.__cached_world_quat_xyzw = np.array( - [fq_img[0], fq_img[1], fq_img[2], pxr_final_quat.GetReal()], dtype=np.float64 - ) - - self._xr_core.set_world_transform_matrix( - self._xr_anchor_headset_path, pxr_mat, self.__anchor_headset_layer_identifier + matrix = self._reference_world_matrix.Get() + reference = np.array([[matrix[i][j] for j in range(4)] for i in range(4)]).T + matrix = self._policy.update( + reference, + lambda: SimulationContext.instance().get_rendering_dt(), + getattr(self, "_previous_headpose", None), ) - except Exception as e: - logger.warning(f"XR: Anchor sync failed: {e}") + self._xr_core.set_world_transform_matrix(self._xr_anchor_headset_path, matrix, self._layer_identifier) + except Exception as exc: + logger.warning("XR: Anchor sync failed: %s", exc) diff --git a/source/isaaclab_teleop/isaaclab_teleop/xr_cfg.py b/source/isaaclab_teleop/isaaclab_teleop/xr_cfg.py index 7382f8570252..9df0db37e1dc 100644 --- a/source/isaaclab_teleop/isaaclab_teleop/xr_cfg.py +++ b/source/isaaclab_teleop/isaaclab_teleop/xr_cfg.py @@ -51,6 +51,40 @@ class XrCfg: This quantity is only effective if :attr:`xr_anchor_pos` is set. """ + render_anchor_pos: tuple[float, float, float] | None = None + """Optional position (in m) used only to frame the XR view. + + When ``None``, rendering and tracked-device poses share :attr:`anchor_pos`, + which preserves the historical behavior. Set this when the operator view + needs a different origin from the world frame used for teleoperation. + """ + + render_anchor_rot: tuple[float, float, float, float] | None = None + """Optional render-only rotation as a quaternion in ``xyzw`` order. + + When ``None``, the render anchor uses :attr:`anchor_rot`. This field is + effective only when either render-anchor override is configured. + """ + + hidden_visual_prims: list[str] = [] + """Prim-path regex patterns hidden from the kitless OVXR view. + + Tasks can use this to hide robot geometry that encloses the eye cameras. + The OVXR session authors visibility on the live stage before + renderer materialization and fails if a configured pattern matches no + imageable prim. Visibility persists for the application Stage's lifetime + and affects all cameras, including PiP; it is not restored on XR close. + """ + + excluded_visual_meshes: list[str] = [] + """Mesh-path regexes excluded from kitless environment cameras by scene partition. + + Unlike ``hidden_visual_prims``, this leaves USD visibility and the source + scene unchanged. Only matching visual meshes in the single-environment + render export enter the ``xr_excluded`` partition. Camera selectors remain + independent; collisions and link/camera transforms must not be matched. + """ + anchor_prim_path: str | None = None """Specifies the prim path to attach the XR anchor to for dynamic positioning. @@ -58,6 +92,10 @@ class XrCfg: allowing the XR camera to move with the prim. This is particularly useful for locomotion robot teleoperation where the robot moves and the XR camera should follow it. + Kit sessions use XRCore to follow the reference. Application-owned OVXR presentation + uses committed physics references for the same rotation and height policies. Input cannot + consume an unseeded or failed publication epoch; no static pose is substituted. + If None, the anchor will use the static :attr:`anchor_pos` and :attr:`anchor_rot` values. """ @@ -109,7 +147,7 @@ class XrCfg: from typing import Any -def remove_camera_configs(env_cfg: Any) -> Any: +def remove_camera_configs(env_cfg: Any, *, keep: tuple[str, ...] = ()) -> Any: """Removes cameras from environments when using XR devices. Having additional cameras cause operation performance issues. This function scans the environment @@ -118,6 +156,9 @@ def remove_camera_configs(env_cfg: Any) -> Any: Args: env_cfg: The environment configuration to modify. + keep: Camera attribute names to retain for the OVXR renderer. + Observation terms are removed for retained cameras as well, so the + application loop never requests a second camera render. Returns: The modified environment configuration with cameras removed. @@ -134,8 +175,9 @@ def remove_camera_configs(env_cfg: Any) -> Any: for attr_name in dir(env_cfg.scene): attr = getattr(env_cfg.scene, attr_name) if isinstance(attr, CameraCfg): - delattr(env_cfg.scene, attr_name) - logger.info(f"Removed camera config: {attr_name}") + if attr_name not in keep: + delattr(env_cfg.scene, attr_name) + logger.info(f"Removed camera config: {attr_name}") # Remove any ObsTerms for the camera if hasattr(env_cfg.observations, "policy"): diff --git a/source/isaaclab_teleop/isaaclab_teleop/xr_scene_partition.py b/source/isaaclab_teleop/isaaclab_teleop/xr_scene_partition.py new file mode 100644 index 000000000000..79eff4aba1d3 --- /dev/null +++ b/source/isaaclab_teleop/isaaclab_teleop/xr_scene_partition.py @@ -0,0 +1,73 @@ +# 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 + +"""Camera-selective visual exclusions for the single-environment XR export.""" + +import re +from contextlib import contextmanager + + +@contextmanager +def partition_visual_meshes(stage, patterns): + """Exclude matching visual meshes from environment cameras during export. + + A temporary session sublayer is flattened into the borrowed render Stage. + The application's source layers, visibility, physics and cameras are not + changed. Patterns match mesh paths, including instance proxies, not links. + """ + if not patterns: + yield () + return + + from pxr import Sdf, Usd, UsdGeom, UsdPhysics + + envs = stage.GetPrimAtPath("/World/envs") + if not envs or [str(p.GetPath()) for p in envs.GetChildren()] != ["/World/envs/env_0"]: + raise ValueError("XR visual partitions require exactly env_0") + expressions = [re.compile(pattern) for pattern in patterns] + matches = [[] for _ in expressions] + for prim in Usd.PrimRange(envs, Usd.TraverseInstanceProxies()): + if not prim.IsA(UsdGeom.Mesh): + continue + for index, expression in enumerate(expressions): + if expression.fullmatch(str(prim.GetPath())): + if prim.HasAPI(UsdPhysics.CollisionAPI) or "collisions" in prim.GetPath().pathString.split("/"): + raise ValueError(f"XR visual exclusion matched collision geometry: {prim.GetPath()}") + matches[index].append(str(prim.GetPath())) + if any(not paths for paths in matches): + raise ValueError("XR visual exclusion pattern matched no visual mesh") + paths = tuple(dict.fromkeys(path for group in matches for path in group)) + layer = Sdf.Layer.CreateAnonymous("xr-visual-partitions.usda") + session = stage.GetSessionLayer() + session.subLayerPaths.insert(0, layer.identifier) + try: + with Usd.EditContext(stage, layer): + for path in paths: + while stage.GetPrimAtPath(path).IsInstanceProxy(): + parent = stage.GetPrimAtPath(path).GetParent() + while parent and not parent.IsInstance(): + parent = parent.GetParent() + if not parent: + raise RuntimeError(f"Cannot author XR visual partition: {path}") + parent.SetInstanceable(False) + if parent.IsInstance(): + raise RuntimeError(f"Stronger source opinion prevents XR deinstancing: {parent.GetPath()}") + partition = UsdGeom.PrimvarsAPI(stage.GetPrimAtPath(path)).CreatePrimvar( + "omni:scenePartition", Sdf.ValueTypeNames.Token, UsdGeom.Tokens.constant + ) + partition.Set("xr_excluded") + if partition.Get() != "xr_excluded": + raise RuntimeError(f"Stronger source opinion prevents XR visual partition: {path}") + settings_path = stage.GetMetadata("renderSettingsPrimPath") or "/Render/IsaacLabSettings" + settings = stage.DefinePrim(settings_path, "RenderSettings") + show_all = settings.CreateAttribute( + "omni:rtx:scenePartitioning:showAllPartitionsByDefault", Sdf.ValueTypeNames.Bool + ) + show_all.Set(False) + if show_all.Get() is not False: + raise RuntimeError("Stronger source opinion prevents XR camera-selective partitions") + yield paths + finally: + session.subLayerPaths.remove(layer.identifier) diff --git a/source/isaaclab_teleop/test/test_cloudxr_lifecycle.py b/source/isaaclab_teleop/test/test_cloudxr_lifecycle.py index 15513137ab3c..16f4dd7deb8c 100644 --- a/source/isaaclab_teleop/test/test_cloudxr_lifecycle.py +++ b/source/isaaclab_teleop/test/test_cloudxr_lifecycle.py @@ -151,6 +151,30 @@ def _make_lifecycle( ) +@pytest.mark.parametrize("entry", ["lifecycle", "device", "factory"]) +def test_positional_recording_options_preserve_public_constructor_contract(entry): + """Existing positional MCAP/debug/haptic arguments must not become XR handles. + + Exercise all three public argument-binding boundaries without opening an XR + session. Native recording/playback correctness has a separate owner. + """ + from isaaclab_teleop.isaac_teleop_device import IsaacTeleopDevice, create_isaac_teleop_device + + cfg = _make_cfg() + if entry == "lifecycle": + lifecycle = TeleopSessionLifecycle(cfg, None, False, False, "capture.mcap", None, True, None) + elif entry == "device": + device = IsaacTeleopDevice(cfg, None, False, False, "capture.mcap", None, True, None) + lifecycle = device._session_lifecycle + else: + device = create_isaac_teleop_device(cfg, "cpu", None, None, False, False, "capture.mcap", None, True, None) + lifecycle = device._session_lifecycle + assert lifecycle._mcap_record_path == "capture.mcap" + assert lifecycle._mcap_replay_path is None + assert lifecycle._enable_debug_visualization is True + assert lifecycle._external_oxr_handles_provider is None + + # ============================================================================ # Shipped .env profile paths # ============================================================================ @@ -209,6 +233,172 @@ def test_session_config_receives_deadline_paced_pipelined_retargeting(self): assert resolved_execution.pacing.safety_margin_s == 0.025 +class TestExternalOpenXRHandles: + """Tests for borrowing OVXR-owned OpenXR handles without the Kit bridge.""" + + def test_provider_handles_are_passed_to_teleop_session(self): + handles = object() + lifecycle = TeleopSessionLifecycle( + _make_cfg(), + use_kit_xr_bridge=False, + external_oxr_handles_provider=lambda: handles, + ) + lifecycle._pipeline = MagicMock() + lifecycle._teleop_control_pipeline = None + session_config_cls = MagicMock(return_value=MagicMock()) + session_cls = MagicMock() + fake_tsm_module = sys.modules["isaacteleop.teleop_session_manager"] + + with ( + patch.object(fake_tsm_module, "TeleopSessionConfig", session_config_cls), + patch.object(fake_tsm_module, "TeleopSession", session_cls), + ): + assert lifecycle.try_start_session() is True + + assert session_config_cls.call_args.kwargs["oxr_handles"] is handles + + def test_provider_none_defers_session_creation(self): + lifecycle = TeleopSessionLifecycle( + _make_cfg(), + use_kit_xr_bridge=False, + external_oxr_handles_provider=lambda: None, + ) + lifecycle._pipeline = MagicMock() + lifecycle._teleop_control_pipeline = None + + assert lifecycle.try_start_session() is False + assert lifecycle.is_active is False + + +@pytest.mark.parametrize("replay", [False, True]) +@pytest.mark.parametrize("rollback_fails", [False, True]) +@pytest.mark.parametrize("error_type", [RuntimeError, KeyboardInterrupt]) +def test_failed_session_entry_is_not_published_as_active(tmp_path, monkeypatch, replay, rollback_fails, error_type): + recording = tmp_path / "recording.mcap" + recording.touch() + replay_module = ModuleType("isaacteleop.deviceio_session") + replay_module.McapReplayConfig = MagicMock() + monkeypatch.setitem(sys.modules, replay_module.__name__, replay_module) + lifecycle = TeleopSessionLifecycle( + _make_cfg(), + use_kit_xr_bridge=False, + mcap_replay_path=str(recording) if replay else None, + ) + lifecycle._pipeline = MagicMock() + startup_error = error_type("session entry failed") + session_cls = MagicMock() + session_cls.return_value.cleanup_failed = rollback_fails + session_cls.return_value.__enter__.side_effect = startup_error + fake_tsm_module = sys.modules["isaacteleop.teleop_session_manager"] + with patch.object(fake_tsm_module, "TeleopSession", session_cls): + with pytest.raises(error_type) as caught: + lifecycle.try_start_session() + assert caught.value is startup_error + assert not lifecycle.is_active + if rollback_fails: + assert lifecycle._session is session_cls.return_value + assert lifecycle.session_cleanup_failed + for operation in (lifecycle.try_start_session, lifecycle.step, lifecycle.stop): + with pytest.raises(error_type) as repeated: + operation() + assert repeated.value is startup_error + assert session_cls.call_count == 1 + session_cls.return_value.__exit__.assert_not_called() + else: + assert lifecycle._session is None + session_cls.return_value.__enter__.side_effect = None + assert lifecycle.try_start_session() + assert session_cls.call_count == 2 + assert lifecycle._session is session_cls.return_value + + +def test_device_failed_entry_stops_partial_session(): + """Python does not call __exit__ when __enter__ fails after runtime launch.""" + from isaaclab_teleop.isaac_teleop_device import IsaacTeleopDevice + + startup_error = RuntimeError("pipeline construction failed") + cfg = _make_cfg() + cfg.pipeline_builder = MagicMock(side_effect=startup_error) + device = IsaacTeleopDevice(cfg, cloudxr_env_file="test.env", use_kit_xr_bridge=False) + launcher_class = MagicMock() + launcher = launcher_class.return_value + with ( + patch.object(device._session_lifecycle, "_run_system_check"), + patch.object(sys.modules["isaacteleop.cloudxr"], "CloudXRLauncher", launcher_class), + patch.dict(os.environ, {"ISAACLAB_CXR_SKIP_AUTOLAUNCH": "0"}), + pytest.raises(RuntimeError) as caught, + ): + with device: + pytest.fail("entry must fail") + assert caught.value is startup_error + launcher.stop.assert_called_once() + assert device._session_lifecycle._cloudxr_launcher is None + + +@pytest.mark.parametrize("body_error", [False, True]) +def test_device_anchor_cleanup_does_not_skip_session_stop(body_error): + from isaaclab_teleop.isaac_teleop_device import IsaacTeleopDevice + + device = IsaacTeleopDevice(_make_cfg(), use_kit_xr_bridge=False) + cleanup_error = RuntimeError("anchor subscription cleanup failed") + primary = ValueError("body failed") if body_error else None + device._anchor_manager.cleanup = MagicMock(side_effect=cleanup_error) + device._session_lifecycle.stop = MagicMock() + try: + if primary is None: + with pytest.raises(RuntimeError) as caught: + device.__exit__(None, None, None) + assert caught.value is cleanup_error + else: + assert device.__exit__(type(primary), primary, None) is False + assert any("anchor subscription" in note for note in primary.__notes__) + device._session_lifecycle.stop.assert_called_once() + finally: + device._anchor_manager.cleanup = MagicMock() + + +@pytest.mark.parametrize("error_type", [RuntimeError, KeyboardInterrupt]) +def test_ui_cleanup_failure_still_drains_session_and_runtime(error_type): + lifecycle = _make_lifecycle() + cleanup_error = error_type("UI cleanup failed") + lifecycle._retargeting_ui_ctx = MagicMock() + lifecycle._retargeting_ui_ctx.__exit__.side_effect = cleanup_error + session, launcher = MagicMock(), MagicMock() + lifecycle._session, lifecycle._cloudxr_launcher = session, launcher + with pytest.raises(error_type) as caught: + lifecycle.stop() + assert caught.value is cleanup_error + session.__exit__.assert_called_once() + launcher.stop.assert_called_once() + assert lifecycle._session is None + assert lifecycle._cloudxr_launcher is None + + +@pytest.mark.parametrize("body_error", [False, True]) +@pytest.mark.parametrize("error_type", [RuntimeError, KeyboardInterrupt]) +def test_session_cleanup_failure_is_visible_and_retains_runtime(body_error, error_type): + lifecycle = _make_lifecycle() + session, launcher = MagicMock(), MagicMock() + cleanup_error = error_type("native drain failed") + primary = ValueError("body failed") if body_error else None + session.__exit__.side_effect = cleanup_error + lifecycle._session, lifecycle._cloudxr_launcher = session, launcher + if primary is None: + with pytest.raises(error_type) as caught: + lifecycle.stop() + assert caught.value is cleanup_error + else: + try: + lifecycle.stop(type(primary), primary, None) + except BaseException as error: + pytest.fail(f"Cleanup replaced the original body failure with {error!r}") + assert any("native drain failed" in note for note in primary.__notes__) + assert lifecycle._session is session + assert lifecycle.session_cleanup_failed + assert lifecycle._cloudxr_launcher is launcher + launcher.stop.assert_not_called() + + # ============================================================================ # _ensure_cloudxr_runtime # ============================================================================ @@ -382,7 +572,8 @@ def test_stop_retains_launcher_on_runtime_error(self): lifecycle, mock_launcher = self._make_started_lifecycle() mock_launcher.stop.side_effect = RuntimeError("process not found") - lifecycle.stop() + with pytest.raises(RuntimeError, match="process not found"): + lifecycle.stop() assert lifecycle._cloudxr_launcher is mock_launcher diff --git a/source/isaaclab_teleop/test/test_control_events.py b/source/isaaclab_teleop/test/test_control_events.py index dafe7a2ac18c..1eb331bc8249 100644 --- a/source/isaaclab_teleop/test/test_control_events.py +++ b/source/isaaclab_teleop/test/test_control_events.py @@ -18,83 +18,17 @@ import dataclasses import json -import sys -from types import ModuleType +from types import SimpleNamespace from unittest.mock import MagicMock import pytest -# --------------------------------------------------------------------------- -# Stub out isaacteleop modules before any isaaclab_teleop imports so the -# tests can run in a plain Python environment without Omniverse. -# --------------------------------------------------------------------------- - -_MODULES_TO_STUB = [ - "isaacteleop", - "isaacteleop.deviceio", - "isaacteleop.deviceio_trackers", - "isaacteleop.retargeting_engine", - "isaacteleop.retargeting_engine.deviceio_source_nodes", - "isaacteleop.retargeting_engine.deviceio_source_nodes.deviceio_tensor_types", - "isaacteleop.retargeting_engine.interface", - "isaacteleop.retargeting_engine.interface.retargeter_core_types", - "isaacteleop.retargeting_engine.interface.tensor_group_type", - "isaacteleop.retargeting_engine_ui", - "isaacteleop.schema", - "isaacteleop.teleop_session_manager", - "isaacteleop.teleop_session_manager.teleop_state_manager_retargeter", - "isaacteleop.teleop_session_manager.teleop_state_manager_types", -] - -_stubs: dict[str, ModuleType | MagicMock] = {} - - -def _install_stubs(): - for name in _MODULES_TO_STUB: - if name not in sys.modules: - _stubs[name] = MagicMock() - sys.modules[name] = _stubs[name] - - from enum import Enum - - class ExecutionState(str, Enum): - UNKNOWN = "unknown" - STOPPED = "stopped" - PAUSED = "paused" - RUNNING = "running" - - @dataclasses.dataclass - class ExecutionEvents: - reset: bool = False - execution_state: ExecutionState = ExecutionState.UNKNOWN - - ee_mod = sys.modules["isaacteleop.retargeting_engine.interface.execution_events"] = ModuleType( - "isaacteleop.retargeting_engine.interface.execution_events" - ) - ee_mod.ExecutionState = ExecutionState # type: ignore[attr-defined] - ee_mod.ExecutionEvents = ExecutionEvents # type: ignore[attr-defined] - - iface = sys.modules["isaacteleop.retargeting_engine.interface"] - iface.ExecutionState = ExecutionState # type: ignore[attr-defined] - iface.ExecutionEvents = ExecutionEvents # type: ignore[attr-defined] - iface.RetargeterIOType = dict # type: ignore[attr-defined] - - class FakeBaseRetargeter: - def __init__(self, name: str) -> None: - self.name = name - - iface.BaseRetargeter = FakeBaseRetargeter # type: ignore[attr-defined] - - tsm_types = sys.modules["isaacteleop.teleop_session_manager.teleop_state_manager_types"] - tsm_types.bool_signal = MagicMock # type: ignore[attr-defined] - - dt_mod = sys.modules["isaacteleop.retargeting_engine.deviceio_source_nodes.deviceio_tensor_types"] - dt_mod.MessageChannelMessagesTrackedGroup = MagicMock # type: ignore[attr-defined] - - -_install_stubs() +# Capture's retargeting API is usable without starting a simulator. Use the real +# optional dependency so this module cannot replace packages seen by other tests. +pytest.importorskip("isaacteleop.retargeting_engine.interface") from isaaclab_teleop.control_events import ControlEvents, poll_control_events # noqa: E402 +from isaaclab_teleop.session_lifecycle import TeleopSessionLifecycle # noqa: E402 from isaaclab_teleop.teleop_message_processor import ( # noqa: E402 TeleopMessageProcessor, _classify_command, @@ -535,3 +469,29 @@ def last_control_events(self): result = poll_control_events(FakeDevice()) assert result.is_active is False assert result.should_reset is True + + +class TestPipelinedControlEvents: + def test_reset_edge_is_emitted_once_per_returned_frame(self): + from isaacteleop.retargeting_engine.interface.execution_events import ExecutionEvents, ExecutionState + + lifecycle = TeleopSessionLifecycle.__new__(TeleopSessionLifecycle) + lifecycle._message_processor = object() + lifecycle._last_control_event_frame_id = None + lifecycle._last_control_events = ControlEvents() + lifecycle._session = SimpleNamespace( + last_context=SimpleNamespace( + execution_events=ExecutionEvents(reset=True, execution_state=ExecutionState.RUNNING) + ), + last_step_info=SimpleNamespace(returned_frame_id=7), + ) + + lifecycle._update_control_events() + assert lifecycle.last_control_events == ControlEvents(is_active=True, should_reset=True) + + lifecycle._update_control_events() + assert lifecycle.last_control_events == ControlEvents(is_active=True, should_reset=False) + + lifecycle._session.last_step_info.returned_frame_id = 8 + lifecycle._update_control_events() + assert lifecycle.last_control_events == ControlEvents(is_active=True, should_reset=True) diff --git a/source/isaaclab_teleop/test/test_hand_debug_viz.py b/source/isaaclab_teleop/test/test_hand_debug_viz.py index 15e22b494ff9..e41bebc7820a 100644 --- a/source/isaaclab_teleop/test/test_hand_debug_viz.py +++ b/source/isaaclab_teleop/test/test_hand_debug_viz.py @@ -7,7 +7,7 @@ """Tests for hand joint debug visualization plumbing. -Covers :meth:`TeleopSessionLifecycle._chain_hand_debug_outputs` (HandsSource +Covers :meth:`TeleopSessionLifecycle._chain_hand_presence_outputs` (HandsSource auto-discovery), :func:`~isaaclab_teleop.visualizers.hand_joint_visualizer._extract_world_joint_positions`, and :func:`~isaaclab_teleop.visualizers.controller_aim_visualizer._aim_world_pose` @@ -21,7 +21,6 @@ from __future__ import annotations import sys -from types import ModuleType from unittest.mock import MagicMock, patch import numpy as np @@ -64,7 +63,7 @@ "omni.kit.xr.system.openxr", ] -_stubs_installed: dict[str, ModuleType | MagicMock] = {} +_stub_patch: pytest.MonkeyPatch | None = None class _FakeHandsSource: @@ -122,41 +121,37 @@ def __init__(self, name: str) -> None: def _install_stubs(): - """Insert MagicMock modules for all heavy dependencies.""" + """Temporarily replace dependencies, including attributes on already loaded modules.""" + global _stub_patch + assert _stub_patch is None + _stub_patch = pytest.MonkeyPatch() for name in _MODULES_TO_STUB: if name not in sys.modules: - sys.modules[name] = _stubs_installed.setdefault(name, MagicMock()) + _stub_patch.setitem(sys.modules, name, MagicMock()) if "." in name: parent_name, child_name = name.rsplit(".", 1) - setattr(sys.modules[parent_name], child_name, sys.modules[name]) + _stub_patch.setattr(sys.modules[parent_name], child_name, sys.modules[name], raising=False) dsn_mod = sys.modules["isaacteleop.retargeting_engine.deviceio_source_nodes"] - dsn_mod.HandsSource = _FakeHandsSource # type: ignore[attr-defined] - dsn_mod.ControllersSource = _FakeControllersSource # type: ignore[attr-defined] + _stub_patch.setattr(dsn_mod, "HandsSource", _FakeHandsSource, raising=False) + _stub_patch.setattr(dsn_mod, "ControllersSource", _FakeControllersSource, raising=False) iface_mod = sys.modules["isaacteleop.retargeting_engine.interface"] - iface_mod.OutputCombiner = _FakeOutputCombiner # type: ignore[attr-defined] - iface_mod.BaseRetargeter = _FakeBaseRetargeter # type: ignore[attr-defined] - iface_mod.RetargeterIOType = dict # type: ignore[attr-defined] + _stub_patch.setattr(iface_mod, "OutputCombiner", _FakeOutputCombiner, raising=False) + _stub_patch.setattr(iface_mod, "BaseRetargeter", _FakeBaseRetargeter, raising=False) + _stub_patch.setattr(iface_mod, "RetargeterIOType", dict, raising=False) tt_mod = sys.modules["isaacteleop.retargeting_engine.tensor_types"] - tt_mod.HandInputIndex = _FakeHandInputIndex # type: ignore[attr-defined] - tt_mod.ControllerInputIndex = _FakeControllerInputIndex # type: ignore[attr-defined] + _stub_patch.setattr(tt_mod, "HandInputIndex", _FakeHandInputIndex, raising=False) + _stub_patch.setattr(tt_mod, "ControllerInputIndex", _FakeControllerInputIndex, raising=False) def _restore_stubs(): - """Remove stubs installed for this test module from ``sys.modules``.""" - for name in reversed(_MODULES_TO_STUB): - stub = _stubs_installed.get(name) - if stub is None: - continue - if "." in name: - parent_name, child_name = name.rsplit(".", 1) - parent = sys.modules.get(parent_name) - if parent is not None and getattr(parent, child_name, None) is stub: - delattr(parent, child_name) - if sys.modules.get(name) is stub: - del sys.modules[name] + """Restore both inserted modules and overwritten attributes exactly.""" + global _stub_patch + if _stub_patch is not None: + _stub_patch.undo() + _stub_patch = None _install_stubs() @@ -192,8 +187,9 @@ def __init__(self, name: str) -> None: class _FakePipeline: """Stand-in for a user pipeline (leaf nodes + action output).""" - def __init__(self, leaves: list) -> None: + def __init__(self, leaves: list, outputs: tuple[str, ...] = ()) -> None: self._leaves = leaves + self._outputs = outputs def get_leaf_nodes(self) -> list: return self._leaves @@ -201,6 +197,9 @@ def get_leaf_nodes(self) -> list: def output(self, key: str) -> str: return f"selector:user_pipeline:{key}" + def output_types(self) -> dict: + return dict.fromkeys(self._outputs, object()) + class _FakeTensorGroup: """Stand-in for an ``OptionalTensorGroup`` carrying joint positions.""" @@ -215,17 +214,36 @@ def __getitem__(self, idx): # =========================================================================== -# _chain_hand_debug_outputs: HandsSource auto-discovery +# _chain_hand_presence_outputs: HandsSource auto-discovery # =========================================================================== class TestChainHandDebugOutputs: + def test_chains_when_hands_source_present(self): + pipeline = _FakePipeline([_FakeLeaf("controllers"), _FakeHandsSource("hands")]) + mapping: dict = {} + TeleopSessionLifecycle._chain_hand_presence_outputs(pipeline, mapping) + assert mapping[TeleopSessionLifecycle.HAND_LEFT_KEY] == "selector:hands:hand_left" + assert mapping[TeleopSessionLifecycle.HAND_RIGHT_KEY] == "selector:hands:hand_right" + def test_mapping_untouched_without_hands_source(self): pipeline = _FakePipeline([_FakeLeaf("controllers"), _FakeLeaf("head")]) mapping: dict = {"action": "selector:user_pipeline:action"} - TeleopSessionLifecycle._chain_hand_debug_outputs(pipeline, mapping) + TeleopSessionLifecycle._chain_hand_presence_outputs(pipeline, mapping) assert mapping == {"action": "selector:user_pipeline:action"} + def test_explicit_filtered_hand_outputs_take_precedence_over_raw_source(self): + pipeline = _FakePipeline( + [_FakeHandsSource("hands")], + outputs=(TeleopSessionLifecycle.HAND_LEFT_KEY, TeleopSessionLifecycle.HAND_RIGHT_KEY), + ) + mapping: dict = {} + TeleopSessionLifecycle._chain_hand_presence_outputs(pipeline, mapping) + assert mapping == { + TeleopSessionLifecycle.HAND_LEFT_KEY: "selector:user_pipeline:hand_left", + TeleopSessionLifecycle.HAND_RIGHT_KEY: "selector:user_pipeline:hand_right", + } + # =========================================================================== # start(): chaining gated by the enable_debug_visualization lifecycle arg @@ -248,10 +266,10 @@ def test_start_chains_when_enabled(self): assert lifecycle.pipeline.output_mapping[TeleopSessionLifecycle.HAND_RIGHT_KEY] == "selector:hands:hand_right" assert TeleopSessionLifecycle._CONTROLLER_LEFT_KEY in lifecycle.pipeline.output_mapping - def test_start_does_not_chain_when_disabled(self): + def test_start_chains_hand_presence_but_not_left_controller_when_disabled(self): lifecycle = self._start(enable_debug_visualization=False) - assert TeleopSessionLifecycle.HAND_LEFT_KEY not in lifecycle.pipeline.output_mapping - assert TeleopSessionLifecycle.HAND_RIGHT_KEY not in lifecycle.pipeline.output_mapping + assert TeleopSessionLifecycle.HAND_LEFT_KEY in lifecycle.pipeline.output_mapping + assert TeleopSessionLifecycle.HAND_RIGHT_KEY in lifecycle.pipeline.output_mapping assert TeleopSessionLifecycle._CONTROLLER_LEFT_KEY not in lifecycle.pipeline.output_mapping # The right controller is always chained for button polling. assert TeleopSessionLifecycle._CONTROLLER_RIGHT_KEY in lifecycle.pipeline.output_mapping diff --git a/source/isaaclab_teleop/test/test_ov_libraries_xr.py b/source/isaaclab_teleop/test/test_ov_libraries_xr.py new file mode 100644 index 000000000000..99597c3d967a --- /dev/null +++ b/source/isaaclab_teleop/test/test_ov_libraries_xr.py @@ -0,0 +1,865 @@ +# 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 + +"""Focused tests for the Isaac Lab host side of external OVXR presentation.""" + +from __future__ import annotations + +import ast +import builtins +import contextlib +import importlib +import sys +import threading +from contextlib import contextmanager +from pathlib import Path +from types import ModuleType, SimpleNamespace + +import pytest + + +class _FakeLease: + def __init__(self): + self.ovrtx_renderer = object() + self.ovstage = object() + self.partition_calls = [] + self.validated_ordinals = [] + self.owner_lane_entries = 0 + self.auxiliary_render_product_path = None + + @contextmanager + def ovstage_owner_lane(self): + self.owner_lane_entries += 1 + yield + + def configure_external_camera_scene_partitions(self, environment_root_path, camera_paths): + self.partition_calls.append((environment_root_path, camera_paths)) + + def validate_external_scene_partitions(self, ordinal): + self.validated_ordinals.append(ordinal) + + +class _FakePublisher: + def __init__(self): + self.calls = [] + self.fail = None + self.required = True + self.closed = False + + def publication_required(self, _publication_id): + return self.required + + def write_complete_scene(self, ordinal, publication_id): + self.calls.append((ordinal, publication_id, threading.get_native_id())) + if self.fail is not None: + raise self.fail + + def close(self): + self.closed = True + + +class _FakeOrdinalLedger: + def __init__(self, stage): + self.stage = stage + self.next_ordinal = 11 + self.reserved = [] + self.published = [] + self.abandoned = [] + self.publish_error = None + self.destroyed = False + self.external_resolutions = [] + + def reserve(self): + ordinal = self.next_ordinal + self.next_ordinal += 1 + self.reserved.append(ordinal) + return ordinal + + def publish(self, ordinal): + if self.publish_error is not None: + raise self.publish_error + self.published.append(ordinal) + + def abandon(self, ordinal): + self.abandoned.append(ordinal) + + def resolve_external(self, ordinal, *, expected_published): + self.external_resolutions.append((ordinal, expected_published)) + return True + + def destroy(self): + self.destroyed = True + + +class _FakeBootstrap: + def __init__(self): + self.lease = _FakeLease() + self.publisher = _FakePublisher() + self.events = [] + self.closed = False + self.aborted = False + self.auxiliary_camera_cfgs = [] + + def capture_scene_publisher(self): + self.events.append("capture") + return self.publisher + + def offer_render_owner_transfer(self): + self.events.append("offer") + return "capability" + + def claim_render_owner_transfer(self, capability): + self.events.append(("claim", capability)) + + def bind_environment(self, env): + self.events.append(("bind", env)) + return self.lease + + def configure_auxiliary_camera(self, camera_cfg, *, max_fps=30.0): + self.auxiliary_camera_cfgs.append(camera_cfg) + + def get_target_frame_T_world(self, path): + return ("target", path) + + def close(self): + self.closed = True + + def abort(self): + self.aborted = True + + +@pytest.fixture +def xr_module(monkeypatch): + """Import the adapter with native renderer dependencies stubbed.""" + isaaclab_ov = ModuleType("isaaclab_ov") + isaaclab_ov.__path__ = [] + ovxr_session = ModuleType("isaaclab_ov.ovxr_session") + ovxr_session.OvxrSessionBootstrap = object + # Execute the actual pure eligibility function with host-owned endpoints. + from isaaclab.sim import SimulationContext + + source = Path(__file__).parents[2] / "isaaclab_ov/isaaclab_ov/ovxr_session.py" + tree = ast.parse(source.read_text()) + check = next( + node + for node in tree.body + if isinstance(node, ast.FunctionDef) and node.name == "validate_ovxr_session_preparation" + ) + namespace = { + "SimulationContext": SimulationContext, + "builtins": builtins, + "_TERMINAL_MODE_SENTINEL": "ISAAC_LAUNCHED_FROM_TERMINAL", + } + exec(compile(ast.Module(body=[check], type_ignores=[]), str(source), "exec"), namespace) + ovxr_session.validate_ovxr_session_preparation = namespace[check.name] + renderers = ModuleType("isaaclab_ov.renderers") + renderers.OVRTXRendererCfg = object + monkeypatch.setitem(sys.modules, "isaaclab_ov", isaaclab_ov) + monkeypatch.setitem(sys.modules, "isaaclab_ov.ovxr_session", ovxr_session) + monkeypatch.setitem(sys.modules, "isaaclab_ov.renderers", renderers) + sys.modules.pop("isaaclab_teleop.ov_libraries_xr", None) + module = importlib.import_module("isaaclab_teleop.ov_libraries_xr") + module._test_declare_ovxr_runtime_prims = module._declare_ovxr_runtime_prims + monkeypatch.setattr(module, "_import_ovxr", _fake_ovxr) + return module + + +def _fake_ovxr(): + return SimpleNamespace(apply_renderer_settings=lambda _settings: None) + + +def test_stage_ordinal_ledger_tracks_one_application_reservation(xr_module): + class ImmediateQuery: + def __init__(self, fetch): + self._fetch = fetch + + def __enter__(self): + return self + + def __exit__(self, *_exc): + pass + + def wait(self): + pass + + def fetch(self): + return self._fetch() + + class ImmediateAdvance: + def wait(self): + pass + + class Stage: + def __init__(self): + self.floor = 10 + self.advances = [] + self.queries = 0 + + def get_attribute_write_floor(self): + self.queries += 1 + return ImmediateQuery(lambda: self.floor) + + def advance_write_floor(self, *, ordinal): + self.advances.append(ordinal) + self.floor = ordinal + return ImmediateAdvance() + + stage = Stage() + ledger = xr_module._StageOrdinalLedger(stage) + assert stage.queries == 1 + + assert ledger.reserve() == 11 + with pytest.raises(RuntimeError, match="still unresolved"): + ledger.reserve() + ledger.abandon(11) + + assert ledger.reserve() == 11 + ledger.publish(11) + assert stage.advances == [11] + assert stage.queries == 2 # Only the earlier abandon needs reconciliation. + + assert ledger.reserve() == 12 + stage.floor = 12 # The explicit-ordinal OVXR call sealed its own write. + assert ledger.resolve_external(12, expected_published=True) + assert stage.queries == 2 + + assert ledger.reserve() == 13 + assert not ledger.resolve_external(13, expected_published=None) + assert stage.queries == 3 + assert ledger.reserve() == 13 + stage.floor = 13 + with pytest.raises(RuntimeError, match="expected unsealed"): + ledger.resolve_external(13, expected_published=False) + assert stage.queries == 4 + # Even a contradicted response must not permit reuse of an observed sealed ordinal. + assert ledger.reserve() == 14 + ledger.abandon(14) + ledger.destroy() + with pytest.raises(RuntimeError, match="closed"): + ledger.reserve() + + +def test_runtime_render_prims_are_type_only_and_precede_attach(xr_module): + class FakePrim: + def __init__(self, type_name=""): + self.type_name = type_name + + def __bool__(self): + return bool(self.type_name) + + def IsValid(self): + return bool(self.type_name) + + def GetTypeName(self): + return self.type_name + + class FakeStage: + def __init__(self): + self.prims = {} + self.definitions = [] + + def GetPrimAtPath(self, path): + return self.prims.get(path, FakePrim()) + + def DefinePrim(self, path, type_name): + self.definitions.append((path, type_name)) + self.prims[path] = FakePrim(type_name) + + stage = FakeStage() + env = SimpleNamespace(unwrapped=SimpleNamespace(scene=SimpleNamespace(stage=stage))) + declared = xr_module._test_declare_ovxr_runtime_prims(env, xr_module.OvLibrariesXrConfig()) + + assert len(declared) == 7 + assert stage.definitions == list(declared) + assert all(len(item) == 2 for item in declared) + + +def test_runtime_render_prim_declaration_preserves_disabled_and_fails_on_conflict(xr_module): + class ExistingPrim: + def IsValid(self): + return True + + def GetTypeName(self): + return "Xform" + + stage = SimpleNamespace(GetPrimAtPath=lambda _path: ExistingPrim(), DefinePrim=lambda *_args: None) + env = SimpleNamespace(unwrapped=SimpleNamespace(scene=SimpleNamespace(stage=stage))) + + assert ( + xr_module._test_declare_ovxr_runtime_prims(env, xr_module.OvLibrariesXrConfig(create_render_products=False)) + == () + ) + with pytest.raises(RuntimeError, match="incompatible type"): + xr_module._test_declare_ovxr_runtime_prims(env, xr_module.OvLibrariesXrConfig()) + + +def test_hidden_visual_prims_fullmatch_imageables_and_fail_closed(xr_module, monkeypatch): + class FakePrim: + def __init__(self, path): + self.path = path + self.visible = True + + def GetPath(self): + return self.path + + def IsA(self, _schema): + return True + + class FakeImageable: + def __init__(self, prim): + self.prim = prim + + def MakeInvisible(self): + self.prim.visible = False + + pxr = ModuleType("pxr") + pxr.UsdGeom = SimpleNamespace(Imageable=FakeImageable) + monkeypatch.setitem(sys.modules, "pxr", pxr) + head = FakePrim("/World/envs/env_0/Robot/head_yaw_link") + body = FakePrim("/World/envs/env_0/Robot/base_link") + stage = SimpleNamespace(Traverse=lambda: iter((head, body))) + env = SimpleNamespace(unwrapped=SimpleNamespace(scene=SimpleNamespace(stage=stage))) + + assert xr_module._apply_hidden_visual_prims(env, ("/World/envs/env_.*/Robot/head_.*",)) == (head.path,) + assert not head.visible and body.visible + with pytest.raises(RuntimeError, match="matched no imageable prims"): + xr_module._apply_hidden_visual_prims(env, ("/World/missing/.*",)) + + +def _make_host(xr_module, monkeypatch): + bootstrap = _FakeBootstrap() + env = SimpleNamespace(close=lambda: bootstrap.events.append("environment-close")) + monkeypatch.setattr(xr_module, "_import_ovxr", _fake_ovxr) + monkeypatch.setattr(xr_module, "_StageOrdinalLedger", _FakeOrdinalLedger) + monkeypatch.setattr( + xr_module, + "_apply_hidden_visual_prims", + lambda _env, _patterns: bootstrap.events.append("hide"), + ) + monkeypatch.setattr(xr_module, "_declare_ovxr_runtime_prims", lambda _env, _cfg: bootstrap.events.append("declare")) + host = xr_module._IsaacLabOvxrApplicationInterface( + bootstrap, + env, + xr_module.OvLibrariesXrConfig(), + ("/hidden",), + ) + return host, bootstrap, env + + +def test_host_lends_exact_pair_and_keeps_application_publication(xr_module, monkeypatch): + host, bootstrap, env = _make_host(xr_module, monkeypatch) + + capability = host.offer_render_owner_transfer() + binding = host.attach_on_render_thread(capability) + with host.publication_guard(): + ordinal = host.publish_complete_scene("step-1") + + assert binding.renderer is bootstrap.lease.ovrtx_renderer + assert binding.stage is bootstrap.lease.ovstage + assert binding.ordinal_ledger.stage is bootstrap.lease.ovstage + assert bootstrap.events == ["capture", "offer", ("claim", "capability"), "hide", "declare", ("bind", env)] + assert ordinal == 11 + assert bootstrap.publisher.calls[0][:2] == (11, "step-1") + host.validate_first_published_scene(ordinal) + assert bootstrap.lease.validated_ordinals == [11] + with host.owner_lane(): + pass + assert bootstrap.lease.owner_lane_entries == 3 + assert host.get_target_frame_T_world("/World/target") == ("target", "/World/target") + + host.close_publication() + host.close_on_render_thread() + host.close_environment_on_render_thread() + assert bootstrap.publisher.closed and bootstrap.closed + assert binding.ordinal_ledger.destroyed + assert bootstrap.events[-1] == "environment-close" + + +def test_host_lends_the_selected_auxiliary_product_without_rendering_it(xr_module, monkeypatch): + bootstrap = _FakeBootstrap() + bootstrap.lease.auxiliary_render_product_path = "/IsaacLabAuxiliaryRender/RenderProduct_robot_pov_cam" + camera = object() + env = SimpleNamespace(unwrapped=SimpleNamespace(scene=SimpleNamespace(sensors={"robot_pov_cam": camera}))) + monkeypatch.setattr(xr_module, "_StageOrdinalLedger", _FakeOrdinalLedger) + monkeypatch.setattr(xr_module, "_apply_hidden_visual_prims", lambda *_args: None) + monkeypatch.setattr(xr_module, "_declare_ovxr_runtime_prims", lambda *_args: None) + host = xr_module._IsaacLabOvxrApplicationInterface( + bootstrap, + env, + xr_module.OvLibrariesXrConfig( + auxiliary_cameras=( + xr_module.OvLibrariesXrCameraConfig("robot_pov_cam", SimpleNamespace(width=720, height=450)), + ), + ), + (), + ) + + binding = host.attach_on_render_thread(host.offer_render_owner_transfer()) + + assert binding.auxiliary_render_product_path == bootstrap.lease.auxiliary_render_product_path + assert not hasattr(binding, "auxiliary_frame_source") + + +def test_host_rejects_a_missing_selected_auxiliary_camera(xr_module, monkeypatch): + bootstrap = _FakeBootstrap() + env = SimpleNamespace(unwrapped=SimpleNamespace(scene=SimpleNamespace(sensors={}))) + monkeypatch.setattr(xr_module, "_apply_hidden_visual_prims", lambda *_args: None) + monkeypatch.setattr(xr_module, "_declare_ovxr_runtime_prims", lambda *_args: None) + host = xr_module._IsaacLabOvxrApplicationInterface( + bootstrap, + env, + xr_module.OvLibrariesXrConfig( + auxiliary_cameras=( + xr_module.OvLibrariesXrCameraConfig("robot_pov_cam", SimpleNamespace(width=720, height=450)), + ), + ), + (), + ) + + with pytest.raises(RuntimeError, match="not present"): + host.attach_on_render_thread(host.offer_render_owner_transfer()) + + +def test_selected_camera_poses_follow_motion_and_reset_before_publication(xr_module, monkeypatch): + host, bootstrap, env = _make_host(xr_module, monkeypatch) + names = ("head", "left_wrist", "right_wrist") + events = [] + + class PoseOnlyCamera: + live = 0 + cached = -1 + fail = False + + def _update_poses(self): + if self.fail: + raise RuntimeError("pose unavailable") + self.cached = self.live + events.append(self) + + @property + def data(self): + pytest.fail("Publication must not fetch/render camera images") + + cameras = {name: PoseOnlyCamera() for name in (*names, "unselected")} + env.scene = SimpleNamespace(sensors=cameras) + host._auxiliary_cameras = tuple(SimpleNamespace(name=name) for name in names) + bootstrap.lease.auxiliary_render_product_paths = tuple(f"/Render/{name}" for name in names) + binding = host.attach_on_render_thread(host.offer_render_owner_transfer()) + published = [] + bootstrap.publisher.write_complete_scene = lambda *_args: published.append( + tuple(cameras[name].cached for name in names) + ) + + for identity, positions in ((1, (0, 2, 3)), (("reset", 1, 1), (0, -2, -3)), (2, (0, 4, 5))): + for name, value in zip(names, positions, strict=True): + cameras[name].live = value + with host.publication_guard(): + host.publish_complete_scene(identity) + assert published[-1] == positions + assert events == [cameras[name] for name in names] * 3 + assert cameras["unselected"].cached == -1 + + bootstrap.publisher.required = False + with host.publication_guard(): + host.publish_complete_scene(2) + assert len(events) == 9 # Reused scene/cadence gaps do not resample poses. + + bootstrap.publisher.required = True + cameras["left_wrist"].fail = True + reserved = list(binding.ordinal_ledger.reserved) + with host.publication_guard(), pytest.raises(RuntimeError, match="pose unavailable"): + host.publish_complete_scene(3) + assert binding.ordinal_ledger.reserved == reserved + assert len(published) == 3 + + +def test_host_abandons_failed_write_and_failed_publish(xr_module, monkeypatch): + host, bootstrap, _env = _make_host(xr_module, monkeypatch) + host.attach_on_render_thread(host.offer_render_owner_transfer()) + bootstrap.publisher.fail = RuntimeError("write failed") + with host.publication_guard(), pytest.raises(RuntimeError, match="write failed"): + host.publish_complete_scene("step-1") + assert host._ordinal_ledger.abandoned == [11] + + bootstrap.publisher.fail = None + host._ordinal_ledger.publish_error = RuntimeError("publish failed") + with host.publication_guard(), pytest.raises(RuntimeError, match="publish failed"): + host.publish_complete_scene("step-2") + assert host._ordinal_ledger.abandoned == [11, 12] + + +@pytest.mark.parametrize("sealed", [False, True]) +def test_host_reconciles_failed_publication_before_reserving_again(xr_module, monkeypatch, sealed): + ledger_type = xr_module._StageOrdinalLedger + host, _bootstrap, _env = _make_host(xr_module, monkeypatch) + host.attach_on_render_thread(host.offer_render_owner_transfer()) + failure = RuntimeError("native publication wait failed") + stage = SimpleNamespace(floor=10) + stage.get_attribute_write_floor = lambda: contextlib.nullcontext( + SimpleNamespace(wait=lambda: None, fetch=lambda: stage.floor) + ) + + def failed_advance(*, ordinal): + if sealed: + stage.floor = ordinal + + def wait(): + raise failure + + return SimpleNamespace(wait=wait) + + stage.advance_write_floor = failed_advance + ledger = ledger_type(stage) + host._ordinal_ledger = ledger + with host.publication_guard(), pytest.raises(RuntimeError) as caught: + host.publish_complete_scene("step-1") + assert caught.value is failure + next_ordinal = ledger.reserve() + assert next_ordinal == (12 if sealed else 11) + ledger.abandon(next_ordinal) + ledger.destroy() + + +def test_failed_publication_with_unknown_floor_retains_reservation_and_first_error(xr_module, monkeypatch): + ledger_type = xr_module._StageOrdinalLedger + host, _bootstrap, _env = _make_host(xr_module, monkeypatch) + host.attach_on_render_thread(host.offer_render_owner_transfer()) + stage = SimpleNamespace(unknown=False) + failure = RuntimeError("native publication wait failed") + + def fetch(): + if stage.unknown: + raise RuntimeError("floor read unavailable") + return 10 + + stage.get_attribute_write_floor = lambda: contextlib.nullcontext(SimpleNamespace(wait=lambda: None, fetch=fetch)) + + def failed_advance(*, ordinal): + stage.unknown = True + + def wait(): + raise failure + + return SimpleNamespace(wait=wait) + + stage.advance_write_floor = failed_advance + ledger = ledger_type(stage) + host._ordinal_ledger = ledger + with host.publication_guard(), pytest.raises(RuntimeError) as caught: + host.publish_complete_scene("step-1") + assert caught.value is failure + assert any("floor read unavailable" in note for note in getattr(caught.value, "__notes__", [])) + with pytest.raises(RuntimeError, match="still unresolved"): + ledger.reserve() + + +@pytest.mark.parametrize("failure", ["write", "publish"]) +def test_publication_retains_primary_cause_when_abandon_fails(xr_module, monkeypatch, failure): + host, bootstrap, _env = _make_host(xr_module, monkeypatch) + host.attach_on_render_thread(host.offer_render_owner_transfer()) + original_cause = ValueError("original publication cause") + first_error = RuntimeError("publication failed") + first_error.__cause__ = original_cause + if failure == "write": + bootstrap.publisher.fail = first_error + else: + host._ordinal_ledger.publish_error = first_error + + def fail_abandon(_ordinal): + raise RuntimeError("abandon failed") + + monkeypatch.setattr(host._ordinal_ledger, "abandon", fail_abandon) + with host.publication_guard(), pytest.raises(RuntimeError) as caught: + host.publish_complete_scene("step-1") + assert caught.value is first_error + assert caught.value.__cause__ is original_cause + assert any("abandon failed" in note for note in caught.value.__notes__) + + +@pytest.mark.parametrize("camera_enabled", [False, True]) +def test_session_delegates_lifecycle_to_isaacteleop_owner(xr_module, monkeypatch, camera_enabled): + calls = [] + + class FakeOwner: + owns_environment = True + render_worker_bootstrap_ready = True + shutdown_requested = False + + def __init__(self, host, config, **kwargs): + calls.append(("init", host, config, kwargs)) + + def start(self, timeout_s): + calls.append(("start", timeout_s)) + return True + + def external_oxr_handles(self): + return "handles" + + def request_render(self, identity, *, force=False): + calls.append(("request", identity, force)) + return 17 + + def request_shutdown(self): + self.shutdown_requested = True + calls.append(("request_shutdown",)) + + def get_target_frame_T_world(self, path): + return ("target", path) + + def close(self): + calls.append(("close",)) + + class FakeMirror: + status = SimpleNamespace(main_thread_fps=30.0, render_thread_fps=40.0) + + def __init__(self, config): + self.config = config + + def record_host_frame(self): + calls.append(("timing",)) + + def set_status_text(self, text): + calls.append(("status", text)) + + mirror_module = importlib.import_module("isaaccapture.ovxr_session_manager.owned_mirror") + monkeypatch.setattr(mirror_module, "OwnedDesktopMirror", FakeMirror) + external = importlib.import_module("isaaccapture.ovxr_session_manager.ovxr_session") + monkeypatch.setattr(external, "OvxrSession", FakeOwner) + bootstrap = _FakeBootstrap() + assert xr_module.OvLibrariesXrConfig().extra_openxr_extensions == () + session = xr_module.OvLibrariesXrSession( + bootstrap, + xr_module.OvLibrariesXrConfig( + extra_openxr_extensions=("XR_EXT_debug_utils",), + desktop_mirror_enabled=True, + desktop_mirror_max_fps=20.0, + auxiliary_cameras=( + xr_module.OvLibrariesXrCameraConfig("robot_pov_cam", SimpleNamespace(width=720, height=450)), + ) + if camera_enabled + else (), + ), + ) + env = SimpleNamespace() + + assert session.start_render_worker( + env, + required_openxr_extensions=("XR_EXT_hand_tracking",), + timeout_s=4.0, + ) + + assert calls[0][0] == "init" + assert calls[0][2].session_mode == "ar" + assert calls[0][3]["required_openxr_extensions"] == ("XR_EXT_hand_tracking",) + assert calls[0][2].extra_openxr_extensions == ("XR_EXT_debug_utils",) + assert calls[0][2].host_wait_render_output is True + (consumer,) = calls[0][3]["frame_consumers"] + assert consumer.config.max_fps == 20.0 + assert consumer.config.initial_status_text == "Waiting for headset" + if camera_enabled: + assert calls[0][2].auxiliary_raster.label == "robot_pov_cam" + else: + assert calls[0][2].auxiliary_raster is None + assert calls[1] == ("start", 4.0) + assert session.external_oxr_handles() == "handles" + assert session.request_render("step") == 17 + assert session.request_render("reset", force=True) == 17 + assert calls[-2:] == [("request", "step", False), ("request", "reset", True)] + monkeypatch.setattr(session._physics_rate, "sample", lambda _count: 114.7) + session.report_host_timing(42, 1.0 / 120.0) + assert calls[-2:] == [ + ("timing",), + ("status", "Main 30.0 FPS Physics 114.7/120 Hz (0.96× RT) Render 40.0 FPS"), + ] + assert session.get_target_frame_T_world("/target") == ("target", "/target") + assert session.async_worker_owns_environment and session.render_worker_bootstrap_ready + assert not session.shutdown_requested + session.request_shutdown() + assert calls[-1] == ("request_shutdown",) + assert session.shutdown_requested + session._owner.shutdown_requested = True + assert session.shutdown_requested + session.close() + assert calls[-1] == ("close",) + + +def test_session_abort_before_owner_delegates_to_bootstrap(xr_module): + bootstrap = _FakeBootstrap() + session = xr_module.OvLibrariesXrSession(bootstrap, xr_module.OvLibrariesXrConfig()) + session.abort() + assert bootstrap.aborted + + +def test_desktop_mirror_rate_must_be_positive_and_finite(xr_module): + with pytest.raises(ValueError, match="desktop_mirror_max_fps"): + xr_module.OvLibrariesXrConfig(desktop_mirror_max_fps=0.0) + with pytest.raises(ValueError, match="desktop_mirror_max_fps"): + xr_module.OvLibrariesXrConfig(desktop_mirror_max_fps=float("nan")) + + +def test_auxiliary_camera_config_is_preconfigured(xr_module, monkeypatch): + with pytest.raises(ValueError, match="name and configuration"): + xr_module.OvLibrariesXrCameraConfig("", object()) + with pytest.raises(ValueError, match="name and configuration"): + xr_module.OvLibrariesXrCameraConfig("robot_pov_cam", None) + with pytest.raises(ValueError, match="max_fps"): + xr_module.OvLibrariesXrCameraConfig("robot_pov_cam", object(), max_fps=0.0) + + bootstrap = _FakeBootstrap() + camera_cfg = SimpleNamespace(width=720, height=450) + monkeypatch.setattr(xr_module, "OvxrSessionBootstrap", lambda *_args: bootstrap) + camera = xr_module.OvLibrariesXrCameraConfig("robot_pov_cam", camera_cfg) + session = xr_module.OvLibrariesXrSession.prepare( + object(), xr_module.OvLibrariesXrConfig(auxiliary_cameras=(camera,)) + ) + assert isinstance(session, xr_module.OvLibrariesXrSession) + assert bootstrap.auxiliary_camera_cfgs == [camera_cfg] + + +@pytest.mark.parametrize( + "settings", ["", "--/rtx/rendermode=MinimalRendering", "--/rtx/rendering/perSensorTickTlas=true"] +) +def test_caller_renderer_settings_precede_renderer_creation(xr_module, monkeypatch, settings): + events = [] + bootstrap = _FakeBootstrap() + monkeypatch.setattr( + xr_module, "_import_ovxr", lambda: SimpleNamespace(apply_renderer_settings=lambda value: events.append(value)) + ) + + def create_bootstrap(*_args): + assert len(events) == 1 + assert events[0] == settings.strip() + return bootstrap + + monkeypatch.setattr(xr_module, "OvxrSessionBootstrap", create_bootstrap) + xr_module.OvLibrariesXrSession.prepare(object(), xr_module.OvLibrariesXrConfig(renderer_settings=settings)) + + +def test_prepare_rejects_existing_context_before_global_settings(xr_module, monkeypatch): + from isaaclab.sim import SimulationContext + + monkeypatch.setattr(SimulationContext, "instance", lambda: object()) + monkeypatch.setattr(xr_module, "_import_ovxr", lambda: pytest.fail("changed existing renderer settings")) + monkeypatch.setattr(xr_module, "OvxrSessionBootstrap", lambda *_: pytest.fail("allocated second bootstrap")) + with pytest.raises(RuntimeError, match="must create the SimulationContext"): + xr_module.OvLibrariesXrSession.prepare(object(), xr_module.OvLibrariesXrConfig()) + + +@pytest.mark.parametrize("terminal_mode", [False, None, 1]) +def test_prepare_rejects_extension_mode_before_global_settings(xr_module, monkeypatch, terminal_mode): + monkeypatch.setattr(builtins, "ISAAC_LAUNCHED_FROM_TERMINAL", terminal_mode, raising=False) + monkeypatch.setattr(xr_module, "_import_ovxr", lambda: pytest.fail("changed renderer settings")) + monkeypatch.setattr(xr_module, "OvxrSessionBootstrap", lambda *_: pytest.fail("allocated bootstrap")) + with pytest.raises(RuntimeError, match="extension mode"): + xr_module.OvLibrariesXrSession.prepare(object(), xr_module.OvLibrariesXrConfig()) + + +def test_counter_rate_uses_application_counter_and_resets_cleanly(xr_module): + rate = xr_module._CounterRate() + assert rate.sample(10, now_ns=0) == 0.0 + assert rate.sample(15, now_ns=500_000_000) == pytest.approx(10.0) + assert rate.sample(20, now_ns=1_000_000_000) == pytest.approx(10.0) + assert rate.sample(0, now_ns=1_100_000_000) == 0.0 + + +def test_disabled_desktop_mirror_does_not_collect_or_format_status(xr_module): + session = xr_module.OvLibrariesXrSession( + _FakeBootstrap(), + xr_module.OvLibrariesXrConfig(desktop_mirror_enabled=False), + ) + session._owner = SimpleNamespace(report_host_timing=lambda: pytest.fail("unexpected mirror work")) + + session.report_host_timing(42, 1.0 / 120.0) + + +def test_config_rejects_unknown_session_mode(xr_module): + with pytest.raises(ValueError, match="session_mode must be 'ar' or 'vr'"): + xr_module.OvLibrariesXrConfig(session_mode="automatic") + + +def test_three_auxiliary_camera_configs_prepare_the_same_bootstrap(xr_module, monkeypatch): + bootstrap = _FakeBootstrap() + monkeypatch.setattr(xr_module, "OvxrSessionBootstrap", lambda *_: bootstrap) + cameras = tuple( + xr_module.OvLibrariesXrCameraConfig( + name=name, camera_cfg=SimpleNamespace(width=640, height=480), max_fps=hz, space="head_locked" + ) + for name, hz in zip(("robot_pov_cam", "left_wrist_cam", "right_wrist_cam"), (20, 10, 10), strict=True) + ) + session = xr_module.OvLibrariesXrSession.prepare(object(), xr_module.OvLibrariesXrConfig(auxiliary_cameras=cameras)) + assert bootstrap.auxiliary_camera_cfgs == [camera.camera_cfg for camera in cameras] + assert session is not None + + +def test_prepare_snapshots_camera_selection_without_mutating_caller(xr_module, monkeypatch): + bootstrap = _FakeBootstrap() + monkeypatch.setattr(xr_module, "OvxrSessionBootstrap", lambda *_: bootstrap) + camera_cfg = SimpleNamespace(width=640, height=480) + camera = xr_module.OvLibrariesXrCameraConfig("robot_pov_cam", camera_cfg) + cfg = xr_module.OvLibrariesXrConfig(auxiliary_cameras=(camera,)) + session = xr_module.OvLibrariesXrSession.prepare(object(), cfg) + cfg.auxiliary_cameras = () + assert session._cfg.auxiliary_cameras == (camera,) + assert session._cfg.auxiliary_cameras[0].camera_cfg is bootstrap.auxiliary_camera_cfgs[0] + + +@pytest.mark.parametrize("fail_during", ["camera", "session"]) +@pytest.mark.parametrize("abort_fails", [False, True]) +def test_prepare_aborts_partial_bootstrap_and_preserves_first_error(xr_module, monkeypatch, fail_during, abort_fails): + bootstrap = _FakeBootstrap() + first_error = RuntimeError("preparation failed") + original_cause = ValueError("invalid camera input") + first_error.add_note("Original preparation diagnostic") + abort_error = RuntimeError("abort failed") + monkeypatch.setattr(xr_module, "_import_ovxr", _fake_ovxr) + monkeypatch.setattr(xr_module, "OvxrSessionBootstrap", lambda *_: bootstrap) + + def fail(*_args, **_kwargs): + raise first_error from original_cause + + def abort(): + bootstrap.aborted = True + if abort_fails: + raise abort_error + + monkeypatch.setattr(bootstrap, "abort", abort) + if fail_during == "camera": + monkeypatch.setattr(bootstrap, "configure_auxiliary_camera", fail) + else: + monkeypatch.setattr(xr_module.OvLibrariesXrSession, "__init__", fail) + + cfg = xr_module.OvLibrariesXrConfig( + auxiliary_cameras=( + xr_module.OvLibrariesXrCameraConfig("robot_pov_cam", SimpleNamespace(width=640, height=480)), + ), + ) + with pytest.raises(RuntimeError) as caught: + xr_module.OvLibrariesXrSession.prepare(object(), cfg) + + assert bootstrap.aborted + assert caught.value is first_error + assert caught.value.__cause__ is original_cause + assert caught.value.__notes__ == ["Original preparation diagnostic"] + ( + [f"Cleanup after failed XR preparation also failed: {abort_error!r}"] if abort_fails else [] + ) + + +@pytest.mark.parametrize("space", ["viewer_start", "world"]) +def test_prepare_rejects_unsupported_quad_placement_before_native_preparation(xr_module, monkeypatch, space): + monkeypatch.setattr(xr_module, "_import_ovxr", lambda: pytest.fail("renderer settings changed before validation")) + monkeypatch.setattr(xr_module, "OvxrSessionBootstrap", lambda *_: pytest.fail("allocated before validation")) + with pytest.raises(ValueError, match="head_locked"): + camera = xr_module.OvLibrariesXrCameraConfig( + name="robot_pov_cam", camera_cfg=SimpleNamespace(width=640, height=480), space=space + ) + xr_module.OvLibrariesXrSession.prepare(object(), xr_module.OvLibrariesXrConfig(auxiliary_cameras=(camera,))) + + +def test_public_xr_exports_resolve_to_implemented_types(xr_module): + import isaaclab_teleop + + assert isaaclab_teleop.OvLibrariesXrConfig is xr_module.OvLibrariesXrConfig + assert isaaclab_teleop.OvLibrariesXrSession is xr_module.OvLibrariesXrSession + assert "OvLibrariesXrFrame" not in isaaclab_teleop.__all__ diff --git a/source/isaaclab_teleop/test/test_ovrtx_transfer_native_harness.py b/source/isaaclab_teleop/test/test_ovrtx_transfer_native_harness.py new file mode 100644 index 000000000000..7f99fa5528ac --- /dev/null +++ b/source/isaaclab_teleop/test/test_ovrtx_transfer_native_harness.py @@ -0,0 +1,177 @@ +# 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 + +"""Native scene publication and renderer ownership transfer.""" + +from __future__ import annotations + +import os +import subprocess +import sys +import threading +from dataclasses import dataclass +from pathlib import Path + +import pytest + +pytestmark = [ + pytest.mark.integration, + pytest.mark.skipif( + os.environ.get("ISAACLAB_RUN_NATIVE_OVRTX_TRANSFER") != "1", + reason="native OVRTX transfer harness is explicitly opt-in", + ), +] + + +@dataclass +class _CaseResult: + offer_thread: int + claim_thread: int | None = None + renderer_id: int | None = None + stage_id: int | None = None + ordinal: int | None = None + error: BaseException | None = None + + +def _make_env_and_bootstrap(): + import gymnasium as gym + from isaaclab_ov.ovxr_session import OvxrSessionBootstrap + from isaaclab_ov.physics import OvPhysxCfg + from isaaclab_ov.renderers import OVRTXRendererCfg + from isaaclab_teleop import remove_camera_configs + + import isaaclab_tasks.contrib.pick_place # noqa: F401 -- registers the exact GR1T2 task + from isaaclab_tasks.utils import parse_env_cfg + + cfg = parse_env_cfg("IsaacContrib-PickPlace-GR1T2-Abs", device="cpu", num_envs=1) + cfg.sim.physics = OvPhysxCfg() + cfg.stabilize_for_kitless_ovphysx() + cfg = remove_camera_configs(cfg) + bootstrap = OvxrSessionBootstrap(cfg.sim, OVRTXRendererCfg()) + try: + return bootstrap, gym.make("IsaacContrib-PickPlace-GR1T2-Abs", cfg=cfg).unwrapped + except BaseException: + bootstrap.abort() + raise + + +def _run_case(*, transfer: bool) -> _CaseResult: + from isaaclab_teleop.ov_libraries_xr import OvLibrariesXrConfig, _IsaacLabOvxrApplicationInterface + + bootstrap, env = _make_env_and_bootstrap() + result = _CaseResult(offer_thread=threading.get_native_id()) + host = _IsaacLabOvxrApplicationInterface(bootstrap, env, OvLibrariesXrConfig(auto_launch_cloudxr=False), ()) + capability = host.offer_render_owner_transfer() + ready, published = threading.Event(), threading.Event() + + def attach(): + binding = host.attach_on_render_thread(capability) + result.renderer_id, result.stage_id = id(binding.renderer), id(binding.stage) + return binding + + def update_stage(binding): + with host.publication_guard(), host.owner_lane(): + binding.renderer.update_from_stage(result.ordinal) + + def close(): + host.close_publication() + try: + host.close_on_render_thread() + finally: + host.close_environment_on_render_thread() + + if not transfer: + try: + binding = attach() + with host.publication_guard(): + result.ordinal = host.publish_complete_scene(("native-control", 0)) + update_stage(binding) + finally: + close() + return result + + with pytest.raises(RuntimeError, match="transfer is pending"): + _ = bootstrap._lease.ovstage + + def worker(): + try: + result.claim_thread = threading.get_native_id() + binding = attach() + ready.set() + if not published.wait(60): + raise TimeoutError("application did not complete its scene publication") + if result.error is None: + update_stage(binding) + with pytest.raises(RuntimeError, match="capability is invalid"): + bootstrap.claim_render_owner_transfer(capability) + except BaseException as exc: + if result.error is None: + result.error = exc + finally: + ready.set() + try: + close() + except BaseException as exc: + if result.error is None: + result.error = exc + + thread = threading.Thread(target=worker, name="ovrtx-native-transfer-worker") + thread.start() + try: + if not ready.wait(90): + raise TimeoutError("renderer claimant did not finish binding") + if result.error is not None: + raise result.error + with host.publication_guard(): + result.ordinal = host.publish_complete_scene(("native-transfer", 0)) + except BaseException as exc: + result.error = exc + finally: + published.set() + thread.join(timeout=90) + assert not thread.is_alive(), "native transfer worker timed out" + if result.error is not None: + raise result.error + assert result.claim_thread is not None and result.claim_thread != result.offer_thread + with pytest.raises(RuntimeError, match="(owner thread|released)"): + _ = bootstrap._lease.ovstage + return result + + +def _run_native_subprocess(transfer: bool) -> None: + # A native non-daemon thread cannot be bounded by Thread.join alone. + # Keep the native lifetime in a child that the standard subprocess timeout owns. + completed = subprocess.run( + [sys.executable, str(Path(__file__).resolve()), "transfer" if transfer else "same-thread"], + env={**os.environ, "ISAAC_LAB_OVRTX_USE_OVSTAGE": "1"}, + capture_output=True, + text=True, + timeout=180, + check=False, + ) + print(completed.stdout) + assert completed.returncode == 0, completed.stderr + + +def test_gr1t2_same_thread_ovxr_session_native_control(): + """One-thread production scene publication remains viable.""" + _run_native_subprocess(False) + + +def test_gr1t2_cross_thread_preuse_owner_transfer_native(): + """The application publishes while the render thread binds, renders and closes the pair.""" + _run_native_subprocess(True) + + +if __name__ == "__main__": + if os.environ.get("ISAACLAB_RUN_NATIVE_OVRTX_TRANSFER") != "1": + raise RuntimeError("native transfer harness requires its explicit opt-in") + result = _run_case(transfer=sys.argv[1] == "transfer") + assert result.renderer_id is not None and result.stage_id is not None + assert result.ordinal is not None + print( + f"offer={result.offer_thread} claim={result.claim_thread} " + f"renderer={result.renderer_id} stage={result.stage_id} ordinal={result.ordinal}" + ) diff --git a/source/isaaclab_teleop/test/test_session_step_failure.py b/source/isaaclab_teleop/test/test_session_step_failure.py index ad33e1e7a5a2..4ffa0f2a00e4 100644 --- a/source/isaaclab_teleop/test/test_session_step_failure.py +++ b/source/isaaclab_teleop/test/test_session_step_failure.py @@ -133,6 +133,110 @@ def _make_failing_lifecycle(error: Exception) -> TeleopSessionLifecycle: class TestStepFailureDiagnosis: + @pytest.mark.parametrize("error_type", [RuntimeError, KeyboardInterrupt]) + def test_failed_drain_retains_owner_and_original_step_error(self, error_type): + step_error = ValueError("retarget worker failed") + drain_error = error_type("worker did not join") + lifecycle = _make_failing_lifecycle(step_error) + session = lifecycle._session + session.__exit__.side_effect = drain_error + with ( + patch.object(TeleopSessionLifecycle, "_kit_xr_session_is_active", return_value=False), + patch.object(lifecycle, "_build_external_inputs", return_value=None), + pytest.raises(ValueError, match="retarget worker failed") as raised, + ): + lifecycle.step() + assert raised.value is step_error + assert raised.value.__cause__ is drain_error + assert lifecycle._session is session + assert any("worker did not join" in note for note in step_error.__notes__) + notes = tuple(step_error.__notes__) + lifecycle.stop(type(step_error), step_error, step_error.__traceback__) + assert tuple(step_error.__notes__) == notes + + def test_replay_step_failure_is_not_restarted_from_the_beginning(self): + error = ValueError("invalid recorded input") + lifecycle = _make_failing_lifecycle(error) + lifecycle._is_replay = True + session = lifecycle._session + with ( + patch.object(lifecycle, "_build_external_inputs", return_value=None), + pytest.raises(ValueError, match="invalid recorded input") as raised, + ): + lifecycle.step() + assert raised.value is error + session.__exit__.assert_called_once() + assert lifecycle._session is None + + def test_xr_shutdown_drain_failure_remains_visible_and_owned(self): + lifecycle = _make_failing_lifecycle(RuntimeError("unused")) + session = lifecycle._session + session.__exit__.side_effect = RuntimeError("native drain failed") + with pytest.raises(RuntimeError, match="native drain failed"): + lifecycle._on_pre_shutdown(None) + assert lifecycle._session is session + + @pytest.mark.parametrize("error_type", [RuntimeError, KeyboardInterrupt]) + def test_terminal_exit_failure_is_latched_across_callbacks_and_restart(self, error_type): + lifecycle = _make_lifecycle() + lifecycle._pipeline = object() + error = error_type("terminal native cleanup failed") + + class TerminalSession: + def __init__(self): + self.entered = True + self.steps = 0 + self.exits = 0 + + def __exit__(self, *_exc): + self.exits += 1 + if self.entered: + self.entered = False + raise error + + def step(self, **_kwargs): + self.steps += 1 + raise AssertionError("must not step a terminal session") + + session = TerminalSession() + lifecycle._session = session + with pytest.raises(error_type) as raised: + lifecycle._on_pre_shutdown(None) + assert raised.value is error + assert lifecycle.session_cleanup_failed + for operation in (lifecycle.step, lifecycle.step, lifecycle.try_start_session, lifecycle.start, lifecycle.stop): + with pytest.raises(error_type) as raised: + operation() + assert raised.value is error + assert session.exits == 1 + assert session.steps == 0 + assert lifecycle.session_cleanup_failed + assert lifecycle._session is session + + def test_failed_submission_keeps_pending_host_reset_for_reentry(self): + lifecycle = _make_failing_lifecycle(ValueError("persistent pipeline error")) + lifecycle._pending_reset = True + with ( + patch.object(TeleopSessionLifecycle, "_kit_xr_session_is_active", return_value=False), + patch.object(lifecycle, "_build_external_inputs", return_value=None), + ): + assert lifecycle.step() is None + assert lifecycle._pending_reset + + def test_ovxr_session_pipeline_failure_also_bounds_restart_rate(self): + lifecycle = _make_failing_lifecycle(ValueError("persistent pipeline error")) + lifecycle._external_oxr_handles_provider = lambda: object() + with ( + patch.object(TeleopSessionLifecycle, "_kit_xr_session_is_active", return_value=False), + patch.object(lifecycle, "_build_external_inputs", return_value=None), + ): + assert lifecycle.step() is None + assert lifecycle._restart_holdoff_until > time.monotonic() + with patch.object(lifecycle, "_try_start_session") as try_start: + for _ in range(5): + assert lifecycle.step() is None + try_start.assert_not_called() + def test_pipeline_error_with_active_xr_sets_holdoff(self): lifecycle = _make_failing_lifecycle(ValueError("Found zero norm quaternions in `quat`.")) session = lifecycle._session diff --git a/source/isaaclab_teleop/test/test_teleop_failure_exit.py b/source/isaaclab_teleop/test/test_teleop_failure_exit.py new file mode 100644 index 000000000000..0b0f32dcb13f --- /dev/null +++ b/source/isaaclab_teleop/test/test_teleop_failure_exit.py @@ -0,0 +1,204 @@ +# 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 + +"""Execute the real public entry without native startup to prove failure/cleanup contracts.""" + +import ast +import contextlib +import logging +import signal +import sys +from pathlib import Path +from types import SimpleNamespace +from unittest.mock import MagicMock, Mock + +import pytest + + +@pytest.fixture +def entry_namespace(): + """Load whole entry/cleanup functions, excluding module-level native app startup.""" + script = Path(__file__).resolve().parents[3] / "scripts/environments/teleoperation/teleop_se3_agent.py" + tree = ast.parse(script.read_text()) + functions = [ + node + for node in tree.body + if isinstance(node, ast.FunctionDef) and node.name in {"main", "_close_teleop_resources"} + ] + namespace = { + "ManagerBasedRLEnvCfg": object, + "_restore_ovxr_shutdown_handlers": Mock(), + "sys": sys, + "logger": logging.getLogger(__name__), + } + exec( + compile(ast.fix_missing_locations(ast.Module(body=functions, type_ignores=[])), str(script), "exec"), namespace + ) + return namespace + + +@pytest.mark.parametrize("stage", ["environment", "device", "haptics", "keyboard", "input"]) +def test_live_entry_failure_closes_resources_and_preserves_cause(monkeypatch, entry_namespace, stage): + """Every startup phase and the live loop must retain its failure after cleanup.""" + import isaaclab_teleop + + failure = RuntimeError(f"{stage} failed") + camera = SimpleNamespace(close=Mock(), bind=lambda _env: contextlib.nullcontext()) + env = SimpleNamespace( + sim=SimpleNamespace(device="cpu", has_gui=False, is_running=lambda: True), reset=Mock(), close=Mock() + ) + interface = MagicMock() + interface.advance.side_effect = failure + monkeypatch.setattr(isaaclab_teleop.XrCameraFeedSession, "prepare", Mock(return_value=camera)) + monkeypatch.setattr( + isaaclab_teleop, + "create_isaac_teleop_device", + Mock(side_effect=failure if stage == "device" else None, return_value=interface), + ) + entry_namespace.update( + { + "args_cli": SimpleNamespace( + task="test-task", + xr=False, + using_ovxr=False, + disable_external_cameras=True, + auto_launch_cloudxr=False, + enable_debug_visualization=False, + cloudxr_env="none", + ), + "gym": SimpleNamespace( + make=Mock( + side_effect=failure if stage == "environment" else None, return_value=SimpleNamespace(unwrapped=env) + ) + ), + "_rtx_rendering_requested": lambda _args: False, + "_resolve_cloudxr_env": lambda *_args: None, + "_make_haptic_io": Mock(side_effect=failure if stage == "haptics" else None, return_value=(Mock(), Mock())), + "_make_control_keyboard": Mock(side_effect=failure if stage == "keyboard" else None), + "_install_ovxr_shutdown_handlers": Mock(return_value={}), + "torch": SimpleNamespace(inference_mode=contextlib.nullcontext), + } + ) + cfg = SimpleNamespace( + isaac_teleop=SimpleNamespace( + teleoperation_active_default=False, + require_initial_bimanual_hand_tracking=False, + action_conditioner=None, + action_validator=None, + ) + ) + with pytest.raises(RuntimeError) as caught: + entry_namespace["main"](cfg, False, True, []) + assert caught.value is failure + camera.close.assert_called_once_with() + assert env.close.call_count == int(stage != "environment") + + +def test_input_cleanup_failure_prevents_borrowed_owner_release(entry_namespace): + env, camera, xr = Mock(), Mock(), Mock() + interface = SimpleNamespace(session_cleanup_failed=True) + with pytest.raises(RuntimeError, match="borrowed XR/environment drain is unproved"): + entry_namespace["_close_teleop_resources"](env, camera, xr, {}, interface) + xr.close.assert_not_called() + camera.close.assert_not_called() + env.close.assert_not_called() + + +@pytest.mark.parametrize( + ("cleanup_stage", "has_primary_error"), + [("xr", True), ("environment", True), ("camera", False), ("signals", False)], +) +def test_cleanup_preserves_first_error_and_never_frees_undrained_stage( + entry_namespace, cleanup_stage, has_primary_error +): + """Cleanup errors must stay visible without replacing the initial cause or risking UAF.""" + events = [] + cleanup_error = RuntimeError("cleanup failed") + primary_error = RuntimeError("original failure") + + def close(name): + events.append(name) + if name == cleanup_stage: + raise cleanup_error + + xr = SimpleNamespace(close=lambda: close("xr"), async_worker_owns_environment=False) + camera = SimpleNamespace(close=lambda: close("camera")) + env = SimpleNamespace(close=lambda: close("environment")) + entry_namespace["_restore_ovxr_shutdown_handlers"] = lambda _: close("signals") + + def exercise(): + try: + if has_primary_error: + raise primary_error + finally: + entry_namespace["_close_teleop_resources"](env, camera, xr, {}) + + with pytest.raises(RuntimeError) as caught: + exercise() + assert caught.value is (primary_error if has_primary_error else cleanup_error) + assert events == ( + ["xr", "camera", "signals"] if cleanup_stage == "xr" else ["xr", "camera", "environment", "signals"] + ) + if has_primary_error: + assert any("cleanup failed" in note for note in primary_error.__notes__) + + +def test_shutdown_signal_stays_owned_until_xr_drain_finishes(entry_namespace): + """A second Ctrl-C requests shutdown again instead of interrupting native cleanup.""" + events = [] + original = signal.signal(signal.SIGINT, lambda *_args: events.append("shutdown")) + + def restore(handlers): + for signum, handler in handlers.items(): + signal.signal(signum, handler) + + def close_xr(): + signal.raise_signal(signal.SIGINT) + events.append("xr") + + entry_namespace["_restore_ovxr_shutdown_handlers"] = restore + try: + entry_namespace["_close_teleop_resources"]( + SimpleNamespace(close=lambda: events.append("environment")), + SimpleNamespace(close=lambda: events.append("camera")), + SimpleNamespace(close=close_xr, async_worker_owns_environment=False), + {signal.SIGINT: original}, + ) + assert events == ["shutdown", "xr", "camera", "environment"] + assert signal.getsignal(signal.SIGINT) is original + finally: + signal.signal(signal.SIGINT, original) + + +@pytest.mark.parametrize("new_context,drain_failed", [(True, False), (False, False), (True, True)]) +def test_partial_construction_cleans_only_new_unowned_context(monkeypatch, entry_namespace, new_context, drain_failed): + """Failed env construction is cleaned, but preexisting/undrained contexts survive.""" + from isaaclab.app import launch_simulation + from isaaclab.physics import PhysicsCfg + from isaaclab.sim import SimulationContext + + previous = object() + state = {"context": previous} + clear = Mock(side_effect=lambda: state.update(context=None)) + monkeypatch.setattr(SimulationContext, "instance", lambda: state["context"]) + monkeypatch.setattr(SimulationContext, "clear_instance", clear) + camera = SimpleNamespace(close=Mock()) + xr = SimpleNamespace( + close=Mock(side_effect=RuntimeError("drain failed") if drain_failed else None), + async_worker_owns_environment=False, + ) + failure = RuntimeError("environment construction failed") + with pytest.raises(RuntimeError) as caught: + with launch_simulation(PhysicsCfg(), cleanup_context=False): + if new_context: + state["context"] = object() + try: + raise failure + finally: + entry_namespace["_close_teleop_resources"](None, camera, xr, {}, previous_context=previous) + assert caught.value is failure + assert clear.call_count == int(new_context and not drain_failed) + if not new_context: + assert state["context"] is previous diff --git a/source/isaaclab_teleop/test/test_teleop_replay_bootstrap.py b/source/isaaclab_teleop/test/test_teleop_replay_bootstrap.py new file mode 100644 index 000000000000..c7ee73fabd73 --- /dev/null +++ b/source/isaaclab_teleop/test/test_teleop_replay_bootstrap.py @@ -0,0 +1,267 @@ +# 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 + +"""Execute the real replay bootstrap without starting a simulator. + +Only device/simulator endpoints are isolated; the replay/reset function bodies +and Kit detection are production code. These tests do not qualify MCAP motion. +""" + +import __future__ + +import ast +import builtins +import statistics +import sys +import time +from contextlib import contextmanager +from pathlib import Path +from types import ModuleType, SimpleNamespace + +import pytest + +from isaaclab.utils.version import has_kit + +_ROOT = Path(__file__).resolve().parents[3] + + +@pytest.mark.parametrize( + "kit_active,run_index,backend,viewer,hard_reset", + [ + (False, 0, "ovphysx", False, True), + (True, 0, "ovphysx", False, True), + (True, 1, "ovphysx", False, True), + (False, 0, "ovphysx", True, True), + (False, 0, "newton_mjwarp", True, False), + ], +) +def test_replay_bootstrap_waits_only_for_an_active_kit_application( + monkeypatch, kit_active, run_index, backend, viewer, hard_reset +): + """A viewer preserves backend reset policy; kitless replay never waits for Kit assets.""" + calls = [] + + class Device: + def __enter__(self): + calls.append("device-enter") + return self + + def reset(self): + calls.append("device-reset") + + def request_start(self): + calls.append("device-start") + + def __exit__(self, *args): + calls.append("device-exit") + + app = ModuleType("omni.kit.app") + app.get_app = lambda: object() + if kit_active: + monkeypatch.setitem(sys.modules, "omni.kit.app", app) + else: + monkeypatch.delitem(sys.modules, "omni.kit.app", raising=False) + wait_module = ModuleType("isaaclab_physx.renderers.isaac_rtx_renderer_utils") + wait_module.wait_for_stage_load = lambda *args, **kwargs: calls.append("kit-wait") + monkeypatch.setitem(sys.modules, wait_module.__name__, wait_module) + original_import = builtins.__import__ + + def import_with_kit_guard(name, *args, **kwargs): + if name == wait_module.__name__ and not kit_active: + raise AssertionError("Kit-only stage loader imported by a kitless replay") + return original_import(name, *args, **kwargs) + + monkeypatch.setattr(builtins, "__import__", import_with_kit_guard) + + def render(): + calls.append("render") + + env = SimpleNamespace( + scene=SimpleNamespace(physics_backend=backend), + sim=SimpleNamespace( + device="cpu", render=render, reset=lambda: calls.append("sim-reset"), is_running=lambda: False + ), + reset=lambda: calls.append("env-reset"), + ) + source_path = _ROOT / "scripts/environments/teleoperation/teleop_replay_agent.py" + names = { + "_run_single_replay", + "_warmup_replay", + "_advance_replay_input", + "_render_replay_idle", + "_replay_requires_hard_sim_reset", + "_reset_replay_environment", + "_assert_run_measured", + } + selected = [ + node + for node in ast.parse(source_path.read_text()).body + if isinstance(node, ast.FunctionDef) and node.name in names + ] + assert {node.name for node in selected} == names + from isaacteleop.teleop_session_manager import RetargetingExecutionConfig + + cfg = SimpleNamespace(retargeting_execution=RetargetingExecutionConfig(mode="pipelined")) + constructed_modes = [] + + def create_device(selected, **kwargs): + constructed_modes.append(selected.retargeting_execution.mode) + assert selected.retargeting_execution.mode == "pipelined" + return Device() + + namespace = { + "args_cli": SimpleNamespace( + replay_file="fixture.mcap", + enable_debug_visualization=False, + max_stage_load_wait_s=5, + replay_start_delay_s=0, + max_replay_duration_s=1, + ), + "_RunStats": lambda: SimpleNamespace(active_iterations=0, active_frame_times_ms=[], active_render_calls=0), + "NvmlGpuStatsProvider": lambda: SimpleNamespace(_available=True, summary=lambda: {}), + "create_isaac_teleop_device": create_device, + "_RENDERER_SETTLE_FRAMES": 30, + "contextmanager": contextmanager, + "has_kit": has_kit, + "time": time, + } + # Avoid executing the script's CLI/application startup during this unit test. + exec( + compile( + ast.Module(body=selected, type_ignores=[]), + str(source_path), + "exec", + flags=__future__.annotations.compiler_flag, + ), + namespace, + ) + presentation = SimpleNamespace(start=lambda *_args: calls.append("viewer-start")) if viewer else None + namespace["_run_single_replay"](env, cfg, None, run_index, 2, presentation) + expected = ["device-enter"] + if hard_reset: + expected.append("sim-reset") + expected.extend(["env-reset", "device-reset", "device-start"]) + if viewer: + expected.append("viewer-start") + if kit_active and run_index == 0: + expected.append("kit-wait") + assert calls == [*expected, "device-exit"] + assert env.sim.render is render + assert constructed_modes == ["pipelined"] + + +@pytest.mark.parametrize( + "stop_at,requested", + [ + ("startup", 2), + ("startup-second-signal", 2), + ("construction-new", 2), + ("construction-previous", 2), + ("after-first", 2), + ("after-first", 1), + ], +) +def test_presented_replay_preserves_startup_ownership_and_batch_completion(monkeypatch, stop_at, requested): + """Failed construction preserves prior owners; a partial batch never reports completion.""" + from isaaclab.sim import SimulationContext + + calls = [] + previous = object() + context_state = {"current": previous} + + def clear_context(): + calls.append("context-clear") + context_state["current"] = None + + monkeypatch.setattr(SimulationContext, "instance", lambda: context_state["current"]) + monkeypatch.setattr(SimulationContext, "clear_instance", clear_context) + handlers = {} + reports = [] + session = SimpleNamespace( + shutdown_requested=False, + async_worker_owns_environment=not stop_at.startswith("construction"), + request_shutdown=lambda: None, + ) + + class Presentation: + def __init__(self, cfg): + self.session = session + self.started = False + + def close(self): + if stop_at == "startup-second-signal": + handlers[2](2, None) + calls.append("presentation-close") + + def install_signal(sig, handler): + handlers[sig] = handler + return + + def make_env(*args, **kwargs): + if stop_at.startswith("startup"): + handlers[1](1, None) + if stop_at.startswith("construction"): + if stop_at == "construction-new": + context_state["current"] = object() + raise RuntimeError("environment construction failed") + return SimpleNamespace(unwrapped=env) + + def replay(**kwargs): + calls.append("replay") + session.shutdown_requested = True + return SimpleNamespace(outcome="success", to_dict=lambda index: {"outcome": "success"}) + + env = SimpleNamespace(sim=SimpleNamespace(is_running=lambda: True)) + source_path = _ROOT / "scripts/environments/teleoperation/teleop_replay_agent.py" + names = {"_run_replay_batch", "_exit_code_for_outcomes", "_build_report", "_aggregate_runs"} + selected = [ + node + for node in ast.parse(source_path.read_text()).body + if isinstance(node, ast.FunctionDef) and node.name in names + ] + namespace = { + "args_cli": SimpleNamespace( + ovxr=True, + require_kit=False, + task="task", + replay_file="fixture.mcap", + num_replays=requested, + stats_output_file=None, + ), + "_ReplayPresentation": Presentation, + "signal": SimpleNamespace(SIGINT=1, SIGTERM=2, signal=install_signal), + "gym": SimpleNamespace(make=make_env), + "_run_single_replay": replay, + "_STATS_SCHEMA_VERSION": 1, + "statistics": statistics, + "_extract_env_perf_cfg": lambda cfg: None, + "_print_stdout_summary": reports.append, + } + exec( + compile( + ast.Module(body=selected, type_ignores=[]), + str(source_path), + "exec", + flags=__future__.annotations.compiler_flag, + ), + namespace, + ) + if stop_at.startswith("startup"): + with pytest.raises(KeyboardInterrupt): + namespace["_run_replay_batch"](SimpleNamespace(isaac_teleop=None), None) + assert calls == ["presentation-close"] + elif stop_at.startswith("construction"): + with pytest.raises(RuntimeError, match="environment construction failed"): + namespace["_run_replay_batch"](SimpleNamespace(isaac_teleop=None), None) + assert calls == ["presentation-close"] + (["context-clear"] if stop_at == "construction-new" else []) + assert context_state["current"] is (None if stop_at == "construction-new" else previous) + else: + result = namespace["_run_replay_batch"](SimpleNamespace(isaac_teleop=None), None) + assert result == (0 if requested == 1 else 1) + assert reports[0]["num_replays"] == 1 + assert reports[0]["requested_num_replays"] == requested + assert reports[0]["batch_complete"] is (requested == 1) + assert calls == ["replay", "presentation-close"] + assert handlers == {1: None, 2: None} diff --git a/source/isaaclab_teleop/test/test_teleop_scripts_smoke.py b/source/isaaclab_teleop/test/test_teleop_scripts_smoke.py index 1796331576ec..1bac32527d8f 100644 --- a/source/isaaclab_teleop/test/test_teleop_scripts_smoke.py +++ b/source/isaaclab_teleop/test/test_teleop_scripts_smoke.py @@ -26,17 +26,21 @@ terminated via its process group. """ +import ast +import importlib.util import os import re import signal import subprocess import sys import time +from collections.abc import Callable from pathlib import Path +from types import SimpleNamespace import pytest - -pytestmark = [pytest.mark.integration, pytest.mark.rendering] +import torch +from isaaclab_teleop.action_policy import prepare_teleop_action, prepare_teleop_backend # Repository root: this file lives at source/isaaclab_teleop/test/.py ROOT = Path(__file__).resolve().parents[3] @@ -167,6 +171,8 @@ def _assert_started_cleanly(output: str, markers: list[str], script: str) -> Non ) +@pytest.mark.integration +@pytest.mark.rendering def test_teleop_se3_agent_starts(tmp_path): """teleop_se3_agent.py builds the Isaac Capture device (past the #6656 RTX/camera regression). @@ -185,6 +191,8 @@ def test_teleop_se3_agent_starts(tmp_path): _assert_started_cleanly(output, [_ISAAC_TELEOP_MARKER], "teleop_se3_agent.py") +@pytest.mark.integration +@pytest.mark.rendering def test_record_demos_starts(tmp_path): """record_demos.py builds the Isaac Capture device without AttributeError-ing on enable_cameras. @@ -204,6 +212,8 @@ def test_record_demos_starts(tmp_path): _assert_started_cleanly(output, [_ISAAC_TELEOP_MARKER], "record_demos.py") +@pytest.mark.integration +@pytest.mark.rendering def test_record_demos_starts_when_reward_references_success(tmp_path): """record_demos.py preserves success-term references while disabling automatic resets.""" argv = [ @@ -239,6 +249,8 @@ def test_xr_experience_declares_only_shipped_extensions(): ) +@pytest.mark.integration +@pytest.mark.rendering def test_record_demos_starts_headless_xr(tmp_path): """record_demos.py starts under ``--xr`` with no ``--viz kit`` (the QA-reported failure). @@ -267,6 +279,8 @@ def test_record_demos_starts_headless_xr(tmp_path): _assert_started_cleanly(output, markers, "record_demos.py --xr") +@pytest.mark.integration +@pytest.mark.rendering def test_teleop_replay_agent_applies_rtx_without_replicator_crash(tmp_path): """teleop_replay_agent.py clears the RTX-apply site (create env) without the replicator crash. @@ -285,3 +299,743 @@ def test_teleop_replay_agent_applies_rtx_without_replicator_crash(tmp_path): ] output = _launch_until_marker(argv, [_ENV_CREATED_MARKER], tmp_path / "replay_agent.log") _assert_started_cleanly(output, [_ENV_CREATED_MARKER], "teleop_replay_agent.py") + + +def test_teleop_readme_programmatic_config_returns_pipeline_not_factory_tuple(): + """Execute the documented public config, without entering an XR session.""" + from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource + from isaacteleop.retargeting_engine.interface import OutputCombiner + + readme = ROOT / "source/isaaclab_teleop/docs/README.md" + section = readme.read_text().split("### 4. Programmatic Usage", 1)[1] + example = re.search(r"```python\n(.*?)```", section, re.DOTALL) + assert example is not None + tree = ast.parse(example.group(1)) + config_statements = [] + for statement in tree.body: + if isinstance(statement, ast.With): + break + config_statements.append(statement) + source = ControllersSource(name="documentation_controllers") + pipeline = OutputCombiner({"action": source.output(ControllersSource.RIGHT)}) + retargeters = [] + namespace = {"my_pipeline_builder": lambda: (pipeline, retargeters)} + exec(compile(ast.Module(body=config_statements, type_ignores=[]), str(readme), "exec"), namespace) + cfg = namespace["cfg"] + assert cfg.pipeline_builder() is pipeline + assert cfg.pipeline_builder().output("action") is not None + assert cfg.retargeters_to_tune() is retargeters + + +@pytest.mark.parametrize( + ("kitless", "selection", "expected", "reject"), + [ + (True, None, "1", False), + (True, " 1 ", " 1 ", False), + (True, "0", "0", True), + (True, "invalid", "invalid", True), + (False, None, None, False), + ], +) +@pytest.mark.parametrize("has_ovphysx", [False, True]) +def test_teleop_ovxr_selects_external_stage_before_runtime( + monkeypatch, kitless, selection, expected, reject, has_ovphysx +): + """The actual task resolver selects OVStage, while incompatible explicit modes fail before launch validation.""" + from types import SimpleNamespace + + import isaaclab_teleop + + from isaaclab_tasks.utils import hydra + + monkeypatch.setattr(hydra, "enumerate_task_presets", lambda _task: None) + name = "ISAAC_LAB_OVRTX_USE_OVSTAGE" + if selection is None: + monkeypatch.delenv(name, raising=False) + else: + monkeypatch.setenv(name, selection) + monkeypatch.setattr(isaaclab_teleop, "remove_camera_configs", lambda cfg, **_kwargs: cfg) + preparations = [] + from isaaclab_ov.physics import OvPhysxCfg + + config = SimpleNamespace( + sim=SimpleNamespace(device="cpu", physics=OvPhysxCfg() if has_ovphysx else object()), + scene=SimpleNamespace(num_envs=1), + terminations=SimpleNamespace(time_out=1), + isaac_teleop=object(), + stabilize_for_kitless_ovphysx=lambda: preparations.append("ovphysx"), + ) + scans = [] + source = ROOT / "scripts/environments/teleoperation/teleop_se3_agent.py" + tree = ast.parse(source.read_text()) + resolve = next(node for node in tree.body if isinstance(node, ast.FunctionDef) and node.name == "_resolve_env_cfg") + namespace = { + "prepare_teleop_backend": prepare_teleop_backend, + "prepare_teleop_action": prepare_teleop_action, + "action_state": {}, + "os": os, + "sys": sys, + "ManagerBasedRLEnvCfg": SimpleNamespace, + "resolve_task_config": lambda *_args, **_kwargs: (config, None), + "validate_launch": lambda *_args: (scans.append(os.environ.get(name)) or ((), ())), + "args_cli": SimpleNamespace( + task="Test", + device="cpu", + num_envs=1, + teleop_device=None, + using_ovxr=kitless, + ovxr_pip_camera=None, + geometry_publication_device=None, + ovrtx_active_cuda_gpus=None, + disable_external_cameras=False, + physics=None, + ), + } + exec(compile(ast.Module(body=[resolve], type_ignores=[]), str(source), "exec"), namespace) + if reject: + with pytest.raises(ValueError, match="requires.*OVStage"): + namespace["_resolve_env_cfg"]() + assert scans == [] + else: + namespace["_resolve_env_cfg"]() + assert scans == [expected] + assert os.environ.get(name) == expected + + assert preparations == (["ovphysx"] if has_ovphysx and not reject else []) + + +def test_teleop_se3_agent_yields_only_when_ovxr_is_idle(monkeypatch): + """A paused kitless loop must yield without slowing Kit or active physics frames.""" + source_path = ROOT / "scripts/environments/teleoperation/teleop_se3_agent.py" + module = ast.parse(source_path.read_text(encoding="utf-8")) + selected = [ + node for node in module.body if isinstance(node, ast.FunctionDef) and node.name == "_pace_ovxr_control_loop" + ] + namespace = {"time": time, "Callable": Callable} + exec(compile(ast.Module(body=selected, type_ignores=[]), str(source_path), "exec"), namespace) + + sleeps = [] + monkeypatch.setattr(time, "sleep", sleeps.append) + monkeypatch.setattr(time, "perf_counter", lambda: 10.003) + yield_idle = namespace["_pace_ovxr_control_loop"] + + timing = {"iteration_started_s": 10.0, "action_period_s": 0.02} + yield_idle(using_ovxr=False, action_applied=False, **timing) + yield_idle(using_ovxr=True, action_applied=True, **timing) + assert sleeps == [] + + yield_idle(using_ovxr=True, action_applied=False, **timing) + assert sleeps == pytest.approx([0.017]) + yield_idle(using_ovxr=True, action_applied=False, iteration_started_s=9.9, action_period_s=0.02) + assert sleeps == pytest.approx([0.017]) # Slow input work needs no added delay. + + +def test_teleop_se3_agent_paces_after_publication(): + """The yield belongs after reset/publication bookkeeping in every loop.""" + source = (ROOT / "scripts/environments/teleoperation/teleop_se3_agent.py").read_text(encoding="utf-8") + force_index = source.index("force_publication = False") + request_index = source.index("ov_libraries_xr_session.request_render(", force_index) + timing_index = source.index("ov_libraries_xr_session.report_host_timing(", request_index) + assert "force_publication = True" in source[force_index:request_index] + assert "pending_publication_identity, force=force_publication" in source[request_index:timing_index] + assert "env.sim.get_physics_dt()" in source[timing_index : timing_index + 200] + yield_index = source.index("_pace_ovxr_control_loop(", request_index) + except_index = source.index("except Exception as e:", yield_index) + assert request_index < timing_index < yield_index < except_index + + +@pytest.mark.parametrize("active,step_hz,expected_polls", [(False, 0.0, 6), (True, 25.0, 3), (None, 25.0, 3)]) +def test_kitless_loop_respects_control_rate_and_processes_reset(active, step_hz, expected_polls): + """The real caller preserves idle pacing and applies an explicit active limit.""" + from types import SimpleNamespace + + import torch + + source_path = ROOT / "scripts/environments/teleoperation/teleop_se3_agent.py" + module = ast.parse(source_path.read_text(encoding="utf-8")) + selected = [ + node + for node in module.body + if isinstance(node, ast.FunctionDef) + and node.name + in { + "_pace_ovxr_control_loop", + "_ovxr_publication_identity", + "_apply_teleop_frame", + } + ] + loop = next(node for node in ast.walk(module) if isinstance(node, ast.FunctionDef) and node.name == "run_loop") + # Isolate the executable loop from CLI/GPU initialization, retaining its body. + loop.body = [ + ast.copy_location(ast.Global(names=node.names), node) if isinstance(node, ast.Nonlocal) else node + for node in loop.body + ] + selected.append(loop) + clock = 0.0 + advances = 0 + resets = [] + refreshes = [] + publications = [] + applied = [] + seeded_from = [] + stepped_values = [] + sequence = active is None + + def sleep(duration): + nonlocal clock + clock += duration + + def advance(): + nonlocal advances, clock + advances += 1 + clock += 0.003 # Caller work consumes part of the independent 20ms action budget. + return torch.ones((1, 36)) if active or (sequence and advances != 4) else None + + def step(value): + applied.append(clock) + stepped_values.append(value.clone()) + + def condition(_env, value, state): + previous = state.get("frames", 0) + seeded_from.append(previous) + state["frames"] = previous + 1 + return value + previous + 2 + + sim = SimpleNamespace( + is_running=lambda: clock < (0.199 if sequence else 0.119), + get_physics_step_count=lambda: 0, + get_physics_dt=lambda: 0.005, + render=lambda: None, + ) + namespace = { + "prepare_teleop_backend": prepare_teleop_backend, + "prepare_teleop_action": prepare_teleop_action, + "action_state": {}, + "time": SimpleNamespace(perf_counter=lambda: clock, sleep=sleep), + "torch": torch, + "env": SimpleNamespace(sim=sim, reset=lambda: resets.append(clock), step_dt=0.02, num_envs=1, step=step), + "env_cfg": SimpleNamespace(isaac_teleop=SimpleNamespace(target_frame_prim_path=None)), + "args_cli": SimpleNamespace(xr=True, using_ovxr=True, step_hz=step_hz), + "use_isaac_teleop": True, + "teleop_interface": SimpleNamespace(advance=advance, reset=lambda: None), + "poll_control_events": lambda _: SimpleNamespace( + is_active=advances not in (2, 4) if sequence else active, should_reset=advances == (5 if sequence else 3) + ), + "ov_libraries_xr_session": SimpleNamespace( + shutdown_requested=False, + wait_for_shutdown=lambda duration: sleep(duration), + request_render=lambda identity, **kwargs: publications.append((identity, kwargs)) or 1, + report_host_timing=lambda *args: None, + ), + "camera_feed_session": SimpleNamespace(refresh=lambda: refreshes.append(clock)), + "initialized_hand_tracking": set(), + "require_initial_bimanual_tracking": False, + "action_conditioner": condition, + "action_validator": lambda value: None, + "Callable": Callable, + "haptic_update": lambda: None, + "haptic_stop": lambda: None, + "should_reset_recording_instance": False, + "skipped_tracking_frames": 0, + "teleoperation_active": False, + "reset_publication_generation": 0, + } + exec(compile(ast.Module(body=selected, type_ignores=[]), str(source_path), "exec"), namespace) + namespace["run_loop"]() + + assert advances == (5 if sequence else expected_polls) + assert clock == pytest.approx( + 0.20 if sequence else 0.12 + ) # Work time is credited, rather than adding a full-period sleep. + assert len(applied) == (3 if sequence else advances if active else 0) + assert len(resets) == 2 # Initial reset plus the public control event. + assert len(refreshes) == 1 + assert len(publications) == (4 if sequence else advances + 1 if active else 2) + assert publications[-1][1] == {"force": True} + + if sequence: + # Both STOP branches clear an established ramp before the next accepted action. + assert seeded_from == [0, 0, 0] + for value in stepped_values: + torch.testing.assert_close(value, torch.full((1, 36), 3.0)) + elif active: + assert seeded_from == [0, 1, 2] + for index, value in enumerate(stepped_values): + torch.testing.assert_close(value, torch.full((1, 36), 3.0 + index)) + + +def test_paused_loop_retries_the_latest_unaccepted_publication(): + """Run the real publication block: no new simulation step is needed to flush B.""" + from types import SimpleNamespace + + source_path = ROOT / "scripts/environments/teleoperation/teleop_se3_agent.py" + tree = ast.parse(source_path.read_text(encoding="utf-8")) + candidates = [ + node + for node in ast.walk(tree) + if isinstance(node, ast.If) + and "state_changed" in ast.unparse(node) + and "ov_libraries_xr_session.request_render" in ast.unparse(node) + ] + block = min(candidates, key=lambda node: len(ast.unparse(node))) + calls = [] + admissions = iter((None, 12)) + + def request(identity, *, force): + calls.append((identity, force)) + return next(admissions) + + namespace = { + "prepare_teleop_backend": prepare_teleop_backend, + "prepare_teleop_action": prepare_teleop_action, + "action_state": {}, + "ov_libraries_xr_session": SimpleNamespace(request_render=request), + "state_changed": True, + "publication_identity": "B", + "pending_publication_identity": None, + "force_publication": False, + } + code = compile(ast.Module(body=[block], type_ignores=[]), str(source_path), "exec") + exec(code, namespace) + assert namespace["pending_publication_identity"] == "B" + namespace["state_changed"] = False + exec(code, namespace) + assert calls == [("B", False), ("B", False)] + assert namespace["pending_publication_identity"] is None + + +def test_teleop_se3_agent_observes_ovxr_session_shutdown(): + """Closing the optional desktop mirror must end the application loop.""" + source = (ROOT / "scripts/environments/teleoperation/teleop_se3_agent.py").read_text(encoding="utf-8") + loop_index = source.index("while env.sim.is_running()") + loop_header = source[loop_index : source.index(":", loop_index)] + + assert "ov_libraries_xr_session.shutdown_requested" in loop_header + assert "signal.SIGINT" in source + assert "signal.SIGTERM" in source + assert "ov_libraries_xr_session.request_shutdown()" in source + + +def test_teleop_se3_agent_defaults_ovxr_to_ar_and_routes_the_override(): + """Kitless XR defaults to AR while retaining an explicit opaque-VR override.""" + source = (ROOT / "scripts/environments/teleoperation/teleop_se3_agent.py").read_text(encoding="utf-8") + + assert '"--ovxr_session_mode"' in source + assert 'choices=("ar", "vr")' in source + assert 'default="ar"' in source + assert "session_mode=args_cli.ovxr_session_mode" in source + + +@pytest.mark.parametrize("selector", [["--physics", "ovphysx"], ["physics=ovphysx"]]) +@pytest.mark.parametrize("ovxr", [False, True]) +def test_teleop_replay_agent_resolves_typed_presets_without_reapplying_physics(monkeypatch, selector, ovxr): + """Public CLI aliases reach task resolution without resetting backend settings.""" + import runpy + + import isaaclab_teleop + from isaaclab_ov.physics import OvPhysxCfg + + from isaaclab.managers import TerminationTermCfg + from isaaclab.sim import SimulationCfg + + source_path = ROOT / "scripts/environments/teleoperation/teleop_replay_agent.py" + argv = [str(source_path), "--task", "Test", "--replay_file", "recording.mcap", *selector] + if ovxr: + argv.append("--ovxr") + monkeypatch.setattr(sys, "argv", argv) + namespace = runpy.run_path(str(source_path))["_prepare_env_cfg"].__globals__ + stabilization = [] + physics = OvPhysxCfg() + cfg = SimpleNamespace( + sim=SimulationCfg(physics=physics), + scene=SimpleNamespace(num_envs=1), + terminations=SimpleNamespace( + success=TerminationTermCfg(func=lambda env: False), + time_out=TerminationTermCfg(func=lambda env: False), + ), + stabilize_for_kitless_ovphysx=lambda: stabilization.append("ovphysx"), + ) + physics = cfg.sim.physics + resolutions = [] + + def resolve(task, agent, *, overrides): + resolutions.append((task, agent, overrides)) + return cfg, None + + namespace["ManagerBasedRLEnvCfg"] = SimpleNamespace + namespace["resolve_task_config"] = resolve + monkeypatch.setattr(isaaclab_teleop, "remove_camera_configs", lambda cfg, **_kwargs: cfg) + namespace["remove_camera_configs"] = isaaclab_teleop.remove_camera_configs + prepared, success = namespace["_prepare_env_cfg"]("test-task", 1, None, overrides=namespace["hydra_overrides"]) + assert resolutions == [("test-task", "", ["physics=ovphysx"])] + assert prepared is cfg and success is not None + assert cfg.sim.physics is physics + assert namespace["args_cli"].require_kit is False + assert not hasattr(namespace["args_cli"], "physics") + assert stabilization == ["ovphysx"] + + +@pytest.mark.parametrize( + "backend,viewer,hard_reset", + [("ovphysx", False, True), ("ovphysx", True, True), ("newton_mjwarp", True, False)], +) +def test_recorded_environment_reset_preserves_backend_policy_and_hand_cursor(backend, viewer, hard_reset): + """Recorded RESET preserves physics ordering and the MCAP cursor with an optional viewer.""" + from types import SimpleNamespace + + source_path = ROOT / "scripts/environments/teleoperation/teleop_replay_agent.py" + tree = ast.parse(source_path.read_text(encoding="utf-8")) + blocks = [ + node for node in ast.walk(tree) if isinstance(node, ast.If) and ast.unparse(node.test) == "ctrl.should_reset" + ] + assert len(blocks) == 1 + body = [node for node in blocks[0].body if not isinstance(node, ast.Continue)] + helpers = [ + node + for node in tree.body + if isinstance(node, ast.FunctionDef) + and node.name + in { + "_handle_reset", + "_reset_replay_environment", + "_replay_requires_hard_sim_reset", + } + ] + calls = [] + namespace = { + "prepare_teleop_backend": prepare_teleop_backend, + "prepare_teleop_action": prepare_teleop_action, + "action_state": {"seed": True}, + "initialized_hand_tracking": {"left", "right"}, + "gym": SimpleNamespace(Env=object), + "IsaacTeleopDevice": object, + "env": SimpleNamespace( + scene=SimpleNamespace(physics_backend=backend), + sim=SimpleNamespace(reset=lambda: calls.append("sim-reset")), + recorder_manager=SimpleNamespace(reset=lambda: calls.append("recorder-reset")), + reset=lambda: calls.append("env-reset"), + ), + "teleop_interface": SimpleNamespace( + reset=lambda: calls.append("device-reset"), request_start=lambda: calls.append("start") + ), + "presentation": SimpleNamespace(publish=lambda *_args, **_kwargs: calls.append("publish-reset")) + if viewer + else None, + } + exec(compile(ast.Module(body=[*helpers, *body], type_ignores=[]), str(source_path), "exec"), namespace) + expected = ["sim-reset"] if hard_reset else [] + expected.extend(["recorder-reset", "env-reset", "device-reset", "start"]) + if viewer: + expected.append("publish-reset") + assert calls == expected + + assert namespace["action_state"] == {} + assert namespace["initialized_hand_tracking"] == set() + + +def _load_replay_reset_helpers(): + """Load the replay reset helpers without importing the application launcher.""" + source = (ROOT / "scripts/environments/teleoperation/teleop_replay_agent.py").read_text(encoding="utf-8") + tree = ast.parse(source) + helper_names = {"_replay_requires_hard_sim_reset", "_reset_replay_environment"} + helper_nodes = [node for node in tree.body if isinstance(node, ast.FunctionDef) and node.name in helper_names] + assert {node.name for node in helper_nodes} == helper_names + namespace: dict[str, object] = {} + exec(compile(ast.Module(body=helper_nodes, type_ignores=[]), "", "exec"), namespace) + return namespace + + +def test_teleop_replay_reset_policy_skips_hard_reset_only_for_newton(): + """Newton keeps its constructed solver while OVPhysX retains the PR-5507 hard reset.""" + requires_hard_reset = _load_replay_reset_helpers()["_replay_requires_hard_sim_reset"] + + assert requires_hard_reset("newton_mjwarp") is False + assert requires_hard_reset("newtonmanager") is False + assert requires_hard_reset("ovphysx") is True + assert requires_hard_reset(None) is True + + +def test_teleop_replay_reset_sequence_preserves_backend_owned_order(): + """The backend policy changes only the optional simulator reset in the reset sequence.""" + reset_environment = _load_replay_reset_helpers()["_reset_replay_environment"] + calls: list[str] = [] + + class _Resetter: + def __init__(self, name: str): + self._name = name + + def reset(self): + calls.append(self._name) + + class _Environment: + sim = _Resetter("sim") + recorder_manager = _Resetter("recorder") + + def reset(self): + calls.append("env") + + env = _Environment() + reset_environment(env, hard_sim_reset=False, reset_recorder=True) + assert calls == ["recorder", "env"] + + calls.clear() + reset_environment(env, hard_sim_reset=True, reset_recorder=True) + assert calls == ["sim", "recorder", "env"] + + calls.clear() + reset_environment(env, hard_sim_reset=False, reset_recorder=False) + assert calls == ["env"] + + +def test_teleop_replay_routes_initial_and_recorded_resets_through_backend_policy(): + """Both replay reset sites must use the same backend-aware hard-reset decision.""" + source = (ROOT / "scripts/environments/teleoperation/teleop_replay_agent.py").read_text(encoding="utf-8") + + assert source.count("hard_sim_reset=_replay_requires_hard_sim_reset(env.scene.physics_backend)") == 2 + assert "env.sim.reset()" not in source[source.index("def _handle_reset") :] + + +@pytest.mark.parametrize("use_isaac_teleop", [False, True]) +def test_teleop_se3_agent_reads_action_policy_only_for_isaac_teleop(use_isaac_teleop): + """Exercise the real policy expressions without launching Kit or devices.""" + from types import SimpleNamespace + + source_path = ROOT / "scripts/environments/teleoperation/teleop_se3_agent.py" + tree = ast.parse(source_path.read_text(encoding="utf-8")) + names = {"require_initial_bimanual_tracking", "action_conditioner", "action_validator"} + assignments = [ + node + for node in ast.walk(tree) + if isinstance(node, ast.Assign) + and any(isinstance(target, ast.Name) and target.id in names for target in node.targets) + ] + assert len(assignments) == 3 + policy = SimpleNamespace( + require_initial_bimanual_hand_tracking=True, action_conditioner=object(), action_validator=object() + ) + namespace = { + "prepare_teleop_backend": prepare_teleop_backend, + "prepare_teleop_action": prepare_teleop_action, + "action_state": {}, + "use_isaac_teleop": use_isaac_teleop, + "env_cfg": SimpleNamespace(isaac_teleop=policy) if use_isaac_teleop else SimpleNamespace(), + } + exec(compile(ast.Module(body=assignments, type_ignores=[]), str(source_path), "exec"), namespace) + assert namespace["require_initial_bimanual_tracking"] is use_isaac_teleop + assert namespace["action_conditioner"] is (policy.action_conditioner if use_isaac_teleop else None) + assert namespace["action_validator"] is (policy.action_validator if use_isaac_teleop else None) + + +def _load_teleop_agent(monkeypatch, *arguments): + path = ROOT / "scripts" / "environments" / "teleoperation" / "teleop_se3_agent.py" + monkeypatch.setattr(sys, "argv", [str(path), *arguments]) + spec = importlib.util.spec_from_file_location("_gr1t2_teleop_agent_test", path) + assert spec is not None and spec.loader is not None + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def test_product_xr_default_resolves_cloudxrjs_and_autolaunch(monkeypatch): + agent = _load_teleop_agent(monkeypatch, "--ovxr") + from isaaclab_teleop import CLOUDXR_JS_ENV + + assert agent.args_cli.cloudxr_env == CLOUDXR_JS_ENV + assert agent.args_cli.auto_launch_cloudxr is True + + +@pytest.mark.parametrize( + "task_name,physics", + [ + ("IsaacContrib-PickPlace-GR1T2-Abs", "isaacsim_physx"), + ("IsaacContrib-PickPlace-GR1T2-Abs", "ovphysx"), + ("IsaacContrib-PickPlace-GR1T2-Abs", "newton_mjwarp"), + ("IsaacContrib-PickPlace-Locomanipulation-G1-Abs", "ovphysx"), + ("IsaacContrib-PickPlace-Locomanipulation-G1-Abs", "newton_mjwarp"), + ("IsaacContrib-PickPlace-FixedBaseUpperBodyIK-G1-Abs", "ovphysx"), + ("IsaacContrib-PickPlace-FixedBaseUpperBodyIK-G1-Abs", "newton_mjwarp"), + ], +) +def test_teleop_launcher_preserves_the_resolved_physics_configuration(monkeypatch, task_name, physics): + """Task preparation must reach launch validation without losing its backend settings.""" + from isaaclab.app.sim_launcher import validate_launch + + from isaaclab_tasks.utils import parse_env_cfg + + agent = _load_teleop_agent(monkeypatch, "--task", task_name, "--physics", physics) + env_cfg = parse_env_cfg(task_name, device="cpu", num_envs=1, overrides=[f"physics={physics}"]) + physics_cfg = env_cfg.sim.physics + prepare_teleop_backend(env_cfg, physics_cfg) + if physics == "newton_mjwarp": + if "Locomanipulation-G1" in task_name: + assert physics_cfg.num_substeps == 2 + assert physics_cfg.solver_cfg.integrator == "implicitfast" + else: + assert physics_cfg.solver_cfg.nconmax == 4096 + assert physics_cfg.solver_cfg.njmax == 1536 + else: + assert hasattr(env_cfg.scene.packing_table.spawn, "_packing_table_collision_prepared") == (physics == "ovphysx") + + configs, _ = validate_launch(SimpleNamespace(sim=SimpleNamespace(physics=physics_cfg)), vars(agent.args_cli).copy()) + assert not hasattr(agent.args_cli, "physics") + assert env_cfg.sim.physics is physics_cfg + assert any(cfg is physics_cfg for cfg in configs) + + +def test_task_action_hook_validates_before_and_after_conditioning(monkeypatch): + agent = _load_teleop_agent(monkeypatch) + calls = [] + + def validate(action): + calls.append(("validate", action)) + return "rejected" if action == 3 else None + + def condition(env, action, state): + calls.append(("condition", env, action, state)) + state["seen"] = True + return action + 1 + + state = {} + action, issue = agent.prepare_teleop_action("env", 1, condition, validate, state) + assert (action, issue, state) == (2, None, {"seen": True}) + assert [call[0] for call in calls] == ["validate", "condition", "validate"] + + calls.clear() + action, issue = agent.prepare_teleop_action("env", 3, condition, validate, state) + assert (action, issue) == (3, "rejected") + assert [call[0] for call in calls] == ["validate"] + + +def test_teleop_agent_installs_late_graceful_shutdown_handlers(monkeypatch): + agent = _load_teleop_agent(monkeypatch) + requests = [] + installed = {} + + class Session: + def request_shutdown(self): + requests.append("shutdown") + + def install(signum, handler): + installed[signum] = handler + return f"previous-{signum}" + + monkeypatch.setattr(agent.signal, "signal", install) + previous = agent._install_ovxr_shutdown_handlers(Session()) + + assert set(installed) == {signal.SIGINT, signal.SIGTERM} + installed[signal.SIGINT](signal.SIGINT, None) + assert requests == ["shutdown"] + + restored = {} + monkeypatch.setattr(agent.signal, "signal", lambda signum, handler: restored.update({signum: handler})) + agent._restore_ovxr_shutdown_handlers(previous) + assert restored == previous + + +@pytest.mark.parametrize( + ("left_active", "right_active", "expected"), + ( + (False, True, "waiting for initial left hand tracking"), + (True, False, "waiting for initial right hand tracking"), + ), +) +def test_kitless_hand_guard_requires_each_hand_once_then_allows_held_dropout( + monkeypatch, left_active, right_active, expected +): + agent = _load_teleop_agent(monkeypatch) + + class OptionalHand: + def __init__(self, active): + self.is_none = not active + + lifecycle = SimpleNamespace( + HAND_LEFT_KEY="hand_left", + HAND_RIGHT_KEY="hand_right", + last_step_result={ + "hand_left": OptionalHand(left_active), + "hand_right": OptionalHand(right_active), + }, + ) + interface = SimpleNamespace(_session_lifecycle=lifecycle) + initialized = set() + + assert agent._required_hand_tracking_issue(interface, initialized) == expected + lifecycle.last_step_result = {"hand_left": OptionalHand(True), "hand_right": OptionalHand(True)} + # Numeric zero is not a sentinel: a present, valid fully-open hand frame is accepted. + assert agent._required_hand_tracking_issue(interface, initialized) is None + assert initialized == {"left", "right"} + + lifecycle.last_step_result = {"hand_left": OptionalHand(False), "hand_right": OptionalHand(False)} + assert agent._required_hand_tracking_issue(interface, initialized) is None + + initialized.clear() + assert agent._required_hand_tracking_issue(interface, initialized) == ( + "waiting for initial left/right hand tracking" + ) + + +def test_kitless_hand_guard_renders_without_step_and_next_valid_frame_resumes(monkeypatch): + agent = _load_teleop_agent(monkeypatch) + + class Sim: + def __init__(self): + self.render_count = 0 + + def render(self): + self.render_count += 1 + + class Env: + def __init__(self): + self.num_envs = 1 + self.sim = Sim() + self.actions = [] + + def step(self, action): + self.actions.append(action.clone()) + + env = Env() + haptics = {"update": 0, "stop": 0} + + def update(): + haptics["update"] += 1 + + def stop(): + haptics["stop"] += 1 + + valid = torch.arange(36, dtype=torch.float32) + + assert agent._apply_teleop_frame(env, valid, None, update, stop) + assert not agent._apply_teleop_frame(env, torch.zeros_like(valid), "missing left hand tracking", update, stop) + assert agent._apply_teleop_frame(env, valid, None, update, stop) + + assert env.sim.render_count == 1 + assert len(env.actions) == 2 + torch.testing.assert_close(env.actions[0], env.actions[1]) + assert haptics == {"update": 2, "stop": 1} + + +def test_desktop_mirror_cli_is_explicit_and_bounded(monkeypatch): + agent = _load_teleop_agent( + monkeypatch, + "--ovxr", + "--ovxr_desktop_mirror", + "--ovxr_desktop_mirror_fps", + "20", + ) + + assert agent.args_cli.ovxr_desktop_mirror + assert agent.args_cli.ovxr_desktop_mirror_fps == 20.0 + + +def test_auxiliary_camera_pip_cli_is_explicit_and_bounded(monkeypatch): + agent = _load_teleop_agent( + monkeypatch, + "--ovxr", + "--ovxr_pip_camera", + "robot_pov_cam", + "--ovxr_pip_fps", + "20", + ) + + assert agent.args_cli.ovxr_pip_camera == ["robot_pov_cam"] + assert agent.args_cli.ovxr_pip_fps == 20.0 diff --git a/source/isaaclab_teleop/test/test_xr_anchor_manager_contract.py b/source/isaaclab_teleop/test/test_xr_anchor_manager_contract.py new file mode 100644 index 000000000000..8ad6542550cc --- /dev/null +++ b/source/isaaclab_teleop/test/test_xr_anchor_manager_contract.py @@ -0,0 +1,264 @@ +# 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 + +"""Focused tests for kitless XR anchor construction.""" + +from __future__ import annotations + +import sys +from types import ModuleType +from unittest.mock import MagicMock + +import numpy as np +import pytest +from isaaclab_teleop import xr_anchor_manager +from isaaclab_teleop.xr_cfg import XrCfg + +from pxr import Usd, UsdGeom + +from isaaclab.sim.utils.stage import use_stage + + +def test_dynamic_external_anchor_commits_pose_before_input_and_preserves_reset(monkeypatch): + from isaaclab_teleop.xr_cfg import XrAnchorRotationMode + from scipy.spatial.transform import Rotation + + monkeypatch.setattr(xr_anchor_manager, "XRCore", None) + cfg = XrCfg( + anchor_prim_path="/World/Robot", + anchor_pos=(1.0, 2.0, -0.95), + fixed_anchor_height=True, + anchor_rotation_mode=XrAnchorRotationMode.FOLLOW_PRIM, + ) + with use_stage(Usd.Stage.CreateInMemory()): + UsdGeom.Xform.Define(xr_anchor_manager.get_current_stage(), "/World/Robot") + manager = xr_anchor_manager.XrAnchorManager(cfg, ovxr_session=True) + with pytest.raises(RuntimeError, match="committed post-reset"): + manager.get_world_matrix() + reference = np.eye(4) + reference[:3, 3] = (3, 4, 5) + committed = [] + epoch, seed = manager.external_epoch + assert seed + assert manager.commit_external_pose(reference, 0.02, epoch, lambda m: committed.append(m.copy())) + np.testing.assert_allclose(committed[-1][0, :3, 3], (4, 6, 4.05)) + reference[:3, 3] = (8, 9, 12) + reference[:3, :3] = Rotation.from_euler("z", 90, degrees=True).as_matrix() + manager.commit_external_pose(reference, 0.02, epoch, lambda m: committed.append(m.copy())) + expected = committed[-1][0].copy() + expected[:3, :3] = expected[:3, :3] @ manager._OXR_TO_USD_ROTATION + np.testing.assert_allclose(manager.get_world_matrix(), expected, atol=1e-7) + np.testing.assert_allclose(expected[:3, 3], (9, 11, 4.05)) + before = manager.get_world_matrix() + np.testing.assert_array_equal(manager.get_world_matrix(), before) + manager.toggle_anchor_rotation() + reference[:3, :3] = np.eye(3) + manager.commit_external_pose(reference, 0.02, epoch, lambda m: committed.append(m.copy())) + np.testing.assert_allclose(manager.get_world_matrix()[:3, :3], before[:3, :3], atol=1e-7) + manager.reset() + assert not manager.commit_external_pose(reference, 0.02, epoch, lambda _: pytest.fail("old epoch sealed")) + with pytest.raises(RuntimeError, match="committed post-reset"): + manager.get_world_matrix() + manager.commit_external_pose(reference, 0.02, manager.external_epoch[0], lambda m: committed.append(m.copy())) + np.testing.assert_allclose(manager.get_world_matrix()[:3, 3], (9, 11, 11.05)) + with pytest.raises(RuntimeError, match="write failed"): + manager.commit_external_pose( + reference, 0.02, manager.external_epoch[0], lambda _: (_ for _ in ()).throw(RuntimeError("write failed")) + ) + with pytest.raises(RuntimeError, match="publication failed"): + manager.get_world_matrix() + + manager.reset() + with pytest.raises(RuntimeError, match="publication failed"): + manager.commit_external_pose(reference, 0.02, epoch, lambda _: pytest.fail("failed pair revived")) + + +def test_standalone_replay_without_kit_xr_preserves_anchor_fallback(monkeypatch): + monkeypatch.setattr(xr_anchor_manager, "XRCore", None) + monkeypatch.setattr(xr_anchor_manager, "_xr_anchor_prim_exists", lambda _path: True) + cfg = XrCfg(anchor_prim_path="/World/Robot", anchor_pos=(1.0, 2.0, 3.0)) + manager = xr_anchor_manager.XrAnchorManager(cfg) + np.testing.assert_allclose(manager.get_world_matrix()[:3, 3], cfg.anchor_pos) + + +def test_ovxr_session_does_not_borrow_an_available_kit_xr_core(monkeypatch): + core = MagicMock() + monkeypatch.setattr(xr_anchor_manager, "XRCore", core) + with use_stage(Usd.Stage.CreateInMemory()): + manager = xr_anchor_manager.XrAnchorManager(XrCfg(), ovxr_session=True) + assert manager.xr_core is None + core.get_singleton.assert_not_called() + + +@pytest.mark.parametrize("capability", ["none", "live-handles", "recorded-input-viewer"]) +def test_public_device_exposes_dynamic_anchor_only_for_ovxr_session(monkeypatch, capability): + from isaaclab_teleop import isaac_teleop_device + from isaaclab_teleop.isaac_teleop_cfg import IsaacTeleopCfg + + monkeypatch.setattr(xr_anchor_manager, "XRCore", None) + monkeypatch.setattr(isaac_teleop_device, "CommandHandler", MagicMock()) + lifecycle = MagicMock() + monkeypatch.setattr(isaac_teleop_device, "TeleopSessionLifecycle", lifecycle) + cfg = IsaacTeleopCfg( + pipeline_builder=lambda: None, + xr_cfg=XrCfg(anchor_prim_path="/World/Robot"), + ) + kwargs = dict(use_kit_xr_bridge=False) + if capability == "live-handles": + kwargs["external_oxr_handles_provider"] = lambda: None + elif capability == "recorded-input-viewer": + kwargs.update(ovxr_session=True, mcap_replay_path="recording.mcap") + with use_stage(Usd.Stage.CreateInMemory()): + device = isaac_teleop_device.IsaacTeleopDevice(cfg, **kwargs) + assert device.xr_anchor_path == "/World/Robot/XRAnchor" + assert (device.external_anchor_state is not None) == (capability != "none") + lifecycle.assert_called_once() + + +def test_no_xr_core_does_not_acquire_carb_settings(monkeypatch): + settings = ModuleType("carb.settings") + settings.get_settings = MagicMock(side_effect=AssertionError("kitless anchor touched carb settings")) + carb = ModuleType("carb") + carb.settings = settings + monkeypatch.setitem(sys.modules, "carb", carb) + monkeypatch.setitem(sys.modules, "carb.settings", settings) + monkeypatch.setattr(xr_anchor_manager, "XRCore", None) + monkeypatch.setattr(xr_anchor_manager, "_xr_anchor_prim_exists", lambda _path: True) + cfg = XrCfg(anchor_pos=(1.0, 2.0, 3.0), anchor_rot=(0.0, 0.0, 0.0, 1.0)) + + manager = xr_anchor_manager.XrAnchorManager(cfg) + + settings.get_settings.assert_not_called() + np.testing.assert_allclose(manager.get_world_matrix()[:3, 3], cfg.anchor_pos) + + +def test_render_anchor_does_not_offset_tracked_device_poses(monkeypatch): + created_prims = [] + monkeypatch.setattr(xr_anchor_manager, "XRCore", None) + monkeypatch.setattr(xr_anchor_manager, "_xr_anchor_prim_exists", lambda _path: False) + monkeypatch.setattr( + xr_anchor_manager, + "_create_prim", + lambda path, **kwargs: created_prims.append((path, kwargs)), + ) + cfg = XrCfg( + anchor_pos=(0.0, 0.0, 0.0), + render_anchor_pos=(0.0, -2.5, 0.0), + ) + + manager = xr_anchor_manager.XrAnchorManager(cfg) + + assert manager.anchor_headset_path == "/World/XRAnchor" + assert manager.render_anchor_path == "/World/XRRenderAnchor" + np.testing.assert_allclose(manager.get_world_matrix()[:3, 3], cfg.anchor_pos) + assert [path for path, _kwargs in created_prims] == ["/World/XRAnchor", "/World/XRRenderAnchor"] + np.testing.assert_allclose(created_prims[1][1]["position"], cfg.render_anchor_pos) + + +def test_replay_reuses_real_stage_scoped_anchors_without_kit(monkeypatch, caplog): + """A second device must not recreate or overwrite the same USD anchors.""" + stage = Usd.Stage.CreateInMemory() + cfg = XrCfg(anchor_pos=(1.0, 2.0, 3.0), render_anchor_pos=(0.0, -2.5, 0.0)) + monkeypatch.setattr(xr_anchor_manager, "XRCore", None) + with use_stage(stage): + first = xr_anchor_manager.XrAnchorManager(cfg) + first.cleanup() + second = xr_anchor_manager.XrAnchorManager(cfg) + second.cleanup() + assert "Failed to create XR" not in caplog.text + for path, position in (("/World/XRAnchor", cfg.anchor_pos), ("/World/XRRenderAnchor", cfg.render_anchor_pos)): + prim = stage.GetPrimAtPath(path) + assert prim.IsValid() + np.testing.assert_allclose( + UsdGeom.Xformable(prim).ComputeLocalToWorldTransform(0).ExtractTranslation(), position + ) + + +def test_replay_refreshes_configured_static_anchors_on_the_same_stage(monkeypatch): + from isaaclab.sim.utils.transforms import resolve_prim_pose + + stage = Usd.Stage.CreateInMemory() + world = UsdGeom.Xform.Define(stage, "/World") + world.AddTranslateOp().Set((2.0, -1.0, 0.5)) + world.AddRotateZOp().Set(90.0) + monkeypatch.setattr(xr_anchor_manager, "XRCore", None) + first_cfg = XrCfg(anchor_pos=(1.0, 2.0, 3.0), render_anchor_pos=(0.0, -2.5, 0.0)) + next_cfg = XrCfg( + anchor_pos=(-1.0, 0.5, 2.0), + anchor_rot=(0.0, 0.0, 1.0, 0.0), + render_anchor_pos=(3.0, -4.0, 0.25), + render_anchor_rot=(0.0, 0.0, 0.0, 1.0), + ) + with use_stage(stage): + first = xr_anchor_manager.XrAnchorManager(first_cfg, ovxr_session=True) + for path, position in ( + (first.anchor_headset_path, first_cfg.anchor_pos), + (first.render_anchor_path, first_cfg.render_anchor_pos), + ): + observed_pos, observed_rot = resolve_prim_pose(stage.GetPrimAtPath(path)) + np.testing.assert_allclose(observed_pos, position, atol=1e-7) + assert abs(np.dot(observed_rot, first_cfg.anchor_rot)) == pytest.approx(1.0) + first.cleanup() + second = xr_anchor_manager.XrAnchorManager(next_cfg, ovxr_session=True) + for path, position, rotation in ( + (second.anchor_headset_path, next_cfg.anchor_pos, next_cfg.anchor_rot), + (second.render_anchor_path, next_cfg.render_anchor_pos, next_cfg.render_anchor_rot), + ): + observed_pos, observed_rot = resolve_prim_pose(stage.GetPrimAtPath(path)) + np.testing.assert_allclose(observed_pos, position, atol=1e-7) + assert abs(np.dot(observed_rot, rotation)) == pytest.approx(1.0) + np.testing.assert_allclose(second.get_world_matrix()[:3, 3], next_cfg.anchor_pos) + second.cleanup() + + +@pytest.mark.parametrize("anchor_path", ["/World/XRAnchor", "/World/XRRenderAnchor"]) +def test_ovxr_session_rejects_an_unusable_stage_anchor(monkeypatch, anchor_path): + """An invalid anchor must not silently misalign tracking and rendering.""" + stage = Usd.Stage.CreateInMemory() + UsdGeom.Scope.Define(stage, anchor_path) + monkeypatch.setattr(xr_anchor_manager, "XRCore", None) + cfg = XrCfg(render_anchor_pos=(0.0, -2.5, 0.0)) + with use_stage(stage), pytest.raises(ValueError, match="is not transformable"): + xr_anchor_manager.XrAnchorManager(cfg, ovxr_session=True) + + +def test_standalone_replay_keeps_warning_for_an_unusable_anchor(monkeypatch, caplog): + stage = Usd.Stage.CreateInMemory() + UsdGeom.Scope.Define(stage, "/World/XRAnchor") + monkeypatch.setattr(xr_anchor_manager, "XRCore", None) + cfg = XrCfg(anchor_pos=(1.0, 2.0, 3.0)) + with use_stage(stage): + manager = xr_anchor_manager.XrAnchorManager(cfg) + np.testing.assert_allclose(manager.get_world_matrix()[:3, 3], cfg.anchor_pos) + manager.cleanup() + assert "is not transformable" in caplog.text + + +def test_kit_anchor_clock_is_lazy_until_subsequent_smoothed_update(monkeypatch): + from isaaclab_teleop import xr_anchor_utils + from isaaclab_teleop.xr_cfg import XrAnchorRotationMode + + cfg = XrCfg(anchor_prim_path=None) + core = MagicMock() + clock = MagicMock() + clock.instance.side_effect = AssertionError("unused simulation clock") + monkeypatch.setattr(xr_anchor_utils, "SimulationContext", clock) + stage = Usd.Stage.CreateInMemory() + UsdGeom.Xform.Define(stage, "/XRAnchor") + with use_stage(stage): + synchronizer = xr_anchor_utils.XrAnchorSynchronizer(core, cfg, "/XRAnchor") + synchronizer.sync_headset_to_anchor() + core.set_world_transform_matrix.assert_called_once() + clock.instance.assert_not_called() + + cfg.anchor_rotation_mode = XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED + policy = xr_anchor_utils._XrAnchorPosePolicy(cfg) + dt = MagicMock(return_value=0.02) + policy.update(np.eye(4), dt) + dt.assert_not_called() + moved = np.eye(4) + moved[0, 3] = 2.0 + policy.update(moved, dt) + dt.assert_called_once_with() diff --git a/source/isaaclab_teleop/test/test_xr_camera_feed.py b/source/isaaclab_teleop/test/test_xr_camera_feed.py index 143b2edfefb3..84685e3f2fb5 100644 --- a/source/isaaclab_teleop/test/test_xr_camera_feed.py +++ b/source/isaaclab_teleop/test/test_xr_camera_feed.py @@ -544,6 +544,36 @@ def test_manual_layout_preserves_per_feed_transforms(): assert resolved is not cfgs +def test_static_world_panel_pose_applies_feed_offset_in_panel_plane(): + cfg = XrCameraFeedCfg(camera_name="robot_pov_cam", offset_m=(0.0, -0.15)) + layout = XrCameraFeedLayoutCfg( + placement="world", + world_position_m=(0.0, 0.8, 1.6), + world_orientation_xyzw=(0.70710678, 0.0, 0.0, 0.70710678), + ) + + space, position, orientation = camera_feed.resolve_static_panel_pose(cfg, layout) + + assert space == "world" + assert position == pytest.approx((0.0, 0.8, 1.45)) + assert orientation == pytest.approx((0.70710678, 0.0, 0.0, 0.70710678)) + + +def test_static_panel_pose_keeps_viewer_start_and_head_locked_offsets(): + cfg = XrCameraFeedCfg(camera_name="robot_pov_cam", offset_m=(0.1, -0.2), distance_m=0.9) + + assert camera_feed.resolve_static_panel_pose(cfg, XrCameraFeedLayoutCfg()) == ( + "viewer_start", + (0.1, -0.2, -0.9), + (0.0, 0.0, 0.0, 1.0), + ) + assert camera_feed.resolve_static_panel_pose(cfg, XrCameraFeedLayoutCfg(placement="head_locked")) == ( + "head_locked", + (0.1, -0.2, -0.9), + (0.0, 0.0, 0.0, 1.0), + ) + + @pytest.mark.parametrize( ("layout", "expected"), [ diff --git a/source/isaaclab_teleop/test/test_xr_scene_partition.py b/source/isaaclab_teleop/test/test_xr_scene_partition.py new file mode 100644 index 000000000000..771b1a8894b6 --- /dev/null +++ b/source/isaaclab_teleop/test/test_xr_scene_partition.py @@ -0,0 +1,102 @@ +# 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 pytest +from isaaclab_ov.stage import export_stage_to_string +from isaaclab_teleop.xr_scene_partition import partition_visual_meshes + +from pxr import Sdf, Usd, UsdGeom, UsdPhysics + + +def _scene(): + stage = Usd.Stage.CreateInMemory() + env = UsdGeom.Xform.Define(stage, "/World/envs/env_0").GetPrim() + env.CreateAttribute("primvars:omni:scenePartition", Sdf.ValueTypeNames.Token).Set("env_0") + UsdGeom.Mesh.Define(stage, "/Asset/head/mesh") + UsdGeom.Mesh.Define(stage, "/Asset/body/mesh") + visual = UsdGeom.Xform.Define(stage, "/World/envs/env_0/Robot/visuals").GetPrim() + visual.GetReferences().AddInternalReference("/Asset") + visual.SetInstanceable(True) + collision = UsdGeom.Mesh.Define(stage, "/World/envs/env_0/Robot/collisions/head").GetPrim() + UsdPhysics.CollisionAPI.Apply(collision) + UsdGeom.Camera.Define(stage, "/World/envs/env_0/Robot/Camera") + return stage + + +@pytest.mark.parametrize("settings_path", [None, "/Render/AuthoredSettings"]) +def test_partition_export_preserves_source_visibility_physics_and_camera(settings_path): + stage = _scene() + if settings_path is not None: + stage.DefinePrim(settings_path, "RenderSettings") + stage.SetMetadata("renderSettingsPrimPath", settings_path) + original = stage.GetRootLayer().ExportToString() + session = stage.GetSessionLayer().ExportToString() + head = "/World/envs/env_0/Robot/visuals/head/mesh" + with partition_visual_meshes(stage, [head]): + prim = stage.GetPrimAtPath(head) + assert UsdGeom.PrimvarsAPI(prim).FindPrimvarWithInheritance("omni:scenePartition").Get() == "xr_excluded" + assert UsdGeom.Imageable(prim).ComputeVisibility() == "inherited" + for path in ("/World/envs/env_0/Robot/visuals/body/mesh", "/World/envs/env_0/Robot/collisions/head"): + assert ( + UsdGeom.PrimvarsAPI(stage.GetPrimAtPath(path)).FindPrimvarWithInheritance("omni:scenePartition").Get() + == "env_0" + ) + assert stage.GetPrimAtPath("/World/envs/env_0/Robot/Camera").IsA(UsdGeom.Camera) + layer = Sdf.Layer.CreateAnonymous() + layer.ImportFromString(export_stage_to_string(stage, 1, source_paths=("/World/envs/env_0",))) + exported = Usd.Stage.Open(layer) + settings = exported.GetPrimAtPath(exported.GetMetadata("renderSettingsPrimPath")) + assert settings.GetAttribute("omni:rtx:scenePartitioning:showAllPartitionsByDefault").Get() is False + assert ( + UsdGeom.PrimvarsAPI(exported.GetPrimAtPath(head)).FindPrimvarWithInheritance("omni:scenePartition").Get() + == "xr_excluded" + ) + assert stage.GetRootLayer().ExportToString() == original + assert stage.GetSessionLayer().ExportToString() == session + assert stage.GetPrimAtPath(head).IsInstanceProxy() + + +@pytest.mark.parametrize("pattern", ["/missing", "/World/envs/env_0/Robot/collisions/head"]) +def test_reject_missing_or_collision_mesh_before_edit(pattern): + stage = _scene() + original = stage.GetSessionLayer().ExportToString() + with pytest.raises(ValueError): + with partition_visual_meshes(stage, [pattern]): + pass + assert stage.GetSessionLayer().ExportToString() == original + + +def test_export_failure_removes_temporary_layer(): + stage = _scene() + before = stage.GetSessionLayer().ExportToString() + with pytest.raises(RuntimeError): + with partition_visual_meshes(stage, ["/World/envs/env_0/Robot/visuals/head/mesh"]): + raise RuntimeError("export failed") + assert stage.GetSessionLayer().ExportToString() == before + + +@pytest.mark.parametrize("conflict", ["instance", "partition", "show_all"]) +def test_stronger_session_opinion_fails_closed_and_is_preserved(conflict): + stage = _scene() + head = "/World/envs/env_0/Robot/visuals/head/mesh" + with Usd.EditContext(stage, stage.GetSessionLayer()): + visual = stage.GetPrimAtPath("/World/envs/env_0/Robot/visuals") + if conflict == "instance": + visual.SetInstanceable(True) + elif conflict == "partition": + visual.SetInstanceable(False) + UsdGeom.PrimvarsAPI(stage.GetPrimAtPath(head)).CreatePrimvar( + "omni:scenePartition", Sdf.ValueTypeNames.Token, UsdGeom.Tokens.constant + ).Set("env_0") + else: + settings = stage.DefinePrim("/Render/IsaacLabSettings", "RenderSettings") + settings.CreateAttribute( + "omni:rtx:scenePartitioning:showAllPartitionsByDefault", Sdf.ValueTypeNames.Bool + ).Set(True) + before = stage.GetSessionLayer().ExportToString() + with pytest.raises(RuntimeError, match="Stronger source opinion"): + with partition_visual_meshes(stage, [head]): + pytest.fail("A stronger source opinion must not silently defeat the exclusion") + assert stage.GetSessionLayer().ExportToString() == before diff --git a/uv.lock b/uv.lock index f19182077e09..07875c75e540 100644 --- a/uv.lock +++ b/uv.lock @@ -22,7 +22,7 @@ overrides = [ { name = "mujoco-warp", specifier = "~=3.15.0" }, { name = "newton", extras = ["sim"], git = "https://github.com/newton-physics/newton.git?rev=71eeba16a717d4dcf0b9dd7056a7b6f863054dc3" }, { name = "newton-usd-schemas", specifier = ">=0.5.0" }, - { name = "numpy", specifier = ">=2" }, + { name = "numpy", specifier = ">=2.3,<2.4" }, { name = "packaging", specifier = ">=20,<27" }, { name = "torch", specifier = "==2.11.0", index = "https://download.pytorch.org/whl/cu130" }, { name = "torchvision", specifier = "==0.26.0", index = "https://download.pytorch.org/whl/cu130" }, @@ -3503,15 +3503,15 @@ wheels = [ [[package]] name = "numpy" -version = "2.5.1" +version = "2.3.5" source = { registry = "https://pypi.org/simple" } -sdist = { url = "https://files.pythonhosted.org/packages/22/fd/89965aa4ac08c74998539fcbf24fa3540f3e15237fbeb6bcf9c908f4aade/numpy-2.5.1.tar.gz", hash = "sha256:a48a113e6afea91f5608793bafa7ef2ad481fefbda87ec5069f483de61cb9fa3", size = 20755553, upload-time = "2026-07-04T17:08:00.933Z" } +sdist = { url = "https://files.pythonhosted.org/packages/76/65/21b3bc86aac7b8f2862db1e808f1ea22b028e30a225a34a5ede9bf8678f2/numpy-2.3.5.tar.gz", hash = "sha256:784db1dcdab56bf0517743e746dfb0f885fc68d948aba86eeec2cba234bdf1c0", size = 20584950, upload-time = "2025-11-16T22:52:42.067Z" } wheels = [ - { url = "https://files.pythonhosted.org/packages/a9/4b/a2b32dd94ee9ffbeecb28152240042a3949db33b1c834d44090b80e1b3b8/numpy-2.5.1-cp312-cp312-manylinux_2_27_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:61ac47e772e6b8ea489e1d2f441a34c5c3ac17327e7ce294cbdf535795ad4e75", size = 15167977, upload-time = "2026-07-04T17:06:21.621Z" }, - { url = "https://files.pythonhosted.org/packages/b8/a9/6e73d68500f80773f65f0654ea932019d6694329a0eb0ed0533de38df376/numpy-2.5.1-cp312-cp312-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:59fda5e192b570217ec2580c96f00e9a7e12ef6866a900eb089b62c1a32545ca", size = 16672469, upload-time = "2026-07-04T17:06:24.064Z" }, - { url = "https://files.pythonhosted.org/packages/24/7d/ad3e59015135f5261c95fd4cafeff159c955febd83a99a1d9250c4233815/numpy-2.5.1-cp312-cp312-musllinux_1_2_aarch64.whl", hash = "sha256:f7119ebff1a9829e9f431a4f9d28e703023bb6b9fe7c8f724467dbfc27c94ab3", size = 16527531, upload-time = "2026-07-04T17:06:26.69Z" }, - { url = "https://files.pythonhosted.org/packages/83/d0/a39b2fbcde9cb17a1dac678f254b33a6336298af9df338824c685425d5e8/numpy-2.5.1-cp312-cp312-musllinux_1_2_x86_64.whl", hash = "sha256:e824c2acf8862052246be5a44c15da1777940c60d010dd2aab897824d9c430f9", size = 18431940, upload-time = "2026-07-04T17:06:29.521Z" }, - { url = "https://files.pythonhosted.org/packages/65/66/53f31807a48a750f9d748da273bc3fcedd12b27ff1f3e373bfec55ef2dc0/numpy-2.5.1-cp312-cp312-win_amd64.whl", hash = "sha256:f7d60026c0bdb1380e83bfa7a0419c4577ee4b9a08880afcb6dadeb74c649fa2", size = 12430966, upload-time = "2026-07-04T17:06:34.926Z" }, + { url = "https://files.pythonhosted.org/packages/74/78/fcd41e5a0ce4f3f7b003da85825acddae6d7ecb60cf25194741b036ca7d6/numpy-2.3.5-cp312-cp312-manylinux_2_27_aarch64.manylinux_2_28_aarch64.whl", hash = "sha256:8b973c57ff8e184109db042c842423ff4f60446239bd585a5131cc47f06f789d", size = 14250702, upload-time = "2025-11-16T22:49:59.632Z" }, + { url = "https://files.pythonhosted.org/packages/b6/23/2a1b231b8ff672b4c450dac27164a8b2ca7d9b7144f9c02d2396518352eb/numpy-2.3.5-cp312-cp312-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:0d8163f43acde9a73c2a33605353a4f1bc4798745a8b1d73183b28e5b435ae28", size = 16606086, upload-time = "2025-11-16T22:50:02.127Z" }, + { url = "https://files.pythonhosted.org/packages/a0/c5/5ad26fbfbe2012e190cc7d5003e4d874b88bb18861d0829edc140a713021/numpy-2.3.5-cp312-cp312-musllinux_1_2_aarch64.whl", hash = "sha256:51c1e14eb1e154ebd80e860722f9e6ed6ec89714ad2db2d3aa33c31d7c12179b", size = 16025985, upload-time = "2025-11-16T22:50:04.536Z" }, + { url = "https://files.pythonhosted.org/packages/d2/fa/dd48e225c46c819288148d9d060b047fd2a6fb1eb37eae25112ee4cb4453/numpy-2.3.5-cp312-cp312-musllinux_1_2_x86_64.whl", hash = "sha256:b46b4ec24f7293f23adcd2d146960559aaf8020213de8ad1909dba6c013bf89c", size = 18542976, upload-time = "2025-11-16T22:50:07.557Z" }, + { url = "https://files.pythonhosted.org/packages/2d/57/8aeaf160312f7f489dea47ab61e430b5cb051f59a98ae68b7133ce8fa06a/numpy-2.3.5-cp312-cp312-win_amd64.whl", hash = "sha256:86945f2ee6d10cdfd67bcb4069c1662dd711f7e2a4343db5cecec06b87cf31aa", size = 12782922, upload-time = "2025-11-16T22:50:12.811Z" }, ] [[package]]