diff --git a/.gitignore b/.gitignore index f3d14020a..acff933ac 100644 --- a/.gitignore +++ b/.gitignore @@ -55,6 +55,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 @@ -65,3 +68,14 @@ deps/v2d/wheels/ # Runtime device files injected by MCP server infrastructure .mcp.json + +# SO-101 leader-gripper assets, fetched by +# examples/mujoco_xr/scripts/fetch-so-arm.sh into the package's own assets +# directory (which is what makes them package data), so only the authored +# wrapper XML beside them 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 f639afbf8..ab2248ca8 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 1cf89b2bb..a37968f47 100644 --- a/docs/source/getting_started/build_from_source/index.rst +++ b/docs/source/getting_started/build_from_source/index.rst @@ -25,6 +25,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..ecfdfcd32 --- /dev/null +++ b/examples/mujoco_xr/CMakeLists.txt @@ -0,0 +1,209 @@ +# 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; cpp/ +# builds the pybind11 module and tests/ registers the ctest entries. +# +# Configured two ways, and the branches below turn on which: +# +# IN-TREE add_subdirectory'd from the root. Builds the extension in place +# so ctest (and a bare `pytest`) import it from the source tree. +# Installs nothing. +# STANDALONE this directory as the top-level project, which scikit-build-core +# drives for `uv pip install ./examples/mujoco_xr`. Sets up for +# itself the three things root scope provided: Python3_EXECUTABLE, +# pybind11, and the extension's output location. +# +# 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() + +# The version is not hardcoded here: the pyproject.toml files are read and +# cross-checked against the installed version, so a drift fails the configure +# rather than surfacing as an ImportError on the headset. +# +# MATCHALL rather than MATCH because pyproject.toml carries the pin twice +# (build-system.requires and project.dependencies) and those must agree. +# Corollary: a `mujoco==` written in prose would be matched too, which +# is why those comments say "the pin below" instead of restating 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) + +# Compare the pins to each other before the probe. The cross-check below is +# transitive, but only on a machine that has mujoco; without one the skip +# message quotes pin[0] alone, so a disagreeing pin would cost a round trip. +# +# Deduplicated into a copy: _mujoco_pins itself must keep one entry per pin, or +# the ZIP_LISTS cross-check below 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 the first symptom would be an + # ImportError with nothing in the install output to explain it. Reaching + # here means build isolation was turned off without supplying mujoco. + 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 this is the one message standing between a green build and an + # example that was silently not compiled, so it names the exact command. That + # interpreter is created BY configure, hence the re-configure 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) + +# Runs only after a successful probe, so a machine with no mujoco gets the skip +# message above rather than a pin complaint. Every pin matters because the +# module compiles against the version installed here and links its versioned +# SONAME, while the pyproject pins decide what an isolated 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 + # (3.11.0rc1, 3.11.0.post1) and VERSION_EQUAL discards them, so it reports + # EQUAL for exactly the strings the regex lets through. 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 and underscore-prefixed on purpose: hand-set directory variables read +# by cpp/CMakeLists.txt through inherited scope, not find_package output, which +# MUJOCO_LIBRARY / MUJOCO_INCLUDE_DIR would read as. +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 because cpp/ cannot use ${CMAKE_SOURCE_DIR} (the repo +# root in-tree, this directory standalone) and rule 2 forbids "../" in CMake +# paths. +set(_mujoco_xr_root "${CMAKE_CURRENT_SOURCE_DIR}") + +add_subdirectory(cpp) + +# BUILD_TESTING is a root option and is undefined standalone, so the ctest +# entries are in-tree only -- which is right: they run against the in-place +# extension and the repo's built isaacteleop, neither of which exists in a wheel +# build. +if(BUILD_TESTING) + add_subdirectory(tests) +endif() + +# No install() rules here, deliberately. Nothing from this example is written to +# install/examples/mujoco_xr/; the wheel is the only run path. Standalone, +# cpp/CMakeLists.txt's own install(TARGETS) is what puts the extension into the +# wheel. 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..f99e6311d --- /dev/null +++ b/examples/mujoco_xr/cpp/CMakeLists.txt @@ -0,0 +1,148 @@ +# 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. The package's __init__.py + # imports `mujoco` before this extension, so the wheel's already-loaded + # library satisfies our NEEDED entry. An RPATH pointing at the build + # interpreter's wheel would silently load a second libmujoco and hand + # mjModel* pointers 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 the `isaacteleop_examples/` + # half and the .so lands outside the package. + # + # The only owner of a build-produced file in the wheel. What keeps the + # package copy from also shipping a stale in-place .so is `sdist.exclude` in + # ../pyproject.toml. The breakage is intermittent: a same-ABI wheel collides + # by name and looks fine, only a cross-ABI build ships two. + install(TARGETS mujoco_xr_py + LIBRARY DESTINATION isaacteleop_examples/mujoco_xr + ) +else() + # In-tree: drop the .so next to __init__.py so the package imports straight + # from the source tree, which is what tests/conftest.py reaches by + # prepending python/ to sys.path (same shape as + # examples/camera_viz/codec/CMakeLists.txt). + # + # ${_mujoco_xr_root} rather than ${CMAKE_SOURCE_DIR}, which is this example's + # own directory in the standalone configure; rule 2 forbids "../" here. + # Covered by .gitignore's repo-wide `*.so`. This copies and never removes, so + # a renamed module leaves a stale .so until the tree is cleaned. + 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..93eb35ebb --- /dev/null +++ b/examples/mujoco_xr/cpp/frames.hpp @@ -0,0 +1,106 @@ +// 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, declared and converted here once; +// the Python side reaches these through the pybind module rather than +// re-deriving them. Rules: +// +// - Quaternions cross as xyzw (OpenXR, and so Teleop's GRIP_ORIENTATION); +// MuJoCo is wxyz and viz::Pose3D is a third spelling. Reorder on every +// crossing, name every variable q_xyzw or q_wxyz, and fill boundary structs +// field by field. +// - R_mj_from_xr = Rz(-90deg) * Rx(+90deg). Axis map: XR -Z -> MJ +x, +// XR +Y -> MJ +z, XR +X -> MJ -y. tests/test_frames.py pins it as: a point +// 1 m in front of the operator at eye height h lands at MuJoCo (+1, 0, h) +// before the workspace translation. +// - p_mj = R * p_xr + t; q_mj = q_mj_from_xr (x) q_xr. +// +// Direction reads by adjacency: p_mj = mj_from_xr_pos(p_xr). Use that `_from_` +// form, 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, placing static scene content only: +// the ghost goes out through mj_from_xr and back through xr_from_mj, so this +// cancels on it and the shipped ghost-only scene shows nothing of it. Two +// independent terms, and zeroing either is a bug: +// x = -1.0 operator standoff: the robot base sits ~1 m in front of the +// operator. Unaffected by the reference space. +// z = -0.73 floor datum: MuJoCo z=0 is a work surface 0.73 m above the floor. +// Correct only against a floor-origin reference space, which the +// session does not ask for -- viz's default origin is the headset's +// start pose. A scene that adds static content owns re-tuning this +// for the origin it actually gets. +inline constexpr std::array kTransMjFromXr = { -1.0, 0.0, -0.73 }; + +// XR quaternion (xyzw) -> MuJoCo world quaternion (wxyz). The ONLY quaternion +// crossing in the app; everything else calls this. +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..70ac88afc --- /dev/null +++ b/examples/mujoco_xr/pyproject.toml @@ -0,0 +1,99 @@ +# 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 +# +# A separate wheel rather than part of `isaacteleop` because _mujoco_xr links +# libmujoco: folded in, the isaacteleop wheel's contents would depend on whether +# the build host happened to have mujoco installed. `install_python_example()` +# cannot be used -- it copies a pure-Python tree and cannot ship a compiled, +# ABI-tagged extension. + +[build-system] +requires = [ + "scikit-build-core>=1.0", + # Not pinned to the pybind11 the root build FetchContents: no + # pybind11-registered type crosses this module's boundary, so the two builds + # share no ABI and need not agree. + "pybind11>=2.12", + # Build-time mujoco, and not optional: CMakeLists.txt compiles the extension + # against this wheel's headers and links its versioned SONAME. Without it a + # PEP-517 isolated build emits a wheel with no extension. + # + # Same version as `dependencies` below, always. CMakeLists.txt matches every + # `mujoco==` in this file, so do not restate the number in prose here -- say + # "the pin below" or a comment edit becomes a configure failure. + "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 an interpreter pin: scikit-build-core tags the wheel with the +# installing interpreter's ABI, so packaging metadata enforces compatibility. +# 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 that is a live hazard: a published isaacteleop exists on + # PyPI, so this resolves happily against a release that is not the viz in + # this checkout. 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..7e5292029 --- /dev/null +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/app.py @@ -0,0 +1,603 @@ +# 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 only clip planes in the app. VizSessionConfig, the renderer's projection +# and the submitted depth must all agree; drift makes world-locked geometry +# swim under head motion, visible only on a headset. No near/far literal exists +# 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 + +# kXr is the only mode: this app needs a headset and a CloudXR runtime. A +# headless fallback should arrive with the CI job that runs it +# (NVIDIA/IsaacTeleop#880), not before. +_DISPLAY_MODE = viz.DisplayMode.kXr + +# Stereo, always. 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 identically from the wheel and the source tree. +# Must stay absolute: scene.xml s a fragment in a subdirectory, and on +# mujoco 3.11.0 a relative model path mis-composes that fragment's mesh paths +# and fails naming a file that is right there on disk. +DEFAULT_SCENE = Path(__file__).parent / "assets" / "scene.xml" + +# The meshes scene.xml s, fetched rather than vendored (see +# scripts/fetch-so-arm.sh). Checked by name before MuJoCo sees the scene, because +# MjModel.from_xml_path's 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 leader gripper, and the left +# controller draws nothing. +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. + + Right-multiplication is what makes it intrinsic, and is the convention + MuJoCo's `euler=` uses. Spelled out rather than calling mju_euler2Quat so + the sequence is visible at the point of use. + """ + 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]. + + Spelled as 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. + + The retargeter is a BaseRetargeter node in the pipeline rather than a + library call beside it. The shipped scene has no robot, so the jaw it drives + is the operator's own trigger; the SO-101 arrives with the scene catalogue and + reads the same output. + """ + 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. + + Filled field by field, never sliced: viz.Pose3D.orientation is (w,x,y,z) + and 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) + # Cross-example venv collisions are real: several examples here ship their + # own .venv, and picking up the wrong isaacteleop is invisible otherwise. + 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 two mocap rows the ghost writes, resolved once at startup. + + Mocap indices, not body ids: mocap_pos/mocap_quat are indexed 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 leaves the grip pose at + (0, 0, 0), which is the MuJoCo scene origin and a place a legitimate pose + could put it. Freezing where it was last seen is the honest rendering of + "tracking lost", so there is no else branch. + + _QUAT_GRIP_FROM_GHOST right-multiplies because it is fixed in the gripper's + own 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 + + # Closedness comes through the pipeline output, so the deadzone and clamp + # are the retargeter's contract rather than this app's. Rotated ABOUT the + # hinge, not placed at it: the jaw body'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) + + # Where the jaw body's origin lands: 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 together with should_render on every + frame before kRunning. Sampling it would make the next real frame compute + dt from 0 and step 50 times inside one display frame. The caller must skip + the sample and leave the accumulator alone. + """ + 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: build the pipeline, aggregate the OpenXR extensions + # its trackers need, put them on the VizSessionConfig, and only then create + # the session. VizSession calls xrCreateInstance, so an extension discovered + # later cannot be added -- and a controller tracker without + # 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". Whether it is honoured is the + # runtime's call: viz only sets the source-alpha blend bit for a non-opaque + # environment, so a VR headset composites black instead. Accepted -- this + # example targets passthrough, and black 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: + # Skip the draw. Deliberately do NOT touch the accumulator. + continue + + renderer.update_scene(model._address, data._address) + # The only check on mjv_updateScene filling mjvScene. Measured on + # mujoco 3.11.0: it prints "WARNING: Pre-allocated visual geom + # buffer is full" on stderr, truncates, and returns normally with + # ngeom == maxgeom, and nobody reads a warning line 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: + # end_frame() 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, which starts a runtime process on entry and tears + # it down on exit: checked here, an unfetched checkout says so plainly + # instead of landing 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..a7cc98c70 --- /dev/null +++ b/examples/mujoco_xr/python/isaacteleop_examples/mujoco_xr/assets/leader/leader_gripper.xml @@ -0,0 +1,122 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..adc98916f --- /dev/null +++ b/examples/mujoco_xr/scripts/fetch-so-arm.sh @@ -0,0 +1,64 @@ +#!/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 this example draws, rather than +# vendoring 2.3 MB of binary STL that Git LFS made every clone pay for. +# +# Nothing calls this at build time, deliberately: an isolated PEP-517 wheel +# build must not reach the network, so this is an explicit step and the app +# fails at startup naming this script. +# +# Fetch, then install. The files land in package data, so they only reach +# site-packages on the next `uv pip install ./examples/mujoco_xr` -- skip the +# reinstall and the ghost works from the source tree and fails from the wheel. +set -euo pipefail + +# The pin. Everything below is reproducible from this one line; bump it and the +# checksums together or the script refuses the download. +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 not decoration: it is where app.py's trigger hinge comes from (the +# `gripper` joint's origin and axis), 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..6ca92b9d8 --- /dev/null +++ b/examples/mujoco_xr/tests/test_app_helpers.py @@ -0,0 +1,87 @@ +# 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 is spelled with comparisons. + + ``min(max(nan, 0), 0.1)`` returns nan -- NaN passes through BOTH limits and + reaches mj_step, which then poisons every qpos in the model. The comparison + form sends it to 0 because ``nan > 0`` is False. + """ + 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`` sets ``should_render = false`` AND + ``predicted_display_time = 0`` together, on every frame before the session + reaches kRunning. Sampling that zero as a clock reading makes the next real + frame compute ``dt = t_now - 0``, which clamps to MAX_DT_S and steps the + simulation 0.1 s inside a single display frame. _frame_clock must report + "no sample here" instead, so the caller can skip it. + """ + + 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..e7f9be876 --- /dev/null +++ b/examples/mujoco_xr/tests/test_frames.py @@ -0,0 +1,105 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Frame-convention tests for the XR -> MuJoCo crossing. + +These pin the two things that are cheap to get wrong and expensive to debug on +hardware: the handedness map, and the quaternion component order. +""" + +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, XR +Y -> MJ +z, XR +X -> MJ -y. + + Checked on the rotation alone by subtracting the workspace translation, so + this test keeps passing if the calibration is re-measured. + """ + 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): + """The definition in frames.hpp, 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: they are independent. + + x is operator standoff (reference-space independent); z is the floor datum + (only meaningful when the reference-space origin is on the floor). + """ + 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. + + ``mj_from_xr_quat`` composes on the LEFT: R_mj = R_conv @ R_xr, so the + result still consumes body-local axes and produces MuJoCo world axes. The + body's local +x, rolled 90 degrees about XR +Z, points along XR +Y, and + XR +Y maps to MuJoCo +z. + + NINETY degrees, not 180, and that matters: a 180-degree roll about XR +Z is + spelled (0, 0, 1, 0), which read as wxyz is a 180-degree roll about XR +Y, + and BOTH send local +x to MuJoCo +y. Such a probe passes whichever way the + binding reads its input and proves nothing. The second half of this test + pins that the probe chosen here 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..c9006dcd3 --- /dev/null +++ b/examples/mujoco_xr/tests/test_ghost.py @@ -0,0 +1,522 @@ +# 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 default scene, which is the one that includes the ghost. + + Skips on an unfetched checkout: the meshes come from + scripts/fetch-so-arm.sh, and saying so beats an "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 pipeline step result, stubbed. ``app._update_ghost`` reads exactly three +# fields through the mapping protocol, and supplying them here rather than +# standing up a TeleopSession 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 operator's hand shove scene content around, + # which is the opposite of an overlay. + assert model.geom_contype[gid] == 0 + assert model.geom_conaffinity[gid] == 0 + # mjModel has no geom_mass -- it is aggregated -- so the `mass="0"` on each + # geom 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, through it, any + # inertia-derived diagnostic somebody later writes. + 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 so it can + articulate without physics: 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" + # No joint anywhere on the ghost. A jointed child of a mocap body + # compiles and honours a written qpos, but mj_step then integrates + # gravity into it -- see the comment in leader_gripper.xml. + assert model.body_jntnum[body] == 0 + + +# --------------------------------------------------------------------------- +# The geometry. All three transforms are DERIVED, and this is the derivation +# checking itself. +# --------------------------------------------------------------------------- + + +def test_the_three_leader_parts_form_one_assembly(): + """Sub-mm where the parts bolt, mm of running 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. + + Asserted as contact plus the size of a real STS3215, which catches the + units trap: this mesh is Menagerie's, in metres, while its neighbours are + print STLs in millimetres. + """ + 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 deliberately absent + from this list: 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. + + The correction is fixed in the GRIPPER's frame, so it right-multiplies. + Left-multiplying rotates the gripper about the world axes and the ghost + swings 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 tuning the shipped + constants 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. + + Asserted on the recovered hinge ANGLE, not on where a point ends up: over + a large sweep a point on the lever traces an arc, so its position along any + fixed axis rises before it falls. + """ + 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 rotation the wrong way 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 lever tip across the joint's 0..100 degrees; a bound of + # 50 mm is the point below which "released" stops reading as OPEN, which is + # the whole reason the range is the joint's and not a comfortable subset. + 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 rather than restated, 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 rest of the gripper at any closedness. + + Checked against all three other parts: an earlier version checked the + bracket alone, and a range that swung the loop into the SERVO passed it. + + Clearance is not the whole bound. Whether the open lever still reads as + being IN the hand is a headset judgement, and there is no honest headless + proxy for it. + + The 0.8 mm bound is thin on purpose: the tightest legitimate pass is + 2.10 mm at the squeezed end, while a lever driven past it to the joint's + -10 degree limit closes to 0.4 mm. Nearest-vertex distance cannot go + negative, so interpenetration shows up as a small positive number. + """ + 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. + + Builds the real pipeline and drives it with synthetic DeviceIO snapshots, + so the key, the indexing and the deadzone are checked against the shipped + retargeter rather than this file's idea of it. + """ + 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) in MuJoCo world is the scene origin -- a legitimate pose. + + Freezing where it was last seen 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..2f5a6ee5a --- /dev/null +++ b/rigs/mujoco_xr.yaml @@ -0,0 +1,35 @@ +# 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, RigConfig.runtime_command +# falls back to DEFAULT_RUNTIME_COMMAND (rig/config.py:42,89-91), so the runtime +# pane is implicit. No producers either: the app opens the OpenXR session itself +# through Televiz and reads controllers straight from the runtime, so there is +# no rendezvous and deliberately no `params:` / `collection_id:`. +# +# Requires both wheels in the environment you launch the rig from: `{python}` +# expands to the launching interpreter (rig/config.py:249), so +# isaacteleop-examples-mujoco-xr must be installed beside `isaacteleop` rather +# than in a private .venv. +# +# uv pip install "isaacteleop[cloudxr]" --find-links=./install/wheels/ --reinstall +# uv pip install ./examples/mujoco_xr # same environment +# +# That is all this rig needs -- the second line compiles the extension itself +# and never reads the CMake build tree. See examples/mujoco_xr/README.md#build. +# +# 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. Without it the app starts a second + # CloudXR runtime, and the runtime is a host singleton on WSS port 48322, so + # the second one kills this rig's own runtime pane: the headset drops + # mid-session and it reads as a runtime crash rather than a config edit. + # find_runtime_footguns() warns about this (rig/config.py:264-288) but is + # never a gate. + command: "{python} -m isaacteleop_examples.mujoco_xr --no-launch-cloudxr-runtime"