From f47881fbfa2417c8973bfe557dd2bed6e86c21e6 Mon Sep 17 00:00:00 2001 From: Jiwen Cai Date: Thu, 6 Aug 2026 03:14:57 +0000 Subject: [PATCH 1/4] examples/mujoco_xr: a MuJoCo scene in XR with the SO-101 leader gripper One OpenXR session shared between VizSession (rendering) and TeleopSession (input), with a MuJoCo scene drawn by Vulkan into images viz owns and handed to ProjectionLayer.submit() by CUDA pointer, never through host memory. Nothing else here does that, and MuJoCo's own renderer is OpenGL and cannot hand Vulkan images to viz -- which is why cpp/ exists at all. The scene is the leader gripper and nothing else, locked to the right controller's grip pose, its trigger driven by the shipped SO101GripperRetargeter as a node in the pipeline graph rather than a library call beside it. Two things worth knowing. The renderer computes its own vertex normals: MuJoCo keeps one averaged normal per welded vertex, which on a CAD part smears every crease and renders as shattered facets rather than as a gripper. And the meshes are fetched, not vendored -- scripts/fetch-so-arm.sh pulls them from SO-ARM100 at a pinned commit, checksum-verified, so no build step reaches the network and the app exits at startup naming the script when they are absent. Nothing in .github/workflows installs mujoco, so the example is never configured in CI and none of its tests have run there. Signed-off-by: Jiwen Cai --- .gitignore | 12 + CMakeLists.txt | 1 + .../build_from_source/index.rst | 19 + examples/mujoco_xr/CMakeLists.txt | 190 +++++ examples/mujoco_xr/README.md | 390 ++++++++++ examples/mujoco_xr/cpp/CMakeLists.txt | 138 ++++ examples/mujoco_xr/cpp/compile_shader.cmake | 49 ++ examples/mujoco_xr/cpp/frames.hpp | 96 +++ examples/mujoco_xr/cpp/mesh_buffers.cpp | 105 +++ examples/mujoco_xr/cpp/mesh_buffers.hpp | 71 ++ examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp | 307 ++++++++ examples/mujoco_xr/cpp/render_target.cpp | 409 ++++++++++ examples/mujoco_xr/cpp/render_target.hpp | 154 ++++ examples/mujoco_xr/cpp/scene_renderer.cpp | 717 ++++++++++++++++++ examples/mujoco_xr/cpp/scene_renderer.hpp | 135 ++++ examples/mujoco_xr/cpp/shaders/scene.frag | 48 ++ examples/mujoco_xr/cpp/shaders/scene.vert | 49 ++ examples/mujoco_xr/pyproject.toml | 91 +++ .../mujoco_xr/__init__.py | 29 + .../mujoco_xr/__main__.py | 11 + .../isaacteleop_examples/mujoco_xr/app.py | 582 ++++++++++++++ .../assets/leader/leader_gripper.xml | 110 +++ .../mujoco_xr/assets/scene.xml | 27 + examples/mujoco_xr/scripts/fetch-so-arm.sh | 59 ++ examples/mujoco_xr/tests/CMakeLists.txt | 38 + examples/mujoco_xr/tests/conftest.py | 20 + examples/mujoco_xr/tests/pyproject.toml | 30 + examples/mujoco_xr/tests/test_app_helpers.py | 84 ++ examples/mujoco_xr/tests/test_frames.py | 92 +++ examples/mujoco_xr/tests/test_ghost.py | 498 ++++++++++++ examples/mujoco_xr/tests/test_projection.py | 100 +++ rigs/mujoco_xr.yaml | 27 + 32 files changed, 4688 insertions(+) create mode 100644 examples/mujoco_xr/CMakeLists.txt create mode 100644 examples/mujoco_xr/README.md create mode 100644 examples/mujoco_xr/cpp/CMakeLists.txt create mode 100644 examples/mujoco_xr/cpp/compile_shader.cmake create mode 100644 examples/mujoco_xr/cpp/frames.hpp create mode 100644 examples/mujoco_xr/cpp/mesh_buffers.cpp create mode 100644 examples/mujoco_xr/cpp/mesh_buffers.hpp create mode 100644 examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp create mode 100644 examples/mujoco_xr/cpp/render_target.cpp create mode 100644 examples/mujoco_xr/cpp/render_target.hpp create mode 100644 examples/mujoco_xr/cpp/scene_renderer.cpp create mode 100644 examples/mujoco_xr/cpp/scene_renderer.hpp create mode 100644 examples/mujoco_xr/cpp/shaders/scene.frag create mode 100644 examples/mujoco_xr/cpp/shaders/scene.vert create mode 100644 examples/mujoco_xr/pyproject.toml create mode 100644 examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__init__.py create mode 100644 examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__main__.py create mode 100644 examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py create mode 100644 examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml create mode 100644 examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml create mode 100755 examples/mujoco_xr/scripts/fetch-so-arm.sh create mode 100644 examples/mujoco_xr/tests/CMakeLists.txt create mode 100644 examples/mujoco_xr/tests/conftest.py create mode 100644 examples/mujoco_xr/tests/pyproject.toml create mode 100644 examples/mujoco_xr/tests/test_app_helpers.py create mode 100644 examples/mujoco_xr/tests/test_frames.py create mode 100644 examples/mujoco_xr/tests/test_ghost.py create mode 100644 examples/mujoco_xr/tests/test_projection.py create mode 100644 rigs/mujoco_xr.yaml diff --git a/.gitignore b/.gitignore index cfdda8806..55548cd41 100644 --- a/.gitignore +++ b/.gitignore @@ -56,6 +56,9 @@ dist/ # SPDX report project.spdx +# MuJoCo writes this to the working directory, not next to the model. +MUJOCO_LOG.TXT + # robotic_grounding source bundle (cloned locally / in CI from # jiwenc-nv/v2d:retargeter; never committed -- repopulated deterministically # from deps/v2d/version.txt). The Teleop wheel build copies this subtree @@ -66,3 +69,12 @@ deps/v2d/wheels/ # Runtime device files injected by MCP server infrastructure .mcp.json + +# SO-101 leader-gripper assets, fetched by scripts/fetch-so-arm.sh into the +# package's assets directory, so only the authored wrapper XML is tracked. +# +# Keep this rule HERE, not in examples/mujoco_xr/.gitignore: scikit-build-core +# resolves .gitignore against the project root, so a rule there would strip the +# meshes out of the wheel too. +/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/* +!/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 188304954..85f6b8a85 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -167,6 +167,7 @@ if(BUILD_EXAMPLES) add_subdirectory(examples/haptic_feedback) if(BUILD_VIZ) add_subdirectory(examples/camera_viz/tests) + add_subdirectory(examples/mujoco_xr) endif() elseif(BUILD_EXAMPLE_TELEOP_ROS2) add_subdirectory(examples/teleop_ros2) diff --git a/docs/source/getting_started/build_from_source/index.rst b/docs/source/getting_started/build_from_source/index.rst index 6238274fa..fc9bee4c2 100644 --- a/docs/source/getting_started/build_from_source/index.rst +++ b/docs/source/getting_started/build_from_source/index.rst @@ -26,6 +26,25 @@ Prerequisites - **uv** for Python dependency management and managed Python - **Internet connection** for downloading dependencies via CMake FetchContent +.. note:: + **Optional — only needed to build the Televiz visualization module,** ``BUILD_VIZ``. + ``BUILD_VIZ`` is auto-detected: it defaults to ``ON`` when all three of the following are + found at configure time and to ``OFF`` otherwise, so a core-only source build still + configures on a machine without them. Watch the + ``-- BUILD_VIZ: (Vulkan=... CUDAToolkit=... glslang=...)`` configure line to see + which one is missing. + + - **Vulkan headers + loader** — ``libvulkan-dev`` on Linux, the LunarG SDK on Windows. + - **CUDA Toolkit** (cudart at link time) — ``nvidia-cuda-toolkit`` or the official NVIDIA + installer. + - **glslangValidator** for compiling shaders to SPIR-V — ``glslang-tools`` on Linux, + ``brew install glslang`` on macOS; ships with the Vulkan SDK on Windows. + + ``BUILD_VIZ=ON`` also pulls in GLFW, whose CMake uses ``pkg_check_modules()`` — install + ``pkg-config`` as well, or the configure fails before viz is reached. Most users do not + need any of this: ``pip install isaacteleop`` already ships the compiled ``isaacteleop.viz`` + module. See `Other Build options`_ for the full option table. + .. _one-time-setup: One time setup diff --git a/examples/mujoco_xr/CMakeLists.txt b/examples/mujoco_xr/CMakeLists.txt new file mode 100644 index 000000000..0a74178b0 --- /dev/null +++ b/examples/mujoco_xr/CMakeLists.txt @@ -0,0 +1,190 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Orchestrator for the MuJoCo XR example. Defines no target of its own. +# +# Configured two ways, and the branches below turn on which: +# +# IN-TREE add_subdirectory'd from the root. Builds the extension in place +# for ctest and a bare `pytest`, and installs nothing. +# STANDALONE top-level, as scikit-build-core drives it for +# `uv pip install ./examples/mujoco_xr`. Sets up for itself what +# root scope provided: Python3_EXECUTABLE, pybind11, output path. +# +# SKBUILD is not the discriminator: a plain `cmake -B build examples/mujoco_xr` +# has none of the root scope either and must fail for the same reasons. + +cmake_minimum_required(VERSION 3.20) + +# CMAKE_SOURCE_DIR is set before any project() call, so this is legal here. +# PROJECT_IS_TOP_LEVEL is not -- it needs the project() this branch is deciding +# whether to make. +if(CMAKE_CURRENT_SOURCE_DIR STREQUAL CMAKE_SOURCE_DIR) + set(_mujoco_xr_standalone TRUE) +else() + set(_mujoco_xr_standalone FALSE) +endif() + +if(_mujoco_xr_standalone) + project(mujoco_xr LANGUAGES CXX) + + # Inherited from the root build in the in-tree case; restated because cpp/ + # compiles as C++20 either way. + set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_POSITION_INDEPENDENT_CODE ON) + + # Development.Module, not the bare `Development` that + # cmake/SetupPython.cmake:83 uses under SKBUILD: `Development` also demands + # libpython, which the manylinux and uv-managed interpreters that install + # this wheel frequently do not ship. + find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module) + + # From the PEP-517 build environment, not FetchContent: an isolated wheel + # build should not clone from GitHub, and nothing pybind11-typed crosses + # this module's boundary. scikit-build-core puts the build env's + # site-packages on CMAKE_PREFIX_PATH, which is what finds it. + find_package(pybind11 CONFIG REQUIRED) +endif() + +# There is deliberately no `option(BUILD_EXAMPLE_MUJOCO_XR ...)`: the probe +# below is the whole gate, and an option on top would report ON while the +# example was skipped. + +# ============================================================================== +# MuJoCo, discovered through the build interpreter's wheel +# ============================================================================== +# No find_package(mujoco): the wheel is the only supported source and is what +# the Python side loads at runtime, so one libmujoco serves both languages. + +# In-tree only in practice: standalone, find_package(Python3 REQUIRED) above has +# already failed the configure. +if(NOT DEFINED Python3_EXECUTABLE) + message(STATUS "mujoco_xr: skipped (Python3_EXECUTABLE is not defined)") + return() +endif() + +# Not hardcoded: the pyproject.toml files are read and cross-checked against the +# installed version, so drift fails the configure rather than surfacing as an +# ImportError on the headset. MATCHALL because pyproject.toml carries the pin +# twice and those must agree -- and because it would match a version written in +# prose too, which is why those comments never restate the number. +set(_mujoco_pin_files "pyproject.toml" "tests/pyproject.toml") +set(_mujoco_pins "") +set(_mujoco_pin_labels "") +foreach(_pin_file IN LISTS _mujoco_pin_files) + file(READ "${CMAKE_CURRENT_SOURCE_DIR}/${_pin_file}" _pin_file_text) + string(REGEX MATCHALL "mujoco==[0-9][0-9a-zA-Z._-]*" _pin_matches "${_pin_file_text}") + if(NOT _pin_matches) + message(FATAL_ERROR "mujoco_xr: ${_pin_file} declares no `mujoco==` pin; " + "this file reads that pin instead of hardcoding one.") + endif() + foreach(_pin_match IN LISTS _pin_matches) + # MATCHALL populates no CMAKE_MATCH_, so strip the fixed prefix. + string(REPLACE "mujoco==" "" _pin_version "${_pin_match}") + list(APPEND _mujoco_pins "${_pin_version}") + list(APPEND _mujoco_pin_labels "${_pin_file}") + endforeach() +endforeach() +list(GET _mujoco_pins 0 _mujoco_declared_pin) + +# Before the probe, because the cross-check below only runs on a machine that +# has mujoco. Deduplicated into a COPY: _mujoco_pins must keep one entry per +# pin, or the ZIP_LISTS cross-check silently stops checking the later ones. +set(_mujoco_distinct_pins "${_mujoco_pins}") +list(REMOVE_DUPLICATES _mujoco_distinct_pins) +list(LENGTH _mujoco_distinct_pins _mujoco_distinct_pin_count) +if(NOT _mujoco_distinct_pin_count EQUAL 1) + message(FATAL_ERROR "mujoco_xr: the mujoco pins disagree -- pyproject.toml (both " + "build-system.requires and project.dependencies) and " + "tests/pyproject.toml must all name the SAME mujoco version " + "(exactly one libmujoco may be loaded in the process). " + "Found: ${_mujoco_pins} in ${_mujoco_pin_labels}") +endif() + +execute_process( + COMMAND "${Python3_EXECUTABLE}" -c + "import mujoco, os; print(mujoco.__version__); print(os.path.dirname(mujoco.__file__))" + OUTPUT_VARIABLE _mujoco_probe + ERROR_VARIABLE _mujoco_probe_err + RESULT_VARIABLE _mujoco_probe_rc + OUTPUT_STRIP_TRAILING_WHITESPACE +) +if(NOT _mujoco_probe_rc EQUAL 0) + # Fatal standalone, where the build IS the example: returning would emit a + # valid wheel with no _mujoco_xr*.so in it, and an unexplained ImportError. + if(_mujoco_xr_standalone) + message(FATAL_ERROR "mujoco_xr: '${Python3_EXECUTABLE}' cannot import mujoco, so the " + "extension cannot be compiled and this wheel would contain no " + "_mujoco_xr*.so at all. pyproject.toml's build-system.requires " + "declares it -- either allow build isolation to install it, or " + "pre-install it into this interpreter: " + "uv pip install --python ${Python3_EXECUTABLE} " + "\"mujoco==${_mujoco_declared_pin}\"") + endif() + # In-tree, the one message between a green build and a silently uncompiled + # example, so it names the exact command. Configure CREATES that + # interpreter, hence the re-configure step in the README. + message(STATUS "mujoco_xr: skipped -- '${Python3_EXECUTABLE}' cannot import mujoco. " + "Install it and re-run cmake --preset with: " + "uv pip install --python ${Python3_EXECUTABLE} \"mujoco==${_mujoco_declared_pin}\"") + return() +endif() + +string(REPLACE "\n" ";" _mujoco_probe_lines "${_mujoco_probe}") +list(GET _mujoco_probe_lines 0 _mujoco_version) +list(GET _mujoco_probe_lines 1 _mujoco_dir) +string(STRIP "${_mujoco_version}" _mujoco_version) +string(STRIP "${_mujoco_dir}" _mujoco_dir) + +# After the probe, so a machine with no mujoco gets the skip message rather than +# a pin complaint. Every pin matters: this module links the installed version's +# SONAME while the pyproject pins decide what the wheel build, the app and ctest +# each resolve -- and exactly one libmujoco may be loaded. +foreach(_pin_file _pin IN ZIP_LISTS _mujoco_pin_labels _mujoco_pins) + # STREQUAL, not VERSION_EQUAL: the regex admits PEP-440 suffixes and + # VERSION_EQUAL discards them, reporting EQUAL for exactly the strings that + # need distinguishing. A pin is an exact string. + if(NOT _pin STREQUAL "${_mujoco_version}") + message(FATAL_ERROR + "mujoco_xr: ${_pin_file} pins mujoco==${_pin}, but '${Python3_EXECUTABLE}' has " + "${_mujoco_version}. The C++ module and the Python app must load ONE libmujoco. Either " + "install the declared pin (uv pip install --python ${Python3_EXECUTABLE} " + "\"mujoco==${_pin}\") or update EVERY pin -- pyproject.toml carries it twice " + "(build-system.requires and project.dependencies) and tests/pyproject.toml once -- " + "to ${_mujoco_version}.") + endif() +endforeach() + +file(GLOB _mujoco_libs "${_mujoco_dir}/libmujoco.so.*") +list(LENGTH _mujoco_libs _mujoco_lib_count) +if(NOT _mujoco_lib_count EQUAL 1) + message(FATAL_ERROR "mujoco_xr: expected exactly one libmujoco.so.* in ${_mujoco_dir}, " + "found ${_mujoco_lib_count}: ${_mujoco_libs}") +endif() +# Lowercase on purpose: hand-set variables read through inherited scope, not the +# find_package output MUJOCO_LIBRARY / MUJOCO_INCLUDE_DIR would imply. +list(GET _mujoco_libs 0 _mujoco_library) +set(_mujoco_include_dir "${_mujoco_dir}/include") +if(NOT EXISTS "${_mujoco_include_dir}/mujoco/mujoco.h") + message(FATAL_ERROR "mujoco_xr: ${_mujoco_include_dir}/mujoco/mujoco.h is missing " + "(is this a source checkout rather than a wheel?)") +endif() + +# The line to grep for: a green build does not imply this example compiled. +message(STATUS "mujoco_xr: ON (mujoco=${_mujoco_version} lib=${_mujoco_library})") + +# Handed down explicitly: ${CMAKE_SOURCE_DIR} means different things in the two +# configures, and "../" is not allowed in CMake paths here. +set(_mujoco_xr_root "${CMAKE_CURRENT_SOURCE_DIR}") + +add_subdirectory(cpp) + +# BUILD_TESTING is undefined standalone, so ctest entries are in-tree only -- +# which is right: they need the in-place extension and the repo's isaacteleop. +if(BUILD_TESTING) + add_subdirectory(tests) +endif() + +# No install() rules here, deliberately: the wheel is the only run path, and +# standalone it is cpp/CMakeLists.txt's install(TARGETS) that fills it. diff --git a/examples/mujoco_xr/README.md b/examples/mujoco_xr/README.md new file mode 100644 index 000000000..b9446689e --- /dev/null +++ b/examples/mujoco_xr/README.md @@ -0,0 +1,390 @@ + + +# MuJoCo XR + +A MuJoCo scene rendered stereoscopically into an Isaac Teleop Televiz XR +session, with an SO-101 leader gripper locked to the operator's right hand. + +Single process, single thread, **one** OpenXR session: + +``` +VizSession(kXr) ──get_oxr_handles()──▶ TeleopSession + │ │ + │ vk_device / vk_physical_device │ controller grip poses + ▼ ▼ +_mujoco_xr.Renderer ──__cuda_array_interface__──▶ ProjectionLayer.submit() +``` + +That is the thesis: `VizSession` (rendering) and `TeleopSession` (input) share +one OpenXR session via `get_oxr_handles()`, and a MuJoCo scene drawn with +Vulkan into images viz owns reaches `ProjectionLayer.submit()` by CUDA pointer +with no copy through host memory. Nothing else in this repository does that. + +**`cpp/` exists because of depth, not because of Vulkan.** +`ProjectionLayer.submit()` takes a CUDA-linear buffer rather than a Vulkan +image, so MuJoCo's own OpenGL renderer could in principle reach it through +GL→CUDA interop. What stops that is depth: `cudaGraphicsGLRegisterImage` +registers no depth format and no multisampled renderbuffer, while +`mjrContext.offDepthStencil` is a combined depth+stencil renderbuffer and +`offsamples` defaults to 4. Colour would register; the per-eye D32F this layer +submits for CloudXR reprojection would need a host round-trip through +`mjr_readPixels` or a patched `mjr_makeContext`. It is the assumption here most +worth re-testing — MuJoCo's renderer draws every geom type, with the scene +XML's materials, lights and shadows, where this one draws lit meshes and +nothing else. + +`_mujoco_xr` links `libmujoco`, so this example ships as its own wheel rather +than inside `isaacteleop` — otherwise that wheel's contents would depend on +whether the build host happened to have `mujoco` installed. Exactly one +`libmujoco` may be loaded in the process, because `mjModel*` / `mjData*` +addresses cross the pybind boundary; `__init__.py` imports `mujoco` before the +extension and asserts both report the same version. + +## Status — read this before anything else + +| | | +|---|---| +| **Covered by tests** | [`ctest -L mujoco_xr`](#tests) — the frame conventions, the projection convention, the clock, the ghost overlay and its jaw channel. All **pure CPU**: no GPU, no headset, no runtime, no window system. | +| **Never executed anywhere** | **The app itself.** `kXr` is the only display mode and it needs a headset plus a CloudXR runtime, so the frame loop, the renderer, OpenXR session sharing via `oxr_handles`, controllers on a shared session, the Vulkan→CUDA→`submit()` path and whether the runtime accepts the depth layer are run by no test and by no developer here. | +| **Wrong by construction until calibrated** | The workspace translation, for any scene that adds static content — see [Frames](#frames-cppframeshpp). The shipped ghost-only scene does not show it. | + +Nothing in `.github/workflows/` installs `mujoco`, so the example is never +configured and **not one of its tests has ever run in CI**. Green means one +developer ran it locally. Wiring examples into CI is +[NVIDIA/IsaacTeleop#880](https://github.com/NVIDIA/IsaacTeleop/issues/880). + +## Scope + +Renderer + MuJoCo + rig, and one scene: `assets/scene.xml` — an **SO-101 +leader gripper ghost** locked to the right controller's grip pose, and nothing +else. No table, no blocks, no ground plane: this is an AR scene and passthrough +is the background. + +The ghost is not decoration. It is a real mesh assembly (4 fetched STLs, so it +exercises the `mjGEOM_MESH` path), and locking it to the hand makes the *grip* +calibration visible — whether the tool sits in the hand the way a hand holds +one. It cannot show a wrong `cpp/frames.hpp`: those constants place it and the +renderer undoes them folding it back into the XR reference space, so the ghost +lands in the hand whatever they say. Only static content shows them, and the +shipped scene has none. + +**Its trigger is driven by the shipped `SO101GripperRetargeter`, as a graph +edge** — the retargeter is a `BaseRetargeter` node inside `_build_pipeline()`, +not a library call beside it, and its closedness output reaches `mjData` and +therefore the screen. There is no robot in the scene, so the jaw it drives is +the operator's own trigger; that is enough to show the edge is live, and the +SO-101 that will read the same output arrives with the scene catalogue. + +Two calibrations, and they are different in kind. `cpp/frames.hpp` is a +*convention* fixed by two specs and cannot be wrong at runtime. +`_QUAT_GRIP_FROM_GHOST` / `_POS_GRIP_FROM_GHOST` in `app.py` are a *measurement* +of how a hand holds a tool — where the fist sits on the handle — derived from +the mesh but only checkable on a headset. See [Frames](#frames-cppframeshpp). + +## Build + +**This example is its own wheel, and the wheel is the only way to run it.** + +```bash +uv pip install "isaacteleop[cloudxr]" --find-links=./install/wheels/ # THIS checkout, not PyPI +uv pip install ./examples/mujoco_xr # same environment +python -m isaacteleop_examples.mujoco_xr # needs a headset +``` + +Both wheels must land in **one** environment, and that is the environment +[`rigs/mujoco_xr.yaml`](../../rigs/mujoco_xr.yaml) runs from. `uv pip install` +compiles the extension through scikit-build-core and does not read the CMake +build tree at all. + +You need `uv`, CMake ≥ 3.21, a C++ compiler, the Vulkan SDK/loader, CUDA, and +`glslangValidator` (`apt install glslang-tools`; the scene shaders are compiled +to SPIR-V at build time, and its absence is a hard `FATAL_ERROR` here). Running +the app additionally needs a GPU with Vulkan + CUDA and a headset. **Build +isolation does not cover the non-Python half of that list**: on a host missing +CUDA, the Vulkan loader or `glslangValidator`, the install fails *inside* the +isolated PEP-517 build with the CMake error wrapped in backend output. + +**`pip install -e` is not supported.** An editable install redirects the package +back to the source tree, which is exactly where the in-tree CMake build drops +*its* `_mujoco_xr*.so` — you would silently import that one instead, and the +wrong `.so` imports fine right up until `mjModel*` crosses the boundary. To +iterate, `uv pip install --reinstall-package isaacteleop-examples-mujoco-xr +./examples/mujoco_xr` (the CMake cache persists via `build-dir`, so it stays +incremental). `--reinstall-package` rather than a bare reinstall because the +version is fixed at `0.0.0`, so `uv` would otherwise skip the rebuild. + +### The in-tree CMake build, which is a separate thing + +The example is **also** wired into the root build, and that path is what +[`ctest`](#tests) runs against: it builds `_mujoco_xr*.so` in place beside +`python/isaacteleop_examples/mujoco_xr/__init__.py` and installs nothing. + +So the extension is compiled twice — once here for `ctest`, once by +scikit-build-core for the wheel, whose ABI tag comes from whichever interpreter +installs it. That is a deliberate trade: collapsing it means either shipping the +root build's tree as a wheel with no ABI tag, or dropping the in-tree `ctest` +path. It collapses for real the day `ctest` runs against the *installed* wheel, +which needs a locally published `isaacteleop` to resolve against — the one on +PyPI is a different build from the viz in this checkout. + +Steps 1 and 3 are the same command, and the repetition is not decorative: on a +fresh clone the interpreter in step 2 does not exist until configure creates it, +and **the mujoco probe runs at configure time**, so it has to run again once the +wheel is there. + +```bash +# 1. Configure once to create the build venv. This first pass necessarily +# reports `-- mujoco_xr: skipped ...` — expected, not a failure. +cmake --preset py3.12 -DBUILD_VIZ=ON + +# 2. Install mujoco into the interpreter configure just created. `python -m pip` +# does not work: that venv has no pip. +uv pip install --python build/cmake-cpython-312/teleop_build_venv/bin/python "mujoco==3.11.0" + +# 3. Re-configure. NOW the probe finds mujoco and the example is added. +cmake --preset py3.12 -DBUILD_VIZ=ON + +# 4. Build. There is no `cmake --install` step for this example. +cmake --build --preset py3.12 --parallel +``` + +A green build does **not** mean this example compiled. The reliable check: + +```bash +cmake --preset py3.12 -DBUILD_VIZ=ON 2>&1 | grep '^-- mujoco_xr:' +``` + +The `ON` line names the exact `libmujoco.so.*` that was linked. There is no +`BUILD_EXAMPLE_MUJOCO_XR` flag — the gate is `BUILD_VIZ` plus whether `mujoco` +is importable from the interpreter CMake resolved. + +**The same trap applies to the ctest list.** `tests/CMakeLists.txt` globs +`test_*.py` at configure time, so adding or deleting a test file leaves the +entry list stale until you re-run step 3. + +## Run + +```bash +python -m isaacteleop_examples.mujoco_xr --help # includes CloudXRLauncher's flags +``` + +Through the rig, which starts the CloudXR runtime alongside the app, from the +repository root: + +```bash +python -m isaacteleop.rig rigs/mujoco_xr.yaml +``` + +`{python}` in the rig expands to the interpreter you launch it with, so both +wheels have to be installed *there* — not in the build venv, which has no +`isaacteleop`. Picking up the wrong venv is silent, so check before you start: + +```bash +python -c "import sys, isaacteleop; from isaacteleop_examples import mujoco_xr; print(sys.executable, isaacteleop.__file__, mujoco_xr.__file__)" +``` + +Both packages must come from the same `site-packages`; the app's startup log +prints the `isaacteleop:` line for the same reason. Against a runtime you +started yourself: + +```bash +python -m isaacteleop.cloudxr --accept-eula # one terminal +python -m isaacteleop_examples.mujoco_xr --no-launch-cloudxr-runtime # another +``` + +`--no-launch-cloudxr-runtime` is not cosmetic: omitting it makes the app start +its own runtime, which is right when nothing else has and fatal when something +has (the runtime is a host singleton on WSS port 48322). If no runtime is +running and you pass it anyway, the failure comes out of `VizSession.create` as +an OpenXR error before any of this example's code runs — **no `[mujoco_xr]` +lines at all** is the tell. + +There is one scene and no flag to change it: `assets/scene.xml` is package data +beside the module, and editing it is how you load something else. There is no +desktop or headless mode either; without a headset the only verification path is +[`ctest -L mujoco_xr`](#tests), which exercises no GPU code at all. + +## Conventions you can break + +### Frames (`cpp/frames.hpp`) + +`R_mj_from_xr = Rz(-90) * Rx(+90)`. XR `-Z` → MuJoCo `+x`, XR `+Y` → MuJoCo +`+z`, XR `+X` → MuJoCo `-y`. Testable definition: a point 1 m in front of the +operator at eye height `h` lands at MuJoCo `(+1, 0, h)` before the workspace +translation. `tests/test_frames.py` checks exactly that. It deliberately differs +from `examples/cloudxr_mujoco_teleop/visualize_poses_mujoco_example.py`, which +applies `Rx(+90)` only (XR-forward → MuJoCo `+y`, not REP-103). + +**`kTransMjFromXr` is the lever, and it is a calibration that is routinely +wrong.** `(-1.0, 0.0, -0.73)`, two independent terms: `x` is operator standoff +(the base sits ~1 m in front of the operator), `z` is a floor datum — MuJoCo +`z = 0` is a work surface 0.73 m above the physical floor. That `z` is only +right against a floor-origin reference space, and the session does not ask for +one: viz's default origin is the headset's start pose, i.e. head height. A +scene that puts static content on the work surface owns re-tuning it. +**Neither term may be zeroed.** + +It places static content only. The ghost goes out through `mj_from_xr` and the +renderer folds it back through `xr_from_mj`, so both constants cancel on it and +the shipped scene — which is the ghost and nothing else — is blind to a wrong +value. Judging one means a scene with something world-locked in it. + +There is no recentre keypress and no runtime override: changing the datum means +editing the constant and rebuilding (~8 s). The procedure is to stand where you +intend to work, start the app on such a scene, read the `frames:` line in the +startup log, compare the virtual surface against the real one, and adjust `z`. A +`--workspace-offset` flag was considered and rejected — `Renderer` bakes +`xr_from_mj_` at construction while the ghost's pose is converted per frame, so +a Python-side offset would move the gripper and leave the scene put, which is +precisely the symptom this example exists to disambiguate. + +### Where the ghost sits on the hand (`app.py`) + +A *second* calibration, and a different kind: `_EULER_GRIP_FROM_GHOST_DEG` and +`_POS_GRIP_FROM_GHOST` place the leader gripper on the operator's hand. Without +them the gripper's body origin — the follower's `gripper` datum, up at the wrist +— lands on the grip pose, so the tool hangs off the hand at an arbitrary angle. + +**These are measured on a headset, not derived.** That is the whole provenance: +it is a claim about how a gripper should look in a hand that is actually holding +a *controller*, and nothing headless can settle it. + +A mesh-derived version was tried first and hardware overruled it. It mapped the +handle loop's principal axis onto the fist axis, the loop's centroid onto the +palm, and the jaw assembly forward of the knuckles — i.e. it assumed the hand +goes *through* the loop, the way it would on the real leader device. Measured +against the shipped values, that model puts the loop centroid 56 mm from the +palm and not straddling it at all. The premise was wrong: you are gripping a +controller, so where the loop falls is a question about the controller in the +hand, not about the loop. + +The mesh geometry is still worth knowing when reading the numbers. +`Handle_SO101` is a closed **loop**, not a bar; the jaw assembly sits off to one +side of it, and the jaws run **60.7°** off the loop's long axis. The OpenXR +**grip** frame they are expressed in (`grip/pose`, not `aim/pose`) is `−Z` little +finger → thumb, `+X` into the palm, `+Y` forward through the knuckles. + +**To re-tune.** The rotation is degrees, intrinsic X-then-Y-then-Z — the same +convention as a MuJoCo `euler=` attribute, pinned by a test against a compiled +model rather than asserted here. Change one angle, `uv pip install +--reinstall-package isaacteleop-examples-mujoco-xr ./examples/mujoco_xr`, +relaunch: `Rz` spins the gripper about its own long axis, `Rx` / `Ry` tilt it in +the hand, and `_POS_GRIP_FROM_GHOST` slides it along the grip axes if the angle +is right but the placement is not. **No test asserts a posture**, deliberately — +they cover the machinery, so re-tuning cannot turn them red. The one that +matters asserts the ghost is *rigidly attached* to the grip frame, which is +true of any calibration and false if the correction is composed on the wrong +side. + +**A trap worth keeping even though the derivation is retired.** MuJoCo rewrites +every mesh into its inertial frame, so recovering an STL's own axes needs +`mesh_pos` / `mesh_quat`. Skip that and you get the *handle's* axis back instead +of the jaws', which is self-consistent, passes an axis-only check, and is wrong +by 60°. The shank's own principal axis is no substitute either — it is a +near-isotropic blob (σ₀/σ₁ = 1.26), so its principal direction is noise. + +### Scene assets + +The renderer draws `mjGEOM_MESH` and nothing else (this is an AR scene; +passthrough is the background, so there is no ground plane to draw), which means +a box, sphere or capsule in the XML renders as nothing. Lighting declared in the +XML is inert — +`cpp/shaders/scene.frag` has one hardcoded directional light and `mjvGLCamera` +is bypassed. + +**`cpp/mesh_buffers.cpp` computes its own vertex normals, and must.** MuJoCo +welds an STL's vertices and keeps one averaged normal per welded vertex, so on a +CAD part every crease gets a normal smeared across it; lit one-sided, those +corners drop to `scene.frag`'s 0.35 ambient floor and the gripper renders as +**shattered facets**, which reads as a broken mesh and is not one. Normals are +instead area-averaged over the faces round each corner that lie within +`kCreaseCos`. The measured counts are in `cpp/mesh_buffers.hpp`, and +`test_ghost.py` fails if anyone reverts to `mjModel`'s. + +The ghost's four STLs are **fetched, not vendored** — 2.3 MB of binary in a +source tree is a poor trade when upstream publishes them at a stable commit, and +Git LFS made every clone pay for them. Run it once, then reinstall, because they +are package data: + +```bash +examples/mujoco_xr/scripts/fetch-so-arm.sh # from the repository root +uv pip install --reinstall-package isaacteleop-examples-mujoco-xr ./examples/mujoco_xr +``` + +Nothing fetches at build time: an isolated PEP-517 wheel build must not reach +the network, so the app fails at startup naming the script and `test_ghost.py` +**skips** with the same reason. Downloads are checksum-verified against a pinned +commit — a silently substituted mesh renders as a broken gripper rather than an +error, which has already cost a debugging session. + +The script also pulls `so101_new_calib.urdf`, which is where the trigger's hinge +and its 0..100° travel come from, so it is on disk to check them against. Three of the four +meshes are leader-specific print parts; the fourth is the **STS3215 servo**, +shared with the follower. It is not decoration — `wrist_roll` is a C-shaped +bracket that wraps the servo, so without it the assembly has an open notch where +the motor belongs and reads as a broken asset. + +It declares **two** mocap bodies — the gripper and its trigger — because the +trigger articulates; a jointed child of a mocap body would be a dynamic joint +that `mj_step` integrates gravity into, and a mocap body is kinematic by +construction. + +The ghost is **opaque**, and `test_ghost.py` asserts it. That removes the +draw-order constraint (at alpha 1.0 the depth test decides everything), the +ghost-writes-depth-into-the-reprojection-buffer concern, and the self-overlap +darkening from `cullMode = VK_CULL_MODE_NONE`. A scene that puts a robot under +the ghost and drops the alpha back takes all three on again: `mjv_updateScene` +emits in geom-id order, so the `` must come **last**. Nothing asserts +that ordering today — it only matters below alpha 1.0, so the test belongs with +the scene that needs it. + +**Pass MuJoCo an absolute scene path.** Measured on mujoco 3.11.0, a *relative* +model path mis-composes the mesh paths of an ``d file in a +subdirectory and fails with `Error opening file ''`. +`DEFAULT_SCENE` in `app.py` is absolute for this reason. + +### Culling + +`cullMode` is `VK_CULL_MODE_NONE` during bring-up, and that is a decision, not +an omission. The projection flips Y (`P[1][1] < 0`), which inverts the effective +winding; get that wrong with culling on and the scene renders **black**, which +is routinely misdiagnosed as a depth or submit bug. Turn it on only after a +headset has confirmed the scene is visible. + +## Tests + +```bash +ctest --test-dir build/cmake-cpython-312 -L mujoco_xr --output-on-failure +``` + +| file | covers | +|---|---| +| `test_frames.py` | the XR→MuJoCo axis map and quaternion order | +| `test_projection.py` | the clip-space convention (Y flip, standard Z, degenerate-fov rejection) | +| `test_app_helpers.py` | the NaN-safe `dt` clamp, the zeroed-`predicted_display_time` guard, the single near/far pair, and that the first-frame projection assertion actually fires | +| `test_ghost.py` | the overlay: that the ghost is opaque, collision-free and carries no mass, that both its bodies are kinematic mocap bodies with no joint anywhere, that the four leader parts form one assembly with sub-mm gaps at the bolted joints and the servo seated in its bracket, that the print STLs are scaled from millimetres and the servo is not, that every corner normal the renderer builds faces the same way as its own triangle (mjModel's do not, and that is what made the ghost render as shattered facets), that the ghost is *rigidly attached* to the grip frame whatever the calibration, that squeezing swings the trigger monotonically from the URDF joint's upper limit to its authored zero without driving the lever through the body, that the shipped `SO101GripperRetargeter` really is the thing driving that channel (built as a real pipeline and fed synthetic DeviceIO snapshots), and that an untracked controller freezes the whole gripper rather than parking it at the scene origin | + +Every one runs on a CPU with no GPU, no headset, no CloudXR runtime and no +window system. Keep it that way: a permanently-skipping test reports green while +covering nothing. + +## Not verified anywhere in CI or on a developer desktop + +**Everything the GPU touches.** The renderer, the Vulkan→CUDA export, +`ProjectionLayer.submit()`, the frame loop that sequences them, OpenXR session +sharing via `oxr_handles`, whether the runtime accepts the depth layer, and +**controllers on a shared session** — none of it is executed by any test or on +any machine here. The grip-to-gripper calibration is a headset-only judgement +by construction: it is a claim about how a hand holds a tool, and no headless +test can confirm it — `tests/test_ghost.py` pins the *machinery* against a +reference calibration and deliberately leaves the shipped constants free to be +tuned. + +Controllers on a shared session have no precedent elsewhere in this repository: +`xrAttachSessionActionSets` is legal once per `XrSession`, Teleop sidesteps it +with `XR_NVX1_action_context`, and the one existing shared-session example +(`examples/oglo_tactile`) exercises only Hand and Head trackers, which use no +actions. Treat that as the likeliest first-run blocker. diff --git a/examples/mujoco_xr/cpp/CMakeLists.txt b/examples/mujoco_xr/cpp/CMakeLists.txt new file mode 100644 index 000000000..7276d7d8a --- /dev/null +++ b/examples/mujoco_xr/cpp/CMakeLists.txt @@ -0,0 +1,138 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# The pybind11 module `_mujoco_xr` -- a MuJoCo -> Vulkan renderer that writes +# into CUDA-visible buffers for viz::ProjectionLayer. +# +# Protocol-only linkage: this module links no viz:: target. It receives +# VkDevice / VkPhysicalDevice / queue-family-index as plain uintptr_t and hands +# back __cuda_array_interface__ objects, so linking viz would buy a dependency +# on its ABI for nothing. For the same reason the pybind11 need not be the one +# the root build FetchContents -- nothing pybind11-registered crosses this +# boundary. Do not pin them together. +# +# Inherited-scope inputs, all set by ../CMakeLists.txt: +# _mujoco_library, _mujoco_include_dir, _mujoco_xr_root, _mujoco_xr_standalone + +cmake_minimum_required(VERSION 3.20) + +find_package(Vulkan REQUIRED) +find_package(CUDAToolkit REQUIRED) + +# ============================================================================== +# Shaders +# ============================================================================== +# Probed here in both configures, like the find_package pair above: this target +# needs the tool, so this target looks for it. Deliberately not reusing the root +# build's `_viz_glslang` cache entry -- writing into it from an example inverts +# the ownership. +find_program(_mujoco_xr_glslang NAMES glslangValidator) +if(NOT _mujoco_xr_glslang) + message(FATAL_ERROR "mujoco_xr: glslangValidator not found; the scene shaders cannot be " + "compiled. Install glslang-tools.") +endif() + +# Module-prefixed so sources include : only +# the generated root goes on the include path, so the target never adds "." +# (rule 3). +set(_shader_gen_root "${CMAKE_CURRENT_BINARY_DIR}/gen") +set(_shader_gen_dir "${_shader_gen_root}/mujoco_xr/shaders") +file(MAKE_DIRECTORY "${_shader_gen_dir}") + +# compile_shader( ): GLSL -> SPIR-V -> constexpr byte +# array header. Local duplicate of src/viz/shaders/cpp/CMakeLists.txt's +# function; see the TODO in compile_shader.cmake for why it is not promoted. +# Appends to `_shader_headers` in the caller's scope so the headers can be +# listed directly as target sources (no extra custom target -- rule 1). +function(compile_shader GLSL_PATH VAR_NAME) + get_filename_component(_glsl_name "${GLSL_PATH}" NAME) + set(_spv_path "${CMAKE_CURRENT_BINARY_DIR}/${_glsl_name}.spv") + set(_header_path "${_shader_gen_dir}/${_glsl_name}.spv.h") + + add_custom_command( + OUTPUT "${_spv_path}" + COMMAND ${_mujoco_xr_glslang} -V "${CMAKE_CURRENT_SOURCE_DIR}/${GLSL_PATH}" -o "${_spv_path}" + DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/${GLSL_PATH}" + COMMENT "Compiling shader ${_glsl_name} -> SPIR-V" + VERBATIM + ) + add_custom_command( + OUTPUT "${_header_path}" + COMMAND ${CMAKE_COMMAND} + -DSPV_PATH=${_spv_path} + -DHEADER_PATH=${_header_path} + -DVAR_NAME=${VAR_NAME} + -P "${CMAKE_CURRENT_SOURCE_DIR}/compile_shader.cmake" + DEPENDS "${_spv_path}" "${CMAKE_CURRENT_SOURCE_DIR}/compile_shader.cmake" + COMMENT "Embedding ${_glsl_name}.spv -> ${VAR_NAME}" + VERBATIM + ) + set(_shader_headers ${_shader_headers} "${_header_path}" PARENT_SCOPE) +endfunction() + +compile_shader(shaders/scene.vert kSceneVertSpv) +compile_shader(shaders/scene.frag kSceneFragSpv) + +# ============================================================================== +# The module +# ============================================================================== +pybind11_add_module(mujoco_xr_py + mujoco_xr_bindings.cpp + mesh_buffers.cpp + render_target.cpp + scene_renderer.cpp + frames.hpp + mesh_buffers.hpp + render_target.hpp + scene_renderer.hpp + ${_shader_headers} +) + +target_include_directories(mujoco_xr_py + PRIVATE + "${_shader_gen_root}" + "${_mujoco_include_dir}" +) + +target_link_libraries(mujoco_xr_py + PRIVATE + Vulkan::Vulkan + # cudart_static matches viz_core (src/viz/core/cpp/CMakeLists.txt): no + # runtime libcudart.so dependency on a machine that has only the driver. + CUDA::cudart_static + "${_mujoco_library}" +) + +target_compile_options(mujoco_xr_py PRIVATE -Wall -Wextra) + +set_target_properties(mujoco_xr_py PROPERTIES + OUTPUT_NAME "_mujoco_xr" + # No RPATH, deliberately -- do not "fix" this. __init__.py imports `mujoco` + # first, so the already-loaded library satisfies our NEEDED entry. An RPATH + # would silently load a second libmujoco and hand mjModel* across two + # copies; without one, a mismatch is a clean ImportError. + BUILD_WITH_INSTALL_RPATH ON + INSTALL_RPATH "" +) + +# ============================================================================== +# Where the .so goes, and it is a different place in each configure +# ============================================================================== +if(_mujoco_xr_standalone) + # CMAKE_INSTALL_PREFIX is scikit-build-core's platlib staging root, so the + # DESTINATION must spell the namespace too -- drop `isaacteleop_examples/` + # and the .so lands outside the package. `sdist.exclude` in + # ../pyproject.toml is what stops a stale in-place .so shipping alongside + # it, and that breakage is intermittent: only a cross-ABI build ships two. + install(TARGETS mujoco_xr_py + LIBRARY DESTINATION isaacteleop_examples/mujoco_xr + ) +else() + # In-tree: drop the .so beside __init__.py, which is what tests/conftest.py + # reaches by prepending python/ to sys.path. ${_mujoco_xr_root} rather than + # ${CMAKE_SOURCE_DIR}, which is this directory in the standalone configure. + # This copies and never removes, so a renamed module leaves a stale .so. + set_target_properties(mujoco_xr_py PROPERTIES + LIBRARY_OUTPUT_DIRECTORY "${_mujoco_xr_root}/python/isaacteleop_examples/mujoco_xr" + ) +endif() diff --git a/examples/mujoco_xr/cpp/compile_shader.cmake b/examples/mujoco_xr/cpp/compile_shader.cmake new file mode 100644 index 000000000..c388952f7 --- /dev/null +++ b/examples/mujoco_xr/cpp/compile_shader.cmake @@ -0,0 +1,49 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Helper script invoked from add_custom_command to convert a SPIR-V binary +# into a C++ header containing an inline constexpr byte array. +# Driven by command-line variables: SPV_PATH, HEADER_PATH, VAR_NAME. +# +# TODO: this is a near-verbatim duplicate of +# src/viz/shaders/cpp/compile_shader.cmake, which hardcodes +# `namespace viz::shaders` in its output and reads SHADERS_GEN_DIR from its +# own directory scope. Promoting it to cmake/ is a viz refactor; it should +# not ride along on an example. + +if(NOT DEFINED SPV_PATH OR NOT DEFINED HEADER_PATH OR NOT DEFINED VAR_NAME) + message(FATAL_ERROR "compile_shader.cmake requires SPV_PATH, HEADER_PATH, VAR_NAME") +endif() + +file(READ "${SPV_PATH}" SPV_CONTENT HEX) +string(LENGTH "${SPV_CONTENT}" SPV_HEX_LEN) +math(EXPR SPV_BYTE_LEN "${SPV_HEX_LEN} / 2") +if(SPV_BYTE_LEN EQUAL 0) + message(FATAL_ERROR "compile_shader.cmake: ${SPV_PATH} is empty") +endif() + +string(REGEX REPLACE "([0-9a-f][0-9a-f])" "0x\\1, " SPV_BYTES "${SPV_CONTENT}") + +set(HEADER_CONTENT +"// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// AUTO-GENERATED FROM ${SPV_PATH} BY compile_shader.cmake. DO NOT EDIT. + +#pragma once + +#include +#include + +namespace mujoco_xr::shaders +{ + +alignas(uint32_t) inline constexpr unsigned char ${VAR_NAME}[] = { + ${SPV_BYTES} +}; +inline constexpr size_t ${VAR_NAME}Size = sizeof(${VAR_NAME}); + +} // namespace mujoco_xr::shaders +") + +file(WRITE "${HEADER_PATH}" "${HEADER_CONTENT}") diff --git a/examples/mujoco_xr/cpp/frames.hpp b/examples/mujoco_xr/cpp/frames.hpp new file mode 100644 index 000000000..1ae6810a0 --- /dev/null +++ b/examples/mujoco_xr/cpp/frames.hpp @@ -0,0 +1,96 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +// The app's only XR<->MuJoCo frame crossing; Python reaches it through the +// pybind module rather than re-deriving it. Rules: +// +// - Quaternions cross as xyzw (OpenXR, and so GRIP_ORIENTATION); MuJoCo is +// wxyz and viz::Pose3D a third spelling. Reorder on every crossing, name +// every variable q_xyzw or q_wxyz, fill boundary structs field by field. +// - R_mj_from_xr = Rz(-90deg) * Rx(+90deg): XR -Z -> MJ +x, +Y -> +z, +// +X -> -y. p_mj = R * p_xr + t; q_mj = q_mj_from_xr (x) q_xr. +// - Name conversions `_from_` so they read by adjacency +// (p_mj = mj_from_xr_pos(p_xr)), never the A_T_B robotics form. + +#include + +#include + +namespace mujoco_xr +{ + +// A handedness convention, fixed by the two specs (OpenXR is y-up / +// -z-forward, MuJoCo is REP-103 z-up), so it cannot be wrong at runtime. If a +// scene's static content appears rotated 90 degrees, this is the bug; the ghost +// cannot show it, because the rotation that places it is undone when the +// renderer folds it back into the XR reference space. +// +// Deliberately diverges from +// examples/cloudxr_mujoco_teleop/visualize_poses_mujoco_example.py, which +// applies Rx(+90) only and maps XR-forward to MuJoCo +y, which is not REP-103. +// Do not "fix" this constant to match it. +inline constexpr std::array kQuatMjFromXr = { 0.5, 0.5, -0.5, -0.5 }; // wxyz + +// A workspace calibration, routinely wrong, and it places static scene content +// only -- it cancels on the ghost, so the shipped scene cannot show it. Two +// independent terms, and zeroing either is a bug: +// x = -1.0 operator standoff, independent of the reference space. +// z = -0.73 floor datum: MuJoCo z=0 is a work surface 0.73 m above the floor. +// Only right against a floor-origin reference space, which this +// session does not ask for -- viz's origin is the headset's start +// pose. A scene with static content owns re-tuning it. +inline constexpr std::array kTransMjFromXr = { -1.0, 0.0, -0.73 }; + +// XR (xyzw) -> MuJoCo world (wxyz). The app's only quaternion crossing. +inline std::array mj_from_xr_quat(const std::array& q_xyzw) +{ + const mjtNum q_wxyz[4] = { q_xyzw[3], q_xyzw[0], q_xyzw[1], q_xyzw[2] }; // reorder + std::array out{}; + mju_mulQuat(out.data(), kQuatMjFromXr.data(), q_wxyz); + return out; +} + +// XR reference-space point -> MuJoCo world point: R * p + t. +inline std::array mj_from_xr_pos(const std::array& p_xr) +{ + std::array out{}; + mju_rotVecQuat(out.data(), p_xr.data(), kQuatMjFromXr.data()); + for (int i = 0; i < 3; ++i) + { + out[i] += kTransMjFromXr[i]; + } + return out; +} + +// Column-major float mat4 of xr_from_mj (the inverse of the above), for +// folding MuJoCo-world geometry into the XR reference space in the renderer: +// p_xr = R^T * (p_mj - t). +inline void xr_from_mj_mat4(float out[16]) +{ + mjtNum r[9]; + mju_quat2Mat(r, kQuatMjFromXr.data()); // row-major R + // Rotation part: R^T, column-major out[c*4 + row] = R^T[row][c] = R[c][row]. + for (int row = 0; row < 3; ++row) + { + for (int c = 0; c < 3; ++c) + { + out[c * 4 + row] = static_cast(r[c * 3 + row]); + } + out[row * 4 + 3] = 0.0f; + } + // Translation: -R^T * t. + for (int row = 0; row < 3; ++row) + { + mjtNum v = 0; + for (int k = 0; k < 3; ++k) + { + v += r[k * 3 + row] * kTransMjFromXr[k]; // R^T[row][k] = R[k][row] + } + out[12 + row] = static_cast(-v); + } + out[12 + 3] = 1.0f; +} + +} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/mesh_buffers.cpp b/examples/mujoco_xr/cpp/mesh_buffers.cpp new file mode 100644 index 000000000..031f4125c --- /dev/null +++ b/examples/mujoco_xr/cpp/mesh_buffers.cpp @@ -0,0 +1,105 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "mesh_buffers.hpp" + +#include +#include +#include +#include + +namespace mujoco_xr +{ + +void build_mesh_buffers(const mjModel* m, MeshBuffers* out) +{ + std::vector& verts = out->verts; + std::vector& indices = out->indices; + verts.clear(); + indices.clear(); + + // Meshes: one vertex per FACE CORNER, carrying a normal computed here. + // See the header for why mjModel's own normals cannot be used. + out->meshes.assign(static_cast(m->nmesh), MeshRange()); + std::vector> face_normal; + std::vector face_area; + std::vector> vertex_faces; + for (int mesh = 0; mesh < m->nmesh; ++mesh) + { + MeshRange& range = out->meshes[static_cast(mesh)]; + range.base_vertex = static_cast(verts.size()); + range.first_index = static_cast(indices.size()); + const float* mverts = m->mesh_vert + 3 * m->mesh_vertadr[mesh]; + const int* mfaces = m->mesh_face + 3 * m->mesh_faceadr[mesh]; + const int facenum = m->mesh_facenum[mesh]; + + // Pass 1: the geometric normal and area of every face, and which faces + // touch each vertex. + face_normal.assign(static_cast(facenum), { 0.0f, 0.0f, 0.0f }); + face_area.assign(static_cast(facenum), 0.0f); + vertex_faces.assign(static_cast(m->mesh_vertnum[mesh]), {}); + for (int f = 0; f < facenum; ++f) + { + const int* face = mfaces + 3 * f; + const float* p[3] = { mverts + 3 * face[0], mverts + 3 * face[1], mverts + 3 * face[2] }; + const float e1[3] = { p[1][0] - p[0][0], p[1][1] - p[0][1], p[1][2] - p[0][2] }; + const float e2[3] = { p[2][0] - p[0][0], p[2][1] - p[0][1], p[2][2] - p[0][2] }; + std::array n = { e1[1] * e2[2] - e1[2] * e2[1], e1[2] * e2[0] - e1[0] * e2[2], + e1[0] * e2[1] - e1[1] * e2[0] }; + const float len = std::sqrt(n[0] * n[0] + n[1] * n[1] + n[2] * n[2]); + face_area[static_cast(f)] = 0.5f * len; + if (len > 0.0f) + { + n[0] /= len; + n[1] /= len; + n[2] /= len; + } + face_normal[static_cast(f)] = n; + for (int k = 0; k < 3; ++k) + { + vertex_faces[static_cast(face[k])].push_back(f); + } + } + + // Pass 2: one vertex per corner, its normal area-averaged over the + // faces round that vertex that lie WITHIN the crease angle of this + // one. Curved surfaces stay smooth; an edge sharper than the threshold + // keeps both of its faces flat. + uint32_t local_count = 0; + for (int f = 0; f < facenum; ++f) + { + const int* face = mfaces + 3 * f; + const std::array& fn = face_normal[static_cast(f)]; + for (int k = 0; k < 3; ++k) + { + float acc[3] = { 0.0f, 0.0f, 0.0f }; + for (int g : vertex_faces[static_cast(face[k])]) + { + const std::array& gn = face_normal[static_cast(g)]; + const float cosine = fn[0] * gn[0] + fn[1] * gn[1] + fn[2] * gn[2]; + if (cosine >= kCreaseCos) + { + const float w = face_area[static_cast(g)]; + acc[0] += gn[0] * w; + acc[1] += gn[1] * w; + acc[2] += gn[2] * w; + } + } + const float len = std::sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]); + Vertex v; + std::memcpy(v.pos, mverts + 3 * face[k], sizeof(v.pos)); + for (int c = 0; c < 3; ++c) + { + // A zero sum needs the face's own normal: it means every + // contribution cancelled, not that the surface has none. + v.normal[c] = len > 0.0f ? acc[c] / len : fn[static_cast(c)]; + } + verts.push_back(v); + indices.push_back(local_count++); + } + } + range.index_count = static_cast(indices.size()) - range.first_index; + } +} + +} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/mesh_buffers.hpp b/examples/mujoco_xr/cpp/mesh_buffers.hpp new file mode 100644 index 000000000..b2d79b400 --- /dev/null +++ b/examples/mujoco_xr/cpp/mesh_buffers.hpp @@ -0,0 +1,71 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +// The welded vertex / index buffers the renderer draws from, built once from +// mjModel. +// +// Normals are computed here, not taken from mjModel: MuJoCo welds an STL's +// vertices and stores one averaged normal per welded vertex (mesh_normalnum == +// mesh_vertnum, mesh_facenormal == mesh_face), so on a CAD part every crease +// gets a normal smeared across it. Measured on the shipped scene under +// test_ghost.py's own predicate (dot(corner normal, own face normal) <= 0): +// 2138 of Wrist_Roll_SO101's 18474 face corners point away from their own +// face, and 9489 of the STS3215's 57240. Lit one-sided those corners drop to +// scene.frag's 0.35 ambient floor and the part renders as shattered facets, a +// shading bug that looks like a broken mesh. So each face gets its own three +// vertices, and each corner an area-weighted average over the faces round it +// that lie within kCreaseCos. +// +// Indices stay mesh-local with a per-mesh base_vertex, which is what +// vkCmdDrawIndexed's vertexOffset consumes directly; absolute indices would +// need every consumer to undo the folding. +// +// No kNearZ / kFarZ here. The Python app owns the clip planes as one named pair +// reaching VizSessionConfig, the projection and the submitted depth; a second +// definition in C++ drifts and makes compositor reprojection wrong on hardware +// nobody can test here. + +#include + +#include +#include + +namespace mujoco_xr +{ + +// The one directional light, in MuJoCo world space, normalized on upload. The +// half-lambert `ambient` term stays a `const float` in shaders/scene.frag: no +// C++ reads it, so hoisting it would cost a uniform to share one float. +inline constexpr float kLightDirWorld[3] = { 0.35f, -0.25f, -1.0f }; + +// Faces meeting at less than this angle are smoothed together; anything +// sharper stays a crease. 35 degrees keeps the SO-101 handle's curve smooth +// and its bolt holes crisp. +inline constexpr float kCreaseCos = 0.819f; // cos(35 deg) + +struct Vertex +{ + float pos[3]; + float normal[3]; +}; + +struct MeshRange +{ + int32_t base_vertex = 0; + uint32_t first_index = 0; + uint32_t index_count = 0; +}; + +struct MeshBuffers +{ + std::vector verts; + std::vector indices; // mesh-local: add base_vertex to deref + std::vector meshes; // indexed by meshid +}; + +// Welds every mesh in `m` into one vertex / index pair. +void build_mesh_buffers(const mjModel* m, MeshBuffers* out); + +} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp b/examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp new file mode 100644 index 000000000..ff2bf7b2b --- /dev/null +++ b/examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp @@ -0,0 +1,307 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 +// +// pybind11 entry point for `mujoco_xr._mujoco_xr`. +// +// Nothing viz-typed crosses this boundary: viz::Pose3D / viz::Fov / ViewInfo +// are registered in the `_viz` module and are not castable here, because this +// module links no viz target. Poses and fovs cross as plain float arrays, +// decomposed on the Python side. +// +// Likewise nothing MuJoCo-typed crosses it: Python owns mjModel / mjData / +// mj_step and passes their addresses as integers; C++ owns mjvScene / +// mjvOption / mjvCamera and calls mjv_updateScene. + +#include "frames.hpp" +#include "mesh_buffers.hpp" +#include "render_target.hpp" +#include "scene_renderer.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace mujoco_xr +{ +namespace +{ + +namespace py = pybind11; + +// A view onto one of the renderer's CUDA-visible staging buffers, shaped for +// viz's `cuda_array_to_viz_buffer` helper, which wants: +// kRGBA8 -> typestr "|u1", shape (H, W, 4) +// kD32F -> typestr "(dev, cfg, reinterpret_cast(model_address)); + } + + SceneRenderer& get() + { + if (!renderer_) + { + throw std::runtime_error("mujoco_xr: renderer has been closed"); + } + return *renderer_; + } + + void close() + { + renderer_.reset(); + } + +private: + std::unique_ptr renderer_; +}; + +} // namespace +} // namespace mujoco_xr + +PYBIND11_MODULE(_mujoco_xr, m) +{ + namespace py = pybind11; + using namespace pybind11::literals; + + m.doc() = "MuJoCo -> Vulkan renderer for Isaac Teleop's Televiz ProjectionLayer."; + + m.def( + "mujoco_version", []() { return std::string(mj_versionString()); }, + "The libmujoco this extension is linked against, as reported at runtime. Compare with " + "mujoco.mj_versionString() -- they MUST be equal, and they are only equal because there is " + "exactly one libmujoco loaded in the process."); + + m.def( + "mesh_triangles", + [](uintptr_t model_address, int meshid) + { + const mjModel* model = reinterpret_cast(model_address); + mujoco_xr::MeshBuffers mb; + mujoco_xr::build_mesh_buffers(model, &mb); + if (meshid < 0 || meshid >= static_cast(mb.meshes.size())) + { + throw std::out_of_range("mujoco_xr: meshid out of range"); + } + const mujoco_xr::MeshRange& r = mb.meshes[static_cast(meshid)]; + std::vector pos, normal; + pos.reserve(r.index_count * 3); + normal.reserve(r.index_count * 3); + const size_t base = static_cast(r.base_vertex); + for (uint32_t i = 0; i < r.index_count; ++i) + { + const mujoco_xr::Vertex& v = mb.verts[base + mb.indices[r.first_index + i]]; + pos.insert(pos.end(), { v.pos[0], v.pos[1], v.pos[2] }); + normal.insert(normal.end(), { v.normal[0], v.normal[1], v.normal[2] }); + } + return std::make_pair(pos, normal); + }, + "model_address"_a, "meshid"_a, + "The vertices the RENDERER draws for one mesh: (positions, normals), both 3 floats per corner in " + "draw order, so a test can check the normals against the geometry they came from. mjModel's own " + "normals are not these -- see cpp/mesh_buffers.hpp."); + + // ── Frames ──────────────────────────────────────────────────────────── + // Exposed rather than reimplemented in Python: kQuatMjFromXr and + // kTransMjFromXr have exactly one definition (frames.hpp) and the Python + // app, the renderer and tests/test_frames.py all read that one. + + m.def( + "mj_from_xr_pos", [](std::array p_xr) { return mujoco_xr::mj_from_xr_pos(p_xr); }, "p_xr"_a, + "XR reference-space point (metres, Y-up) -> MuJoCo world point (Z-up). Applies both the " + "handedness rotation and the workspace translation."); + + m.def( + "mj_from_xr_quat", [](std::array q_xyzw) { return mujoco_xr::mj_from_xr_quat(q_xyzw); }, "q_xyzw"_a, + "XR orientation as xyzw (the order OpenXR and Teleop's GRIP_ORIENTATION use) -> MuJoCo world " + "orientation as wxyz. The ONLY quaternion crossing in the app."); + + // Attributes rather than m.def getters, and SCREAMING_CASE: a getter would + // export as a snake_case attribute, putting `quat_mj_from_xr` beside + // `mj_from_xr_quat` with only word order telling a constant from a + // transform. Immutable tuples; the values and their prose live in + // frames.hpp. + m.attr("QUAT_MJ_FROM_XR") = py::tuple(py::cast(mujoco_xr::kQuatMjFromXr)); + m.attr("TRANS_MJ_FROM_XR") = py::tuple(py::cast(mujoco_xr::kTransMjFromXr)); + + // ── Projection ──────────────────────────────────────────────────────── + + m.def( + "projection_from_fov", + [](std::array fov_lrud, float near_z, float far_z) + { + const auto p = mujoco_xr::projection_from_fov(fov_lrud, near_z, far_z); + return std::vector(p.begin(), p.end()); + }, + "fov_lrud"_a, "near_z"_a, "far_z"_a, + "Column-major 4x4 Vulkan-convention projection from (angle_left, angle_right, angle_up, angle_down) " + "in radians. Same code path the renderer uses; exposed so the clip convention is testable without a " + "GPU. Raises ValueError on a degenerate (all-zero) fov."); + + // ── Renderer ────────────────────────────────────────────────────────── + + py::class_(m, "CudaImageView", + R"doc( +Non-owning CUDA view of one of the renderer's staging buffers. + +Exposes ``__cuda_array_interface__``, which is all +``isaacteleop.viz.ProjectionLayer.submit()`` needs. Do NOT hold one past the +frame it came from, and never past ``Renderer.close()``: the memory belongs to +the renderer. +)doc") + .def_property_readonly("__cuda_array_interface__", &mujoco_xr::CudaImageView::cuda_array_interface); + + py::class_(m, "Renderer", + R"doc( +MuJoCo scene renderer writing into CUDA-visible colour + depth buffers. + +Constructed from a live ``isaacteleop.viz.VizSession``'s raw handles -- it +BORROWS that Vulkan device and queue rather than creating its own, which is +what lets the exported memory be imported by the same CUDA context viz uses. + +Per frame, in this order and on ONE thread:: + + info = session.begin_frame() + if info.should_render: + mujoco.mj_step(model, data) # Python owns the simulation + renderer.update_scene(m_addr, d_addr) + renderer.render(poses, fovs) # poses/fovs from info.views + layer.submit(renderer.color(0), renderer.depth(0), ...) + session.end_frame() + +``render()`` blocks until the GPU work has retired, so the buffers are safe to +submit the moment it returns. +)doc") + .def(py::init(), + "vk_physical_device"_a, "vk_device"_a, "vk_queue_family_index"_a, "width"_a, "height"_a, "view_count"_a, + "near_z"_a, "far_z"_a, "model_address"_a, + "All handles are plain integers: VizSession.vk_physical_device / .vk_device / " + ".vk_queue_family_index, and mujoco.MjModel._address.") + .def( + "update_scene", + [](mujoco_xr::PyRenderer& self, uintptr_t model_address, uintptr_t data_address) + { + return self.get().update_scene( + reinterpret_cast(model_address), reinterpret_cast(data_address)); + }, + "model_address"_a, "data_address"_a, + "One mjv_updateScene for the frame. Call AFTER mj_step, on the same thread. mjData is treated as " + "const. Returns the geom count.") + .def( + "render", + [](mujoco_xr::PyRenderer& self, std::vector poses_xyz_qwxyz, std::vector fovs_lrud) + { + // Releasing the GIL keeps a long GPU wait from blocking the + // interpreter, but it also drops the only mechanical + // serialisation against a second thread calling into viz on the + // same borrowed VkQueue. The single-threaded contract in + // scene_renderer.hpp is now the only thing holding: do not + // multi-thread the frame loop without real queue + // synchronisation. + py::gil_scoped_release release; + self.get().render(poses_xyz_qwxyz, fovs_lrud); + }, + "poses_xyz_qwxyz"_a, "fovs_lrud"_a, + "Render every view. `poses_xyz_qwxyz` is view_count*7 floats (x, y, z, qw, qx, qy, qz) and " + "`fovs_lrud` is view_count*4 (angle_left, angle_right, angle_up, angle_down) -- flatten them " + "from FrameInfo.views. Blocks until the GPU work retires.") + .def( + "projection", + [](mujoco_xr::PyRenderer& self, int view) + { + const auto& p = self.get().projection(view); + return std::vector(p.begin(), p.end()); + }, + "view"_a, + "The column-major 4x4 projection used for `view` on the last render(), so the caller can assert " + "the clip convention per frame.") + .def( + "color", + [](mujoco_xr::PyRenderer& self, int view) + { + const auto& t = self.get().view_target(view); + return mujoco_xr::CudaImageView{ reinterpret_cast(t.color().cuda_ptr()), t.width(), + t.height(), /*is_depth=*/false }; + }, + // keep_alive<0, 1>: the returned CudaImageView is a bare device + // pointer into the Renderer's exported memory. Without this, a + // caller who writes `buf = renderer.color(0)` and drops its last + // reference to `renderer` gets a use-after-free at submit time, + // with no Python-level symptom pointing back here. + py::keep_alive<0, 1>(), "view"_a, + "RGBA8 colour for `view` as a CudaImageView. Valid until the next render().") + .def( + "depth", + [](mujoco_xr::PyRenderer& self, int view) + { + const auto& t = self.get().view_target(view); + return mujoco_xr::CudaImageView{ reinterpret_cast(t.depth().cuda_ptr()), t.width(), + t.height(), /*is_depth=*/true }; + }, + py::keep_alive<0, 1>(), "view"_a, // see color() above + "D32_SFLOAT depth for `view` as a CudaImageView, standard Z: near -> 0.0, far -> 1.0. Valid until " + "the next render().") + .def_property_readonly("view_count", [](mujoco_xr::PyRenderer& self) { return self.get().view_count(); }) + .def_property_readonly("ngeom", [](mujoco_xr::PyRenderer& self) { return self.get().ngeom(); }) + .def_property_readonly("maxgeom", [](mujoco_xr::PyRenderer& self) { return self.get().maxgeom(); }) + .def("close", &mujoco_xr::PyRenderer::close, + "Release the Vulkan and CUDA resources. Must happen BEFORE VizSession.destroy(), since the device " + "is borrowed from it."); +} diff --git a/examples/mujoco_xr/cpp/render_target.cpp b/examples/mujoco_xr/cpp/render_target.cpp new file mode 100644 index 000000000..e3cccba32 --- /dev/null +++ b/examples/mujoco_xr/cpp/render_target.cpp @@ -0,0 +1,409 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "render_target.hpp" + +#include +#include +#include +#include + +// The CUDA RUNTIME API in a plain .cpp, not a .cu and not the driver API. +// The constraint that matters is the file extension: the root project is +// `project(IsaacTeleop ... LANGUAGES CXX)`, so a .cu would need +// enable_language(CUDA) plus an architecture list, and .cu/.cuh escape +// clang-format, REUSE and the copyright-year hook. cudart needs none of that +// -- src/viz/core/cpp/device_image.cpp does exactly this, in a .cpp, against +// cudaImportExternalMemory. Using the runtime API rather than the driver API +// also keeps us in the same primary context viz's cudart already selected, +// which is what makes these pointers legible to ProjectionLayer.submit(). + +namespace mujoco_xr +{ + +// Declared in render_target.hpp: scene_renderer.cpp uses both of these too. +void check_vk(VkResult result, const char* what) +{ + if (result != VK_SUCCESS) + { + throw std::runtime_error(std::string("mujoco_xr: ") + what + " failed: VkResult=" + std::to_string(result)); + } +} + +uint32_t find_memory_type(VkPhysicalDevice physical_device, uint32_t type_bits, VkMemoryPropertyFlags properties) +{ + VkPhysicalDeviceMemoryProperties mem_props; + vkGetPhysicalDeviceMemoryProperties(physical_device, &mem_props); + for (uint32_t i = 0; i < mem_props.memoryTypeCount; ++i) + { + if ((type_bits & (1u << i)) != 0 && (mem_props.memoryTypes[i].propertyFlags & properties) == properties) + { + return i; + } + } + throw std::runtime_error("mujoco_xr: no Vulkan memory type matching requested properties"); +} + +namespace +{ + +// CUDA is used only in this TU, so its check stays with internal linkage. +void check_cuda(cudaError_t result, const char* what) +{ + if (result != cudaSuccess) + { + throw std::runtime_error(std::string("mujoco_xr: ") + what + " failed: " + cudaGetErrorString(result)); + } +} + +constexpr VkFormat kColorFormat = VK_FORMAT_R8G8B8A8_UNORM; +constexpr VkFormat kDepthFormat = VK_FORMAT_D32_SFLOAT; + +void create_attachment(const BorrowedDevice& dev, + uint32_t width, + uint32_t height, + VkFormat format, + VkImageUsageFlags usage, + VkImageAspectFlags aspect, + VkImage* out_image, + VkDeviceMemory* out_memory, + VkImageView* out_view) +{ + VkImageCreateInfo info{}; + info.sType = VK_STRUCTURE_TYPE_IMAGE_CREATE_INFO; + info.imageType = VK_IMAGE_TYPE_2D; + info.format = format; + info.extent = { width, height, 1 }; + info.mipLevels = 1; + info.arrayLayers = 1; + info.samples = VK_SAMPLE_COUNT_1_BIT; + info.tiling = VK_IMAGE_TILING_OPTIMAL; + info.usage = usage; + info.sharingMode = VK_SHARING_MODE_EXCLUSIVE; + info.initialLayout = VK_IMAGE_LAYOUT_UNDEFINED; + check_vk(vkCreateImage(dev.device, &info, nullptr, out_image), "vkCreateImage(attachment)"); + + VkMemoryRequirements reqs; + vkGetImageMemoryRequirements(dev.device, *out_image, &reqs); + VkMemoryAllocateInfo alloc{}; + alloc.sType = VK_STRUCTURE_TYPE_MEMORY_ALLOCATE_INFO; + alloc.allocationSize = reqs.size; + alloc.memoryTypeIndex = + find_memory_type(dev.physical_device, reqs.memoryTypeBits, VK_MEMORY_PROPERTY_DEVICE_LOCAL_BIT); + check_vk(vkAllocateMemory(dev.device, &alloc, nullptr, out_memory), "vkAllocateMemory(attachment)"); + check_vk(vkBindImageMemory(dev.device, *out_image, *out_memory, 0), "vkBindImageMemory(attachment)"); + + VkImageViewCreateInfo view_info{}; + view_info.sType = VK_STRUCTURE_TYPE_IMAGE_VIEW_CREATE_INFO; + view_info.image = *out_image; + view_info.viewType = VK_IMAGE_VIEW_TYPE_2D; + view_info.format = format; + view_info.subresourceRange.aspectMask = aspect; + view_info.subresourceRange.levelCount = 1; + view_info.subresourceRange.layerCount = 1; + check_vk(vkCreateImageView(dev.device, &view_info, nullptr, out_view), "vkCreateImageView(attachment)"); +} + +} // namespace + +// ── ExportedBuffer ───────────────────────────────────────────────────────── + +ExportedBuffer::~ExportedBuffer() +{ + destroy(); +} + +void ExportedBuffer::create(const BorrowedDevice& dev, VkDeviceSize size_bytes) +{ + device_ = dev.device; + size_bytes_ = size_bytes; + + VkExternalMemoryBufferCreateInfo ext_buffer_info{}; + ext_buffer_info.sType = VK_STRUCTURE_TYPE_EXTERNAL_MEMORY_BUFFER_CREATE_INFO; + ext_buffer_info.handleTypes = VK_EXTERNAL_MEMORY_HANDLE_TYPE_OPAQUE_FD_BIT; + + VkBufferCreateInfo info{}; + info.sType = VK_STRUCTURE_TYPE_BUFFER_CREATE_INFO; + info.pNext = &ext_buffer_info; + info.size = size_bytes; + info.usage = VK_BUFFER_USAGE_TRANSFER_DST_BIT; + info.sharingMode = VK_SHARING_MODE_EXCLUSIVE; + check_vk(vkCreateBuffer(device_, &info, nullptr, &buffer_), "vkCreateBuffer(exported)"); + + VkMemoryRequirements reqs; + vkGetBufferMemoryRequirements(device_, buffer_, &reqs); + + VkExportMemoryAllocateInfo export_info{}; + export_info.sType = VK_STRUCTURE_TYPE_EXPORT_MEMORY_ALLOCATE_INFO; + export_info.handleTypes = VK_EXTERNAL_MEMORY_HANDLE_TYPE_OPAQUE_FD_BIT; + + VkMemoryAllocateInfo alloc{}; + alloc.sType = VK_STRUCTURE_TYPE_MEMORY_ALLOCATE_INFO; + alloc.pNext = &export_info; + alloc.allocationSize = reqs.size; + alloc.memoryTypeIndex = + find_memory_type(dev.physical_device, reqs.memoryTypeBits, VK_MEMORY_PROPERTY_DEVICE_LOCAL_BIT); + check_vk(vkAllocateMemory(device_, &alloc, nullptr, &memory_), "vkAllocateMemory(exported)"); + check_vk(vkBindBufferMemory(device_, buffer_, memory_, 0), "vkBindBufferMemory(exported)"); + + auto get_memory_fd = reinterpret_cast(vkGetDeviceProcAddr(device_, "vkGetMemoryFdKHR")); + if (get_memory_fd == nullptr) + { + throw std::runtime_error( + "mujoco_xr: vkGetMemoryFdKHR is not available on the borrowed VkDevice. VizSession is supposed to enable " + "VK_KHR_external_memory_fd on every device it creates -- if this fires, the device did not come from viz."); + } + VkMemoryGetFdInfoKHR fd_info{}; + fd_info.sType = VK_STRUCTURE_TYPE_MEMORY_GET_FD_INFO_KHR; + fd_info.memory = memory_; + fd_info.handleType = VK_EXTERNAL_MEMORY_HANDLE_TYPE_OPAQUE_FD_BIT; + check_vk(get_memory_fd(device_, &fd_info, &memory_fd_), "vkGetMemoryFdKHR"); + + // No cudaSetDevice here, deliberately: viz's VkContext::init() already + // matched the current CUDA device to this Vulkan physical device by UUID + // on this thread, and the app is single-threaded by construction. + cudaExternalMemory_t ext_mem = nullptr; + cudaExternalMemoryHandleDesc ext_desc{}; + ext_desc.type = cudaExternalMemoryHandleTypeOpaqueFd; + ext_desc.handle.fd = memory_fd_; + ext_desc.size = reqs.size; + ext_desc.flags = 0; + check_cuda(cudaImportExternalMemory(&ext_mem, &ext_desc), "cudaImportExternalMemory"); + cuda_external_memory_ = ext_mem; + + // CUDA dup'd the fd on import; close ours so we do not leak one per buffer. + ::close(memory_fd_); + memory_fd_ = -1; + + cudaExternalMemoryBufferDesc buf_desc{}; + buf_desc.offset = 0; + buf_desc.size = size_bytes_; + buf_desc.flags = 0; + check_cuda(cudaExternalMemoryGetMappedBuffer(&cuda_ptr_, ext_mem, &buf_desc), "cudaExternalMemoryGetMappedBuffer"); +} + +void ExportedBuffer::destroy() +{ + if (cuda_ptr_ != nullptr) + { + (void)cudaFree(cuda_ptr_); + cuda_ptr_ = nullptr; + } + if (cuda_external_memory_ != nullptr) + { + (void)cudaDestroyExternalMemory(static_cast(cuda_external_memory_)); + cuda_external_memory_ = nullptr; + } + if (memory_fd_ >= 0) + { + // Only reachable when the import failed before we closed it. + ::close(memory_fd_); + memory_fd_ = -1; + } + if (device_ != VK_NULL_HANDLE) + { + if (buffer_ != VK_NULL_HANDLE) + { + vkDestroyBuffer(device_, buffer_, nullptr); + buffer_ = VK_NULL_HANDLE; + } + if (memory_ != VK_NULL_HANDLE) + { + vkFreeMemory(device_, memory_, nullptr); + memory_ = VK_NULL_HANDLE; + } + } + device_ = VK_NULL_HANDLE; + size_bytes_ = 0; +} + +// ── ViewTarget ───────────────────────────────────────────────────────────── + +ViewTarget::~ViewTarget() +{ + destroy(); +} + +void ViewTarget::create(const BorrowedDevice& dev, VkRenderPass render_pass, uint32_t width, uint32_t height) +{ + device_ = dev.device; + width_ = width; + height_ = height; + + create_attachment(dev, width, height, kColorFormat, + VK_IMAGE_USAGE_COLOR_ATTACHMENT_BIT | VK_IMAGE_USAGE_TRANSFER_SRC_BIT, VK_IMAGE_ASPECT_COLOR_BIT, + &color_image_, &color_memory_, &color_view_); + create_attachment(dev, width, height, kDepthFormat, + VK_IMAGE_USAGE_DEPTH_STENCIL_ATTACHMENT_BIT | VK_IMAGE_USAGE_TRANSFER_SRC_BIT, + VK_IMAGE_ASPECT_DEPTH_BIT, &depth_image_, &depth_memory_, &depth_view_); + + const VkImageView attachments[2] = { color_view_, depth_view_ }; + VkFramebufferCreateInfo fb{}; + fb.sType = VK_STRUCTURE_TYPE_FRAMEBUFFER_CREATE_INFO; + fb.renderPass = render_pass; + fb.attachmentCount = 2; + fb.pAttachments = attachments; + fb.width = width; + fb.height = height; + fb.layers = 1; + check_vk(vkCreateFramebuffer(device_, &fb, nullptr, &framebuffer_), "vkCreateFramebuffer"); + + // Tightly packed, so __cuda_array_interface__ can report strides=None: + // RGBA8 is 4 bytes/px, D32_SFLOAT is 4 bytes/px. + const VkDeviceSize pixels = static_cast(width) * height; + color_staging_.create(dev, pixels * 4); + depth_staging_.create(dev, pixels * 4); +} + +void ViewTarget::destroy() +{ + color_staging_.destroy(); + depth_staging_.destroy(); + if (device_ == VK_NULL_HANDLE) + { + return; + } + if (framebuffer_ != VK_NULL_HANDLE) + { + vkDestroyFramebuffer(device_, framebuffer_, nullptr); + framebuffer_ = VK_NULL_HANDLE; + } + if (color_view_ != VK_NULL_HANDLE) + { + vkDestroyImageView(device_, color_view_, nullptr); + color_view_ = VK_NULL_HANDLE; + } + if (color_image_ != VK_NULL_HANDLE) + { + vkDestroyImage(device_, color_image_, nullptr); + color_image_ = VK_NULL_HANDLE; + } + if (color_memory_ != VK_NULL_HANDLE) + { + vkFreeMemory(device_, color_memory_, nullptr); + color_memory_ = VK_NULL_HANDLE; + } + if (depth_view_ != VK_NULL_HANDLE) + { + vkDestroyImageView(device_, depth_view_, nullptr); + depth_view_ = VK_NULL_HANDLE; + } + if (depth_image_ != VK_NULL_HANDLE) + { + vkDestroyImage(device_, depth_image_, nullptr); + depth_image_ = VK_NULL_HANDLE; + } + if (depth_memory_ != VK_NULL_HANDLE) + { + vkFreeMemory(device_, depth_memory_, nullptr); + depth_memory_ = VK_NULL_HANDLE; + } + device_ = VK_NULL_HANDLE; +} + +void ViewTarget::record_readback(VkCommandBuffer cmd) const +{ + VkBufferImageCopy region{}; + region.bufferOffset = 0; + region.bufferRowLength = 0; // 0 = tightly packed to imageExtent.width + region.bufferImageHeight = 0; + region.imageSubresource.mipLevel = 0; + region.imageSubresource.baseArrayLayer = 0; + region.imageSubresource.layerCount = 1; + region.imageExtent = { width_, height_, 1 }; + + region.imageSubresource.aspectMask = VK_IMAGE_ASPECT_COLOR_BIT; + vkCmdCopyImageToBuffer(cmd, color_image_, VK_IMAGE_LAYOUT_TRANSFER_SRC_OPTIMAL, color_staging_.buffer(), 1, ®ion); + + region.imageSubresource.aspectMask = VK_IMAGE_ASPECT_DEPTH_BIT; + vkCmdCopyImageToBuffer(cmd, depth_image_, VK_IMAGE_LAYOUT_TRANSFER_SRC_OPTIMAL, depth_staging_.buffer(), 1, ®ion); +} + +// ── Render pass ──────────────────────────────────────────────────────────── + +VkRenderPass create_scene_render_pass(VkDevice device) +{ + VkAttachmentDescription attachments[2]{}; + // Colour. clearValue alpha is 0 in the renderer: this is an AR scene and + // the compositor shows passthrough wherever we did not draw. + attachments[0].format = kColorFormat; + attachments[0].samples = VK_SAMPLE_COUNT_1_BIT; + attachments[0].loadOp = VK_ATTACHMENT_LOAD_OP_CLEAR; + attachments[0].storeOp = VK_ATTACHMENT_STORE_OP_STORE; + attachments[0].stencilLoadOp = VK_ATTACHMENT_LOAD_OP_DONT_CARE; + attachments[0].stencilStoreOp = VK_ATTACHMENT_STORE_OP_DONT_CARE; + attachments[0].initialLayout = VK_IMAGE_LAYOUT_UNDEFINED; + // Ends in TRANSFER_SRC so record_readback() needs no extra barrier. + attachments[0].finalLayout = VK_IMAGE_LAYOUT_TRANSFER_SRC_OPTIMAL; + // Depth. STORE, not DONT_CARE: the depth buffer is an output here, not + // scratch -- it goes to XrCompositionLayerDepthInfoKHR via ProjectionLayer. + attachments[1].format = kDepthFormat; + attachments[1].samples = VK_SAMPLE_COUNT_1_BIT; + attachments[1].loadOp = VK_ATTACHMENT_LOAD_OP_CLEAR; + attachments[1].storeOp = VK_ATTACHMENT_STORE_OP_STORE; + attachments[1].stencilLoadOp = VK_ATTACHMENT_LOAD_OP_DONT_CARE; + attachments[1].stencilStoreOp = VK_ATTACHMENT_STORE_OP_DONT_CARE; + attachments[1].initialLayout = VK_IMAGE_LAYOUT_UNDEFINED; + attachments[1].finalLayout = VK_IMAGE_LAYOUT_TRANSFER_SRC_OPTIMAL; + + VkAttachmentReference color_ref{ 0, VK_IMAGE_LAYOUT_COLOR_ATTACHMENT_OPTIMAL }; + VkAttachmentReference depth_ref{ 1, VK_IMAGE_LAYOUT_DEPTH_STENCIL_ATTACHMENT_OPTIMAL }; + + VkSubpassDescription subpass{}; + subpass.pipelineBindPoint = VK_PIPELINE_BIND_POINT_GRAPHICS; + subpass.colorAttachmentCount = 1; + subpass.pColorAttachments = &color_ref; + subpass.pDepthStencilAttachment = &depth_ref; + + // Make the render-pass writes visible to the transfer reads that follow. + VkSubpassDependency deps[2]{}; + deps[0].srcSubpass = VK_SUBPASS_EXTERNAL; + deps[0].dstSubpass = 0; + deps[0].srcStageMask = VK_PIPELINE_STAGE_TRANSFER_BIT; + deps[0].dstStageMask = VK_PIPELINE_STAGE_COLOR_ATTACHMENT_OUTPUT_BIT | VK_PIPELINE_STAGE_EARLY_FRAGMENT_TESTS_BIT; + deps[0].srcAccessMask = VK_ACCESS_TRANSFER_READ_BIT; + deps[0].dstAccessMask = VK_ACCESS_COLOR_ATTACHMENT_WRITE_BIT | VK_ACCESS_DEPTH_STENCIL_ATTACHMENT_WRITE_BIT; + deps[1].srcSubpass = 0; + deps[1].dstSubpass = VK_SUBPASS_EXTERNAL; + deps[1].srcStageMask = VK_PIPELINE_STAGE_COLOR_ATTACHMENT_OUTPUT_BIT | VK_PIPELINE_STAGE_LATE_FRAGMENT_TESTS_BIT; + deps[1].dstStageMask = VK_PIPELINE_STAGE_TRANSFER_BIT; + deps[1].srcAccessMask = VK_ACCESS_COLOR_ATTACHMENT_WRITE_BIT | VK_ACCESS_DEPTH_STENCIL_ATTACHMENT_WRITE_BIT; + deps[1].dstAccessMask = VK_ACCESS_TRANSFER_READ_BIT; + + VkRenderPassCreateInfo info{}; + info.sType = VK_STRUCTURE_TYPE_RENDER_PASS_CREATE_INFO; + info.attachmentCount = 2; + info.pAttachments = attachments; + info.subpassCount = 1; + info.pSubpasses = &subpass; + info.dependencyCount = 2; + info.pDependencies = deps; + + VkRenderPass render_pass = VK_NULL_HANDLE; + check_vk(vkCreateRenderPass(device, &info, nullptr, &render_pass), "vkCreateRenderPass"); + return render_pass; +} + +BorrowedDevice borrow_device(uintptr_t physical_device, uintptr_t device, uint32_t queue_family_index) +{ + BorrowedDevice dev; + dev.physical_device = reinterpret_cast(physical_device); + dev.device = reinterpret_cast(device); + dev.queue_family_index = queue_family_index; + if (dev.physical_device == VK_NULL_HANDLE || dev.device == VK_NULL_HANDLE) + { + throw std::runtime_error( + "mujoco_xr: VizSession handed over a null VkDevice / VkPhysicalDevice. Create the renderer AFTER " + "VizSession.create()."); + } + // queueCount is 1 on both of viz's device-creation paths, so index 0 is + // viz's own queue -- we share it rather than racing a second one. + vkGetDeviceQueue(dev.device, dev.queue_family_index, 0, &dev.queue); + if (dev.queue == VK_NULL_HANDLE) + { + throw std::runtime_error("mujoco_xr: vkGetDeviceQueue returned null for the borrowed queue family"); + } + return dev; +} + +} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/render_target.hpp b/examples/mujoco_xr/cpp/render_target.hpp new file mode 100644 index 000000000..a832af7a9 --- /dev/null +++ b/examples/mujoco_xr/cpp/render_target.hpp @@ -0,0 +1,154 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +// Offscreen colour + depth attachments, and the linear CUDA-visible copies of +// them that viz::ProjectionLayer.submit() consumes. +// +// Both an image and a buffer, because a colour/depth attachment has to be an +// OPTIMAL-tiled VkImage: we render into that, then vkCmdCopyImageToBuffer into +// a tightly-packed VkBuffer whose memory was allocated exportable. CUDA imports +// the buffer and gets the plain linear device pointer that +// __cuda_array_interface__ describes; importing the tiled image directly yields +// a cudaArray_t, which viz::VizBuffer does not model. +// +// The Vulkan device is borrowed from isaacteleop.viz.VizSession, never created +// here. viz already enables VK_KHR_external_memory + VK_KHR_external_memory_fd +// on both device-creation paths, so borrowing gets the export path with no viz +// changes -- and guarantees the CUDA device matches the Vulkan physical device +// by UUID, because VkContext::init() did that match. + +#include + +#include + +namespace mujoco_xr +{ + +// The two Vulkan helpers every TU in this module needs. They live here, and +// not once per .cpp, because scene_renderer.cpp includes this header anyway +// (through scene_renderer.hpp) and two byte-identical copies drift. + +// Throw a std::runtime_error naming `what` unless `result` is VK_SUCCESS. +void check_vk(VkResult result, const char* what); + +// First memory type satisfying both the allocation's type_bits and `properties`. +uint32_t find_memory_type(VkPhysicalDevice physical_device, uint32_t type_bits, VkMemoryPropertyFlags properties); + +// Handles handed over as plain integers by VizSession. Nothing viz-typed. +struct BorrowedDevice +{ + VkPhysicalDevice physical_device = VK_NULL_HANDLE; + VkDevice device = VK_NULL_HANDLE; + uint32_t queue_family_index = 0; + // viz creates its device with queueCount == 1, so index 0 is viz's own + // queue. We share it: one thread, and our submits interleave with viz's + // between begin_frame() and end_frame(). + VkQueue queue = VK_NULL_HANDLE; +}; + +// A VkBuffer whose memory is exported as an fd and imported into CUDA. +class ExportedBuffer +{ +public: + ExportedBuffer() = default; + ~ExportedBuffer(); + + ExportedBuffer(const ExportedBuffer&) = delete; + ExportedBuffer& operator=(const ExportedBuffer&) = delete; + + void create(const BorrowedDevice& dev, VkDeviceSize size_bytes); + void destroy(); + + VkBuffer buffer() const + { + return buffer_; + } + // Linear CUDA device pointer aliasing the same memory. Valid for the + // lifetime of this object. + void* cuda_ptr() const + { + return cuda_ptr_; + } + +private: + VkDevice device_ = VK_NULL_HANDLE; + VkBuffer buffer_ = VK_NULL_HANDLE; + VkDeviceMemory memory_ = VK_NULL_HANDLE; + VkDeviceSize size_bytes_ = 0; + int memory_fd_ = -1; + void* cuda_external_memory_ = nullptr; // cudaExternalMemory_t + void* cuda_ptr_ = nullptr; +}; + +// Everything one eye needs: the attachments, the framebuffer, and the two +// CUDA-visible staging buffers. +class ViewTarget +{ +public: + ViewTarget() = default; + ~ViewTarget(); + + ViewTarget(const ViewTarget&) = delete; + ViewTarget& operator=(const ViewTarget&) = delete; + + void create(const BorrowedDevice& dev, VkRenderPass render_pass, uint32_t width, uint32_t height); + void destroy(); + + VkFramebuffer framebuffer() const + { + return framebuffer_; + } + // Records the two image -> linear-buffer copies. Must be called after + // vkCmdEndRenderPass; the render pass leaves both attachments in + // TRANSFER_SRC_OPTIMAL. + void record_readback(VkCommandBuffer cmd) const; + + const ExportedBuffer& color() const + { + return color_staging_; + } + const ExportedBuffer& depth() const + { + return depth_staging_; + } + uint32_t width() const + { + return width_; + } + uint32_t height() const + { + return height_; + } + +private: + VkDevice device_ = VK_NULL_HANDLE; + uint32_t width_ = 0; + uint32_t height_ = 0; + VkImage color_image_ = VK_NULL_HANDLE; + VkDeviceMemory color_memory_ = VK_NULL_HANDLE; + VkImageView color_view_ = VK_NULL_HANDLE; + VkImage depth_image_ = VK_NULL_HANDLE; + VkDeviceMemory depth_memory_ = VK_NULL_HANDLE; + VkImageView depth_view_ = VK_NULL_HANDLE; + VkFramebuffer framebuffer_ = VK_NULL_HANDLE; + ExportedBuffer color_staging_; + ExportedBuffer depth_staging_; +}; + +// R8G8B8A8_UNORM colour + D32_SFLOAT depth, both stored and both left in +// TRANSFER_SRC_OPTIMAL so record_readback() can copy them straight out. +// +// D32_SFLOAT and NOT a reversed-Z variant: the depth values we hand to +// ProjectionLayer are the raw window-space z, and the projection built in +// scene_renderer.cpp maps z_view = -near -> 0.0 and z_view = -far -> 1.0. +// (Two doc comments in viz say "reverse-Z"; the code is standard Z. Believe +// the code -- and the per-frame assertion in the Python app.) +VkRenderPass create_scene_render_pass(VkDevice device); + +// Borrow VizSession's queue. Separate from BorrowedDevice's aggregate init so +// the caller does not have to declare vkGetDeviceQueue. +BorrowedDevice borrow_device(uintptr_t physical_device, uintptr_t device, uint32_t queue_family_index); + +} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/scene_renderer.cpp b/examples/mujoco_xr/cpp/scene_renderer.cpp new file mode 100644 index 000000000..d4f3a362e --- /dev/null +++ b/examples/mujoco_xr/cpp/scene_renderer.cpp @@ -0,0 +1,717 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "scene_renderer.hpp" + +#include "frames.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace mujoco_xr +{ + +namespace +{ + +// mjvScene capacity. Not a knob: the only failure mode is "the scene has more +// geoms than this", which is a hard error rather than something to tune at +// runtime, and 20k is ~30x what a tabletop scene produces. +constexpr int kMaxGeom = 20000; + +// check_vk() and find_memory_type() come from render_target.hpp. That header +// is included by scene_renderer.hpp and NOT redundantly: BorrowedDevice +// (scene_renderer.hpp:118, by value) and ViewTarget (:134, inside a vector) +// both need to be complete there. + +// Well inside the 128-byte Vulkan-guaranteed push-constant budget. No separate +// normal matrix: every geom drawn here is a mesh, whose mjvGeom.mat is a pure +// rotation, so model's upper 3x3 transforms normals unchanged. +struct PushConstants +{ + float model[16]; // column-major world-from-local (rotation, pos) + float color[4]; +}; +static_assert(sizeof(PushConstants) <= 128, "push constant budget"); + +struct EyeUbo +{ + float viewproj[16]; + float light_dir[4]; +}; + +// out = a * b, column-major 4x4. +void mat4_mul(float out[16], const float a[16], const float b[16]) +{ + float r[16]; + for (int c = 0; c < 4; ++c) + { + for (int row = 0; row < 4; ++row) + { + r[c * 4 + row] = a[0 * 4 + row] * b[c * 4 + 0] + a[1 * 4 + row] * b[c * 4 + 1] + + a[2 * 4 + row] * b[c * 4 + 2] + a[3 * 4 + row] * b[c * 4 + 3]; + } + } + std::memcpy(out, r, sizeof(r)); +} + +// Vulkan-convention projection (y-down clip, depth 0..1) from an OpenXR-style +// asymmetric fov. Algebraically identical to glm::frustumRH_ZO on +// l = n*tan(angleLeft), r = n*tan(angleRight), b = n*tan(angleUp), +// t = n*tan(angleDown) -- note the DELIBERATE angleUp -> bottom swap, which is +// what viz itself does in src/viz/session/cpp/xr_backend.cpp's +// fov_to_projection_matrix. That swap is the y flip; the renderer must NOT +// flip y a second time. +// +// Consequences, all asserted per frame on the Python side: +// out[0] = P[0][0] > 0 +// out[5] = P[1][1] < 0 <- the load-bearing one; it drives winding +// out[10] = P[2][2] < 0, out[11] = P[2][3] == -1, out[14] = P[3][2] < 0 +// i.e. STANDARD Z (z_view = -near -> 0.0, -far -> 1.0), not +// reverse-Z, whatever two stale viz doc comments claim. +void proj_from_fov(const float fov_lrud[4], float near_z, float far_z, float out[16]) +{ + const float tl = std::tan(fov_lrud[0]); + const float tr = std::tan(fov_lrud[1]); + const float tu = std::tan(fov_lrud[2]); + const float td = std::tan(fov_lrud[3]); + std::memset(out, 0, 16 * sizeof(float)); + out[0] = 2.0f / (tr - tl); + out[8] = (tr + tl) / (tr - tl); + out[5] = 2.0f / (td - tu); // (td - tu) < 0 flips y for Vulkan clip space + out[9] = (td + tu) / (td - tu); + out[10] = far_z / (near_z - far_z); + out[14] = (far_z * near_z) / (near_z - far_z); + out[11] = -1.0f; +} + +// Inverse of a rigid pose (the view pose is eye-in-reference-space): +// V = [R^T | -R^T t]. Quaternion arrives as wxyz, matching viz::Pose3D. +void view_from_pose(const float pos[3], const float q_wxyz[4], float out[16]) +{ + const float w = q_wxyz[0]; + const float x = q_wxyz[1]; + const float y = q_wxyz[2]; + const float z = q_wxyz[3]; + // Row-major R from quaternion. + const float R[9] = { 1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y), + 2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x), + 2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y) }; + std::memset(out, 0, 16 * sizeof(float)); + // Column-major out: rotation part = R^T -> out[c*4 + r] = R^T[r][c] = R[c*3 + r]. + for (int r = 0; r < 3; ++r) + { + for (int c = 0; c < 3; ++c) + { + out[c * 4 + r] = R[c * 3 + r]; + } + } + for (int r = 0; r < 3; ++r) + { + out[12 + r] = -(R[0 * 3 + r] * pos[0] + R[1 * 3 + r] * pos[1] + R[2 * 3 + r] * pos[2]); + } + out[15] = 1.0f; +} + +void create_host_buffer(const BorrowedDevice& dev, + VkDeviceSize size, + VkBufferUsageFlags usage, + VkBuffer* out_buffer, + VkDeviceMemory* out_memory) +{ + VkBufferCreateInfo info{}; + info.sType = VK_STRUCTURE_TYPE_BUFFER_CREATE_INFO; + info.size = size; + info.usage = usage; + info.sharingMode = VK_SHARING_MODE_EXCLUSIVE; + check_vk(vkCreateBuffer(dev.device, &info, nullptr, out_buffer), "vkCreateBuffer"); + + VkMemoryRequirements reqs; + vkGetBufferMemoryRequirements(dev.device, *out_buffer, &reqs); + VkMemoryAllocateInfo alloc{}; + alloc.sType = VK_STRUCTURE_TYPE_MEMORY_ALLOCATE_INFO; + alloc.allocationSize = reqs.size; + alloc.memoryTypeIndex = find_memory_type(dev.physical_device, reqs.memoryTypeBits, + VK_MEMORY_PROPERTY_HOST_VISIBLE_BIT | VK_MEMORY_PROPERTY_HOST_COHERENT_BIT); + check_vk(vkAllocateMemory(dev.device, &alloc, nullptr, out_memory), "vkAllocateMemory"); + check_vk(vkBindBufferMemory(dev.device, *out_buffer, *out_memory, 0), "vkBindBufferMemory"); +} + +VkShaderModule make_shader_module(VkDevice device, const unsigned char* code, size_t size_bytes) +{ + VkShaderModuleCreateInfo info{}; + info.sType = VK_STRUCTURE_TYPE_SHADER_MODULE_CREATE_INFO; + info.codeSize = size_bytes; + info.pCode = reinterpret_cast(code); + VkShaderModule module = VK_NULL_HANDLE; + check_vk(vkCreateShaderModule(device, &info, nullptr, &module), "vkCreateShaderModule"); + return module; +} + +} // namespace + +std::array projection_from_fov(const std::array& fov_lrud, float near_z, float far_z) +{ + if (!(near_z > 0.0f) || !(far_z > near_z)) + { + throw std::invalid_argument("mujoco_xr: require 0 < near_z < far_z"); + } + if (fov_lrud[1] <= fov_lrud[0] || fov_lrud[2] <= fov_lrud[3]) + { + throw std::invalid_argument( + "mujoco_xr: degenerate fov (need angle_right > angle_left and angle_up > angle_down). A " + "default-constructed viz::Fov is four zeros, and rendering one yields P[0][0] = +inf with a " + "NaN column -- a blank headset and no error. Fix the FrameInfo.views the session handed over; " + "do not relax this check."); + } + std::array out{}; + proj_from_fov(fov_lrud.data(), near_z, far_z, out.data()); + return out; +} + +SceneRenderer::SceneRenderer(const BorrowedDevice& dev, const Config& config, const mjModel* model) + : dev_(dev), config_(config) +{ + if (config_.width == 0 || config_.height == 0) + { + throw std::invalid_argument("mujoco_xr: renderer resolution must be non-zero"); + } + if (config_.view_count != 2) + { + throw std::invalid_argument("mujoco_xr: view_count must be 2 (stereo); mono is not supported"); + } + if (!(config_.near_z > 0.0f) || !(config_.far_z > config_.near_z)) + { + throw std::invalid_argument("mujoco_xr: require 0 < near_z < far_z (pass the app's single near/far pair)"); + } + if (model == nullptr) + { + throw std::invalid_argument("mujoco_xr: model address is null"); + } + + try + { + xr_from_mj_mat4(xr_from_mj_); + + mjv_defaultOption(&scene_option_); + mjv_defaultFreeCamera(model, &camera_); + mjv_defaultScene(&scene_); + mjv_makeScene(model, &scene_, kMaxGeom); + scene_made_ = true; + + render_pass_ = create_scene_render_pass(dev_.device); + upload_geometry(model); + create_pipeline(); + create_uniforms(); + + view_targets_ = std::vector(config_.view_count); + projections_.assign(config_.view_count, std::array{}); + for (uint32_t i = 0; i < config_.view_count; ++i) + { + view_targets_[i].create(dev_, render_pass_, config_.width, config_.height); + } + } + catch (...) + { + destroy(); + throw; + } +} + +SceneRenderer::~SceneRenderer() +{ + destroy(); +} + +void SceneRenderer::destroy() +{ + if (dev_.device != VK_NULL_HANDLE) + { + (void)vkDeviceWaitIdle(dev_.device); + } + // View targets first: they hold CUDA imports of exported memory, and the + // VkDeviceMemory must outlive the mapping. + view_targets_.clear(); + + if (dev_.device != VK_NULL_HANDLE) + { + for (size_t i = 0; i < ubos_.size(); ++i) + { + if (ubo_mapped_[i] != nullptr) + { + vkUnmapMemory(dev_.device, ubo_memory_[i]); + } + if (ubos_[i] != VK_NULL_HANDLE) + { + vkDestroyBuffer(dev_.device, ubos_[i], nullptr); + } + if (ubo_memory_[i] != VK_NULL_HANDLE) + { + vkFreeMemory(dev_.device, ubo_memory_[i], nullptr); + } + } + ubos_.clear(); + ubo_memory_.clear(); + ubo_mapped_.clear(); + descriptor_sets_.clear(); + + if (fence_ != VK_NULL_HANDLE) + { + vkDestroyFence(dev_.device, fence_, nullptr); + fence_ = VK_NULL_HANDLE; + } + if (command_pool_ != VK_NULL_HANDLE) + { + vkDestroyCommandPool(dev_.device, command_pool_, nullptr); + command_pool_ = VK_NULL_HANDLE; + command_buffer_ = VK_NULL_HANDLE; + } + if (vertex_buffer_ != VK_NULL_HANDLE) + { + vkDestroyBuffer(dev_.device, vertex_buffer_, nullptr); + vertex_buffer_ = VK_NULL_HANDLE; + } + if (vertex_memory_ != VK_NULL_HANDLE) + { + vkFreeMemory(dev_.device, vertex_memory_, nullptr); + vertex_memory_ = VK_NULL_HANDLE; + } + if (index_buffer_ != VK_NULL_HANDLE) + { + vkDestroyBuffer(dev_.device, index_buffer_, nullptr); + index_buffer_ = VK_NULL_HANDLE; + } + if (index_memory_ != VK_NULL_HANDLE) + { + vkFreeMemory(dev_.device, index_memory_, nullptr); + index_memory_ = VK_NULL_HANDLE; + } + if (pipeline_ != VK_NULL_HANDLE) + { + vkDestroyPipeline(dev_.device, pipeline_, nullptr); + pipeline_ = VK_NULL_HANDLE; + } + if (pipeline_layout_ != VK_NULL_HANDLE) + { + vkDestroyPipelineLayout(dev_.device, pipeline_layout_, nullptr); + pipeline_layout_ = VK_NULL_HANDLE; + } + if (descriptor_pool_ != VK_NULL_HANDLE) + { + vkDestroyDescriptorPool(dev_.device, descriptor_pool_, nullptr); + descriptor_pool_ = VK_NULL_HANDLE; + } + if (dsl_ != VK_NULL_HANDLE) + { + vkDestroyDescriptorSetLayout(dev_.device, dsl_, nullptr); + dsl_ = VK_NULL_HANDLE; + } + if (render_pass_ != VK_NULL_HANDLE) + { + vkDestroyRenderPass(dev_.device, render_pass_, nullptr); + render_pass_ = VK_NULL_HANDLE; + } + } + + if (scene_made_) + { + mjv_freeScene(&scene_); + scene_made_ = false; + } + // The geometry index is NOT a Vulkan handle and is the other half of the + // same bug: leaving stale ranges here would let a draw index into a + // destroyed buffer -- in bounds, entirely wrong, and invisible to the + // validation layers. + mesh_ranges_.clear(); +} + +void SceneRenderer::upload_geometry(const mjModel* model) +{ + MeshBuffers mb; + build_mesh_buffers(model, &mb); + mesh_ranges_ = mb.meshes; + + const VkDeviceSize vsize = mb.verts.size() * sizeof(Vertex); + const VkDeviceSize isize = mb.indices.size() * sizeof(uint32_t); + if (vsize == 0 || isize == 0) + { + throw std::runtime_error("mujoco_xr: model produced no renderable geometry"); + } + create_host_buffer(dev_, vsize, VK_BUFFER_USAGE_VERTEX_BUFFER_BIT, &vertex_buffer_, &vertex_memory_); + create_host_buffer(dev_, isize, VK_BUFFER_USAGE_INDEX_BUFFER_BIT, &index_buffer_, &index_memory_); + + void* map = nullptr; + check_vk(vkMapMemory(dev_.device, vertex_memory_, 0, vsize, 0, &map), "vkMapMemory(vertex)"); + std::memcpy(map, mb.verts.data(), vsize); + vkUnmapMemory(dev_.device, vertex_memory_); + check_vk(vkMapMemory(dev_.device, index_memory_, 0, isize, 0, &map), "vkMapMemory(index)"); + std::memcpy(map, mb.indices.data(), isize); + vkUnmapMemory(dev_.device, index_memory_); +} + +void SceneRenderer::create_pipeline() +{ + VkDescriptorSetLayoutBinding binding{}; + binding.binding = 0; + binding.descriptorType = VK_DESCRIPTOR_TYPE_UNIFORM_BUFFER; + binding.descriptorCount = 1; + binding.stageFlags = VK_SHADER_STAGE_VERTEX_BIT | VK_SHADER_STAGE_FRAGMENT_BIT; + VkDescriptorSetLayoutCreateInfo dsl_info{}; + dsl_info.sType = VK_STRUCTURE_TYPE_DESCRIPTOR_SET_LAYOUT_CREATE_INFO; + dsl_info.bindingCount = 1; + dsl_info.pBindings = &binding; + check_vk(vkCreateDescriptorSetLayout(dev_.device, &dsl_info, nullptr, &dsl_), "vkCreateDescriptorSetLayout"); + + VkPushConstantRange pc_range{ VK_SHADER_STAGE_VERTEX_BIT | VK_SHADER_STAGE_FRAGMENT_BIT, 0, sizeof(PushConstants) }; + VkPipelineLayoutCreateInfo pl_info{}; + pl_info.sType = VK_STRUCTURE_TYPE_PIPELINE_LAYOUT_CREATE_INFO; + pl_info.setLayoutCount = 1; + pl_info.pSetLayouts = &dsl_; + pl_info.pushConstantRangeCount = 1; + pl_info.pPushConstantRanges = &pc_range; + check_vk(vkCreatePipelineLayout(dev_.device, &pl_info, nullptr, &pipeline_layout_), "vkCreatePipelineLayout"); + + VkShaderModule vs = make_shader_module(dev_.device, shaders::kSceneVertSpv, shaders::kSceneVertSpvSize); + VkShaderModule fs = make_shader_module(dev_.device, shaders::kSceneFragSpv, shaders::kSceneFragSpvSize); + + VkPipelineShaderStageCreateInfo stages[2]{}; + stages[0].sType = VK_STRUCTURE_TYPE_PIPELINE_SHADER_STAGE_CREATE_INFO; + stages[0].stage = VK_SHADER_STAGE_VERTEX_BIT; + stages[0].module = vs; + stages[0].pName = "main"; + stages[1].sType = VK_STRUCTURE_TYPE_PIPELINE_SHADER_STAGE_CREATE_INFO; + stages[1].stage = VK_SHADER_STAGE_FRAGMENT_BIT; + stages[1].module = fs; + stages[1].pName = "main"; + + VkVertexInputBindingDescription vbind{ 0, sizeof(Vertex), VK_VERTEX_INPUT_RATE_VERTEX }; + VkVertexInputAttributeDescription vattrs[2] = { { 0, 0, VK_FORMAT_R32G32B32_SFLOAT, offsetof(Vertex, pos) }, + { 1, 0, VK_FORMAT_R32G32B32_SFLOAT, offsetof(Vertex, normal) } }; + VkPipelineVertexInputStateCreateInfo vin{}; + vin.sType = VK_STRUCTURE_TYPE_PIPELINE_VERTEX_INPUT_STATE_CREATE_INFO; + vin.vertexBindingDescriptionCount = 1; + vin.pVertexBindingDescriptions = &vbind; + vin.vertexAttributeDescriptionCount = 2; + vin.pVertexAttributeDescriptions = vattrs; + + VkPipelineInputAssemblyStateCreateInfo ia{}; + ia.sType = VK_STRUCTURE_TYPE_PIPELINE_INPUT_ASSEMBLY_STATE_CREATE_INFO; + ia.topology = VK_PRIMITIVE_TOPOLOGY_TRIANGLE_LIST; + + VkPipelineViewportStateCreateInfo vp{}; + vp.sType = VK_STRUCTURE_TYPE_PIPELINE_VIEWPORT_STATE_CREATE_INFO; + vp.viewportCount = 1; + vp.scissorCount = 1; + + VkPipelineRasterizationStateCreateInfo rs{}; + rs.sType = VK_STRUCTURE_TYPE_PIPELINE_RASTERIZATION_STATE_CREATE_INFO; + rs.polygonMode = VK_POLYGON_MODE_FILL; + // CULLING IS OFF DURING BRING-UP, and that is a decision, not an omission. + // MuJoCo geoms are CCW, and the projection above already flips y (P[1][1] + // < 0), which inverts the effective winding. Get that wrong with culling + // ON and the scene renders BLACK, which is routinely misdiagnosed as a + // depth or a submit bug. MuJoCo's mesh assets also mix winding across + // OBJ/STL sources. Turn this on only once a headset has confirmed the + // scene is visible, and only together with frontFace. + rs.cullMode = VK_CULL_MODE_NONE; + rs.frontFace = VK_FRONT_FACE_COUNTER_CLOCKWISE; + rs.lineWidth = 1.0f; + + VkPipelineMultisampleStateCreateInfo ms{}; + ms.sType = VK_STRUCTURE_TYPE_PIPELINE_MULTISAMPLE_STATE_CREATE_INFO; + ms.rasterizationSamples = VK_SAMPLE_COUNT_1_BIT; + + VkPipelineDepthStencilStateCreateInfo ds{}; + ds.sType = VK_STRUCTURE_TYPE_PIPELINE_DEPTH_STENCIL_STATE_CREATE_INFO; + ds.depthTestEnable = VK_TRUE; + ds.depthWriteEnable = VK_TRUE; + ds.depthCompareOp = VK_COMPARE_OP_LESS_OR_EQUAL; + ds.maxDepthBounds = 1.0f; + + // The alpha channel is the AR passthrough mask, so alpha composites + // (A = A_src + (1 - A_src) * A_dst) rather than being replaced: with + // dstAlpha = ZERO a translucent geom drawn over an opaque one would drop + // that pixel's alpha and the compositor would blend passthrough through the + // robot. The result is PREMULTIPLIED, which is what viz's layers declare -- + // it never sets XR_COMPOSITION_LAYER_UNPREMULTIPLIED_ALPHA_BIT. The comment + // at src/viz/session/cpp/xr_backend.cpp:1202-1203 claims straight alpha + // while the code beside it sets no such bit; believe the code. + VkPipelineColorBlendAttachmentState blend{}; + blend.blendEnable = VK_TRUE; // the scene XML may set an rgba alpha < 1 + blend.srcColorBlendFactor = VK_BLEND_FACTOR_SRC_ALPHA; + blend.dstColorBlendFactor = VK_BLEND_FACTOR_ONE_MINUS_SRC_ALPHA; + blend.colorBlendOp = VK_BLEND_OP_ADD; + blend.srcAlphaBlendFactor = VK_BLEND_FACTOR_ONE; + blend.dstAlphaBlendFactor = VK_BLEND_FACTOR_ONE_MINUS_SRC_ALPHA; + blend.alphaBlendOp = VK_BLEND_OP_ADD; + blend.colorWriteMask = + VK_COLOR_COMPONENT_R_BIT | VK_COLOR_COMPONENT_G_BIT | VK_COLOR_COMPONENT_B_BIT | VK_COLOR_COMPONENT_A_BIT; + VkPipelineColorBlendStateCreateInfo cb{}; + cb.sType = VK_STRUCTURE_TYPE_PIPELINE_COLOR_BLEND_STATE_CREATE_INFO; + cb.attachmentCount = 1; + cb.pAttachments = &blend; + + VkDynamicState dyn_states[2] = { VK_DYNAMIC_STATE_VIEWPORT, VK_DYNAMIC_STATE_SCISSOR }; + VkPipelineDynamicStateCreateInfo dyn{}; + dyn.sType = VK_STRUCTURE_TYPE_PIPELINE_DYNAMIC_STATE_CREATE_INFO; + dyn.dynamicStateCount = 2; + dyn.pDynamicStates = dyn_states; + + VkGraphicsPipelineCreateInfo info{}; + info.sType = VK_STRUCTURE_TYPE_GRAPHICS_PIPELINE_CREATE_INFO; + info.stageCount = 2; + info.pStages = stages; + info.pVertexInputState = &vin; + info.pInputAssemblyState = &ia; + info.pViewportState = &vp; + info.pRasterizationState = &rs; + info.pMultisampleState = &ms; + info.pDepthStencilState = &ds; + info.pColorBlendState = &cb; + info.pDynamicState = &dyn; + info.layout = pipeline_layout_; + info.renderPass = render_pass_; + info.subpass = 0; + + const VkResult r = vkCreateGraphicsPipelines(dev_.device, VK_NULL_HANDLE, 1, &info, nullptr, &pipeline_); + vkDestroyShaderModule(dev_.device, vs, nullptr); + vkDestroyShaderModule(dev_.device, fs, nullptr); + check_vk(r, "vkCreateGraphicsPipelines"); +} + +void SceneRenderer::create_uniforms() +{ + VkDescriptorPoolSize pool_size{ VK_DESCRIPTOR_TYPE_UNIFORM_BUFFER, config_.view_count }; + VkDescriptorPoolCreateInfo pool_info{}; + pool_info.sType = VK_STRUCTURE_TYPE_DESCRIPTOR_POOL_CREATE_INFO; + pool_info.maxSets = config_.view_count; + pool_info.poolSizeCount = 1; + pool_info.pPoolSizes = &pool_size; + check_vk(vkCreateDescriptorPool(dev_.device, &pool_info, nullptr, &descriptor_pool_), "vkCreateDescriptorPool"); + + ubos_.assign(config_.view_count, VK_NULL_HANDLE); + ubo_memory_.assign(config_.view_count, VK_NULL_HANDLE); + ubo_mapped_.assign(config_.view_count, nullptr); + descriptor_sets_.assign(config_.view_count, VK_NULL_HANDLE); + + for (uint32_t i = 0; i < config_.view_count; ++i) + { + create_host_buffer(dev_, sizeof(EyeUbo), VK_BUFFER_USAGE_UNIFORM_BUFFER_BIT, &ubos_[i], &ubo_memory_[i]); + check_vk(vkMapMemory(dev_.device, ubo_memory_[i], 0, sizeof(EyeUbo), 0, &ubo_mapped_[i]), "vkMapMemory(ubo)"); + + VkDescriptorSetAllocateInfo alloc{}; + alloc.sType = VK_STRUCTURE_TYPE_DESCRIPTOR_SET_ALLOCATE_INFO; + alloc.descriptorPool = descriptor_pool_; + alloc.descriptorSetCount = 1; + alloc.pSetLayouts = &dsl_; + check_vk(vkAllocateDescriptorSets(dev_.device, &alloc, &descriptor_sets_[i]), "vkAllocateDescriptorSets"); + + VkDescriptorBufferInfo buf{ ubos_[i], 0, sizeof(EyeUbo) }; + VkWriteDescriptorSet write{}; + write.sType = VK_STRUCTURE_TYPE_WRITE_DESCRIPTOR_SET; + write.dstSet = descriptor_sets_[i]; + write.dstBinding = 0; + write.descriptorCount = 1; + write.descriptorType = VK_DESCRIPTOR_TYPE_UNIFORM_BUFFER; + write.pBufferInfo = &buf; + vkUpdateDescriptorSets(dev_.device, 1, &write, 0, nullptr); + } + + VkCommandPoolCreateInfo pool{}; + pool.sType = VK_STRUCTURE_TYPE_COMMAND_POOL_CREATE_INFO; + pool.flags = VK_COMMAND_POOL_CREATE_RESET_COMMAND_BUFFER_BIT; + pool.queueFamilyIndex = dev_.queue_family_index; + check_vk(vkCreateCommandPool(dev_.device, &pool, nullptr, &command_pool_), "vkCreateCommandPool"); + + VkCommandBufferAllocateInfo cmd_alloc{}; + cmd_alloc.sType = VK_STRUCTURE_TYPE_COMMAND_BUFFER_ALLOCATE_INFO; + cmd_alloc.commandPool = command_pool_; + cmd_alloc.level = VK_COMMAND_BUFFER_LEVEL_PRIMARY; + cmd_alloc.commandBufferCount = 1; + check_vk(vkAllocateCommandBuffers(dev_.device, &cmd_alloc, &command_buffer_), "vkAllocateCommandBuffers"); + + VkFenceCreateInfo fence_info{}; + fence_info.sType = VK_STRUCTURE_TYPE_FENCE_CREATE_INFO; + check_vk(vkCreateFence(dev_.device, &fence_info, nullptr, &fence_), "vkCreateFence"); +} + +int SceneRenderer::update_scene(const mjModel* model, mjData* data) +{ + if (model == nullptr || data == nullptr) + { + throw std::invalid_argument("mujoco_xr: update_scene got a null mjModel* / mjData*"); + } + mjv_updateScene(model, data, &scene_option_, nullptr, &camera_, mjCAT_ALL, &scene_); + return scene_.ngeom; +} + +const std::array& SceneRenderer::projection(int view) const +{ + if (view < 0 || static_cast(view) >= config_.view_count) + { + throw std::out_of_range("mujoco_xr: view index out of range"); + } + return projections_[static_cast(view)]; +} + +const ViewTarget& SceneRenderer::view_target(int view) const +{ + if (view < 0 || static_cast(view) >= config_.view_count) + { + throw std::out_of_range("mujoco_xr: view index out of range"); + } + return view_targets_[static_cast(view)]; +} + +void SceneRenderer::render(const std::vector& poses_xyz_qwxyz, const std::vector& fovs_lrud) +{ + const size_t n = config_.view_count; + if (poses_xyz_qwxyz.size() != n * 7 || fovs_lrud.size() != n * 4) + { + throw std::invalid_argument( + "mujoco_xr: render() expects view_count*7 pose floats and view_count*4 fov " + "floats; the renderer's view_count must match len(FrameInfo.views)"); + } + + // Per-view uniforms first, so the whole command buffer can be recorded and + // submitted once. + float light[3]; + const float light_len = std::sqrt(kLightDirWorld[0] * kLightDirWorld[0] + kLightDirWorld[1] * kLightDirWorld[1] + + kLightDirWorld[2] * kLightDirWorld[2]); + for (int i = 0; i < 3; ++i) + { + light[i] = kLightDirWorld[i] / light_len; + } + + for (size_t v = 0; v < n; ++v) + { + const float* pose = poses_xyz_qwxyz.data() + v * 7; + const float* fov = fovs_lrud.data() + v * 4; + float proj[16]; + float view[16]; + float pv[16]; + proj_from_fov(fov, config_.near_z, config_.far_z, proj); + std::memcpy(projections_[v].data(), proj, sizeof(proj)); + view_from_pose(pose, pose + 3, view); + mat4_mul(pv, proj, view); + + EyeUbo ubo{}; + mat4_mul(ubo.viewproj, pv, xr_from_mj_); + ubo.light_dir[0] = light[0]; + ubo.light_dir[1] = light[1]; + ubo.light_dir[2] = light[2]; + ubo.light_dir[3] = 0.0f; + std::memcpy(ubo_mapped_[v], &ubo, sizeof(ubo)); + } + + check_vk(vkResetCommandBuffer(command_buffer_, 0), "vkResetCommandBuffer"); + VkCommandBufferBeginInfo begin{}; + begin.sType = VK_STRUCTURE_TYPE_COMMAND_BUFFER_BEGIN_INFO; + begin.flags = VK_COMMAND_BUFFER_USAGE_ONE_TIME_SUBMIT_BIT; + check_vk(vkBeginCommandBuffer(command_buffer_, &begin), "vkBeginCommandBuffer"); + + for (size_t v = 0; v < n; ++v) + { + // Alpha 0: AR passthrough shows wherever nothing was drawn. + VkClearValue clears[2]{}; + clears[0].color = { { 0.0f, 0.0f, 0.0f, 0.0f } }; + clears[1].depthStencil = { 1.0f, 0 }; + + VkRenderPassBeginInfo rp{}; + rp.sType = VK_STRUCTURE_TYPE_RENDER_PASS_BEGIN_INFO; + rp.renderPass = render_pass_; + rp.framebuffer = view_targets_[v].framebuffer(); + rp.renderArea.extent = { config_.width, config_.height }; + rp.clearValueCount = 2; + rp.pClearValues = clears; + vkCmdBeginRenderPass(command_buffer_, &rp, VK_SUBPASS_CONTENTS_INLINE); + + // Standard (non-flipped) viewport: the y flip lives in the projection + // and must not be applied twice. + VkViewport viewport{ 0.0f, 0.0f, static_cast(config_.width), static_cast(config_.height), + 0.0f, 1.0f }; + VkRect2D scissor{ { 0, 0 }, { config_.width, config_.height } }; + vkCmdSetViewport(command_buffer_, 0, 1, &viewport); + vkCmdSetScissor(command_buffer_, 0, 1, &scissor); + + vkCmdBindPipeline(command_buffer_, VK_PIPELINE_BIND_POINT_GRAPHICS, pipeline_); + vkCmdBindDescriptorSets( + command_buffer_, VK_PIPELINE_BIND_POINT_GRAPHICS, pipeline_layout_, 0, 1, &descriptor_sets_[v], 0, nullptr); + const VkDeviceSize zero = 0; + vkCmdBindVertexBuffers(command_buffer_, 0, 1, &vertex_buffer_, &zero); + vkCmdBindIndexBuffer(command_buffer_, index_buffer_, 0, VK_INDEX_TYPE_UINT32); + + for (int i = 0; i < scene_.ngeom; ++i) + { + const mjvGeom* g = scene_.geoms + i; + // Meshes only. A plane, sphere or capsule in the scene XML renders + // as nothing -- this is an AR scene and passthrough is the + // background, so there is no ground plane to draw. + if (g->type != mjGEOM_MESH) + { + continue; + } + // dataid = 2*meshid (mesh) or 2*meshid+1 (hull): even only. + if (g->dataid < 0 || (g->dataid & 1) != 0) + { + continue; + } + const int meshid = g->dataid >> 1; + if (meshid >= static_cast(mesh_ranges_.size())) + { + continue; + } + const MeshRange& range = mesh_ranges_[static_cast(meshid)]; + if (range.index_count == 0) + { + continue; + } + + PushConstants pc{}; + // g->mat is row-major; column-major model[c*4 + r] = mat[r*3 + c]. + for (int c = 0; c < 3; ++c) + { + for (int r = 0; r < 3; ++r) + { + pc.model[c * 4 + r] = g->mat[r * 3 + c]; + } + pc.model[c * 4 + 3] = 0; + } + pc.model[12] = g->pos[0]; + pc.model[13] = g->pos[1]; + pc.model[14] = g->pos[2]; + pc.model[15] = 1; + std::memcpy(pc.color, g->rgba, sizeof(pc.color)); + + vkCmdPushConstants(command_buffer_, pipeline_layout_, + VK_SHADER_STAGE_VERTEX_BIT | VK_SHADER_STAGE_FRAGMENT_BIT, 0, sizeof(pc), &pc); + vkCmdDrawIndexed(command_buffer_, range.index_count, 1, range.first_index, range.base_vertex, 0); + } + + vkCmdEndRenderPass(command_buffer_); + view_targets_[v].record_readback(command_buffer_); + } + + check_vk(vkEndCommandBuffer(command_buffer_), "vkEndCommandBuffer"); + + check_vk(vkResetFences(dev_.device, 1, &fence_), "vkResetFences"); + VkSubmitInfo submit{}; + submit.sType = VK_STRUCTURE_TYPE_SUBMIT_INFO; + submit.commandBufferCount = 1; + submit.pCommandBuffers = &command_buffer_; + check_vk(vkQueueSubmit(dev_.queue, 1, &submit, fence_), "vkQueueSubmit"); + // Host-side sync rather than an exported timeline semaphore. Coarse, but + // correct and simple: once the fence signals, the readback copies have + // retired and the exported memory is safe for CUDA to read. The + // alternative (a Vulkan->CUDA semaphore) would only buy overlap that a + // single-threaded frame loop cannot use, because + // ProjectionLayer.submit() blocks on cudaStreamSynchronize anyway. + check_vk(vkWaitForFences(dev_.device, 1, &fence_, VK_TRUE, UINT64_MAX), "vkWaitForFences"); +} + +} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/scene_renderer.hpp b/examples/mujoco_xr/cpp/scene_renderer.hpp new file mode 100644 index 000000000..74f2696fe --- /dev/null +++ b/examples/mujoco_xr/cpp/scene_renderer.hpp @@ -0,0 +1,135 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +// mjvScene -> Vulkan: meshes only (this is an AR scene, so there is no ground +// plane and passthrough is the background), one pipeline, push constants, one +// directional light, no textures, no shadows, no sorting. +// +// View and projection come from the per-view pose + fov in FrameInfo.views; +// mjvGLCamera is bypassed. mjvCamera exists only to give mjv_updateScene a +// viewpoint for culling/LOD, and is a central free camera so one eye's frustum +// cannot cull geometry out of the other's. +// +// C++ owns mjvScene/mjvOption/mjvCamera; Python owns mjModel/mjData/mj_step. +// render() must run on the mj_step thread, after it, and treats mjData as +// const. Threading the frame loop breaks this silently: geometry one step +// stale reads as jitter, not as a race. + +#include "mesh_buffers.hpp" +#include "render_target.hpp" + +#include +#include + +#include +#include +#include + +namespace mujoco_xr +{ + +// Column-major Vulkan-convention projection for one asymmetric fov +// (angle_left, angle_right, angle_up, angle_down, radians). Free function so a +// test can pin the clip convention without a GPU or a VizSession. +std::array projection_from_fov(const std::array& fov_lrud, float near_z, float far_z); + +class SceneRenderer +{ +public: + struct Config + { + uint32_t width = 0; + uint32_t height = 0; + // Stereo only. Kept as a field because the render loops and per-view + // resources read it, not because mono is supported. + uint32_t view_count = 2; + // Single-sourced by the Python app and passed in: the SAME pair also + // goes into VizSessionConfig.xr_near_z / xr_far_z and therefore into + // XrCompositionLayerDepthInfoKHR. There is no default and no literal + // anywhere in this module, because a drift between the depth we encode + // and the range the runtime is told makes compositor reprojection + // wrong, and the symptom (world-locked geometry swimming under head + // motion) is only visible on hardware. + float near_z = 0.0f; + float far_z = 0.0f; + }; + + SceneRenderer(const BorrowedDevice& dev, const Config& config, const mjModel* model); + ~SceneRenderer(); + + SceneRenderer(const SceneRenderer&) = delete; + SceneRenderer& operator=(const SceneRenderer&) = delete; + + // mjv_updateScene, exactly once per frame. Returns the geom count. + int update_scene(const mjModel* model, mjData* data); + + // Renders every view in one queue submit and blocks until the readback + // copies have retired, so the CUDA pointers are safe to hand to + // ProjectionLayer.submit() the moment this returns. + // + // poses_xyz_qwxyz: view_count * 7 floats -- position (x, y, z) then + // orientation (w, x, y, z), matching viz.Pose3D's spelling. + // fovs_lrud: view_count * 4 floats -- angle_left, angle_right, angle_up, + // angle_down, in radians, matching viz.Fov's field order. + void render(const std::vector& poses_xyz_qwxyz, const std::vector& fovs_lrud); + + // The column-major projection used for `view` on the last render(), so the + // app can assert the clip convention per frame. + const std::array& projection(int view) const; + + const ViewTarget& view_target(int view) const; + uint32_t view_count() const + { + return config_.view_count; + } + int ngeom() const + { + return scene_.ngeom; + } + int maxgeom() const + { + return scene_.maxgeom; + } + +private: + void create_pipeline(); + void upload_geometry(const mjModel* model); + void create_uniforms(); + void destroy(); + + BorrowedDevice dev_; + Config config_; + + VkRenderPass render_pass_ = VK_NULL_HANDLE; + VkDescriptorSetLayout dsl_ = VK_NULL_HANDLE; + VkDescriptorPool descriptor_pool_ = VK_NULL_HANDLE; + VkPipelineLayout pipeline_layout_ = VK_NULL_HANDLE; + VkPipeline pipeline_ = VK_NULL_HANDLE; + VkCommandPool command_pool_ = VK_NULL_HANDLE; + VkCommandBuffer command_buffer_ = VK_NULL_HANDLE; + VkFence fence_ = VK_NULL_HANDLE; + + std::vector descriptor_sets_; + std::vector ubos_; + std::vector ubo_memory_; + std::vector ubo_mapped_; + std::vector view_targets_; + std::vector> projections_; + + VkBuffer vertex_buffer_ = VK_NULL_HANDLE; + VkDeviceMemory vertex_memory_ = VK_NULL_HANDLE; + VkBuffer index_buffer_ = VK_NULL_HANDLE; + VkDeviceMemory index_memory_ = VK_NULL_HANDLE; + + std::vector mesh_ranges_; + float xr_from_mj_[16] = { 0 }; + + mjvScene scene_{}; + mjvOption scene_option_{}; + mjvCamera camera_{}; + bool scene_made_ = false; +}; + +} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/shaders/scene.frag b/examples/mujoco_xr/cpp/shaders/scene.frag new file mode 100644 index 000000000..4a8f9adb3 --- /dev/null +++ b/examples/mujoco_xr/cpp/shaders/scene.frag @@ -0,0 +1,48 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Not covered by clang_format_check or the REUSE / copyright-year hooks: +// `.frag` is in neither cmake/ClangFormat.cmake's pattern list nor the +// `files:` regex in .pre-commit-config.yaml. So the SPDX lines above are +// hand-written and must be kept by hand. +// +// Deliberately NOT run through clang-format: it is a C++ formatter and it +// mangles GLSL layout blocks. `clang-format-14 --dry-run -Werror` fails on +// src/viz/shaders/cpp/textured_quad.vert too -- hand-formatted GLSL is the +// established convention here, not an oversight. Since no tool will ever +// arbitrate the shape of these files, this one follows that same precedent +// BY HAND: 4-space indent, opening braces on their own line. +// +// Half-lambert with one hardcoded directional light. Alpha passes through +// straight (unpremultiplied): the background clears to alpha 0 so AR +// passthrough shows behind the scene. + +#version 450 + +layout(location = 0) in vec3 v_normal_w; + +layout(set = 0, binding = 0) uniform Eye +{ + mat4 viewproj; + vec4 light_dir; +} eye; + +// Must match scene.vert's block exactly: both stages share one push-constant +// range, so a field here that the vertex shader does not have shifts `color` +// to an offset the host never wrote. +layout(push_constant) uniform PC +{ + mat4 model; + vec4 color; +} pc; + +layout(location = 0) out vec4 out_color; + +void main() +{ + vec3 n = normalize(v_normal_w); + vec3 l = normalize(-eye.light_dir.xyz); + float diff = max(dot(n, l), 0.0); + const float ambient = 0.35; + out_color = vec4(pc.color.rgb * (ambient + (1.0 - ambient) * diff), pc.color.a); +} diff --git a/examples/mujoco_xr/cpp/shaders/scene.vert b/examples/mujoco_xr/cpp/shaders/scene.vert new file mode 100644 index 000000000..8bf3e3374 --- /dev/null +++ b/examples/mujoco_xr/cpp/shaders/scene.vert @@ -0,0 +1,49 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Not covered by clang_format_check or the REUSE / copyright-year hooks: +// `.vert` is in neither cmake/ClangFormat.cmake's pattern list nor the +// `files:` regex in .pre-commit-config.yaml. So the SPDX lines above are +// hand-written and must be kept by hand. +// +// Deliberately NOT run through clang-format: it is a C++ formatter and it +// mangles GLSL layout blocks. `clang-format-14 --dry-run -Werror` fails on +// src/viz/shaders/cpp/textured_quad.vert too -- hand-formatted GLSL is the +// established convention here, not an oversight. Since no tool will ever +// arbitrate the shape of these files, this one follows that same precedent +// BY HAND: 4-space indent, opening braces on their own line. +// +// MuJoCo XR scene shader: one pipeline, meshes only. Geometry is in MuJoCo +// world space; eye.viewproj already folds in xr_from_mj and the per-view +// pose/fov handed over by viz (mjvGLCamera is bypassed by design). + +#version 450 + +layout(location = 0) in vec3 in_pos; +layout(location = 1) in vec3 in_normal; + +layout(set = 0, binding = 0) uniform Eye +{ + mat4 viewproj; // P * V * xr_from_mj + vec4 light_dir; // world-space travel direction of the one light +} eye; + +layout(push_constant) uniform PC +{ + mat4 model; // world from geom-local (rotation, translation) + vec4 color; +} pc; + +// The ONLY varying. The fragment shader lights with a directional light, which +// needs no world position -- do not add one back "for future point lights" +// until there is a point light. +layout(location = 0) out vec3 v_normal_w; + +void main() +{ + vec4 pw = pc.model * vec4(in_pos, 1.0); + // model's upper 3x3 is a pure rotation (mjvGeom.mat, no scale), so it is + // its own inverse-transpose and needs no separate normal matrix. + v_normal_w = mat3(pc.model) * in_normal; + gl_Position = eye.viewproj * pw; +} diff --git a/examples/mujoco_xr/pyproject.toml b/examples/mujoco_xr/pyproject.toml new file mode 100644 index 000000000..57526f94d --- /dev/null +++ b/examples/mujoco_xr/pyproject.toml @@ -0,0 +1,91 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# This example is its own wheel, and the wheel is the only way to run it: +# +# uv pip install ./examples/mujoco_xr +# python -m isaacteleop_examples.mujoco_xr # needs a headset + CloudXR +# +# Its own wheel because _mujoco_xr links libmujoco: folded into `isaacteleop`, +# that wheel's contents would depend on whether the build host had mujoco. +# `install_python_example()` cannot ship a compiled, ABI-tagged extension. + +[build-system] +requires = [ + "scikit-build-core>=1.0", + # Not pinned to the root build's pybind11: no pybind11-registered type + # crosses this boundary, so the two share no ABI. + "pybind11>=2.12", + # Not optional: the extension compiles against this wheel's headers, and + # without it an isolated build emits a wheel with no extension. Always equal + # to `dependencies` below -- and never restate the number in prose here, + # since CMakeLists.txt matches every `mujoco==` in this file. + "mujoco==3.11.0", +] +build-backend = "scikit_build_core.build" + +[project] +# The dist name mirrors the import path rather than the directory name, so an +# installed example does not claim a bare top-level `mujoco_xr` in +# site-packages, right next to the real `mujoco`. +name = "isaacteleop-examples-mujoco-xr" +version = "0.0.0" # Internal example - not versioned +description = "MuJoCo scene rendered into an Isaac Teleop Televiz XR session" + +# A range, not a pin: scikit-build-core tags the wheel with the installing +# interpreter's ABI. Bounds match ISAAC_TELEOP_PYTHON_VERSION_MIN / +# _MAX_EXCLUSIVE in the root CMakeLists.txt. +requires-python = ">=3.11,<3.14" + +dependencies = [ + # Run-time mujoco, equal to the build-time pin above. Exactly one libmujoco + # may be loaded: mjModel* / mjData* addresses cross the pybind boundary. + "mujoco==3.11.0", + # Unversioned, and a live hazard: a published isaacteleop exists on PyPI, so + # this resolves happily against a release that is not this checkout's viz. + # Install the locally built wheel first, into the same environment: + # uv pip install "isaacteleop[cloudxr]" --find-links=./install/wheels/ + "isaacteleop", + # app.py imports numpy directly. mujoco would pull it in anyway, but a + # transitive dependency imported by name breaks on an upstream change. + "numpy", +] + +[tool.scikit-build] +# Floor matches CMakeLists.txt's cmake_minimum_required; the <4 cap mirrors the +# root pyproject.toml. +cmake.version = ">=3.20,<4" +cmake.build-type = "Release" + +# Persistent, so `uv pip install --reinstall-package ...` stays incremental +# instead of reconfiguring from scratch. `{cache_tag}` keeps per-interpreter +# caches apart. `**/build/` is gitignored. +build-dir = "build/wheel-{cache_tag}" + +# `wheel.packages` owns every authored file under the package; CMake's +# install(TARGETS) owns the one build-produced file. Their intersection must be +# empty, and without this line it is not, because the in-tree build drops its +# own .so there for ctest. Measured: omit it and a cp313 wheel ships the root +# build's stale cp312 .so as well. +# +# `sdist.exclude`, not `wheel.exclude`: the latter is applied twice, so `*.so` +# there would delete the freshly compiled extension too. +[tool.scikit-build.sdist] +exclude = ["python/isaacteleop_examples/mujoco_xr/*.so"] + +# Key is the path inside the wheel, value the source directory. +# +# `isaacteleop_examples` is deliberately not listed: it is a PEP 420 namespace +# with no __init__.py and no owner, and scikit-build-core creates the +# intermediate directory from this key. Adding an __init__.py there (or listing +# the directory as a package) makes it a regular package owned by this wheel, +# and a second example distribution then collides or is shadowed. +[tool.scikit-build.wheel] +packages = { "isaacteleop_examples/mujoco_xr" = "python/isaacteleop_examples/mujoco_xr" } + +# The absent [tool.scikit-build.editable] block is deliberate. `pip install -e` +# is not supported: an editable install redirects the package back to the source +# tree, which is where the in-tree CMake build drops its own _mujoco_xr*.so, so +# you would silently import that one instead. `mode = "redirect"` is already the +# default, so adding the block would not help. Use +# `uv pip install --reinstall-package isaacteleop-examples-mujoco-xr .` instead. diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__init__.py b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__init__.py new file mode 100644 index 000000000..c78ab93b6 --- /dev/null +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__init__.py @@ -0,0 +1,29 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""MuJoCo scene rendered into an Isaac Teleop Televiz XR session.""" + +# Load order is load-bearing -- do not let an import sorter move this. +# `import mujoco` pulls the wheel's libmujoco into the process first, and +# `_mujoco_xr` carries a NEEDED entry for that same versioned SONAME with no +# RPATH, so it binds to the already-loaded library. That is what guarantees one +# libmujoco, and so that the mjModel*/mjData* addresses Python hands the +# renderer match the layout it was compiled against. +import mujoco as _mujoco + +from . import _mujoco_xr + +if _mujoco.mj_versionString() != _mujoco_xr.mujoco_version(): + raise ImportError( + "mujoco_xr: two different libmujoco libraries are loaded -- " + f"the `mujoco` wheel reports {_mujoco.mj_versionString()} but the compiled " + f"extension reports {_mujoco_xr.mujoco_version()}. The extension is what has to be " + "rebuilt. Both `mujoco==` pins in examples/mujoco_xr/pyproject.toml (build-system.requires " + "and project.dependencies) must name one version, and reinstalling recompiles against it: " + "uv pip install --reinstall ./examples/mujoco_xr. (If you hit this from the in-tree ctest " + "path instead, the extension came from the root build: install that same version into " + "build//teleop_build_venv/bin/python and re-run cmake --preset.) " + "mjModel* / mjData* pointers cannot cross this boundary otherwise." + ) + +__all__ = ["_mujoco_xr"] diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__main__.py b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__main__.py new file mode 100644 index 000000000..0b4cb8ca9 --- /dev/null +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__main__.py @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Entry point: ``python -m isaacteleop_examples.mujoco_xr``.""" + +import sys + +from .app import main + +if __name__ == "__main__": + sys.exit(main(sys.argv)) diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py new file mode 100644 index 000000000..f26244f6a --- /dev/null +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py @@ -0,0 +1,582 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""A MuJoCo scene drawn into a Televiz XR session. + +One OpenXR session shared between VizSession (rendering) and TeleopSession +(input); the scene is drawn by Vulkan into images viz owns and reaches +ProjectionLayer.submit() by CUDA pointer, never through host memory. + + VizSession(kXr) ──get_oxr_handles()──▶ TeleopSession + │ │ + │ vk_device / vk_physical_device │ controller grip poses + ▼ ▼ │ + _mujoco_xr.Renderer ──__cuda_array_interface__──▶ ProjectionLayer │ + ▲ │ + └──────────────── mjData.mocap_pos/_quat ◀─────────────────────┘ + +C++ owns mjvScene/mjvOption/mjvCamera; Python owns mjModel/mjData/mj_step, so +everything reading a controller and writing mjData is testable without a GPU. + +Frame order is load-bearing: input is sampled before the physics it feeds, on +every frame that will step or draw. +""" + +from __future__ import annotations + +import argparse +import importlib.metadata +import logging +import math +import sys +from pathlib import Path +from typing import NamedTuple + +import mujoco +import numpy as np + +from isaacteleop import viz +from isaacteleop.cloudxr import CloudXRLauncher +from isaacteleop.oxr import OpenXRSessionHandles +from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource +from isaacteleop.retargeting_engine.interface import OutputCombiner +from isaacteleop.retargeting_engine.tensor_types import ControllerInputIndex +from isaacteleop.retargeters.SO101.gripper_retargeter import ( + GRIPPER_COMMAND_KEY, + SO101GripperRetargeter, +) +from isaacteleop.teleop_session_manager import ( + TeleopSession, + TeleopSessionConfig, + get_required_oxr_extensions_from_pipeline, +) + +from . import _mujoco_xr + +LOG = logging.getLogger("mujoco_xr") + +# The app's only clip planes. VizSessionConfig, the projection and the submitted +# depth must all agree, or world-locked geometry swims under head motion -- and +# only a headset shows it. There is no near/far literal in cpp/, by construction. +NEAR_Z = 0.05 +FAR_Z = 50.0 + +# Wall-clock ceiling for one simulation advance. See _clamp_dt. +MAX_DT_S = 0.1 + +# The only mode: this needs a headset and a CloudXR runtime. A headless fallback +# should arrive with the CI job that runs it (NVIDIA/IsaacTeleop#880). +_DISPLAY_MODE = viz.DisplayMode.kXr + +# layer.submit() in _loop is spelled out per eye and cannot read this name, so +# changing it means editing that call too. +_VIEW_COUNT = 2 + +_CLOCK_SOURCE = ( + "FrameInfo.predicted_display_time; frames with no prediction are skipped, " + "not sampled as 0" +) + +# Package data, so it resolves the same from the wheel and the source tree. Keep +# it ABSOLUTE: on mujoco 3.11.0 a relative model path mis-composes the mesh paths +# of an d fragment and fails naming a file that is right there on disk. +DEFAULT_SCENE = Path(__file__).parent / "assets" / "scene.xml" + +# Checked by name before MuJoCo sees the scene: its failure for a missing +# target is a bare "Error opening file .stl", naming a file +# nobody asked for. +FETCH_SCRIPT = "examples/mujoco_xr/scripts/fetch-so-arm.sh" +_LEADER_ASSETS = Path(__file__).parent / "assets" / "leader" +_LEADER_MESHES = ( + "Wrist_Roll_SO101.stl", + "Trigger_SO101.stl", + "Handle_SO101.stl", + "STS3215_03a.stl", +) + + +def _missing_leader_assets() -> list[str]: + """Names of the fetched meshes that are not on disk. Empty when fetched.""" + return [n for n in _LEADER_MESHES if not (_LEADER_ASSETS / n).is_file()] + + +# One hand and no flag: the ghost is a right-handed gripper. +GHOST_HAND = ControllersSource.RIGHT + +# The two mocap bodies leader_gripper.xml declares. +GHOST_BODY = "leader_ghost" +GHOST_JAW_BODY = "leader_ghost_jaw" + +# ── Where the ghost sits on the hand ─────────────────────────────────────── +# Measured on a headset, not derived: this is a claim about a hand holding a +# CONTROLLER, so do not re-derive it from the mesh -- a model assuming the hand +# passes through the handle loop puts the loop centroid 56 mm from the palm. +# +# Euler degrees, intrinsic XYZ, i.e. MuJoCo's `euler=` (pinned by a test). To +# re-tune, change one angle and reinstall: Rz spins the gripper about its long +# axis, Rx/Ry tilt it, _POS_GRIP_FROM_GHOST slides it along the grip axes +# (-Z little finger -> thumb, +X into the palm, +Y through the knuckles). No +# test asserts a posture, so re-tuning cannot turn them red. +_EULER_GRIP_FROM_GHOST_DEG = (60, 180, 270) +_POS_GRIP_FROM_GHOST = np.array((0, 0.02, -0.025)) + +# ── The trigger hinge ────────────────────────────────────────────────────── +# The follower's `gripper` revolute joint, from SO-ARM100's +# so101_new_calib.urdf: origin xyz="0.0202 0.0188 -0.0234" rpy="1.5708 0 0", +# axis "0 0 1". The right source even for the LEADER's trigger, which is +# mounted in the follower's moving-jaw slot and shares the hinge. The axis +# below is that "0 0 1" carried through the joint frame's 90-degree roll. +# +# Do not re-derive either from the meshes: a pivot from the nearest +# trigger-to-shank vertex pair and an axis from the grip frame both look right +# at the joint's zero and are wrong by the far end of its travel. +_TRIGGER_HINGE_POS = np.array((0.0202, 0.0188, -0.0234)) # metres, ghost frame +_TRIGGER_HINGE_AXIS = np.array((0.0, -1.0, 0.0)) # unit, ghost frame + +# The travel is the URDF joint's own: `upper="1.74533"` is 100.0 degrees, and +# squeezed is its authored zero. A released end short of that does not read as +# an OPEN gripper on a headset, which is the only place this can be judged. +# Do not extend to the joint's lower limit (-10 deg): that end swings the lever +# 0.4 mm into the servo. The tightest pass across 0..100 is 2.1 mm, at the +# squeezed end. +_TRIGGER_RELEASED_RAD = math.radians(100.0) # closedness 0, jaw wide open +_TRIGGER_SQUEEZED_RAD = 0.0 # closedness 1, tucked to the authored pose + + +def _quat_from_euler_deg(angles_deg) -> np.ndarray: + """Intrinsic X-then-Y-then-Z degrees -> a wxyz quaternion, MuJoCo's `euler=`. + + Right-multiplication is what makes it intrinsic. Spelled out rather than + calling mju_euler2Quat so the sequence is visible where it is used. + """ + quat = np.array((1.0, 0.0, 0.0, 0.0)) + for axis, angle in zip(np.eye(3), angles_deg): + step = np.empty(4) + mujoco.mju_axisAngle2Quat(step, axis, math.radians(angle)) + composed = np.empty(4) + mujoco.mju_mulQuat(composed, quat, step) + quat = composed + return quat + + +# ── Derived below; nothing from here on is authored ──────────────────────── +_QUAT_GRIP_FROM_GHOST = _quat_from_euler_deg(_EULER_GRIP_FROM_GHOST_DEG) + + +def _clamp_dt(dt: float) -> float: + """NaN-safe clamp into [0, MAX_DT_S]. + + Comparisons, not min/max: max(nan, 0) is nan, so the obvious form passes + NaN through both limits and into mj_step. + """ + if dt > 0: + return MAX_DT_S if dt > MAX_DT_S else dt + return 0.0 + + +def _build_pipeline() -> OutputCombiner: + """Controllers, plus the shipped SO-101 jaw retargeter as a graph edge. + + A BaseRetargeter node in the pipeline, not a library call beside it. With no + robot in the scene the jaw it drives is the operator's own trigger. + """ + controllers = ControllersSource(name="controllers") + jaw = SO101GripperRetargeter(name="ghost_jaw", input_device=GHOST_HAND).connect( + {GHOST_HAND: controllers.output(GHOST_HAND)} + ) + return OutputCombiner( + { + ControllersSource.LEFT: controllers.output(ControllersSource.LEFT), + ControllersSource.RIGHT: controllers.output(ControllersSource.RIGHT), + GRIPPER_COMMAND_KEY: jaw.output(GRIPPER_COMMAND_KEY), + } + ) + + +def _flatten_xr_views(info) -> tuple[list[float], list[float]]: + """FrameInfo.views -> the flat float arrays the renderer takes. + + Field by field, never sliced: viz.Pose3D.orientation is (w,x,y,z) while a + controller's GRIP_ORIENTATION is (x,y,z,w). + """ + poses: list[float] = [] + fovs: list[float] = [] + for view in info.views: + px, py, pz = view.pose.position + qw, qx, qy, qz = view.pose.orientation + poses.extend((px, py, pz, qw, qx, qy, qz)) + fovs.extend( + ( + view.fov.angle_left, + view.fov.angle_right, + view.fov.angle_up, + view.fov.angle_down, + ) + ) + return poses, fovs + + +def _assert_projection(p: list[float], near: float, far: float) -> None: + """Per-frame, because the projection is rebuilt from per-frame fov. + + `p` is column-major. Depth is asserted as the shipped contract + (near -> 0, far -> 1); two viz doc comments claim reverse-Z, the code is + standard Z. + """ + p00, p11, p23 = p[0], p[5], p[11] + assert p00 > 0.0, ( + f"P[0][0]={p00}: left/right swapped, or a zeroed Fov reached the projection" + ) + # The load-bearing one: b = n*tan(angleUp) > 0 and t = n*tan(angleDown) < 0 + # give 2n/(t-b) < 0. That negative is the Y flip, which drives triangle + # winding -- a depth-range check touches only P[2][2] / P[2][3] / P[3][2] + # and would not notice it going positive. + assert p11 < 0.0, ( + f"P[1][1]={p11}: the angleUp->bottom Y flip is gone; winding will invert" + ) + assert abs(p23 + 1.0) < 1e-6, f"P[2][3]={p23}: not a standard perspective divide" + + # Asserted as the contract we ship rather than as somebody else's formula, + # so it survives a viz refactor. + for z_view, expected in ((-near, 0.0), (-far, 1.0)): + clip_z = p[10] * z_view + p[14] + clip_w = p[11] * z_view + p[15] + assert abs(clip_z / clip_w - expected) < 1e-4, ( + f"depth encoding broken: z_view={z_view} maps to {clip_z / clip_w}, expected {expected}" + ) + + +def _log_startup(resolution) -> None: + """One block naming every assumption that is invisible at runtime.""" + try: + version = importlib.metadata.version("isaacteleop") + except importlib.metadata.PackageNotFoundError: + version = "" + trans = _mujoco_xr.TRANS_MJ_FROM_XR + + LOG.info("scene: %s", DEFAULT_SCENE) + # Several examples ship their own .venv, and picking up the wrong + # isaacteleop is invisible without this line. + LOG.info( + "isaacteleop: %s (version %s)", Path(viz.__file__).resolve().parent, version + ) + LOG.info( + "mujoco: %s (extension links %s)", + mujoco.mj_versionString(), + _mujoco_xr.mujoco_version(), + ) + LOG.info( + "views: %d (stereo) view resolution: %sx%s", + _VIEW_COUNT, + resolution.width, + resolution.height, + ) + LOG.info( + "clip: near=%.4f far=%.2f (one pair -> VizSessionConfig, projection, submitted depth)", + NEAR_Z, + FAR_Z, + ) + LOG.info( + "frames: mj_from_xr translation = (%.3f, %.3f, %.3f) m. x is operator standoff; z is a FLOOR datum, " + "which the session's reference space does not currently establish -- see cpp/frames.hpp. Neither term may " + "be zeroed.", + trans[0], + trans[1], + trans[2], + ) + LOG.info("clock: %s", _CLOCK_SOURCE) + LOG.info( + "depth submission: requested (ProjectionLayer depth_format=D32F). Whether the runtime ACCEPTED it is " + "not queryable -- XrBackend::depth_layer_enabled_ is private with no accessor or binding. The absence " + "of errors is NOT confirmation." + ) + + +class _GhostChannels(NamedTuple): + """The ghost's two mocap rows, resolved once at startup. + + Mocap indices, not body ids: mocap_pos/mocap_quat index by body_mocapid, + and a body id there writes into another body's row. + """ + + body: int + jaw: int + + +def _resolve_ghost(model) -> _GhostChannels: + """Both ghost mocap rows. The shipped scene always declares them.""" + body = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, GHOST_BODY) + jaw = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, GHOST_JAW_BODY) + if body < 0 or jaw < 0: + raise RuntimeError( + f"mujoco_xr: {DEFAULT_SCENE} declares no `{GHOST_BODY}` / " + f"`{GHOST_JAW_BODY}` pair; it must assets/leader/leader_gripper.xml." + ) + return _GhostChannels(int(model.body_mocapid[body]), int(model.body_mocapid[jaw])) + + +def _update_ghost(data, ghost: _GhostChannels, result) -> None: + """Lock the leader gripper to the GHOST_HAND grip pose; swing its trigger. + + Keep the validity gate: an untracked controller reports (0, 0, 0), which is + the scene origin and a place a legitimate pose could put it, so freezing is + the honest rendering of "tracking lost" and there is no else branch. + _QUAT_GRIP_FROM_GHOST right-multiplies because it is fixed in the gripper's + frame; left-multiplying swings the ghost around the room as the operator turns. + """ + controller = result[GHOST_HAND] + if controller.is_none: + return + if not bool(controller[ControllerInputIndex.GRIP_IS_VALID]): + return + position = controller[ControllerInputIndex.GRIP_POSITION] + orientation = controller[ControllerInputIndex.GRIP_ORIENTATION] + p_xr = [float(position[0]), float(position[1]), float(position[2])] + q_xyzw = [ + float(orientation[0]), + float(orientation[1]), + float(orientation[2]), + float(orientation[3]), + ] + + q_grip = np.array(_mujoco_xr.mj_from_xr_quat(q_xyzw), dtype=float) + p_grip = np.array(_mujoco_xr.mj_from_xr_pos(p_xr), dtype=float) + + q_body = np.empty(4) + mujoco.mju_mulQuat(q_body, q_grip, _QUAT_GRIP_FROM_GHOST) + p_offset = np.empty(3) + mujoco.mju_rotVecQuat(p_offset, _POS_GRIP_FROM_GHOST, q_grip) + p_body = p_grip + p_offset + + data.mocap_pos[ghost.body] = p_body + data.mocap_quat[ghost.body] = q_body + + # The deadzone and clamp are the retargeter's contract, not this app's. + # Rotated ABOUT the hinge, not placed at it: the jaw's XML rest pose equals + # the ghost's, so the pivot lives in exactly one place. + closedness = float(result[GRIPPER_COMMAND_KEY][0]) + angle = _TRIGGER_RELEASED_RAD + closedness * ( + _TRIGGER_SQUEEZED_RAD - _TRIGGER_RELEASED_RAD + ) + q_hinge = np.empty(4) + mujoco.mju_axisAngle2Quat(q_hinge, _TRIGGER_HINGE_AXIS, angle) + q_jaw = np.empty(4) + mujoco.mju_mulQuat(q_jaw, q_body, q_hinge) + + # Rotating the ghost frame about the hinge maps 0 to (pivot - R_hinge.pivot). + swung = np.empty(3) + mujoco.mju_rotVecQuat(swung, _TRIGGER_HINGE_POS, q_hinge) + offset = np.empty(3) + mujoco.mju_rotVecQuat(offset, _TRIGGER_HINGE_POS - swung, q_body) + + data.mocap_pos[ghost.jaw] = p_body + offset + data.mocap_quat[ghost.jaw] = q_jaw + + +def _frame_clock(info) -> float | None: + """The simulation clock, or None if this frame carries no time. + + viz zeroes predicted_display_time with should_render on every frame before + kRunning; sampling it makes the next real frame compute dt from 0 and step + 50 times in one display frame. The caller must skip it entirely. + """ + if info.predicted_display_time == 0: + return None + return info.predicted_display_time / 1e9 + + +def run() -> int: + model = mujoco.MjModel.from_xml_path(str(DEFAULT_SCENE)) + data = mujoco.MjData(model) + + # Order is load-bearing: VizSession calls xrCreateInstance, so an extension + # discovered after it cannot be added -- and a controller tracker missing + # XR_NVX1_action_context is silently dead rather than an error. + pipeline = _build_pipeline() + required_extensions = get_required_oxr_extensions_from_pipeline(pipeline) + + config = viz.VizSessionConfig() + config.mode = _DISPLAY_MODE + config.app_name = "MuJoCoXR" + config.xr_near_z = NEAR_Z + config.xr_far_z = FAR_Z + config.required_extensions = required_extensions + # Alpha 0 = "show passthrough here", honoured at the runtime's discretion: + # viz sets the source-alpha blend bit only for a non-opaque environment, so + # a VR headset composites black instead, which is legible rather than broken. + config.clear_color = (0.0, 0.0, 0.0, 0.0) + + viz_session = viz.VizSession.create(config) + renderer = None + try: + resolution = viz_session.get_recommended_resolution() + + layer_config = viz.ProjectionLayerConfig() + layer_config.name = "mujoco_scene" + layer_config.view_resolution = resolution + layer_config.color_format = viz.PixelFormat.kRGBA8 + layer_config.depth_format = viz.PixelFormat.kD32F + layer_config.stereo = _VIEW_COUNT == 2 + layer = viz_session.add_projection_layer(layer_config) + + renderer = _mujoco_xr.Renderer( + vk_physical_device=viz_session.vk_physical_device, + vk_device=viz_session.vk_device, + vk_queue_family_index=viz_session.vk_queue_family_index, + width=resolution.width, + height=resolution.height, + view_count=_VIEW_COUNT, + near_z=NEAR_Z, + far_z=FAR_Z, + model_address=model._address, + ) + + _log_startup(resolution) + + # After the startup block, so its line reads as part of the same report. + ghost = _resolve_ghost(model) + LOG.info( + "leader ghost: bound to mocap %d (body) / %d (trigger); trigger driven by " + "SO101GripperRetargeter, %.0f deg released to %.0f deg squeezed", + ghost.body, + ghost.jaw, + math.degrees(_TRIGGER_RELEASED_RAD), + math.degrees(_TRIGGER_SQUEEZED_RAD), + ) + + oxr = viz_session.get_oxr_handles() + if oxr is None: + raise RuntimeError( + "VizSession is in kXr mode but produced no OpenXR handles; the backend did not initialize." + ) + teleop_config = TeleopSessionConfig( + app_name="MuJoCoXR", + pipeline=pipeline, + # Never pass trackers=: TeleopSession discovers them from the + # pipeline graph, and passing them again duplicates the set. + oxr_handles=OpenXRSessionHandles(*oxr), + ) + with TeleopSession(teleop_config) as teleop_session: + _loop(viz_session, layer, renderer, model, data, teleop_session, ghost) + finally: + # The renderer borrows viz_session's device: it must go first. + if renderer is not None: + renderer.close() + viz_session.destroy() + return 0 + + +def _loop(viz_session, layer, renderer, model, data, teleop_session, ghost) -> None: + view_count = renderer.view_count + previous_clock: float | None = None + # Fixed-step accumulator. NOT reset or drained on a non-render frame: the + # simulation owes that time regardless of whether anything was displayed. + accumulator = 0.0 + checked_projection = False + + while not viz_session.should_close(): + info = viz_session.begin_frame() + try: + # None means "this frame carries no usable timestamp" -- skip the + # sample entirely rather than recording a zero. See _frame_clock. + now = _frame_clock(info) + if now is not None: + if previous_clock is not None: + accumulator += _clamp_dt(now - previous_clock) + previous_clock = now + + # Input above the should_render gate and above the step loop, so + # it precedes the physics it feeds. Gated on "will step or will + # draw" rather than every frame: an ungated teleop_session.step() + # calls xrSyncActions on the unthrottled pre-kRunning burst, which + # is hundreds of frames in milliseconds. + result = None + will_step = accumulator >= model.opt.timestep + if will_step or info.should_render: + result = teleop_session.step() + _update_ghost(data, ghost, result) + + steps = 0 + while accumulator >= model.opt.timestep and steps < 64: + mujoco.mj_step(model, data) + accumulator -= model.opt.timestep + steps += 1 + + if not info.should_render: + # Deliberately does NOT touch the accumulator. + continue + + renderer.update_scene(model._address, data._address) + # mjv_updateScene truncates on overflow and returns normally, with + # only a stderr warning nobody reads in a frame loop. + if renderer.ngeom >= renderer.maxgeom: + raise RuntimeError( + f"mjvScene is full: ngeom={renderer.ngeom} maxgeom={renderer.maxgeom}. " + "Geometry is being dropped -- raise kMaxGeom in " + "cpp/scene_renderer.cpp." + ) + + # A view-count mismatch is rejected by render() below, which sees + # the flattened lengths and says so in those terms. There is + # deliberately no second check here. + poses, fovs = _flatten_xr_views(info) + renderer.render(poses, fovs) + + # First rendered frame only: the fov changes per frame but the clip + # convention does not, and tests/test_projection.py pins it headless. + if not checked_projection: + for view in range(view_count): + _assert_projection(renderer.projection(view), NEAR_Z, FAR_Z) + LOG.info( + "projection convention verified on the first rendered frame (P[1][1] < 0, near->0, far->1)" + ) + checked_projection = True + + layer.submit( + renderer.color(0), + renderer.depth(0), + renderer.color(1), + renderer.depth(1), + ) + finally: + # Follows EVERY begin_frame(), including the should_render == False + # path and any exception above. Skipping it wedges the frame loop. + viz_session.end_frame() + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter + ) + parser.add_argument("--verbose", action="store_true", help="Debug-level logging.") + CloudXRLauncher.add_launcher_arguments(parser) + args = parser.parse_args(argv[1:]) + + logging.basicConfig( + level=logging.DEBUG if args.verbose else logging.INFO, + format="[mujoco_xr] %(message)s", + ) + + # Before launch_context starts the runtime, so an unfetched checkout says so + # plainly instead of buried in the runtime's own startup logging. + missing = _missing_leader_assets() + if missing: + raise SystemExit( + f"mujoco_xr: the leader gripper meshes are not fetched ({', '.join(missing)}).\n" + f" Run {FETCH_SCRIPT} from the repository root, then reinstall:\n" + " uv pip install --reinstall-package isaacteleop-examples-mujoco-xr " + "./examples/mujoco_xr" + ) + + with CloudXRLauncher.launch_context(args) as launcher: + if launcher is not None: + LOG.info("CloudXR runtime started (WSS log: %s)", launcher.wss_log_path) + try: + return run() + except KeyboardInterrupt: + LOG.info("interrupted") + return 0 + + +if __name__ == "__main__": + sys.exit(main(sys.argv)) diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml new file mode 100644 index 000000000..63304b28c --- /dev/null +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml new file mode 100644 index 000000000..e3897e8f9 --- /dev/null +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml @@ -0,0 +1,27 @@ + + + + + diff --git a/examples/mujoco_xr/scripts/fetch-so-arm.sh b/examples/mujoco_xr/scripts/fetch-so-arm.sh new file mode 100755 index 000000000..7a24f1405 --- /dev/null +++ b/examples/mujoco_xr/scripts/fetch-so-arm.sh @@ -0,0 +1,59 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Fetches the SO-101 leader-gripper assets, rather than vendoring 2.3 MB of +# binary STL that Git LFS made every clone pay for. +# +# Nothing calls this at build time: an isolated PEP-517 wheel build must not +# reach the network, so it is an explicit step and the app names it at startup. +# The files are package data, so REINSTALL afterwards -- skip that and the ghost +# works from the source tree and fails from the wheel. +set -euo pipefail + +# The pin. Bump it and the checksums together or the download is refused. +COMMIT="fda892cba81032c46c40976a48c9ceadbf40a9ca" +REPO="TheRobotStudio/SO-ARM100" + +DEST="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)/python/isaacteleop_examples/mujoco_xr/assets/leader" + +# upstream path local name sha256 +# +# The URDF is where app.py's trigger hinge comes from, and having it on disk is +# what lets test_ghost.py check those constants against their source. +ASSETS=( + "STL/SO101/Individual/Wrist_Roll_SO101.stl Wrist_Roll_SO101.stl de3a65044dd4ae8bcb9659d8ca2b49598e3f5571edf89f45ad975e9776a7ffee" + "STL/SO101/Individual/Trigger_SO101.stl Trigger_SO101.stl 48ecec3a3710cffdc0ae96d28547e49ddf4cbc93ccd915be7549f78e00ad2850" + "STL/SO101/Individual/Handle_SO101.stl Handle_SO101.stl fb8757bdff009c04c207481dd664813ccdac2ad989acea6057df780b52327281" + "Simulation/SO101/assets/sts3215_03a_v1.stl STS3215_03a.stl a37c871fb502483ab96c256baf457d36f2e97afc9205313d9c5ab275ef941cd0" + "Simulation/SO101/so101_new_calib.urdf so101_new_calib.urdf 3a65d2d35e68a8d2f0c2cc176d19b884506543c93ba72980145b80abe276022c" + "LICENSE LICENSE c71d239df91726fc519c6eb72d318ec65820627232b2f796219e87dcf35d0ab4" +) + +mkdir -p "$DEST" +echo "Fetching SO-ARM100 assets at ${COMMIT:0:12} into ${DEST}" + +for entry in "${ASSETS[@]}"; do + read -r remote local sha <<<"$entry" + target="${DEST}/${local}" + if [[ -f "$target" ]] && echo "${sha} ${target}" | sha256sum --check --status; then + echo " ok ${local}" + continue + fi + url="https://raw.githubusercontent.com/${REPO}/${COMMIT}/${remote}" + echo " fetching ${local}" + curl -fsSL "$url" -o "${target}.part" + # A raw.githubusercontent path is not immutable in practice, and a silently + # substituted mesh renders as a broken gripper rather than an error. + if ! echo "${sha} ${target}.part" | sha256sum --check --status; then + rm -f "${target}.part" + echo "ERROR: checksum mismatch for ${remote}." >&2 + echo " Upstream changed, or COMMIT and the hashes above disagree." >&2 + exit 1 + fi + mv "${target}.part" "$target" +done + +echo +echo "Done. These are package data, so install before running:" +echo " uv pip install --reinstall-package isaacteleop-examples-mujoco-xr ./examples/mujoco_xr" diff --git a/examples/mujoco_xr/tests/CMakeLists.txt b/examples/mujoco_xr/tests/CMakeLists.txt new file mode 100644 index 000000000..3d660c2af --- /dev/null +++ b/examples/mujoco_xr/tests/CMakeLists.txt @@ -0,0 +1,38 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# mujoco_xr tests. Each test_*.py registers as its own ctest entry under the +# `mujoco_xr` label -- same shape as examples/camera_viz/tests/CMakeLists.txt. +# +# Keep every test here unit-level: no GPU, no headset, no CloudXR runtime, no +# window system. A test gated on hardware one developer has reports green by +# skipping, and examples have no CI to run it in (NVIDIA/IsaacTeleop#880). The +# cost is that the Vulkan -> CUDA -> submit path is covered nowhere, which +# README.md states under "Not verified anywhere in CI or on a developer +# desktop". + +file(GLOB TEST_FILES + RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" + "${CMAKE_CURRENT_SOURCE_DIR}/test_*.py" +) + +foreach(test_file ${TEST_FILES}) + get_filename_component(test_name "${test_file}" NAME_WE) + add_test( + NAME "mujoco_xr_${test_name}" + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev + pytest -v --tb=short "${test_file}" + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + ) + # Resolves isaacteleop only; the example's own package is reached from the + # source tree by conftest.py. + # + # If a second root is ever added: CMake's ENVIRONMENT property is a + # semicolon-separated list, so two "PYTHONPATH=..." elements silently + # produce two entries and only the last survives. Join additional roots with + # ':' inside one quoted string. + set_tests_properties("mujoco_xr_${test_name}" PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$" + LABELS "mujoco_xr" + ) +endforeach() diff --git a/examples/mujoco_xr/tests/conftest.py b/examples/mujoco_xr/tests/conftest.py new file mode 100644 index 000000000..d8d327618 --- /dev/null +++ b/examples/mujoco_xr/tests/conftest.py @@ -0,0 +1,20 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Prepend examples/mujoco_xr/python/ so `isaacteleop_examples.mujoco_xr` +# resolves against the in-tree source, and with it the _mujoco_xr*.so that +# cpp/CMakeLists.txt builds in place beside __init__.py. Doing it here rather +# than in the ctest ENVIRONMENT keeps a bare `pytest` working too. +# +# python/, not python/isaacteleop_examples/: `isaacteleop_examples` is a PEP 420 +# namespace, so what goes on sys.path is the directory containing it. Do not add +# an __init__.py to make an import work -- that breaks the installed wheel's +# ability to share the namespace. +# +# isaacteleop is not resolved here: it comes from the PYTHONPATH the ctest +# registration sets, or from the ambient environment when run by hand. + +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent.parent / "python")) diff --git a/examples/mujoco_xr/tests/pyproject.toml b/examples/mujoco_xr/tests/pyproject.toml new file mode 100644 index 000000000..25875cbf7 --- /dev/null +++ b/examples/mujoco_xr/tests/pyproject.toml @@ -0,0 +1,30 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Pyproject for the mujoco_xr tests. It exists so that `uv run` resolves HERE +# rather than walking up to examples/ and then to the repository root project +# (which has no mujoco and entirely different deps). Precedent: +# examples/camera_viz/tests/pyproject.toml. + +[project] +# Cosmetic -- nothing reads it, and ../CMakeLists.txt's pin check reads this +# file rather than this field. +name = "isaacteleop-examples-mujoco-xr-tests" +version = "0.0.0" +# Pinned because these tests import the ABI-specific _mujoco_xr*.so built by the +# preset's interpreter. +requires-python = "==3.12.*" + +[project.optional-dependencies] +dev = [ + "pytest", + "numpy", + # Keep in sync with the pin in ../pyproject.toml; ../CMakeLists.txt reads + # both files and fails the configure on a disagreement. It matches every + # `mujoco==` here, so do not restate the number in prose -- say "the pin + # below" or a comment edit becomes a configure failure. + "mujoco==3.11.0", +] + +[tool.pytest.ini_options] +pythonpath = ["."] diff --git a/examples/mujoco_xr/tests/test_app_helpers.py b/examples/mujoco_xr/tests/test_app_helpers.py new file mode 100644 index 000000000..f4caee0dc --- /dev/null +++ b/examples/mujoco_xr/tests/test_app_helpers.py @@ -0,0 +1,84 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Pure helpers from the app that guard against silent-corruption bugs.""" + +import pytest + +app = pytest.importorskip( + "isaacteleop_examples.mujoco_xr.app", reason="isaacteleop is not on PYTHONPATH" +) + + +@pytest.mark.parametrize( + ("raw", "expected"), + [ + (0.011, 0.011), + (0.0, 0.0), + (-1.0, 0.0), # clock went backwards + (5.0, app.MAX_DT_S), # a long stall + (float("inf"), app.MAX_DT_S), + ], +) +def test_clamp_dt(raw, expected): + assert app._clamp_dt(raw) == expected + + +def test_clamp_dt_sends_nan_to_zero(): + """The whole reason the clamp uses comparisons. + + ``min(max(nan, 0), 0.1)`` is nan, so NaN passes both limits into mj_step and + poisons every qpos. ``nan > 0`` is False, so the comparison form sends it to 0. + """ + assert app._clamp_dt(float("nan")) == 0.0 + + +def test_frame_clock_refuses_the_zeroed_timestamp(): + """Regression: the 50-step physics lurch at every session start. + + ``viz_session.cpp:255-256`` zeroes ``predicted_display_time`` with + ``should_render`` on every pre-kRunning frame. Sampling that zero makes the + next real frame compute ``dt = t_now - 0``, clamp to MAX_DT_S, and step 0.1 s + inside one display frame. + """ + + class _Info: + predicted_display_time = 0 + + assert app._frame_clock(_Info()) is None + + _Info.predicted_display_time = 2_000_000_000 # ns + assert app._frame_clock(_Info()) == 2.0 + + +def test_near_far_are_a_single_sane_pair(): + assert 0.0 < app.NEAR_Z < app.FAR_Z + # viz defaults far to 100.0; an arm's-length scene does not want that + # precision spent 50-100 m away. + assert app.FAR_Z <= 100.0 + + +def test_assert_projection_rejects_a_lost_y_flip(): + """The assertion has to actually fire, or it is decoration.""" + from isaacteleop_examples.mujoco_xr import _mujoco_xr + + good = _mujoco_xr.projection_from_fov([-0.7, 0.7, 0.7, -0.7], app.NEAR_Z, app.FAR_Z) + app._assert_projection(good, app.NEAR_Z, app.FAR_Z) + + flipped = list(good) + flipped[5] = -flipped[5] # P[1][1] positive: the angleUp->bottom swap is gone + with pytest.raises(AssertionError, match=r"P\[1\]\[1\]"): + app._assert_projection(flipped, app.NEAR_Z, app.FAR_Z) + + +def test_assert_projection_rejects_reverse_z(): + from isaacteleop_examples.mujoco_xr import _mujoco_xr + + p = list( + _mujoco_xr.projection_from_fov([-0.7, 0.7, 0.7, -0.7], app.NEAR_Z, app.FAR_Z) + ) + # Swap the depth endpoints: near -> 1, far -> 0. + p[10] = -p[10] - 1.0 + p[14] = -p[14] + with pytest.raises(AssertionError, match="depth encoding"): + app._assert_projection(p, app.NEAR_Z, app.FAR_Z) diff --git a/examples/mujoco_xr/tests/test_frames.py b/examples/mujoco_xr/tests/test_frames.py new file mode 100644 index 000000000..5ceee2bce --- /dev/null +++ b/examples/mujoco_xr/tests/test_frames.py @@ -0,0 +1,92 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""The XR -> MuJoCo crossing: the handedness map and the quaternion order. + +Both are cheap to get wrong and expensive to debug on hardware. +""" + +import math + +import numpy as np +import pytest + +from isaacteleop_examples.mujoco_xr import _mujoco_xr + + +def test_extension_and_wheel_share_one_libmujoco(): + import mujoco + + assert _mujoco_xr.mujoco_version() == mujoco.mj_versionString() + + +def test_axis_map_is_rep103(): + """XR -Z -> MJ +x, +Y -> +z, +X -> -y, on the rotation alone. + + The workspace translation is subtracted, so re-measuring it cannot fail this. + """ + t = np.asarray(_mujoco_xr.TRANS_MJ_FROM_XR) + + forward = np.asarray(_mujoco_xr.mj_from_xr_pos([0.0, 0.0, -1.0])) - t + up = np.asarray(_mujoco_xr.mj_from_xr_pos([0.0, 1.0, 0.0])) - t + right = np.asarray(_mujoco_xr.mj_from_xr_pos([1.0, 0.0, 0.0])) - t + + np.testing.assert_allclose(forward, [1.0, 0.0, 0.0], atol=1e-12) + np.testing.assert_allclose(up, [0.0, 0.0, 1.0], atol=1e-12) + np.testing.assert_allclose(right, [0.0, -1.0, 0.0], atol=1e-12) + + +@pytest.mark.parametrize("eye_height", [0.0, 1.2, 1.6]) +def test_point_one_metre_in_front_at_eye_height(eye_height): + """frames.hpp's definition, executable: a point 1 m in front of the operator + at eye height h lands at MuJoCo (+1, 0, h), before the workspace translation. + """ + t = np.asarray(_mujoco_xr.TRANS_MJ_FROM_XR) + p_mj = np.asarray(_mujoco_xr.mj_from_xr_pos([0.0, eye_height, -1.0])) - t + np.testing.assert_allclose(p_mj, [1.0, 0.0, eye_height], atol=1e-12) + + +def test_translation_has_both_terms(): + """Neither term may be silently zeroed: x is operator standoff and z the + floor datum, and they are independent.""" + t = _mujoco_xr.TRANS_MJ_FROM_XR + assert t[0] != 0.0, "operator standoff was zeroed" + assert t[2] != 0.0, "floor datum was zeroed" + assert t[1] == 0.0 + + +def test_identity_orientation_maps_to_the_convention_quaternion(): + q_xyzw_identity = [0.0, 0.0, 0.0, 1.0] + q_wxyz = _mujoco_xr.mj_from_xr_quat(q_xyzw_identity) + np.testing.assert_allclose(q_wxyz, _mujoco_xr.QUAT_MJ_FROM_XR, atol=1e-12) + + +def test_quaternion_input_order_is_xyzw_not_wxyz(): + """A 90-degree roll about XR +Z, spelled xyzw. + + NINETY degrees, not 180: a 180-degree roll is (0, 0, 1, 0), which read as + wxyz is a roll about XR +Y, and BOTH send local +x to MuJoCo +y -- a probe + that passes whichever way the binding reads its input. The second half pins + that this probe does discriminate. + """ + import mujoco + + s = math.sin(math.radians(45.0)) + q_xyzw = [0.0, 0.0, s, s] # (x, y, z, w) = 90 deg about z_xr + q_wxyz = np.asarray(_mujoco_xr.mj_from_xr_quat(q_xyzw)) + + local_x = np.zeros(3) + mujoco.mju_rotVecQuat(local_x, np.array([1.0, 0.0, 0.0]), q_wxyz) + np.testing.assert_allclose(local_x, [0.0, 0.0, 1.0], atol=1e-12) + + # The same four numbers misread as wxyz are a 180-degree rotation about + # (0, s, s), which lands on MuJoCo +y instead. So the assertion above is + # genuinely sensitive to the component order. + q_misread = np.asarray( + _mujoco_xr.mj_from_xr_quat([q_xyzw[1], q_xyzw[2], q_xyzw[3], q_xyzw[0]]) + ) + misread_x = np.zeros(3) + mujoco.mju_rotVecQuat(misread_x, np.array([1.0, 0.0, 0.0]), q_misread) + np.testing.assert_allclose(misread_x, [0.0, 1.0, 0.0], atol=1e-12) + + assert math.isclose(float(np.linalg.norm(q_wxyz)), 1.0, rel_tol=1e-9) diff --git a/examples/mujoco_xr/tests/test_ghost.py b/examples/mujoco_xr/tests/test_ghost.py new file mode 100644 index 000000000..63d1f275e --- /dev/null +++ b/examples/mujoco_xr/tests/test_ghost.py @@ -0,0 +1,498 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""The leader-gripper ghost: the overlay, its geometry, and when it is written. + +Everything here is headless. The one thing it cannot check is what the ghost +looks like through a headset, which is also the only thing that can settle the +two residual risks named in ``assets/leader/leader_gripper.xml``. +""" + +import math +from xml.etree import ElementTree + +import numpy as np +import pytest + +app = pytest.importorskip( + "isaacteleop_examples.mujoco_xr.app", + reason="isaacteleop is not on PYTHONPATH", +) +_mujoco_xr = pytest.importorskip("isaacteleop_examples.mujoco_xr._mujoco_xr") +mujoco = pytest.importorskip("mujoco") + +from isaacteleop.retargeting_engine.tensor_types import ( # noqa: E402 + ControllerInputIndex, +) + +GHOST_GEOMS = ( + "leader_ghost_wrist_roll", + "leader_ghost_motor", + "leader_ghost_trigger", + "leader_ghost_handle", +) + + +def _default_scene(): + """The shipped scene, skipping on an unfetched checkout. + + Saying which meshes are missing beats MuJoCo's "Error opening file". + """ + missing = app._missing_leader_assets() + if missing: + pytest.skip( + f"leader meshes not fetched ({', '.join(missing)}); run {app.FETCH_SCRIPT}" + ) + return mujoco.MjModel.from_xml_path(str(app.DEFAULT_SCENE)) + + +def _scene(model, data): + mujoco.mj_forward(model, data) + option = mujoco.MjvOption() + mujoco.mjv_defaultOption(option) + camera = mujoco.MjvCamera() + mujoco.mjv_defaultFreeCamera(model, camera) + scene = mujoco.MjvScene(model, 20000) + mujoco.mjv_updateScene( + model, data, option, None, camera, mujoco.mjtCatBit.mjCAT_ALL, scene + ) + return scene + + +def _geom_verts_world(model, data, name): + gid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) + mesh = model.geom_dataid[gid] + adr, num = model.mesh_vertadr[mesh], model.mesh_vertnum[mesh] + verts = np.array(model.mesh_vert[adr : adr + num], dtype=float) + rot = data.geom_xmat[gid].reshape(3, 3) + return verts @ rot.T + data.geom_xpos[gid] + + +def _nearest_gap(a, b, stride=7, block=200): + a = a[::stride] + b = b[::stride] + best = math.inf + for i in range(0, len(a), block): + d = np.linalg.norm(a[i : i + block, None, :] - b[None, :, :], axis=2) + best = min(best, float(d.min())) + return best + + +# --------------------------------------------------------------------------- +# A stubbed pipeline result. ``app._update_ghost`` reads three fields through +# the mapping protocol, and stubbing them is what keeps this file headless. +# --------------------------------------------------------------------------- + + +class _Controller: + is_none = False + + def __init__(self, valid, pos=(0.0, 0.0, 0.0), quat_xyzw=(0.0, 0.0, 0.0, 1.0)): + self._fields = { + ControllerInputIndex.GRIP_IS_VALID: valid, + ControllerInputIndex.GRIP_POSITION: pos, + ControllerInputIndex.GRIP_ORIENTATION: quat_xyzw, + } + + def __getitem__(self, index): + return self._fields[index] + + +class _NoController: + """What the pipeline yields for a hand it has no sample for.""" + + is_none = True + + def __getitem__(self, index): # pragma: no cover -- reaching this IS the bug + raise AssertionError("an is_none controller must never be read") + + +def _result(controller, closedness=0.0): + """Both hands plus the jaw channel, shaped like the real combiner output.""" + other = ( + app.ControllersSource.LEFT + if app.GHOST_HAND == app.ControllersSource.RIGHT + else app.ControllersSource.RIGHT + ) + return { + app.GHOST_HAND: controller, + other: _NoController(), + app.GRIPPER_COMMAND_KEY: [closedness], + } + + +# --------------------------------------------------------------------------- +# The transparency design. This is the claim that replaced a second Vulkan +# pipeline, so it is the one that has to be asserted rather than believed. +# --------------------------------------------------------------------------- + + +def test_the_ghost_is_opaque_and_collides_with_nothing(): + """Opaque, so draw order and the blending risks stop mattering. + + Read off the SCENE geom: model.geom_rgba still holds MuJoCo's default, so + asserting that would pass on a translucent ghost too. + """ + model = _default_scene() + data = mujoco.MjData(model) + scene = _scene(model, data) + by_objid = { + int(scene.geoms[i].objid): i + for i in range(scene.ngeom) + if scene.geoms[i].objtype == mujoco.mjtObj.mjOBJ_GEOM + } + for name in GHOST_GEOMS: + gid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, name) + assert scene.geoms[by_objid[gid]].rgba[3] == pytest.approx(1.0) + # Contact would let the hand shove scene content around. + assert model.geom_contype[gid] == 0 + assert model.geom_conaffinity[gid] == 0 + # mjModel aggregates geom mass, so `mass="0"` is checked where it lands. A + # mocap body is kinematic either way, but a non-zero mass here would change + # the model's total and any inertia-derived diagnostic built on it. + for body_name in (app.GHOST_BODY, app.GHOST_JAW_BODY): + body = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, body_name) + assert model.body_mass[body] == 0.0 + + +def test_both_ghost_bodies_are_mocap_and_kinematic(): + """Two mocap bodies, no joints, parented to world. + + The trigger is a second mocap body rather than a jointed child because + mj_step integrates gravity into a joint (measured: 0.06 rad over 50 steps). + """ + model = _default_scene() + bodies = [ + mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, n) + for n in (app.GHOST_BODY, app.GHOST_JAW_BODY) + ] + assert all(b >= 0 for b in bodies) + for body in bodies: + assert model.body_mocapid[body] >= 0 + assert model.body_parentid[body] == 0, "a mocap body must be a child of world" + assert model.body_jntnum[body] == 0 + + +# --------------------------------------------------------------------------- +# The geometry. All three transforms are DERIVED; this is the derivation +# checking itself. +# --------------------------------------------------------------------------- + + +def test_the_three_leader_parts_form_one_assembly(): + """Sub-mm where the parts bolt, mm of clearance where one pivots. + + An STL refresh that broke the shared CAD datum opens these gaps rather than + quietly rendering three pieces near each other. + """ + model = _default_scene() + data = mujoco.MjData(model) + mujoco.mj_forward(model, data) + verts = {n: _geom_verts_world(model, data, n) for n in GHOST_GEOMS} + + bolted = _nearest_gap( + verts["leader_ghost_wrist_roll"], verts["leader_ghost_handle"] + ) + assert bolted < 1e-3, f"shank-to-handle gap {bolted * 1000:.2f} mm" + for other in ("leader_ghost_trigger",): + for part in ("leader_ghost_wrist_roll", "leader_ghost_handle"): + gap = _nearest_gap(verts[part], verts[other]) + assert gap < 5e-3, f"{part} to {other} gap {gap * 1000:.2f} mm" + + +def test_the_servo_fills_the_notch_in_the_wrist_bracket(): + """`wrist_roll` is a C-shaped bracket; the servo is what sits in it. + + Contact plus the size of a real STS3215, which catches the units trap: this + mesh is Menagerie's, in metres, while its neighbours are mm print STLs. + """ + model = _default_scene() + data = mujoco.MjData(model) + mujoco.mj_forward(model, data) + servo = _geom_verts_world(model, data, "leader_ghost_motor") + bracket = _geom_verts_world(model, data, "leader_ghost_wrist_roll") + assert _nearest_gap(servo, bracket) < 1e-3, "the servo is not seated in the bracket" + extent = np.ptp(servo, axis=0) + assert np.allclose(np.sort(extent), (0.0248, 0.0396, 0.0454), atol=2e-3), ( + f"servo spans {np.round(extent * 1000, 1)} mm -- an STS3215 is 45x25x40" + ) + + +def test_the_renderers_normals_agree_with_the_geometry_they_shade(): + """Every corner normal must face the same way as its own triangle. + + The renderer computes these; mjModel's own normals are smeared across each + crease and fail this test (cpp/mesh_buffers.hpp has the measurements), so + reverting cpp/mesh_buffers.cpp to them turns this red. + + The bound is the crease angle itself: smoothing may tilt a corner normal + toward its neighbours, but never past 90 degrees from its own face. + """ + model = _default_scene() + for name in ("leader_wrist_roll", "leader_trigger", "leader_handle"): + mesh = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_MESH, name) + pos, normal = _mujoco_xr.mesh_triangles(model._address, mesh) + pos = np.asarray(pos, dtype=float).reshape(-1, 3, 3) + normal = np.asarray(normal, dtype=float).reshape(-1, 3, 3) + + geometric = np.cross(pos[:, 1] - pos[:, 0], pos[:, 2] - pos[:, 0]) + geometric /= np.linalg.norm(geometric, axis=1, keepdims=True) + 1e-30 + dots = np.einsum("ij,ikj->ik", geometric, normal) + assert dots.min() > 0.0, ( + f"{name}: {int((dots <= 0).sum())} of {dots.size} corner normals face away from " + f"their own triangle (worst {dots.min():+.3f})" + ) + assert np.allclose(np.linalg.norm(normal, axis=2), 1.0, atol=1e-5), ( + f"{name}: normals are not unit length" + ) + + +def test_the_leader_meshes_are_scaled_from_millimetres(): + """`scale="0.001"`, and getting it wrong does not read as "a big mesh" -- + the camera ends up inside a 65 m solid. The servo is absent from this list + deliberately: it is authored in metres and carries no scale.""" + model = _default_scene() + for name in ("leader_wrist_roll", "leader_trigger", "leader_handle"): + mesh = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_MESH, name) + assert mesh >= 0 + adr, num = model.mesh_vertadr[mesh], model.mesh_vertnum[mesh] + verts = np.array(model.mesh_vert[adr : adr + num], dtype=float) + extent = float(np.ptp(verts, axis=0).max()) + assert 0.02 < extent < 0.30, f"{name} spans {extent:.3f} m" + + +# --------------------------------------------------------------------------- +# When the ghost is written. +# --------------------------------------------------------------------------- + + +def test_the_ghost_is_rigidly_attached_to_the_grip_frame(): + """The contract the calibration must satisfy, whatever its value. + + Left-multiplying instead swings the ghost around the room as the operator + turns, while still looking right at one orientation -- which is what makes + it survive a spot check. Asserted as invariance rather than a posture, so + re-tuning on a headset cannot turn it red. + """ + model = _default_scene() + data = mujoco.MjData(model) + ghost = app._resolve_ghost(model) + + seen = [] + for grip_pos, grip_quat_xyzw in ( + ((0.0, 1.2, -0.5), (0.0, 0.0, 0.0, 1.0)), + ((0.31, 1.24, -0.42), (0.0, 0.3826834, 0.0, 0.9238795)), + ((-0.2, 0.9, -0.8), (0.5, 0.5, 0.5, 0.5)), + ): + app._update_ghost( + data, ghost, _result(_Controller(True, grip_pos, grip_quat_xyzw)) + ) + q_world_from_grip = np.array(_mujoco_xr.mj_from_xr_quat(list(grip_quat_xyzw))) + inverse, relative = np.empty(4), np.empty(4) + mujoco.mju_negQuat(inverse, q_world_from_grip) + mujoco.mju_mulQuat(relative, inverse, np.array(data.mocap_quat[ghost.body])) + + rot = np.empty(9) + mujoco.mju_quat2Mat(rot, q_world_from_grip) + offset = ( + np.array(data.mocap_pos[ghost.body]) + - np.array(_mujoco_xr.mj_from_xr_pos(list(grip_pos))) + ) @ rot.reshape(3, 3) + seen.append((relative, offset)) + + for relative, offset in seen[1:]: + assert np.allclose(relative, seen[0][0], atol=1e-6), ( + "the ghost's orientation in the grip frame changes with the " + "controller's orientation -- the correction is composed on the wrong side" + ) + assert np.allclose(offset, seen[0][1], atol=1e-6), ( + "the ghost's offset in the grip frame changes with the controller's " + "orientation -- the translation is not being rotated with the grip" + ) + # And it is the configured correction, not some other rigid attachment. + assert np.allclose(seen[0][0], app._QUAT_GRIP_FROM_GHOST, atol=1e-6) + assert np.allclose(seen[0][1], app._POS_GRIP_FROM_GHOST, atol=1e-6) + + +def test_squeezing_drives_the_jaw_from_released_to_squeezed(): + """Closedness 0..1 must drive the hinge from released to squeezed. + + On the recovered ANGLE, not on where a point ends up: over a large sweep a + point on the lever traces an arc, rising along any fixed axis before falling. + """ + model = _default_scene() + data = mujoco.MjData(model) + ghost = app._resolve_ghost(model) + controller = _Controller(True, (0.0, 1.2, -0.5)) + + def hinge_angle_at(closedness): + app._update_ghost(data, ghost, _result(controller, closedness)) + mujoco.mj_forward(model, data) + inverse, hinge = np.empty(4), np.empty(4) + mujoco.mju_negQuat(inverse, np.array(data.mocap_quat[ghost.body])) + mujoco.mju_mulQuat(hinge, inverse, np.array(data.mocap_quat[ghost.jaw])) + # Signed against the hinge axis, so a wrong-way rotation reads negative + # rather than folding onto the same magnitude. + turn = 2.0 * math.atan2(float(np.linalg.norm(hinge[1:])), float(hinge[0])) + if float(np.dot(hinge[1:], app._TRIGGER_HINGE_AXIS)) < 0: + turn = -turn + return turn + + angles = [hinge_angle_at(c) for c in (0.0, 0.25, 0.5, 0.75, 1.0)] + assert angles[0] == pytest.approx(app._TRIGGER_RELEASED_RAD, abs=1e-6) + assert angles[-1] == pytest.approx(app._TRIGGER_SQUEEZED_RAD, abs=1e-6) + assert all(b < a for a, b in zip(angles, angles[1:])), ( + f"squeezing did not close the jaw monotonically: {np.round(angles, 4)}" + ) + + # And it is big enough to see: the far end of the lever sweeps ~90 mm. + def trigger_at(closedness): + app._update_ghost(data, ghost, _result(controller, closedness)) + mujoco.mj_forward(model, data) + return _geom_verts_world(model, data, "leader_ghost_trigger") + + travel = float(np.linalg.norm(trigger_at(1.0) - trigger_at(0.0), axis=1).max()) + # 84.5 mm at the tip across the joint's 0..100 degrees. Below ~50 mm + # "released" stops reading as OPEN, which is why the range is the joint's. + assert travel > 0.05, f"the trigger moves {travel * 1000:.1f} mm -- not visible" + + +def test_the_released_end_is_the_urdf_joints_upper_limit(): + """The travel is the URDF's, not a tuned number. + + Read out of the fetched so101_new_calib.urdf, so the constant is checked + against its source instead of against itself. + """ + urdf = app._LEADER_ASSETS / "so101_new_calib.urdf" + if not urdf.is_file(): + pytest.skip(f"{urdf.name} not fetched; run {app.FETCH_SCRIPT}") + tree = ElementTree.parse(urdf) + joint = next(j for j in tree.iter("joint") if j.get("name") == "gripper") + upper = float(joint.find("limit").get("upper")) + assert app._TRIGGER_RELEASED_RAD == pytest.approx(upper, abs=1e-4) + # The other end is the joint's authored zero, NOT its lower limit, which + # swings the lever into the servo. + assert app._TRIGGER_SQUEEZED_RAD == 0.0 + assert float(joint.find("limit").get("lower")) == pytest.approx( + math.radians(-10.0), abs=1e-4 + ) + + +def test_the_trigger_clears_the_whole_gripper_across_its_driven_range(): + """The lever must not pass through the gripper at any closedness. + + Against all three other parts, not the bracket alone: a range that swung + the loop into the SERVO once passed a bracket-only check. The 0.8 mm bound + is thin on purpose -- the tightest legitimate pass is 2.10 mm, while a lever + driven to the joint's -10 degree limit closes to 0.4 mm, and nearest-vertex + distance cannot go negative so interpenetration reads as a small positive. + """ + model = _default_scene() + data = mujoco.MjData(model) + ghost = app._resolve_ghost(model) + others = ( + "leader_ghost_wrist_roll", + "leader_ghost_motor", + "leader_ghost_handle", + ) + + worst = (0.0, "", 1e9) + for step in range(9): + closedness = step / 8 + app._update_ghost( + data, ghost, _result(_Controller(True, (0.0, 1.2, -0.5)), closedness) + ) + mujoco.mj_forward(model, data) + trigger = _geom_verts_world(model, data, "leader_ghost_trigger") + for part in others: + gap = _nearest_gap(trigger, _geom_verts_world(model, data, part)) + if gap < worst[2]: + worst = (closedness, part, gap) + assert worst[2] > 0.8e-3, ( + f"the trigger is {worst[2] * 1000:.2f} mm into {worst[1]} at closedness " + f"{worst[0]:.3f} -- the driven range pushes it through the body" + ) + + +def test_the_shipped_retargeter_drives_the_jaw_channel(): + """The graph edge itself: trigger -> SO101GripperRetargeter -> combiner key. + + The real pipeline on synthetic DeviceIO snapshots, so the key, the indexing + and the deadzone are the shipped retargeter's, not this file's idea of them. + """ + from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource + from isaacteleop.retargeting_engine.interface.tensor_group import TensorGroup + from isaacteleop.schema import ( + ControllerInputState, + ControllerPose, + ControllerSnapshot, + ControllerSnapshotTrackedT, + Point, + Pose, + Quaternion, + ) + + def snapshot(trigger): + pose = ControllerPose( + Pose(Point(0.1, 1.2, -0.4), Quaternion(0.0, 0.0, 0.0, 1.0)), True + ) + state = ControllerInputState( + primary_click=False, + secondary_click=False, + thumbstick_click=False, + menu_click=False, + thumbstick_x=0.0, + thumbstick_y=0.0, + squeeze_value=0.0, + trigger_value=trigger, + ) + return ControllerSnapshotTrackedT(ControllerSnapshot(pose, pose, state)) + + pipeline = app._build_pipeline() + spec = ControllersSource(name="controllers").input_spec() + + def closedness(trigger): + inputs = {} + for name in spec: + group = TensorGroup(spec[name]) + group[0] = snapshot(trigger) + inputs[name] = group + out = pipeline.execute_pipeline({"controllers": inputs}) + assert app.GRIPPER_COMMAND_KEY in out + return float(out[app.GRIPPER_COMMAND_KEY][0]) + + assert closedness(0.0) == pytest.approx(0.0) + assert closedness(1.0) == pytest.approx(1.0) + # The retargeter's own released-end deadzone, not this app's: (0.5 - 0.05) / 0.95. + assert closedness(0.5) == pytest.approx(0.4737, abs=1e-4) + + +def test_an_untracked_controller_freezes_the_whole_gripper(): + """(0, 0, 0) is the scene origin, a pose a tracked controller could hold. + + Freezing is the honest rendering of "tracking lost", and the jaw freezes + with the body rather than articulating on a stale pose. + """ + model = _default_scene() + data = mujoco.MjData(model) + ghost = app._resolve_ghost(model) + app._update_ghost( + data, ghost, _result(_Controller(True, (0.2, 1.3, -0.5)), closedness=0.0) + ) + seen_body = data.mocap_pos[ghost.body].copy() + seen_jaw = data.mocap_quat[ghost.jaw].copy() + + for controller in (_Controller(False, (9.0, 9.0, 9.0)), _NoController()): + for _ in range(3): + app._update_ghost(data, ghost, _result(controller, closedness=1.0)) + assert np.array_equal(data.mocap_pos[ghost.body], seen_body) + assert np.array_equal(data.mocap_quat[ghost.jaw], seen_jaw) + + +def test_a_scene_without_the_ghost_fragment_is_rejected(): + """The shipped scene must declare both mocap bodies; say so if it stops.""" + model = mujoco.MjModel.from_xml_string( + '' + ) + with pytest.raises(RuntimeError, match=app.GHOST_BODY): + app._resolve_ghost(model) diff --git a/examples/mujoco_xr/tests/test_projection.py b/examples/mujoco_xr/tests/test_projection.py new file mode 100644 index 000000000..d4d315bd4 --- /dev/null +++ b/examples/mujoco_xr/tests/test_projection.py @@ -0,0 +1,100 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Clip-space convention tests for the renderer's projection. + +The projection is transcribed from viz's own ``fov_to_projection_matrix`` +(src/viz/session/cpp/xr_backend.cpp), including its deliberate angleUp -> bottom +swap. These tests pin the four properties that matter, none of which needs a +GPU, a headset or a VizSession. + +`p` is COLUMN-major, so ``p[c * 4 + r]`` is ``P[c][r]``. +""" + +import math + +import pytest + +from isaacteleop_examples.mujoco_xr import _mujoco_xr + +NEAR = 0.05 +FAR = 50.0 + +# A plausible asymmetric headset fov, in radians. +FOV = [math.radians(-45.0), math.radians(42.0), math.radians(48.0), math.radians(-46.0)] + + +def _project(p, x, y, z): + """Column-major mat4 times (x, y, z, 1), returning NDC after the w-divide.""" + v = (x, y, z, 1.0) + clip = [sum(p[c * 4 + r] * v[c] for c in range(4)) for r in range(4)] + return [clip[0] / clip[3], clip[1] / clip[3], clip[2] / clip[3]] + + +def test_x_scale_is_positive(): + p = _mujoco_xr.projection_from_fov(FOV, NEAR, FAR) + assert p[0] > 0.0, "P[0][0] <= 0 means left/right are swapped" + + +def test_y_scale_is_negative_the_deliberate_flip(): + """The load-bearing assertion. + + viz maps angleUp to the frustum's BOTTOM, giving 2n/(t-b) < 0. That + negative IS the Y flip, and it drives triangle winding. A depth-range check + touches only P[2][2] / P[2][3] / P[3][2] and would not catch it. + """ + p = _mujoco_xr.projection_from_fov(FOV, NEAR, FAR) + assert p[5] < 0.0 + + +def test_depth_is_standard_z_not_reverse_z(): + """near -> 0.0, far -> 1.0. + + Two doc comments in viz claim reverse-Z; the code is standard Z. This test + is what catches anyone who believes the comments -- reverse-Z would make + P[2][2] positive and swap these two endpoints. + """ + p = _mujoco_xr.projection_from_fov(FOV, NEAR, FAR) + assert p[10] < 0.0 + assert p[14] < 0.0 + assert p[11] == pytest.approx(-1.0) + + assert _project(p, 0.0, 0.0, -NEAR)[2] == pytest.approx(0.0, abs=1e-6) + assert _project(p, 0.0, 0.0, -FAR)[2] == pytest.approx(1.0, abs=1e-6) + + +def test_depth_is_monotonic_between_the_planes(): + p = _mujoco_xr.projection_from_fov(FOV, NEAR, FAR) + depths = [_project(p, 0.0, 0.0, -z)[2] for z in (NEAR, 0.5, 5.0, FAR)] + assert depths == sorted(depths) + + +def test_symmetric_fov_centres_the_optical_axis(): + half = math.radians(40.0) + p = _mujoco_xr.projection_from_fov([-half, half, half, -half], NEAR, FAR) + assert p[8] == pytest.approx(0.0, abs=1e-6) + assert p[9] == pytest.approx(0.0, abs=1e-6) + # A point on the near plane at the right edge of a symmetric frustum lands + # on x_ndc = +1. + edge = NEAR * math.tan(half) + assert _project(p, edge, 0.0, -NEAR)[0] == pytest.approx(1.0, abs=1e-5) + + +def test_a_default_constructed_fov_is_rejected_loudly(): + """A default-constructed viz::Fov is four ZEROS, and must never render. + + Feeding that through gives right - left == 0 -> P[0][0] = +inf and + P[2][0] = P[2][1] = NaN, i.e. an all-NaN frame with no error anywhere. + ``FrameInfo.views`` is filled by the runtime, so a degenerate fov is a + runtime/session bug the app cannot prevent -- only refuse. Throwing here + turns a silently blank headset into a named failure. + """ + with pytest.raises(ValueError): + _mujoco_xr.projection_from_fov([0.0, 0.0, 0.0, 0.0], NEAR, FAR) + + +def test_near_far_are_validated(): + with pytest.raises(ValueError): + _mujoco_xr.projection_from_fov(FOV, 0.0, FAR) + with pytest.raises(ValueError): + _mujoco_xr.projection_from_fov(FOV, FAR, NEAR) diff --git a/rigs/mujoco_xr.yaml b/rigs/mujoco_xr.yaml new file mode 100644 index 000000000..22c806e00 --- /dev/null +++ b/rigs/mujoco_xr.yaml @@ -0,0 +1,27 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Run with: python -m isaacteleop.rig rigs/mujoco_xr.yaml +# +# Two panes, one declared: with no `runtime:` key the runtime pane comes from +# DEFAULT_RUNTIME_COMMAND. No producers either -- the app opens the OpenXR +# session itself and reads controllers straight from the runtime, so there is no +# rendezvous and deliberately no `params:` / `collection_id:`. +# +# `{python}` expands to the launching interpreter, so both wheels must be +# installed there rather than in a private .venv: +# +# uv pip install "isaacteleop[cloudxr]" --find-links=./install/wheels/ --reinstall +# uv pip install ./examples/mujoco_xr # same environment +# +# See rigs/se3_tracker.yaml for the fully annotated exemplar of every key. +name: mujoco_xr +description: CloudXR runtime + MuJoCo scene in XR with the SO-101 leader gripper +cwd: .. # -> Teleop repo root +consumers: + - name: mujoco xr app (requires headset) + # Keep --no-launch-cloudxr-runtime. The runtime is a host singleton on WSS + # port 48322, so a second one kills this rig's own pane: the headset drops + # mid-session and reads as a runtime crash rather than a config edit. + # find_runtime_footguns() warns but never gates. + command: "{python} -m isaacteleop_examples.mujoco_xr --no-launch-cloudxr-runtime" From b322c5156282d2218e55dd9e5ac5807b90d5ac84 Mon Sep 17 00:00:00 2001 From: Farbod Motlagh Date: Mon, 10 Aug 2026 13:58:50 -0700 Subject: [PATCH 2/4] examples/mujoco_xr: draw with MuJoCo's renderer, read back through a PBO The hand-written Vulkan renderer is gone. mjr_render draws into MuJoCo's offscreen framebuffer; cpp/gl_readback.cpp blits that into a sampleable pair, runs one fullscreen pass, and glReadPixels into a GL_PIXEL_PACK_BUFFER that cudaGraphicsGLRegisterBuffer imports -- which is the CUDA-linear pointer ProjectionLayer.submit() already took. Still no host copy, and now every geom type, the scene XML's materials, lights and shadows, and MuJoCo's own mesh handling. No Vulkan, no glslangValidator, no viz change. README.md claimed this was impossible because cudaGraphicsGLRegisterImage registers no depth format and no multisampled renderbuffer. That is true, and it is the wrong entry point: RegisterBuffer has neither restriction. Three things checked against MuJoCo 3.11.0's source rather than assumed. frustum_width is a HALF-width and IS used (render_gl3.c setView), so an asymmetric per-eye fov is expressible exactly -- mjvisualize.h calls the field "not used for rendering" and is wrong, and leaving it 0 silently switches on a viewport-aspect fallback that renders something plausible from a fov carrying nothing. mjr_render is reverse Z, so the readback shader's 1.0 - d is MuJoCo's own flipDepthIfRequired moved onto the GPU. And glBlitFramebuffer rejects a depth blit between differing formats, so the blit target is matched to whatever MuJoCo picked (DEPTH32F_STENCIL8 or DEPTH24_STENCIL8, per ARB_depth_buffer_float) rather than assumed. tests/test_readback.py drives the real GPU path headlessly, which nothing here did before: that row 0 is the top of the operator's view, that the image is not mirrored, that the submitted depth is standard Z with the background at exactly 1.0, and that the eyes carry parallax of the right sign. It skips with a reason when there is no GPU. It caught mjr_render drawing into whatever framebuffer is bound -- leave ours bound and the next frame goes to it, with no GL error anywhere. Two things this loses. The crease-aware vertex normals go with the renderer, and mjModel's own are smeared: measured on the pinned meshes, 11.4% / 4.5% / 1.0% / 16.6% of face corners point away from their own triangle, and render_gl3.c lights one-sided, so the ghost may render as shattered facets. test_ghost.py pins those fractions, and the fix -- if a headset says one is needed -- belongs in the asset, not in a second renderer. Separately, a multi-GPU host now needs MUJOCO_EGL_DEVICE_ID: the OpenGL context is created independently of viz's Vulkan device, so the renderer checks and names both device numbers rather than render into the wrong card's memory. Everything downstream of the readback is still executed nowhere -- submit(), the frame loop, OpenXR session sharing, and whether the runtime accepts the depth layer -- and the example remains unconfigured in CI. Signed-off-by: Farbod Motlagh --- examples/mujoco_xr/README.md | 170 +++-- examples/mujoco_xr/cpp/CMakeLists.txt | 92 +-- examples/mujoco_xr/cpp/compile_shader.cmake | 49 -- examples/mujoco_xr/cpp/gl.cpp | 132 ++++ examples/mujoco_xr/cpp/gl.hpp | 152 ++++ examples/mujoco_xr/cpp/gl_readback.cpp | 398 ++++++++++ examples/mujoco_xr/cpp/gl_readback.hpp | 89 +++ examples/mujoco_xr/cpp/glcamera.hpp | 77 ++ examples/mujoco_xr/cpp/mesh_buffers.cpp | 105 --- examples/mujoco_xr/cpp/mesh_buffers.hpp | 71 -- examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp | 206 ++--- examples/mujoco_xr/cpp/render_target.cpp | 409 ---------- examples/mujoco_xr/cpp/render_target.hpp | 154 ---- examples/mujoco_xr/cpp/scene_renderer.cpp | 718 +++--------------- examples/mujoco_xr/cpp/scene_renderer.hpp | 98 +-- examples/mujoco_xr/cpp/shaders/scene.frag | 48 -- examples/mujoco_xr/cpp/shaders/scene.vert | 49 -- .../mujoco_xr/__init__.py | 18 +- .../isaacteleop_examples/mujoco_xr/app.py | 133 ++-- .../mujoco_xr/assets/scene.xml | 18 +- examples/mujoco_xr/tests/CMakeLists.txt | 17 +- examples/mujoco_xr/tests/test_app_helpers.py | 58 +- examples/mujoco_xr/tests/test_ghost.py | 66 +- examples/mujoco_xr/tests/test_projection.py | 105 ++- examples/mujoco_xr/tests/test_readback.py | 176 +++++ 25 files changed, 1587 insertions(+), 2021 deletions(-) delete mode 100644 examples/mujoco_xr/cpp/compile_shader.cmake create mode 100644 examples/mujoco_xr/cpp/gl.cpp create mode 100644 examples/mujoco_xr/cpp/gl.hpp create mode 100644 examples/mujoco_xr/cpp/gl_readback.cpp create mode 100644 examples/mujoco_xr/cpp/gl_readback.hpp create mode 100644 examples/mujoco_xr/cpp/glcamera.hpp delete mode 100644 examples/mujoco_xr/cpp/mesh_buffers.cpp delete mode 100644 examples/mujoco_xr/cpp/mesh_buffers.hpp delete mode 100644 examples/mujoco_xr/cpp/render_target.cpp delete mode 100644 examples/mujoco_xr/cpp/render_target.hpp delete mode 100644 examples/mujoco_xr/cpp/shaders/scene.frag delete mode 100644 examples/mujoco_xr/cpp/shaders/scene.vert create mode 100644 examples/mujoco_xr/tests/test_readback.py diff --git a/examples/mujoco_xr/README.md b/examples/mujoco_xr/README.md index b9446689e..144e43f1b 100644 --- a/examples/mujoco_xr/README.md +++ b/examples/mujoco_xr/README.md @@ -13,28 +13,46 @@ Single process, single thread, **one** OpenXR session: ``` VizSession(kXr) ──get_oxr_handles()──▶ TeleopSession │ │ - │ vk_device / vk_physical_device │ controller grip poses + │ recommended resolution │ controller grip poses ▼ ▼ -_mujoco_xr.Renderer ──__cuda_array_interface__──▶ ProjectionLayer.submit() +mjr_render ─blit─▶ flip + depth-invert ─glReadPixels─▶ PBO ═CUDA═▶ submit() ``` That is the thesis: `VizSession` (rendering) and `TeleopSession` (input) share -one OpenXR session via `get_oxr_handles()`, and a MuJoCo scene drawn with -Vulkan into images viz owns reaches `ProjectionLayer.submit()` by CUDA pointer -with no copy through host memory. Nothing else in this repository does that. - -**`cpp/` exists because of depth, not because of Vulkan.** -`ProjectionLayer.submit()` takes a CUDA-linear buffer rather than a Vulkan -image, so MuJoCo's own OpenGL renderer could in principle reach it through -GL→CUDA interop. What stops that is depth: `cudaGraphicsGLRegisterImage` -registers no depth format and no multisampled renderbuffer, while -`mjrContext.offDepthStencil` is a combined depth+stencil renderbuffer and -`offsamples` defaults to 4. Colour would register; the per-eye D32F this layer -submits for CloudXR reprojection would need a host round-trip through -`mjr_readPixels` or a patched `mjr_makeContext`. It is the assumption here most -worth re-testing — MuJoCo's renderer draws every geom type, with the scene -XML's materials, lights and shadows, where this one draws lit meshes and -nothing else. +one OpenXR session via `get_oxr_handles()`, and **MuJoCo's own renderer** +reaches `ProjectionLayer.submit()` by CUDA pointer with no copy through host +memory. Nothing else in this repository does that. + +**`cpp/` is a readback, not a renderer.** `mjr_render` draws into MuJoCo's +offscreen framebuffer; `cpp/gl_readback.cpp` blits that into a sampleable pair, +runs one fullscreen pass, and reads the result into a pixel-pack buffer that +CUDA imports. Every step stays in video memory. The whole module is ~700 lines +and owns no shading, no meshes and no camera maths beyond six frustum numbers. + +The trick is which CUDA entry point is used. `cudaGraphicsGLRegisterImage` +registers no depth format and no multisampled renderbuffer, and +`mjrContext.offDepthStencil` is both — that is the wall a naive port hits. +`cudaGraphicsGLRegisterBuffer` has neither restriction, and `glReadPixels` into +a bound `GL_PIXEL_PACK_BUFFER` is a device-to-device transfer, so the CUDA-linear +buffer `submit()` already wants falls straight out of it. + +The fullscreen pass exists for two conversions, both of which are silent +failures on anything short of a headset: + +| | MuJoCo writes | ProjectionLayer is promised | +|---|---|---| +| row 0 | bottom (`glClipControl(GL_LOWER_LEFT, ...)`) | top | +| depth | reverse Z, near → 1 (`GL_GEQUAL`, `glClearDepth(0)`) | near → 0 | + +The depth line is `1.0 - d`, which is exactly what MuJoCo's own +`mjr_readPixels` does on the CPU (`flipDepthIfRequired`, render_gl2.c); doing it +in the shader is what keeps the host out of the loop. + +**What this costs.** One blit, one fullscreen pass and one pack-buffer copy per +eye per frame, all in VRAM, against a path that already copies image → staging +buffer → mailbox array → swapchain. **What it buys:** every geom type, the scene +XML's materials, lights, shadows and reflections, and MuJoCo's own mesh +handling. `_mujoco_xr` links `libmujoco`, so this example ships as its own wheel rather than inside `isaacteleop` — otherwise that wheel's contents would depend on @@ -47,8 +65,8 @@ extension and asserts both report the same version. | | | |---|---| -| **Covered by tests** | [`ctest -L mujoco_xr`](#tests) — the frame conventions, the projection convention, the clock, the ghost overlay and its jaw channel. All **pure CPU**: no GPU, no headset, no runtime, no window system. | -| **Never executed anywhere** | **The app itself.** `kXr` is the only display mode and it needs a headset plus a CloudXR runtime, so the frame loop, the renderer, OpenXR session sharing via `oxr_handles`, controllers on a shared session, the Vulkan→CUDA→`submit()` path and whether the runtime accepts the depth layer are run by no test and by no developer here. | +| **Covered by tests** | [`ctest -L mujoco_xr`](#tests) — the frame conventions, the frustum, the clock, the ghost overlay and its jaw channel, all pure CPU; **plus `test_readback.py`, which drives the real GPU path** (mjr_render → blit → flip/invert → PBO → CUDA) on any machine with a GPU, and skips loudly without one. | +| **Never executed anywhere** | **The XR half.** `kXr` is the only display mode and needs a headset plus a CloudXR runtime, so the frame loop, `ProjectionLayer.submit()`, OpenXR session sharing via `oxr_handles`, controllers on a shared session and whether the runtime accepts the depth layer are run by no test and by no developer here. | | **Wrong by construction until calibrated** | The workspace translation, for any scene that adds static content — see [Frames](#frames-cppframeshpp). The shipped ghost-only scene does not show it. | Nothing in `.github/workflows/` installs `mujoco`, so the example is never @@ -99,13 +117,20 @@ Both wheels must land in **one** environment, and that is the environment compiles the extension through scikit-build-core and does not read the CMake build tree at all. -You need `uv`, CMake ≥ 3.21, a C++ compiler, the Vulkan SDK/loader, CUDA, and -`glslangValidator` (`apt install glslang-tools`; the scene shaders are compiled -to SPIR-V at build time, and its absence is a hard `FATAL_ERROR` here). Running -the app additionally needs a GPU with Vulkan + CUDA and a headset. **Build -isolation does not cover the non-Python half of that list**: on a host missing -CUDA, the Vulkan loader or `glslangValidator`, the install fails *inside* the -isolated PEP-517 build with the CMake error wrapped in backend output. +You need `uv`, CMake ≥ 3.21, a C++ compiler and CUDA. No Vulkan and no +`glslangValidator`: the readback shader is a string compiled at runtime by the +driver, and the module links no Vulkan and no OpenGL (`cpp/gl.hpp` resolves the +~30 GL entry points it calls through the platform `GetProcAddress`, against the +context `mujoco.GLContext` created). Running the app additionally needs a GPU +with EGL + CUDA and a headset. **Build isolation does not cover the non-Python +half of that list**: on a host missing CUDA the install fails *inside* the +isolated PEP-517 build, with the CMake error wrapped in backend output. + +**On a multi-GPU host, set `MUJOCO_EGL_DEVICE_ID`.** The OpenGL context has to +land on the same card viz picked, and nothing makes that happen by default — +`MUJOCO_EGL_DEVICE_ID` indexes EGL devices, which need not agree with CUDA's +ordering. The renderer checks at construction and names both device numbers +rather than render into the wrong card's memory. **`pip install -e` is not supported.** An editable install redirects the package back to the source tree, which is exactly where the in-tree CMake build drops @@ -204,8 +229,9 @@ lines at all** is the tell. There is one scene and no flag to change it: `assets/scene.xml` is package data beside the module, and editing it is how you load something else. There is no -desktop or headless mode either; without a headset the only verification path is -[`ctest -L mujoco_xr`](#tests), which exercises no GPU code at all. +desktop or headless display mode; without a headset the verification path is +[`ctest -L mujoco_xr`](#tests), which does now exercise the GPU path — but +nothing downstream of `ProjectionLayer.submit()`. ## Conventions you can break @@ -228,7 +254,7 @@ scene that puts static content on the work surface owns re-tuning it. **Neither term may be zeroed.** It places static content only. The ghost goes out through `mj_from_xr` and the -renderer folds it back through `xr_from_mj`, so both constants cancel on it and +eye pose goes out through the same transform, so both constants cancel on it and the shipped scene — which is the ghost and nothing else — is blind to a wrong value. Judging one means a scene with something world-locked in it. @@ -236,10 +262,10 @@ There is no recentre keypress and no runtime override: changing the datum means editing the constant and rebuilding (~8 s). The procedure is to stand where you intend to work, start the app on such a scene, read the `frames:` line in the startup log, compare the virtual surface against the real one, and adjust `z`. A -`--workspace-offset` flag was considered and rejected — `Renderer` bakes -`xr_from_mj_` at construction while the ghost's pose is converted per frame, so -a Python-side offset would move the gripper and leave the scene put, which is -precisely the symptom this example exists to disambiguate. +`--workspace-offset` flag was considered and rejected: a Python-side offset +applied to one of the two conversions and not the other would move the gripper +and leave the scene put, which is precisely the symptom this example exists to +disambiguate. ### Where the ghost sits on the hand (`app.py`) @@ -288,21 +314,25 @@ near-isotropic blob (σ₀/σ₁ = 1.26), so its principal direction is noise. ### Scene assets -The renderer draws `mjGEOM_MESH` and nothing else (this is an AR scene; -passthrough is the background, so there is no ground plane to draw), which means -a box, sphere or capsule in the XML renders as nothing. Lighting declared in the -XML is inert — -`cpp/shaders/scene.frag` has one hardcoded directional light and `mjvGLCamera` -is bypassed. - -**`cpp/mesh_buffers.cpp` computes its own vertex normals, and must.** MuJoCo -welds an STL's vertices and keeps one averaged normal per welded vertex, so on a -CAD part every crease gets a normal smeared across it; lit one-sided, those -corners drop to `scene.frag`'s 0.35 ambient floor and the gripper renders as -**shattered facets**, which reads as a broken mesh and is not one. Normals are -instead area-averaged over the faces round each corner that lie within -`kCreaseCos`. The measured counts are in `cpp/mesh_buffers.hpp`, and -`test_ghost.py` fails if anyone reverts to `mjModel`'s. +Every geom type draws, and the XML's materials, lights, shadows and +reflections are live — this is `mjr_render`, so the scene file means what the +MuJoCo docs say it means. `scene.xml` declares no lights, so what lights it is +`model.vis.headlight`, on by default. + +**Open risk: the ghost may render as shattered facets.** MuJoCo stores one +averaged normal per welded vertex (`mesh_normalnum == mesh_vertnum`, +`mesh_facenormal == mesh_face`), so a crease on a CAD part gets a normal smeared +across it, and `render_gl3.c` lights one-sided +(`glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 0)`). Measured on mujoco 3.11.0 against +the pinned meshes, the share of face corners whose normal points away from its +own triangle is 11.4% (`wrist_roll`), 4.5% (`trigger`), 1.0% (`handle`) and +16.6% (the servo); `test_ghost.py` pins those numbers. + +Drawing with MuJoCo's renderer means drawing with those normals, and **no +headless test can say how it looks**. If it does shatter, the fix belongs in the +asset or the compiler — `smoothnormal` on the ``, or upstream winding — +not in a hand-written renderer. An earlier revision of this example carried one +for exactly this reason; see `git log` for what that cost. The ghost's four STLs are **fetched, not vendored** — 2.3 MB of binary in a source tree is a poor trade when upstream publishes them at a stable commit, and @@ -333,10 +363,9 @@ that `mj_step` integrates gravity into, and a mocap body is kinematic by construction. The ghost is **opaque**, and `test_ghost.py` asserts it. That removes the -draw-order constraint (at alpha 1.0 the depth test decides everything), the -ghost-writes-depth-into-the-reprojection-buffer concern, and the self-overlap -darkening from `cullMode = VK_CULL_MODE_NONE`. A scene that puts a robot under -the ghost and drops the alpha back takes all three on again: `mjv_updateScene` +draw-order constraint (at alpha 1.0 the depth test decides everything) and the +ghost-writes-depth-into-the-reprojection-buffer concern. A scene that puts a +robot under the ghost and drops the alpha back takes both on again: `mjv_updateScene` emits in geom-id order, so the `` must come **last**. Nothing asserts that ordering today — it only matters below alpha 1.0, so the test belongs with the scene that needs it. @@ -346,14 +375,6 @@ model path mis-composes the mesh paths of an ``d file in a subdirectory and fails with `Error opening file ''`. `DEFAULT_SCENE` in `app.py` is absolute for this reason. -### Culling - -`cullMode` is `VK_CULL_MODE_NONE` during bring-up, and that is a decision, not -an omission. The projection flips Y (`P[1][1] < 0`), which inverts the effective -winding; get that wrong with culling on and the scene renders **black**, which -is routinely misdiagnosed as a depth or submit bug. Turn it on only after a -headset has confirmed the scene is visible. - ## Tests ```bash @@ -363,21 +384,26 @@ ctest --test-dir build/cmake-cpython-312 -L mujoco_xr --output-on-failure | file | covers | |---|---| | `test_frames.py` | the XR→MuJoCo axis map and quaternion order | -| `test_projection.py` | the clip-space convention (Y flip, standard Z, degenerate-fov rejection) | -| `test_app_helpers.py` | the NaN-safe `dt` clamp, the zeroed-`predicted_display_time` guard, the single near/far pair, and that the first-frame projection assertion actually fires | -| `test_ghost.py` | the overlay: that the ghost is opaque, collision-free and carries no mass, that both its bodies are kinematic mocap bodies with no joint anywhere, that the four leader parts form one assembly with sub-mm gaps at the bolted joints and the servo seated in its bracket, that the print STLs are scaled from millimetres and the servo is not, that every corner normal the renderer builds faces the same way as its own triangle (mjModel's do not, and that is what made the ghost render as shattered facets), that the ghost is *rigidly attached* to the grip frame whatever the calibration, that squeezing swings the trigger monotonically from the URDF joint's upper limit to its authored zero without driving the lever through the body, that the shipped `SO101GripperRetargeter` really is the thing driving that channel (built as a real pipeline and fed synthetic DeviceIO snapshots), and that an untracked controller freezes the whole gripper rather than parking it at the scene origin | +| `test_projection.py` | the mjvGLCamera frustum (that it is the fov projected onto the near plane, and that the half-width is set so mjr_render's aspect fallback stays off) and the standard-Z depth contract | +| `test_app_helpers.py` | the NaN-safe `dt` clamp, the zeroed-`predicted_display_time` guard, the single near/far pair, and that the first-frame frustum assertion passes on the real thing and fires on each way it can go wrong | +| `test_readback.py` | **the GPU path**: that something is drawn at all, that row 0 is the top of the operator's view and the image is not mirrored, that the depth handed to `submit()` is standard Z with the background at exactly 1.0, and that the two eyes carry parallax of the right sign. Skips with a reason when there is no GPU | +| `test_ghost.py` | the overlay: that the ghost is opaque, collision-free and carries no mass, that both its bodies are kinematic mocap bodies with no joint anywhere, that the four leader parts form one assembly with sub-mm gaps at the bolted joints and the servo seated in its bracket, that the print STLs are scaled from millimetres and the servo is not, the measured share of mjModel normals that face away from their own triangle (a pin on a known defect, not a property we want -- see [Scene assets](#scene-assets)), that the ghost is *rigidly attached* to the grip frame whatever the calibration, that squeezing swings the trigger monotonically from the URDF joint's upper limit to its authored zero without driving the lever through the body, that the shipped `SO101GripperRetargeter` really is the thing driving that channel (built as a real pipeline and fed synthetic DeviceIO snapshots), and that an untracked controller freezes the whole gripper rather than parking it at the scene origin | -Every one runs on a CPU with no GPU, no headset, no CloudXR runtime and no -window system. Keep it that way: a permanently-skipping test reports green while -covering nothing. +All but `test_readback.py` run on a CPU with no GPU, no headset, no CloudXR +runtime and no window system; keep it that way, because a permanently-skipping +test reports green while covering nothing. `test_readback.py` is the deliberate +exception: what it covers is otherwise invisible until someone is wearing a +headset, and it needs no headset itself. ## Not verified anywhere in CI or on a developer desktop -**Everything the GPU touches.** The renderer, the Vulkan→CUDA export, -`ProjectionLayer.submit()`, the frame loop that sequences them, OpenXR session -sharing via `oxr_handles`, whether the runtime accepts the depth layer, and -**controllers on a shared session** — none of it is executed by any test or on -any machine here. The grip-to-gripper calibration is a headset-only judgement +**Everything downstream of the readback.** `ProjectionLayer.submit()`, the +frame loop that sequences it, OpenXR session sharing via `oxr_handles`, whether +the runtime accepts the depth layer, and **controllers on a shared session** — +none of it is executed by any test or on any machine here. `test_readback.py` +covers the render and the CUDA hand-off and stops at `submit()`. How the ghost +*looks* is unverified too, and there is a named reason to expect trouble: see +the normals warning under [Scene assets](#scene-assets). The grip-to-gripper calibration is a headset-only judgement by construction: it is a claim about how a hand holds a tool, and no headless test can confirm it — `tests/test_ghost.py` pins the *machinery* against a reference calibration and deliberately leaves the shipped constants free to be diff --git a/examples/mujoco_xr/cpp/CMakeLists.txt b/examples/mujoco_xr/cpp/CMakeLists.txt index 7276d7d8a..dbe170c3a 100644 --- a/examples/mujoco_xr/cpp/CMakeLists.txt +++ b/examples/mujoco_xr/cpp/CMakeLists.txt @@ -1,105 +1,49 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 # -# The pybind11 module `_mujoco_xr` -- a MuJoCo -> Vulkan renderer that writes +# The pybind11 module `_mujoco_xr` -- MuJoCo's own OpenGL renderer, read back # into CUDA-visible buffers for viz::ProjectionLayer. # -# Protocol-only linkage: this module links no viz:: target. It receives -# VkDevice / VkPhysicalDevice / queue-family-index as plain uintptr_t and hands -# back __cuda_array_interface__ objects, so linking viz would buy a dependency -# on its ABI for nothing. For the same reason the pybind11 need not be the one -# the root build FetchContents -- nothing pybind11-registered crosses this -# boundary. Do not pin them together. +# Protocol-only linkage: this module links no viz:: target. It hands back +# __cuda_array_interface__ objects, so linking viz would buy a dependency on its +# ABI for nothing. For the same reason the pybind11 need not be the one the root +# build FetchContents -- nothing pybind11-registered crosses this boundary. Do +# not pin them together. +# +# No OpenGL on the link line either: cpp/gl.hpp resolves what it calls through +# the platform GetProcAddress, against the context `mujoco.GLContext` created. +# Adding libGL here would introduce a second dispatch path for the same context. # # Inherited-scope inputs, all set by ../CMakeLists.txt: # _mujoco_library, _mujoco_include_dir, _mujoco_xr_root, _mujoco_xr_standalone cmake_minimum_required(VERSION 3.20) -find_package(Vulkan REQUIRED) find_package(CUDAToolkit REQUIRED) -# ============================================================================== -# Shaders -# ============================================================================== -# Probed here in both configures, like the find_package pair above: this target -# needs the tool, so this target looks for it. Deliberately not reusing the root -# build's `_viz_glslang` cache entry -- writing into it from an example inverts -# the ownership. -find_program(_mujoco_xr_glslang NAMES glslangValidator) -if(NOT _mujoco_xr_glslang) - message(FATAL_ERROR "mujoco_xr: glslangValidator not found; the scene shaders cannot be " - "compiled. Install glslang-tools.") -endif() - -# Module-prefixed so sources include : only -# the generated root goes on the include path, so the target never adds "." -# (rule 3). -set(_shader_gen_root "${CMAKE_CURRENT_BINARY_DIR}/gen") -set(_shader_gen_dir "${_shader_gen_root}/mujoco_xr/shaders") -file(MAKE_DIRECTORY "${_shader_gen_dir}") - -# compile_shader( ): GLSL -> SPIR-V -> constexpr byte -# array header. Local duplicate of src/viz/shaders/cpp/CMakeLists.txt's -# function; see the TODO in compile_shader.cmake for why it is not promoted. -# Appends to `_shader_headers` in the caller's scope so the headers can be -# listed directly as target sources (no extra custom target -- rule 1). -function(compile_shader GLSL_PATH VAR_NAME) - get_filename_component(_glsl_name "${GLSL_PATH}" NAME) - set(_spv_path "${CMAKE_CURRENT_BINARY_DIR}/${_glsl_name}.spv") - set(_header_path "${_shader_gen_dir}/${_glsl_name}.spv.h") - - add_custom_command( - OUTPUT "${_spv_path}" - COMMAND ${_mujoco_xr_glslang} -V "${CMAKE_CURRENT_SOURCE_DIR}/${GLSL_PATH}" -o "${_spv_path}" - DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/${GLSL_PATH}" - COMMENT "Compiling shader ${_glsl_name} -> SPIR-V" - VERBATIM - ) - add_custom_command( - OUTPUT "${_header_path}" - COMMAND ${CMAKE_COMMAND} - -DSPV_PATH=${_spv_path} - -DHEADER_PATH=${_header_path} - -DVAR_NAME=${VAR_NAME} - -P "${CMAKE_CURRENT_SOURCE_DIR}/compile_shader.cmake" - DEPENDS "${_spv_path}" "${CMAKE_CURRENT_SOURCE_DIR}/compile_shader.cmake" - COMMENT "Embedding ${_glsl_name}.spv -> ${VAR_NAME}" - VERBATIM - ) - set(_shader_headers ${_shader_headers} "${_header_path}" PARENT_SCOPE) -endfunction() - -compile_shader(shaders/scene.vert kSceneVertSpv) -compile_shader(shaders/scene.frag kSceneFragSpv) - -# ============================================================================== -# The module -# ============================================================================== pybind11_add_module(mujoco_xr_py mujoco_xr_bindings.cpp - mesh_buffers.cpp - render_target.cpp + gl.cpp + gl_readback.cpp scene_renderer.cpp frames.hpp - mesh_buffers.hpp - render_target.hpp + gl.hpp + glcamera.hpp + gl_readback.hpp scene_renderer.hpp - ${_shader_headers} ) target_include_directories(mujoco_xr_py PRIVATE - "${_shader_gen_root}" "${_mujoco_include_dir}" ) target_link_libraries(mujoco_xr_py PRIVATE - Vulkan::Vulkan - # cudart_static matches viz_core (src/viz/core/cpp/CMakeLists.txt): no - # runtime libcudart.so dependency on a machine that has only the driver. + # Matches viz_core: no runtime libcudart.so on a driver-only machine. CUDA::cudart_static + # gl.cpp's loader needs dlopen/dlsym. + ${CMAKE_DL_LIBS} "${_mujoco_library}" ) diff --git a/examples/mujoco_xr/cpp/compile_shader.cmake b/examples/mujoco_xr/cpp/compile_shader.cmake deleted file mode 100644 index c388952f7..000000000 --- a/examples/mujoco_xr/cpp/compile_shader.cmake +++ /dev/null @@ -1,49 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -# -# Helper script invoked from add_custom_command to convert a SPIR-V binary -# into a C++ header containing an inline constexpr byte array. -# Driven by command-line variables: SPV_PATH, HEADER_PATH, VAR_NAME. -# -# TODO: this is a near-verbatim duplicate of -# src/viz/shaders/cpp/compile_shader.cmake, which hardcodes -# `namespace viz::shaders` in its output and reads SHADERS_GEN_DIR from its -# own directory scope. Promoting it to cmake/ is a viz refactor; it should -# not ride along on an example. - -if(NOT DEFINED SPV_PATH OR NOT DEFINED HEADER_PATH OR NOT DEFINED VAR_NAME) - message(FATAL_ERROR "compile_shader.cmake requires SPV_PATH, HEADER_PATH, VAR_NAME") -endif() - -file(READ "${SPV_PATH}" SPV_CONTENT HEX) -string(LENGTH "${SPV_CONTENT}" SPV_HEX_LEN) -math(EXPR SPV_BYTE_LEN "${SPV_HEX_LEN} / 2") -if(SPV_BYTE_LEN EQUAL 0) - message(FATAL_ERROR "compile_shader.cmake: ${SPV_PATH} is empty") -endif() - -string(REGEX REPLACE "([0-9a-f][0-9a-f])" "0x\\1, " SPV_BYTES "${SPV_CONTENT}") - -set(HEADER_CONTENT -"// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 -// -// AUTO-GENERATED FROM ${SPV_PATH} BY compile_shader.cmake. DO NOT EDIT. - -#pragma once - -#include -#include - -namespace mujoco_xr::shaders -{ - -alignas(uint32_t) inline constexpr unsigned char ${VAR_NAME}[] = { - ${SPV_BYTES} -}; -inline constexpr size_t ${VAR_NAME}Size = sizeof(${VAR_NAME}); - -} // namespace mujoco_xr::shaders -") - -file(WRITE "${HEADER_PATH}" "${HEADER_CONTENT}") diff --git a/examples/mujoco_xr/cpp/gl.cpp b/examples/mujoco_xr/cpp/gl.cpp new file mode 100644 index 000000000..7e5b6790f --- /dev/null +++ b/examples/mujoco_xr/cpp/gl.cpp @@ -0,0 +1,132 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "gl.hpp" + +#include +#include +#include + +namespace mujoco_xr +{ +namespace gl +{ + +#define MUJOCO_XR_GL_DEFINE(ret, name, args) ret(*name) args = nullptr; +MUJOCO_XR_GL_FUNCTIONS(MUJOCO_XR_GL_DEFINE) +#undef MUJOCO_XR_GL_DEFINE + +namespace +{ + +using ProcLoader = void* (*)(const char*); + +// RTLD_NOLOAD first: we want the copy the process already loaded, since that is +// the one `mujoco.GLContext` made a context on -- a second copy resolves against +// a dispatch table with no current context. Plain dlopen as a fallback. +void* open_already_loaded(const char* soname) +{ + void* handle = dlopen(soname, RTLD_LAZY | RTLD_NOLOAD); + if (handle == nullptr) + { + handle = dlopen(soname, RTLD_LAZY); + } + return handle; +} + +// eglGetProcAddress / glXGetProcAddress, whichever this process has. Both +// return the libglvnd dispatch stub for the calling thread's current context, +// so either serves whatever MUJOCO_GL selected; we try EGL first because +// headless (the only mode this example runs in) is the EGL path. +ProcLoader find_proc_loader() +{ + struct Candidate + { + const char* soname; + const char* symbol; + }; + static constexpr Candidate kCandidates[] = { + { "libEGL.so.1", "eglGetProcAddress" }, + { "libGLX.so.0", "glXGetProcAddressARB" }, + { "libGL.so.1", "glXGetProcAddressARB" }, + { "libGL.so.1", "glXGetProcAddress" }, + }; + for (const Candidate& c : kCandidates) + { + void* handle = open_already_loaded(c.soname); + if (handle == nullptr) + { + continue; + } + void* sym = dlsym(handle, c.symbol); + if (sym != nullptr) + { + return reinterpret_cast(sym); + } + } + throw std::runtime_error( + "mujoco_xr: found neither eglGetProcAddress nor glXGetProcAddress. The OpenGL context must be " + "created (mujoco.GLContext) BEFORE the renderer, on this thread."); +} + +bool loaded = false; + +} // namespace + +void load() +{ + if (loaded) + { + return; + } + const ProcLoader get_proc = find_proc_loader(); + + // Assigned through a void* rather than a reinterpret_cast per line: the + // -Wall build rejects casting an object pointer straight to a function + // pointer, and GetProcAddress is defined to return one anyway. +#define MUJOCO_XR_GL_LOAD(ret, name, args) \ + { \ + void* sym = get_proc("gl" #name); \ + if (sym == nullptr) \ + { \ + throw std::runtime_error(std::string("mujoco_xr: OpenGL entry point gl" #name \ + " is unavailable. Either no context is current on this thread, " \ + "or it is older than OpenGL 3.3.")); \ + } \ + name = reinterpret_cast(sym); \ + } + MUJOCO_XR_GL_FUNCTIONS(MUJOCO_XR_GL_LOAD) +#undef MUJOCO_XR_GL_LOAD + + loaded = true; +} + +void check(const char* what) +{ + GLenum first = GL_NO_ERROR; + for (GLenum err = GetError(); err != GL_NO_ERROR; err = GetError()) + { + if (first == GL_NO_ERROR) + { + first = err; + } + } + if (first != GL_NO_ERROR) + { + throw std::runtime_error(std::string("mujoco_xr: OpenGL error 0x") + + [](GLenum e) + { + static const char* kHex = "0123456789abcdef"; + std::string s; + for (int shift = 12; shift >= 0; shift -= 4) + { + s.push_back(kHex[(e >> shift) & 0xF]); + } + return s; + }(first) + + " during " + what); + } +} + +} // namespace gl +} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/gl.hpp b/examples/mujoco_xr/cpp/gl.hpp new file mode 100644 index 000000000..69b80e242 --- /dev/null +++ b/examples/mujoco_xr/cpp/gl.hpp @@ -0,0 +1,152 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +// The handful of OpenGL entry points this module calls, resolved at runtime. +// +// No GL headers and no libGL on the link line: the context belongs to +// `mujoco.GLContext` (EGL, GLX or OSMesa, per MUJOCO_GL), and linking libGL here +// would add a second dispatch path to it. Everything used is core GL 3.3, well +// under the 4.5 mjr_render itself needs for glClipControl. + +#include +#include + +namespace mujoco_xr +{ +namespace gl +{ + +// ── Types ───────────────────────────────────────────────────────────────── +using GLenum = unsigned int; +using GLbitfield = unsigned int; +using GLuint = unsigned int; +using GLint = int; +using GLsizei = int; +using GLfloat = float; +using GLchar = char; +using GLboolean = unsigned char; +using GLsizeiptr = std::ptrdiff_t; +using GLintptr = std::ptrdiff_t; + +// ── Enums ───────────────────────────────────────────────────────────────── +// Spelled out rather than included, for the reason in the file comment. Values +// are from the OpenGL registry and are ABI, not choices. +constexpr GLenum GL_NO_ERROR = 0; +constexpr GLenum GL_FALSE = 0; +constexpr GLenum GL_TRUE = 1; +constexpr GLenum GL_TRIANGLES = 0x0004; +constexpr GLenum GL_UNSIGNED_BYTE = 0x1401; +constexpr GLenum GL_FLOAT = 0x1406; +constexpr GLenum GL_RED = 0x1903; +constexpr GLenum GL_RGBA = 0x1908; +constexpr GLenum GL_DEPTH_COMPONENT = 0x1902; +constexpr GLenum GL_DEPTH_STENCIL = 0x84F9; +constexpr GLenum GL_UNSIGNED_INT_24_8 = 0x84FA; +constexpr GLenum GL_FLOAT_32_UNSIGNED_INT_24_8_REV = 0x8DAD; +constexpr GLenum GL_RGBA8 = 0x8058; +constexpr GLenum GL_R32F = 0x822E; +constexpr GLenum GL_DEPTH24_STENCIL8 = 0x88F0; +constexpr GLenum GL_DEPTH32F_STENCIL8 = 0x8CAD; +constexpr GLenum GL_FRAMEBUFFER_ATTACHMENT_COMPONENT_TYPE = 0x8211; +constexpr GLenum GL_DEPTH_ATTACHMENT = 0x8D00; +constexpr GLenum GL_TEXTURE_2D = 0x0DE1; +constexpr GLenum GL_TEXTURE0 = 0x84C0; +constexpr GLenum GL_TEXTURE1 = 0x84C1; +constexpr GLenum GL_TEXTURE_MIN_FILTER = 0x2801; +constexpr GLenum GL_TEXTURE_MAG_FILTER = 0x2800; +constexpr GLenum GL_TEXTURE_WRAP_S = 0x2802; +constexpr GLenum GL_TEXTURE_WRAP_T = 0x2803; +constexpr GLenum GL_NEAREST = 0x2600; +constexpr GLenum GL_CLAMP_TO_EDGE = 0x812F; +constexpr GLenum GL_FRAMEBUFFER = 0x8D40; +constexpr GLenum GL_READ_FRAMEBUFFER = 0x8CA8; +constexpr GLenum GL_DRAW_FRAMEBUFFER = 0x8CA9; +constexpr GLenum GL_COLOR_ATTACHMENT0 = 0x8CE0; +constexpr GLenum GL_COLOR_ATTACHMENT1 = 0x8CE1; +constexpr GLenum GL_DEPTH_STENCIL_ATTACHMENT = 0x821A; +constexpr GLenum GL_FRAMEBUFFER_COMPLETE = 0x8CD5; +constexpr GLenum GL_COLOR_BUFFER_BIT = 0x00004000; +constexpr GLenum GL_DEPTH_BUFFER_BIT = 0x00000100; +constexpr GLenum GL_PIXEL_PACK_BUFFER = 0x88EB; +constexpr GLenum GL_STREAM_READ = 0x88E1; +constexpr GLenum GL_FRAGMENT_SHADER = 0x8B30; +constexpr GLenum GL_VERTEX_SHADER = 0x8B31; +constexpr GLenum GL_COMPILE_STATUS = 0x8B81; +constexpr GLenum GL_LINK_STATUS = 0x8B82; +constexpr GLenum GL_INFO_LOG_LENGTH = 0x8B84; +constexpr GLenum GL_DEPTH_TEST = 0x0B71; +constexpr GLenum GL_CULL_FACE = 0x0B44; +constexpr GLenum GL_BLEND = 0x0BE2; +constexpr GLenum GL_SCISSOR_TEST = 0x0C11; +constexpr GLenum GL_PACK_ALIGNMENT = 0x0D05; +constexpr GLenum GL_DRAW_FRAMEBUFFER_BINDING = 0x8CA6; + +// ── Entry points ────────────────────────────────────────────────────────── +// Function pointers rather than declarations, filled by load(). `extern` so +// every TU shares one copy. +#define MUJOCO_XR_GL_FUNCTIONS(X) \ + X(void, Enable, (GLenum)) \ + X(void, Disable, (GLenum)) \ + X(GLenum, GetError, ()) \ + X(void, Viewport, (GLint, GLint, GLsizei, GLsizei)) \ + X(void, PixelStorei, (GLenum, GLint)) \ + X(void, GetIntegerv, (GLenum, GLint*)) \ + X(void, ReadPixels, (GLint, GLint, GLsizei, GLsizei, GLenum, GLenum, void*)) \ + X(void, DrawArrays, (GLenum, GLint, GLsizei)) \ + X(void, GenTextures, (GLsizei, GLuint*)) \ + X(void, DeleteTextures, (GLsizei, const GLuint*)) \ + X(void, BindTexture, (GLenum, GLuint)) \ + X(void, TexImage2D, (GLenum, GLint, GLint, GLsizei, GLsizei, GLint, GLenum, GLenum, const void*)) \ + X(void, TexParameteri, (GLenum, GLenum, GLint)) \ + X(void, ActiveTexture, (GLenum)) \ + X(void, GenFramebuffers, (GLsizei, GLuint*)) \ + X(void, DeleteFramebuffers, (GLsizei, const GLuint*)) \ + X(void, BindFramebuffer, (GLenum, GLuint)) \ + X(void, FramebufferTexture2D, (GLenum, GLenum, GLenum, GLuint, GLint)) \ + X(GLenum, CheckFramebufferStatus, (GLenum)) \ + X(void, GetFramebufferAttachmentParameteriv, (GLenum, GLenum, GLenum, GLint*)) \ + X(void, BlitFramebuffer, (GLint, GLint, GLint, GLint, GLint, GLint, GLint, GLint, GLbitfield, GLenum)) \ + X(void, DrawBuffers, (GLsizei, const GLenum*)) \ + X(void, ReadBuffer, (GLenum)) \ + X(void, GenBuffers, (GLsizei, GLuint*)) \ + X(void, DeleteBuffers, (GLsizei, const GLuint*)) \ + X(void, BindBuffer, (GLenum, GLuint)) \ + X(void, BufferData, (GLenum, GLsizeiptr, const void*, GLenum)) \ + X(void, GenVertexArrays, (GLsizei, GLuint*)) \ + X(void, DeleteVertexArrays, (GLsizei, const GLuint*)) \ + X(void, BindVertexArray, (GLuint)) \ + X(GLuint, CreateShader, (GLenum)) \ + X(void, ShaderSource, (GLuint, GLsizei, const GLchar* const*, const GLint*)) \ + X(void, CompileShader, (GLuint)) \ + X(void, GetShaderiv, (GLuint, GLenum, GLint*)) \ + X(void, GetShaderInfoLog, (GLuint, GLsizei, GLsizei*, GLchar*)) \ + X(void, DeleteShader, (GLuint)) \ + X(GLuint, CreateProgram, ()) \ + X(void, AttachShader, (GLuint, GLuint)) \ + X(void, LinkProgram, (GLuint)) \ + X(void, GetProgramiv, (GLuint, GLenum, GLint*)) \ + X(void, GetProgramInfoLog, (GLuint, GLsizei, GLsizei*, GLchar*)) \ + X(void, DeleteProgram, (GLuint)) \ + X(void, UseProgram, (GLuint)) \ + X(GLint, GetUniformLocation, (GLuint, const GLchar*)) \ + X(void, Uniform1i, (GLint, GLint)) + +#define MUJOCO_XR_GL_DECLARE(ret, name, args) extern ret(*name) args; +MUJOCO_XR_GL_FUNCTIONS(MUJOCO_XR_GL_DECLARE) +#undef MUJOCO_XR_GL_DECLARE + +// Resolves every entry point above against the CURRENT context. Idempotent, so +// callers need not track whether it has run. Throws std::runtime_error naming +// the first function that could not be resolved, which in practice means either +// no context is current or the context is too old. +void load(); + +// Throws std::runtime_error naming `what` if glGetError() is not GL_NO_ERROR. +// Drains the error queue either way, so one stale error cannot fail every later +// check. +void check(const char* what); + +} // namespace gl +} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/gl_readback.cpp b/examples/mujoco_xr/cpp/gl_readback.cpp new file mode 100644 index 000000000..93f66a9e4 --- /dev/null +++ b/examples/mujoco_xr/cpp/gl_readback.cpp @@ -0,0 +1,398 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "gl_readback.hpp" + +#include +#include +#include +#include +#include + +namespace mujoco_xr +{ + +using namespace gl; + +namespace +{ + +void check_cuda(cudaError_t err, const char* what) +{ + if (err != cudaSuccess) + { + throw std::runtime_error(std::string("mujoco_xr: ") + what + " failed: " + cudaGetErrorString(err)); + } +} + +// gl_VertexID -> one viewport-covering triangle, so there is no vertex buffer. +constexpr const char* kVertexSource = R"glsl( +#version 330 core +out vec2 vUv; +void main() +{ + vec2 p = vec2((gl_VertexID << 1) & 2, gl_VertexID & 2); + vUv = p; + gl_Position = vec4(p * 2.0 - 1.0, 0.0, 1.0); +} +)glsl"; + +// The two conversions the XR layer needs, in the one place they happen. +constexpr const char* kFragmentSource = R"glsl( +#version 330 core +uniform sampler2D uColor; +uniform sampler2D uDepth; +in vec2 vUv; +layout(location = 0) out vec4 oColor; +layout(location = 1) out float oDepth; +void main() +{ + vec2 uv = vec2(vUv.x, 1.0 - vUv.y); // GL bottom-up -> XR top-down + oColor = texture(uColor, uv); + oDepth = 1.0 - texture(uDepth, uv).r; // mjr_render reverse Z -> near 0, far 1 +} +)glsl"; + +GLuint compile(GLenum stage, const char* source) +{ + const GLuint shader = CreateShader(stage); + ShaderSource(shader, 1, &source, nullptr); + CompileShader(shader); + GLint ok = GL_FALSE; + GetShaderiv(shader, GL_COMPILE_STATUS, &ok); + if (ok != static_cast(GL_TRUE)) + { + GLint len = 0; + GetShaderiv(shader, GL_INFO_LOG_LENGTH, &len); + std::string log(static_cast(len > 0 ? len : 1), '\0'); + GetShaderInfoLog(shader, len, nullptr, log.data()); + DeleteShader(shader); + throw std::runtime_error("mujoco_xr: readback shader failed to compile: " + log); + } + return shader; +} + +GLuint make_texture(GLenum internal_format, GLenum format, GLenum type, uint32_t width, uint32_t height) +{ + GLuint tex = 0; + GenTextures(1, &tex); + BindTexture(GL_TEXTURE_2D, tex); + TexImage2D(GL_TEXTURE_2D, 0, static_cast(internal_format), static_cast(width), + static_cast(height), 0, format, type, nullptr); + TexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + TexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + TexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + TexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + BindTexture(GL_TEXTURE_2D, 0); + return tex; +} + +// A pack buffer and its CUDA registration. ReadOnly: submit() only reads it. +void make_pbo(size_t size_bytes, GLuint* out_pbo, void** out_resource) +{ + GenBuffers(1, out_pbo); + BindBuffer(GL_PIXEL_PACK_BUFFER, *out_pbo); + BufferData(GL_PIXEL_PACK_BUFFER, static_cast(size_bytes), nullptr, GL_STREAM_READ); + BindBuffer(GL_PIXEL_PACK_BUFFER, 0); + check("pixel pack buffer allocation"); + + cudaGraphicsResource_t res = nullptr; + check_cuda(cudaGraphicsGLRegisterBuffer(&res, *out_pbo, cudaGraphicsRegisterFlagsReadOnly), + "cudaGraphicsGLRegisterBuffer"); + *out_resource = res; +} + +// `src_fbo`'s depth format, as the triple a matching texture needs. +struct DepthFormat +{ + GLenum internal_format; + GLenum format; + GLenum type; +}; + +// Restores the draw-framebuffer binding this object was constructed under. +struct ScopedDrawFramebuffer +{ + GLint previous = 0; + + ScopedDrawFramebuffer() + { + GetIntegerv(GL_DRAW_FRAMEBUFFER_BINDING, &previous); + } + ~ScopedDrawFramebuffer() + { + BindFramebuffer(GL_FRAMEBUFFER, static_cast(previous)); + } + + ScopedDrawFramebuffer(const ScopedDrawFramebuffer&) = delete; + ScopedDrawFramebuffer& operator=(const ScopedDrawFramebuffer&) = delete; +}; + +DepthFormat depth_format_of(GLuint src_fbo) +{ + BindFramebuffer(GL_READ_FRAMEBUFFER, src_fbo); + GLint component_type = 0; + GetFramebufferAttachmentParameteriv( + GL_READ_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_FRAMEBUFFER_ATTACHMENT_COMPONENT_TYPE, &component_type); + BindFramebuffer(GL_READ_FRAMEBUFFER, 0); + check("querying the MuJoCo offscreen depth format"); + + if (static_cast(component_type) == GL_FLOAT) + { + return { GL_DEPTH32F_STENCIL8, GL_DEPTH_STENCIL, GL_FLOAT_32_UNSIGNED_INT_24_8_REV }; + } + return { GL_DEPTH24_STENCIL8, GL_DEPTH_STENCIL, GL_UNSIGNED_INT_24_8 }; +} + +} // namespace + +Readback::~Readback() +{ + destroy(); +} + +void Readback::build_program() +{ + const GLuint vs = compile(GL_VERTEX_SHADER, kVertexSource); + const GLuint fs = compile(GL_FRAGMENT_SHADER, kFragmentSource); + program_ = CreateProgram(); + AttachShader(program_, vs); + AttachShader(program_, fs); + LinkProgram(program_); + DeleteShader(vs); + DeleteShader(fs); + + GLint ok = GL_FALSE; + GetProgramiv(program_, GL_LINK_STATUS, &ok); + if (ok != static_cast(GL_TRUE)) + { + GLint len = 0; + GetProgramiv(program_, GL_INFO_LOG_LENGTH, &len); + std::string log(static_cast(len > 0 ? len : 1), '\0'); + GetProgramInfoLog(program_, len, nullptr, log.data()); + throw std::runtime_error("mujoco_xr: readback program failed to link: " + log); + } + + UseProgram(program_); + Uniform1i(GetUniformLocation(program_, "uColor"), 0); + Uniform1i(GetUniformLocation(program_, "uDepth"), 1); + UseProgram(0); + check("readback program setup"); +} + +void Readback::create(uint32_t width, uint32_t height, uint32_t view_count, GLuint src_fbo) +{ + if (width == 0 || height == 0 || view_count == 0) + { + throw std::invalid_argument("mujoco_xr: readback needs a non-empty size and at least one view"); + } + load(); + width_ = width; + height_ = height; + + const ScopedDrawFramebuffer restore_binding; + + build_program(); + GenVertexArrays(1, &vao_); + + const DepthFormat depth = depth_format_of(src_fbo); + views_.resize(view_count); + for (View& v : views_) + { + v.blit_color = make_texture(GL_RGBA8, GL_RGBA, GL_UNSIGNED_BYTE, width, height); + v.blit_depth = make_texture(depth.internal_format, depth.format, depth.type, width, height); + GenFramebuffers(1, &v.blit_fbo); + BindFramebuffer(GL_FRAMEBUFFER, v.blit_fbo); + FramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, v.blit_color, 0); + FramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_STENCIL_ATTACHMENT, GL_TEXTURE_2D, v.blit_depth, 0); + if (CheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE) + { + throw std::runtime_error("mujoco_xr: the readback blit framebuffer is incomplete"); + } + + v.out_color = make_texture(GL_RGBA8, GL_RGBA, GL_UNSIGNED_BYTE, width, height); + v.out_depth = make_texture(GL_R32F, GL_RED, GL_FLOAT, width, height); + GenFramebuffers(1, &v.out_fbo); + BindFramebuffer(GL_FRAMEBUFFER, v.out_fbo); + FramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, v.out_color, 0); + FramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT1, GL_TEXTURE_2D, v.out_depth, 0); + if (CheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE) + { + throw std::runtime_error("mujoco_xr: the readback output framebuffer is incomplete"); + } + + const size_t pixels = static_cast(width) * height; + make_pbo(pixels * 4, &v.color_pbo, &v.color_resource); + make_pbo(pixels * sizeof(float), &v.depth_pbo, &v.depth_resource); + } + check("readback resource creation"); +} + +void Readback::unmap(View& v, bool throw_on_error) +{ + if (!v.mapped) + { + return; + } + cudaGraphicsResource_t resources[2] = { static_cast(v.color_resource), + static_cast(v.depth_resource) }; + const cudaError_t err = cudaGraphicsUnmapResources(2, resources, nullptr); + v.mapped = false; + v.color_device_ptr = nullptr; + v.depth_device_ptr = nullptr; + if (throw_on_error) + { + check_cuda(err, "cudaGraphicsUnmapResources"); + } +} + +void Readback::capture(uint32_t view, GLuint src_fbo) +{ + if (view >= views_.size()) + { + throw std::out_of_range("mujoco_xr: readback view index out of range"); + } + View& v = views_[view]; + // glReadPixels below is graphics access, illegal while CUDA holds them. + unmap(v, /*throw_on_error=*/true); + + // mjr_render draws into whatever is bound when it is called. Leaving our + // own framebuffer bound sends the NEXT frame to it, and the symptom is an + // empty image with no GL error anywhere. + const ScopedDrawFramebuffer restore_binding; + + const GLint w = static_cast(width_); + const GLint h = static_cast(height_); + + BindFramebuffer(GL_READ_FRAMEBUFFER, src_fbo); + BindFramebuffer(GL_DRAW_FRAMEBUFFER, v.blit_fbo); + BlitFramebuffer(0, 0, w, h, 0, 0, w, h, GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT, GL_NEAREST); + + // MuJoCo leaves these on, and none may touch a pass that only moves pixels. + // Not restored: mjr_render's initGL3 sets all four again every frame. + Disable(GL_DEPTH_TEST); + Disable(GL_CULL_FACE); + Disable(GL_BLEND); + Disable(GL_SCISSOR_TEST); + + BindFramebuffer(GL_FRAMEBUFFER, v.out_fbo); + const GLenum targets[2] = { GL_COLOR_ATTACHMENT0, GL_COLOR_ATTACHMENT1 }; + DrawBuffers(2, targets); + Viewport(0, 0, w, h); + UseProgram(program_); + ActiveTexture(GL_TEXTURE0); + BindTexture(GL_TEXTURE_2D, v.blit_color); + ActiveTexture(GL_TEXTURE1); + BindTexture(GL_TEXTURE_2D, v.blit_depth); + BindVertexArray(vao_); + DrawArrays(GL_TRIANGLES, 0, 3); + BindVertexArray(0); + UseProgram(0); + + // Tight rows: VizBuffer's pitch is width * bpp, and the 4-byte default + // alignment would pad an odd-width RGBA8 row. + PixelStorei(GL_PACK_ALIGNMENT, 1); + + ReadBuffer(GL_COLOR_ATTACHMENT0); + BindBuffer(GL_PIXEL_PACK_BUFFER, v.color_pbo); + ReadPixels(0, 0, w, h, GL_RGBA, GL_UNSIGNED_BYTE, nullptr); + + ReadBuffer(GL_COLOR_ATTACHMENT1); + BindBuffer(GL_PIXEL_PACK_BUFFER, v.depth_pbo); + ReadPixels(0, 0, w, h, GL_RED, GL_FLOAT, nullptr); + + BindBuffer(GL_PIXEL_PACK_BUFFER, 0); + check("readback capture"); +} + +void Readback::map() +{ + for (View& v : views_) + { + if (v.mapped) + { + continue; + } + // Orders itself after this thread's GL work, so no glFinish is needed. + cudaGraphicsResource_t resources[2] = { static_cast(v.color_resource), + static_cast(v.depth_resource) }; + check_cuda(cudaGraphicsMapResources(2, resources, nullptr), "cudaGraphicsMapResources"); + v.mapped = true; + + size_t size = 0; + check_cuda(cudaGraphicsResourceGetMappedPointer(&v.color_device_ptr, &size, resources[0]), + "cudaGraphicsResourceGetMappedPointer(color)"); + check_cuda(cudaGraphicsResourceGetMappedPointer(&v.depth_device_ptr, &size, resources[1]), + "cudaGraphicsResourceGetMappedPointer(depth)"); + } +} + +const Readback::View& Readback::at(uint32_t view) const +{ + if (view >= views_.size()) + { + throw std::out_of_range("mujoco_xr: readback view index out of range"); + } + const View& v = views_[view]; + if (!v.mapped) + { + throw std::runtime_error("mujoco_xr: no rendered frame for this view yet -- call render() first"); + } + return v; +} + +void* Readback::color_ptr(uint32_t view) const +{ + return at(view).color_device_ptr; +} + +void* Readback::depth_ptr(uint32_t view) const +{ + return at(view).depth_device_ptr; +} + +void Readback::destroy() +{ + for (View& v : views_) + { + unmap(v, /*throw_on_error=*/false); + if (v.color_resource != nullptr) + { + (void)cudaGraphicsUnregisterResource(static_cast(v.color_resource)); + } + if (v.depth_resource != nullptr) + { + (void)cudaGraphicsUnregisterResource(static_cast(v.depth_resource)); + } + // GL deletes only if a context is still current; the caller owns that + // ordering (Renderer.close() before mujoco.GLContext.free()). + if (BindFramebuffer != nullptr) + { + const GLuint buffers[2] = { v.color_pbo, v.depth_pbo }; + DeleteBuffers(2, buffers); + const GLuint framebuffers[2] = { v.blit_fbo, v.out_fbo }; + DeleteFramebuffers(2, framebuffers); + const GLuint textures[4] = { v.blit_color, v.blit_depth, v.out_color, v.out_depth }; + DeleteTextures(4, textures); + } + } + views_.clear(); + + if (BindFramebuffer != nullptr) + { + if (vao_ != 0) + { + DeleteVertexArrays(1, &vao_); + vao_ = 0; + } + if (program_ != 0) + { + DeleteProgram(program_); + program_ = 0; + } + } + width_ = 0; + height_ = 0; +} + +} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/gl_readback.hpp b/examples/mujoco_xr/cpp/gl_readback.hpp new file mode 100644 index 000000000..a12aaeab7 --- /dev/null +++ b/examples/mujoco_xr/cpp/gl_readback.hpp @@ -0,0 +1,89 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +// MuJoCo's offscreen framebuffer -> the CUDA-linear buffers +// viz::ProjectionLayer.submit() consumes, with no host round trip. Stage by +// stage in README.md. +// +// Use cudaGraphicsGLRegisterBUFFER, never RegisterImage: RegisterImage takes no +// depth format and no multisampled renderbuffer, and mjrContext.offDepthStencil +// is both. + +#include "gl.hpp" + +#include +#include + +namespace mujoco_xr +{ + +class Readback +{ +public: + Readback() = default; + ~Readback(); + + Readback(const Readback&) = delete; + Readback& operator=(const Readback&) = delete; + + // `src_fbo` is mjrContext.offFBO, needed here and not only in capture(): + // glBlitFramebuffer rejects a depth blit between differing formats, and + // MuJoCo picks DEPTH32F_STENCIL8 or DEPTH24_STENCIL8 depending on + // ARB_depth_buffer_float, so the blit target is matched to what it chose. + void create(uint32_t width, uint32_t height, uint32_t view_count, gl::GLuint src_fbo); + void destroy(); + + // Steps 1-3 for one view, reading from `src_fbo` (mjrContext.offFBO). + // Unmaps that view's buffers first, so a pointer handed out by ptr() is + // valid only until the next capture() of the same view. + void capture(uint32_t view, gl::GLuint src_fbo); + + // Step 4. Call once after the last capture() of the frame. + void map(); + + void* color_ptr(uint32_t view) const; + void* depth_ptr(uint32_t view) const; + + uint32_t width() const + { + return width_; + } + uint32_t height() const + { + return height_; + } + +private: + struct View + { + gl::GLuint blit_fbo = 0; + gl::GLuint blit_color = 0; // RGBA8 texture + gl::GLuint blit_depth = 0; // DEPTH24_STENCIL8 texture + gl::GLuint out_fbo = 0; + gl::GLuint out_color = 0; // RGBA8 texture + gl::GLuint out_depth = 0; // R32F texture + gl::GLuint color_pbo = 0; + gl::GLuint depth_pbo = 0; + void* color_resource = nullptr; // cudaGraphicsResource_t + void* depth_resource = nullptr; + void* color_device_ptr = nullptr; + void* depth_device_ptr = nullptr; + bool mapped = false; + }; + + void build_program(); + // throw_on_error false on the teardown path, which is reached from a + // destructor with the GL context possibly already gone. + void unmap(View& v, bool throw_on_error); + const View& at(uint32_t view) const; + + uint32_t width_ = 0; + uint32_t height_ = 0; + gl::GLuint program_ = 0; + gl::GLuint vao_ = 0; + std::vector views_; +}; + +} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/glcamera.hpp b/examples/mujoco_xr/cpp/glcamera.hpp new file mode 100644 index 000000000..0a3be2351 --- /dev/null +++ b/examples/mujoco_xr/cpp/glcamera.hpp @@ -0,0 +1,77 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +// One XR view's asymmetric fov -> the mjvGLCamera frustum fields MuJoCo builds +// its projection from. Free functions, no GPU and no MuJoCo state, so +// tests/test_projection.py can pin the convention headless. +// +// `frustum_width` is a HALF-width, and at 0 mjr_render derives the horizontal +// extent from the viewport aspect instead (render_gl3.c setView), which renders +// something plausible from a fov carrying nothing. Always set it; reject a fov +// that would leave it 0. mjvisualize.h calls the field "not used for +// rendering", which is wrong as of 3.11.0. + +#include +#include +#include + +namespace mujoco_xr +{ + +// MuJoCo's own spelling and units: extents on the near plane, half_width +// symmetric about center. +struct Frustum +{ + float center = 0.0f; + float half_width = 0.0f; + float bottom = 0.0f; + float top = 0.0f; + float near_z = 0.0f; + float far_z = 0.0f; +}; + +// fov_lrud is (left, right, up, down) radians -- viz::Fov's and XrFovf's field +// order, with left and down normally negative. No y flip here: OpenGL clip +// space is y-up like the fov, and the flip happens once, on readback. +inline Frustum frustum_from_fov(const std::array& fov_lrud, float near_z, float far_z) +{ + if (!(near_z > 0.0f) || !(far_z > near_z)) + { + throw std::invalid_argument("mujoco_xr: need 0 < near_z < far_z"); + } + const float left = near_z * std::tan(fov_lrud[0]); + const float right = near_z * std::tan(fov_lrud[1]); + const float top = near_z * std::tan(fov_lrud[2]); + const float bottom = near_z * std::tan(fov_lrud[3]); + + Frustum f; + f.center = 0.5f * (right + left); + f.half_width = 0.5f * (right - left); + f.bottom = bottom; + f.top = top; + f.near_z = near_z; + f.far_z = far_z; + + // A default-constructed viz::Fov is all zeros, and a zero half_width is + // exactly what turns the aspect-ratio fallback on. Refuse it. + if (!(f.half_width > 0.0f) || !(f.top > f.bottom)) + { + throw std::invalid_argument( + "mujoco_xr: degenerate fov -- angle_right must exceed angle_left and angle_up must exceed " + "angle_down. An all-zero fov means FrameInfo.views was never filled."); + } + return f; +} + +// A view-space distance -> the depth handed to ProjectionLayer.submit(): +// standard Z, near -> 0, far -> 1. NOT what MuJoCo writes -- mjr_render is +// reverse Z (glClipControl ZERO_TO_ONE, GL_GEQUAL, glClearDepth(0)), so the two +// differ by exactly `1 - d`, the subtraction in gl_readback.cpp's shader. +inline float submitted_depth(float distance, float near_z, float far_z) +{ + return far_z * (distance - near_z) / (distance * (far_z - near_z)); +} + +} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/mesh_buffers.cpp b/examples/mujoco_xr/cpp/mesh_buffers.cpp deleted file mode 100644 index 031f4125c..000000000 --- a/examples/mujoco_xr/cpp/mesh_buffers.cpp +++ /dev/null @@ -1,105 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#include "mesh_buffers.hpp" - -#include -#include -#include -#include - -namespace mujoco_xr -{ - -void build_mesh_buffers(const mjModel* m, MeshBuffers* out) -{ - std::vector& verts = out->verts; - std::vector& indices = out->indices; - verts.clear(); - indices.clear(); - - // Meshes: one vertex per FACE CORNER, carrying a normal computed here. - // See the header for why mjModel's own normals cannot be used. - out->meshes.assign(static_cast(m->nmesh), MeshRange()); - std::vector> face_normal; - std::vector face_area; - std::vector> vertex_faces; - for (int mesh = 0; mesh < m->nmesh; ++mesh) - { - MeshRange& range = out->meshes[static_cast(mesh)]; - range.base_vertex = static_cast(verts.size()); - range.first_index = static_cast(indices.size()); - const float* mverts = m->mesh_vert + 3 * m->mesh_vertadr[mesh]; - const int* mfaces = m->mesh_face + 3 * m->mesh_faceadr[mesh]; - const int facenum = m->mesh_facenum[mesh]; - - // Pass 1: the geometric normal and area of every face, and which faces - // touch each vertex. - face_normal.assign(static_cast(facenum), { 0.0f, 0.0f, 0.0f }); - face_area.assign(static_cast(facenum), 0.0f); - vertex_faces.assign(static_cast(m->mesh_vertnum[mesh]), {}); - for (int f = 0; f < facenum; ++f) - { - const int* face = mfaces + 3 * f; - const float* p[3] = { mverts + 3 * face[0], mverts + 3 * face[1], mverts + 3 * face[2] }; - const float e1[3] = { p[1][0] - p[0][0], p[1][1] - p[0][1], p[1][2] - p[0][2] }; - const float e2[3] = { p[2][0] - p[0][0], p[2][1] - p[0][1], p[2][2] - p[0][2] }; - std::array n = { e1[1] * e2[2] - e1[2] * e2[1], e1[2] * e2[0] - e1[0] * e2[2], - e1[0] * e2[1] - e1[1] * e2[0] }; - const float len = std::sqrt(n[0] * n[0] + n[1] * n[1] + n[2] * n[2]); - face_area[static_cast(f)] = 0.5f * len; - if (len > 0.0f) - { - n[0] /= len; - n[1] /= len; - n[2] /= len; - } - face_normal[static_cast(f)] = n; - for (int k = 0; k < 3; ++k) - { - vertex_faces[static_cast(face[k])].push_back(f); - } - } - - // Pass 2: one vertex per corner, its normal area-averaged over the - // faces round that vertex that lie WITHIN the crease angle of this - // one. Curved surfaces stay smooth; an edge sharper than the threshold - // keeps both of its faces flat. - uint32_t local_count = 0; - for (int f = 0; f < facenum; ++f) - { - const int* face = mfaces + 3 * f; - const std::array& fn = face_normal[static_cast(f)]; - for (int k = 0; k < 3; ++k) - { - float acc[3] = { 0.0f, 0.0f, 0.0f }; - for (int g : vertex_faces[static_cast(face[k])]) - { - const std::array& gn = face_normal[static_cast(g)]; - const float cosine = fn[0] * gn[0] + fn[1] * gn[1] + fn[2] * gn[2]; - if (cosine >= kCreaseCos) - { - const float w = face_area[static_cast(g)]; - acc[0] += gn[0] * w; - acc[1] += gn[1] * w; - acc[2] += gn[2] * w; - } - } - const float len = std::sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]); - Vertex v; - std::memcpy(v.pos, mverts + 3 * face[k], sizeof(v.pos)); - for (int c = 0; c < 3; ++c) - { - // A zero sum needs the face's own normal: it means every - // contribution cancelled, not that the surface has none. - v.normal[c] = len > 0.0f ? acc[c] / len : fn[static_cast(c)]; - } - verts.push_back(v); - indices.push_back(local_count++); - } - } - range.index_count = static_cast(indices.size()) - range.first_index; - } -} - -} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/mesh_buffers.hpp b/examples/mujoco_xr/cpp/mesh_buffers.hpp deleted file mode 100644 index b2d79b400..000000000 --- a/examples/mujoco_xr/cpp/mesh_buffers.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#pragma once - -// The welded vertex / index buffers the renderer draws from, built once from -// mjModel. -// -// Normals are computed here, not taken from mjModel: MuJoCo welds an STL's -// vertices and stores one averaged normal per welded vertex (mesh_normalnum == -// mesh_vertnum, mesh_facenormal == mesh_face), so on a CAD part every crease -// gets a normal smeared across it. Measured on the shipped scene under -// test_ghost.py's own predicate (dot(corner normal, own face normal) <= 0): -// 2138 of Wrist_Roll_SO101's 18474 face corners point away from their own -// face, and 9489 of the STS3215's 57240. Lit one-sided those corners drop to -// scene.frag's 0.35 ambient floor and the part renders as shattered facets, a -// shading bug that looks like a broken mesh. So each face gets its own three -// vertices, and each corner an area-weighted average over the faces round it -// that lie within kCreaseCos. -// -// Indices stay mesh-local with a per-mesh base_vertex, which is what -// vkCmdDrawIndexed's vertexOffset consumes directly; absolute indices would -// need every consumer to undo the folding. -// -// No kNearZ / kFarZ here. The Python app owns the clip planes as one named pair -// reaching VizSessionConfig, the projection and the submitted depth; a second -// definition in C++ drifts and makes compositor reprojection wrong on hardware -// nobody can test here. - -#include - -#include -#include - -namespace mujoco_xr -{ - -// The one directional light, in MuJoCo world space, normalized on upload. The -// half-lambert `ambient` term stays a `const float` in shaders/scene.frag: no -// C++ reads it, so hoisting it would cost a uniform to share one float. -inline constexpr float kLightDirWorld[3] = { 0.35f, -0.25f, -1.0f }; - -// Faces meeting at less than this angle are smoothed together; anything -// sharper stays a crease. 35 degrees keeps the SO-101 handle's curve smooth -// and its bolt holes crisp. -inline constexpr float kCreaseCos = 0.819f; // cos(35 deg) - -struct Vertex -{ - float pos[3]; - float normal[3]; -}; - -struct MeshRange -{ - int32_t base_vertex = 0; - uint32_t first_index = 0; - uint32_t index_count = 0; -}; - -struct MeshBuffers -{ - std::vector verts; - std::vector indices; // mesh-local: add base_vertex to deref - std::vector meshes; // indexed by meshid -}; - -// Welds every mesh in `m` into one vertex / index pair. -void build_mesh_buffers(const mjModel* m, MeshBuffers* out); - -} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp b/examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp index ff2bf7b2b..3f9ec6626 100644 --- a/examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp +++ b/examples/mujoco_xr/cpp/mujoco_xr_bindings.cpp @@ -3,18 +3,13 @@ // // pybind11 entry point for `mujoco_xr._mujoco_xr`. // -// Nothing viz-typed crosses this boundary: viz::Pose3D / viz::Fov / ViewInfo -// are registered in the `_viz` module and are not castable here, because this -// module links no viz target. Poses and fovs cross as plain float arrays, -// decomposed on the Python side. -// -// Likewise nothing MuJoCo-typed crosses it: Python owns mjModel / mjData / -// mj_step and passes their addresses as integers; C++ owns mjvScene / -// mjvOption / mjvCamera and calls mjv_updateScene. +// Nothing typed crosses this boundary in either direction. viz::Pose3D / Fov +// are registered in `_viz` and not castable here (this module links no viz +// target), so poses and fovs cross as flat float arrays; mjModel / mjData cross +// as integer addresses, Python owning them and C++ owning mjvScene / mjrContext. #include "frames.hpp" -#include "mesh_buffers.hpp" -#include "render_target.hpp" +#include "glcamera.hpp" #include "scene_renderer.hpp" #include @@ -25,7 +20,6 @@ #include #include #include -#include #include namespace mujoco_xr @@ -35,14 +29,9 @@ namespace namespace py = pybind11; -// A view onto one of the renderer's CUDA-visible staging buffers, shaped for -// viz's `cuda_array_to_viz_buffer` helper, which wants: -// kRGBA8 -> typestr "|u1", shape (H, W, 4) -// kD32F -> typestr " "|u1" (H, W, 4), kD32F -> "(dev, cfg, reinterpret_cast(model_address)); + renderer_ = std::make_unique(cfg, reinterpret_cast(model_address)); } SceneRenderer& get() @@ -113,6 +92,18 @@ class PyRenderer std::unique_ptr renderer_; }; +CudaImageView image_view(SceneRenderer& r, int view, bool is_depth) +{ + if (view < 0 || static_cast(view) >= r.view_count()) + { + throw std::out_of_range("mujoco_xr: view index out of range"); + } + const Readback& rb = r.readback(); + const auto index = static_cast(view); + void* ptr = is_depth ? rb.depth_ptr(index) : rb.color_ptr(index); + return CudaImageView{ reinterpret_cast(ptr), rb.width(), rb.height(), is_depth }; +} + } // namespace } // namespace mujoco_xr @@ -121,100 +112,76 @@ PYBIND11_MODULE(_mujoco_xr, m) namespace py = pybind11; using namespace pybind11::literals; - m.doc() = "MuJoCo -> Vulkan renderer for Isaac Teleop's Televiz ProjectionLayer."; + m.doc() = "MuJoCo's OpenGL renderer, read back into CUDA for Isaac Teleop's Televiz ProjectionLayer."; m.def( "mujoco_version", []() { return std::string(mj_versionString()); }, "The libmujoco this extension is linked against, as reported at runtime. Compare with " - "mujoco.mj_versionString() -- they MUST be equal, and they are only equal because there is " - "exactly one libmujoco loaded in the process."); - - m.def( - "mesh_triangles", - [](uintptr_t model_address, int meshid) - { - const mjModel* model = reinterpret_cast(model_address); - mujoco_xr::MeshBuffers mb; - mujoco_xr::build_mesh_buffers(model, &mb); - if (meshid < 0 || meshid >= static_cast(mb.meshes.size())) - { - throw std::out_of_range("mujoco_xr: meshid out of range"); - } - const mujoco_xr::MeshRange& r = mb.meshes[static_cast(meshid)]; - std::vector pos, normal; - pos.reserve(r.index_count * 3); - normal.reserve(r.index_count * 3); - const size_t base = static_cast(r.base_vertex); - for (uint32_t i = 0; i < r.index_count; ++i) - { - const mujoco_xr::Vertex& v = mb.verts[base + mb.indices[r.first_index + i]]; - pos.insert(pos.end(), { v.pos[0], v.pos[1], v.pos[2] }); - normal.insert(normal.end(), { v.normal[0], v.normal[1], v.normal[2] }); - } - return std::make_pair(pos, normal); - }, - "model_address"_a, "meshid"_a, - "The vertices the RENDERER draws for one mesh: (positions, normals), both 3 floats per corner in " - "draw order, so a test can check the normals against the geometry they came from. mjModel's own " - "normals are not these -- see cpp/mesh_buffers.hpp."); + "mujoco.mj_versionString() -- they MUST be equal, and they are only equal because there is exactly one " + "libmujoco loaded in the process."); // ── Frames ──────────────────────────────────────────────────────────── - // Exposed rather than reimplemented in Python: kQuatMjFromXr and - // kTransMjFromXr have exactly one definition (frames.hpp) and the Python - // app, the renderer and tests/test_frames.py all read that one. + // Exposed rather than reimplemented, so frames.hpp stays the one definition. m.def( "mj_from_xr_pos", [](std::array p_xr) { return mujoco_xr::mj_from_xr_pos(p_xr); }, "p_xr"_a, - "XR reference-space point (metres, Y-up) -> MuJoCo world point (Z-up). Applies both the " - "handedness rotation and the workspace translation."); + "XR reference-space point (metres, Y-up) -> MuJoCo world point (Z-up). Applies both the handedness " + "rotation and the workspace translation."); m.def( "mj_from_xr_quat", [](std::array q_xyzw) { return mujoco_xr::mj_from_xr_quat(q_xyzw); }, "q_xyzw"_a, "XR orientation as xyzw (the order OpenXR and Teleop's GRIP_ORIENTATION use) -> MuJoCo world " "orientation as wxyz. The ONLY quaternion crossing in the app."); - // Attributes rather than m.def getters, and SCREAMING_CASE: a getter would - // export as a snake_case attribute, putting `quat_mj_from_xr` beside - // `mj_from_xr_quat` with only word order telling a constant from a - // transform. Immutable tuples; the values and their prose live in - // frames.hpp. + // SCREAMING_CASE attributes, not getters: a snake_case getter would put + // `quat_mj_from_xr` beside `mj_from_xr_quat`, with only word order telling a + // constant from a transform. m.attr("QUAT_MJ_FROM_XR") = py::tuple(py::cast(mujoco_xr::kQuatMjFromXr)); m.attr("TRANS_MJ_FROM_XR") = py::tuple(py::cast(mujoco_xr::kTransMjFromXr)); // ── Projection ──────────────────────────────────────────────────────── m.def( - "projection_from_fov", + "frustum_from_fov", [](std::array fov_lrud, float near_z, float far_z) { - const auto p = mujoco_xr::projection_from_fov(fov_lrud, near_z, far_z); - return std::vector(p.begin(), p.end()); + const mujoco_xr::Frustum f = mujoco_xr::frustum_from_fov(fov_lrud, near_z, far_z); + return std::vector{ f.center, f.half_width, f.bottom, f.top, f.near_z, f.far_z }; }, "fov_lrud"_a, "near_z"_a, "far_z"_a, - "Column-major 4x4 Vulkan-convention projection from (angle_left, angle_right, angle_up, angle_down) " - "in radians. Same code path the renderer uses; exposed so the clip convention is testable without a " - "GPU. Raises ValueError on a degenerate (all-zero) fov."); + "The mjvGLCamera frustum fields for one asymmetric fov (angle_left, angle_right, angle_up, angle_down) " + "in radians, as (center, half_width, bottom, top, near, far). Same code path the renderer uses; exposed " + "so the convention is testable without a GPU. Raises ValueError on a degenerate fov or a bad near/far."); + + m.def( + "submitted_depth", + [](float distance, float near_z, float far_z) { return mujoco_xr::submitted_depth(distance, near_z, far_z); }, + "distance"_a, "near_z"_a, "far_z"_a, + "What a view-space distance ahead of the eye becomes in the depth buffer handed to " + "ProjectionLayer.submit(): standard Z, near -> 0, far -> 1. MuJoCo's renderer writes the reverse; " + "shaders/readback inverts it."); // ── Renderer ────────────────────────────────────────────────────────── py::class_(m, "CudaImageView", R"doc( -Non-owning CUDA view of one of the renderer's staging buffers. +Non-owning CUDA view of one of the renderer's pixel-pack buffers. Exposes ``__cuda_array_interface__``, which is all ``isaacteleop.viz.ProjectionLayer.submit()`` needs. Do NOT hold one past the frame it came from, and never past ``Renderer.close()``: the memory belongs to -the renderer. +the renderer and is unmapped on the next ``render()``. )doc") .def_property_readonly("__cuda_array_interface__", &mujoco_xr::CudaImageView::cuda_array_interface); py::class_(m, "Renderer", R"doc( -MuJoCo scene renderer writing into CUDA-visible colour + depth buffers. +MuJoCo's OpenGL renderer, read back into CUDA-visible colour + depth buffers. -Constructed from a live ``isaacteleop.viz.VizSession``'s raw handles -- it -BORROWS that Vulkan device and queue rather than creating its own, which is -what lets the exported memory be imported by the same CUDA context viz uses. +An OpenGL context must be current on this thread BEFORE construction, on the +same GPU viz chose (``mujoco.GLContext``; set ``MUJOCO_EGL_DEVICE_ID`` if the +machine has more than one card). The constructor checks this and raises rather +than render into another card's memory. Per frame, in this order and on ONE thread:: @@ -225,15 +192,11 @@ Per frame, in this order and on ONE thread:: renderer.render(poses, fovs) # poses/fovs from info.views layer.submit(renderer.color(0), renderer.depth(0), ...) session.end_frame() - -``render()`` blocks until the GPU work has retired, so the buffers are safe to -submit the moment it returns. )doc") - .def(py::init(), - "vk_physical_device"_a, "vk_device"_a, "vk_queue_family_index"_a, "width"_a, "height"_a, "view_count"_a, + .def(py::init(), "width"_a, "height"_a, "view_count"_a, "near_z"_a, "far_z"_a, "model_address"_a, - "All handles are plain integers: VizSession.vk_physical_device / .vk_device / " - ".vk_queue_family_index, and mujoco.MjModel._address.") + "`model_address` is mujoco.MjModel._address. No Vulkan handles: this renderer reaches viz through " + "CUDA alone, and finds viz's GPU as the process's current CUDA device.") .def( "update_scene", [](mujoco_xr::PyRenderer& self, uintptr_t model_address, uintptr_t data_address) @@ -248,60 +211,39 @@ submit the moment it returns. "render", [](mujoco_xr::PyRenderer& self, std::vector poses_xyz_qwxyz, std::vector fovs_lrud) { - // Releasing the GIL keeps a long GPU wait from blocking the - // interpreter, but it also drops the only mechanical - // serialisation against a second thread calling into viz on the - // same borrowed VkQueue. The single-threaded contract in - // scene_renderer.hpp is now the only thing holding: do not - // multi-thread the frame loop without real queue - // synchronisation. - py::gil_scoped_release release; + // No gil_scoped_release: this all runs on the GL context bound + // to THIS thread, and releasing the GIL would let another + // thread issue GL on a context it does not hold. self.get().render(poses_xyz_qwxyz, fovs_lrud); }, "poses_xyz_qwxyz"_a, "fovs_lrud"_a, "Render every view. `poses_xyz_qwxyz` is view_count*7 floats (x, y, z, qw, qx, qy, qz) and " - "`fovs_lrud` is view_count*4 (angle_left, angle_right, angle_up, angle_down) -- flatten them " - "from FrameInfo.views. Blocks until the GPU work retires.") + "`fovs_lrud` is view_count*4 (angle_left, angle_right, angle_up, angle_down) -- flatten them from " + "FrameInfo.views.") .def( - "projection", - [](mujoco_xr::PyRenderer& self, int view) - { - const auto& p = self.get().projection(view); - return std::vector(p.begin(), p.end()); - }, - "view"_a, - "The column-major 4x4 projection used for `view` on the last render(), so the caller can assert " - "the clip convention per frame.") + "frustum", [](mujoco_xr::PyRenderer& self, int view) { return self.get().frustum(view); }, "view"_a, + "The mjvGLCamera frustum used for `view` on the last render(), as (center, half_width, bottom, top, " + "near, far), so the caller can assert the convention per frame.") .def( "color", [](mujoco_xr::PyRenderer& self, int view) - { - const auto& t = self.get().view_target(view); - return mujoco_xr::CudaImageView{ reinterpret_cast(t.color().cuda_ptr()), t.width(), - t.height(), /*is_depth=*/false }; - }, - // keep_alive<0, 1>: the returned CudaImageView is a bare device - // pointer into the Renderer's exported memory. Without this, a - // caller who writes `buf = renderer.color(0)` and drops its last - // reference to `renderer` gets a use-after-free at submit time, - // with no Python-level symptom pointing back here. + { return mujoco_xr::image_view(self.get(), view, /*is_depth=*/false); }, + // keep_alive<0, 1>: the view is a bare device pointer into the + // Renderer's buffers, so a caller who keeps `buf = renderer.color(0)` + // and drops `renderer` would use-after-free at submit time. py::keep_alive<0, 1>(), "view"_a, "RGBA8 colour for `view` as a CudaImageView. Valid until the next render().") .def( "depth", [](mujoco_xr::PyRenderer& self, int view) - { - const auto& t = self.get().view_target(view); - return mujoco_xr::CudaImageView{ reinterpret_cast(t.depth().cuda_ptr()), t.width(), - t.height(), /*is_depth=*/true }; - }, + { return mujoco_xr::image_view(self.get(), view, /*is_depth=*/true); }, py::keep_alive<0, 1>(), "view"_a, // see color() above - "D32_SFLOAT depth for `view` as a CudaImageView, standard Z: near -> 0.0, far -> 1.0. Valid until " - "the next render().") + "float32 depth for `view` as a CudaImageView, standard Z: near -> 0.0, far -> 1.0. Valid until the " + "next render().") .def_property_readonly("view_count", [](mujoco_xr::PyRenderer& self) { return self.get().view_count(); }) .def_property_readonly("ngeom", [](mujoco_xr::PyRenderer& self) { return self.get().ngeom(); }) .def_property_readonly("maxgeom", [](mujoco_xr::PyRenderer& self) { return self.get().maxgeom(); }) .def("close", &mujoco_xr::PyRenderer::close, - "Release the Vulkan and CUDA resources. Must happen BEFORE VizSession.destroy(), since the device " - "is borrowed from it."); + "Release the OpenGL and CUDA resources. Must happen while the GL context is still current, so " + "BEFORE mujoco.GLContext.free()."); } diff --git a/examples/mujoco_xr/cpp/render_target.cpp b/examples/mujoco_xr/cpp/render_target.cpp deleted file mode 100644 index e3cccba32..000000000 --- a/examples/mujoco_xr/cpp/render_target.cpp +++ /dev/null @@ -1,409 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#include "render_target.hpp" - -#include -#include -#include -#include - -// The CUDA RUNTIME API in a plain .cpp, not a .cu and not the driver API. -// The constraint that matters is the file extension: the root project is -// `project(IsaacTeleop ... LANGUAGES CXX)`, so a .cu would need -// enable_language(CUDA) plus an architecture list, and .cu/.cuh escape -// clang-format, REUSE and the copyright-year hook. cudart needs none of that -// -- src/viz/core/cpp/device_image.cpp does exactly this, in a .cpp, against -// cudaImportExternalMemory. Using the runtime API rather than the driver API -// also keeps us in the same primary context viz's cudart already selected, -// which is what makes these pointers legible to ProjectionLayer.submit(). - -namespace mujoco_xr -{ - -// Declared in render_target.hpp: scene_renderer.cpp uses both of these too. -void check_vk(VkResult result, const char* what) -{ - if (result != VK_SUCCESS) - { - throw std::runtime_error(std::string("mujoco_xr: ") + what + " failed: VkResult=" + std::to_string(result)); - } -} - -uint32_t find_memory_type(VkPhysicalDevice physical_device, uint32_t type_bits, VkMemoryPropertyFlags properties) -{ - VkPhysicalDeviceMemoryProperties mem_props; - vkGetPhysicalDeviceMemoryProperties(physical_device, &mem_props); - for (uint32_t i = 0; i < mem_props.memoryTypeCount; ++i) - { - if ((type_bits & (1u << i)) != 0 && (mem_props.memoryTypes[i].propertyFlags & properties) == properties) - { - return i; - } - } - throw std::runtime_error("mujoco_xr: no Vulkan memory type matching requested properties"); -} - -namespace -{ - -// CUDA is used only in this TU, so its check stays with internal linkage. -void check_cuda(cudaError_t result, const char* what) -{ - if (result != cudaSuccess) - { - throw std::runtime_error(std::string("mujoco_xr: ") + what + " failed: " + cudaGetErrorString(result)); - } -} - -constexpr VkFormat kColorFormat = VK_FORMAT_R8G8B8A8_UNORM; -constexpr VkFormat kDepthFormat = VK_FORMAT_D32_SFLOAT; - -void create_attachment(const BorrowedDevice& dev, - uint32_t width, - uint32_t height, - VkFormat format, - VkImageUsageFlags usage, - VkImageAspectFlags aspect, - VkImage* out_image, - VkDeviceMemory* out_memory, - VkImageView* out_view) -{ - VkImageCreateInfo info{}; - info.sType = VK_STRUCTURE_TYPE_IMAGE_CREATE_INFO; - info.imageType = VK_IMAGE_TYPE_2D; - info.format = format; - info.extent = { width, height, 1 }; - info.mipLevels = 1; - info.arrayLayers = 1; - info.samples = VK_SAMPLE_COUNT_1_BIT; - info.tiling = VK_IMAGE_TILING_OPTIMAL; - info.usage = usage; - info.sharingMode = VK_SHARING_MODE_EXCLUSIVE; - info.initialLayout = VK_IMAGE_LAYOUT_UNDEFINED; - check_vk(vkCreateImage(dev.device, &info, nullptr, out_image), "vkCreateImage(attachment)"); - - VkMemoryRequirements reqs; - vkGetImageMemoryRequirements(dev.device, *out_image, &reqs); - VkMemoryAllocateInfo alloc{}; - alloc.sType = VK_STRUCTURE_TYPE_MEMORY_ALLOCATE_INFO; - alloc.allocationSize = reqs.size; - alloc.memoryTypeIndex = - find_memory_type(dev.physical_device, reqs.memoryTypeBits, VK_MEMORY_PROPERTY_DEVICE_LOCAL_BIT); - check_vk(vkAllocateMemory(dev.device, &alloc, nullptr, out_memory), "vkAllocateMemory(attachment)"); - check_vk(vkBindImageMemory(dev.device, *out_image, *out_memory, 0), "vkBindImageMemory(attachment)"); - - VkImageViewCreateInfo view_info{}; - view_info.sType = VK_STRUCTURE_TYPE_IMAGE_VIEW_CREATE_INFO; - view_info.image = *out_image; - view_info.viewType = VK_IMAGE_VIEW_TYPE_2D; - view_info.format = format; - view_info.subresourceRange.aspectMask = aspect; - view_info.subresourceRange.levelCount = 1; - view_info.subresourceRange.layerCount = 1; - check_vk(vkCreateImageView(dev.device, &view_info, nullptr, out_view), "vkCreateImageView(attachment)"); -} - -} // namespace - -// ── ExportedBuffer ───────────────────────────────────────────────────────── - -ExportedBuffer::~ExportedBuffer() -{ - destroy(); -} - -void ExportedBuffer::create(const BorrowedDevice& dev, VkDeviceSize size_bytes) -{ - device_ = dev.device; - size_bytes_ = size_bytes; - - VkExternalMemoryBufferCreateInfo ext_buffer_info{}; - ext_buffer_info.sType = VK_STRUCTURE_TYPE_EXTERNAL_MEMORY_BUFFER_CREATE_INFO; - ext_buffer_info.handleTypes = VK_EXTERNAL_MEMORY_HANDLE_TYPE_OPAQUE_FD_BIT; - - VkBufferCreateInfo info{}; - info.sType = VK_STRUCTURE_TYPE_BUFFER_CREATE_INFO; - info.pNext = &ext_buffer_info; - info.size = size_bytes; - info.usage = VK_BUFFER_USAGE_TRANSFER_DST_BIT; - info.sharingMode = VK_SHARING_MODE_EXCLUSIVE; - check_vk(vkCreateBuffer(device_, &info, nullptr, &buffer_), "vkCreateBuffer(exported)"); - - VkMemoryRequirements reqs; - vkGetBufferMemoryRequirements(device_, buffer_, &reqs); - - VkExportMemoryAllocateInfo export_info{}; - export_info.sType = VK_STRUCTURE_TYPE_EXPORT_MEMORY_ALLOCATE_INFO; - export_info.handleTypes = VK_EXTERNAL_MEMORY_HANDLE_TYPE_OPAQUE_FD_BIT; - - VkMemoryAllocateInfo alloc{}; - alloc.sType = VK_STRUCTURE_TYPE_MEMORY_ALLOCATE_INFO; - alloc.pNext = &export_info; - alloc.allocationSize = reqs.size; - alloc.memoryTypeIndex = - find_memory_type(dev.physical_device, reqs.memoryTypeBits, VK_MEMORY_PROPERTY_DEVICE_LOCAL_BIT); - check_vk(vkAllocateMemory(device_, &alloc, nullptr, &memory_), "vkAllocateMemory(exported)"); - check_vk(vkBindBufferMemory(device_, buffer_, memory_, 0), "vkBindBufferMemory(exported)"); - - auto get_memory_fd = reinterpret_cast(vkGetDeviceProcAddr(device_, "vkGetMemoryFdKHR")); - if (get_memory_fd == nullptr) - { - throw std::runtime_error( - "mujoco_xr: vkGetMemoryFdKHR is not available on the borrowed VkDevice. VizSession is supposed to enable " - "VK_KHR_external_memory_fd on every device it creates -- if this fires, the device did not come from viz."); - } - VkMemoryGetFdInfoKHR fd_info{}; - fd_info.sType = VK_STRUCTURE_TYPE_MEMORY_GET_FD_INFO_KHR; - fd_info.memory = memory_; - fd_info.handleType = VK_EXTERNAL_MEMORY_HANDLE_TYPE_OPAQUE_FD_BIT; - check_vk(get_memory_fd(device_, &fd_info, &memory_fd_), "vkGetMemoryFdKHR"); - - // No cudaSetDevice here, deliberately: viz's VkContext::init() already - // matched the current CUDA device to this Vulkan physical device by UUID - // on this thread, and the app is single-threaded by construction. - cudaExternalMemory_t ext_mem = nullptr; - cudaExternalMemoryHandleDesc ext_desc{}; - ext_desc.type = cudaExternalMemoryHandleTypeOpaqueFd; - ext_desc.handle.fd = memory_fd_; - ext_desc.size = reqs.size; - ext_desc.flags = 0; - check_cuda(cudaImportExternalMemory(&ext_mem, &ext_desc), "cudaImportExternalMemory"); - cuda_external_memory_ = ext_mem; - - // CUDA dup'd the fd on import; close ours so we do not leak one per buffer. - ::close(memory_fd_); - memory_fd_ = -1; - - cudaExternalMemoryBufferDesc buf_desc{}; - buf_desc.offset = 0; - buf_desc.size = size_bytes_; - buf_desc.flags = 0; - check_cuda(cudaExternalMemoryGetMappedBuffer(&cuda_ptr_, ext_mem, &buf_desc), "cudaExternalMemoryGetMappedBuffer"); -} - -void ExportedBuffer::destroy() -{ - if (cuda_ptr_ != nullptr) - { - (void)cudaFree(cuda_ptr_); - cuda_ptr_ = nullptr; - } - if (cuda_external_memory_ != nullptr) - { - (void)cudaDestroyExternalMemory(static_cast(cuda_external_memory_)); - cuda_external_memory_ = nullptr; - } - if (memory_fd_ >= 0) - { - // Only reachable when the import failed before we closed it. - ::close(memory_fd_); - memory_fd_ = -1; - } - if (device_ != VK_NULL_HANDLE) - { - if (buffer_ != VK_NULL_HANDLE) - { - vkDestroyBuffer(device_, buffer_, nullptr); - buffer_ = VK_NULL_HANDLE; - } - if (memory_ != VK_NULL_HANDLE) - { - vkFreeMemory(device_, memory_, nullptr); - memory_ = VK_NULL_HANDLE; - } - } - device_ = VK_NULL_HANDLE; - size_bytes_ = 0; -} - -// ── ViewTarget ───────────────────────────────────────────────────────────── - -ViewTarget::~ViewTarget() -{ - destroy(); -} - -void ViewTarget::create(const BorrowedDevice& dev, VkRenderPass render_pass, uint32_t width, uint32_t height) -{ - device_ = dev.device; - width_ = width; - height_ = height; - - create_attachment(dev, width, height, kColorFormat, - VK_IMAGE_USAGE_COLOR_ATTACHMENT_BIT | VK_IMAGE_USAGE_TRANSFER_SRC_BIT, VK_IMAGE_ASPECT_COLOR_BIT, - &color_image_, &color_memory_, &color_view_); - create_attachment(dev, width, height, kDepthFormat, - VK_IMAGE_USAGE_DEPTH_STENCIL_ATTACHMENT_BIT | VK_IMAGE_USAGE_TRANSFER_SRC_BIT, - VK_IMAGE_ASPECT_DEPTH_BIT, &depth_image_, &depth_memory_, &depth_view_); - - const VkImageView attachments[2] = { color_view_, depth_view_ }; - VkFramebufferCreateInfo fb{}; - fb.sType = VK_STRUCTURE_TYPE_FRAMEBUFFER_CREATE_INFO; - fb.renderPass = render_pass; - fb.attachmentCount = 2; - fb.pAttachments = attachments; - fb.width = width; - fb.height = height; - fb.layers = 1; - check_vk(vkCreateFramebuffer(device_, &fb, nullptr, &framebuffer_), "vkCreateFramebuffer"); - - // Tightly packed, so __cuda_array_interface__ can report strides=None: - // RGBA8 is 4 bytes/px, D32_SFLOAT is 4 bytes/px. - const VkDeviceSize pixels = static_cast(width) * height; - color_staging_.create(dev, pixels * 4); - depth_staging_.create(dev, pixels * 4); -} - -void ViewTarget::destroy() -{ - color_staging_.destroy(); - depth_staging_.destroy(); - if (device_ == VK_NULL_HANDLE) - { - return; - } - if (framebuffer_ != VK_NULL_HANDLE) - { - vkDestroyFramebuffer(device_, framebuffer_, nullptr); - framebuffer_ = VK_NULL_HANDLE; - } - if (color_view_ != VK_NULL_HANDLE) - { - vkDestroyImageView(device_, color_view_, nullptr); - color_view_ = VK_NULL_HANDLE; - } - if (color_image_ != VK_NULL_HANDLE) - { - vkDestroyImage(device_, color_image_, nullptr); - color_image_ = VK_NULL_HANDLE; - } - if (color_memory_ != VK_NULL_HANDLE) - { - vkFreeMemory(device_, color_memory_, nullptr); - color_memory_ = VK_NULL_HANDLE; - } - if (depth_view_ != VK_NULL_HANDLE) - { - vkDestroyImageView(device_, depth_view_, nullptr); - depth_view_ = VK_NULL_HANDLE; - } - if (depth_image_ != VK_NULL_HANDLE) - { - vkDestroyImage(device_, depth_image_, nullptr); - depth_image_ = VK_NULL_HANDLE; - } - if (depth_memory_ != VK_NULL_HANDLE) - { - vkFreeMemory(device_, depth_memory_, nullptr); - depth_memory_ = VK_NULL_HANDLE; - } - device_ = VK_NULL_HANDLE; -} - -void ViewTarget::record_readback(VkCommandBuffer cmd) const -{ - VkBufferImageCopy region{}; - region.bufferOffset = 0; - region.bufferRowLength = 0; // 0 = tightly packed to imageExtent.width - region.bufferImageHeight = 0; - region.imageSubresource.mipLevel = 0; - region.imageSubresource.baseArrayLayer = 0; - region.imageSubresource.layerCount = 1; - region.imageExtent = { width_, height_, 1 }; - - region.imageSubresource.aspectMask = VK_IMAGE_ASPECT_COLOR_BIT; - vkCmdCopyImageToBuffer(cmd, color_image_, VK_IMAGE_LAYOUT_TRANSFER_SRC_OPTIMAL, color_staging_.buffer(), 1, ®ion); - - region.imageSubresource.aspectMask = VK_IMAGE_ASPECT_DEPTH_BIT; - vkCmdCopyImageToBuffer(cmd, depth_image_, VK_IMAGE_LAYOUT_TRANSFER_SRC_OPTIMAL, depth_staging_.buffer(), 1, ®ion); -} - -// ── Render pass ──────────────────────────────────────────────────────────── - -VkRenderPass create_scene_render_pass(VkDevice device) -{ - VkAttachmentDescription attachments[2]{}; - // Colour. clearValue alpha is 0 in the renderer: this is an AR scene and - // the compositor shows passthrough wherever we did not draw. - attachments[0].format = kColorFormat; - attachments[0].samples = VK_SAMPLE_COUNT_1_BIT; - attachments[0].loadOp = VK_ATTACHMENT_LOAD_OP_CLEAR; - attachments[0].storeOp = VK_ATTACHMENT_STORE_OP_STORE; - attachments[0].stencilLoadOp = VK_ATTACHMENT_LOAD_OP_DONT_CARE; - attachments[0].stencilStoreOp = VK_ATTACHMENT_STORE_OP_DONT_CARE; - attachments[0].initialLayout = VK_IMAGE_LAYOUT_UNDEFINED; - // Ends in TRANSFER_SRC so record_readback() needs no extra barrier. - attachments[0].finalLayout = VK_IMAGE_LAYOUT_TRANSFER_SRC_OPTIMAL; - // Depth. STORE, not DONT_CARE: the depth buffer is an output here, not - // scratch -- it goes to XrCompositionLayerDepthInfoKHR via ProjectionLayer. - attachments[1].format = kDepthFormat; - attachments[1].samples = VK_SAMPLE_COUNT_1_BIT; - attachments[1].loadOp = VK_ATTACHMENT_LOAD_OP_CLEAR; - attachments[1].storeOp = VK_ATTACHMENT_STORE_OP_STORE; - attachments[1].stencilLoadOp = VK_ATTACHMENT_LOAD_OP_DONT_CARE; - attachments[1].stencilStoreOp = VK_ATTACHMENT_STORE_OP_DONT_CARE; - attachments[1].initialLayout = VK_IMAGE_LAYOUT_UNDEFINED; - attachments[1].finalLayout = VK_IMAGE_LAYOUT_TRANSFER_SRC_OPTIMAL; - - VkAttachmentReference color_ref{ 0, VK_IMAGE_LAYOUT_COLOR_ATTACHMENT_OPTIMAL }; - VkAttachmentReference depth_ref{ 1, VK_IMAGE_LAYOUT_DEPTH_STENCIL_ATTACHMENT_OPTIMAL }; - - VkSubpassDescription subpass{}; - subpass.pipelineBindPoint = VK_PIPELINE_BIND_POINT_GRAPHICS; - subpass.colorAttachmentCount = 1; - subpass.pColorAttachments = &color_ref; - subpass.pDepthStencilAttachment = &depth_ref; - - // Make the render-pass writes visible to the transfer reads that follow. - VkSubpassDependency deps[2]{}; - deps[0].srcSubpass = VK_SUBPASS_EXTERNAL; - deps[0].dstSubpass = 0; - deps[0].srcStageMask = VK_PIPELINE_STAGE_TRANSFER_BIT; - deps[0].dstStageMask = VK_PIPELINE_STAGE_COLOR_ATTACHMENT_OUTPUT_BIT | VK_PIPELINE_STAGE_EARLY_FRAGMENT_TESTS_BIT; - deps[0].srcAccessMask = VK_ACCESS_TRANSFER_READ_BIT; - deps[0].dstAccessMask = VK_ACCESS_COLOR_ATTACHMENT_WRITE_BIT | VK_ACCESS_DEPTH_STENCIL_ATTACHMENT_WRITE_BIT; - deps[1].srcSubpass = 0; - deps[1].dstSubpass = VK_SUBPASS_EXTERNAL; - deps[1].srcStageMask = VK_PIPELINE_STAGE_COLOR_ATTACHMENT_OUTPUT_BIT | VK_PIPELINE_STAGE_LATE_FRAGMENT_TESTS_BIT; - deps[1].dstStageMask = VK_PIPELINE_STAGE_TRANSFER_BIT; - deps[1].srcAccessMask = VK_ACCESS_COLOR_ATTACHMENT_WRITE_BIT | VK_ACCESS_DEPTH_STENCIL_ATTACHMENT_WRITE_BIT; - deps[1].dstAccessMask = VK_ACCESS_TRANSFER_READ_BIT; - - VkRenderPassCreateInfo info{}; - info.sType = VK_STRUCTURE_TYPE_RENDER_PASS_CREATE_INFO; - info.attachmentCount = 2; - info.pAttachments = attachments; - info.subpassCount = 1; - info.pSubpasses = &subpass; - info.dependencyCount = 2; - info.pDependencies = deps; - - VkRenderPass render_pass = VK_NULL_HANDLE; - check_vk(vkCreateRenderPass(device, &info, nullptr, &render_pass), "vkCreateRenderPass"); - return render_pass; -} - -BorrowedDevice borrow_device(uintptr_t physical_device, uintptr_t device, uint32_t queue_family_index) -{ - BorrowedDevice dev; - dev.physical_device = reinterpret_cast(physical_device); - dev.device = reinterpret_cast(device); - dev.queue_family_index = queue_family_index; - if (dev.physical_device == VK_NULL_HANDLE || dev.device == VK_NULL_HANDLE) - { - throw std::runtime_error( - "mujoco_xr: VizSession handed over a null VkDevice / VkPhysicalDevice. Create the renderer AFTER " - "VizSession.create()."); - } - // queueCount is 1 on both of viz's device-creation paths, so index 0 is - // viz's own queue -- we share it rather than racing a second one. - vkGetDeviceQueue(dev.device, dev.queue_family_index, 0, &dev.queue); - if (dev.queue == VK_NULL_HANDLE) - { - throw std::runtime_error("mujoco_xr: vkGetDeviceQueue returned null for the borrowed queue family"); - } - return dev; -} - -} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/render_target.hpp b/examples/mujoco_xr/cpp/render_target.hpp deleted file mode 100644 index a832af7a9..000000000 --- a/examples/mujoco_xr/cpp/render_target.hpp +++ /dev/null @@ -1,154 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#pragma once - -// Offscreen colour + depth attachments, and the linear CUDA-visible copies of -// them that viz::ProjectionLayer.submit() consumes. -// -// Both an image and a buffer, because a colour/depth attachment has to be an -// OPTIMAL-tiled VkImage: we render into that, then vkCmdCopyImageToBuffer into -// a tightly-packed VkBuffer whose memory was allocated exportable. CUDA imports -// the buffer and gets the plain linear device pointer that -// __cuda_array_interface__ describes; importing the tiled image directly yields -// a cudaArray_t, which viz::VizBuffer does not model. -// -// The Vulkan device is borrowed from isaacteleop.viz.VizSession, never created -// here. viz already enables VK_KHR_external_memory + VK_KHR_external_memory_fd -// on both device-creation paths, so borrowing gets the export path with no viz -// changes -- and guarantees the CUDA device matches the Vulkan physical device -// by UUID, because VkContext::init() did that match. - -#include - -#include - -namespace mujoco_xr -{ - -// The two Vulkan helpers every TU in this module needs. They live here, and -// not once per .cpp, because scene_renderer.cpp includes this header anyway -// (through scene_renderer.hpp) and two byte-identical copies drift. - -// Throw a std::runtime_error naming `what` unless `result` is VK_SUCCESS. -void check_vk(VkResult result, const char* what); - -// First memory type satisfying both the allocation's type_bits and `properties`. -uint32_t find_memory_type(VkPhysicalDevice physical_device, uint32_t type_bits, VkMemoryPropertyFlags properties); - -// Handles handed over as plain integers by VizSession. Nothing viz-typed. -struct BorrowedDevice -{ - VkPhysicalDevice physical_device = VK_NULL_HANDLE; - VkDevice device = VK_NULL_HANDLE; - uint32_t queue_family_index = 0; - // viz creates its device with queueCount == 1, so index 0 is viz's own - // queue. We share it: one thread, and our submits interleave with viz's - // between begin_frame() and end_frame(). - VkQueue queue = VK_NULL_HANDLE; -}; - -// A VkBuffer whose memory is exported as an fd and imported into CUDA. -class ExportedBuffer -{ -public: - ExportedBuffer() = default; - ~ExportedBuffer(); - - ExportedBuffer(const ExportedBuffer&) = delete; - ExportedBuffer& operator=(const ExportedBuffer&) = delete; - - void create(const BorrowedDevice& dev, VkDeviceSize size_bytes); - void destroy(); - - VkBuffer buffer() const - { - return buffer_; - } - // Linear CUDA device pointer aliasing the same memory. Valid for the - // lifetime of this object. - void* cuda_ptr() const - { - return cuda_ptr_; - } - -private: - VkDevice device_ = VK_NULL_HANDLE; - VkBuffer buffer_ = VK_NULL_HANDLE; - VkDeviceMemory memory_ = VK_NULL_HANDLE; - VkDeviceSize size_bytes_ = 0; - int memory_fd_ = -1; - void* cuda_external_memory_ = nullptr; // cudaExternalMemory_t - void* cuda_ptr_ = nullptr; -}; - -// Everything one eye needs: the attachments, the framebuffer, and the two -// CUDA-visible staging buffers. -class ViewTarget -{ -public: - ViewTarget() = default; - ~ViewTarget(); - - ViewTarget(const ViewTarget&) = delete; - ViewTarget& operator=(const ViewTarget&) = delete; - - void create(const BorrowedDevice& dev, VkRenderPass render_pass, uint32_t width, uint32_t height); - void destroy(); - - VkFramebuffer framebuffer() const - { - return framebuffer_; - } - // Records the two image -> linear-buffer copies. Must be called after - // vkCmdEndRenderPass; the render pass leaves both attachments in - // TRANSFER_SRC_OPTIMAL. - void record_readback(VkCommandBuffer cmd) const; - - const ExportedBuffer& color() const - { - return color_staging_; - } - const ExportedBuffer& depth() const - { - return depth_staging_; - } - uint32_t width() const - { - return width_; - } - uint32_t height() const - { - return height_; - } - -private: - VkDevice device_ = VK_NULL_HANDLE; - uint32_t width_ = 0; - uint32_t height_ = 0; - VkImage color_image_ = VK_NULL_HANDLE; - VkDeviceMemory color_memory_ = VK_NULL_HANDLE; - VkImageView color_view_ = VK_NULL_HANDLE; - VkImage depth_image_ = VK_NULL_HANDLE; - VkDeviceMemory depth_memory_ = VK_NULL_HANDLE; - VkImageView depth_view_ = VK_NULL_HANDLE; - VkFramebuffer framebuffer_ = VK_NULL_HANDLE; - ExportedBuffer color_staging_; - ExportedBuffer depth_staging_; -}; - -// R8G8B8A8_UNORM colour + D32_SFLOAT depth, both stored and both left in -// TRANSFER_SRC_OPTIMAL so record_readback() can copy them straight out. -// -// D32_SFLOAT and NOT a reversed-Z variant: the depth values we hand to -// ProjectionLayer are the raw window-space z, and the projection built in -// scene_renderer.cpp maps z_view = -near -> 0.0 and z_view = -far -> 1.0. -// (Two doc comments in viz say "reverse-Z"; the code is standard Z. Believe -// the code -- and the per-frame assertion in the Python app.) -VkRenderPass create_scene_render_pass(VkDevice device); - -// Borrow VizSession's queue. Separate from BorrowedDevice's aggregate init so -// the caller does not have to declare vkGetDeviceQueue. -BorrowedDevice borrow_device(uintptr_t physical_device, uintptr_t device, uint32_t queue_family_index); - -} // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/scene_renderer.cpp b/examples/mujoco_xr/cpp/scene_renderer.cpp index d4f3a362e..d49203cf9 100644 --- a/examples/mujoco_xr/cpp/scene_renderer.cpp +++ b/examples/mujoco_xr/cpp/scene_renderer.cpp @@ -4,12 +4,11 @@ #include "scene_renderer.hpp" #include "frames.hpp" +#include "glcamera.hpp" -#include -#include - -#include -#include +#include +#include +#include #include #include @@ -19,208 +18,107 @@ namespace mujoco_xr namespace { -// mjvScene capacity. Not a knob: the only failure mode is "the scene has more -// geoms than this", which is a hard error rather than something to tune at -// runtime, and 20k is ~30x what a tabletop scene produces. +// Not a knob: overflowing it is a hard error, and 20k is ~30x a tabletop scene. constexpr int kMaxGeom = 20000; -// check_vk() and find_memory_type() come from render_target.hpp. That header -// is included by scene_renderer.hpp and NOT redundantly: BorrowedDevice -// (scene_renderer.hpp:118, by value) and ViewTarget (:134, inside a vector) -// both need to be complete there. - -// Well inside the 128-byte Vulkan-guaranteed push-constant budget. No separate -// normal matrix: every geom drawn here is a mesh, whose mjvGeom.mat is a pure -// rotation, so model's upper 3x3 transforms normals unchanged. -struct PushConstants -{ - float model[16]; // column-major world-from-local (rotation, pos) - float color[4]; -}; -static_assert(sizeof(PushConstants) <= 128, "push constant budget"); - -struct EyeUbo -{ - float viewproj[16]; - float light_dir[4]; -}; +// OpenXR view space: -Z forward, +Y up. +constexpr std::array kXrForward = { 0.0, 0.0, -1.0 }; +constexpr std::array kXrUp = { 0.0, 1.0, 0.0 }; -// out = a * b, column-major 4x4. -void mat4_mul(float out[16], const float a[16], const float b[16]) +void check_cuda(cudaError_t err, const char* what) { - float r[16]; - for (int c = 0; c < 4; ++c) + if (err != cudaSuccess) { - for (int row = 0; row < 4; ++row) - { - r[c * 4 + row] = a[0 * 4 + row] * b[c * 4 + 0] + a[1 * 4 + row] * b[c * 4 + 1] + - a[2 * 4 + row] * b[c * 4 + 2] + a[3 * 4 + row] * b[c * 4 + 3]; - } + throw std::runtime_error(std::string("mujoco_xr: ") + what + " failed: " + cudaGetErrorString(err)); } - std::memcpy(out, r, sizeof(r)); } -// Vulkan-convention projection (y-down clip, depth 0..1) from an OpenXR-style -// asymmetric fov. Algebraically identical to glm::frustumRH_ZO on -// l = n*tan(angleLeft), r = n*tan(angleRight), b = n*tan(angleUp), -// t = n*tan(angleDown) -- note the DELIBERATE angleUp -> bottom swap, which is -// what viz itself does in src/viz/session/cpp/xr_backend.cpp's -// fov_to_projection_matrix. That swap is the y flip; the renderer must NOT -// flip y a second time. -// -// Consequences, all asserted per frame on the Python side: -// out[0] = P[0][0] > 0 -// out[5] = P[1][1] < 0 <- the load-bearing one; it drives winding -// out[10] = P[2][2] < 0, out[11] = P[2][3] == -1, out[14] = P[3][2] < 0 -// i.e. STANDARD Z (z_view = -near -> 0.0, -far -> 1.0), not -// reverse-Z, whatever two stale viz doc comments claim. -void proj_from_fov(const float fov_lrud[4], float near_z, float far_z, float out[16]) +// viz's VkContext::init() already cudaSetDevice'd the card matching its Vulkan +// device, so "viz's GPU" is just the current CUDA device. The GL context is +// made independently, and nothing makes it land on the same card. +void require_gl_on_viz_device() { - const float tl = std::tan(fov_lrud[0]); - const float tr = std::tan(fov_lrud[1]); - const float tu = std::tan(fov_lrud[2]); - const float td = std::tan(fov_lrud[3]); - std::memset(out, 0, 16 * sizeof(float)); - out[0] = 2.0f / (tr - tl); - out[8] = (tr + tl) / (tr - tl); - out[5] = 2.0f / (td - tu); // (td - tu) < 0 flips y for Vulkan clip space - out[9] = (td + tu) / (td - tu); - out[10] = far_z / (near_z - far_z); - out[14] = (far_z * near_z) / (near_z - far_z); - out[11] = -1.0f; -} + int cuda_device = -1; + check_cuda(cudaGetDevice(&cuda_device), "cudaGetDevice"); -// Inverse of a rigid pose (the view pose is eye-in-reference-space): -// V = [R^T | -R^T t]. Quaternion arrives as wxyz, matching viz::Pose3D. -void view_from_pose(const float pos[3], const float q_wxyz[4], float out[16]) -{ - const float w = q_wxyz[0]; - const float x = q_wxyz[1]; - const float y = q_wxyz[2]; - const float z = q_wxyz[3]; - // Row-major R from quaternion. - const float R[9] = { 1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y), - 2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x), - 2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y) }; - std::memset(out, 0, 16 * sizeof(float)); - // Column-major out: rotation part = R^T -> out[c*4 + r] = R^T[r][c] = R[c*3 + r]. - for (int r = 0; r < 3; ++r) + unsigned int count = 0; + int gl_devices[8] = { 0 }; + const cudaError_t err = cudaGLGetDevices(&count, gl_devices, 8, cudaGLDeviceListAll); + if (err != cudaSuccess || count == 0) { - for (int c = 0; c < 3; ++c) - { - out[c * 4 + r] = R[c * 3 + r]; - } + throw std::runtime_error( + std::string("mujoco_xr: the current OpenGL context is on no CUDA-capable device (") + cudaGetErrorString(err) + + "). Create it with mujoco.GLContext BEFORE the renderer, on this thread, with MUJOCO_GL=egl."); } - for (int r = 0; r < 3; ++r) + for (unsigned int i = 0; i < count; ++i) { - out[12 + r] = -(R[0 * 3 + r] * pos[0] + R[1 * 3 + r] * pos[1] + R[2 * 3 + r] * pos[2]); + if (gl_devices[i] == cuda_device) + { + return; + } } - out[15] = 1.0f; + throw std::runtime_error( + "mujoco_xr: the OpenGL context is on CUDA device " + std::to_string(gl_devices[0]) + " but viz is on device " + + std::to_string(cuda_device) + + ". Set MUJOCO_EGL_DEVICE_ID to viz's GPU; a cross-device pixel-pack buffer cannot be imported."); } -void create_host_buffer(const BorrowedDevice& dev, - VkDeviceSize size, - VkBufferUsageFlags usage, - VkBuffer* out_buffer, - VkDeviceMemory* out_memory) +// A direction from XR into MuJoCo world: rotation only, no workspace offset. +std::array mj_from_xr_dir(const std::array& q_mj, const std::array& v_xr) { - VkBufferCreateInfo info{}; - info.sType = VK_STRUCTURE_TYPE_BUFFER_CREATE_INFO; - info.size = size; - info.usage = usage; - info.sharingMode = VK_SHARING_MODE_EXCLUSIVE; - check_vk(vkCreateBuffer(dev.device, &info, nullptr, out_buffer), "vkCreateBuffer"); - - VkMemoryRequirements reqs; - vkGetBufferMemoryRequirements(dev.device, *out_buffer, &reqs); - VkMemoryAllocateInfo alloc{}; - alloc.sType = VK_STRUCTURE_TYPE_MEMORY_ALLOCATE_INFO; - alloc.allocationSize = reqs.size; - alloc.memoryTypeIndex = find_memory_type(dev.physical_device, reqs.memoryTypeBits, - VK_MEMORY_PROPERTY_HOST_VISIBLE_BIT | VK_MEMORY_PROPERTY_HOST_COHERENT_BIT); - check_vk(vkAllocateMemory(dev.device, &alloc, nullptr, out_memory), "vkAllocateMemory"); - check_vk(vkBindBufferMemory(dev.device, *out_buffer, *out_memory, 0), "vkBindBufferMemory"); -} - -VkShaderModule make_shader_module(VkDevice device, const unsigned char* code, size_t size_bytes) -{ - VkShaderModuleCreateInfo info{}; - info.sType = VK_STRUCTURE_TYPE_SHADER_MODULE_CREATE_INFO; - info.codeSize = size_bytes; - info.pCode = reinterpret_cast(code); - VkShaderModule module = VK_NULL_HANDLE; - check_vk(vkCreateShaderModule(device, &info, nullptr, &module), "vkCreateShaderModule"); - return module; + std::array out{}; + mju_rotVecQuat(out.data(), v_xr.data(), q_mj.data()); + return out; } } // namespace -std::array projection_from_fov(const std::array& fov_lrud, float near_z, float far_z) +SceneRenderer::SceneRenderer(const Config& config, const mjModel* model) : config_(config) { - if (!(near_z > 0.0f) || !(far_z > near_z)) + if (model == nullptr) { - throw std::invalid_argument("mujoco_xr: require 0 < near_z < far_z"); + throw std::invalid_argument("mujoco_xr: null mjModel*"); } - if (fov_lrud[1] <= fov_lrud[0] || fov_lrud[2] <= fov_lrud[3]) + if (config_.width == 0 || config_.height == 0 || config_.view_count == 0) { - throw std::invalid_argument( - "mujoco_xr: degenerate fov (need angle_right > angle_left and angle_up > angle_down). A " - "default-constructed viz::Fov is four zeros, and rendering one yields P[0][0] = +inf with a " - "NaN column -- a blank headset and no error. Fix the FrameInfo.views the session handed over; " - "do not relax this check."); + throw std::invalid_argument("mujoco_xr: renderer needs a non-empty size and at least one view"); } - std::array out{}; - proj_from_fov(fov_lrud.data(), near_z, far_z, out.data()); - return out; -} + // Validates near/far up front, on a fov that is certainly non-degenerate. + (void)frustum_from_fov({ -0.5f, 0.5f, 0.5f, -0.5f }, config_.near_z, config_.far_z); -SceneRenderer::SceneRenderer(const BorrowedDevice& dev, const Config& config, const mjModel* model) - : dev_(dev), config_(config) -{ - if (config_.width == 0 || config_.height == 0) - { - throw std::invalid_argument("mujoco_xr: renderer resolution must be non-zero"); - } - if (config_.view_count != 2) + gl::load(); + require_gl_on_viz_device(); + + mjv_defaultOption(&scene_option_); + mjv_defaultFreeCamera(model, &camera_); + mjv_makeScene(model, &scene_, kMaxGeom); + scene_made_ = true; + // Each eye is drawn on its own, into the whole offscreen buffer. + scene_.stereo = mjSTEREO_NONE; + + mjr_defaultContext(&context_); + mjr_makeContext(model, &context_, mjFONTSCALE_100); + context_made_ = true; + // Overrides whatever the scene declared. + mjr_resizeOffscreen(static_cast(config_.width), static_cast(config_.height), &context_); + if (context_.offWidth != static_cast(config_.width) || context_.offHeight != static_cast(config_.height)) { - throw std::invalid_argument("mujoco_xr: view_count must be 2 (stereo); mono is not supported"); + throw std::runtime_error("mujoco_xr: mjr_resizeOffscreen did not take; the GL context is too small or lost"); } - if (!(config_.near_z > 0.0f) || !(config_.far_z > config_.near_z)) + if (context_.offSamples != 0) { - throw std::invalid_argument("mujoco_xr: require 0 < near_z < far_z (pass the app's single near/far pair)"); + throw std::runtime_error( + "mujoco_xr: model.vis.quality.offsamples must be 0. Multisample renderbuffers cannot be blitted with a " + "y flip in one step, and MuJoCo resolves them only inside mjr_readPixels, which this path does not call."); } - if (model == nullptr) + mjr_setBuffer(mjFB_OFFSCREEN, &context_); + if (context_.currentBuffer != mjFB_OFFSCREEN) { - throw std::invalid_argument("mujoco_xr: model address is null"); + throw std::runtime_error("mujoco_xr: the offscreen framebuffer is unavailable in this OpenGL context"); } - try - { - xr_from_mj_mat4(xr_from_mj_); - - mjv_defaultOption(&scene_option_); - mjv_defaultFreeCamera(model, &camera_); - mjv_defaultScene(&scene_); - mjv_makeScene(model, &scene_, kMaxGeom); - scene_made_ = true; - - render_pass_ = create_scene_render_pass(dev_.device); - upload_geometry(model); - create_pipeline(); - create_uniforms(); - - view_targets_ = std::vector(config_.view_count); - projections_.assign(config_.view_count, std::array{}); - for (uint32_t i = 0; i < config_.view_count; ++i) - { - view_targets_[i].create(dev_, render_pass_, config_.width, config_.height); - } - } - catch (...) - { - destroy(); - throw; - } + readback_.create(config_.width, config_.height, config_.view_count, context_.offFBO); + cameras_.resize(config_.view_count); } SceneRenderer::~SceneRenderer() @@ -230,314 +128,17 @@ SceneRenderer::~SceneRenderer() void SceneRenderer::destroy() { - if (dev_.device != VK_NULL_HANDLE) + readback_.destroy(); + if (context_made_) { - (void)vkDeviceWaitIdle(dev_.device); + mjr_freeContext(&context_); + context_made_ = false; } - // View targets first: they hold CUDA imports of exported memory, and the - // VkDeviceMemory must outlive the mapping. - view_targets_.clear(); - - if (dev_.device != VK_NULL_HANDLE) - { - for (size_t i = 0; i < ubos_.size(); ++i) - { - if (ubo_mapped_[i] != nullptr) - { - vkUnmapMemory(dev_.device, ubo_memory_[i]); - } - if (ubos_[i] != VK_NULL_HANDLE) - { - vkDestroyBuffer(dev_.device, ubos_[i], nullptr); - } - if (ubo_memory_[i] != VK_NULL_HANDLE) - { - vkFreeMemory(dev_.device, ubo_memory_[i], nullptr); - } - } - ubos_.clear(); - ubo_memory_.clear(); - ubo_mapped_.clear(); - descriptor_sets_.clear(); - - if (fence_ != VK_NULL_HANDLE) - { - vkDestroyFence(dev_.device, fence_, nullptr); - fence_ = VK_NULL_HANDLE; - } - if (command_pool_ != VK_NULL_HANDLE) - { - vkDestroyCommandPool(dev_.device, command_pool_, nullptr); - command_pool_ = VK_NULL_HANDLE; - command_buffer_ = VK_NULL_HANDLE; - } - if (vertex_buffer_ != VK_NULL_HANDLE) - { - vkDestroyBuffer(dev_.device, vertex_buffer_, nullptr); - vertex_buffer_ = VK_NULL_HANDLE; - } - if (vertex_memory_ != VK_NULL_HANDLE) - { - vkFreeMemory(dev_.device, vertex_memory_, nullptr); - vertex_memory_ = VK_NULL_HANDLE; - } - if (index_buffer_ != VK_NULL_HANDLE) - { - vkDestroyBuffer(dev_.device, index_buffer_, nullptr); - index_buffer_ = VK_NULL_HANDLE; - } - if (index_memory_ != VK_NULL_HANDLE) - { - vkFreeMemory(dev_.device, index_memory_, nullptr); - index_memory_ = VK_NULL_HANDLE; - } - if (pipeline_ != VK_NULL_HANDLE) - { - vkDestroyPipeline(dev_.device, pipeline_, nullptr); - pipeline_ = VK_NULL_HANDLE; - } - if (pipeline_layout_ != VK_NULL_HANDLE) - { - vkDestroyPipelineLayout(dev_.device, pipeline_layout_, nullptr); - pipeline_layout_ = VK_NULL_HANDLE; - } - if (descriptor_pool_ != VK_NULL_HANDLE) - { - vkDestroyDescriptorPool(dev_.device, descriptor_pool_, nullptr); - descriptor_pool_ = VK_NULL_HANDLE; - } - if (dsl_ != VK_NULL_HANDLE) - { - vkDestroyDescriptorSetLayout(dev_.device, dsl_, nullptr); - dsl_ = VK_NULL_HANDLE; - } - if (render_pass_ != VK_NULL_HANDLE) - { - vkDestroyRenderPass(dev_.device, render_pass_, nullptr); - render_pass_ = VK_NULL_HANDLE; - } - } - if (scene_made_) { mjv_freeScene(&scene_); scene_made_ = false; } - // The geometry index is NOT a Vulkan handle and is the other half of the - // same bug: leaving stale ranges here would let a draw index into a - // destroyed buffer -- in bounds, entirely wrong, and invisible to the - // validation layers. - mesh_ranges_.clear(); -} - -void SceneRenderer::upload_geometry(const mjModel* model) -{ - MeshBuffers mb; - build_mesh_buffers(model, &mb); - mesh_ranges_ = mb.meshes; - - const VkDeviceSize vsize = mb.verts.size() * sizeof(Vertex); - const VkDeviceSize isize = mb.indices.size() * sizeof(uint32_t); - if (vsize == 0 || isize == 0) - { - throw std::runtime_error("mujoco_xr: model produced no renderable geometry"); - } - create_host_buffer(dev_, vsize, VK_BUFFER_USAGE_VERTEX_BUFFER_BIT, &vertex_buffer_, &vertex_memory_); - create_host_buffer(dev_, isize, VK_BUFFER_USAGE_INDEX_BUFFER_BIT, &index_buffer_, &index_memory_); - - void* map = nullptr; - check_vk(vkMapMemory(dev_.device, vertex_memory_, 0, vsize, 0, &map), "vkMapMemory(vertex)"); - std::memcpy(map, mb.verts.data(), vsize); - vkUnmapMemory(dev_.device, vertex_memory_); - check_vk(vkMapMemory(dev_.device, index_memory_, 0, isize, 0, &map), "vkMapMemory(index)"); - std::memcpy(map, mb.indices.data(), isize); - vkUnmapMemory(dev_.device, index_memory_); -} - -void SceneRenderer::create_pipeline() -{ - VkDescriptorSetLayoutBinding binding{}; - binding.binding = 0; - binding.descriptorType = VK_DESCRIPTOR_TYPE_UNIFORM_BUFFER; - binding.descriptorCount = 1; - binding.stageFlags = VK_SHADER_STAGE_VERTEX_BIT | VK_SHADER_STAGE_FRAGMENT_BIT; - VkDescriptorSetLayoutCreateInfo dsl_info{}; - dsl_info.sType = VK_STRUCTURE_TYPE_DESCRIPTOR_SET_LAYOUT_CREATE_INFO; - dsl_info.bindingCount = 1; - dsl_info.pBindings = &binding; - check_vk(vkCreateDescriptorSetLayout(dev_.device, &dsl_info, nullptr, &dsl_), "vkCreateDescriptorSetLayout"); - - VkPushConstantRange pc_range{ VK_SHADER_STAGE_VERTEX_BIT | VK_SHADER_STAGE_FRAGMENT_BIT, 0, sizeof(PushConstants) }; - VkPipelineLayoutCreateInfo pl_info{}; - pl_info.sType = VK_STRUCTURE_TYPE_PIPELINE_LAYOUT_CREATE_INFO; - pl_info.setLayoutCount = 1; - pl_info.pSetLayouts = &dsl_; - pl_info.pushConstantRangeCount = 1; - pl_info.pPushConstantRanges = &pc_range; - check_vk(vkCreatePipelineLayout(dev_.device, &pl_info, nullptr, &pipeline_layout_), "vkCreatePipelineLayout"); - - VkShaderModule vs = make_shader_module(dev_.device, shaders::kSceneVertSpv, shaders::kSceneVertSpvSize); - VkShaderModule fs = make_shader_module(dev_.device, shaders::kSceneFragSpv, shaders::kSceneFragSpvSize); - - VkPipelineShaderStageCreateInfo stages[2]{}; - stages[0].sType = VK_STRUCTURE_TYPE_PIPELINE_SHADER_STAGE_CREATE_INFO; - stages[0].stage = VK_SHADER_STAGE_VERTEX_BIT; - stages[0].module = vs; - stages[0].pName = "main"; - stages[1].sType = VK_STRUCTURE_TYPE_PIPELINE_SHADER_STAGE_CREATE_INFO; - stages[1].stage = VK_SHADER_STAGE_FRAGMENT_BIT; - stages[1].module = fs; - stages[1].pName = "main"; - - VkVertexInputBindingDescription vbind{ 0, sizeof(Vertex), VK_VERTEX_INPUT_RATE_VERTEX }; - VkVertexInputAttributeDescription vattrs[2] = { { 0, 0, VK_FORMAT_R32G32B32_SFLOAT, offsetof(Vertex, pos) }, - { 1, 0, VK_FORMAT_R32G32B32_SFLOAT, offsetof(Vertex, normal) } }; - VkPipelineVertexInputStateCreateInfo vin{}; - vin.sType = VK_STRUCTURE_TYPE_PIPELINE_VERTEX_INPUT_STATE_CREATE_INFO; - vin.vertexBindingDescriptionCount = 1; - vin.pVertexBindingDescriptions = &vbind; - vin.vertexAttributeDescriptionCount = 2; - vin.pVertexAttributeDescriptions = vattrs; - - VkPipelineInputAssemblyStateCreateInfo ia{}; - ia.sType = VK_STRUCTURE_TYPE_PIPELINE_INPUT_ASSEMBLY_STATE_CREATE_INFO; - ia.topology = VK_PRIMITIVE_TOPOLOGY_TRIANGLE_LIST; - - VkPipelineViewportStateCreateInfo vp{}; - vp.sType = VK_STRUCTURE_TYPE_PIPELINE_VIEWPORT_STATE_CREATE_INFO; - vp.viewportCount = 1; - vp.scissorCount = 1; - - VkPipelineRasterizationStateCreateInfo rs{}; - rs.sType = VK_STRUCTURE_TYPE_PIPELINE_RASTERIZATION_STATE_CREATE_INFO; - rs.polygonMode = VK_POLYGON_MODE_FILL; - // CULLING IS OFF DURING BRING-UP, and that is a decision, not an omission. - // MuJoCo geoms are CCW, and the projection above already flips y (P[1][1] - // < 0), which inverts the effective winding. Get that wrong with culling - // ON and the scene renders BLACK, which is routinely misdiagnosed as a - // depth or a submit bug. MuJoCo's mesh assets also mix winding across - // OBJ/STL sources. Turn this on only once a headset has confirmed the - // scene is visible, and only together with frontFace. - rs.cullMode = VK_CULL_MODE_NONE; - rs.frontFace = VK_FRONT_FACE_COUNTER_CLOCKWISE; - rs.lineWidth = 1.0f; - - VkPipelineMultisampleStateCreateInfo ms{}; - ms.sType = VK_STRUCTURE_TYPE_PIPELINE_MULTISAMPLE_STATE_CREATE_INFO; - ms.rasterizationSamples = VK_SAMPLE_COUNT_1_BIT; - - VkPipelineDepthStencilStateCreateInfo ds{}; - ds.sType = VK_STRUCTURE_TYPE_PIPELINE_DEPTH_STENCIL_STATE_CREATE_INFO; - ds.depthTestEnable = VK_TRUE; - ds.depthWriteEnable = VK_TRUE; - ds.depthCompareOp = VK_COMPARE_OP_LESS_OR_EQUAL; - ds.maxDepthBounds = 1.0f; - - // The alpha channel is the AR passthrough mask, so alpha composites - // (A = A_src + (1 - A_src) * A_dst) rather than being replaced: with - // dstAlpha = ZERO a translucent geom drawn over an opaque one would drop - // that pixel's alpha and the compositor would blend passthrough through the - // robot. The result is PREMULTIPLIED, which is what viz's layers declare -- - // it never sets XR_COMPOSITION_LAYER_UNPREMULTIPLIED_ALPHA_BIT. The comment - // at src/viz/session/cpp/xr_backend.cpp:1202-1203 claims straight alpha - // while the code beside it sets no such bit; believe the code. - VkPipelineColorBlendAttachmentState blend{}; - blend.blendEnable = VK_TRUE; // the scene XML may set an rgba alpha < 1 - blend.srcColorBlendFactor = VK_BLEND_FACTOR_SRC_ALPHA; - blend.dstColorBlendFactor = VK_BLEND_FACTOR_ONE_MINUS_SRC_ALPHA; - blend.colorBlendOp = VK_BLEND_OP_ADD; - blend.srcAlphaBlendFactor = VK_BLEND_FACTOR_ONE; - blend.dstAlphaBlendFactor = VK_BLEND_FACTOR_ONE_MINUS_SRC_ALPHA; - blend.alphaBlendOp = VK_BLEND_OP_ADD; - blend.colorWriteMask = - VK_COLOR_COMPONENT_R_BIT | VK_COLOR_COMPONENT_G_BIT | VK_COLOR_COMPONENT_B_BIT | VK_COLOR_COMPONENT_A_BIT; - VkPipelineColorBlendStateCreateInfo cb{}; - cb.sType = VK_STRUCTURE_TYPE_PIPELINE_COLOR_BLEND_STATE_CREATE_INFO; - cb.attachmentCount = 1; - cb.pAttachments = &blend; - - VkDynamicState dyn_states[2] = { VK_DYNAMIC_STATE_VIEWPORT, VK_DYNAMIC_STATE_SCISSOR }; - VkPipelineDynamicStateCreateInfo dyn{}; - dyn.sType = VK_STRUCTURE_TYPE_PIPELINE_DYNAMIC_STATE_CREATE_INFO; - dyn.dynamicStateCount = 2; - dyn.pDynamicStates = dyn_states; - - VkGraphicsPipelineCreateInfo info{}; - info.sType = VK_STRUCTURE_TYPE_GRAPHICS_PIPELINE_CREATE_INFO; - info.stageCount = 2; - info.pStages = stages; - info.pVertexInputState = &vin; - info.pInputAssemblyState = &ia; - info.pViewportState = &vp; - info.pRasterizationState = &rs; - info.pMultisampleState = &ms; - info.pDepthStencilState = &ds; - info.pColorBlendState = &cb; - info.pDynamicState = &dyn; - info.layout = pipeline_layout_; - info.renderPass = render_pass_; - info.subpass = 0; - - const VkResult r = vkCreateGraphicsPipelines(dev_.device, VK_NULL_HANDLE, 1, &info, nullptr, &pipeline_); - vkDestroyShaderModule(dev_.device, vs, nullptr); - vkDestroyShaderModule(dev_.device, fs, nullptr); - check_vk(r, "vkCreateGraphicsPipelines"); -} - -void SceneRenderer::create_uniforms() -{ - VkDescriptorPoolSize pool_size{ VK_DESCRIPTOR_TYPE_UNIFORM_BUFFER, config_.view_count }; - VkDescriptorPoolCreateInfo pool_info{}; - pool_info.sType = VK_STRUCTURE_TYPE_DESCRIPTOR_POOL_CREATE_INFO; - pool_info.maxSets = config_.view_count; - pool_info.poolSizeCount = 1; - pool_info.pPoolSizes = &pool_size; - check_vk(vkCreateDescriptorPool(dev_.device, &pool_info, nullptr, &descriptor_pool_), "vkCreateDescriptorPool"); - - ubos_.assign(config_.view_count, VK_NULL_HANDLE); - ubo_memory_.assign(config_.view_count, VK_NULL_HANDLE); - ubo_mapped_.assign(config_.view_count, nullptr); - descriptor_sets_.assign(config_.view_count, VK_NULL_HANDLE); - - for (uint32_t i = 0; i < config_.view_count; ++i) - { - create_host_buffer(dev_, sizeof(EyeUbo), VK_BUFFER_USAGE_UNIFORM_BUFFER_BIT, &ubos_[i], &ubo_memory_[i]); - check_vk(vkMapMemory(dev_.device, ubo_memory_[i], 0, sizeof(EyeUbo), 0, &ubo_mapped_[i]), "vkMapMemory(ubo)"); - - VkDescriptorSetAllocateInfo alloc{}; - alloc.sType = VK_STRUCTURE_TYPE_DESCRIPTOR_SET_ALLOCATE_INFO; - alloc.descriptorPool = descriptor_pool_; - alloc.descriptorSetCount = 1; - alloc.pSetLayouts = &dsl_; - check_vk(vkAllocateDescriptorSets(dev_.device, &alloc, &descriptor_sets_[i]), "vkAllocateDescriptorSets"); - - VkDescriptorBufferInfo buf{ ubos_[i], 0, sizeof(EyeUbo) }; - VkWriteDescriptorSet write{}; - write.sType = VK_STRUCTURE_TYPE_WRITE_DESCRIPTOR_SET; - write.dstSet = descriptor_sets_[i]; - write.dstBinding = 0; - write.descriptorCount = 1; - write.descriptorType = VK_DESCRIPTOR_TYPE_UNIFORM_BUFFER; - write.pBufferInfo = &buf; - vkUpdateDescriptorSets(dev_.device, 1, &write, 0, nullptr); - } - - VkCommandPoolCreateInfo pool{}; - pool.sType = VK_STRUCTURE_TYPE_COMMAND_POOL_CREATE_INFO; - pool.flags = VK_COMMAND_POOL_CREATE_RESET_COMMAND_BUFFER_BIT; - pool.queueFamilyIndex = dev_.queue_family_index; - check_vk(vkCreateCommandPool(dev_.device, &pool, nullptr, &command_pool_), "vkCreateCommandPool"); - - VkCommandBufferAllocateInfo cmd_alloc{}; - cmd_alloc.sType = VK_STRUCTURE_TYPE_COMMAND_BUFFER_ALLOCATE_INFO; - cmd_alloc.commandPool = command_pool_; - cmd_alloc.level = VK_COMMAND_BUFFER_LEVEL_PRIMARY; - cmd_alloc.commandBufferCount = 1; - check_vk(vkAllocateCommandBuffers(dev_.device, &cmd_alloc, &command_buffer_), "vkAllocateCommandBuffers"); - - VkFenceCreateInfo fence_info{}; - fence_info.sType = VK_STRUCTURE_TYPE_FENCE_CREATE_INFO; - check_vk(vkCreateFence(dev_.device, &fence_info, nullptr, &fence_), "vkCreateFence"); } int SceneRenderer::update_scene(const mjModel* model, mjData* data) @@ -550,22 +151,14 @@ int SceneRenderer::update_scene(const mjModel* model, mjData* data) return scene_.ngeom; } -const std::array& SceneRenderer::projection(int view) const -{ - if (view < 0 || static_cast(view) >= config_.view_count) - { - throw std::out_of_range("mujoco_xr: view index out of range"); - } - return projections_[static_cast(view)]; -} - -const ViewTarget& SceneRenderer::view_target(int view) const +std::vector SceneRenderer::frustum(int view) const { if (view < 0 || static_cast(view) >= config_.view_count) { throw std::out_of_range("mujoco_xr: view index out of range"); } - return view_targets_[static_cast(view)]; + const mjvGLCamera& c = cameras_[static_cast(view)]; + return { c.frustum_center, c.frustum_width, c.frustum_bottom, c.frustum_top, c.frustum_near, c.frustum_far }; } void SceneRenderer::render(const std::vector& poses_xyz_qwxyz, const std::vector& fovs_lrud) @@ -574,144 +167,53 @@ void SceneRenderer::render(const std::vector& poses_xyz_qwxyz, const std: if (poses_xyz_qwxyz.size() != n * 7 || fovs_lrud.size() != n * 4) { throw std::invalid_argument( - "mujoco_xr: render() expects view_count*7 pose floats and view_count*4 fov " - "floats; the renderer's view_count must match len(FrameInfo.views)"); + "mujoco_xr: render() expects view_count*7 pose floats and view_count*4 fov floats; the renderer's " + "view_count must match len(FrameInfo.views)"); } - // Per-view uniforms first, so the whole command buffer can be recorded and - // submitted once. - float light[3]; - const float light_len = std::sqrt(kLightDirWorld[0] * kLightDirWorld[0] + kLightDirWorld[1] * kLightDirWorld[1] + - kLightDirWorld[2] * kLightDirWorld[2]); - for (int i = 0; i < 3; ++i) - { - light[i] = kLightDirWorld[i] / light_len; - } + const mjrRect viewport{ 0, 0, static_cast(config_.width), static_cast(config_.height) }; for (size_t v = 0; v < n; ++v) { const float* pose = poses_xyz_qwxyz.data() + v * 7; const float* fov = fovs_lrud.data() + v * 4; - float proj[16]; - float view[16]; - float pv[16]; - proj_from_fov(fov, config_.near_z, config_.far_z, proj); - std::memcpy(projections_[v].data(), proj, sizeof(proj)); - view_from_pose(pose, pose + 3, view); - mat4_mul(pv, proj, view); - EyeUbo ubo{}; - mat4_mul(ubo.viewproj, pv, xr_from_mj_); - ubo.light_dir[0] = light[0]; - ubo.light_dir[1] = light[1]; - ubo.light_dir[2] = light[2]; - ubo.light_dir[3] = 0.0f; - std::memcpy(ubo_mapped_[v], &ubo, sizeof(ubo)); - } - - check_vk(vkResetCommandBuffer(command_buffer_, 0), "vkResetCommandBuffer"); - VkCommandBufferBeginInfo begin{}; - begin.sType = VK_STRUCTURE_TYPE_COMMAND_BUFFER_BEGIN_INFO; - begin.flags = VK_COMMAND_BUFFER_USAGE_ONE_TIME_SUBMIT_BIT; - check_vk(vkBeginCommandBuffer(command_buffer_, &begin), "vkBeginCommandBuffer"); + // The eye crosses into MuJoCo world, not the geometry the other way: + // mjr_render draws MuJoCo world and takes a camera, not a view matrix. + const std::array q_xyzw = { pose[4], pose[5], pose[6], pose[3] }; + const std::array q_mj = mj_from_xr_quat(q_xyzw); + const std::array p_mj = mj_from_xr_pos({ pose[0], pose[1], pose[2] }); + const std::array forward = mj_from_xr_dir(q_mj, kXrForward); + const std::array up = mj_from_xr_dir(q_mj, kXrUp); - for (size_t v = 0; v < n; ++v) - { - // Alpha 0: AR passthrough shows wherever nothing was drawn. - VkClearValue clears[2]{}; - clears[0].color = { { 0.0f, 0.0f, 0.0f, 0.0f } }; - clears[1].depthStencil = { 1.0f, 0 }; + const Frustum f = frustum_from_fov({ fov[0], fov[1], fov[2], fov[3] }, config_.near_z, config_.far_z); - VkRenderPassBeginInfo rp{}; - rp.sType = VK_STRUCTURE_TYPE_RENDER_PASS_BEGIN_INFO; - rp.renderPass = render_pass_; - rp.framebuffer = view_targets_[v].framebuffer(); - rp.renderArea.extent = { config_.width, config_.height }; - rp.clearValueCount = 2; - rp.pClearValues = clears; - vkCmdBeginRenderPass(command_buffer_, &rp, VK_SUBPASS_CONTENTS_INLINE); - - // Standard (non-flipped) viewport: the y flip lives in the projection - // and must not be applied twice. - VkViewport viewport{ 0.0f, 0.0f, static_cast(config_.width), static_cast(config_.height), - 0.0f, 1.0f }; - VkRect2D scissor{ { 0, 0 }, { config_.width, config_.height } }; - vkCmdSetViewport(command_buffer_, 0, 1, &viewport); - vkCmdSetScissor(command_buffer_, 0, 1, &scissor); - - vkCmdBindPipeline(command_buffer_, VK_PIPELINE_BIND_POINT_GRAPHICS, pipeline_); - vkCmdBindDescriptorSets( - command_buffer_, VK_PIPELINE_BIND_POINT_GRAPHICS, pipeline_layout_, 0, 1, &descriptor_sets_[v], 0, nullptr); - const VkDeviceSize zero = 0; - vkCmdBindVertexBuffers(command_buffer_, 0, 1, &vertex_buffer_, &zero); - vkCmdBindIndexBuffer(command_buffer_, index_buffer_, 0, VK_INDEX_TYPE_UINT32); - - for (int i = 0; i < scene_.ngeom; ++i) + mjvGLCamera& cam = cameras_[v]; + cam = mjvGLCamera{}; + for (int i = 0; i < 3; ++i) { - const mjvGeom* g = scene_.geoms + i; - // Meshes only. A plane, sphere or capsule in the scene XML renders - // as nothing -- this is an AR scene and passthrough is the - // background, so there is no ground plane to draw. - if (g->type != mjGEOM_MESH) - { - continue; - } - // dataid = 2*meshid (mesh) or 2*meshid+1 (hull): even only. - if (g->dataid < 0 || (g->dataid & 1) != 0) - { - continue; - } - const int meshid = g->dataid >> 1; - if (meshid >= static_cast(mesh_ranges_.size())) - { - continue; - } - const MeshRange& range = mesh_ranges_[static_cast(meshid)]; - if (range.index_count == 0) - { - continue; - } - - PushConstants pc{}; - // g->mat is row-major; column-major model[c*4 + r] = mat[r*3 + c]. - for (int c = 0; c < 3; ++c) - { - for (int r = 0; r < 3; ++r) - { - pc.model[c * 4 + r] = g->mat[r * 3 + c]; - } - pc.model[c * 4 + 3] = 0; - } - pc.model[12] = g->pos[0]; - pc.model[13] = g->pos[1]; - pc.model[14] = g->pos[2]; - pc.model[15] = 1; - std::memcpy(pc.color, g->rgba, sizeof(pc.color)); - - vkCmdPushConstants(command_buffer_, pipeline_layout_, - VK_SHADER_STAGE_VERTEX_BIT | VK_SHADER_STAGE_FRAGMENT_BIT, 0, sizeof(pc), &pc); - vkCmdDrawIndexed(command_buffer_, range.index_count, 1, range.first_index, range.base_vertex, 0); + cam.pos[i] = static_cast(p_mj[i]); + cam.forward[i] = static_cast(forward[i]); + cam.up[i] = static_cast(up[i]); } + cam.frustum_center = f.center; + cam.frustum_width = f.half_width; + cam.frustum_bottom = f.bottom; + cam.frustum_top = f.top; + cam.frustum_near = f.near_z; + cam.frustum_far = f.far_z; + cam.orthographic = 0; - vkCmdEndRenderPass(command_buffer_); - view_targets_[v].record_readback(command_buffer_); - } + // mjv_updateScene wrote both cameras from mjvCamera; overwrite them + // after it, and both, because mjSTEREO_NONE renders their average. + scene_.camera[0] = cam; + scene_.camera[1] = cam; - check_vk(vkEndCommandBuffer(command_buffer_), "vkEndCommandBuffer"); + mjr_render(viewport, &scene_, &context_); + readback_.capture(static_cast(v), context_.offFBO); + } - check_vk(vkResetFences(dev_.device, 1, &fence_), "vkResetFences"); - VkSubmitInfo submit{}; - submit.sType = VK_STRUCTURE_TYPE_SUBMIT_INFO; - submit.commandBufferCount = 1; - submit.pCommandBuffers = &command_buffer_; - check_vk(vkQueueSubmit(dev_.queue, 1, &submit, fence_), "vkQueueSubmit"); - // Host-side sync rather than an exported timeline semaphore. Coarse, but - // correct and simple: once the fence signals, the readback copies have - // retired and the exported memory is safe for CUDA to read. The - // alternative (a Vulkan->CUDA semaphore) would only buy overlap that a - // single-threaded frame loop cannot use, because - // ProjectionLayer.submit() blocks on cudaStreamSynchronize anyway. - check_vk(vkWaitForFences(dev_.device, 1, &fence_, VK_TRUE, UINT64_MAX), "vkWaitForFences"); + readback_.map(); } } // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/scene_renderer.hpp b/examples/mujoco_xr/cpp/scene_renderer.hpp index 74f2696fe..6f2cf5945 100644 --- a/examples/mujoco_xr/cpp/scene_renderer.hpp +++ b/examples/mujoco_xr/cpp/scene_renderer.hpp @@ -3,38 +3,27 @@ #pragma once -// mjvScene -> Vulkan: meshes only (this is an AR scene, so there is no ground -// plane and passthrough is the background), one pipeline, push constants, one -// directional light, no textures, no shadows, no sorting. +// MuJoCo's own renderer, driven once per eye into an offscreen framebuffer that +// Readback turns into CUDA pointers. Shading, materials, lights and shadows all +// stay MuJoCo's -- none of it lives here. // -// View and projection come from the per-view pose + fov in FrameInfo.views; -// mjvGLCamera is bypassed. mjvCamera exists only to give mjv_updateScene a -// viewpoint for culling/LOD, and is a central free camera so one eye's frustum -// cannot cull geometry out of the other's. +// The OpenGL context is NOT created here: `mujoco.GLContext` makes it, and it +// must be current on this thread and on viz's GPU before the constructor runs. +// The constructor checks the GPU rather than render into another card's memory. // // C++ owns mjvScene/mjvOption/mjvCamera; Python owns mjModel/mjData/mj_step. -// render() must run on the mj_step thread, after it, and treats mjData as -// const. Threading the frame loop breaks this silently: geometry one step -// stale reads as jitter, not as a race. +// render() runs on the mj_step thread, after it, and treats mjData as const. -#include "mesh_buffers.hpp" -#include "render_target.hpp" +#include "gl_readback.hpp" #include -#include -#include #include #include namespace mujoco_xr { -// Column-major Vulkan-convention projection for one asymmetric fov -// (angle_left, angle_right, angle_up, angle_down, radians). Free function so a -// test can pin the clip convention without a GPU or a VizSession. -std::array projection_from_fov(const std::array& fov_lrud, float near_z, float far_z); - class SceneRenderer { public: @@ -42,21 +31,16 @@ class SceneRenderer { uint32_t width = 0; uint32_t height = 0; - // Stereo only. Kept as a field because the render loops and per-view - // resources read it, not because mono is supported. + // A field because the render loop reads it, not because mono works. uint32_t view_count = 2; - // Single-sourced by the Python app and passed in: the SAME pair also - // goes into VizSessionConfig.xr_near_z / xr_far_z and therefore into - // XrCompositionLayerDepthInfoKHR. There is no default and no literal - // anywhere in this module, because a drift between the depth we encode - // and the range the runtime is told makes compositor reprojection - // wrong, and the symptom (world-locked geometry swimming under head - // motion) is only visible on hardware. + // No default, and no near/far literal anywhere in cpp/: this pair must + // equal VizSessionConfig.xr_near_z / xr_far_z, or the runtime reprojects + // the submitted depth against the wrong range. float near_z = 0.0f; float far_z = 0.0f; }; - SceneRenderer(const BorrowedDevice& dev, const Config& config, const mjModel* model); + SceneRenderer(const Config& config, const mjModel* model); ~SceneRenderer(); SceneRenderer(const SceneRenderer&) = delete; @@ -65,21 +49,20 @@ class SceneRenderer // mjv_updateScene, exactly once per frame. Returns the geom count. int update_scene(const mjModel* model, mjData* data); - // Renders every view in one queue submit and blocks until the readback - // copies have retired, so the CUDA pointers are safe to hand to - // ProjectionLayer.submit() the moment this returns. - // - // poses_xyz_qwxyz: view_count * 7 floats -- position (x, y, z) then - // orientation (w, x, y, z), matching viz.Pose3D's spelling. - // fovs_lrud: view_count * 4 floats -- angle_left, angle_right, angle_up, - // angle_down, in radians, matching viz.Fov's field order. + // Draws every view and leaves the CUDA pointers mapped, ready for + // ProjectionLayer.submit() the moment this returns. `poses_xyz_qwxyz` is + // view_count*7 (position then w,x,y,z, viz.Pose3D's spelling) and + // `fovs_lrud` view_count*4 radians in viz.Fov's field order. void render(const std::vector& poses_xyz_qwxyz, const std::vector& fovs_lrud); - // The column-major projection used for `view` on the last render(), so the - // app can assert the clip convention per frame. - const std::array& projection(int view) const; + // Last render()'s frustum for `view`, as (center, half_width, bottom, top, + // near, far), so the app can assert the convention per frame. + std::vector frustum(int view) const; - const ViewTarget& view_target(int view) const; + const Readback& readback() const + { + return readback_; + } uint32_t view_count() const { return config_.view_count; @@ -94,42 +77,19 @@ class SceneRenderer } private: - void create_pipeline(); - void upload_geometry(const mjModel* model); - void create_uniforms(); void destroy(); - BorrowedDevice dev_; Config config_; - - VkRenderPass render_pass_ = VK_NULL_HANDLE; - VkDescriptorSetLayout dsl_ = VK_NULL_HANDLE; - VkDescriptorPool descriptor_pool_ = VK_NULL_HANDLE; - VkPipelineLayout pipeline_layout_ = VK_NULL_HANDLE; - VkPipeline pipeline_ = VK_NULL_HANDLE; - VkCommandPool command_pool_ = VK_NULL_HANDLE; - VkCommandBuffer command_buffer_ = VK_NULL_HANDLE; - VkFence fence_ = VK_NULL_HANDLE; - - std::vector descriptor_sets_; - std::vector ubos_; - std::vector ubo_memory_; - std::vector ubo_mapped_; - std::vector view_targets_; - std::vector> projections_; - - VkBuffer vertex_buffer_ = VK_NULL_HANDLE; - VkDeviceMemory vertex_memory_ = VK_NULL_HANDLE; - VkBuffer index_buffer_ = VK_NULL_HANDLE; - VkDeviceMemory index_memory_ = VK_NULL_HANDLE; - - std::vector mesh_ranges_; - float xr_from_mj_[16] = { 0 }; + Readback readback_; mjvScene scene_{}; mjvOption scene_option_{}; mjvCamera camera_{}; + mjrContext context_{}; bool scene_made_ = false; + bool context_made_ = false; + + std::vector cameras_; }; } // namespace mujoco_xr diff --git a/examples/mujoco_xr/cpp/shaders/scene.frag b/examples/mujoco_xr/cpp/shaders/scene.frag deleted file mode 100644 index 4a8f9adb3..000000000 --- a/examples/mujoco_xr/cpp/shaders/scene.frag +++ /dev/null @@ -1,48 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -// Not covered by clang_format_check or the REUSE / copyright-year hooks: -// `.frag` is in neither cmake/ClangFormat.cmake's pattern list nor the -// `files:` regex in .pre-commit-config.yaml. So the SPDX lines above are -// hand-written and must be kept by hand. -// -// Deliberately NOT run through clang-format: it is a C++ formatter and it -// mangles GLSL layout blocks. `clang-format-14 --dry-run -Werror` fails on -// src/viz/shaders/cpp/textured_quad.vert too -- hand-formatted GLSL is the -// established convention here, not an oversight. Since no tool will ever -// arbitrate the shape of these files, this one follows that same precedent -// BY HAND: 4-space indent, opening braces on their own line. -// -// Half-lambert with one hardcoded directional light. Alpha passes through -// straight (unpremultiplied): the background clears to alpha 0 so AR -// passthrough shows behind the scene. - -#version 450 - -layout(location = 0) in vec3 v_normal_w; - -layout(set = 0, binding = 0) uniform Eye -{ - mat4 viewproj; - vec4 light_dir; -} eye; - -// Must match scene.vert's block exactly: both stages share one push-constant -// range, so a field here that the vertex shader does not have shifts `color` -// to an offset the host never wrote. -layout(push_constant) uniform PC -{ - mat4 model; - vec4 color; -} pc; - -layout(location = 0) out vec4 out_color; - -void main() -{ - vec3 n = normalize(v_normal_w); - vec3 l = normalize(-eye.light_dir.xyz); - float diff = max(dot(n, l), 0.0); - const float ambient = 0.35; - out_color = vec4(pc.color.rgb * (ambient + (1.0 - ambient) * diff), pc.color.a); -} diff --git a/examples/mujoco_xr/cpp/shaders/scene.vert b/examples/mujoco_xr/cpp/shaders/scene.vert deleted file mode 100644 index 8bf3e3374..000000000 --- a/examples/mujoco_xr/cpp/shaders/scene.vert +++ /dev/null @@ -1,49 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -// Not covered by clang_format_check or the REUSE / copyright-year hooks: -// `.vert` is in neither cmake/ClangFormat.cmake's pattern list nor the -// `files:` regex in .pre-commit-config.yaml. So the SPDX lines above are -// hand-written and must be kept by hand. -// -// Deliberately NOT run through clang-format: it is a C++ formatter and it -// mangles GLSL layout blocks. `clang-format-14 --dry-run -Werror` fails on -// src/viz/shaders/cpp/textured_quad.vert too -- hand-formatted GLSL is the -// established convention here, not an oversight. Since no tool will ever -// arbitrate the shape of these files, this one follows that same precedent -// BY HAND: 4-space indent, opening braces on their own line. -// -// MuJoCo XR scene shader: one pipeline, meshes only. Geometry is in MuJoCo -// world space; eye.viewproj already folds in xr_from_mj and the per-view -// pose/fov handed over by viz (mjvGLCamera is bypassed by design). - -#version 450 - -layout(location = 0) in vec3 in_pos; -layout(location = 1) in vec3 in_normal; - -layout(set = 0, binding = 0) uniform Eye -{ - mat4 viewproj; // P * V * xr_from_mj - vec4 light_dir; // world-space travel direction of the one light -} eye; - -layout(push_constant) uniform PC -{ - mat4 model; // world from geom-local (rotation, translation) - vec4 color; -} pc; - -// The ONLY varying. The fragment shader lights with a directional light, which -// needs no world position -- do not add one back "for future point lights" -// until there is a point light. -layout(location = 0) out vec3 v_normal_w; - -void main() -{ - vec4 pw = pc.model * vec4(in_pos, 1.0); - // model's upper 3x3 is a pure rotation (mjvGeom.mat, no scale), so it is - // its own inverse-transpose and needs no separate normal matrix. - v_normal_w = mat3(pc.model) * in_normal; - gl_Position = eye.viewproj * pw; -} diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__init__.py b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__init__.py index c78ab93b6..8ce4f1cde 100644 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__init__.py +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/__init__.py @@ -3,12 +3,18 @@ """MuJoCo scene rendered into an Isaac Teleop Televiz XR session.""" -# Load order is load-bearing -- do not let an import sorter move this. -# `import mujoco` pulls the wheel's libmujoco into the process first, and -# `_mujoco_xr` carries a NEEDED entry for that same versioned SONAME with no -# RPATH, so it binds to the already-loaded library. That is what guarantees one -# libmujoco, and so that the mjModel*/mjData* addresses Python hands the -# renderer match the layout it was compiled against. +import os as _os + +# Must precede `import mujoco`, which reads MUJOCO_GL at import time. EGL rather +# than the GLFW default because this renders offscreen, usually with no display, +# and only the EGL path honours MUJOCO_EGL_DEVICE_ID. setdefault, so an explicit +# MUJOCO_GL still wins. +_os.environ.setdefault("MUJOCO_GL", "egl") + +# Load order is load-bearing -- do not let an import sorter move this. `import +# mujoco` pulls the wheel's libmujoco in first, and `_mujoco_xr` has a NEEDED +# entry for that same SONAME with no RPATH, so it binds to the already-loaded +# copy. That is what guarantees one libmujoco, and so one mjModel* layout. import mujoco as _mujoco from . import _mujoco_xr diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py index f26244f6a..8d9db841f 100644 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py @@ -4,22 +4,24 @@ """A MuJoCo scene drawn into a Televiz XR session. One OpenXR session shared between VizSession (rendering) and TeleopSession -(input); the scene is drawn by Vulkan into images viz owns and reaches +(input); the scene is drawn by MuJoCo's own renderer and reaches ProjectionLayer.submit() by CUDA pointer, never through host memory. VizSession(kXr) ──get_oxr_handles()──▶ TeleopSession │ │ - │ vk_device / vk_physical_device │ controller grip poses + │ recommended resolution │ controller grip poses ▼ ▼ │ _mujoco_xr.Renderer ──__cuda_array_interface__──▶ ProjectionLayer │ ▲ │ └──────────────── mjData.mocap_pos/_quat ◀─────────────────────┘ -C++ owns mjvScene/mjvOption/mjvCamera; Python owns mjModel/mjData/mj_step, so -everything reading a controller and writing mjData is testable without a GPU. +The renderer needs an OpenGL context current on this thread, made below and torn +down after it; viz and the renderer meet through CUDA alone, on VizSession's GPU. -Frame order is load-bearing: input is sampled before the physics it feeds, on -every frame that will step or draw. +C++ owns mjvScene/mjvOption/mjvCamera/mjrContext; Python owns +mjModel/mjData/mj_step, so everything reading a controller and writing mjData is +testable without a GPU. Frame order is load-bearing: input is sampled before the +physics it feeds, on every frame that will step or draw. """ from __future__ import annotations @@ -216,37 +218,39 @@ def _flatten_xr_views(info) -> tuple[list[float], list[float]]: return poses, fovs -def _assert_projection(p: list[float], near: float, far: float) -> None: - """Per-frame, because the projection is rebuilt from per-frame fov. +def _assert_frustum(f: list[float], fov, near: float, far: float) -> None: + """The frustum handed to mjvGLCamera, checked against the fov it came from. - `p` is column-major. Depth is asserted as the shipped contract - (near -> 0, far -> 1); two viz doc comments claim reverse-Z, the code is - standard Z. + `f` is (center, half_width, bottom, top, near, far). The projection's shape + is MuJoCo's business; which numbers reach it is this app's. """ - p00, p11, p23 = p[0], p[5], p[11] - assert p00 > 0.0, ( - f"P[0][0]={p00}: left/right swapped, or a zeroed Fov reached the projection" - ) - # The load-bearing one: b = n*tan(angleUp) > 0 and t = n*tan(angleDown) < 0 - # give 2n/(t-b) < 0. That negative is the Y flip, which drives triangle - # winding -- a depth-range check touches only P[2][2] / P[2][3] / P[3][2] - # and would not notice it going positive. - assert p11 < 0.0, ( - f"P[1][1]={p11}: the angleUp->bottom Y flip is gone; winding will invert" + center, half_width, bottom, top, f_near, f_far = f + + # At zero half_width mjr_render derives the horizontal extent from the + # viewport aspect, rendering something plausible from a fov carrying nothing. + assert half_width > 0.0 and top > bottom, ( + f"degenerate frustum {f}: a zeroed Fov reached the camera" ) - assert abs(p23 + 1.0) < 1e-6, f"P[2][3]={p23}: not a standard perspective divide" - - # Asserted as the contract we ship rather than as somebody else's formula, - # so it survives a viz refactor. - for z_view, expected in ((-near, 0.0), (-far, 1.0)): - clip_z = p[10] * z_view + p[14] - clip_w = p[11] * z_view + p[15] - assert abs(clip_z / clip_w - expected) < 1e-4, ( - f"depth encoding broken: z_view={z_view} maps to {clip_z / clip_w}, expected {expected}" + # float32 tolerances throughout: the frustum crosses as C floats, so an + # exact comparison against a Python float fails on rounding alone. + for name, got, want in ( + ("left", center - half_width, near * math.tan(fov.angle_left)), + ("right", center + half_width, near * math.tan(fov.angle_right)), + ("bottom", bottom, near * math.tan(fov.angle_down)), + ("top", top, near * math.tan(fov.angle_up)), + ): + assert abs(got - want) <= 1e-6 * max(1.0, abs(want)), ( + f"frustum {name}={got}, expected {want}" ) + # viz's XrCompositionLayerDepthInfoKHR pair must be the encoding pair, or + # the runtime reprojects against the wrong range. + assert abs(f_near - near) <= 1e-6 * near and abs(f_far - far) <= 1e-6 * far, ( + f"clip planes drifted: camera has ({f_near}, {f_far}), viz was told ({near}, {far})" + ) + -def _log_startup(resolution) -> None: +def _log_startup(resolution, gl_backend: str) -> None: """One block naming every assumption that is invisible at runtime.""" try: version = importlib.metadata.version("isaacteleop") @@ -271,6 +275,11 @@ def _log_startup(resolution) -> None: resolution.width, resolution.height, ) + LOG.info( + "renderer: MuJoCo's own (mjr_render), OpenGL backend %s, offsamples=0. Its output is blitted, " + "y-flipped and depth-inverted, then read back into a pixel-pack buffer CUDA imports -- no host copy.", + gl_backend, + ) LOG.info( "clip: near=%.4f far=%.2f (one pair -> VizSessionConfig, projection, submitted depth)", NEAR_Z, @@ -408,6 +417,7 @@ def run() -> int: viz_session = viz.VizSession.create(config) renderer = None + gl_context = None try: resolution = viz_session.get_recommended_resolution() @@ -419,10 +429,17 @@ def run() -> int: layer_config.stereo = _VIEW_COUNT == 2 layer = viz_session.add_projection_layer(layer_config) + # After VizSession.create, which cudaSetDevice's the GPU behind its + # Vulkan device; the renderer checks this context landed on that one. + gl_context = mujoco.GLContext(resolution.width, resolution.height) + gl_context.make_current() + + # MuJoCo resolves multisample renderbuffers only inside mjr_readPixels, + # which this path never calls, and a multisample source cannot be + # blitted with a y flip in one step. + model.vis.quality.offsamples = 0 + renderer = _mujoco_xr.Renderer( - vk_physical_device=viz_session.vk_physical_device, - vk_device=viz_session.vk_device, - vk_queue_family_index=viz_session.vk_queue_family_index, width=resolution.width, height=resolution.height, view_count=_VIEW_COUNT, @@ -431,7 +448,7 @@ def run() -> int: model_address=model._address, ) - _log_startup(resolution) + _log_startup(resolution, type(gl_context).__module__) # After the startup block, so its line reads as part of the same report. ghost = _resolve_ghost(model) @@ -452,16 +469,18 @@ def run() -> int: teleop_config = TeleopSessionConfig( app_name="MuJoCoXR", pipeline=pipeline, - # Never pass trackers=: TeleopSession discovers them from the - # pipeline graph, and passing them again duplicates the set. + # Never pass trackers=: TeleopSession discovers them from the graph, + # and passing them again duplicates the set. oxr_handles=OpenXRSessionHandles(*oxr), ) with TeleopSession(teleop_config) as teleop_session: _loop(viz_session, layer, renderer, model, data, teleop_session, ghost) finally: - # The renderer borrows viz_session's device: it must go first. + # Innermost first: the renderer's GL objects need a current context. if renderer is not None: renderer.close() + if gl_context is not None: + gl_context.free() viz_session.destroy() return 0 @@ -469,27 +488,25 @@ def run() -> int: def _loop(viz_session, layer, renderer, model, data, teleop_session, ghost) -> None: view_count = renderer.view_count previous_clock: float | None = None - # Fixed-step accumulator. NOT reset or drained on a non-render frame: the - # simulation owes that time regardless of whether anything was displayed. + # NOT reset or drained on a non-render frame: the simulation owes that time + # whether or not anything was displayed. accumulator = 0.0 - checked_projection = False + checked_frustum = False while not viz_session.should_close(): info = viz_session.begin_frame() try: - # None means "this frame carries no usable timestamp" -- skip the - # sample entirely rather than recording a zero. See _frame_clock. + # None means no usable timestamp -- skip the sample, never record 0. now = _frame_clock(info) if now is not None: if previous_clock is not None: accumulator += _clamp_dt(now - previous_clock) previous_clock = now - # Input above the should_render gate and above the step loop, so - # it precedes the physics it feeds. Gated on "will step or will - # draw" rather than every frame: an ungated teleop_session.step() - # calls xrSyncActions on the unthrottled pre-kRunning burst, which - # is hundreds of frames in milliseconds. + # Above both the should_render gate and the step loop, so it + # precedes the physics it feeds. Gated rather than every frame: an + # ungated step() calls xrSyncActions on the unthrottled + # pre-kRunning burst, hundreds of frames in milliseconds. result = None will_step = accumulator >= model.opt.timestep if will_step or info.should_render: @@ -516,21 +533,23 @@ def _loop(viz_session, layer, renderer, model, data, teleop_session, ghost) -> N "cpp/scene_renderer.cpp." ) - # A view-count mismatch is rejected by render() below, which sees - # the flattened lengths and says so in those terms. There is - # deliberately no second check here. + # No view-count check here: render() sees the flattened lengths and + # rejects a mismatch in those terms. poses, fovs = _flatten_xr_views(info) renderer.render(poses, fovs) - # First rendered frame only: the fov changes per frame but the clip - # convention does not, and tests/test_projection.py pins it headless. - if not checked_projection: + # First rendered frame only: the fov changes per frame, the + # convention does not. + if not checked_frustum: for view in range(view_count): - _assert_projection(renderer.projection(view), NEAR_Z, FAR_Z) + _assert_frustum( + renderer.frustum(view), info.views[view].fov, NEAR_Z, FAR_Z + ) LOG.info( - "projection convention verified on the first rendered frame (P[1][1] < 0, near->0, far->1)" + "frustum verified on the first rendered frame (matches FrameInfo fov, clip planes agree " + "with VizSessionConfig)" ) - checked_projection = True + checked_frustum = True layer.submit( renderer.color(0), diff --git a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml index e3897e8f9..e5c7baeb4 100644 --- a/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/scene.xml @@ -7,20 +7,14 @@ The shipped scene: the SO-101 leader gripper ghost, and nothing else. No ground plane and no static furniture -- this is an AR scene and passthrough is the background. -What a ghost-only scene cannot show: the ghost is placed from the controller -pose through mj_from_xr and rendered back through xr_from_mj, so both -cpp/frames.hpp constants cancel and it lands on the hand whatever they are. Only -static content is placed by them, so adding any here puts kTransMjFromXr under -test -- its z = -0.73 claims MuJoCo z = 0 is a work surface 0.73 m above the -floor. - -The renderer draws mjGEOM_MESH only, so a box, sphere or capsule added here -renders as nothing. +What a ghost-only scene cannot show: cpp/frames.hpp's constants place the ghost +and the eye pose alike, so they cancel and it lands on the hand whatever they +are. Only static content is placed by them, so adding any here puts +kTransMjFromXr under test. --> - +